对geometry_msgs/PoseStamped的解读

详细参考官方链接：geometry_msgs/PoseStamped Documentationhttp://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html

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

#include "geometry_msgs/PoseStamped.h"

2 、通过定义msg对象调用数据类型为std_msgs/Header,geometry_msgs/Pose的两个成员header, pose，即geometry_msgs::PoseStamped msg；msg.header、msg.pose。通过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为单位．

常用形式：

#navpoint.msg
geometry_msgs/PoseStamped target_pose
uint8 floor
uint8 type


target_pose 的类型为geometry_msgs/PoseStamped 

# A Pose with reference coordinate frame and timestamp
Header header
Pose pose

Record PoseStamped := { _header : Header.Header; _pose : Pose.Pose}.

简单构造

demo1
#构造header
target_pose.header.seq = 0;
target_pose.header.stamp =ros::Time::now();//如果有问题就使用Time(0)获取时间戳，确保类型一致
target_pose.header.frame_id = "map";
#构造pose
target_pose.pose.position.x = x1;
target_pose.pose.position.y = y1;
target_pose.pose.position.z = 0.0;
target_pose.pose.orientation.x = 0.0;
target_pose.pose.orientation.y = 0.0;
target_pose.pose.orientation.w = 1.0;

demo2
geometry_msgs::PoseStamped Start;
Start.header.seq = 0;
Start.header.stamp = ros::Time::now();//如果有问题就使用Time(0)获取时间戳，确保类型一致
Start.header.frame_id = "map";
Start.pose.position.x = x1;
Start.pose.position.y = y1;
Start.pose.position.z = 0.0;
Start.pose.orientation.x = 0.0;
Start.pose.orientation.y = 0.0;
Start.pose.orientation.w = 1.0;

def publish_waypoints(self):
        """
        Publish the ROS message containing the waypoints
        """
        msg = Path()
        msg.header.frame_id = "map"
        msg.header.stamp = rospy.Time.now()
        if self.current_route is not None:
            for wp in self.current_route:
                pose = PoseStamped()
                pose.pose.position.x = wp[0].transform.location.x
                pose.pose.position.y = -wp[0].transform.location.y
                pose.pose.position.z = wp[0].transform.location.z

                quaternion = tf.transformations.quaternion_from_euler(
                    0, 0, -math.radians(wp[0].transform.rotation.yaw))
                pose.pose.orientation.x = quaternion[0]
                pose.pose.orientation.y = quaternion[1]
                pose.pose.orientation.z = quaternion[2]
                pose.pose.orientation.w = quaternion[3]
                msg.poses.append(pose)

        self.waypoint_publisher.publish(msg)
        rospy.loginfo("Published {} waypoints.".format(len(msg.poses)))