修改service_practice_ws/src/demo_cpp_service/src/patrol_client.cpp
cpp
复制代码
#include <cstdlib>
#include <ctime>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "service_interface/srv/patrol.hpp"
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/msg/parameter_value.hpp"
#include "rcl_interfaces/msg/parameter_type.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
using SetP = rcl_interfaces::srv::SetParameters;
using namespace std::chrono_literals;
using Patrol = service_interface::srv::Patrol;
class PatrolClient : public rclcpp::Node
{
private:
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Client<Patrol>::SharedPtr patrol_client_;
public:
PatrolClient() : Node("patrol_client")
{
patrol_client_ = this->create_client<Patrol>("patrol");
timer_ = this->create_wall_timer(6s, std::bind(&PatrolClient::timer_callback, this));
}
void timer_callback()
{
while(!patrol_client_->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "interrupted when wait for service");
return;
}
RCLCPP_INFO(this->get_logger(), "wait for server..");
}
auto request = std::make_shared<Patrol::Request>();
request->target_x = rand() % 15;
request->target_y = rand() % 15;
RCLCPP_INFO(this->get_logger(), "request for patrol (%f, %f)", request->target_x, request->target_y);
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(), "process success");
}
else if(response->result == Patrol::Response::FAIL)
{
RCLCPP_INFO(this->get_logger(), "process failed");
}
}
);
}
std::shared_ptr<SetP::Response> call_set_parameters(rcl_interfaces::msg::Parameter& parameter)
{
auto param_client = this->create_client<SetP>("/turtle_controller/set_parameters");
while(!param_client->wait_for_service(std::chrono::seconds(1)))
{
if(!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "interrupted when wait service");
return nullptr;
}
RCLCPP_INFO(this->get_logger(), "wait service");
}
auto request = std::make_shared<SetP::Request>();
request->parameters.push_back(parameter);
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)
{
auto param = rcl_interfaces::msg::Parameter();
param.name = "k";
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 = call_set_parameters(param);
if(response == nullptr)
{
RCLCPP_WARN(this->get_logger(), "update parameter failed");
return;
}
else
{
for(auto result : response->results)
{
if (result.successful)
{
RCLCPP_INFO(this->get_logger(), "parameter k is updated: %f", k);
}
else
{
RCLCPP_WARN(this->get_logger(), "parameter k update failed for : %s", result.reason.c_str());
}
}
}
}
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<PatrolClient>();
node->update_server_param_k(1.5);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
修改service_practice_ws/src/demo_cpp_service/src/turtle_control.cpp
cpp
复制代码
#include "rclcpp/rclcpp.hpp"
#include "service_interface/srv/patrol.hpp"
#include "turtlesim/msg/pose.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
using service_interface::srv::Patrol;
using SetParametersResult = rcl_interfaces::msg::SetParametersResult;
class TurtleController : public rclcpp::Node
{
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_);
velocity_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", 10);
pose_subscription_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose", 10,
std::bind(&TurtleController::on_pose_received_, this, std::placeholders::_1));
//添加服务
patrol_server_ = this->create_service<Patrol>(
"patrol",
[&](const std::shared_ptr<Patrol::Request> request,
std::shared_ptr<Patrol::Response> response) -> void{
if((0 < request->target_y && request->target_x < 12.0f)
&& (0 < request->target_y && request->target_y < 12.0f)){
target_x_ = request->target_x;
target_y_ = request->target_y;
response->result = Patrol::Response::SUCCESS;
}
else
{
response->result = Patrol::Response::FAIL;
}
}
);
//添加参数设置回调
parameters_callback_handle_ = this->add_on_set_parameters_callback(
[&](const std::vector<rclcpp::Parameter>& parameters)->SetParametersResult
{
for(auto parameter : parameters)
{
RCLCPP_INFO(this->get_logger(), "set parameter %s to %f", parameter.get_name().c_str(), parameter.as_double());
if (parameter.get_name() == "k")
{
k_ = parameter.as_double();
}
else if(parameter.get_name() == "max_speed")
{
max_speed_ = parameter.as_double();
}
}
auto result = SetParametersResult();
result.successful = true;
return result;
}
);
velocity_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", 10);
}
void on_pose_received_(const turtlesim::msg::Pose::SharedPtr pose)
{
auto message = geometry_msgs::msg::Twist();
double current_x = pose->x;
double current_y = pose->y;
// RCLCPP_INFO(this->get_logger(), "current location: (x=%f, y=%f)", current_x, current_y);
double distance = std::sqrt((target_x_ - current_x)*(target_x_-current_x)+(target_y_-current_y)*(target_y_-current_y));
double angle = std::atan2(target_y_-current_y, target_x_-current_x)-pose->theta;
if(distance>0.1)
{
if (fabs(angle)>0.2)
{
message.angular.z = fabs(angle);
}
else
{
message.linear.x = k_*distance;
}
}
if (message.linear.x > max_speed_)
{
message.linear.x = max_speed_;
}
velocity_publisher_->publish(message);
}
private:
rclcpp::Service<Patrol>::SharedPtr patrol_server_;
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_subscription_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_publisher_;
OnSetParametersCallbackHandle::SharedPtr parameters_callback_handle_;
double target_x_{0.0};
double target_y_{0.0};
double k_{1.0};
double max_speed_{7.0};
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<TurtleController>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
构建运行查看结果，可以发现在/turtle_controller下面有k和max_speed服务
bash
复制代码
colcon build
ros2 run turtlesim turtlesim_node
ros2 run demo_cpp_service patrol_client
ros2 run demo_cpp_service turtle_control
ros2 param list
----------------------------输出-----------------------------
/patrol_client:
qos_overrides./parameter_events.publisher.depth
qos_overrides./parameter_events.publisher.durability
qos_overrides./parameter_events.publisher.history
qos_overrides./parameter_events.publisher.reliability
use_sim_time
/rqt_gui_py_node_1968386:
use_sim_time
/turtle_controller:
k
max_speed
qos_overrides./parameter_events.publisher.depth
qos_overrides./parameter_events.publisher.durability
qos_overrides./parameter_events.publisher.history
qos_overrides./parameter_events.publisher.reliability
use_sim_time
/turtlesim:
background_b
background_g
background_r
qos_overrides./parameter_events.publisher.depth
qos_overrides./parameter_events.publisher.durability
qos_overrides./parameter_events.publisher.history
qos_overrides./parameter_events.publisher.reliability
use_sim_time
