1、简介
在我们运行小乌龟案例的时候,我们可以通过方向键控制其运行,那我们怎么通过自己的代码来控制其运行呢。方法其实很简单,由于小乌龟的案例是通过话题通信的方式,那么我们就可以通过相关命令获取该通信的话题,话题所包含的消息类型以及详细信息,然后通过代码设置相应的话题即可。
2、发布方实现步骤
1.话题获取
- 通过计算图
rqt_graph
- 通过
rostopic list
返回结果:rostopic list /rosout /rosout_agg /statistics /turtle1/cmd_vel /turtle1/color_sensor /turtle1/pose
2.消息获取
- rostopic type 节点名/话题名
- 返回结果:
rostopic type /turtle1/cmd_vel geometry_msgs/Twist
3.获取消息格式
- rosmsg info 消息名
- 返回结果:
rosmsg info geometry_msgs/Twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z
其中,linear代表线速度,angular代表角速度,单位为弧度。在本项目中,线速度只有X方向,角速度只有偏航角速度。
- 1弧度:弧长等于半径时所对应的角度为1弧度
- 三个角度:
- 翻滚角:绕X轴旋转
- 俯仰角:绕Y轴旋转
- 偏航角:绕Z轴旋转
4.代码实现
- C++
/* 目标:通过节点替换,控制小乌龟运动 节点替换是通过话题相应的话题进行替换,所以需要提前知道: 1.话题名:/turtle1/cmd_vel rqt_graph rostopic list 2.话题的消息类型:geometry_msgs/Twist rostopic type 话题名 3.话题消息的具体信息: geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z rosmsg info 消息名 */ //1.包含头文件 #include"ros/ros.h" //包含具有相应话题消息类型的头文件 #include"geometry_msgs/Twist.h" int main(int argc, char *argv[]){ setlocale(LC_ALL,""); //2.初始化ros节点 ros::init(argc,argv,"turtle_control"); ros::NodeHandle nh; //3.创建发布者对象 ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1000); //4.发布控制信息 geometry_msgs::Twist msg; msg.linear.x = 1.0; msg.linear.y = 0.0; msg.linear.z = 0.0; msg.angular.x = 0.0; msg.angular.y = 0.0; msg.angular.z = 1.0; ros::Rate r(1); while(ros::ok()){ pub.publish(msg); ros::spinOnce(); } return 0; }
- Python
#! /usr/bin/env python from re import T from turtle import pu import rospy from geometry_msgs.msg import Twist if __name__ == "__main__": #初始化ros节点 rospy.init_node("turtle_control_p") #创建发布者对象 pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=1000) #发布信息 rate = rospy.Rate(10) msg = Twist() msg.linear.x = 1.0 msg.linear.y = 0.0 msg.linear.z = 0.0 msg.angular.x = 0.0 msg.angular.y = 0.0 msg.angular.z = 1.0 #循环发布 rospy.is_shutdown():判断节点是否关闭 while not rospy.is_shutdown(): pub.publish(msg) rate.sleep()
3、订阅方实现步骤
1.获取话题和消息
- rostopic list
- 返回结果:
rostopic list /rosout /rosout_agg /turtle1/cmd_vel /turtle1/color_sensor /turtle1/pose d102@d102-W65KJ1-KK1:/media/d102/EPAN/Desktop/code_study_ubuntu/rosdemo_05$ rostopic type /turtle1/pose turtlesim/Pose d102@d102-W65KJ1-KK1:/media/d102/EPAN/Desktop/code_study_ubuntu/rosdemo_05$ rosmsg info turtlesim/Pose float32 x float32 y float32 theta float32 linear_velocity float32 angular_velocity
2.更改配置文件
在该项目中,由于使用了turtlesim功能包,所以需要添加该功能包依赖,修改CMakeLists.txt文件:
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
turtlesim
)
3.代码实现
- C++
/* 话题:/turtle1/pose 消息类型:turtlesim/Pose */ #include"ros/ros.h" #include"turtlesim/Pose.h" void doPose(const turtlesim::Pose::ConstPtr &p){ ROS_INFO("乌龟位姿信息:x=%.2f,y=%.2f,theta=%.2f,lv=%.2f,av=%.2f", p->x,p->y,p->theta,p->linear_velocity,p->angular_velocity); } int main(int argc, char *argv[]){ setlocale(LC_ALL,""); ros::init(argc,argv,"turtle_pose_sub"); ros::NodeHandle nh; ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose); ros::spin(); return 0; }
- Python
#! /usr/bin/env python
"""
目标:获取小乌龟位置,并打印出来
话题:/turtle1/pose
消息类型:turtlesim/Pose
运行前准备:turtle_start.launch
"""
import rospy
from turtlesim.msg import Pose
def doPose(data1):#data1为形参
rospy.loginfo("x=%.2f,y=%.2f,theta=%.2f",data1.x,data1.y,data1.theta)
if __name__ == "__main__":
rospy.init_node("turtle_pose_p")
sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=1000)
rospy.spin()