专栏首页机器人课程与技术使用机器人操作系统ROS 2和仿真软件Gazebo 9主题进阶实战(七)- mobot速度发布与里程计订阅

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

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

这过程包括:

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

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

实现效果参考如下视频:

ROS2和Gazebo9中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,速度发布和位置订阅的代码。

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • 使用机器人操作系统ROS 2和仿真软件Gazebo 9服务进阶实战(八)- mobot行驶至目标位置

    案例非常简单,稍微复杂一下,如上述机器人案例,发送目标坐标,机器人行驶到目标坐标。

    zhangrelay
  • 使用机器人操作系统ROS 2和仿真软件Gazebo 9行动进阶实战(九)- mobot区域巡逻

    行动(action)比服务更为灵活和复杂。在给出行动具体说明之前,先简要复习一下:

    zhangrelay
  • ESP8266和ROS调试一些问题汇总

    rosrun rosserial_arduino serial_node.py _port:=/dev/ttyUSB0

    zhangrelay
  • 自定义相机中如何实现二维码扫描功能

    Android平台中要实现二维码扫描功能的话,最常用的开源库要推zxing和zbar了。不过zbar已经好几年没有更新了,而zxing由Google开源并持续维...

    雪月清
  • MyBatis:基本应用

    软件开发常用的架构是三层架构,之所以流行是因为有着清晰的任务划分。一般包括以下三层:

    RendaZhang
  • 认识html元素

    前端现在越来越火,可以用去年热门的词语来形容——“风口上的猪”。希望这个系列的文集能够给“毫无任何基础,但是想转到前端的人”一点帮助。 认识一个html文档的基...

    用户1667431
  • salesforce 零基础学习(三十六)通过Process Builder以及Apex代码实现锁定记录( Lock Record)

    上一篇内容是通过Process Builder和Approval Processes实现锁定记录的功能,有的时候,往往锁定一条记录需要很多的限制条件,如果通过A...

    用户1169343
  • 12.9 Nginx域名重定向

    Nginx域名重定向目录概要 更改test.com.conf server { listen 80; server_name test.com ...

    运维小白
  • 漫谈大数据平台安全风险与建设

    上篇文章《漫谈大数据平台架构》(阅读原文查看)大家应该对大数据平台有了一个整体架构上的理解和认识,作为姊妹篇,本篇着重讲解大数据平台安全风险与建设。

    FB客服
  • 我们已慢慢变成黑客帝国的插管人类

    当时人类很信任一位智者,这位智者建了一个图书馆,将整个星球的信息都整理到这个图书馆里,并把图书馆的钥匙复制很多份,交到星球的每一个住民手里,这把钥匙叫做 - 搜...

    挖数

扫码关注云+社区

领取腾讯云代金券