在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这种编程方式是官方教程推荐学习的方式。详细代码解析请务必查阅官网和认真阅读源码。