.. _program_listing_file__tmp_ws_src_rviz_visual_tools_include_rviz_visual_tools_rviz_visual_tools.hpp: Program Listing for File rviz_visual_tools.hpp ============================================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/rviz_visual_tools/include/rviz_visual_tools/rviz_visual_tools.hpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp // Copyright 2021 PickNik Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistributions of source code must retain the above copyright // notice, this list of conditions and the following disclaimer. // // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * Neither the name of the PickNik Inc. nor the names of its // contributors may be used to endorse or promote products derived from // this software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. /* Author: Dave Coleman , Andy McEvoy Desc: Helper functions for displaying basic shape markers in Rviz */ #pragma once // rviz_visual_tools #include #include // C++ #include #include // Eigen #include #include // Rviz #include #include // Messages #include #include // #include #include #include #include #include // Import/export for windows dll's and visibility for gcc shared libraries. #ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries #ifdef rviz_visual_tools_EXPORTS // we are building a shared lib/dll #define RVIZ_VISUAL_TOOLS_DECL ROS_HELPER_EXPORT #else // we are using shared lib/dll #define RVIZ_VISUAL_TOOLS_DECL ROS_HELPER_IMPORT #endif #else // ros is being built around static libraries #define RVIZ_VISUAL_TOOLS_DECL #endif namespace rviz_visual_tools { // Default constants static const std::string RVIZ_MARKER_TOPIC = "/rviz_visual_tools"; static const double SMALL_SCALE = 0.001; static const double LARGE_SCALE = 100; // Note: when adding new colors to colors, also add them to getRandColor() function enum Colors { BLACK = 0, BROWN = 1, BLUE = 2, CYAN = 3, GREY = 4, DARK_GREY = 5, GREEN = 6, LIME_GREEN = 7, MAGENTA = 8, ORANGE = 9, PURPLE = 10, RED = 11, PINK = 12, WHITE = 13, YELLOW = 14, TRANSLUCENT = 15, TRANSLUCENT_LIGHT = 16, TRANSLUCENT_DARK = 17, RAND = 18, CLEAR = 19, DEFAULT = 20 // i.e. 'do not change default color' }; enum Scales { XXXXSMALL = 1, XXXSMALL = 2, XXSMALL = 3, XSMALL = 4, SMALL = 5, MEDIUM = 6, // same as REGULAR LARGE = 7, XLARGE = 8, XXLARGE = 9, XXXLARGE = 10, XXXXLARGE = 11, }; enum EulerConvention { XYZ = 0, ZYX, // This is the ROS standard: http://www.ros.org/reps/rep-0103.html ZXZ }; struct RandomPoseBounds { double x_min_, x_max_; double y_min_, y_max_; double z_min_, z_max_; double elevation_min_, elevation_max_; double azimuth_min_, azimuth_max_; double angle_min_, angle_max_; RandomPoseBounds(double x_min = 0.0, double x_max = 1.0, double y_min = 0.0, double y_max = 1.0, double z_min = 0.0, double z_max = 1.0, double elevation_min = 0.0, double elevation_max = M_PI, double azimuth_min = 0.0, double azimuth_max = 2 * M_PI, double angle_min = 0.0, double angle_max = 2 * M_PI) { x_min_ = x_min; x_max_ = x_max; y_min_ = y_min; y_max_ = y_max; z_min_ = z_min; z_max_ = z_max; elevation_min_ = elevation_min; elevation_max_ = elevation_max; azimuth_min_ = azimuth_min; azimuth_max_ = azimuth_max; angle_min_ = angle_min; angle_max_ = angle_max; } }; struct RandomCuboidBounds { double cuboid_size_min_, cuboid_size_max_; explicit RandomCuboidBounds(double cuboid_size_min = 0.02, double cuboid_size_max = 0.15) { cuboid_size_min_ = cuboid_size_min; cuboid_size_max_ = cuboid_size_max; } }; class RvizVisualTools { private: void initialize(); public: template explicit RvizVisualTools(const std::string& base_frame, const std::string& marker_topic, NodePtr node, const RemoteControlPtr& remote_control = nullptr) : RvizVisualTools(base_frame, marker_topic, node->get_node_base_interface(), node->get_node_topics_interface(), node->get_node_graph_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), remote_control) { } explicit RvizVisualTools( const std::string& base_frame, const std::string& marker_topic, const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr& node_base_interface, const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr& topics_interface, const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr& graph_interface, const rclcpp::node_interfaces::NodeClockInterface::SharedPtr& clock_interface, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr& logging_interface, const RemoteControlPtr& remote_control = nullptr); ~RvizVisualTools() { } bool deleteMarker(const std::string& ns, std::size_t id); bool deleteAllMarkers(); bool deleteAllMarkers(const std::string& ns); void resetMarkerCounts(); bool loadRvizMarkers(); void setMarkerTopic(const std::string& topic) { marker_topic_ = topic; } void loadMarkerPub(bool wait_for_subscriber = true); bool waitForMarkerSub(double wait_time = 5); [[deprecated("waitForMarkerPub deprecated. Use waitForMarkerSub instad")]] bool waitForMarkerPub(double wait_time = 5) { return waitForMarkerSub(wait_time); } template bool waitForSubscriber(std::shared_ptr >& pub, double wait_time = 5.0) { // if the user does not want to wait return no connection if (!wait_for_subscriber_) { return false; } // Will wait at most this amount of time rclcpp::Time max_time(clock_interface_->get_clock()->now() + rclcpp::Duration::from_seconds(wait_time)); // This is wrong. It returns only the number of subscribers that have already // established their direct connections to this publisher // How often to check for subscribers rclcpp::Duration loop_duration = rclcpp::Duration::from_seconds(1.0 / 200.0); if (!pub) { RCLCPP_ERROR(logger_, "loadMarkerPub() has not been called yet, unable to wait for subscriber."); } std::string topic_name = pub->get_topic_name(); int num_existing_subscribers = graph_interface_->count_subscribers(topic_name); if (wait_time > 0 && num_existing_subscribers == 0) { RCLCPP_INFO_STREAM(logger_, "Topic " << pub->get_topic_name() << " waiting " << wait_time << " seconds for subscriber."); } // Wait for subscriber while (wait_time > 0 && num_existing_subscribers == 0 && rclcpp::ok()) { if (clock_interface_->get_clock()->now() > max_time) // Check if timed out { RCLCPP_WARN_STREAM( logger_, "Topic " << pub->get_topic_name() << " unable to connect to any subscribers within " << wait_time << " sec. It is possible initially published visual messages will be lost."); pub_rviz_markers_connected_ = false; return pub_rviz_markers_connected_; } // Check again num_existing_subscribers = graph_interface_->count_subscribers(topic_name); // Sleep rclcpp::sleep_for(std::chrono::nanoseconds(loop_duration.nanoseconds())); } if (!rclcpp::ok()) { pub_rviz_markers_connected_ = false; return false; } pub_rviz_markers_connected_ = (num_existing_subscribers != 0); return pub_rviz_markers_connected_; } void setAlpha(double alpha) { alpha_ = alpha; } void setLifetime(double lifetime); static Colors getRandColor(); std_msgs::msg::ColorRGBA getColor(Colors color) const; static Colors intToRvizColor(std::size_t color); static rviz_visual_tools::Scales intToRvizScale(std::size_t scale); static std::string scaleToString(Scales scale); std_msgs::msg::ColorRGBA createRandColor() const; static double slerp(double start, double end, double range, double value); std_msgs::msg::ColorRGBA getColorScale(double value) const; geometry_msgs::msg::Vector3 getScale(Scales scale, double marker_scale = 1.0) const; Eigen::Isometry3d getVectorBetweenPoints(const Eigen::Vector3d& a, const Eigen::Vector3d& b) const; Eigen::Vector3d getCenterPoint(const Eigen::Vector3d& a, const Eigen::Vector3d& b) const; const std::string getBaseFrame() const { return base_frame_; } void setBaseFrame(const std::string& base_frame) { base_frame_ = base_frame; loadRvizMarkers(); } double getGlobalScale() const { return global_scale_; } void setGlobalScale(double global_scale) { global_scale_ = global_scale; } bool publishMarker(visualization_msgs::msg::Marker& marker); void enableBatchPublishing(bool enable = true); void enableFrameLocking(bool enable = true); bool triggerEvery(std::size_t queueSize); bool trigger(); bool publishMarkers(visualization_msgs::msg::MarkerArray& markers); bool publishCone(const Eigen::Isometry3d& pose, double angle, Colors color = TRANSLUCENT, double scale = 1.0); bool publishCone(const geometry_msgs::msg::Pose& pose, double angle, Colors color = TRANSLUCENT, double scale = 1.0); bool publishABCDPlane(const double A, const double B, const double C, const double D, Colors color = TRANSLUCENT, double x_width = 1.0, double y_width = 1.0); bool publishNormalAndDistancePlane(const Eigen::Vector3d& normal, const double d, const Colors color = TRANSLUCENT, const double x_width = 1.0, const double y_width = 1.0); bool publishXYPlane(const Eigen::Isometry3d& pose, Colors color = TRANSLUCENT, double scale = 1.0); bool publishXYPlane(const geometry_msgs::msg::Pose& pose, Colors color = TRANSLUCENT, double scale = 1.0); bool publishXZPlane(const Eigen::Isometry3d& pose, Colors color = TRANSLUCENT, double scale = 1.0); bool publishXZPlane(const geometry_msgs::msg::Pose& pose, Colors color = TRANSLUCENT, double scale = 1.0); bool publishYZPlane(const Eigen::Isometry3d& pose, Colors color = TRANSLUCENT, double scale = 1.0); bool publishYZPlane(const geometry_msgs::msg::Pose& pose, Colors color = TRANSLUCENT, double scale = 1.0); bool publishSphere(const Eigen::Isometry3d& pose, Colors color = BLUE, Scales scale = MEDIUM, const std::string& ns = "Sphere", std::size_t id = 0); bool publishSphere(const Eigen::Vector3d& point, Colors color = BLUE, Scales scale = MEDIUM, const std::string& ns = "Sphere", std::size_t id = 0); bool publishSphere(const Eigen::Vector3d& point, Colors color, double scale, const std::string& ns = "Sphere", std::size_t id = 0); bool publishSphere(const geometry_msgs::msg::Point& point, Colors color = BLUE, Scales scale = MEDIUM, const std::string& ns = "Sphere", std::size_t id = 0); bool publishSphere(const geometry_msgs::msg::Pose& pose, Colors color = BLUE, Scales scale = MEDIUM, const std::string& ns = "Sphere", std::size_t id = 0); bool publishSphere(const geometry_msgs::msg::Pose& pose, Colors color, double scale, const std::string& ns = "Sphere", std::size_t id = 0); bool publishSphere(const geometry_msgs::msg::Pose& pose, Colors color, const geometry_msgs::msg::Vector3 scale, const std::string& ns = "Sphere", std::size_t id = 0); bool publishSphere(const geometry_msgs::msg::Pose& pose, const std_msgs::msg::ColorRGBA& color, const geometry_msgs::msg::Vector3 scale, const std::string& ns = "Sphere", std::size_t id = 0); bool publishSphere(const Eigen::Isometry3d& pose, const std_msgs::msg::ColorRGBA& color, const geometry_msgs::msg::Vector3 scale, const std::string& ns = "Sphere", std::size_t id = 0); bool publishSphere(const Eigen::Vector3d& point, const std_msgs::msg::ColorRGBA& color, const geometry_msgs::msg::Vector3 scale, const std::string& ns = "Sphere", std::size_t id = 0); bool publishSphere(const geometry_msgs::msg::PoseStamped& pose, Colors color, const geometry_msgs::msg::Vector3 scale, const std::string& ns = "Sphere", std::size_t id = 0); bool publishSpheres(const EigenSTL::vector_Vector3d& points, Colors color = BLUE, Scales scale = MEDIUM, const std::string& ns = "Spheres"); bool publishSpheres(const EigenSTL::vector_Vector3d& points, Colors color, double scale = 0.1, const std::string& ns = "Spheres"); bool publishSpheres(const std::vector& points, Colors color = BLUE, Scales scale = MEDIUM, const std::string& ns = "Spheres"); bool publishSpheres(const std::vector& points, Colors color = BLUE, double scale = 0.1, const std::string& ns = "Spheres"); bool publishSpheres(const std::vector& points, Colors color, const geometry_msgs::msg::Vector3& scale, const std::string& ns = "Spheres"); bool publishSpheres(const EigenSTL::vector_Vector3d& points, const std::vector& colors, Scales scale = MEDIUM, const std::string& ns = "Spheres"); bool publishSpheres(const std::vector& points, const std::vector& colors, const geometry_msgs::msg::Vector3& scale, const std::string& ns = "Spheres"); bool publishXArrow(const Eigen::Isometry3d& pose, Colors color = RED, Scales scale = MEDIUM, double length = 0.0); bool publishXArrow(const geometry_msgs::msg::Pose& pose, Colors color = RED, Scales scale = MEDIUM, double length = 0.0); bool publishXArrow(const geometry_msgs::msg::PoseStamped& pose, Colors color = RED, Scales scale = MEDIUM, double length = 0.0); bool publishYArrow(const Eigen::Isometry3d& pose, Colors color = GREEN, Scales scale = MEDIUM, double length = 0.0); bool publishYArrow(const geometry_msgs::msg::Pose& pose, Colors color = GREEN, Scales scale = MEDIUM, double length = 0.0); bool publishYArrow(const geometry_msgs::msg::PoseStamped& pose, Colors color = GREEN, Scales scale = MEDIUM, double length = 0.0); bool publishZArrow(const Eigen::Isometry3d& pose, Colors color = BLUE, Scales scale = MEDIUM, double length = 0.0, std::size_t id = 0); bool publishZArrow(const geometry_msgs::msg::Pose& pose, Colors color = BLUE, Scales scale = MEDIUM, double length = 0.0); bool publishZArrow(const geometry_msgs::msg::PoseStamped& pose, Colors color = BLUE, Scales scale = MEDIUM, double length = 0.0); bool publishZArrow(const geometry_msgs::msg::PoseStamped& pose, Colors color = BLUE, Scales scale = MEDIUM, double length = 0.0, std::size_t id = 0); bool publishArrow(const Eigen::Isometry3d& pose, Colors color = BLUE, Scales scale = MEDIUM, double length = 0.0, std::size_t id = 0); bool publishArrow(const geometry_msgs::msg::Pose& pose, Colors color = BLUE, Scales scale = MEDIUM, double length = 0.0, std::size_t id = 0); bool publishArrow(const geometry_msgs::msg::PoseStamped& pose, Colors color = BLUE, Scales scale = MEDIUM, double length = 0.0, std::size_t id = 0); bool publishArrow(const geometry_msgs::msg::Point& start, const geometry_msgs::msg::Point& end, Colors color = BLUE, Scales scale = MEDIUM, std::size_t id = 0); bool publishCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, Colors color = BLUE); bool publishCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std_msgs::msg::ColorRGBA& color); bool publishCuboid(const geometry_msgs::msg::Point& point1, const geometry_msgs::msg::Point& point2, Colors color = BLUE, const std::string& ns = "Cuboid", std::size_t id = 0); bool publishCuboid(const geometry_msgs::msg::Point& point1, const geometry_msgs::msg::Point& point2, const std_msgs::msg::ColorRGBA& color, const std::string& ns = "Cuboid", std::size_t id = 0); bool publishCuboid(const Eigen::Isometry3d& pose, double depth, double width, double height, const std_msgs::msg::ColorRGBA& color); bool publishCuboid(const Eigen::Isometry3d& pose, double depth, double width, double height, Colors color = BLUE); bool publishCuboid(const geometry_msgs::msg::Pose& pose, double depth, double width, double height, Colors color = BLUE); bool publishCuboid(const geometry_msgs::msg::Pose& pose, double depth, double width, double height, const std_msgs::msg::ColorRGBA& color); bool publishLine(const Eigen::Isometry3d& point1, const Eigen::Isometry3d& point2, Colors color = BLUE, Scales scale = MEDIUM); bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, Colors color = BLUE, Scales scale = MEDIUM); bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, Colors color, double radius); bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std_msgs::msg::ColorRGBA& color, Scales scale = MEDIUM); bool publishLine(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std_msgs::msg::ColorRGBA& color, double radius); bool publishLine(const geometry_msgs::msg::Point& point1, const geometry_msgs::msg::Point& point2, Colors color = BLUE, Scales scale = MEDIUM); bool publishLine(const geometry_msgs::msg::Point& point1, const geometry_msgs::msg::Point& point2, const std_msgs::msg::ColorRGBA& color, Scales scale = MEDIUM); bool publishLine(const geometry_msgs::msg::Point& point1, const geometry_msgs::msg::Point& point2, const std_msgs::msg::ColorRGBA& color, const geometry_msgs::msg::Vector3& scale); bool publishLines(const EigenSTL::vector_Vector3d& aPoints, const EigenSTL::vector_Vector3d& bPoints, const std::vector& colors, Scales scale = MEDIUM); bool publishLines(const std::vector& aPoints, const std::vector& bPoints, const std::vector& colors, const geometry_msgs::msg::Vector3& scale); bool publishLineStrip(const std::vector& path, Colors color = RED, Scales scale = MEDIUM, const std::string& ns = "Path"); bool publishPath(const std::vector& path, Colors color = RED, Scales scale = MEDIUM, const std::string& ns = "Path"); 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"); bool publishPath(const std::vector& path, Colors color = RED, double radius = 0.01, const std::string& ns = "Path"); bool publishPath(const EigenSTL::vector_Vector3d& path, Colors color = RED, double radius = 0.01, const std::string& ns = "Path"); bool publishPath(const EigenSTL::vector_Isometry3d& path, Colors color = RED, double radius = 0.01, const std::string& ns = "Path"); bool publishPath(const EigenSTL::vector_Vector3d& path, const std::vector& colors, double radius = 0.01, const std::string& ns = "Path"); bool publishPath(const EigenSTL::vector_Vector3d& path, const std::vector& colors, double radius, const std::string& ns = "Path"); bool publishPolygon(const geometry_msgs::msg::Polygon& polygon, Colors color = RED, Scales scale = MEDIUM, const std::string& ns = "Polygon"); bool publishWireframeCuboid(const Eigen::Isometry3d& pose, double depth, double width, double height, Colors color = BLUE, const std::string& ns = "Wireframe Cuboid", std::size_t id = 0); bool publishWireframeCuboid(const Eigen::Isometry3d& pose, const Eigen::Vector3d& min_point, const Eigen::Vector3d& max_point, Colors color = BLUE, const std::string& ns = "Wireframe Cuboid", std::size_t id = 0); bool publishWireframeRectangle(const Eigen::Isometry3d& pose, double height, double width, Colors color = BLUE, Scales scale = MEDIUM, std::size_t id = 0); bool publishWireframeRectangle(const Eigen::Isometry3d& pose, const Eigen::Vector3d& p1_in, const Eigen::Vector3d& p2_in, const Eigen::Vector3d& p3_in, const Eigen::Vector3d& p4_in, Colors color, Scales scale); bool publishAxisLabeled(const Eigen::Isometry3d& pose, const std::string& label, Scales scale = MEDIUM, Colors color = WHITE); bool publishAxisLabeled(const geometry_msgs::msg::Pose& pose, const std::string& label, Scales scale = MEDIUM, Colors color = WHITE); bool publishAxis(const geometry_msgs::msg::Pose& pose, Scales scale = MEDIUM, const std::string& ns = "Axis"); bool publishAxis(const Eigen::Isometry3d& pose, Scales scale = MEDIUM, const std::string& ns = "Axis"); bool publishAxis(const geometry_msgs::msg::Pose& pose, double length, double radius = 0.01, const std::string& ns = "Axis"); bool publishAxis(const Eigen::Isometry3d& pose, double length, double radius = 0.01, const std::string& ns = "Axis"); private: bool publishAxisInternal(const Eigen::Isometry3d& pose, double length = 0.1, double radius = 0.01, const std::string& ns = "Axis"); public: bool publishAxisPath(const EigenSTL::vector_Isometry3d& path, Scales scale = MEDIUM, const std::string& ns = "Axis Path"); bool publishAxisPath(const EigenSTL::vector_Isometry3d& path, double length = 0.1, double radius = 0.01, const std::string& ns = "Axis Path"); bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, Colors color = BLUE, Scales scale = MEDIUM, const std::string& ns = "Cylinder"); bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, Colors color, double radius = 0.01, const std::string& ns = "Cylinder"); bool publishCylinder(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std_msgs::msg::ColorRGBA& color, double radius = 0.01, const std::string& ns = "Cylinder"); bool publishCylinder(const Eigen::Isometry3d& pose, Colors color = BLUE, double height = 0.1, double radius = 0.01, const std::string& ns = "Cylinder"); bool publishCylinder(const geometry_msgs::msg::Pose& pose, Colors color = BLUE, double height = 0.1, double radius = 0.01, const std::string& ns = "Cylinder"); bool publishCylinder(const geometry_msgs::msg::Pose& pose, const std_msgs::msg::ColorRGBA& color, double height = 0.1, double radius = 0.01, const std::string& ns = "Cylinder"); bool publishMesh(const Eigen::Isometry3d& pose, const std::string& file_name, Colors color = CLEAR, double scale = 1, const std::string& ns = "mesh", std::size_t id = 0); bool publishMesh(const geometry_msgs::msg::Pose& pose, const std::string& file_name, Colors color = CLEAR, double scale = 1, const std::string& ns = "mesh", std::size_t id = 0); bool publishMesh(const Eigen::Isometry3d& pose, const shape_msgs::msg::Mesh& mesh, Colors color = CLEAR, double scale = 1, const std::string& ns = "mesh", std::size_t id = 0); bool publishMesh(const geometry_msgs::msg::Pose& pose, const shape_msgs::msg::Mesh& mesh, Colors color = CLEAR, double scale = 1, const std::string& ns = "mesh", std::size_t id = 0); // TODO(mlautman): port graph_msgs // bool publishGraph(const graph_msgs::msg::GeometryGraph& graph, colors color, double radius); bool publishText(const Eigen::Isometry3d& pose, const std::string& text, Colors color = WHITE, Scales scale = MEDIUM, bool static_id = true); bool publishText(const Eigen::Isometry3d& pose, const std::string& text, Colors color, const geometry_msgs::msg::Vector3 scale, bool static_id = true); bool publishText(const geometry_msgs::msg::Pose& pose, const std::string& text, Colors color = WHITE, Scales scale = MEDIUM, bool static_id = true); bool publishText(const geometry_msgs::msg::Pose& pose, const std::string& text, Colors color, const geometry_msgs::msg::Vector3 scale, bool static_id = true); static geometry_msgs::msg::Pose convertPose(const Eigen::Isometry3d& pose); static void convertPoseSafe(const Eigen::Isometry3d& pose, geometry_msgs::msg::Pose& pose_msg); static Eigen::Isometry3d convertPose(const geometry_msgs::msg::Pose& pose); static void convertPoseSafe(const geometry_msgs::msg::Pose& pose_msg, Eigen::Isometry3d& pose); static Eigen::Isometry3d convertPoint32ToPose(const geometry_msgs::msg::Point32& point); static geometry_msgs::msg::Pose convertPointToPose(const geometry_msgs::msg::Point& point); static Eigen::Isometry3d convertPointToPose(const Eigen::Vector3d& point); static geometry_msgs::msg::Point convertPoseToPoint(const Eigen::Isometry3d& pose); static Eigen::Vector3d convertPoint(const geometry_msgs::msg::Point& point); static Eigen::Vector3d convertPoint32(const geometry_msgs::msg::Point32& point); static geometry_msgs::msg::Point32 convertPoint32(const Eigen::Vector3d& point); static geometry_msgs::msg::Point convertPoint(const geometry_msgs::msg::Vector3& point); static geometry_msgs::msg::Point convertPoint(const Eigen::Vector3d& point); static Eigen::Isometry3d convertFromXYZRPY(double tx, double ty, double tz, double rx, double ry, double rz, EulerConvention convention); // ZYX is ROS standard static Eigen::Isometry3d convertFromXYZRPY(const std::vector& transform6, EulerConvention convention); // ZYX is ROS standard // TODO(davetcoleman): add opposite conversion that uses Eigen::Vector3d rpy = // pose.rotation().eulerAngles(0, 1, 2); static void convertToXYZRPY(const Eigen::Isometry3d& pose, std::vector& xyzrpy); static void convertToXYZRPY(const Eigen::Isometry3d& pose, double& x, double& y, double& z, double& roll, double& pitch, double& yaw); static void generateRandomPose(geometry_msgs::msg::Pose& pose, RandomPoseBounds pose_bounds = RandomPoseBounds()); static void generateRandomPose(Eigen::Isometry3d& pose, RandomPoseBounds pose_bounds = RandomPoseBounds()); static void generateRandomCuboid(geometry_msgs::msg::Pose& cuboid_pose, double& depth, double& width, double& height, RandomPoseBounds pose_bounds = RandomPoseBounds(), RandomCuboidBounds cuboid_bounds = RandomCuboidBounds()); static geometry_msgs::msg::Pose getIdentityPose(); static bool posesEqual(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, double threshold = 0.000001); static double dRand(double min, double max); static float fRand(float min, float max); static int iRand(int min, int max); static void printTranslation(const Eigen::Vector3d& translation); static void printTransform(const Eigen::Isometry3d& transform); static void printTransformRPY(const Eigen::Isometry3d& transform); static void printTransformFull(const Eigen::Isometry3d& transform); bool getPsychedelicMode() const { return psychedelic_mode_; } void setPsychedelicMode(bool psychedelic_mode = true) { psychedelic_mode_ = psychedelic_mode; } bool prompt(const std::string& msg); RemoteControlPtr& getRemoteControl(); void setRemoteControl(const RemoteControlPtr& remote_control); void loadRemoteControl(); int32_t getArrowId() const; int32_t getSphereId() const; int32_t getBlockId() const; int32_t getCylinderId() const; int32_t getMeshId() const; int32_t getTextId() const; int32_t getCuboidId() const; int32_t getLineStripId() const; int32_t getLineListId() const; int32_t getSpheresId() const; int32_t getTriangleId() const; protected: // Node Interfaces rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface_; rclcpp::node_interfaces::NodeGraphInterface::SharedPtr graph_interface_; rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_; rclcpp::Logger logger_; // Optional remote control RemoteControlPtr remote_control_; // ROS publishers rclcpp::Publisher::SharedPtr pub_rviz_markers_; // for rviz visualization // markers bool pub_rviz_markers_connected_ = false; bool pub_rviz_markers_waited_ = false; bool wait_for_subscriber_ = false; // Strings std::string marker_topic_; // topic to publish to rviz std::string base_frame_; // name of base link // Duration to have Rviz markers persist, 0 for infinity builtin_interfaces::msg::Duration marker_lifetime_; // Settings bool batch_publishing_enabled_ = true; bool frame_locking_enabled_ = false; double alpha_ = 1.0; // opacity of all markers double global_scale_ = 1.0; // allow all markers to be increased by a constanct factor // Cached Rviz Marker Array visualization_msgs::msg::MarkerArray markers_; // Cached Rviz markers visualization_msgs::msg::Marker arrow_marker_; visualization_msgs::msg::Marker sphere_marker_; visualization_msgs::msg::Marker block_marker_; visualization_msgs::msg::Marker cylinder_marker_; visualization_msgs::msg::Marker mesh_marker_; visualization_msgs::msg::Marker text_marker_; visualization_msgs::msg::Marker cuboid_marker_; visualization_msgs::msg::Marker line_strip_marker_; visualization_msgs::msg::Marker line_list_marker_; visualization_msgs::msg::Marker spheres_marker_; visualization_msgs::msg::Marker reset_marker_; visualization_msgs::msg::Marker triangle_marker_; // Just for fun. bool psychedelic_mode_ = false; // Chose random colors from this list static const std::array ALL_RAND_COLORS; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html }; // class typedef std::shared_ptr RvizVisualToolsPtr; typedef std::shared_ptr RvizVisualToolsConstPtr; } // namespace rviz_visual_tools