diff --git a/include/moveit_visual_tools/moveit_visual_tools.h b/include/moveit_visual_tools/moveit_visual_tools.h index 5a4ba5a..83c1ab1 100644 --- a/include/moveit_visual_tools/moveit_visual_tools.h +++ b/include/moveit_visual_tools/moveit_visual_tools.h @@ -485,6 +485,21 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools */ bool publishWorkspaceParameters(const moveit_msgs::WorkspaceParameters& params); + /** + * \brief Check if the robot state is in collision inside the planning scene and visualize the result. + * If the state is not colliding only the robot state is published. If there is a collision the colliding + * links are ghlighted and the contact points are visualized with markers. + * \param robot_state - The robot state to check for collisions + * \param planning_scene - The planning scene to use for collision checks + * \param highlight_link_color - The color to use for highligting colliding links + * \param contact_point_color - The color to use for contact point markers + * \result - True if there is a collision + */ + bool checkAndPublishCollision(const moveit::core::RobotState& robot_state, + const planning_scene::PlanningScene* planning_scene, + const rviz_visual_tools::colors& highlight_link_color = rviz_visual_tools::RED, + const rviz_visual_tools::colors& contact_point_color = rviz_visual_tools::PURPLE); + /** * \brief Given a planning scene and robot state, publish any collisions * \param robot_state @@ -496,6 +511,17 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools const planning_scene::PlanningScene* planning_scene, const rviz_visual_tools::colors& color = rviz_visual_tools::RED); + /** + * \brief Given a contact map and planning scene, publish the contact points + * \param contacts - The populated contacts to visualize + * \param planning_scene + * \param color - display color of markers + * \return true on success + */ + bool publishContactPoints(const collision_detection::CollisionResult::ContactMap& contacts, + const planning_scene::PlanningScene* planning_scene, + const rviz_visual_tools::colors& color = rviz_visual_tools::RED); + /** * \brief Move a joint group in MoveIt for visualization * make sure you have already set the planning group name @@ -607,11 +633,14 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools * To use, add a RobotState marker to Rviz and subscribe to the DISPLAY_ROBOT_STATE_TOPIC, above * \param robot_state - joint values of robot * \param color - how to highlight the robot (solid-ly) if desired, default keeps color as specified in URDF + * \param highlight_links - if the |color| is not |DEFAULT|, allows selective robot links to be highlighted. by default (empty) all links are highlighted */ bool publishRobotState(const moveit::core::RobotState& robot_state, - const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT); + const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT, + const std::vector& highlight_links = {}); bool publishRobotState(const moveit::core::RobotStatePtr& robot_state, - const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT); + const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT, + const std::vector& highlight_links = {}); /** * \brief Fake removing a Robot State display in Rviz by simply moving it very far away diff --git a/src/moveit_visual_tools.cpp b/src/moveit_visual_tools.cpp index 5926ba2..aea88e2 100644 --- a/src/moveit_visual_tools.cpp +++ b/src/moveit_visual_tools.cpp @@ -1057,6 +1057,33 @@ bool MoveItVisualTools::publishWorkspaceParameters(const moveit_msgs::WorkspaceP "Planning_Workspace", 1); } +bool MoveItVisualTools::checkAndPublishCollision(const moveit::core::RobotState& robot_state, + const planning_scene::PlanningScene* planning_scene, + const rviz_visual_tools::colors& highlight_link_color, + const rviz_visual_tools::colors& contact_point_color) +{ + // Compute the contacts if any + collision_detection::CollisionRequest c_req; + collision_detection::CollisionResult c_res; + c_req.contacts = true; + c_req.max_contacts = 10; + c_req.max_contacts_per_pair = 3; + c_req.verbose = true; + + // Check for collisions + planning_scene->checkCollision(c_req, c_res, robot_state); + std::vector highlight_links; + for (const auto& contact : c_res.contacts) + { + highlight_links.push_back(contact.first.first); + highlight_links.push_back(contact.first.second); + } + + publishRobotState(robot_state, highlight_link_color, highlight_links); + publishContactPoints(c_res.contacts, planning_scene, contact_point_color); + return c_res.collision; +} + bool MoveItVisualTools::publishContactPoints(const moveit::core::RobotState& robot_state, const planning_scene::PlanningScene* planning_scene, const rviz_visual_tools::colors& color) @@ -1071,12 +1098,18 @@ bool MoveItVisualTools::publishContactPoints(const moveit::core::RobotState& rob // Check for collisions planning_scene->checkCollision(c_req, c_res, robot_state); + return publishContactPoints(c_res.contacts, planning_scene, color); +} +bool MoveItVisualTools::publishContactPoints(const collision_detection::CollisionResult::ContactMap& contacts, + const planning_scene::PlanningScene* planning_scene, + const rviz_visual_tools::colors& color) +{ // Display - if (c_res.contact_count > 0) + if (!contacts.empty()) { visualization_msgs::MarkerArray arr; - collision_detection::getCollisionMarkersFromContacts(arr, planning_scene->getPlanningFrame(), c_res.contacts); + collision_detection::getCollisionMarkersFromContacts(arr, planning_scene->getPlanningFrame(), contacts); ROS_INFO_STREAM_NAMED(LOGNAME, "Completed listing of explanations for invalid states."); // Check for markers @@ -1087,6 +1120,7 @@ bool MoveItVisualTools::publishContactPoints(const moveit::core::RobotState& rob for (std::size_t i = 0; i < arr.markers.size(); ++i) { arr.markers[i].ns = "Collision"; + arr.markers[i].id = i; arr.markers[i].scale.x = 0.04; arr.markers[i].scale.y = 0.04; arr.markers[i].scale.z = 0.04; @@ -1395,17 +1429,24 @@ bool MoveItVisualTools::publishRobotState(const std::vector joint_positi } bool MoveItVisualTools::publishRobotState(const robot_state::RobotStatePtr& robot_state, - const rviz_visual_tools::colors& color) + const rviz_visual_tools::colors& color, + const std::vector& highlight_links) { - return publishRobotState(*robot_state.get(), color); + return publishRobotState(*robot_state.get(), color, highlight_links); } bool MoveItVisualTools::publishRobotState(const robot_state::RobotState& robot_state, - const rviz_visual_tools::colors& color) + const rviz_visual_tools::colors& color, + const std::vector& highlight_links) { + // when only a subset of links should be colored, the default message is used rather than the cached solid robot messages + rviz_visual_tools::colors base_color = color; + if (!highlight_links.empty()) + base_color = rviz_visual_tools::DEFAULT; + // Reference to the correctly colored version of message (they are cached) // May not exist yet but this will create it - moveit_msgs::DisplayRobotState& display_robot_msg = display_robot_msgs_[color]; + moveit_msgs::DisplayRobotState& display_robot_msg = display_robot_msgs_[base_color]; // Check if a robot state message already exists for this color if (display_robot_msg.highlight_links.size() == 0) // has not been colored yet, lets create that @@ -1413,8 +1454,9 @@ bool MoveItVisualTools::publishRobotState(const robot_state::RobotState& robot_s if (color != rviz_visual_tools::DEFAULT) // ignore color highlights when set to default { // Get links names - const std::vector& link_names = - robot_state.getRobotModel()->getLinkModelsWithCollisionGeometry(); + const std::vector& link_names = + highlight_links.empty() ? robot_state.getRobotModel()->getLinkModelNamesWithCollisionGeometry() : + highlight_links; display_robot_msg.highlight_links.resize(link_names.size()); // Get color @@ -1423,7 +1465,7 @@ bool MoveItVisualTools::publishRobotState(const robot_state::RobotState& robot_s // Color every link for (std::size_t i = 0; i < link_names.size(); ++i) { - display_robot_msg.highlight_links[i].id = link_names[i]->getName(); + display_robot_msg.highlight_links[i].id = link_names[i]; display_robot_msg.highlight_links[i].color = color_rgba; } } @@ -1453,6 +1495,10 @@ bool MoveItVisualTools::publishRobotState(const robot_state::RobotState& robot_s pub_robot_state_.publish(display_robot_msg); ros::spinOnce(); + // remove highlight links from default message + if (!highlight_links.empty()) + display_robot_msg.highlight_links.clear(); + return true; }