ROS rviz display (visualization_msgs/Marker)

Reference blog:
(1) ROS’s rviz displays various methods of historical movement trajectories and paths (visualization_msgs/Marker, nav_msgs/Path)
(2) ROS Notes visualization_msgs-Marker Learning
(3) Publish odometer information based on ROS

Reference documentation:
(1) rvizDisplayTypesMarker
(2) rvizTutorialsMarkers: Basic Shapes

0 Preface

(1) In ROS, odom, base_link and map are common coordinate systems (frames) used for robot positioning and navigation.

• The odom coordinate system is the odometer coordinate system of the robot, relative to the movement of the starting position.
• The base_link coordinate system is the body coordinate system of the robot and has nothing to do with the movement of the robot.
• The map coordinate system is the global map coordinate system where the robot is located, providing a fixed reference frame for positioning and navigating the robot’s position in the global environment.

(2) visualization_msgs::MarkerArray is a message type in ROS, used to publish multiple Marker arrays in RViz
The visualization_msgs::MarkerArray message consists of the following fields:

markers (std::vector): An array containing multiple visualization_msgs::Marker objects. Each Marker object describes a visual element, such as point, line, arrow, text, etc.

By using the visualization_msgs::MarkerArray message, you can publish multiple Markers at the same time and display them as an array in RViz.
(3) Some important fields of the visualization_msgs/Marker message

• header: The header information of the message, including frame_id and timestamp, etc.
• ns: namespace, used to group Markers.
• id: The unique identifier of the Marker, used to identify different Markers.
• type: The type of Marker, specifying the shape to be displayed. It can be set using the constants defined by the visualization_msgs/Marker message, such as visualization_msgs::Marker::SPHERE indicating a sphere.
• action: The operation type of the Marker, specifying how to handle the Marker in RViz. Common – operation types include ADD, DELETE and DELETEALL.
• pose: Marker’s pose, specifying the Marker’s position and direction in three-dimensional space.
• scale: The size of the Marker, used to control the size or line width of the Marker.
• color: The color of the Marker. You can set the RGBA value to specify the color and opacity.
• points: A list of points for shapes such as lines and polygons. Each point is represented by a geometry_msgs/- Point message.
• text: String content for the text shape.
• lifetime: The life cycle of the Marker, used to control the display time of the Marker in RViz.

(4) Types of various markers
Can be viewed at rvizDisplayTypesMarker
Here are some commonly used ones:

ARROW=0//Arrow
CUBE=1//cube
SPHERE=2//ball
CYLINDER=3//Cylinder
LINE_STRIP=4//Line (connection of points)
LINE_LIST=5//Line sequence
CUBE_LIST=6//Cube sequence
SPHERE_LIST=7//ball sequence
POINTS=8//point set
TEXT_VIEW_FACING=9//Display 3D text
MESH_RESOURCE=10//Grid
TRIANGLE_LIST=11//Triangle sequence

1 Realize drawing a circular trajectory in rviz

Refer to the boss’s blog: ROS displays trajectory in rviz in real time (use of nav_msgs/Path message)
(1) New construction

mkdir -p showpath/src
cd src
catkin_create_pkg showpath roscpp rospy sensor_msgs std_msgs nav_msgs tf
cd..
catkin_make

(2) Edit the main function showpath.cpp

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>

main (int argc, char **argv)
{<!-- -->
    ros::init (argc, argv, "showpath");

    ros::NodeHandleph;
    ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("trajectory",1, true);

    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    nav_msgs::Path path;
    //nav_msgs::Path path;
    path.header.stamp=current_time;
    path.header.frame_id="odom";


    double x = 0.0;
    double y = 0.0;
    double th = 0.0;
    double vx = 0.1; // x-direction speed
    double vy = -0.1;//y-direction velocity
    double vth = 0.1;//angular velocity

    ros::Rate loop_rate(1);
    while (ros::ok())
    {<!-- -->

        current_time = ros::Time::now();
        //compute odometry in a typical way given the velocities of the robot
        double dt = 0.1;
        double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
        double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
        double delta_th = vth * dt;

        x + = delta_x;
        y + = delta_y;
        th + = delta_th;


        geometry_msgs::PoseStamped this_pose_stamped;
        this_pose_stamped.pose.position.x = x;
        this_pose_stamped.pose.position.y = y;

        geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(th);
        this_pose_stamped.pose.orientation.x = goal_quat.x;
        this_pose_stamped.pose.orientation.y = goal_quat.y;
        this_pose_stamped.pose.orientation.z = goal_quat.z;
        this_pose_stamped.pose.orientation.w = goal_quat.w;

        this_pose_stamped.header.stamp=current_time;
        this_pose_stamped.header.frame_id="odom";
        path.poses.push_back(this_pose_stamped);

        path_pub.publish(path);
        ros::spinOnce(); // check for incoming messages

        last_time = current_time;
        loop_rate.sleep();
    }

    return 0;
}

(3)vimCMakeLists.txt

add_executable(showpath src/showpath.cpp)
target_link_libraries(showpath ${catkin_LIBRARIES})

(4) Compile and run
Move to the created project workspace

catkin_make

(5) Enter in different terminals

roscore

source ./devel/setup.bash
rosrun showpath showpath

rostopic echo /trajectory

rosrun rviz rviz

Enter odom in the Fixed Frame of globe option
Click add on the left
Select path
Select in the topic option of path
/trajectory

2 visualization_msgs/Marker

In the visualization_msgs::Marker message type, the pose.position field is used to describe the position of the visualization element. It is a field of type geometry_msgs::Point, containing three components x, y and z, Represents the position of the visual elements in the coordinate system respectively. This location is the center point or base point of the visual element.

1. First you need to publish the visualization_marker topic

ros::Publisher vis_pub = node_handle.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );

2. Then just fill in a visualization_msgs/Marker message and publish it

visualization_msgs::Marker marker;
marker.header.frame_id = "base_link";
marker.header.stamp = ros::Time();
marker.ns = "my_namespace";
marker.id = 0;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = 1;
marker.pose.position.y = 1;
marker.pose.position.z = 1;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = 1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
marker.color.a = 1.0; // Don't forget to set the alpha!
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
//only if using a MESH_RESOURCE marker type:
marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
vis_pub.publish(marker);

Don’t forget to set marker.color.a = 1, otherwise the marker will not be visible

There is also a visualization_msgs/MarkerArray message, which allows you to publish multiple markers at the same time. In this case you want to publish in visualization_marker_array topic.
Reference Code:

#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>

int main(int argc, char** argv)
{<!-- -->
    ros::init(argc, argv, "marker_publisher");
    ros::NodeHandle nh;

    ros::Publisher marker_array_pub = nh.advertise<visualization_msgs::MarkerArray>("marker_array", 10);
    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("marker", 10);

    // Create MarkerArray message
    visualization_msgs::MarkerArray marker_array;

    //Create the first Marker
    visualization_msgs::Marker marker1;
    marker1.header.frame_id = "map";
    marker1.header.stamp = ros::Time::now();
    marker1.lifetime = ros::Duration(); //Set the persistence attribute to false
    marker1.ns = "marker1";
    marker1.id = 1;
    marker1.type = visualization_msgs::Marker::SPHERE;
    marker1.pose.position.x = 1.0;
    marker1.pose.position.y = 2.0;
    marker1.pose.position.z = 0.0;
    marker1.pose.orientation.w = 1.0;
    marker1.scale.x = 0.5;
    marker1.scale.y = 0.5;
    marker1.scale.z = 0.5;
    marker1.color.r = 1.0;
    marker1.color.g = 0.0;
    marker1.color.b = 0.0;
    marker1.color.a = 1.0;

    //Create a second Marker
    visualization_msgs::Marker marker2;
    marker2.header.frame_id = "map";
    marker2.header.stamp = ros::Time::now();
    marker2.lifetime = ros::Duration(); //Set the persistence attribute to false
    marker2.ns = "marker2";
    marker2.id = 2;
    marker2.type = visualization_msgs::Marker::CUBE;
    marker2.pose.position.x = -1.0;
    marker2.pose.position.y = 2.0;
    marker2.pose.position.z = 0.0;
    marker2.pose.orientation.w = 1.0;
    marker2.scale.x = 0.5;
    marker2.scale.y = 0.5;
    marker2.scale.z = 0.5;
    marker2.color.r = 0.0;
    marker2.color.g = 1.0;
    marker2.color.b = 0.0;
    marker2.color.a = 1.0;

    marker_array.markers.clear();
    marker_array.markers.push_back(marker1);
    marker_array.markers.push_back(marker2);

    ros::Rate rate(1); // The publishing frequency is 1Hz

    while (ros::ok()) {<!-- -->
        //Publish MarkerArray message
        marker_array_pub.publish(marker_array);

        // Publish a single Marker message
        marker_pub.publish(marker1);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

Note that the timestamp attached to the marked message above is ros::Time(), which is time Zero (0). RViz handles this differently than any other time. If you use ros::Time::now() or any other non-zero value, rviz will only show the marker if that time is close enough to the current time, where “close enough” depends on the TF. However, with time 0, the marker will be displayed regardless of the current time.

3 Practical combat

(1) Create a new project

mkdir -p catkin_ws/src
cd catkin_ws
catikin_create_pkg test roscpp rospy std_msgs nav_msgs tf
catkin_make

(2) Create a new test.cpp in the catkin_ws/src/test/src directory and modify CMakeLists.txt

vim test.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/Marker.h>

ros::Publisher car_model;
visualization_msgs::Marker car_marker;

void visual_car(const double x, const double y, const double th)
{<!-- -->
    car_marker.pose.orientation = tf::createQuaternionMsgFromYaw(th);
    car_marker.header.frame_id = "map";
    car_marker.header.stamp = ros::Time::now();
    car_marker.ns = "basic_shapes";
    
    car_marker.id = 0;
    car_marker.type = visualization_msgs::Marker::CUBE;// cube
    car_marker.action = visualization_msgs::Marker::ADD;

    car_marker.pose.position.x = 3.0;
    car_marker.pose.position.y = 4.0;
    car_marker.pose.position.z = 0;
    car_marker.scale.x = 4.0; // car commander
    car_marker.scale.y = 2.0; // car width
    car_marker.scale.z = 1.5; // car height
    car_marker.color.a = 1.0;
    car_marker.color.r = 1.0f;
    car_marker.color.g = 1.0f;
    car_marker.color.b = 0.0f;
    car_marker.lifetime = ros::Duration();
    car_model.publish(car_marker);
}

int main(int argc, char** argv)
{<!-- -->
    ros::init(argc, argv, "Car");
    ros::NodeHandle n;
    car_model = n.advertise<visualization_msgs::Marker>("/ydw/car", 100);
    
    ros::Rate rate(10);
    while (ros::ok())
    {<!-- -->
        visual_car(0.0, 0.0, 0.0);
        rate.sleep();
    }
    return 0;
}

vim CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(test)

find_package(catkin REQUIRED COMPONENTS
  nav_msgs
  roscpp
  rospy
  std_msgs
  tf
)

catkin_package(
  INCLUDE_DIRS include
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_executable(test1 src/test.cpp)

target_link_libraries(test1
  ${catkin_LIBRARIES}
)

(3) Compile and open the following terminal. A blank line represents a new terminal.

catkin_make

roscore

source devel/setup.bash
rosrun test test1

rosrun rviz rviz

(4) After opening the rviz interface, add Marker