ROS学习笔记(2)

目标：发布nav_msgs/Path消息，并在rviz中画出圆形轨迹（Python）

首先，用rosmsg show 命令，查看nav_msgs/Path的消息结构。

# //nav_msgs/Path数据类型
# Header header
# geometry_msgs/PoseStamped[] poses
# //类型扩展：
# Header header
#     uint32 seq
#     time stamp
#     string frame_id
# geometry_msgs/PoseStamped[] poses
#     Header header
#         uint32 seq
#         time stamp
#         string frame_id
#     geometry_msgs/Pose pose
#         geometry_msgs/Point position
#             float64 x
#             float64 y
#             float64 z
#         geometry_msgs/Quaternion orientation
#             float64 x
#             float64 y
#             float64 z
#             float64 w

 可以看到nav_msgs/Path分为两层，一层是Header，一层是geometry_msgs/PoseStamped，其中geometry_msgs/PoseStamped是以数组形式发布，因此我们在编写Python程序时应该import两个消息类型

from nav_msgs.msg import Path

from geometry_msgs.msg import PoseStamped

 以及需要用到的包

import rospy

import math

from tf.transformations import quaternion_from_euler   #欧拉转四元数

定义发布函数pub_o_path()：

def pub_o_path():

    #定义发布者，应该注意的是发布的消息类型始终为Path，Path之下的消息类型geometry_msgs/PoseStamped只是用于赋值，而不是使用PoseStamped发布

    pub_path = rospy.Publisher("path_pubulisher", Path, queue_size = 10)

    msg = Path()

    #指定frame_id和stamp，frame_id是在rviz显示的时候会用到，需要在rviz中fixed frame位置输入你定义的frame_id，这里使用rviz默认的map

    #stamp是时间辍，看了很多例子一般是使用rospy.Time.now()，不知道还有没有其他的设定，挖个坑。

    msg.header.frame_id = "map"

    msg.header.stamp =  rospy.Time.now()

    rate = rospy.Rate(10)

    #由于是定义了一个做圆周的物体，因此还需要给它一些初始值

    x = 0.0

    y = 0.0

    th = 0.0

    vx = 0.1

    vy = -0.1

    vth = 0.1

    #开始循环发布消息

    while not rospy.is_shutdown():

        #定义求出圆的运动方程，并保存经过的点

        dt = 0.1

        delta_x = (vx * math.cos(th) - vy * math.sin(th)) * dt

        delta_y = (vx * math.sin(th) + vy * math.cos(th)) * dt

        delta_th = vth * dt

        x += delta_x

        y += delta_y

        th  += delta_th

        #定义一个变量对PoseStamped进行赋值

        #再次强调，应该注意的是发布的消息类型始终为Path，Path之下的消息类型geometry_msgs/PoseStamped只是用于赋值，而不是使用PoseStamped发布

        pose = PoseStamped()

        pose.pose.position.x = x

        pose.pose.position.y = y

        #由于th是用欧拉角表示，而PoseStamped中是用四元数表示角度用的，因此需要将th转换为四元数表示

        quaternion = quaternion_from_euler(0,0,th)

        pose.pose.orientation.x = quaternion[0]

        pose.pose.orientation.y = quaternion[1]

        pose.pose.orientation.z = quaternion[2]

        pose.pose.orientation.w = quaternion[3]

        #之前提到过PoseStamped消息类型是以列表的形式保存，因此需要将坐标和角度信息保存保存至msg中：

                # //nav_msgs/Path数据类型

                # Header header

                # geometry_msgs/PoseStamped[] poses

        msg.poses.append(pose)

        #发布消息

        pub_path.publish(msg)

        rospy.loginfo("x = {}, y = {}, th = {}".format(x, y, th))

        rate.sleep()

至此函数主体编写完成，再加个主函数。PS：我喜欢把init_node放在主函数中，大佬别喷。

if __name__ == "__main__":
    rospy.init_node("show_path", anonymous=True)
    try:
        pub_o_path()
    except rospy.ROSInterruptException:
        pass

程序运行后，运行rviz,在add中按Topic查找，找到你发布的Topic并添加进来即可

这里需要注意的有几点：

1、如果要对某个消息进行操作，需要将这个消息用一个变量代替，然后再进行操作，如：

msg = Path()

pose = PoseStamped()

我也不知道为啥不能直接操作，如不能使用Path.poses.pose.position.x = x ，而是用的pose.pose.position.x = x，如果直接用前者这种会报错

Traceback (most recent call last):
  File "/home/xxx/ProjectSet/Pycharm/Ros/catkin_ws/src/path/src/o.py", line 78, in <module>
    pub_o_path()
  File "/home/xxx/ProjectSet/Pycharm/Ros/catkin_ws/src/path/src/o.py", line 58, in pub_o_path
    Path.poses.pose.position.x = x
AttributeError: 'member_descriptor' object has no attribute 'pose'

2、获得的坐标以及四元数，后要其存入msg(也就是Path())的列表中

msg.poses.append(pose)

3、发布消息是发布Path()，而不是发布PoseStamped()

 pub_path.publish(msg)

 

本人刚开始学习ROS，如果有不对的地方，请大佬们不吝赐教，谢谢。

参考资料：

https://blog.csdn.net/u013834525/article/details/80447931

https://blog.csdn.net/ktigerhero3/article/details/70256437

https://www.programcreek.com/python/example/70252/geometry_msgs.msg.PoseStamped