申明:ROS学习参考了古月居老师的Blibli视频,强烈推荐大家看视频学习,本博客仅记录自己的学习经历和心得,欢迎大家一起讨论!
对比上面两个话题模型可以发现,turtlesim::Pose话题是ros本身定义好的,如果这些话题模型不满足我们的需求,就需要自己重新定义话题模型,如上图中的learning_topic::Person.
话题消息如何自定义呢?
string name
uint8 sex
uint8 age
.
.
uint8 unknown = 0 #宏定义
uint8 male = 1
uint8 female = 2
message_generation
message_runtime
find_package(… message_generation)
.
add_message_files(FILES Person.msg)
generate_messages(DEPENDENCIES std_msgs)
.
catkin_package(…message_runtime)
在learning_topic软件包下创建msg文件夹(命名不可修改)
创建Person .msg文件,里面进行数据接口定义
string name
uint8 sex
uint8 age
uint8 unknown = 0 #宏定义
uint8 male = 1
uint8 female = 2
<build_depend>message_generationbuild_depend> #动态生成程序(编译依赖)
<exec_depend>message_runtimeexec_depend> #运行依赖
find_package(… message_generation)#将刚才的编译功能包添加进去
add_message_files(FILES Person.msg) #将 .msg文件编译成对应的不同的程序文件
generate_messages(DEPENDENCIES std_msgs)
catkin_package(…message_runtime) #将编译依赖放入
cd ~/catkin_ws
catkin_make
具体的过程前两讲已经详细介绍过,本讲重点比较代码区别:
(想要详细了解的可以跳转——ROS学习(二)——发布者Publisher的编程实现)
或 ROS学习(三)——订阅者Subscriber编程实现
/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/
/**
* 该例程将发布/person_info话题,自定义消息类型learning_topic::Person
*/
#include
//改为添加对应的头文件
#include "learning_topic/Person.h" //#include
int main(int argc, char **argv)
{
// ROS节点初始化,定义新的节点名字
ros::init(argc, argv, "person_publisher");
//ros::init(argc,argv,"velocity_publisher");
// 创建节点句柄
ros::NodeHandle n;
// 创建一个Publisher,发布名为/person_info的topic,消息类型为learning_topic::Person,队列长度10
ros::Publisher person_info_pub = n.advertise<learning_topic::Person>("/person_info", 10);
//ros::Publisher turtle_vel_pub = n.advertise("/turtle1/cmd_vel",10);
// 设置循环的频率,频率为1Hz
ros::Rate loop_rate(1);
int count = 0;
while (ros::ok())
{
// 初始化learning_topic::Person类型的消息
learning_topic::Person person_msg;
person_msg.name = "Tom";
person_msg.age = 18;
person_msg.sex = learning_topic::Person::male;//宏调用
/*
geometry_msgs::Twist vel_msg;
vel_msg.linear.x=2.0;
vel_msg.angular.z=1.0;
*/
// 发布消息
person_info_pub.publish(person_msg);
//turtle_vel_pub.publish(vel_msg);
ROS_INFO("Publish Person Info: name:%s age:%d sex:%d",
person_msg.name.c_str(), person_msg.age, person_msg.sex);
//注意:字符串形式的输出:.c_str()
// 按照循环频率延时
loop_rate.sleep();
}
return 0;
}
add_executable(person_publisher src/person_publisher.cpp)
target_link_libraries(person_publisher ${catkin_LIBRARIES})
add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)
#!/usr/bin/env python
# -*- coding: utf-8 -*-
########################################################################
#### Copyright 2020 GuYueHome (www.guyuehome.com). ###
########################################################################
# 该例程将发布/person_info话题,自定义消息类型learning_topic::Person
## 该例程将发布turtle1/cmd_vel话题,消息类型geometry_msgs::Twist
import rospy
from learning_topic.msg import Person
# from geometry_msgs.msg import Twist
def person_publisher():
# ROS节点初始化
rospy.init_node('person_publisher', anonymous=True)
#rospy.init_node('velocity_publisher', anonymous=True)
# 创建一个Publisher,发布名为/person_info的topic,消息类型为learning_topic::Person,队列长度10
person_info_pub = rospy.Publisher('/person_info', Person, queue_size=10)
## 创建一个Publisher,发布名为/turtle1/cmd_vel的topic,消息类型为geometry_msgs::Twist,队列长度10
#turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
#设置循环的频率
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# 初始化learning_topic::Person类型的消息
person_msg = Person()
person_msg.name = "Tom";
person_msg.age = 18;
person_msg.sex = Person.male;
# 初始化geometry_msgs::Twist类型的消息
#vel_msg = Twist()
#vel_msg.linear.x = 0.5
#vel_msg.angular.z = 0.2
# 发布消息
person_info_pub.publish(person_msg)
rospy.loginfo("Publsh person message[%s, %d, %d]",
person_msg.name, person_msg.age, person_msg.sex)
# 按照循环频率延时
rate.sleep()
if __name__ == '__main__':
try:
person_publisher()
except rospy.ROSInterruptException:
pass
/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/
/**
* 该例程将订阅/person_info话题,自定义消息类型learning_topic::Person
*/
#include
#include "learning_topic/Person.h"
//#include
// 接收到订阅的消息后,会进入消息回调函数
void personInfoCallback(const learning_topic::Person::ConstPtr& msg)
//void poseCallback(const turtlesim::Pose::ConstPtr& msg)//指针
{
// 将接收到的消息打印出来
ROS_INFO("Subcribe Person Info: name:%s age:%d sex:%d",
msg->name.c_str(), msg->age, msg->sex);
//ROS_INFO("Turtle pose: x:%0.6f,y:%0.6f",msg->x,msg->y);
}
int main(int argc, char **argv)
{
// 初始化ROS节点
ros::init(argc, argv, "person_subscriber");
//ros::init(argc,argv,"pose_subscriber");
// 创建节点句柄
ros::NodeHandle n;
// 创建一个Subscriber,订阅名为/person_info的topic,注册回调函数personInfoCallback
ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);
//ros::Subscriber pose_sub=n.subscribe("/turtle1/pose",10,poseCallback);
// 循环等待回调函数
ros::spin();
return 0;
}
add_executable(person_subscriber src/person_subscriber.cpp)
target_link_libraries(person_subscriber ${catkin_LIBRARIES})
add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)
#!/usr/bin/env python
# -*- coding: utf-8 -*-
########################################################################
#### Copyright 2020 GuYueHome (www.guyuehome.com). ###
########################################################################
# 该例程将订阅/person_info话题,自定义消息类型learning_topic::Person
import rospy
from learning_topic.msg import Person
#from turtlesim.msg import Pose
def personInfoCallback(msg):
rospy.loginfo("Subcribe Person Info: name:%s age:%d sex:%d",
msg.name, msg.age, msg.sex)
#def poseCallback(msg):
#rospy.loginfo("Turtle pose: x:%0.6f, y:%0.6f", msg.x, msg.y)
def person_subscriber():
# ROS节点初始化
rospy.init_node('person_subscriber', anonymous=True)
# 创建一个Subscriber,订阅名为/person_info的topic,注册回调函数personInfoCallback
rospy.Subscriber("/person_info", Person, personInfoCallback)
#rospy.Subscriber("/turtle1/pose", Pose, poseCallback)
# 循环等待回调函数
rospy.spin()
if __name__ == '__main__':
person_subscriber()
roscore
rosrun learning_topic person_publisher
rosrun learning_topic person_subscriber
注意:python程序运行时,直接写person_publisher.py或者_subscriber.py
对比结论,在不同的Publisher和Subscriber文件中,主要的区别就在于消息类型和话题不同,剩下的格式几乎相同,还需多加练习!!
本讲完,接下来会持续更新!