ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

change the ns value like:

void draw_box(PointT min_point, PointT max_point, int number) {
    static ros::Publisher pub_marker = nh_.advertise<visualization_msgs::Marker>("/seg_box",1);
    visualization_msgs::Marker lines;
    lines.header.frame_id = MAIN_FRAME_ID;
    lines.header.stamp = ros::Time::now();
    lines.type = visualization_msgs::Marker::LINE_LIST;
    lines.action = visualization_msgs::Marker::ADD;
    lines.scale.x = 0.01;
    lines.scale.y = 0.01;
    lines.color.a = 1.0;

    std::stringstream ss;
    ss << number;

    lines.ns = "my lines_" + ss.str();
    lines.id = 1;

    geometry_msgs::Point pt1;
    pt1.x = min_point.x;
    pt1.y = min_point.y;
    pt1.z = min_point.z;

    geometry_msgs::Point pt2;
    pt2.x = min_point.x;
    pt2.y = min_point.y;
    pt2.z = max_point.z;

    geometry_msgs::Point pt3;
    pt3.x = max_point.x;
    pt3.y = min_point.y;
    pt3.z = min_point.z;

    geometry_msgs::Point pt4;
    pt4.x = max_point.x;
    pt4.y = min_point.y;
    pt4.z = max_point.z;

    geometry_msgs::Point pt5;
    pt5.x = min_point.x;
    pt5.y = max_point.y;
    pt5.z = min_point.z;

    geometry_msgs::Point pt6;
    pt6.x = min_point.x;
    pt6.y = max_point.y;
    pt6.z = max_point.z;

    geometry_msgs::Point pt7;
    pt7.x = max_point.x;
    pt7.y = max_point.y;
    pt7.z = min_point.z;

    geometry_msgs::Point pt8;
    pt8.x = max_point.x;
    pt8.y = max_point.y;
    pt8.z = max_point.z;

    lines.points.push_back(pt1);
    lines.points.push_back(pt2);
    lines.points.push_back(pt1);
    lines.points.push_back(pt3);
    lines.points.push_back(pt3);
    lines.points.push_back(pt4);
    lines.points.push_back(pt2);
    lines.points.push_back(pt4);

    lines.points.push_back(pt5);
    lines.points.push_back(pt6);
    lines.points.push_back(pt5);
    lines.points.push_back(pt7);
    lines.points.push_back(pt7);
    lines.points.push_back(pt8);
    lines.points.push_back(pt8);
    lines.points.push_back(pt6);


    lines.points.push_back(pt2);
    lines.points.push_back(pt6);
    lines.points.push_back(pt1);
    lines.points.push_back(pt5);
    lines.points.push_back(pt4);
    lines.points.push_back(pt8);
    lines.points.push_back(pt3);
    lines.points.push_back(pt7);



    if (number == 0) {
        lines.color.r = 1.0f;
    }
    else if (number == 1) {
        lines.color.g = 1.0f;
    }   else if (number == 2) {
        lines.color.b = 1.0f;
    }else if (number == 3) {
        lines.color.b = 1.0f;
        lines.color.g = 1.0f;
    }else if (number == 4) {
        lines.color.r = 1.0f;
        lines.color.g = 1.0f;
    }else if (number == 5) {
        lines.color.r = 1.0f;
        lines.color.b = 1.0f;
    }else if (number > 5) {
        lines.color.r = 1.0f;
        lines.color.g = 1.0f;
        lines.color.b = 1.0f;
    }

    pub_marker.publish(lines);

}