ros2 创建发布和服务

1.创建工作空间

mkdir -p dev_ws/src

2.创建功能包

ros2 pkg create test_demo --build-type ament_cmake --node-name my_demo --dependencies std_msgs rclcpp

 3. 创建发布topic

    3.1 头文件

#include "rclcpp/rclcpp.hpp" //rclcpp/rclcpp.hpp是ROS2中常用C++接口的头文件，使用C++编写的ROS2节点程序一定需要包含该头文件

#include <sensor_msgs/msg/image.hpp>

#include <cv_bridge/cv_bridge.h>

    3.2 实现

　　//初始化函数 

　　rclcpp::init(argc, argv);

//1.创建节点     

    rclcpp::Node::SharedPtr node_handle = rclcpp::Node::make_shared("test_demo");

    //2.发布者发布“rgb_image”话题

    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub = node_handle->create_publisher<sensor_msgs::msg::Image>("rgb_image",10);

   //3. 设置发送频率

　rclcpp::WallRate loop_rate(1);

    while(rclcpp::ok()) {

　　sensor_msgs::msg::Image msg;

　　msg.encoding = sensor_msgs::image_encodings::RGB8;

　　.....

      //4. 发布

　　pub->publish(msg);

        loop_rate.sleep();

}

4. 添加依赖项

  4.1 package.xml

在dev_ws/src/test_dmo/package.xml中,ament_cmake描述的编译依赖过后之后粘贴下面两行 添加：

<exec_depend>rclcpp</exec_depend>

 

　　 <buildtool_depend>std_msgs</buildtool_depend>

 　　 <buildtool_depend>sensor_msgs</buildtool_depend>

  　　<buildtool_depend>cv_bridge</buildtool_depend>  //rgb需要

 

  　　<depend>std_msgs</depend>

  　　<depend>sensor_msgs</depend>

  　　<depend>cv_bridge</depend>

  //rgb图像需要

4..2 CMakeLists.txt

 打开同目录下的CMakeLists.txt文件。在已有的依赖find_package(ament_cmake REQUIRED)下面添加：

find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)

find_package(cv_bridgeREQUIRED)

然后设置编译规则，添加一个my_demo(节点)的可执行文件，以便ros2 run可以运行这个文件

add_executable(my_demo src/test_demo.cpp)

ament_target_dependencies(test_demo rclcpp std_msgs sensor_msgs cv_bridge)

然后需要添加install(TARGETS…)部分以便于ros2 run可以找到这个可执行文件

install(TARGETS

  test_demo

  DESTINATION lib/${PROJECT_NAME}

5. 编译

 回到工作空间目录： 

 colcon build 

或者 colcon build --package-select test_demo

6. 运行

 回到工作空间目录： 

 source install/setup.bash

 ros2 run test_demo my_demo

7.rivz2 查看图像

8. 同一个node下创建服务

   8.1 创建srv 文件

   cd dev_ws/src/test_demo

   mkdir srv

   touch AddTwoInts.srv

   srv文件内容：

int64 a
int64 b
---
int64 sum

  8.2 配置

   package.xml添加：

<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

CMakeLists.txt中添加：

#添加依赖

find_package(rosidl_default_generators REQUIRED)

#声明srv文件所属工程

rosidl_generate_interfaces(${PROJECT_NAME} "srv/AddTwoInts.srv" DEPENDENCIES )

　　# 为了使用同一个包中的msg,srv,需要rosidl_target_interfaces， 把.srv编译成对应的头文件（通过ROS2的IDL模块 产生了头文件）, 必须要有，否则找不到服务的头文件

　　rosidl_target_interfaces(test_demo ${PROJECT_NAME}  "rosidl_typesupport_cpp")

8.3 编译

 回到工作空间： colcon build

8.4实现

//源文件件中引入服务的头文件，编译时自动生成

　　#include "test_demo/srv/add_two_ints.hpp"

      //2. 创建服务

　　rclcpp::Service<test_demo::srv::AddTwoInts>::SharedPtr service =

    　　node_handle->create_service<test_demo::srv::AddTwoInts>("add_two_ints", &add);

　　 //3.一直等待服务回调

       rclcpp::spin(node_handle);

 

    //4. add回调函数

void add(const std::shared_ptr<test_demo::srv::AddTwoInts::Request> request,

          std::shared_ptr<test_demo::srv::AddTwoInts::Response>  response)

{

  response->sum = request->a + request->b;

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %d" " b: %d",

                request->a, request->b);

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%d]", (long int)response->sum);

}

 

8.4编译

回到工作空间目录： 

 colcon build 

或者 colcon build --package-select test_demo

8.5. 运行

 回到工作空间目录： 

 source install/setup.bash

 ros2 run test_demo my_demo

8.6 测试服务

  打开另一个终端

 source install/setup.bash

//注意：key 后的：要有空格

  ros2 service call /add_two_ints test_demo/srv/AddTwoInts '{a: 22,b: 33}'

8.7 相关命令

  ros2 service list -t