Communication mechanism cases

After learning the three communication mechanisms , Practice , Don't talk much , Go straight to the project ！

Item 1 ： Topic Publishing

Requirements describe : The tortoise motion control is realized by coding , Let the little turtle do circular motion .

Implementation analysis :

1. Tortoise movement control is realized , There are two key nodes , One is the tortoise motion display node turtlesim_node, The other is the control node , The two realize communication in the subscription and publication mode , Tortoise movement display node can be called directly , Motion control nodes were used before turtle_teleop_key Control by keyboard , Now you need to customize the control node .
2. When the control node realizes itself , First, you need to understand the topics and messages used by the control node and the display node , have access to ros Command combined with calculation diagram to obtain .
3. After knowing the topic and news , adopt C++ or Python Write node motion control , Through the specified topic , Just publish the message according to a certain logic .

Implementation process :

1. Through the calculation diagram, combined with ros Command to get topic and message information .
2. Coding to achieve motion control node .
3. start-up roscore、turtlesim_node And custom control nodes , View the run results .

1. Topic and message acquisition

Get ready : First start the keyboard to control the tortoise movement case .

Start the tortoise movement display node ：rosrun turtlesim turtlesim_node

Start keyboard control node ：rosrun turtlesim turtle_teleop_key

1.1 Topic acquisition

Get the topic :/turtle1/cmd_vel

Check the topic through the calculation chart , Start the calculation chart :

rqt_graph
Or by rostopic List topics :

rostopic list

1.2 Information access

Get message type :geometry_msgs/Twist

rostopic type /turtle1/cmd_vel
Get message format :rosmsg info geometry_msgs/Twist
In response to the results :

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

linear( Linear velocity ) Under the xyz Corresponding to x、y and z Speed in direction ( The unit is m/s);

angular( angular velocity ) Under the xyz They correspond to each other x Roll on the shaft 、y On axis pitch and z Yaw speed on axis ( The unit is rad/s).

2. Implement the publishing node

To create a function package, you need to rely on the function package : roscpp rospy std_msgs geometry_msgs

Code test01_pub_twist.cpp

/*
     To write  ROS  node , Control the little turtle to draw a circle 

     preparation :
        1. obtain topic( It is known that : /turtle1/cmd_vel)
        2. Get message type ( It is known that : geometry_msgs/Twist)
        3. Before running , Pay attention to start first  turtlesim_node  node 

     Implementation process :
        1. Include header file 
        2. initialization  ROS  node 
        3. Create publisher object 
        4. Circularly issue motion control messages 
*/

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

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2. initialization  ROS  node 
    ros::init(argc,argv,"my_control");
    ros::NodeHandle nh;
    // 3. Create publisher object 
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    //4-2. Set the transmission frequency 
    ros::Rate rate(10);
    // 4. Circularly issue motion control messages 
    //4-1. Organize messages 
    geometry_msgs::Twist twist;
    twist.linear.x = 1.0;
    twist.linear.y= 0.0;
    twist.linear.z = 0.0;

    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 0.5;
    //4-3. Cycle to send 
    while(ros::ok())
    {
       pub.publish(twist);
       // Sleep 
       rate.sleep();
       // come back 
       ros::spinOnce(); 
    }
    return 0;
}

The system has its own topic to control the tortoise to move at a fixed frequency ：

  Customize the topic to control the tortoise to move at a fixed frequency :

rosrun plumbing_test test01_pub_twist

Item 2 ： Topic subscription

Requirements describe :  It is known that turtlesim The tortoise display node in , Will release the current turtle's pose ( The coordinates and orientation of the tortoise in the form ), Request control of tortoise movement , And print the current turtle's posture from time to time .

Implementation analysis :

1. First , You need to start the tortoise display and motion control node and control the tortoise motion .
2. To pass the ROS command , To get the topic and news of tortoise posture .
3. Write a subscription node , Subscribe and print the turtle's posture .

Implementation process :

1. adopt ros Command to get topic and message information .
2. Coding to achieve pose acquisition node .
3. start-up roscore、turtlesim_node 、 Control node and pose subscription node , Control the movement of the tortoise and output the posture of the tortoise .

1. Topic and message acquisition

Get the topic :/turtle1/pose

rostopic list

Get message type :turtlesim/Pose

rostopic type  /turtle1/pose
Get message format :

rosmsg info turtlesim/Pose
In response to the results :

​float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

 theta： toward

linear_velocity： Linear velocity

angular_velocity： angular velocity

2. Implement the subscription node

To create a function package, you need to rely on the function package : roscpp rospy std_msgs turtlesim

Create a launch The file starts the tortoise display and control movement function at the same time start_turtle.launch

<!--  Start tortoise GUI And keyboard control node  -->
<launch>
    <!--  Tortoise GUI -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output= "screen" />
    <!--  Keyboard control  -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />

</launch>

First in the terminal source ./devel/setup.bash, Then start roslaunch plumbing_test start_turtle.launch

First, get the turtle's posture by command ：rostopic echo /turtle1/pose

Then write the code test02_sub_pose.cpp

/*  
     Subscribe to the posture of the little turtle :  Get the coordinates of the little turtle in the form from time to time and print 
     preparation :
        1. Get topic name  /turtle1/pose
        2. Get message type  turtlesim/Pose
        3. Start before operation  turtlesim_node  And  turtle_teleop_key  node 

     Implementation process :
        1. Include header file 
        2. initialization  ROS  node 
        3. establish  ROS  Handle 
        4. Create subscriber object 
        5. The callback function processes the subscribed data 
        6.spin
*/

#include "ros/ros.h"
#include "turtlesim/Pose.h"
void doPose(const turtlesim::Pose::ConstPtr &pose){
    ROS_INFO(" The tortoise's position and posture coordinates ： coordinate (%.2f,%.2f), toward (%.2f), Linear velocity ：%.2f, angular velocity ：%.2f",
    pose->x,pose->y,pose->theta,pose->linear_velocity,pose->angular_velocity);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2. initialization  ROS  node 
    ros::init(argc,argv,"sub_pose");
    // 3. establish  ROS  Handle 
    ros::NodeHandle nh;
    // 4. Create subscriber object 
    ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
    // 5. The callback function processes the subscribed data 
    ros::spin();
    return 0;
}

Modify the configuration file package.xml

<build_depend>turtlesim</build_depend>

<exec_depend>turtlesim</exec_depend>

  Modify the configuration file CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  std_msgs
  turtlesim
)

add_executable(test02_sub_pose src/test02_sub_pose.cpp)

add_dependencies(test02_sub_pose ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(test02_sub_pose
  ${catkin_LIBRARIES}
)

Recompile and source ./devel/setup.bash after , Again rosrun plumbing_test test02_sub_pose, You can observe the pose transformation , Sometimes this step will go wrong , Restart vscode Recompile it .

Item three ： The service call

Requirements describe : Coding implementation direction turtlesim Send a request , Generate a turtle at the specified position on the form of the turtle display node , This is a service request operation .

Implementation analysis :

1. First , You need to start the tortoise display node .
2. To pass the ROS command , To get the service name and service message type of the tortoise generated service .
3. Write the service request node , Generate new turtles .

Implementation process :

1. adopt ros Command to obtain service and service message information .
2. Code the service request node .
3. start-up roscore、turtlesim_node 、 Tortoise generates nodes , Generate new turtles .

1. Service name and service message acquisition

Get the topic :/spawn

rosservice list

Get message type :turtlesim/Spawn

rosservice type /spawn

Get message format :

rossrv info turtlesim/Spawn

In response to the results :

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

2. Service client implementation

To create a function package, you need to rely on the function package : roscpp rospy std_msgs turtlesim

/*
     Make a little turtle 
     preparation :
        1. Service topics  /spawn
        2. Service message type  turtlesim/Spawn
        3. Start before operation  turtlesim_node  node 

     Implementation process :
        1. Include header file 
           Need to include  turtlesim  Resources under the package , Pay attention to  package.xml  To configure 
        2. initialization  ros  node 
        3. establish  ros  Handle 
        4. establish  service  client 
        5. Wait for the service to start 
        6. Send a request 
        7. Process response 

*/

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

int main(int argc,char *argv[])
{
    setlocale(LC_ALL,"");
    // 2. initialization  ros  node 
    ros::init(argc,argv,"service_call");
    // 3. establish  ros  Handle 
    ros::NodeHandle nh;
    // 4. establish  service  client 
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    // 6. Send a request 
    turtlesim::Spawn spawn;
    spawn.request.x = 1.0;
    spawn.request.y = 4.0;
    spawn.request.theta = 1.57;
    spawn.request.name = "turtle2";
    // 5. Wait for the service to start 
    client.waitForExistence();
    bool flag = client.call(spawn);
    //flag Receive response status , The response result will also be set to gold spawn object 
    // 7. Process response results 
    if(flag)
    {
        ROS_INFO(" Tortoise generation succeeded , The new turtle is called :%s",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO(" request was aborted ！！！");
    }
    return 0;
}

1.roslaunch plumbing_test start_turtle.launch

  among URI Refers to the resource path of the node

Args It refers to the materials that need to be submitted when requesting services

 --- Above is the request and below is the response

Use command mode to generate a new turtle

1.roslaunch plumbing_test start_turtle.launch

2.source ./devel/setup.bash

3.rosrun plumbing_test demo03_service_client

Item four ： Parameter setting

Requirements describe :  modify turtlesim Tortoise displays the background color of the node form , It is known that the background color is displayed in the form of parameter server rgb The way to set up .

Implementation analysis :

1. First , You need to start the tortoise display node .
2. To pass the ROS command , To get the parameters for setting the background color in the parameter server .
3. Write parameter setting node , Modify the parameter value in the parameter server .

Implementation process :

1. adopt ros Command to get parameters .
2. Code the parameter setting node .
3. start-up roscore、turtlesim_node And parameter setting node , View the run results .

1. Parameter name acquisition

Get parameter list :rosparam list

In response to the results :

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

2. Parameters change

#include "ros/ros.h"

/*
       demand ： Modify parameters in the server turtlesim  Background color related parameters 
          1. initialization ROS node 
          2. It is not necessary to create a node handle ( And the subsequent API of )
          3. Modify the parameters 
*/
int main(int argc, char *argv[])
{
    //1. initialization ROS node 
    ros::init(argc,argv,"change_bgColor");
    //2. It is not necessary to create a node handle ( And the subsequent API of )
    //ros::NodeHandle nh("turtlesim");
    //nh.setParam("background_r",255);
    //nh.setParam("background_g",255);
    //nh.setParam("background_b",255);

    ros::NodeHandle nh;
    nh.setParam("/turtlesim/background_r",0);
    nh.setParam("/turtlesim/background_g",50);
    nh.setParam("/turtlesim/background_b",100);
    //3. Modify the parameters 
    //ros::param::set("/turtlesim/background_r",0);
    //ros::param::set("/turtlesim/background_g",0);
    //ros::param::set("/turtlesim/background_b",0);
    return 0;
}

1.roscore

2.rosrun plumbing_test test04_param

3.rosrun turtlesim turtlesim_node