There is a detailed analysis and interpretation of the geometry_msgs/PoseStamped message. This is very important, and this itself is a kind of message we use a lot. geometry_msgs means geometry data type.
Also, I really read this picture, and then look at the topical messages printed by ROS, it is very clear! ! ! !
https://blog.csdn.net/sinat_16643223/article/details/114572760
Reprinted from: https://www.jianshu.com/p/5c75c24f0fe6
Robot operating system ROS: from entry to abandonment (3) release and receive different messages 2
Robot operating system ROS: from entry to abandonment (3) release and receive different messages 2
In the last lecture, we talked about how to publish and receive messages of int, float and array types. These are all C++-owned messages and are summarized under the std_msgs namespace in ROS (namespace skr?). Let's take a look at how to publish and receive ROS's own unique message type: PoseStamped. Pose is easy to understand. It is the position and orientation of the robot. What about Stamped? Stamped stands for timestamped, where timestamp refers to the total nanoseconds from 00: 00: 00 on January 1, 1970, Greenwich Mean Time to the present. Almost all computers can use this time, which is convenient for unification. So what PoseStamped records is a message such as the pose of the robot plus the time to record the pose. This message is included in the category geometry_msgs. So you can search for ROS, geometry message first. The first web page should be the following
http://wiki.ros.org/geometry_msgs
. There are a lot of different types of messages, which you may use in the future. But here we use PoseStamped as an example, the pose plus time is still more representative.
Find the page PoseStampedand click to enter, you will find the following content
If you want to use this message, then see this page. According to what we said before, you can at least do the following things
1: Include the header file #include "geometry_msgs/PoseStamped.h"so that we can use this type of message
2: By geometry_msgs::PoseStamped msgdefining a call The object of msg, the object can use the header,posetwo data members in the figure above . That you can msg.header, msg.poseto call the data type std_msgs/Header, geometry_msgs/Posethe two data members.
But we don't know exactly what the header and pose are. But as I said before, any complex data type is composed of simple ones. The light blue font under the page is a link that can be clicked in. Let's first click to std_msgs/Headersee what it is. You will see the content below.
uint32 and string are the basic variables. It Compact Message Definitioncan be known that if we std_msgs::Header msg_headercreate an object of type msg_header, then it can call a member of type unit32 through msg_header.seq seq. In other words, in the code you can use something like
int a = 1 , b;
msgs_header.seq = 1;
//or
b = msg_header.seq;
This code msgs_header.seqassigns or assigns it to other integer variables. Maybe you have to ask, isn't seq defined uint32? My definition of a and b is int, will there be a problem? possible. At this time, I need to use the following website
http://wiki.ros.org/msg to
find out what the C++ definition of uint32 corresponds to, and find that it is uint32_t, so you use it directly, uint32_t defines a and b is the best. Int is an integer, uint32_t is an unsigned 32-bit integer. The range of the two is different. You can check this on the Internet, but the specific value of a and b will not change within the range of their intersection. In short, in the same way, you can msg_header.frame_idassign a variable of type string.
But what is this time? Standard data types don't have this thing. But the Raw Message Definitionexplanation given above is stamp.secto return the time in seconds from the beginning of the epoch, and stamp.nsecreturn the time stamp_secin nanoseconds from the beginning.
msgs_heder.stamp calls stamp, stamp.sec calls sec to get the epoch time, then msgs_header.stamp.sec can get the current time, in seconds,
usually we definitely need a more accurate time that includes nanoseconds, suppose we want to The current precise time is assigned to the variable store_time, then the code is similar to the following.
double store_time;
store_time = msg_header.stamp.sec + 1e-9*msg_header.stamp.nsec; //unit s
//or
store_time = msgs_header.stamp.sec * 1e9 + msg_header.stamp.nsec; //unit ns
The unit of nsec is nanoseconds. We have to multiply by 1e-9 to convert it to seconds. The second line gets the time in seconds. The time obtained in the fourth line is in ns.
Okay, the header said so much, currently we want the stamp here to record a time. We defined it at the beginning geometry_msgs::PoseStamped msg.
msg contains the data member header, and std_msgs/Headerthe data member of the heap contains stamp, then we can call the member stamp whose data type is time through msg.header.stamp , why we can do this, we will talk about the next one. ROS can easily get the current time ros::Time::now(), and it returns a variable of type time. So we can pass
msg.header.stamp = ros::Time::now()
To get the current time and store it in this message. After assignment, what if I want to retrieve the time when I recorded that pose and it is of double type? According to the above content, it is easy to get.
double store_time;
store_time = msg.header.stamp.sec + 1e-9*msg.header.stamp.nsec; //unit s
//or
store_time = msg.header.stamp.sec * 1e9 + msg.header.stamp.nsec; //unit ns
Be careful not to think that msg.header is equal to msg_header... It’s just that I happened to std_msgs::Headername the object msg_header in the above example . Zhang San Li Si is fine. msg.headerIt is the geometry_msgs::PoseStampedobject msg call data member header, and msg_headeris std_msgs::Headerthe object.
After talking about the header, we can look at another data member poseof msg . Similarly, we click on the light blue font in the first picture to geometry_msgs/Posesee what basic variables it is made of. After clicking in, we enter the interface defined by Pose. The data members of Pose are positionand orientation, very easy to understand, position and direction. Their data types are geometry_msgs/Pointand respectively geometry_msgs/Quaternion. These two are not basic data types either, you can continue to click in until the variable has only basic data types. For the sake of brevity, I directly drew a picture to list the members included in PoseStamped one by one.
Look at this picture at a glance. (If you use other people's things for free, you have to advertise. This picture is drawn with an online software called ProcessOn, which is very convenient for drawing flowcharts, mind maps, structure diagrams, etc.).
The Header part has already been talked about. Let's continue to look at posethe member
1 contained in the geometry/msgs/Pose type data is geometry_msgs/Point type position, and positioncontains three float64 variables x,y,z. This is easy to understand. How to define the position of the robot? Three-dimensional coordinates x, y, z will do.
2 is the'oreintation' of geometry_msgs/Quaternion type, and oreintationcontains four float64 variables x,y,z,w, quaterion Chinese quaternion, which is a thing used to indicate direction. The disadvantage of quaternion is that it is not very vivid. It is difficult for people who are not familiar with quaternion to conceive in their minds what direction the robot is currently in. When we generally use Euler angles to indicate the direction, there are three numbers roll, pitch, yaw, which are more intuitive, but when Euler angles indicate the direction, we will encounter an embarrassing problem called Gimbal Lock (universal lock), so in ROS A quaternion is used to express the direction uniformly. Of course, ROS provides some APIs to convert quaternions into Euler angles or rotation matrices, but when storing poses, they are all stored in quaternions. If you don’t know the quaternion, you can check it online.
The structure of the data is finished, how to use it? And msg.header.stamp.seccall int32 types sec as a member, you can msg.pose.position.xcall or assignment float64 members of x. Directly use the code to illustrate. Next we have to create a pub_poseStamped.cppfile called andsub_poseStamped.cppdocument. In the first file, let’s manually generate some poses, um,. I believe that many students don’t have sensors that can directly communicate with ROS. After generating these poses, we publish them and use the receiver to receive them.
As before, we first pub_sub_test/srccreate a pub_poseStamped.cppfile named in. Write the following content in the file.
#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h" //include posestamp head file
#include <cmath>//for sqrt() function
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<geometry_msgs::PoseStamped>("chatter", 10); //initialize chatter
ros::Rate loop_rate(10);
//generate pose by ourselves.
double positionX, positionY, positionZ;
double orientationX, orientationY, orientationZ, orientationW;
//We just make the robot has fixed orientation. Normally quaternion needs to be normalized, which means x^2 + y^2 + z^2 +w^2 = 1
double fixedOrientation = 0.1;
orientationX = fixedOrientation ;
orientationY = fixedOrientation ;
orientationZ = fixedOrientation ;
orientationW = sqrt(1.0 - 3.0*fixedOrientation*fixedOrientation);
double count = 0.0;
while (ros::ok())
{
//We just make the position x,y,z all the same. The X,Y,Z increase linearly
positionX = count;
positionY = count;
positionZ = count;
geometry_msgs::PoseStamped msg;
//assign value to poseStamped
//First assign value to "header".
ros::Time currentTime = ros::Time::now();
msg.header.stamp = currentTime;
//Then assign value to "pose", which has member position and orientation
msg.pose.position.x = positionX;
msg.pose.position.y = positionY;
msg.pose.position.z = positionY;
msg.pose.orientation.x = orientationX;
msg.pose.orientation.y = orientationY;
msg.pose.orientation.z = orientationZ;
msg.pose.orientation.w = orientationW;
ROS_INFO("we publish the robot's position and orientaion");
ROS_INFO("the position(x,y,z) is %f , %f, %f", msg.pose.position.x, msg.pose.position.y, msg.pose.position.z);
ROS_INFO("the orientation(x,y,z,w) is %f , %f, %f, %f", msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w);
ROS_INFO("the time we get the pose is %f", msg.header.stamp.sec + 1e-9*msg.header.stamp.nsec);
std::cout<<"\n \n"<<std::endl; //add two more blank row so that we can see the message more clearly
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
We fixedOrientationassign the value of 0.1 to the variables in the code of the publisher , and then assign them to the created double type variables respectively orientationX,Y,Z,W. In the loop, orientationX,Y,Z,Wassign values to the member variables of msg we created msg.pose.orientation.x y z w. Why is it so assignable? Through the above data structure picture, we know that msg.pose.orientation.x y z wall variables are of float64 type, and after assigning a few of them, the orientation of pose can be defined. If the orientation is the same number, then the robot does not rotate.
Then we directly assign count to the xyz of the pose's position, and the count is incremented in the loop, so XYZ is incremented at the same time and the same. If we draw a three-dimensional coordinate axis XYZ, then the motion state of the robot simulated here is equivalent to the robot traveling in a straight line along the diagonal of the coordinate axis at a constant speed.
Create another one at the same location sub_poseStamped.cpp. Write the following content.
#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
void chatterCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) //Note it is geometry_msgs::PoseStamped, not std_msgs::PoseStamped
{
ROS_INFO("I heard the pose from the robot");
ROS_INFO("the position(x,y,z) is %f , %f, %f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
ROS_INFO("the orientation(x,y,z,w) is %f , %f, %f, %f", msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
ROS_INFO("the time we get the pose is %f", msg->header.stamp.sec + 1e-9*msg->header.stamp.nsec);
std::cout<<"\n \n"<<std::endl; //add two more blank row so that we can see the message more clearly
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "listener");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("chatter", 10, chatterCallback);
ros::spin();
return 0;
}
After writing, we also open pub_sub_testthe CMakeLists.txt located in the directory to add the contents of the two files to be compiled. After saving and exiting, use catkin_make to compile (note that the command catkin_make must be used in the directory catkin_ws).
The result was a compilation error! ! The error may look like the following
error_poseStamped.png
I don't understand what it means. Compare carefully to see pub_int8, pub_stringif there is any discrepancy between the previous content and your previous compilation and other files? After careful inspection, it was found that there was nothing. All steps are the same. So where is the problem?
Remember when we created pub_sub_testthis package, we created it like this
catkin_create_pkg pub_sub_test std_msgs roscpp rospy
We say that the three dependencies std_msgs roscpp rospyare like C++ header files. If we want to use the std_msgs package that comes with ROS, we need to add this dependency. So now the problem is obvious, we are missing to add geometry_msgsthis dependency. As you can imagine, if we used the following content when creating the package, there would be no problem
catkin_create_pkg pub_sub_test std_msgs geometry_msgs roscpp rospy
But our package has been created, and we cannot recreate a package with the same name to add dependencies. What should I do?
When we need to add a new dependency, we need to modify two files, one is CMakeLists.txt in the package directory, and the other is package.xml located in the same location.
Open CMakeLists.txt and find that it is just the first few lines. There are the following contents.
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
)
The content in the brackets corresponds exactly to the dependencies we added when we created the package, so you don't need to think about it, you must add geometry_msgs after it, and it will look like the following, save and exit.
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
geometry_msgs
)
After adding, save and exit. Then open the package.xml. (Double-click to open it may not be able to modify the content, or use gedit or something to open). After opening, you can see the content below the document
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
Aha, there are std_msgs, rospy, roscpp, but each appears three times, then again, don't care what it means, we only need to follow the same grammar in this document to make it geometry_msgsappear three times. After the change, the same location of the file becomes the following content
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
Save and exit. At this time, compile with catkin_make again, and it will succeed. Changing the contents of the above two documents is equivalent to adding dependencies when creating the package geometry_msgs. After the compilation is successful, the method of running the program is the same as the previous example.
to sum up
In this lecture, we talked about a slightly more complicated message type: poseStamped. We need to use the help of ROS wiki to know what data members it contains. After understanding the data members it contains, use the method of class object to refer to the data member (just like msg.pose.orientation) to call or assign the corresponding type. Member. Because we don't have a sensor on our hands, we have generated double variable assignments, starting from the bottom float64 xyz and so on, to simulate the movement of the robot. But in fact, if a good positioning sensor has its own ROS interface, it is very likely to directly generate geometry_msgs/Pointtype variables. Assuming the name is called currentPosition, then we msg.pose.position = currentPositioncan assign a value to the pose position, because it pose.positionis also a geometry_msgs/Pointtype variable. Everyone will know when you touch the specific sensor.
In addition, this lecture talks about how to add new ROS dependencies.
So now we have talked about how to publish string, int8, array and poseStamped. Regarding the different types of news, we will stop here. I hope that you will know how to find and understand the corresponding information in the ROS wiki when facing the different types of messages you want to publish in the future.
In the next lecture, we will introduce how to publish and receive messages in the class. Before that, in order to take care of students who are not so familiar with the language, we may first review the content of the C++ class in the next lecture. Namespaces and templates are also mentioned, and correspond to the syntax that appears in ROS. Students who are familiar with the language can skip the next lecture.