diff --git a/include/moveit_visual_tools/moveit_visual_tools.h b/include/moveit_visual_tools/moveit_visual_tools.h index f7e92dd..26ad2d6 100644 --- a/include/moveit_visual_tools/moveit_visual_tools.h +++ b/include/moveit_visual_tools/moveit_visual_tools.h @@ -357,11 +357,11 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools * \param color to display the collision object with * \return true on sucess **/ - bool publishCollisionCuboid(const Eigen::Affine3d& pose, double width, double depth, double height, const std::string& name, - const rviz_visual_tools::colors& color); + bool publishCollisionCuboid(const Eigen::Affine3d& pose, double width, double depth, double height, + const std::string& name, const rviz_visual_tools::colors& color); - bool publishCollisionCuboid(const geometry_msgs::Pose& pose, double x, double y, double z, const std::string& name, - const rviz_visual_tools::colors& color); + 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 cylinder between two points diff --git a/src/moveit_visual_tools.cpp b/src/moveit_visual_tools.cpp index 15ab7bd..c239060 100644 --- a/src/moveit_visual_tools.cpp +++ b/src/moveit_visual_tools.cpp @@ -687,16 +687,17 @@ bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Point& point return processCollisionObjectMsg(collision_obj, color); } -bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Affine3d& pose, double x, double y, double z, +bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Affine3d& pose, double width, double depth, double height, const std::string& name, const rviz_visual_tools::colors& color) { geometry_msgs::Pose pose_msg; tf::poseEigenToMsg(pose, pose_msg); - return publishCollisionCuboid(pose_msg, x, y, z, name, color); + return publishCollisionCuboid(pose_msg, width, depth, height, name, color); } -bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth, double height, - const std::string& name, const rviz_visual_tools::colors& color) +bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth, + double height, const std::string& name, + const rviz_visual_tools::colors& color) { moveit_msgs::CollisionObject collision_obj; collision_obj.header.stamp = ros::Time::now();