visualization_msgs: can't visualize LINE on RViz

asked 2020-04-22 14:26:12 -0500

RayROS gravatar image

updated 2020-04-22 15:03:35 -0500

Hello I am trying to draw a line along the y-axes right when launching RViz. You can find the small example node here if you would like to give it a try.

Below a print screen of what is happening when launching RViz (basically I can't see the line):

happening

Below a print screen of what I am trying to achieve when launching RViz:

expected

Node starts correctly. I set the initial static-transform in the following way:

rosrun tf static_transform_publisher 0 0 0 0 0 0 1 map line_frame 10

Message from rosrun rqt_tf_tree rqt_tf_tree says that everything is working correctly, below a screenshot showing the tree:

tf_tree

#include <ros/ros.h>
#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <math.h>
#include <visualization_msgs/Marker.h>
// for drawing y lines as reference along y axis of the lidar
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
#include <pcl/pcl_config.h>


class DrawLinePublish
{
    // this function will show the y along the whole ax

    void draw_line(const tf2::Vector3 &p1, const tf2::Vector3 &p2,
              float r, float g, float b, int id){

              visualization_msgs::Marker line;
              line.type = visualization_msgs::Marker::LINE_LIST;
              line.action = visualization_msgs::Marker::MODIFY;

              line.header.frame_id = "/line_frame";
              line.color.r = r;
              line.color.g = g;
              line.color.b = b;
              line.color.a = 1.0f;
              line.id = id;
              line.pose.position.x = 0;
              line.pose.position.y = 0;
              line.pose.orientation.w = 1;
              geometry_msgs::Point gp1, gp2;
              gp1.x = p1.x();
              gp1.y = p1.y();
              gp1.z = p1.z();
              gp2.x = p2.x();
              gp2.y = p2.y();
              gp2.z = p2.z();
              line.points.push_back(gp1);
              line.points.push_back(gp2);
              line_pub.publish(line);
    }             

    void clear_line(int id) {
        visualization_msgs::Marker line;
        line.type = visualization_msgs::Marker::LINE_LIST;
        line.action = visualization_msgs::Marker::DELETE;
        line.id = id;
        line_pub.publish(line);
    }

    public:
    DrawLinePublish()
    {
        this->line_sub = this->nh.subscribe<visualization_msgs::Marker>("/velodyne_lines", 5, &DrawLinePublish::linesCallBack, this);
        this->line_pub = this->nh.advertise<visualization_msgs::Marker> ("output", 1);
    }

    void linesCallBack(visualization_msgs::Marker line)
    {      
        std::cout << "Received measurement: lines are drawing " << std::endl;
        std::cout << "-------- Received lines callback --------- " << std::endl;

        // Green lines for the y axes (Basically the origin of the reference frame as shown in picture)
        draw_line(tf2::Vector3(0, 10, 0),
                tf2::Vector3(0, -10, 0), 0, 1, 0, 20000);
    }


    private:
        ros::NodeHandle nh;
        ros::Subscriber line_sub;
        ros::Publisher line_pub;
};

int main(int argv, char** argc)
{
    // initialise the node  
    ros::init(argv, argc, "linedrawing");
    std::cout << "Process node initialised" << std::endl;
    std::cout << "PCL VERSION = " << PCL_VERSION << std::endl;
    // create instance
    DrawLinePublish process;
    // Handle ROS communication events
    ros::spin();
    return 0;
}

The whole node starts without any problem, all the reference frames seems to be fine and the visualization_msgs is correctly added to RViz but the line does not show up. What am I missing? Thank you for pointing to the right direction for solving the problem.

edit retag flag offensive close merge delete