ROS building block model

To create a block model in ROS, you first need to open rviz, create a marker in rviz and set the marker Topic. Initialize a node and a marker in the python file, change the X and Y values of the marker and publish the changed marker to realize the displacement of the object block.

1、Start rviz

Start vision.launch in mycobot_ai package under ROS working directory to start rviz. The specific commands are as follows:

Before starting this command, make sure that the port value in vision. Launch is consistent with the actual manipulator name.

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

2、Create Box

In the rviz interface, click Add to add a marker and set the marker Topic to test_maker。 As shown in the figure below. image-20210728173016739

image-20210728173112372

image-20210728173308183

3、Block movement

After opening rviz and creating the block, open the command line terminal and run the following command line to see the yellow block moving in the model.

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

4、Code explanation

​First, import some necessary ROS dependency libraries. Secondly, create a custom class and initialize a node, a publisher object and a marker class in the class initialization function. Finally, set some three-dimensional angles for the box to move in the model.

​Here we will explain in detail some attribute values of the following marker objects:

  • type:representation type;

  • action:indicates an action of this marker;;

  • ns :indicates the name of this marker;

  • scale:indicates the actual size of the object block;

  • x、y、z:respectively indicate its length, width and height, and the unit is meter;

  • color :indicates the color of the block, and a indicates transparency;

  • r、g、b:respectively correspond to RGB of the color;

  • pose.position:indicates the coordinates of the actual position;

  • x、y、z:respectively represent the length, width and height of three-dimensional coordinates;

  • pose.orientation :represents a four-dimensional vector; For four-dimensional vectors, it doesn't matter if you don't have special requirements and don't understand them.

# encoding: utf-8

import rospy
import time
from visualization_msgs.msg import marker

class Send_marker(object):
    def __init__(self):
        # Inherit object class objects
        super(Send_marker, self).__init__()
        # Initialize a node. If the node is not created, the information cannot be published
        rospy.init_node("send_marker", anonymous=True)
        # Create a publisher to publish marker
        self.pub = rospy.Publisher("/test_marker", marker, queue_size=1)
        # Create a block model that marker uses to create
        self.marker = marker()
        # Configure its ownership relationship, and its coordinates are relative to / joint1.
        # /Joint1 represents the bottom of the manipulator in the model.
        self.marker.header.frame_id = "/joint1"
        # Set marker's name
        self.marker.ns = "test_marker"
        # Set the type of marker to be square
        self.marker.type = self.marker.CUBE
        # Set marker's action to add (marker without this name will add one)
        self.marker.action = self.marker.ADD
        # Set the actual size of marker, in m
        self.marker.scale.x = 0.04
        self.marker.scale.y = 0.04
        self.marker.scale.z = 0.04
        # Set marker's color, 1.0 for 255 (this represents a ratio conversion)
        self.marker.color.a = 1.0
        self.marker.color.g = 1.0
        self.marker.color.r = 1.0
        # Initialize marker's position and its four-dimensional attitude
        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

    # Modify coordinates and publish marker
    def pub_marker(self, x, y, z=0.03):
        # Set marker's timestamp
        self.marker.header.stamp = rospy.Time.now()
        # Set marker's spatial coordinates
        self.marker.pose.position.x = x
        self.marker.pose.position.y = y
        self.marker.pose.position.z = z
        # Release marker
        self.pub.publish(self.marker)

    # Let marker happen 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()

results matching ""

    No results matching ""