ROS--geometry_msgs/PoseStanped消息解读

http://wiki.ros.org/geometry_msgs
可以看到不同类型的消息，点击PoseStamped进入PoseStamped message 页面

1、通过包含头文件可以调用该类型的消息

#include "geometry_msgs/PoseStamped.h"

2、通过定义msg对象调用数据类型为std_msgs/Header,geometry_msgs/Pose的两个成员header, pose

geometry_msgs::PoseStamped msg
msg.header  ,   msg.pose

点击浅蓝色字体链接进入std_msgs/Header页面

通过Compact Message Definition可以知道，我们通过std_msgs::Header msg_header创建了msg_header类型的对象，就可以通过msg_header.seq调用类型为unit32的成员seq进行赋值或者赋值给其他变量。

int n;
msgs_header.seq = 1; 
//or
n = msg_header.seq;

同理可以给msg_header.frame_id赋值string类型的变量
msg_header.stamp.sec得到从epoch开始的秒为单位的时间，msg_header.stamp.nsec得到从stamp_sec开始的纳秒时间
msgs_heder.stamp调用stamp，stamp.sec调用sec得到epoch的时间，那么msgs_header.stamp.sec就可以获取当前的时间，秒为单位
nsec的单位是纳秒，我们要乘以1e-9才能转换为秒，第二行得到的是时间以秒为单位．第四行得到的时间是以ns为单位．

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

例 记录时间

geometry_msgs::PoseStamped msg
msg.header.stamp = ros::Time::now()

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

msg包含数据成员header，而header作为std_msgs/Header的数据成员包含stamp，那么我们可以通过msg.header.stamp调用数据类型为time的成员stamp。ROS可以很方便获取当前时间ros::Time::now()，返回的是time类型的变量．所以可以把当前时间存储在这个message中。

点击浅蓝色字体链接geometry_msgs/Pose

Pose的数据成员是位置和方向position和orientation，geometry_msgs/Point类型的position，含三个float64的变量x,y,z

geometry_msgs/Quaternion类型的’oreintation’，包含四个float64的变量x,y,z,w

和msg.header.stamp.sec调用int32类型的成员sec一样，可以用msg.pose.position.x调用或者赋值float64的成员x

创建一个名字叫pub_poseStamped.cpp的文件
#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"  
#include //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;
}

给pose的orientation赋值相同的数，机器人就没用旋转
给position赋值相同的递增值，机器人沿着坐标轴对角线匀速直线行驶

再创建一个sub_poseStamped.cpp
#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;
}