Permalink
Browse files

Added collision visualization output to the rviz goal state display.

  • Loading branch information...
jon-weisz committed Jan 19, 2015
1 parent 37fae8f commit 65afc306875ac2a362b8e04ccab171ef4ac759d0
@@ -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_;
@@ -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());

0 comments on commit 65afc30

Please sign in to comment.