diff --git a/include/rviz_visual_tools/rviz_visual_tools.h b/include/rviz_visual_tools/rviz_visual_tools.h index 6398aae..9a238c3 100644 --- a/include/rviz_visual_tools/rviz_visual_tools.h +++ b/include/rviz_visual_tools/rviz_visual_tools.h @@ -46,9 +46,10 @@ #include // Eigen -#include #include +#include + // Rviz #include #include @@ -57,12 +58,12 @@ #include // Messages -#include -#include -#include +#include #include #include -#include +#include +#include +#include #include // rviz_visual_tools @@ -502,15 +503,15 @@ class RvizVisualTools * \return true on success */ bool publishSpheres(const EigenSTL::vector_Vector3d& points, colors color = BLUE, scales scale = MEDIUM, - const std::string& ns = "Spheres"); + const std::string& ns = "Spheres", std::size_t id = 0); bool publishSpheres(const EigenSTL::vector_Vector3d& points, colors color, double scale = 0.1, - const std::string& ns = "Spheres"); + const std::string& ns = "Spheres", std::size_t id = 0); bool publishSpheres(const std::vector& points, colors color = BLUE, scales scale = MEDIUM, - const std::string& ns = "Spheres"); + const std::string& ns = "Spheres", std::size_t id = 0); bool publishSpheres(const std::vector& points, colors color = BLUE, double scale = 0.1, - const std::string& ns = "Spheres"); + const std::string& ns = "Spheres", std::size_t id = 0); bool publishSpheres(const std::vector& points, colors color, - const geometry_msgs::Vector3& scale, const std::string& ns = "Spheres"); + const geometry_msgs::Vector3& scale, const std::string& ns = "Spheres", std::size_t id = 0); /** * \brief Display a marker of a series of spheres, with the possibility of different colors @@ -521,9 +522,9 @@ class RvizVisualTools * \return true on success */ bool publishSpheres(const EigenSTL::vector_Vector3d& points, const std::vector& colors, scales scale = MEDIUM, - const std::string& ns = "Spheres"); + const std::string& ns = "Spheres", std::size_t id = 0); bool publishSpheres(const std::vector& points, const std::vector& colors, - const geometry_msgs::Vector3& scale, const std::string& ns = "Spheres"); + const geometry_msgs::Vector3& scale, const std::string& ns = "Spheres", std::size_t id = 0); /** * \brief Display an arrow along the x-axis of a pose @@ -649,12 +650,15 @@ class RvizVisualTools * \param bPoints - x,y,z of end of line, as a vector * \param colors - an enum pre-defined name of a color * \param scale - an enum pre-defined name of a size + * \param ns - namespace of marker * \return true on success */ bool publishLines(const EigenSTL::vector_Vector3d& aPoints, const EigenSTL::vector_Vector3d& bPoints, - const std::vector& colors, scales scale = MEDIUM); + const std::vector& colors, scales scale = MEDIUM, const std::string& ns = "Line Array", + std::size_t id = 0); bool publishLines(const std::vector& aPoints, const std::vector& bPoints, - const std::vector& colors, const geometry_msgs::Vector3& scale); + const std::vector& colors, const geometry_msgs::Vector3& scale, + const std::string& ns = "Line Array", std::size_t id = 0); /** * \brief Display a series of connected lines using the LINE_STRIP method - deprecated because visual bugs @@ -676,17 +680,19 @@ class RvizVisualTools * \return true on success */ bool publishPath(const std::vector& path, colors color = RED, scales scale = MEDIUM, - const std::string& ns = "Path"); + const std::string& ns = "Path", std::size_t id = 0); bool publishPath(const std::vector& path, colors color, scales scale, - const std::string& ns = "Path"); - bool publishPath(const EigenSTL::vector_Isometry3d& path, colors color, scales scale, const std::string& ns = "Path"); - bool publishPath(const EigenSTL::vector_Vector3d& path, colors color, scales scale, const std::string& ns = "Path"); + const std::string& ns = "Path", std::size_t id = 0); + bool publishPath(const EigenSTL::vector_Isometry3d& path, colors color, scales scale, const std::string& ns = "Path", + std::size_t id = 0); + bool publishPath(const EigenSTL::vector_Vector3d& path, colors color, scales scale, const std::string& ns = "Path", + std::size_t id = 0); bool publishPath(const std::vector& path, colors color = RED, double radius = 0.01, - const std::string& ns = "Path"); + const std::string& ns = "Path", std::size_t id = 0); bool publishPath(const EigenSTL::vector_Vector3d& path, colors color = RED, double radius = 0.01, - const std::string& ns = "Path"); + const std::string& ns = "Path", std::size_t id = 0); bool publishPath(const EigenSTL::vector_Isometry3d& path, colors color = RED, double radius = 0.01, - const std::string& ns = "Path"); + const std::string& ns = "Path", std::size_t id = 0); /** * \brief Display a marker of a series of connected colored cylinders @@ -819,11 +825,11 @@ class RvizVisualTools * \return true on success */ bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color = BLUE, - scales scale = MEDIUM, const std::string& ns = "Cylinder"); + scales scale = MEDIUM, const std::string& ns = "Cylinder", std::size_t id = 0); bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color, double radius = 0.01, - const std::string& ns = "Cylinder"); + const std::string& ns = "Cylinder", std::size_t id = 0); bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std_msgs::ColorRGBA& color, - double radius = 0.01, const std::string& ns = "Cylinder"); + double radius = 0.01, const std::string& ns = "Cylinder", std::size_t id = 0); /** * \brief Display a marker of a cylinder @@ -834,11 +840,11 @@ class RvizVisualTools * \return true on success */ bool publishCylinder(const Eigen::Isometry3d& pose, colors color = BLUE, double height = 0.1, double radius = 0.01, - const std::string& ns = "Cylinder"); + const std::string& ns = "Cylinder", std::size_t id = 0); bool publishCylinder(const geometry_msgs::Pose& pose, colors color = BLUE, double height = 0.1, double radius = 0.01, - const std::string& ns = "Cylinder"); + const std::string& ns = "Cylinder", std::size_t id = 0); bool publishCylinder(const geometry_msgs::Pose& pose, const std_msgs::ColorRGBA& color, double height = 0.1, - double radius = 0.01, const std::string& ns = "Cylinder"); + double radius = 0.01, const std::string& ns = "Cylinder", std::size_t id = 0); /** * \brief Display a mesh from file diff --git a/src/rviz_visual_tools.cpp b/src/rviz_visual_tools.cpp index 1911b96..73b188c 100644 --- a/src/rviz_visual_tools.cpp +++ b/src/rviz_visual_tools.cpp @@ -39,9 +39,9 @@ #include // Conversions +#include #include #include -#include // C++ #include @@ -241,7 +241,7 @@ bool RvizVisualTools::loadRvizMarkers() // Create a sphere point // Add the point pair to the line message sphere_marker_.points.resize(1); - sphere_marker_.colors.resize(1); + // sphere_marker_.colors.resize(1); // Lifetime sphere_marker_.lifetime = marker_lifetime_; // Constants @@ -812,7 +812,7 @@ bool RvizVisualTools::publishMarker(visualization_msgs::Marker& marker) { // Add single marker to array marker.frame_locked = frame_locking_enabled_; - markers_.markers.push_back(marker); + markers_.markers.emplace_back(marker); // Determine if we should publish now if (!batch_publishing_enabled_) @@ -1522,20 +1522,21 @@ bool RvizVisualTools::publishAxisPath(const EigenSTL::vector_Isometry3d& path, d } bool RvizVisualTools::publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color, - scales scale, const std::string& ns) + scales scale, const std::string& ns, std::size_t id) { double radius = getScale(scale).x; - return publishCylinder(point1, point2, getColor(color), radius, ns); + return publishCylinder(point1, point2, getColor(color), radius, ns, id); } bool RvizVisualTools::publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, colors color, - double radius, const std::string& ns) + double radius, const std::string& ns, std::size_t id) { - return publishCylinder(point1, point2, getColor(color), radius, ns); + return publishCylinder(point1, point2, getColor(color), radius, ns, id); } bool RvizVisualTools::publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, - const std_msgs::ColorRGBA& color, double radius, const std::string& ns) + const std_msgs::ColorRGBA& color, double radius, const std::string& ns, + std::size_t id) { // Distance between two points double height = (point1 - point2).lpNorm<2>(); @@ -1553,28 +1554,35 @@ bool RvizVisualTools::publishCylinder(const Eigen::Vector3d& point1, const Eigen pose = pose * rotation; // Turn into msg - return publishCylinder(convertPose(pose), color, height, radius, ns); + return publishCylinder(convertPose(pose), color, height, radius, ns, id); } bool RvizVisualTools::publishCylinder(const Eigen::Isometry3d& pose, colors color, double height, double radius, - const std::string& ns) + const std::string& ns, std::size_t id) { - return publishCylinder(convertPose(pose), color, height, radius, ns); + return publishCylinder(convertPose(pose), color, height, radius, ns, id); } bool RvizVisualTools::publishCylinder(const geometry_msgs::Pose& pose, colors color, double height, double radius, - const std::string& ns) + const std::string& ns, std::size_t id) { - return publishCylinder(pose, getColor(color), height, radius, ns); + return publishCylinder(pose, getColor(color), height, radius, ns, id); } bool RvizVisualTools::publishCylinder(const geometry_msgs::Pose& pose, const std_msgs::ColorRGBA& color, double height, - double radius, const std::string& ns) + double radius, const std::string& ns, std::size_t id) { // Set the timestamp cylinder_marker_.header.stamp = ros::Time::now(); cylinder_marker_.ns = ns; - cylinder_marker_.id++; + if (id == 0) + { + cylinder_marker_.id++; + } + else + { + cylinder_marker_.id = id; + } // Set the pose cylinder_marker_.pose = pose; @@ -1892,7 +1900,8 @@ bool RvizVisualTools::publishLine(const geometry_msgs::Point& point1, const geom } bool RvizVisualTools::publishLines(const EigenSTL::vector_Vector3d& aPoints, const EigenSTL::vector_Vector3d& bPoints, - const std::vector& colors, scales scale) + const std::vector& colors, scales scale, const std::string& ns, + std::size_t id) { BOOST_ASSERT_MSG(aPoints.size() == bPoints.size() && bPoints.size() == colors.size(), "Mismatching vector sizes: " "aPoints, bPoints, and colors"); @@ -1909,23 +1918,31 @@ bool RvizVisualTools::publishLines(const EigenSTL::vector_Vector3d& aPoints, con // Convert color to ROS Msg colors_msg.push_back(getColor(colors[i])); } - BOOST_ASSERT_MSG(a_points_msg.size() == b_points_msg.size() && b_points_msg.size() == colors_msg.size(), - "Mismatched " - "vector sizes"); + BOOST_ASSERT_MSG(a_points_msg.size() == b_points_msg.size() && b_points_msg.size() == colors_msg.size(), "Mismatched " + "vector " + "sizes"); - return publishLines(a_points_msg, b_points_msg, colors_msg, getScale(scale)); + return publishLines(a_points_msg, b_points_msg, colors_msg, getScale(scale), ns, id); } bool RvizVisualTools::publishLines(const std::vector& aPoints, const std::vector& bPoints, - const std::vector& colors, const geometry_msgs::Vector3& scale) + const std::vector& colors, const geometry_msgs::Vector3& scale, + const std::string& ns, std::size_t id) { // Setup marker line_list_marker_.header.stamp = ros::Time(); - line_list_marker_.ns = "Line Array"; + line_list_marker_.ns = ns; // Provide a new id every call to this function - line_list_marker_.id++; + if (id == 0) + { + line_list_marker_.id++; + } + else + { + line_list_marker_.id = id; + } line_list_marker_.scale = scale; line_list_marker_.scale.z = 0; @@ -1944,6 +1961,8 @@ bool RvizVisualTools::publishLines(const std::vector& aPoi line_list_marker_.colors.push_back(colors[i]); } + line_list_marker_.lifetime = marker_lifetime_; + // Testing BOOST_ASSERT_MSG(line_list_marker_.colors.size() == line_list_marker_.points.size(), "Arrays mismatch in size"); BOOST_ASSERT_MSG(line_list_marker_.colors.size() == aPoints.size() * 2, "Colors arrays mismatch in size"); @@ -1989,7 +2008,7 @@ bool RvizVisualTools::publishLineStrip(const std::vector& } bool RvizVisualTools::publishPath(const std::vector& path, colors color, scales scale, - const std::string& ns) + const std::string& ns, std::size_t id) { std::vector point_path(path.size()); for (std::size_t i = 0; i < path.size(); ++i) @@ -1997,29 +2016,29 @@ bool RvizVisualTools::publishPath(const std::vector& path, point_path[i] = path[i].position; } - return publishPath(point_path, color, getScale(scale).x, ns); + return publishPath(point_path, color, getScale(scale).x, ns, id); } bool RvizVisualTools::publishPath(const std::vector& path, colors color, scales scale, - const std::string& ns) + const std::string& ns, std::size_t id) { - return publishPath(path, color, getScale(scale).x, ns); + return publishPath(path, color, getScale(scale).x, ns, id); } bool RvizVisualTools::publishPath(const EigenSTL::vector_Isometry3d& path, colors color, scales scale, - const std::string& ns) + const std::string& ns, std::size_t id) { - return publishPath(path, color, getScale(scale).x, ns); + return publishPath(path, color, getScale(scale).x, ns, id); } bool RvizVisualTools::publishPath(const EigenSTL::vector_Vector3d& path, colors color, scales scale, - const std::string& ns) + const std::string& ns, std::size_t id) { - return publishPath(path, color, getScale(scale).x, ns); + return publishPath(path, color, getScale(scale).x, ns, id); } bool RvizVisualTools::publishPath(const std::vector& path, colors color, double radius, - const std::string& ns) + const std::string& ns, std::size_t id) { if (path.size() < 2) { @@ -2030,14 +2049,14 @@ bool RvizVisualTools::publishPath(const std::vector& path, // Create the cylinders for (std::size_t i = 1; i < path.size(); ++i) { - publishCylinder(convertPoint(path[i - 1]), convertPoint(path[i]), color, radius, ns); + publishCylinder(convertPoint(path[i - 1]), convertPoint(path[i]), color, radius, ns, id); } return true; } bool RvizVisualTools::publishPath(const EigenSTL::vector_Vector3d& path, colors color, double radius, - const std::string& ns) + const std::string& ns, std::size_t id) { if (path.size() < 2) { @@ -2048,14 +2067,14 @@ bool RvizVisualTools::publishPath(const EigenSTL::vector_Vector3d& path, colors // Create the cylinders for (std::size_t i = 1; i < path.size(); ++i) { - publishCylinder(path[i - 1], path[i], color, radius, ns); + publishCylinder(path[i - 1], path[i], color, radius, ns, id); } return true; } bool RvizVisualTools::publishPath(const EigenSTL::vector_Isometry3d& path, colors color, double radius, - const std::string& ns) + const std::string& ns, std::size_t id) { if (path.size() < 2) { @@ -2066,7 +2085,7 @@ bool RvizVisualTools::publishPath(const EigenSTL::vector_Isometry3d& path, color // Create the cylinders for (std::size_t i = 1; i < path.size(); ++i) { - publishCylinder(path[i - 1].translation(), path[i].translation(), color, radius, ns); + publishCylinder(path[i - 1].translation(), path[i].translation(), color, radius, ns, id); } return true; @@ -2381,7 +2400,7 @@ bool RvizVisualTools::publishWireframeRectangle(const Eigen::Isometry3d& pose, c } bool RvizVisualTools::publishSpheres(const EigenSTL::vector_Vector3d& points, colors color, scales scale, - const std::string& ns) + const std::string& ns, std::size_t id) { std::vector points_msg; @@ -2390,11 +2409,11 @@ bool RvizVisualTools::publishSpheres(const EigenSTL::vector_Vector3d& points, co points_msg.push_back(convertPoint(point)); } - return publishSpheres(points_msg, color, scale, ns); + return publishSpheres(points_msg, color, scale, ns, id); } bool RvizVisualTools::publishSpheres(const EigenSTL::vector_Vector3d& points, colors color, double scale, - const std::string& ns) + const std::string& ns, std::size_t id) { std::vector points_msg; @@ -2403,39 +2422,47 @@ bool RvizVisualTools::publishSpheres(const EigenSTL::vector_Vector3d& points, co points_msg.push_back(convertPoint(point)); } - return publishSpheres(points_msg, color, scale, ns); + return publishSpheres(points_msg, color, scale, ns, id); } bool RvizVisualTools::publishSpheres(const std::vector& points, colors color, double scale, - const std::string& ns) + const std::string& ns, std::size_t id) { geometry_msgs::Vector3 scale_vector; scale_vector.x = scale * global_scale_; scale_vector.y = scale * global_scale_; scale_vector.z = scale * global_scale_; - return publishSpheres(points, color, scale_vector, ns); + return publishSpheres(points, color, scale_vector, ns, id); } bool RvizVisualTools::publishSpheres(const std::vector& points, colors color, scales scale, - const std::string& ns) + const std::string& ns, std::size_t id) { - return publishSpheres(points, color, getScale(scale), ns); + return publishSpheres(points, color, getScale(scale), ns, id); } bool RvizVisualTools::publishSpheres(const std::vector& points, colors color, - const geometry_msgs::Vector3& scale, const std::string& ns) + const geometry_msgs::Vector3& scale, const std::string& ns, std::size_t id) { spheres_marker_.header.stamp = ros::Time(); spheres_marker_.ns = ns; - // Provide a new id every call to this function - spheres_marker_.id++; + if (id == 0) + { // Provide a new id every call to this function + spheres_marker_.id++; + } + else + { + spheres_marker_.id = id; + } std_msgs::ColorRGBA this_color = getColor(color); spheres_marker_.scale = scale; spheres_marker_.color = this_color; spheres_marker_.colors.clear(); + spheres_marker_.lifetime = marker_lifetime_; + spheres_marker_.points = points; // straight copy // Convert path coordinates @@ -2449,7 +2476,7 @@ bool RvizVisualTools::publishSpheres(const std::vector& po } bool RvizVisualTools::publishSpheres(const EigenSTL::vector_Vector3d& points, const std::vector& colors, - scales scale, const std::string& ns) + scales scale, const std::string& ns, std::size_t id) { BOOST_ASSERT_MSG(points.size() == colors.size(), "Mismatching vector sizes: points and colors"); @@ -2464,18 +2491,25 @@ bool RvizVisualTools::publishSpheres(const EigenSTL::vector_Vector3d& points, co colors_msg.push_back(getColor(colors[i])); } - return publishSpheres(points_msg, colors_msg, getScale(scale), ns); + return publishSpheres(points_msg, colors_msg, getScale(scale), ns, id); } bool RvizVisualTools::publishSpheres(const std::vector& points, const std::vector& colors, - const geometry_msgs::Vector3& scale, const std::string& ns) + const geometry_msgs::Vector3& scale, const std::string& ns, std::size_t id) { spheres_marker_.header.stamp = ros::Time(); spheres_marker_.ns = ns; - // Provide a new id every call to this function - spheres_marker_.id++; + if (id == 0) + { + // Provide a new id every call to this function + spheres_marker_.id++; + } + else + { + spheres_marker_.id = id; + } spheres_marker_.scale = scale; spheres_marker_.points = points; // straight copy