八、话题通信之话题的替代与监听

• 1、简介
• 2、发布方实现步骤
• • 1.话题获取
  • 2.消息获取
  • 3.获取消息格式
  • 4.代码实现
• 3、订阅方实现步骤
• • 1.获取话题和消息
  • 2.更改配置文件
  • 3.代码实现

在我们运行小乌龟案例的时候，我们可以通过方向键控制其运行，那我们怎么通过自己的代码来控制其运行呢。方法其实很简单，由于小乌龟的案例是通过话题通信的方式，那么我们就可以通过相关命令获取该通信的话题，话题所包含的消息类型以及详细信息，然后通过代码设置相应的话题即可。

1. 通过计算图
   rqt_graph
2. 通过rostopic list
   返回结果：

   rostopic list
   /rosout
   /rosout_agg
   /statistics
   /turtle1/cmd_vel
   /turtle1/color_sensor
   /turtle1/pose
   

• rostopic type 节点名/话题名
• 返回结果：

  rostopic type /turtle1/cmd_vel 
  geometry_msgs/Twist
  

• 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轴旋转

1. C++

   //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("/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;
   }
   

2. 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()
   

• 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
  

在该项目中，由于使用了turtlesim功能包，所以需要添加该功能包依赖，修改CMakeLists.txt文件：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  turtlesim
)

1. C++

   #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("/turtle1/pose",1000,doPose);
       ros::spin();
       return 0;
   }
   

2. 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()