使用机器人操作系统ROS 2和仿真软件Gazebo 9主题进阶实战（七）- mobot速度发布与里程计订阅

在ROS2课程中已经学过并掌握了一个基本的发布器和订阅器（C++），官网的教程全部掌握大致需要20分钟吧。

这过程包括：

• 创建一个功能包
• 编程实现一个发布节点
• 编程实现一个订阅节点
• 编译与运行

这部分内容作为复习，放置于文末，本文在Gazebo 9仿真环境中，使用mobot编程实现一个速度发布器和里程计订阅。

实现效果参考如下视频：

在mobot/src文件夹，新建pub_vel.cpp和sub_pose.cpp。

要点：

• 随机速度如何实现？
• 速度发布和里程计订阅需要包含的头文件？

提示：

• rand()
• geometry_msgs/msg/twist
• nav_msgs/msg/odometry

为了避免难度过大，这里提供turtlesim随机速度发布和里程计订阅的源代码示例。

pubvel.cpp

      #include <iostream>
      #include <chrono>
      #include "rclcpp/rclcpp.hpp"
      #include "geometry_msgs/msg/twist.hpp"
      using namespace std::chrono_literals;
      int main(int argc, char * argv[])
      {
        rclcpp::init(argc, argv);
       auto node = rclcpp::Node::make_shared("publish_velocity");
       auto publisher = node->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", 10);
        geometry_msgs::msg::Twist message;
       rclcpp::WallRate loop_rate(500ms);
       while (rclcpp::ok()) {
       message.linear.x = ((double)rand()/(RAND_MAX));
       message.angular.z = 4.0*((double)rand()/(RAND_MAX))-2;
       RCLCPP_INFO(node->get_logger(), "Publishing /turtle1/cmd_vel : linear='%f',angular='%f'", message.linear.x, message.angular.z);
       publisher->publish(message);
       rclcpp::spin_some(node);
       loop_rate.sleep();
        }
        rclcpp::shutdown();
       return 0;
      }
  
 
 

这样等于速度发布直接给答案了……

但是里程计订阅有所不同！

subpose.cpp

      #include "rclcpp/rclcpp.hpp"
      #include "turtlesim/msg/pose.hpp"
      rclcpp::Node::SharedPtr g_node = nullptr;
      void topic_callback(const turtlesim::msg::Pose::SharedPtr msg)
      {
        RCLCPP_INFO(g_node->get_logger(), "I heard: turtle1/pose position='%f','%f'; direction='%f'", msg->x, msg->y, msg->theta);
      }
      int main(int argc, char * argv[])
      {
        rclcpp::init(argc, argv);
        g_node = rclcpp::Node::make_shared("subscribe_to_pose");
       auto subscription =
       g_node->create_subscription<turtlesim::msg::Pose>("turtle1/pose", 10, topic_callback);
        rclcpp::spin(g_node);
        rclcpp::shutdown();
        subscription = nullptr;
        g_node = nullptr;
       return 0;
      }
  
 
 

mobot实现此功能订阅里程计位置姿态信息，请参考提示。

然后需要修改一下CMakeLists.txt，在合适位置增加如下代码：

      add_executable(pub_vel src/pub_vel.cpp)
      ament_target_dependencies(pub_vel rclcpp geometry_msgs)
      add_executable(sub_pose src/sub_pose.cpp)
      ament_target_dependencies(sub_pose rclcpp nav_msgs)
      install(TARGETS
        pub_vel
        sub_pose
        DESTINATION lib/${PROJECT_NAME})
  
 
 

然后编译并运行如下命令即可，分别在不同的终端哦。

• ros2 launch mobot topic.launch.py 

如何编写launch，参考之前章节。

• ros2 run mobot sub_pose

• ros2 run mobot pub_vel 

当然也可以在launch中将pub_vel和sub_pose添加，这样就无需新开窗口了，如何实现？

关于发布器和订阅器的更多说明：

• ROS1为功能实现，代码其实没有美感，更不用说精致了，优美的代码是艺术品！
• ROS2更进一步，代码风格更好

比如发布器：

类ROS1方式实现代码如下，（不推荐，不推荐，不推荐！！！）：

      #include <iostream>
      #include <chrono>
      #include "rclcpp/rclcpp.hpp"
      #include "std_msgs/msg/string.hpp"
      using namespace std::chrono_literals;
      int main(int argc, char * argv[])
      {
        rclcpp::init(argc, argv);
       auto node = rclcpp::Node::make_shared("minimal_publisher");
       auto publisher = node->create_publisher<std_msgs::msg::String>("topic", 10);
        std_msgs::msg::String message;
       auto publish_count = 0;
       rclcpp::WallRate loop_rate(500ms);
       while (rclcpp::ok()) {
       message.data = "Hello, world! " + std::to_string(publish_count++);
       RCLCPP_INFO(node->get_logger(), "Publishing: '%s'", message.data.c_str());
       publisher->publish(message);
       rclcpp::spin_some(node);
       loop_rate.sleep();
        }
        rclcpp::shutdown();
       return 0;
      }
  
 
 

但请务必注意！官方解释：

我们不推荐使用这种样式，因为在同一个可执行文件中不可能有多个节点组成。请参阅其中一个子类的例子，了解 "新 "推荐的样式。这个例子只是为了完整，因为它与 "经典 "独立的 ROS 节点类似。

新推荐样式有两种：

• 第一种：lambda

      #include <chrono>
      #include <memory>
      #include "rclcpp/rclcpp.hpp"
      #include "std_msgs/msg/string.hpp"
      using namespace std::chrono_literals;
      class MinimalPublisher : public rclcpp::Node
      {
      public:
        MinimalPublisher()
        : Node("minimal_publisher"), count_(0)
        {
       publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
      auto timer_callback =
       [this]() -> void {
      auto message = std_msgs::msg::String();
       message.data = "Hello, ros2 world! " + std::to_string(this->count_++);
       RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
      this->publisher_->publish(message);
       };
       timer_ = this->create_wall_timer(500ms, timer_callback);
        }
      private:
        rclcpp::TimerBase::SharedPtr timer_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
       size_t count_;
      };
      int main(int argc, char * argv[])
      {
        rclcpp::init(argc, argv);
        rclcpp::spin(std::make_shared<MinimalPublisher>());
        rclcpp::shutdown();
       return 0;
      }
  
 
 

这其实才是优雅的代码，当然它的问题是（初学者一眼看不懂了！）：

使用了一个精致的C++11 lambda函数来缩短回调语法，但代价是让代码看起来更难理解。

• 第二种：member_function

      #include <chrono>
      #include <memory>
      #include "rclcpp/rclcpp.hpp"
      #include "std_msgs/msg/string.hpp"
      using namespace std::chrono_literals;
      class MinimalPublisher : public rclcpp::Node
      {
      public:
        MinimalPublisher()
        : Node("minimal_publisher"), count_(0)
        {
       publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
       timer_ = this->create_wall_timer(
      500ms, std::bind(&MinimalPublisher::timer_callback, this));
        }
      private:
       void timer_callback()
        {
      auto message = std_msgs::msg::String();
       message.data = "Hello, world! " + std::to_string(count_++);
       RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
       publisher_->publish(message);
        }
        rclcpp::TimerBase::SharedPtr timer_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
       size_t count_;
      };
      int main(int argc, char * argv[])
      {
        rclcpp::init(argc, argv);
        rclcpp::spin(std::make_shared<MinimalPublisher>());
        rclcpp::shutdown();
       return 0;
      }
  
 
 

这段代码并不优雅，但十分规范！！！推荐使用。

使用std::bind()来注册一个成员函数作为定时器的回调。

上述三段代码所实现的功能其实是一样的。

订阅器：

类似ROS1风格，不推荐，不推荐，不推荐！！！

      #include "rclcpp/rclcpp.hpp"
      #include "std_msgs/msg/string.hpp"
      rclcpp::Node::SharedPtr g_node = nullptr;
      /* We do not recommend this style anymore, because composition of multiple
       * nodes in the same executable is not possible. Please see one of the subclass
       * examples for the "new" recommended styles. This example is only included
       * for completeness because it is similar to "classic" standalone ROS nodes. */
      void topic_callback(const std_msgs::msg::String::SharedPtr msg)
      {
        RCLCPP_INFO(g_node->get_logger(), "I heard: '%s'", msg->data.c_str());
      }
      int main(int argc, char * argv[])
      {
        rclcpp::init(argc, argv);
        g_node = rclcpp::Node::make_shared("minimal_subscriber");
       auto subscription =
       g_node->create_subscription<std_msgs::msg::String>("topic", 10, topic_callback);
        rclcpp::spin(g_node);
        rclcpp::shutdown();
        subscription = nullptr;
        g_node = nullptr;
       return 0;
      }
  
 
 

直接看官方注释吧：

我们不再推荐这种样式，因为在同一个可执行文件中不可能有多个节点组成。请参阅其中一个子类的例子，了解 "新 "推荐的样式。这个例子只是为了完整，因为它与 "经典 "独立的 ROS 节点类似。
TODO(clalancette)。

  subscription = nullptr;
  g_node = nullptr;

最好是把这两个nullptr分配都去掉，让destructors来处理，但我们不能，因为https://github.com/eProsima/Fast-RTPS/issues/235 。 一旦这个问题解决了，我们或许应该考虑删除这两个任务。

• 第一种：lambda

      #include <iostream>
      #include <memory>
      #include "rclcpp/rclcpp.hpp"
      #include "std_msgs/msg/string.hpp"
      class MinimalSubscriber : public rclcpp::Node
      {
      public:
        MinimalSubscriber()
        : Node("minimal_subscriber")
        {
       subscription_ = this->create_subscription<std_msgs::msg::String>(
      "topic",
      10,
       [this](std_msgs::msg::String::UniquePtr msg) {
       RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
       });
        }
      private:
        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
      };
      int main(int argc, char * argv[])
      {
        rclcpp::init(argc, argv);
        rclcpp::spin(std::make_shared<MinimalSubscriber>());
        rclcpp::shutdown();
       return 0;
      }
  
 
 

• 第二种：member_function

      #include <memory>
      #include "rclcpp/rclcpp.hpp"
      #include "std_msgs/msg/string.hpp"
      using std::placeholders::_1;
      class MinimalSubscriber : public rclcpp::Node
      {
      public:
        MinimalSubscriber()
        : Node("minimal_subscriber")
        {
       subscription_ = this->create_subscription<std_msgs::msg::String>(
      "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
        }
      private:
       void topic_callback(const std_msgs::msg::String::SharedPtr msg)
        {
       RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
        }
        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
      };
      int main(int argc, char * argv[])
      {
        rclcpp::init(argc, argv);
        rclcpp::spin(std::make_shared<MinimalSubscriber>());
        rclcpp::shutdown();
       return 0;
      }
  
 
 

至此，其实ROS2的发布订阅并未全部讲完，后续还会新开一节再深入讲解。

member_function这种编程方式是官方教程推荐学习的方式。详细代码解析请务必查阅官网和认真阅读源码。

附加题：

• 使用新式编程风格实现turtlesim和mobot，速度发布和位置订阅的代码。

文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。

原文链接：zhangrelay.blog.csdn.net/article/details/106040763