From 65afc306875ac2a362b8e04ccab171ef4ac759d0 Mon Sep 17 00:00:00 2001
From: Jon Weisz <jweisz@cs.columbia.edu>
Date: Mon, 19 Jan 2015 13:49:03 -0500
Subject: [PATCH] Added collision visualization output to the rviz goal state
 display.

---
 .../motion_planning_display.h                 |  9 ++-
 .../src/motion_planning_display.cpp           | 62 ++++++++++++++++++-
 2 files changed, 69 insertions(+), 2 deletions(-)

diff --git a/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h
index 0f22c13c2c..2e16d528ef 100644
--- a/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h
+++ b/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h
@@ -82,7 +82,8 @@ class MotionPlanningDisplay : public PlanningSceneDisplay
 {
   Q_OBJECT
 
-  public:
+    void showLinkCollisions();
+public:
 
   MotionPlanningDisplay();
 
@@ -206,6 +207,8 @@ private Q_SLOTS:
   void executeMainLoopJobs();
   void clearTrajectoryTrail();
   void publishInteractiveMarkers(bool pose_update);
+  void publishCollisionMarkers(visualization_msgs::MarkerArray& markers);
+
 
   void recomputeQueryStartStateMetrics();
   void recomputeQueryGoalStateMetrics();
@@ -250,6 +253,10 @@ private Q_SLOTS:
   std::vector<rviz::Robot*> trajectory_trail_;
   ros::Subscriber planning_group_sub_;
   ros::Subscriber trajectory_topic_sub_;
+
+  ros::Publisher collision_marker_array_publisher_;
+  visualization_msgs::MarkerArray collision_markers_;
+
   ros::NodeHandle private_handle_, node_handle_;
   bool animating_path_;
   int current_state_;
diff --git a/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
index ad83c6d18c..e62eb1a7d2 100644
--- a/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
+++ b/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
@@ -40,7 +40,7 @@
 #include <rviz/visualization_manager.h>
 #include <rviz/robot/robot.h>
 #include <rviz/robot/robot_link.h>
-
+#include <moveit/collision_detection/collision_tools.h>
 #include <rviz/properties/property.h>
 #include <rviz/properties/string_property.h>
 #include <rviz/properties/bool_property.h>
@@ -220,6 +220,8 @@ MotionPlanningDisplay::MotionPlanningDisplay() :
   background_process_.setJobUpdateEvent(boost::bind(&MotionPlanningDisplay::backgroundJobUpdate, this, _1, _2));
 
   connect(this, SIGNAL(timeToShowNewTrail()), this, SLOT(changedShowTrail()));
+
+  collision_marker_array_publisher_ = node_handle_.advertise<visualization_msgs::MarkerArray>("goal_collision_marker_array",100);
 }
 
 // ******************************************************************************************
@@ -771,6 +773,7 @@ void MotionPlanningDisplay::addStatusText(const std::string &text)
 {
   if (frame_)
     frame_->ui_->status_text->append(QString::fromStdString(text));
+    ROS_ERROR_STREAM(text);
 }
 
 void MotionPlanningDisplay::addStatusText(const std::vector<std::string> &text)
@@ -829,6 +832,42 @@ void MotionPlanningDisplay::drawQueryGoalState()
 
       // update link colors
       std::vector<std::string> collision_links;
+      collision_detection::CollisionRequest req;
+      collision_detection::CollisionResult res;
+      req.contacts = true;
+      req.max_contacts = 100;
+      req.max_contacts_per_pair = 10;
+      req.verbose = true;
+      req.group_name = getCurrentPlanningGroup();
+      getPlanningSceneRO()->checkCollisionUnpadded(req, res, *state);
+      if(res.collision)
+      {
+        if(res.contact_count > 0)
+        {
+          std_msgs::ColorRGBA color;
+                 color.r = 1.0;
+                 color.g = 0.0;
+                 color.b = 1.0;
+                 color.a = 0.5;
+                 visualization_msgs::MarkerArray markers;
+                 collision_detection::getCollisionMarkersFromContacts(markers,
+                                                                      "/root",
+                                                                      res.contacts,
+                                                                      color,
+                                                                      ros::Duration(), // remain until deleted
+                                                                      0.01);           // radius
+         publishCollisionMarkers(markers);
+
+        }
+      }
+      else
+      {
+          ROS_INFO("Not colliding");
+
+               // delete the old collision point markers
+               visualization_msgs::MarkerArray empty_marker_array;
+               publishCollisionMarkers(empty_marker_array);
+      }
       getPlanningSceneRO()->getCollidingLinks(collision_links, *state);
       status_links_goal_.clear();
       for (std::size_t i = 0 ; i < collision_links.size() ; ++i)
@@ -837,6 +876,7 @@ void MotionPlanningDisplay::drawQueryGoalState()
       {
         collision_detection::CollisionResult::ContactMap pairs;
         getPlanningSceneRO()->getCollidingPairs(pairs, *state);
+
         setStatusTextColor(query_goal_color_property_->getColor());
         addStatusText("Goal state colliding links:");
         for (collision_detection::CollisionResult::ContactMap::const_iterator it = pairs.begin() ; it != pairs.end() ; ++it)
@@ -884,6 +924,25 @@ void MotionPlanningDisplay::resetInteractiveMarkers()
   addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false), "publishInteractiveMarkers");
 }
 
+void MotionPlanningDisplay::publishCollisionMarkers(visualization_msgs::MarkerArray& markers)
+{
+  // delete old markers
+  if (collision_markers_.markers.size())
+  {
+    for (int i=0; i<collision_markers_.markers.size(); i++)
+      collision_markers_.markers[i].action = visualization_msgs::Marker::DELETE;
+
+    collision_marker_array_publisher_.publish(collision_markers_);
+  }
+
+  // move new markers into g_collision_points
+  std::swap(collision_markers_.markers, markers.markers);
+
+  // draw new markers (if there are any)
+  if (collision_markers_.markers.size())
+    collision_marker_array_publisher_.publish(collision_markers_);
+}
+
 void MotionPlanningDisplay::publishInteractiveMarkers(bool pose_update)
 {
   if (robot_interaction_)
@@ -1049,6 +1108,7 @@ bool MotionPlanningDisplay::isIKSolutionCollisionFree(robot_state::RobotState *s
     return true;
 }
 
+
 void MotionPlanningDisplay::updateLinkColors()
 {
   unsetAllColors(&query_robot_start_->getRobot());