ROS建立方块模型

​ 在ros中建立方块模型，首先需要开启rviz，并在rviz创建一个marker并设置Marker Topic。在python文件中初始化一个节点以及一个Marker，改变Marker的X，Y值并发布改变后的Marker，就可以实现物块的位移了。

1、启动rviz

启动ros工作目录下的mycobot_ai包内的vision.launch即可启动rviz。具体命令如下：

启动该命令前，请确保vision.launch中的port值与实际机械臂名称保持一致。

roslaunch <ros-workspace>/src/mycobot/mycobot_ai/launch/vision.launch

2、创建方块

在rviz界面中点击add添加一个marker，并设置Marker Topic为test_marker。如下图所示。

3、实现方块移动

开启rviz，并创建方块后，打开命令行终端运行以下命令行即可看见黄色物块在模型中运动。

python <ros-workspace>/src/mycobot/mycobot_ai/scripts/send_marker.py

4、代码讲解

​ 首先，导入一些必要的ros依赖库。其次创建一个自定义类，在类的初始化函数中初始化一个节点、一个发布者对象以及一个Marker类。最后，为方块设置一些三维空间角度，让其在模型中移动。

​ 这里详细讲解以下Marker对象的一些属性值：type表示类型；action表示这个Marker的一个动作；ns表示这个Marker的名称；scale表示物块的实际大小x、y、z分别表示其长、宽、高单位是米；color表示物块的颜色a表示透明度、r、g、b分别对应颜色的RGB；pose.position表示实际位置的坐标，x、y、z分别表示三维坐标的长宽高；pose.orientation表示一个四维向量；对于四维向量，如果没有特别的需求对其不了解也没有关系。

# encoding: utf-8

import rospy
import time
from visualization_msgs.msg import Marker

class Send_marker(object):
    def __init__(self):
        # 继承object类对象
        super(Send_marker, self).__init__()
        # 初始化一个节点，如果没有创建节点会导致无法发布信息
        rospy.init_node("send_marker", anonymous=True)
        # 创建一个发布者，用来发布marker
        self.pub = rospy.Publisher("/test_marker", Marker, queue_size=1)
        # 创建一个marker用来创建方块模型
        self.marker = Marker()
        # 配置其所属关系，其坐标均是相对于/joint1而言的。
        # /joint1在模型中代表机械臂的底部
        self.marker.header.frame_id = "/joint1"
        # 设置marker的名称
        self.marker.ns = "test_marker"
        # 设置marker的类型是方块
        self.marker.type = self.marker.CUBE
        # 设置marker的动作为添加（没有这个名称的marker就为其添加一个）
        self.marker.action = self.marker.ADD
        # 设置marker的实际大小情况，单位为m
        self.marker.scale.x = 0.04
        self.marker.scale.y = 0.04
        self.marker.scale.z = 0.04
        # 设置marker的颜色，1.0表示255（这表示着一种比率换算）
        self.marker.color.a = 1.0
        self.marker.color.g = 1.0
        self.marker.color.r = 1.0
        # 初始化marker的位置以及其四维姿态
        self.marker.pose.position.x = 0
        self.marker.pose.position.y = 0
        self.marker.pose.position.z = 0.03
        self.marker.pose.orientation.x = 0
        self.marker.pose.orientation.y = 0
        self.marker.pose.orientation.z = 0
        self.marker.pose.orientation.w = 1.0

    # 修改坐标并发布marker
    def pub_marker(self, x, y, z=0.03):
        # 设置marker的时间戳
        self.marker.header.stamp = rospy.Time.now()
        # 设置marker的空间坐标
        self.marker.pose.position.x = x
        self.marker.pose.position.y = y
        self.marker.pose.position.z = z
        # 发布marker
        self.pub.publish(self.marker)

    # 让marker发生y
    def run(self):
        time.sleep(1) 
        self.pub_marker(0.1, -0.1)
        time.sleep(1)
        self.pub_marker(0.15, 0)
        time.sleep(1)
        self.pub_marker(0.15, -0.1)
        time.sleep(1)
        self.pub_marker(0.2, -0.1)
        time.sleep(1)
        self.pub_marker(0.15, -0.05)
        time.sleep(1)


if __name__ == '__main__':
    marker = Send_marker()
    marker.run()