ROS2通信（全）C++

一、ROS2通信机制简介
四部分组成：
1、节点（对应单一功能模块，例如雷达驱动节点、摄像头）
2、话题（具有相同话题的节点可以通信到一起）
3、通信模型（话题通信、服务通信、动作通信、服务通信）
4、接口（数据载体，是数据传输的特定格式 msg文件用于话题通信 srv文件用于服务通信 action文件用于动作通信）

首先创建工作空间

mkdir -p ws01_plumbing/src

然后进入工作空间

cd ws01_plumbling

最后编译即可创建完成

colcon build

1，创建自定义接口文件

cd src
ros2 pkg create --build-type ament_cmake base_interfaces_demo

创建msg,action,srv文件夹

文件夹中创建子文件，首字母要大写

编辑子文件

Student.msg

string  name
int32   age
float64 height

Progress.action

int32 num
---
int32 sum
---
float64 progress

Addints.srv

int32 num1
int32 num2
---
int32 sum

编辑CMakeLists.txt

增加

find_package(rosidl_default_generators REQUIRED)  ###新增
# 为接口文件生成源码
rosidl_generate_interfaces( ${PROJECT_NAME}   ###新增
  "msg/Student.msg"
  "srv/Addints.srv"
  "action/Progress.action"
)

编辑package.xml

msg和srv需要添加

<build_depend>rosidl_default_generators</build_depend>
  <!-- 执行依赖 -->
  <exec_depend>rosidl_default_runtime</exec_depend>
  <!-- 声明当前包所属的功能包组 -->
  <member_of_group>rosidl_interface_packages</member_of_group>

action需要添加

<buildtool_depend>rosidl_default_generators</buildtool_depend>
  
  <depend>action_msgs</depend>

进行编译

colcon build --packages-select base_interfaces_demo

调用命令进行检验

ros2 interface show base_interfaces_demo/msg/Student.msg
ros2 interface show base_interfaces_demo/action/Progress.action
ros2 interface show base_interfaces_demo/srv/Addints.srv

如果出现自己写的数据，即编译成功

2，准备工作

创建话题通信功能包cpp01_topic,服务通信cpp02_service，动作通信cpp03_action,参数通信cpp04_param

ros2 pkg create cpp01_topic --build-type ament_cmake --dependencies rclcpp std_msgs base_interfaces_demo --node-name demo01_talker_str
ros2 pkg create cpp02_service --build-type ament_cmake --dependencies rclcpp base_interfaces_demo --node-name demo01_serive
ros2 pkg create cpp01_action --build-type ament_cmake --dependencies rclcpp rclcpp_action base_interfaces_demo --node-name demo01_action_serive
ros2 pkg create cpp01_param --build-type ament_cmake --dependencies rclcpp --node-name demo01_param_serive

在各自的src中创建c++文件

例如在cpp01_topic/src中创建demo02_listener_str

编辑CMakeList.txt

例如cpp01_topic

cmake_minimum_required(VERSION 3.8)
project(cpp01_topic)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()
#引用外部依赖包
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(base_interfaces_demo REQUIRED)

add_executable(demo01_talker_str src/demo01_talker_str.cpp)
add_executable(demo02_listener_str src/demo02_listener_str.cpp)

target_include_directories(demo01_talker_str PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(demo01_talker_str PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
  demo01_talker_str
  "rclcpp"
  "std_msgs"
  "base_interfaces_demo"
)
ament_target_dependencies(
  demo02_listener_str
  "rclcpp"
  "std_msgs"
  "base_interfaces_demo"      
)

install(TARGETS 
  demo01_talker_str
  demo02_listener_str
  DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

 其实这个只用增加
 

#映射源文件与可执行文件
add_executable(demo02_listener_str src/demo02_listener_str.cpp)
#设置目标依赖库
ament_target_dependencies(
  demo02_listener_str
  "rclcpp"
  "std_msgs"
  "base_interfaces_demo"      
)
#定义安装规则
install(TARGETS 
  demo01_talker_str
  demo02_listener_str
  DESTINATION lib/${PROJECT_NAME})

package.xml不需要编辑，因为新增的文件包不需要依赖其他的文件

3，话题通信

编写发布者

/* 
需求：以某个固定频率发送学生信息“姓名，年龄，身高”，每发布一条，；年龄+1
流程：
  1.包含头文件
  2.初始化ROS2客户端
  3.自定义节点类
    3.1.创建消息发布方
    3.2.创建定时器
    3.3.组织并发布消息
  4.调用spin函数，传入自定义类对象指针
  5.释放资源 
  */
 //1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "base_interfaces_demo/msg/student.hpp"

using namespace std::chrono_literals;//可以在自变量后面跟上时间单位 用于后面定时器的定时设置1s
using base_interfaces_demo::msg::Student;
//3.自定义节点类
class TalkerStu: public rclcpp::Node{
public:
  TalkerStu():Node("talkerstu_node_cpp"),count(0){
    RCLCPP_INFO(this->get_logger(),"发布节点创建！");
    // 3.1.创建消息发布方
    /* 
      模板：被发布的消息类型；
      参数：
        1.话题名称；
        2.QOS（消息队列长度）
        3.默认值不用管
      返回值：发布对象指针
     */
    publisher_ = this->create_publisher<Student>("chatterstu",10);//10代表消息队列长度
    // 3.2.创建定时器
    /*
      模板：可以不使用 
      参数：
        1.时间间隔；
        2.回调函数；
      返回值：定时器对象指针
     */
    timer_ = this->create_wall_timer(1s,std::bind(&TalkerStu::on_timer,this));
  }
private:
  void on_timer(){
    // 3.3.组织并发布消息
    auto message = Student();
    message.name = "huluwa";
    message.age = count;
    count += 1;
    message.height = 150.00;
    RCLCPP_INFO(this->get_logger(),"学生信息：%s,%d,%.2f",message.name.c_str(),message.age,message.height);
    publisher_->publish(message);
  }
//SharePtr为创建对象指针类型的变量，可以存储这种指针类型的地址，例如 int*p；
  rclcpp::Publisher<Student>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  size_t count;
};
int main(int argc, char ** argv)
{
  //2.初始化ROS2客户端
  rclcpp::init(argc,argv);
创建一个context容器
  //4.调用spin函数，传入自定义类对象指针
//make_shared 是创建这种类型的指针，已经存储了初始化的指针
  rclcpp::spin(std::make_shared<TalkerStu>());
  //5.释放资源，删除context
  rclcpp::shutdown();
  return 0;
}

再编写订阅者

/* 
    需求：订阅发布方发布的消息，并在终端输出
    流程：
        1.包含头文件
        2.初始化ROS2客户端
        3.自定义节点类
            3.1.创建订阅方
            3.2.解析并输出数据
        4.调用spin函数，并传入节点对象指针
        5.资源释放
 */
//1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "base_interfaces_demo/msg/student.hpp"

using base_interfaces_demo::msg::Student;

//3.自定义节点类
class ListenerStu: public rclcpp::Node{
public:
    ListenerStu():Node("listenerstu_node_cpp"){
        RCLCPP_INFO(this->get_logger(),"订阅方创建！");
        //3.1.创建订阅方
        /* 
            模板：消息类型；
            参数：
                1.话题名称
                2.QOS，队列长度
                3.回调函数
            返回值：订阅对象指针
         */
        subscription_ = this->create_subscription<Student>("chatterstu",10,std::bind(&ListenerStu::do_cb,this,std::placeholders::_1));
    }
private:
    void do_cb(const Student &msg){
        //3.2.解析并输出数据
        RCLCPP_INFO(this->get_logger(),"学生信息：%s,%d,%.2f",msg.name.c_str(),msg.age,msg.height);
    }
    rclcpp::Subscription<Student>::SharedPtr subscription_;
};


int main(int argc, char const *argv[])
{
    //2.初始化ROS2客户端
    rclcpp::init(argc,argv);
    //4.调用spin函数，并传入节点对象指针
    rclcpp::spin(std::make_shared<ListenerStu>());
    //5.资源释放
    rclcpp::shutdown();
    return 0;
}

补充 

//SharePtr为创建对象指针类型的变量，可以存储这种指针类型的地址，例如 int*p；
//make_shared 是创建这种类型的指针，已经存储了初始化的指针
如果需要打印%s，需要c_str()
    头文件引入：
        rclcpp/rclcpp.hpp: ROS2 C++库的主要头文件。
        std_msgs/msg/string.hpp: 包含用于发布字符串消息的头文件。

    命名空间：
        使用using namespace std::chrono_literals;引入chrono命名空间，用于后面设置定时器的时间间隔。

    自定义节点类 Talker：
        继承自rclcpp::Node类，表示这是一个ROS2节点。
        构造函数中初始化节点，并创建了消息发布者和定时器。

    消息发布者和定时器的创建：
        create_publisher创建了一个发布者，用于发布字符串消息到名为 "chatter" 的话题，消息队列长度为 10。
        create_wall_timer创建了一个定时器，每隔1秒触发一次，触发时调用on_timer成员函数。

    on_timer 成员函数：
        组织并发布带有编号的消息，编号每次递增。
        使用RCLCPP_INFO输出日志，显示发布的消息内容。

    main 函数：
        初始化ROS2客户端。
        创建Talker类的实例并传入spin函数，使节点进入主循环。
        最后释放ROS2资源并返回0。

 4，服务通信

编辑服务端

#include "rclcpp/rclcpp.hpp"
#include "base_interfaces_demo/srv/addints.hpp"

using base_interfaces_demo::srv::Addints;
//使用占位符
using std::placeholders::_1;
using std::placeholders::_2;

class AddintsSerive : public rclcpp::Node{
public:
   AddintsSerive():Node("addint_serivr_node_cpp")
   {

RCLCPP_INFO(this->get_logger(),"服务端创建");
  server_ = this->create_service<Addints>("addints",std::bind(&AddintsSerive::add,this,_1,_2));
   }
//被封装，rep为接口文件里Request,res为Response指针
   void add(const Addints::Request::SharedPtr req, const Addints::Response::SharedPtr res){
  res->sum = req->num1 + req->num2;
  RCLCPP_INFO(this->get_logger(),"收到数据：(%d,%d),进行响应：%d",req->num1,req->num2,res->sum);

   }
   private:

   rclcpp::Service<Addints>::SharedPtr server_;



};



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

rclcpp::spin(std::make_shared<AddintsSerive>());

rclcpp::shutdown();
return 0;
}

编辑客户端

#include "rclcpp/rclcpp.hpp"
#include "base_interfaces_demo/srv/addints.hpp"

using base_interfaces_demo::srv::Addints;
using namespace std::chrono_literals;
class AddintsCilent : public rclcpp::Node{

public:
AddintsCilent(): rclcpp::Node("add_int_client_node_cpp"){
 RCLCPP_INFO(this->get_logger(),"客户端创建");
 client_ = this->create_client<Addints>("addints");

}

 bool connect_server(){
        //在指定超时时间内连接服务器，如果连接上了，那么返回true，否则返回false
        // client_->wait_for_service(1s);
        while(!client_->wait_for_service(1s))
        {
            //对ctrl+c进行特殊处理，如果不处理对于想要提前终止客户端的操作会异常
            if(!rclcpp::ok())
            {
                RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"强行终止客户端！");
                return false;
            }
            RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"服务连接中！");
        }
        return true;
    }

  rclcpp::Client<Addints>::FutureAndRequestId send_request(int num1, int num2)
    {
        //组织请求数据
        //发送
        /* 
        rclcpp::Client<base_interfaces_demo::srv::AddInts>::FutureAndRequestId 
        async_send_request(std::shared_ptr<base_interfaces_demo::srv::AddInts_Request> request)
         */
        auto request = std::make_shared<Addints::Request>();
        request->num1 = num1;
        request->num2 = num2;
        return client_->async_send_request(request);
    }

private:

rclcpp::Client<Addints>::SharedPtr client_;


};



int main(int argc,char ** argv)
{
if(argc!=3)
{
RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"请提交两个整型数字！");
 return 1;   
}
rclcpp::init(argc,argv);

 auto client = std::make_shared<AddintsCilent>();
    bool flag = client->connect_server();
 if (!flag)
    {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"服务器连接失败，程序退出！");
        return 0;
    }
    //调用请求提交函数，接受并处理响应结果
    auto future = client->send_request(atoi(argv[1]),atoi(argv[2]));
    //处理响应
    if (rclcpp::spin_until_future_complete(client,future) == rclcpp::FutureReturnCode::SUCCESS)//成功
    {
        RCLCPP_INFO(client->get_logger(),"响应成功!sum = %d",future.get()->sum);
    }
    else //失败
    {
        RCLCPP_INFO(client->get_logger(),"响应失败！");
    }
rclcpp::shutdown();
return 0;
}
//在编译时写的数据，通过argv[]来获取数据

5，动作通信

编辑服务端

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces_demo/action/progress.hpp"
using std::placeholders::_1;
using std::placeholders::_2;

using base_interfaces_demo::action::Progress;
class ActionServer : public rclcpp::Node
{

public:
ActionServer(): Node("progress_action_server_node_cpp")
{

server_ = rclcpp_action::create_server<Progress>(this,"get_sum",std::bind(&ActionServer::handle_goal,this,_1,_2),std::bind(&ActionServer::handle_cancal,this,_1),std::bind(&ActionServer::handle_accepted,this,_1));

}
//处理提交的目标值（回调函数）
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const Progress::Goal> goal)
{
//判断提交的数值是fu大于1，是就接受，不是就拒绝
(void) uuid;
if(goal->num<=1){
return rclcpp_action::GoalResponse::REJECT;
}
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
//处理取消请求
rclcpp_action::CancelResponse handle_cancal(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
{
(void)goal_handle;

return rclcpp_action::CancelResponse::ACCEPT;
}
//生成连续反馈与最终响应
void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle){
int num = goal_handle->get_goal()->num;
int sum=0;
auto feedback =std::make_shared<Progress::Feedback>();
rclcpp::Rate rate(1.0);
for (int i = 0; i < num; i++)
{
  sum += i;
  double progress=i/(double)num;
  feedback->progress =progress;
  RCLCPP_INFO(this->get_logger(),"%.2f",progress);
  goal_handle->publish_feedback(feedback);
  rate.sleep();
}
if(rclcpp::ok())
{
auto result = std::make_shared<Progress::Result>();
result->sum =sum;
goal_handle->succeed(result);
}

}

void handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
{
//新建一个线程
std::thread(std::bind(&ActionServer::execute,this,goal_handle)).detach();

}






private:
rclcpp_action::Server<Progress>::SharedPtr server_;



};







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

rclcpp::spin(std::make_shared<ActionServer>());
rclcpp::shutdown();
  return 0;
}

编辑客户端

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces_demo/action/progress.hpp"

using std::placeholders::_1;
using std::placeholders::_2;
using namespace std::chrono_literals;
using base_interfaces_demo::action::Progress;
class ActionClient : public rclcpp::Node
{

public:
ActionClient(): Node("progress_action_client_node_cpp")
{

client_ = rclcpp_action::create_client<Progress>(this,"get_sum");


}


void send_goal(int num)
{
if(!client_->wait_for_action_server(10s))
{

  return;
}
auto goal =Progress::Goal();
goal.num = num ;
rclcpp_action::Client<Progress>::SendGoalOptions option;
option.goal_response_callback = std::bind(&ActionClient::goal_response_callback,this,_1);
option.feedback_callback = std::bind(&ActionClient::feedback_callback,this,_1,_2);
option.result_callback = std::bind(&ActionClient::result_callback,this,_1);
auto future = client_->async_send_goal(goal,option);
//发送请求，里面要求的数据类型为Progress::Goal()和rclcpp_action::Client<Progress>::SendGoalOptions，2.4.3 要设置里面的数据
}
void goal_response_callback(rclcpp_action::ClientGoalHandle<Progress>::SharedPtr goal_handle){
if(!goal_handle)
{
  RCLCPP_INFO(this->get_logger(),"NO");
}
else
{

  RCLCPP_INFO(this->get_logger(),"YES");
}
}

void feedback_callback(rclcpp_action::ClientGoalHandle<Progress>::SharedPtr goal_handle,const std::shared_ptr<const Progress::Feedback> feedback){
(void)goal_handle;
double progress = feedback->progress;
RCLCPP_INFO(this->get_logger(),"%.2f",progress);
}

void result_callback(const rclcpp_action::ClientGoalHandle<Progress>::WrappedResult & result){

if(result.code == rclcpp_action::ResultCode::SUCCEEDED)
{
RCLCPP_INFO(this->get_logger(),"%d",result.result->sum);

}
}                
private:

rclcpp_action::Client<Progress>::SharedPtr client_;

};

6，参数通信

编辑服务端

#include "rclcpp/rclcpp.hpp"

class ParamServer : public rclcpp::Node
{
public:
ParamServer():Node("param_server_node_cpp",rclcpp::NodeOptions().allow_undeclared_parameters(true))
{
RCLCPP_INFO(this->get_logger(),"服务端创建");

}
void declare_param(){
this->declare_parameter("car_name","tiger");
this->declare_parameter("width",1.55);
this->declare_parameter("wheels",5);
this->set_parameter(rclcpp::Parameter("high","10"));
}
void get_param(){
auto car = this->get_parameter("car_name");
RCLCPP_INFO(this->get_logger(),"%s=%s",car.get_name().c_str(),car.as_string().c_str());  



}
void update_param(){
this->set_parameter(rclcpp::Parameter("width",6.50));
    
}
void del_param(){
this->undeclare_parameter("high");
    
}

};


int main(int argc, char ** argv)
{
rclcpp::init(argc,argv);
auto node = std::make_shared<ParamServer>();
node->declare_param();
node->get_param();
node->update_param();
node->del_param();
rclcpp::spin(node);

rclcpp::shutdown();
  return 0;

}

编辑客户端

#include "rclcpp/rclcpp.hpp"
using namespace std::chrono_literals;
class ParamClient : public rclcpp::Node
{
public:

ParamClient():Node("param_client_node_cpp")
{
RCLCPP_INFO(this->get_logger(),"客户端创建");
param_client = std::make_shared<rclcpp::SyncParametersClient>(this,"param_server_node_cpp");
}
bool connect_server(){
        //在指定超时时间内连接服务器，如果连接上了，那么返回true，否则返回false
        // client_->wait_for_service(1s);
        while(!param_client->wait_for_service(1s))
        {
            //对ctrl+c进行特殊处理，如果不处理对于想要提前终止客户端的操作会异常
            if(!rclcpp::ok())
            {
                RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"强行终止客户端！");
                return false;
            }
            RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"服务连接中！");
        }
        return true;
    }
void get_param(){

std::string car = param_client->get_parameter<std::string>("car_name");
double qwe = param_client->get_parameter<double>("width");
RCLCPP_INFO(this->get_logger(),"%s",car.c_str());
RCLCPP_INFO(this->get_logger(),"%.2f",qwe);
}
void update_param(){
param_client->set_parameters({rclcpp::Parameter("width",3.00)});
double cww = param_client->get_parameter<double>("width");
RCLCPP_INFO(this->get_logger(),"%.2f",cww);

}



private:

rclcpp::SyncParametersClient::SharedPtr param_client;


};

int main(int argc, char ** argv)
{
rclcpp::init(argc,argv);
auto node = std::make_shared<ParamClient>();
bool feed = node->connect_server();
if (!feed)
    {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"服务器连接失败，程序退出！");
        return 0;
    }
node->get_param();
node->update_param();


rclcpp::shutdown();
  return 0;

}