ROS实践笔记10

ROS通信机制实操

案例一

需求描述：编码实现乌龟运动控制，使乌龟做圆周运动。

实现分析：

1. 乌龟运动控制实现，关键节点有两个，一个是乌龟运动显示节点 turtlesim_node，另一个是控制 节点，二者是订阅发布模式实现通信的，乌龟运动显示节点直接调用即可，运动控制节点之前是使用的 turtle_teleop_key通过键盘 控制，现在需要自定义控制节点。
2. 控制节点自实现时，首先需要了解控制节点与显示节点通信使用的话题与消息，可以使用ros命令结合计算图来获取。
3. 了解了话题与消息之后，通过 C++ 或 Python 编写运动控制节点，通过指定的话题，按照一定的逻辑发布消息即可。

实现流程：

1. 通过计算图结合ros命令获取话题与消息信息。
2. 编码实现运动控制节点。
3. 启动 roscore、turtlesim_node 以及自定义的控制节点，查看运行结果。

具体实现：

1. 打开三个终端运行乌龟指令

roscore

rosrun turtlesim turtlesim_node

rosrun turtlesim turtle_teleop_key 

2. 打开新终端运行计算图

rqt_graph

从运算图中得到键盘控制节点名"teleop_turtle"，乌龟图像节点名"turtlesim"，以及话题名称"turtle1/cmd_vel"

3. 通过命令

rostopic info /turtle1/cmd_vel
//或者
rostopic type /turtle1/cmd_vel

获取到话题的消息载体类型：

geometry_msgs/Twist

4. 使用rosmsg命令获取消息载体的格式

rosmsg info geometry_msgs/Twist
//或者
rosmsg show geometry_msgs/Twist

格式为：

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
//线速度，x表示前进和后退的速度。
//y表示左右运动的速度
//z表示垂直于x，y平面向上或者向下的速度
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z
//角速度，单位是rad/s
//以客机为例
//以机身为为x轴，机翼为y轴，垂直于xy平面且于x轴，y相交为z轴
//以x轴为转轴转动称为翻滚，
//以y轴为转轴转动称为俯仰，
//以z轴为转轴转动称为偏航，
//不排除yz反了

c++实现案例一

# include"ros/ros.h"
# include"geometry_msgs/Twist.h"

int main(int argc, char*argv[])
{
    ros::init(argc,argv,"key_op");
    ros::NodeHandle nh;
    ros::Publisher pub=nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    geometry_msgs::Twist speed;
    speed.linear.x=1.0;
    speed.angular.z=0.5;
    ros::Rate rate(10);
    ros::Duration(3).sleep();
    int i=1000;
    while(i)
    {
        pub.publish(speed);
        i--;
        rate.sleep();
    }
    ros::spinOnce();
    return 0;
}

python实现案例一

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist

if __name__=="__main__" :
    rospy.init_node("key_op_p")
    pub=rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=10)
    rate = rospy.Rate(10)
    twist = Twist()
    twist.linear.x=1.0
    twist.angular.z=0.7
    rospy.sleep(3)
    while not rospy.is_shutdown() :
        pub.publish(twist)
        rate.sleep()

案例二

需求描述: 已知turtlesim中的乌龟显示节点，会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向)，要求控制乌龟运动，并时时打印当前乌龟的位姿。

实现分析:

1. 首先，需要启动乌龟显示以及运动控制节点并控制乌龟运动。
2. 要通过ROS命令，来获取乌龟位姿发布的话题以及消息。
3. 编写订阅节点，订阅并打印乌龟的位姿。

实现流程:

1. 首先，需要启动乌龟显示以及运动控制节点并控制乌龟运动。
2. 要通过ROS命令，来获取乌龟位姿发布的话题以及消息。
3. 编写订阅节点，订阅并打印乌龟的位姿。

具体实现：

1. 获取话题:/turtle1/pose

rostopic list

2. 获取消息类型:turtlesim/Pose

rostopic type  /turtle1/pose

3. 获取消息格式:

rosmsg info turtlesim/Pose

​float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

4. 实现订阅节点

创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim

c++案例二实现：

# include"ros/ros.h"
# include"turtlesim/Pose.h"


void out_pose(const turtlesim::Pose::ConstPtr &pose)
{
    ROS_INFO("当前乌龟的位姿信息为:");
    ROS_INFO("x = %.2f",pose->x);
    ROS_INFO("y = %.2f",pose->y);
    ROS_INFO("theta = %.2f",pose->theta);//朝向
    ROS_INFO("linear_velocity = %.2f",pose->linear_velocity);
    ROS_INFO("angular_velocity = %.2f",pose->angular_velocity);
}
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"pose_c");
    ros::NodeHandle nh;
    ros::Rate rate(10);
    ros::Subscriber sub=nh.subscribe("/turtle1/pose",10,out_pose);
    rate.sleep();
    ros::spin();
    return 0;
}

python案例二实现

#! /usr/bin/env python

import rospy

from turtlesim.msg import Pose

def out_pose(pose):
    rospy.loginfo("x = %.2f",pose.x)
    rospy.loginfo("y = %.2f",pose.y)
    rospy.loginfo("theta = %.2f",pose.theta)
    rospy.loginfo("linear_velocity = %.2f",pose.linear_velocity)
    rospy.loginfo("angular_velocity = %.2f",pose.angular_velocity)
if __name__ == "__main__":
    rospy.init_node("pose_p")
    sub = rospy.Subscriber("/turtle1/pose",Pose,out_pose,queue_size = 10)
    rospy.spin()

案例三

需求描述:编码实现向 turtlesim 发送请求，在乌龟显示节点的窗体指定位置生成一乌龟，这是一个服务请求操作。

实现分析:

1. 首先，需要启动乌龟显示节点。
2. 要通过ROS命令，来获取乌龟生成服务的服务名称以及服务消息类型。
3. 编写服务请求节点，生成新的乌龟。

实现流程:

1. 通过ros命令获取服务与服务消息信息。
2. 编码实现服务请求节点。
3. 启动 roscore、turtlesim_node 、乌龟生成节点，生成新的乌龟。

具体实现：

1. 获取话题:/spawn

rosservice list

2. 获取消息类型:turtlesim/Spawn

rosservice type /spawn

3. 获取消息格式:

rossrv info turtlesim/Spawn

得到

float32 x
float32 y
float32 theta
string name
---
string name

c++实现

# include"ros/ros.h"
# include"turtlesim/Spawn.h"

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"service_c");
    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn spa;
    spa.request.name="qqqqqqqq";
    //每一只乌龟的名字都是唯一的
    spa.request.theta=3.14;
    spa.request.x=1.3;
    spa.request.y=1.7;
    client.waitForExistence();
    if(client.call(spa))    ROS_INFO("YES");
    else    ROS_INFO("NO");
    return 0;
}

python实现

#! /usr/bin/env python

import rospy
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse

if __name__ == "__main__":
    rospy.init_node("service_p")
    client = rospy.ServiceProxy("/spawn",Spawn)
    client.wait_for_service()
    spa=SpawnRequest()
    spa.x=1.7
    spa.y=3.0
    spa.theta=3.00
    spa.name="ti111me"
    try:
        client.call(spa)
        rospy.loginfo("YES")
    except Exception as e:
        rospy.loginfo("NO")

案例四

需求描述: 修改turtlesim乌龟显示节点窗体的背景色，已知背景色是通过参数服务器的方式以 RGB 方式设置的。

实现分析:

1. 首先，需要启动乌龟显示节点。
2. 要通过ROS命令，来获取参数服务器中设置背景色的参数。
3. 编写参数设置节点，修改参数服务器中的参数值。

实现流程:

1. 通过ros命令获取参数。
2. 编码实现服参数设置节点。
3. 启动 roscore、turtlesim_node 与参数设置节点，查看运行结果。

具体实现：

获取参数列表:

rosparam list

得到

/turtlesim/background_b
/turtlesim/background_g
/turtlesim/background_r

c++实现

# include"ros/ros.h"

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"param_c");
    ros::NodeHandle nh;
    nh.setParam("/turtlesim/background_b",69);
    nh.setParam("/turtlesim/background_g",0);
    nh.setParam("/turtlesim/background_r",0);
    return 0;
}

python实现

#! /usr/bin/env python

import rospy

if __name__=="__main__":
    rospy.init_node("param_p")
    rospy.set_param("/turtlesim/background_r",99)
    rospy.set_param("/turtlesim/background_g",0)
    rospy.set_param("/turtlesim/background_b",0)

注意：重启roscore会重置参数