4_Publieher programming implementation

4_Publisher programming implementation

We explained how to create workspaces and function packages earlier, but we only created an empty workspace and function package without implementing anything

If we want to further add functionality to the feature pack, we will inevitably need to add Publisher and Subscriber.

Next, we will explain the programming implementation of Publisher and Subscriber. We also take the little turtle emulator as an example, but this time we will use programming to make the little turtle move instead of using command line tools.

---

0. Clear tasks

The Publisher and Subscriber we mentioned earlier are essentially nodes, so we will implement a node that can publish messages, that is, Publisher

The specific performance of the topic model we talked about in the little turtle emulator is as follows

Publisher is the teleop_key node we started before, and Subscriber is the turtlesim node

Topic is /tirtle1/cmd_vel, the message we have seen before, including linear velocity and angular velocity

We want to write our own Publisher, so first the Publisher needs to register with the ROS Master, and then publish the Message of the specified format to /turtle1/cmd_vel

1. Create a feature package

Because the node exists in the form of a function package, we create our own function package below, and then we add it to it

The first three dependencies that must be required are roscpp, rospy, std_msgs

Here we need to transfer data with turtlesim, so we also need to rely on the interface of turtlesim

In addition, we also need to publish a message in a specified format, namely geometry_msgs, so we also need to rely on this interface

jack@ubuntu:~/myFirstRosProject/src$ catkin_create_pkg myturtle_velocity_publishernode_cpp roscpp rospy std_msgs geometry_msgs turtlesim

2. C++ implementation

Before we start writing, we need to understand what we need to do to implement a Publisher in C++.

As mentioned above, Publisher is essentially a node, and nodes appear in the form of function packages, so we are actually writing a function package, and this function package has only one Publisher, so we need to compile after writing, and Compilation requires us to modify the content in CMakeLists

Therefore, we use C++ to implement a Publisher divided into two or three, the first step is to write the code, the second step is to modify the content in CmakeLists, the third step is to compile, and finally verify whether the Node we wrote can be used

1. Write code

1. Open the workspace of the current directory
   and use Vscode to write here. How to use Vscode to build a ROS development environment? I will write a note in the future.

   jack@ubuntu:~/myFirstRosProject/src$ code 				  myturtle_velocity_publishernode_cpp/
   

   The folder opened on the left is the working space of the Node we are writing, and then we will srccreate a new c++ file
   

2. Create a new cpp code file in the src directory of the current Node and write it
   

3. happy writing

   The meaning of each sentence is explained in the comments

   The process of writing a ROS node is as follows:

   1. Initialize ros node & register node information in ROS Master
      Corresponding to this sentence ros::init(argc,argv,"my_velocity_publisher_cpp")
      , argc and argv are system variables, indicating the number of parameters passed in by the user at runtime, argv is a list of all incoming parameters, "my_velocity_publisher_cpp" is our custom node name,
      It should be noted that the naming rules of nodes in ros are a combination of lowercase letters/underscores/numbers, and uppercase letters are not recommended
      This interface not only initializes the node, but also registers

   2. create publisher

      Corresponding to this sentence, ros::Publisher turtleVelocityPublihser_cpp=n.advertise<geometry_msgs::Twist> ("/turtle1/cmd_vel",10)
      we need to point out the type of message to be published, the name of the topic to be published and the queue length of the message

   3. publish data
      corresponds to this sentenceturtleVelocityPublihser_cpp.publish(velocity_msg)

   4. output information to the command line
      corresponds to this sentence
      ROS_INFO("Publish the velocity [%0.2f m/s, %0.2f rad/s]\n\t\t Info from Jack",velocity_msg.linear.x,velocity_msg.angular.z);

   /*
   *   This is a node created by Jack Wang to practice ROS publisher
   */
   
   // Include head files
   #include <ros/ros.h>
   #include <geometry_msgs/Twist.h>
   
   // Receive system variable
   // argc is the number of system variable, ** argv is the list of all the system variables
   int main(int argc, char ** argv){
   
       // Initialize ROS node
       // argc,argv,name of the node
       ros::init(argc,argv,"my_velocity_publisher_cpp");
   
       // Create node handle
       ros::NodeHandle n;
   
       // Create a ros publisher which publishes topic named /turtle1/cmd_vel
       // and the type of message it sends is geometry_msgs::Twist
       // max length of the message quene is 10
       ros::Publisher turtleVelocityPublihser_cpp=n.advertise<geometry_msgs::Twist>
           ("/turtle1/cmd_vel",10);
       
       // Set loop frequency to 10
       ros::Rate loop_rate(10);
   
       // Things to do in while loop
       while (ros::ok())
       {
           // Initialize geometry_msgs::Twist message
           geometry_msgs::Twist velocity_msg;
   
           // Set the message
           velocity_msg.angular.z=0.2;   // Set the angular velocity to 0.2 rad/s
           velocity_msg.linear.x=0.5;    // Set the linear velocity to 0.5 m/s
   
           // Publish the message
           turtleVelocityPublihser_cpp.publish(velocity_msg);
   
           // Print info on the terminal
           ROS_INFO("Publish the velocity [%0.2f m/s, %0.2f rad/s]\n\t\t Info from Jack",
               velocity_msg.linear.x,velocity_msg.angular.z
           );
   
           // Delay as setting frequency
           loop_rate.sleep();
       }
       return 0;
   }
   

2. Modify the content in Cmakelists

As mentioned earlier, CmakeLists uses a simple language to describe how to compile a project

We mainly set catkin to compile the file we just added when compiling this package

So there are two main points, the first specifies to compile the file we just added, and the second specifies the dependent library of the file

1. Open the CmakeLists of the node

   

2. Add the following two lines at the specified location

   add_executable(myVelocityPublisherNode src/myVelocityPublisherNode.cpp)
   target_link_libraries(myVelocityPublisherNode ${catkin_LIBRARIES})
   

   The first sentence indicates the file that needs to be compiled and the name of the compiled executable file
   The second sentence indicates the library files that need to be added

   

3. compile

Next we go back to the root directory of the workspace and use catkin_make to compile

jack@ubuntu:~/myFirstRosProject$ catkin_make

4. Run the written node

Above we created a function package ourselves, and there is only one node in this function package

Next we will run the node we wrote

It is divided into two steps. The first step is to add environment variables so that Tabthe key can complete the package written by yourself. The second step is to run

1. Add an environment variable
   Note that this environment variable only takes effect on the command line that has currently run the following command

   jack@ubuntu:~/myFirstRosProject$ source ./devel/setup.bash 
   

   

2. Running your own package
   is the same as running the little turtle emulator, we first start ros master, and then use the rosrun command to run our own package

   

Note

Through the above operations, we finally know that a node is actually an executable file, and when we run this node, we actually run the executable file

And just like the above turtlesim_node and our my_velocity_publisher_cpp, there will be data transmission (ie geometry_msgs::Twist), so the transmission of data between nodes can be compared to the communication between processes

In addition, the location of the final executable file of my_velocity_publisher_cpp we run is in ./devel/lib/包名this folder

3. Python implementation

Since Python is a scripting language, it is not necessary to modify the content of CmakeLists and then compile it like using C++ to write node above

But we are actually very similar in the code, and the writing and running process is also very similar, just add the environment variable directly and run it

1. Write code

The code flow is the same as the code flow of the C++ version above, and we also need to create a package. Generally speaking, when we develop a software package normally, there may be many nodes

And some nodes are written in C++, and some nodes are written in Python. At this time, we will create a new script folder under the created package, and put all the nodes written in Python in it.

And all C++ written nodes are placed in src, after writing, modify the CmakeLists file of the current package

1. Open the folder and create the Python file

   jack@ubuntu:~/myFirstRosProject/src/myturtle_velocity_publishernode_cpp$ code scripts/myVelocityPublisherNode.py
   

   

2. happy writing code

   #!/usr/bin/env python3
   # -*- coding: utf-8 -*-
   
   # Importing libraries
   import rospy
   from geometry_msgs.msg import Twist
   
   
   # Encapsulate the function
   def myVelocityPublisher():
   
       # Initialize ros node
       rospy.init_node("my_velocity_publisher_py",anonymous=True)
   
       # Create a publisher
       turtle_vel_pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=10)
   
       # Set loop rate
       rate = rospy.Rate(10)
   
       while not rospy.is_shutdown():
           velocity_msgs=Twist()
           velocity_msgs.angular.z=0.2
           velocity_msgs.linear.x=0.5
   
           turtle_vel_pub.publish(velocity_msgs)
           rospy.loginfo("Publish the velocity [%0.2f m/s, %0.2f rad/s]\n\t\t Info from Jack",\
               velocity_msgs.linear.x,velocity_msgs.angular.z)
   
           rate.sleep()
   
   
   # Main program
   if __name__=="__main__":
       try:
           myVelocityPublisher()
       except rospy.ROSInitException:
           print("my_velocity_publisher_py is terminated !")
   

   

2. Run the code

After we finish editing, we can call the Python interpreter to run, or run after giving executable permission

1. grant executable permissions

   Use the following command to add executable permissions

   jack@ubuntu:~/myFirstRosProject/src/myturtle_velocity_publishernode_cpp/scripts$ chmod u+x myVelocityPublisherNode.py 
   

   

2. Adding environment variables
   is the same as the result after compiling C++, we start ros master first, and then run the rosrun node, but it should be noted that the scripts we write directly also need to add environment variables, so that they can be completed with Tab

   jack@ubuntu:~$ source myFirstRosProject/devel/setup.bash
   

   

3. rosrun can run

   jack@ubuntu:~$ rosrun myturtle_velocity_publishernode_cpp myVelocityPublisherNode.py