[ROS Study Notes 8] Practical Operation of Communication Mechanism

[ROS Study Notes 8] Practical Operation of Communication Mechanism

Written in the front, this series of notes refers to the tutorial of AutoLabor, the specific project address is here

---

foreword

Realize the motion control of the turtle through coding, let the little turtle make a circular motion, as shown below:

1. Topic release

Requirement description: Code to realize the motion control of the turtle, let the little turtle do circular motion.

Realize analysis:

1. Turtle motion control implementation, there are two key nodes, one is the turtle motion display node turtlesim_node, and the other is the control node, the two are in the subscription release mode to achieve communication, the turtle motion display node can be called directly, the motion control node was used before The turtle_teleop_key is controlled via the keyboard and now requires a custom control node.
2. When the control node is self-implemented, it is first necessary to understand the topics and messages used in the communication between the control node and the display node, which can be obtained by using the ros command combined with the calculation graph.
3. After understanding the topic and the message, write the motion control node through C++ or Python, and publish the message according to a certain logic through the specified topic.

Implementation process:

1. Obtain topic and message information through the calculation graph combined with the ros command.
2. Coding implements motion control nodes.
3. Start roscore, turtlesim_node and the custom control node to view the running results.

0. Turtle control demo

Respectively enter the terminal

rosrun turtlesim turtlesim_node
rosrun turtlesim turtle_teleop_key

1. Obtaining topics and news

Preparation: First start the keyboard to control the turtle movement case

rostopic listGet topics by listing them:/turtle1/cmd_vel

rostopic list

Then get the message and get the message type:geometry_msgs/Twist

rostopic info /turtle1/cmd_vel

The result is as follows:

2. Realize the release node

Implementation of Cpp

/*
    编写 ROS 节点，控制小乌龟画圆

    准备工作:
        1.获取topic(已知: /turtle1/cmd_vel)
        2.获取消息类型(已知: geometry_msgs/Twist)
        3.运行前，注意先启动 turtlesim_node 节点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建发布者对象
        4.循环发布运动控制消息
*/

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

int main(int argc, char *argv[])
{
    
    
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"control");
    ros::NodeHandle nh;
    // 3.创建发布者对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1000);
    // 4.循环发布运动控制消息
    //4-1.组织消息
    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 = 2.0;

    //4-2.设置发送频率
    ros::Rate r(10);
    //4-3.循环发送
    while (ros::ok())
    {
    
    
        pub.publish(msg);

        ros::spinOnce();
    }


    return 0;
}

Implementation of Python

#! /usr/bin/env python
"""
    编写 ROS 节点，控制小乌龟画圆

    准备工作:
        1.获取topic(已知: /turtle1/cmd_vel)
        2.获取消息类型(已知: geometry_msgs/Twist)
        3.运行前，注意先启动 turtlesim_node 节点

    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建发布者对象
        4.循环发布运动控制消息

"""

import rospy
from geometry_msgs.msg import Twist

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("control_circle_p")
    # 3.创建发布者对象
    pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=1000)
    # 4.循环发布运动控制消息
    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 = 0.5

    while not rospy.is_shutdown():
        pub.publish(msg)
        rate.sleep()

3. Run

1. start roscore
2. Start the turtle display node
3. Execute Motion Control Node

---

2. Topic Subscription

Requirement description: It is known that the turtle display node in turtlesim will publish the current pose of the turtle (the coordinates and orientation of the turtle in the form), and it is required to control the movement of the turtle, and print the current pose of the turtle from time to time.

The effect is as follows:

Realize analysis:

1. First, you need to start the turtle display and motion control nodes and control the turtle movement.
2. To get the topic and news of the turtle pose through the ROS command.
3. Write a subscription node, subscribe and print the pose of the turtle.

Implementation process:

1. Obtain topic and message information through the ros command.
2. Code to realize the pose acquisition node.
3. Start roscore, turtlesim_node, control node and pose subscription node to control the turtle's movement and output the pose of the turtle.

1. Obtaining topics and news

Get topic:/turtle1/pose

rostopic list

Get message type:turtlesim/Pose

rostopic type /turtle1/pose

Get message format:

rosmsg info turtlesim/Pose

Response result:

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

2. Realize the subscription node

The function packages that need to be relied upon to create a function package:roscpp rospy std_msgs turtlesim

Cpp implementation:

/*  
    订阅小乌龟的位姿: 时时获取小乌龟在窗体中的坐标并打印
    准备工作:
        1.获取话题名称 /turtle1/pose
        2.获取消息类型 turtlesim/Pose
        3.运行前启动 turtlesim_node 与 turtle_teleop_key 节点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建订阅者对象
        5.回调函数处理订阅的数据
        6.spin
*/

#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,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"sub_pose");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    // 5.回调函数处理订阅的数据
    // 6.spin
    ros::spin();
    return 0;
}

Python implementation:

#! /usr/bin/env python
"""
    订阅小乌龟的位姿: 时时获取小乌龟在窗体中的坐标并打印
    准备工作:
        1.获取话题名称 /turtle1/pose
        2.获取消息类型 turtlesim/Pose
        3.运行前启动 turtlesim_node 与 turtle_teleop_key 节点

    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建订阅者对象
        4.回调函数处理订阅的数据
        5.spin

"""

import rospy
from turtlesim.msg import Pose

def doPose(data):
    rospy.loginfo("乌龟坐标:x=%.2f, y=%.2f,theta=%.2f",data.x,data.y,data.theta)

if __name__ == "__main__":

    # 2.初始化 ROS 节点
    rospy.init_node("sub_pose_p")

    # 3.创建订阅者对象
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=1000)
    #     4.回调函数处理订阅的数据
    #     5.spin
    rospy.spin()

3. run

First, start roscore;

Then start the turtle display node and execute the motion control node;

Finally, start the turtle pose subscription node;

The final execution result is similar to the demonstration result.

---

3. Service call

**Description of requirements: **Coding implementation to send a request to turtlesim, and generate a turtle at the specified position of the form of the turtle display node, which is a service request operation.

The effect is as follows:

Realize analysis:

1. First, the turtle display node needs to be started.
2. To obtain the service name and service message type of the turtle-generated service through the ROS command.
3. Write a service request node and generate a new turtle.

Implementation process:

1. Use the ros command to obtain service and service message information.
2. Coding implements the service request node.
3. Start roscore, turtlesim_node, and turtle generation nodes to generate new turtles.

1. Service name and service message acquisition

Get topic:/spawn

rosservice list

Get message type:turtlesim/Spawn

rosservice type /spawn

Get message format:

rossrv info turtlesim/Spawn

Response result:

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

2. Server implementation

Create a function package that needs to depend on the function package: roscpp rospy std_msgs turtlesim

Implementation of Cpp:

/*
    生成一只小乌龟
    准备工作:
        1.服务话题 /spawn
        2.服务消息类型 turtlesim/Spawn
        3.运行前先启动 turtlesim_node 节点

    实现流程:
        1.包含头文件
          需要包含 turtlesim 包下资源，注意在 package.xml 配置
        2.初始化 ros 节点
        3.创建 ros 句柄
        4.创建 service 客户端
        5.等待服务启动
        6.发送请求
        7.处理响应

*/

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

int main(int argc, char *argv[])
{
    
    
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"set_turtle");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 service 客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    // 5.等待服务启动
    // client.waitForExistence();
    ros::service::waitForService("/spawn");
    // 6.发送请求
    turtlesim::Spawn spawn;
    spawn.request.x = 1.0;
    spawn.request.y = 1.0;
    spawn.request.theta = 1.57;
    spawn.request.name = "my_turtle";
    bool flag = client.call(spawn);
    // 7.处理响应结果
    if (flag)
    {
    
    
        ROS_INFO("新的乌龟生成,名字:%s",spawn.response.name.c_str());
    } else {
    
    
        ROS_INFO("乌龟生成失败！！！");
    }


    return 0;
}

Python implementation:

#! /usr/bin/env python
"""
    生成一只小乌龟
    准备工作:
        1.服务话题 /spawn
        2.服务消息类型 turtlesim/Spawn
        3.运行前先启动 turtlesim_node 节点

    实现流程:
        1.导包
          需要包含 turtlesim 包下资源，注意在 package.xml 配置
        2.初始化 ros 节点
        3.创建 service 客户端
        4.等待服务启动
        5.发送请求
        6.处理响应

"""

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

if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("set_turtle_p")
    # 3.创建 service 客户端
    client = rospy.ServiceProxy("/spawn",Spawn)
    # 4.等待服务启动
    client.wait_for_service()
    # 5.发送请求
    req = SpawnRequest()
    req.x = 2.0
    req.y = 2.0
    req.theta = -1.57
    req.name = "my_turtle_p"
    try:
        response = client.call(req)
        # 6.处理响应
        rospy.loginfo("乌龟创建成功!，叫:%s",response.name)
    except expression as identifier:
        rospy.loginfo("服务调用失败")

3. Run

First, start roscore;

Then start the turtle display node;

Finally, start the turtle to generate the request node;

The final execution result is similar to the demonstration result.

---

4. Parameter setting

Requirement description: Modify the background color of the turtlesim display node form. It is known that the background color is set in rgb mode through the parameter server.

The effect is as follows:

Realize analysis:

1. First, the turtle display node needs to be started.
2. To obtain the parameters for setting the background color in the parameter server through the ROS command.
3. Write a parameter setting node to modify the parameter value in the parameter server.

Implementation process:

1. Obtain parameters through the ros command.
2. Coding realizes server parameter setting node.
3. Start roscore, turtlesim_node and parameter setting nodes to view the running results.

1. Obtaining the parameter name

Get the parameter list:

rosparam list

Response result:

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

2. Parameter modification

Cpp implementation

/*
    注意命名空间的使用。

*/
#include "ros/ros.h"


int main(int argc, char *argv[])
{
    
    
    ros::init(argc,argv,"haha");

    ros::NodeHandle nh("turtlesim");
    //ros::NodeHandle nh;

    // ros::param::set("/turtlesim/background_r",0);
    // ros::param::set("/turtlesim/background_g",0);
    // ros::param::set("/turtlesim/background_b",0);

    nh.setParam("background_r",0);
    nh.setParam("background_g",0);
    nh.setParam("background_b",0);


    return 0;
}

Implementation of Python

#! /usr/bin/env python

import rospy

if __name__ == "__main__":
    rospy.init_node("hehe")
    # rospy.set_param("/turtlesim/background_r",255)
    # rospy.set_param("/turtlesim/background_g",255)
    # rospy.set_param("/turtlesim/background_b",255)
    rospy.set_param("background_r",255)
    rospy.set_param("background_g",255)
    rospy.set_param("background_b",255)  # 调用时，需要传入 __ns:=xxx

3. Run

First, start roscore;

Then start the background color setting node;

Finally start the turtle display node;

The final execution result is similar to the demonstration result.

PS: Pay attention to the order of node startup. If you start the turtle display node first, and then start the background color setting node, the color setting will not take effect.

Reference

http://www.autolabor.com.cn/book/ROSTutorials/di-2-zhang-ros-jia-gou-she-ji/23-fu-wu-tong-xin/224-fu-wu-tong-xin-zi-ding-yi-srv-diao-yong-b-python.html