新建工程
mkdir -p ros2_ws/src
cd ros2_ws/src
在src中添加程序并新建发布者文件
ros2 pkg create example_topic_rclcpp --build-type ament_cmake --dependencies rclcpp
touch example_topic_rclcpp/src/topic_publisher_01.cpp
打开新建的topic_publisher_01.cpp进行编辑
cd example_topic_rclcpp/src
gedit topic_publisher_01.cpp
将下面的一段代码复制进去
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
class TopicPublisher01 : public rclcpp::Node
{
public:
// 构造函数,有一个参数为节点名称
TopicPublisher01(std::string name) : Node(name)
{
RCLCPP_INFO(this->get_logger(), "大家好,我是%s.", name.c_str());
// 创建发布者
command_publisher_ = this->create_publisher<std_msgs::msg::String>("command", 10);
// 创建定时器,500ms为周期,定时发布
timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&TopicPublisher01::timer_callback, this));
}
private:
void timer_callback()
{
// 创建消息
std_msgs::msg::String message;
message.data = "forward";
// 日志打印
RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
// 发布消息
command_publisher_->publish(message);
}
// 声名定时器指针
rclcpp::TimerBase::SharedPtr timer_;
// 声明话题发布者指针
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr command_publisher_;
};
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
/*产生一个的节点*/
auto node = std::make_shared<TopicPublisher01>("topic_publisher_01");
/* 运行节点,并检测退出信号*/
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
下边新建订阅者文件
touch topic_subscribe_01.cpp
打开topic_subscribe_01.cpp并编辑
gedit topic_subscribe_01.cpp
复制以下代码进去
#include "std_msgs/msg/string.hpp"
#include <serial/serial.h>
class TopicSubscribe01 : public rclcpp::Node
{
public:
TopicSubscribe01(std::string name) : Node(name)
{
RCLCPP_INFO(this->get_logger(), "大家好,我是%s.", name.c_str());
// 创建一个订阅者订阅话题
command_subscribe_ = this->create_subscription<std_msgs::msg::String>("command", 10, std::bind(&TopicSubscribe01::command_callback, this, std::placeholders::_1));
}
private:
// 声明一个订阅者
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr command_subscribe_;
// 收到话题数据的回调函数
void command_callback(const std_msgs::msg::String::SharedPtr msg)
{
double speed = 0.0f;
if(msg->data == "forward")
{
speed = 0.2f;
}
RCLCPP_INFO(this->get_logger(), "收到[%s]指令,发送速度 %f", msg->data.c_str(),speed);
};
};
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
/*产生一个的节点*/
auto node = std::make_shared<TopicSubscribe01>("topic_subscribe_01");
/* 运行节点,并检测退出信号*/
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
两个文件写好之后,对CMakelists.txt和package.xml进行修改
首先对CMakelists.txt进行修改
将下面两段添加到CMakelist.txt最后
add_executable(topic_publisher_01 src/topic_publisher_01.cpp)
ament_target_dependencies(topic_publisher_01 rclcpp std_msgs)
install(TARGETS
topic_publisher_01
DESTINATION lib/${PROJECT_NAME}
)
add_executable(topic_subscribe_01 src/topic_subscribe_01.cpp)
ament_target_dependencies(topic_subscribe_01 rclcpp std_msgs)
install(TARGETS
topic_subscribe_01
DESTINATION lib/${PROJECT_NAME}
)
将下面两行代码
<depend>rclcpp</depend>
<depend>std_msgs</depend>
添加到下图所示位置
<buildtool_depend>ament_cmake</buildtool_depend>
(插入代码)
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
最后效果如图
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
修改完之后就可以进行编译了。
在ros2_ws下打开终端
colcon build
source install/setup.bash
先打开订阅者
ros2 run example_topic_rclcpp topic_subscribe_01
重新打开一个终端,打开发布者
source install/setup.bash
ros2 run example_topic_rclcpp topic_publisher_01
最终效果如图
ros2订阅发布到此结束。