ROS1 升级到 ROS2 的一个小例子

目录

一　ROS 1 源代码

二　迁移到 ROS2 后的代码

三　ROS1 迁移到 ROS2 的修改的内容

　　　3.1  头文件

　　　3.2  节点

　　　3.3  周期

　　　3.4  消息类型

　　　3.5  订阅消息

　　　3.6  发布消息

　　　3.7  调度（处理）

           3.8  时间（now）

 一、ROS 1 源代码 ：

#include <sstream>
#include <ros/ros.h>
#include <std_msgs/String.h>

static void OnChatSub(const std_msgs::String::ConstPtr& msg) {
   ROS_INFO("%s", msg->data.c_str());
}

int main(int argc, char **argv) {
   ros::init(argc, argv, "talker");

   ros::NodeHandle n;
   ros::Rate loop_rate(10);
   ros::Publisher pub = n.advertise<std_msgs::String>("/chat/pub", 1000);
   ros::Subscriber sub = nh.subscribe("/chat/sub", 10, OnChatSub);

   int count = 0;
   std_msgs::String msg;
   while (ros::ok()) {
      std::stringstream ss; {
         ss << "hello world " << count++;
         msg.data = ss.str();
         ROS_INFO("%s", msg.data.c_str());
      } 

      pub.publish(msg);
      ros::spinOnce();
      loop_rate.sleep();
   }

   return 0;
}

  二、迁移到 ROS2 后的代码：

#include <sstream>
#include <rclcpp/rclcpp.hpp>        // <----- #include <ros/ros.h>
#include <std_msgs/msg/string.hpp>  // <----- #include <std_msgs/String.h>


int main(int argc, char **argv) {
   rclcpp::init(argc, argv);
   auto talker_node = rclcpp::Node::make_shared("talker");
                                    // <---- ros::init(argc, argv, "talcker");

   rclcpp::Rate loop_rate(10);      // <---- ros::Rate loop_rate(10);

   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub;    // <---- ros::Publisher pub;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub; // <---- ros::Subscriber sub;

        // <---- ros::NodeHandle nh;
        // <---- pub = nh.advertise<std_msgs::String>("/chat/pub", 1000);
        // <---- sub = nh.subscribe("/chat/sub", 10, OnChatSub);

   pub = talker_node->create_publisher<std_msgs::msg::String>("/chat/pub", 1000);
   sub = talker_node->create_subscription<std_msgs::msg::String>("/chat/sub", 10, 
       [this](const std_msgs::msg::String& msg) {
           ROS_INFO("%s", msg.data.c_str());
       }
   );

   int count = 0;
   std_msgs::msg::String msg;       // <---- std_msgs::String msg;
   while (rclcpp::ok()) {           // <---- ros::ok()
      std::stringstream ss; {
         ss << "hello world " << count++;
         msg.data = ss.str();
         ROS_INFO("%s", msg.data.c_str());
      } 

      pub->publish(msg);
      rclcpp::spin_some(talker_node);　　// <---- ros::spinOnce();
      loop_rate.sleep();
   }

   return 0;
}

 三、ROS1 迁移到 ROS2 代码中 需要修改的内容：

  3.1  头文件

// #include <ros/ros.h>
   ----> #include <rclcpp/rclcpp.hpp>

// #include <std_msgs/String.h>
   ----> #include <std_msgs/msg/string.hpp>

// #include <geometry_msgs/Twist.h>
   ----> #include <geometry_msgs/msg/twist.hpp>

  3.2 节点

// ros::init(argc, argv, "talcker");
   ---->
        // 一行代码变成两行代码
        rclcpp::init(argc, argv);
        auto talker_node = rclcpp::Node::make_shared("talker");

  3.3 周期

// ros::Rate loop_rate(10);
   ----> rclcpp::Rate loop_rate(10);

// ros::NodeHandle nh;
// nh.shutdown();
   ----> rclcpp::shutdown();

 3.4 消息类型

// std_msgs::String msg;
   ----> std_msgs::msg::String msg;
         // 消息都要加 msg 命名空间
         // 消息的头文件从 .h 改成了 .hpp

 3.5 订阅消息

// ros::Subscriber sub;
   ----> rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub;

// ros::NodeHandle nh;
// sub = nh.subscribe("/chat/sub", 10, OnChatSub);
   ----> sub = talker_node->create_subscription<std_msgs::msg::String>("/chat/sub", 10, 
　　　　　　　　　　　　[this](const std_msgs::msg::String& msg) {
                    　　ROS_INFO("%s", msg.data.c_str());
　　　　　　　　　　　　}
               );

 3.6 发布消息

// ros::Publisher pub;
   ----> rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub;

// ros::NodeHandle nh;
// pub = nh.advertise<std_msgs::String>("/chat/pub", 1000);
   ----> pub = talker_node->create_publisher<std_msgs::msg::String>("/chat/pub", 1000);

  3.7 调度（处理）

1. 单次调用（非阻塞）
   // ros::spinOnce();
      ----> rclcpp::spin_some(talker_node);

2. 死循环调用（阻塞）
   // ros::spin();
      ----> rclcpp::spin(talker_node);   

  3.8 时间（now）

// ros::Time::now();
   ----> rclcpp::Clock().now();

// 时间差
auto start = std::chrono::system_clock::now();
// do something
auto end = std::chrono::system_clock::now();
std::chrono::duration<double> sec = end - start;
printf("diff: %.2f", sec.count());

四 错误处理

  1. this specialization of rclcpp::get_message_type_support_handle() should never be called

ros2 run app_z1 app_z1
terminate called after throwing an instance of 'std::runtime_error'
  what():  this specialization of rclcpp::get_message_type_support_handle() should never be called
[ros2run]: Aborted
-----------------------------------------------------------------------
解决方法：检查回调参数的定义