class PatrolClientNode: public rclcpp::Node //定义类，继承自rclcpp::Node
{
public:
PatrolClientNode():Node("turtle_controller")
{
srand(time(NULL));//初始化随机数种子

    patrol_client_ = this->create_client<Patrol>("patrol");
    //创建客户端
    timer_ = this->create_wall_timer(10s,[&]()->void{
    //1检测服务端是否上线
    while (!this->patrol_client_->wait_for_service(1s))
    {
        if(!rclcpp::ok()){
            RCLCPP_ERROR(this->get_logger(),"等待服务上线过程中，rclcpp挂了，我退下了");
            return;
        }
        RCLCPP_ERROR(this->get_logger(),"等待服务上线中");
    }
    //2构造请求的对象
    auto request = std::make_shared<Patrol::Request>();
    request->target_x = rand() % 12;
    request->target_y = rand() % 12;//取余防止超过12
    RCLCPP_INFO(this->get_logger(),"准备好目标点%f,%f",
    request->target_x,request->target_y);
    //3发送请求,调动函数，异步发送请求
    this->patrol_client_->async_send_request(request,[&]
    (rclcpp::Client<Patrol>::SharedFuture result_future)-> void{
        auto response = result_future.get();
        if(response->result==Patrol::Response::SUCCESS){
            RCLCPP_INFO(this->get_logger(),"请求巡逻目标点成功");
        }
        if(response->result==Patrol::Response::FAIL){
            RCLCPP_INFO(this->get_logger(),"请求巡逻目标点失败"); 
        }
        });    
    });
    
}

//创建客户端发送请求，返回结果
SetP::Response::SharedPtr call_set_parameter(const rcl_interfaces::msg::Parameter &param)
{
    auto param_client_ = this->create_client<SetP>("/turtle_controller/set_parameters");
    //更改客户端名（智能指针），服务类型名，服务名称
    //创建客户端
    //1检测服务端是否上线
    while (!this->patrol_client_->wait_for_service(1s))
    {
        if(!rclcpp::ok()){
            RCLCPP_ERROR(this->get_logger(),"等待服务上线过程中，rclcpp挂了，我退下了");
            return nullptr;
        }
        RCLCPP_ERROR(this->get_logger(),"等待服务上线中");
    }
    //2构造请求的对象
    auto request = std::make_shared<SetP::Request>();
    request->parameters.push_back(param);
    //3发送请求,调动函数，异步发送请求
    auto future = param_client_->async_send_request(request);
    rclcpp::spin_until_future_complete(this->get_node_base_interface(),future);
    auto response = future.get();
    return response;
}

//更新参数
void update_server_param_k(double k)
{
    //1创建参数对象
    auto param = rcl_interfaces::msg::Parameter();
    param.name = "k";
    //2创建参数值
    auto param_value  = rcl_interfaces::msg::ParameterValue();
    param_value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
    param_value.double_value = k;
    param.value = param_value;
    //请求更新参数并处理
    auto response = this->call_set_parameter(param);
    if(response==NULL){
        RCLCPP_INFO(this->get_logger(),"参数更新失败");
        return;
        
    }
    for (auto result:response->results)
    //返回值是result[]数组，遍历数组
    {
        if (result.successful==false)
        {
            RCLCPP_INFO(this->get_logger(),"参数更新失败，原因：%s",result.reason.c_str());
        }else{
            RCLCPP_INFO(this->get_logger(),"参数更新成功");
        }
        
    }
    
}

private://私有成员变量 声明定时器和客户端
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Client<Patrol>::SharedPtr patrol_client_;
};

int main(int argc,char* argv[])//俩命令行参数
{
rclcpp::init(argc,argv);
auto node = std::make_shared<PatrolClientNode>();//创建节点turtle-circle，创建TCN类的共享指针
node->update_server_param_k(4.0);
rclcpp::spin(node);//启动节点循环
rclcpp::shutdown();//关闭环境
return 0;
}
//turtle_control.cpp
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "turtlesim/msg/pose.hpp"
#include "chapt4_interfaces/srv/patrol.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
#include "rclcpp/parameter_client.hpp"
#include "rclcpp/parameter.hpp"

using Patrol = chapt4_interfaces::srv::Patrol;
using SetParametersResult = rcl_interfaces::msg::SetParametersResult;

class TurtleController: public rclcpp::Node //定义类，继承自rclcpp::Node
{
private:
OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
rclcpp::Service<Patrol>::SharedPtr patrol_server_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_; //发布者的智能指针
rclcpp::Subscriptionturtlesim::msg::Pose::SharedPtr subscriber_; //订阅者的智能共享指针
double target_x_{1.0};
double target_y_{1.0};
double k_{1.0};//比例系数
double max_speed_{3.0};

public:
TurtleController() : Node("turtle_controller")
{
this->declare_parameter("k",1.0);
this->declare_parameter("max_speed",1.0);
this->get_parameter("k",k_);
this->get_parameter("max_speed",max_speed_);
this->set_parameter(rclcpp::Parameter("k",2.0));

    parameter_callback_handle_ = this->add_on_set_parameters_callback([&](const 
    std::vector<rclcpp::Parameter> & parameters)->
        rcl_interfaces::msg::SetParametersResult{
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = true;
        for (const auto & parameter : parameters) {
            RCLCPP_INFO(this->get_logger(),"更新参数的值%s=%f",parameter.get_name().
            c_str(),parameter.as_double());
            if(parameter.get_name()=="k")
            {
                k_ = parameter.as_double();
            }
            if(parameter.get_name()=="max_speed")
            {
                max_speed_ = parameter.as_double();
            }
        }
        return result;
        });
    patrol_server_ = this->create_service<Patrol>("patrol",[&](const 
    Patrol::Request::SharedPtr request,Patrol::Response::SharedPtr response) -> void{
        if(
            (0 < request->target_x && request->target_x < 12.0f)&&
            (0 < request->target_y && request->target_y < 12.0f)
        ){
            this->target_x_ = request->target_x;
            this->target_y_ = request->target_y;
            response->result = Patrol::Response::SUCCESS;
        }else{
            response->result = Patrol::Response::FAIL;
        }
        //额外调用添加的回调函数，做到第一时间更新参数
    });
    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
        "/turtle1/cmd_vel", 10);
    subscriber_ = this->create_subscription<turtlesim::msg::Pose>(
        "/turtle1/pose", 10,
        std::bind(&TurtleController::on_pose_received_, this, std::placeholders::_1)); 
    
}

private:
void on_pose_received_(const turtlesim::msg::Pose::SharedPtr pose){
//1.获取当前位置
auto current_x = pose->x;
auto current_y = pose->y;
RCLCPP_INFO(get_logger(),"当前:x=%f,y=%f",current_x,current_y);

    //2.计算当前海龟位置跟目标位置之间的距离差和角度差
    auto distance = std::sqrt(
        (target_x_-current_x)*(target_x_-current_x)+
        (target_y_-current_y)*(target_y_-current_y)      
    );
    auto angle = std::atan2( (target_y_-current_y), (target_x_-current_x)) - pose->theta;

    //3.控制策略
    auto msg = geometry_msgs::msg::Twist();
    if(distance>0.1){
        if(fabs(angle)>0.2){
            msg.angular.z = fabs(angle);
        }else{
            msg.linear.x = k_*distance;
        }
    }

    //4.限制线速度最大值
    if(msg.linear.x > max_speed_){
        msg.linear.x = max_speed_;
    }
    publisher_->publish(msg);
}

};

int main(int argc,char* argv[])//俩命令行参数
{
rclcpp::init(argc,argv);
auto node = std::make_shared<TurtleController>();//创建节点turtle-circle，创建TCN类的共享指针
rclcpp::spin(node);//启动节点循环
rclcpp::shutdown();//关闭环境
return 0;
}