ROS笔记(4)——发布者Publisher与订阅者Subscribe的编程实现
发布者
以小海龟的话题消息为例,编程实现发布者通过/turtle1/cmd_vel 话题向 turtlesim节点发送消息,流程如图
步骤一 创建功能包(工作空间为~/catkin_ws/src)
$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim
步骤二 编写C++代码,如下
#include<ros/ros.h>
#include<geometry_msgs/Twist.h>int main(int argc, char*argv[])
{/* 初始化ros节点 */ros::init(argc,argv,"velovity_publisher");//创建节点句柄ros::NodeHandle n;//创建一个Publisher,发布名为turtle1/cmd_vel 的topic,消息类型为 geometry_msgs ::Twist.h 队列长度为10 ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);ros::Rate loop_rate(10);while(ros::ok()){//初始化消息geometry_msgs::Twist vel_msg;vel_msg.linear.x = 0.5;vel_msg.angular.z = 0.2;//发布消息turtle_vel_pub.publish(vel_msg);//打印日志ROS_INFO("velocity_publisher : msg [%0.2f m/s,%0.2f rad/s]",vel_msg.linear.x,vel_msg.angular.z);loop_rate.sleep();}return 0;
}
补充项:使用vscode编写C++代码,ROS的头文件引用问题
解决方案如下:
打开您的VS Code项目或工作空间。
在菜单栏中,选择“查看”(View) -> “命令面板”(Command Palette)。
在搜索框中输入“C++: Edit Configuration”,并选择“C++: Edit Configurations (UI)”选项。
在这个UI界面中,您需要添加以下两个路径:
在“编译”(Compile)标签页下,选择“高级”(Advanced)选项。
在“includePath”中添加ROS的include文件夹的路径,如
/opt/ros/<ROS_VERSION>/include
。在“browse.path”中添加ROS的lib文件夹的路径,如
/opt/ros/<ROS_VERSION>/lib
。单击“确定”(OK)保存您的更改。
步骤三 配置CMakeLists.txt
add_executable(velocity_publisher src/velocity_publisher.cpp)
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})
步骤四 编译运行
$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_topic velocity_publisher
此时,小海龟接受到消息作圆周运动。
订阅者
订阅者编译与运行方式同上,以订阅/turtle1/pose topic 获取小海龟坐标为例,代码如下
#include<ros/ros.h>
#include"turtlesim/Pose.h"void poseCallback(const turtlesim::Pose::ConstPtr &msg)
{ROS_INFO("pose:x %0.6f, y %0.6f",msg->x,msg->y);
}int main(int argc, char *argv[])
{/* code *///初始化ros节点ros::init(argc,argv,"pose_subscriber");//创建节点句柄ros::NodeHandle n;//创建一个订阅者,订阅名为 /turtle1/pose 的 topic ros::Subscriber pose_sub =n.subscribe("/turtle1/pose",10,poseCallback);//阻塞ros::spin();return 0;
}
附录:roscpp C++官方文档 roscpp: roscpp
rospy Python官网文档 http://docs.ros.org/en/melodic/api/rospy/html/
python对应写法
发布者
# 该例程将发布turtle1/cmd_vel话题,消息类型geometry_msgs::Twistimport rospy
from geometry_msgs.msg import Twistdef velocity_publisher():# ROS节点初始化rospy.init_node('velocity_publisher', anonymous=True)# 创建一个Publisher,发布名为/turtle1/cmd_vel的topic,消息类型为geometry_msgs::Twist,队列长度10turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)#设置循环的频率rate = rospy.Rate(10) while not rospy.is_shutdown():# 初始化geometry_msgs::Twist类型的消息vel_msg = Twist()vel_msg.linear.x = 0.5vel_msg.angular.z = 0.2# 发布消息turtle_vel_pub.publish(vel_msg)rospy.loginfo("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z)# 按照循环频率延时rate.sleep()if __name__ == '__main__':try:velocity_publisher()except rospy.ROSInterruptException:pass
订阅者
# 该例程将订阅/turtle1/pose话题,消息类型turtlesim::Poseimport rospy
from turtlesim.msg import Posedef poseCallback(msg):rospy.loginfo("Turtle pose: x:%0.6f, y:%0.6f", msg.x, msg.y)def pose_subscriber():# ROS节点初始化rospy.init_node('pose_subscriber', anonymous=True)# 创建一个Subscriber,订阅名为/turtle1/pose的topic,注册回调函数poseCallbackrospy.Subscriber("/turtle1/pose", Pose, poseCallback)# 循环等待回调函数rospy.spin()if __name__ == '__main__':pose_subscriber()