From 522874f7be487ae3a8f57f0a335fcf96d51a750f Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Sun, 18 Aug 2019 01:59:11 -0600 Subject: [PATCH] publish cuboids with size --- .../moveit_visual_tools/moveit_visual_tools.h | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/include/moveit_visual_tools/moveit_visual_tools.h b/include/moveit_visual_tools/moveit_visual_tools.h index 6d0f459..6d49227 100644 --- a/include/moveit_visual_tools/moveit_visual_tools.h +++ b/include/moveit_visual_tools/moveit_visual_tools.h @@ -366,6 +366,25 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools bool publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth, double height, const std::string& name, const rviz_visual_tools::colors& color); + /** + * \brief Create a MoveIt collision rectangular cuboid at the given pose + * \param pose - position of the centroid of the cube + * \param size - the size (x,y,z) of the object in its local frame + * \param name - semantic name of MoveIt collision object + * \param color to display the collision object with + * \return true on sucess + **/ + bool publishCollisionCuboid(const Eigen::Isometry3d& pose, const Eigen::Vector3d& size, + const std::string& name, const rviz_visual_tools::colors& color) + { + return publishCollisionCuboid(pose, size.x(), size.y(), size.z(), name, color); + } + bool publishCollisionCuboid(const geometry_msgs::Pose& pose, const geometry_msgs::Vector3& size, + const std::string& name, const rviz_visual_tools::colors& color) + { + return publishCollisionCuboid(pose, size.x, size.y, size.z, name, color); + } + /** * \brief Create a MoveIt Collision cylinder between two points * \param point a - x,y,z in space of a point