ROS2学习之路之服务(client)_ros2 client call

关键代码

//创建分组
rclcpp::CallbackGroup::SharedPtr ClientCallBackGroup_;//不运行在构造中创建，否则不好使
//声明
rclcpp::Client<msg_srv::srv::Test>::SharedPtr  client1_；
//初始化
client1_ = nodePtr_->create_client<msg_srv::srv::Test>("Srv1", rmw_qos_profile_default, ClientCallBackGroup_);
.//请求主函数
std::thread{[this]{
rclcpp::WallRate loop_rate(1.0);//1Hz
while (rclcpp::ok())
{
	  auto request = std::make_shared<msg_srv::srv::Test::Request>();
	  // auto Response = std::make_shared<msg_srv::srv::Test::Response>()
	  request-> goal_ids.push_back(1);
	  request-> id = "srv1-client1";
	  RCLCPP_INFO(nodePtr_->get_logger(), " response ");
	  auto result = client1_->async_send_request(request); 
	  auto response = result.get();
	  RCLCPP_INFO(nodePtr_->get_logger(), " response: %d ", response->success);
	
	  loop_rate.sleep();
}
}}.detach();
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

eg.1

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int8.hpp>
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include "std_msgs/msg/string.hpp"
#include "msg_srv/srv/test.hpp"
#include "msg_srv/msg/test.hpp"
using namespace std::chrono_literals;
using std::placeholders::_1;
using std::placeholders::_1;

class MinimalNode
{
  private:
    rclcpp::Node::SharedPtr nodePtr_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_,subscription1_,subscription2_;
    rclcpp::Service<msg_srv::srv::Test>::SharedPtr  Srv1_,Srv2_;
    rclcpp::Client<msg_srv::srv::Test>::SharedPtr  client1_,client2_;
    rclcpp::CallbackGroup::SharedPtr srvCallBackGroup_;
    rclcpp::CallbackGroup::SharedPtr callBackGroup_;
    rclcpp::CallbackGroup::SharedPtr ClientCallBackGroup_;

  public:
    MinimalNode(rclcpp::Node::SharedPtr& nodePtr):nodePtr_(nodePtr)
    {
      //一个节点，并且在main函数中生成执行器，三个回调分组

      callBackGroup_      = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      srvCallBackGroup_   = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      ClientCallBackGroup_   = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::Reentrant);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      
      rclcpp::SubscriptionOptions options;
      options.callback_group     = callBackGroup_;


      subscription_  = nodePtr_->create_subscription<std_msgs::msg::String>("topic", 10, std::bind(&MinimalNode::topic_callback, this, _1),options);
      subscription1_ = nodePtr_->create_subscription<std_msgs::msg::String>("topic1", 10, std::bind(&MinimalNode::topic_callback1, this, _1),options);
      subscription2_ = nodePtr_->create_subscription<std_msgs::msg::String>("topic2", 10, std::bind(&MinimalNode::topic_callback2, this, _1),options);

      Srv1_ = nodePtr_->create_service<msg_srv::srv::Test>("Srv1", std::bind(&MinimalNode::srv_callback1, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srvCallBackGroup_);
      Srv2_ = nodePtr_->create_service<msg_srv::srv::Test>("Srv2", std::bind(&MinimalNode::srv_callback2, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srvCallBackGroup_);
      
      client1_ = nodePtr_->create_client<msg_srv::srv::Test>("Srv1", rmw_qos_profile_default, ClientCallBackGroup_);
      client2_ = nodePtr_->create_client<msg_srv::srv::Test>("client2", rmw_qos_profile_default, ClientCallBackGroup_);

    }

    void topic_callback1(const std_msgs::msg::String::SharedPtr msg) const {RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'", msg->data.c_str());}
    void topic_callback2(const std_msgs::msg::String::SharedPtr msg) const {RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'", msg->data.c_str());}
    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const {RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s' ", msg->data.c_str());} 
    bool srv_callback1(const std::shared_ptr<msg_srv::srv::Test::Request> req, std::shared_ptr<msg_srv::srv::Test::Response> res) const
    {
      std::vector<int> num(req->goal_ids);
      std::string id = req->id;
      res->success = false;
      RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'  call success!!!", req->id.c_str());
      return false;
    }
    bool srv_callback2(const std::shared_ptr<msg_srv::srv::Test::Request> req, std::shared_ptr<msg_srv::srv::Test::Response> res) const
    {
      std::vector<int> num(req->goal_ids);
      std::string id = req->id;
      res->success = false;
      RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'  call success!!!", req->id.c_str());
      return false;
    }
    void run()
    {
      RCLCPP_INFO(nodePtr_->get_logger(), "111111111");
      std::thread{[this]{
            rclcpp::WallRate loop_rate(1.0);//1Hz
            while (rclcpp::ok())
            {

              auto request = std::make_shared<msg_srv::srv::Test::Request>();
              // auto Response = std::make_shared<msg_srv::srv::Test::Response>();

              request-> goal_ids.push_back(1);
              request-> id = "srv1-client1";
              RCLCPP_INFO(nodePtr_->get_logger(), " response ");
              auto result = client1_->async_send_request(request); 
              auto response = result.get();
              RCLCPP_INFO(nodePtr_->get_logger(), " response: %d ", response->success);

               loop_rate.sleep();
            }
      }}.detach();
      // executor_->spin();
    }

};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);

  auto nodePtr = rclcpp::Node::make_shared("mpac_points_filter");

  auto heartBeatPubPtr = nodePtr->create_publisher<std_msgs::msg::Int8>("heartBeat", 10);

  std_msgs::msg::Int8 heartBeatData;
  heartBeatData.data = 6;  //6-filter node
  auto heartBeatTimer = nodePtr->create_wall_timer(std::chrono::seconds(1), [heartBeatPubPtr, heartBeatData]() { heartBeatPubPtr->publish(heartBeatData); });

  RCLCPP_INFO(nodePtr->get_logger(), "start");
  MinimalNode vcq(nodePtr);
  vcq.run();
  
  auto exec_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::executor::create_default_executor_arguments(), 5);
  exec_->add_node(nodePtr);
  exec_->spin();
  exec_->remove_node(nodePtr);

  RCLCPP_INFO(nodePtr->get_logger(), "end");

  rclcpp::shutdown();

  return 0;
}


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123