ompl_visual_tools.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, University of Colorado, Boulder
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Univ of CO, Boulder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman <dave@dav.ee>
00036    Desc:   Tools for displaying OMPL components in Rviz
00037 */
00038 
00039 // This library
00040 #include <ompl_visual_tools/ompl_visual_tools.h>
00041 
00042 // ROS
00043 #include <ros/ros.h>
00044 #include <graph_msgs/GeometryGraph.h>
00045 #include <graph_msgs/Edges.h>
00046 
00047 // Boost
00048 #include <boost/numeric/ublas/matrix.hpp>
00049 #include <boost/numeric/ublas/io.hpp>
00050 
00051 // OMPL
00052 #include <ompl/base/spaces/SE3StateSpace.h>
00053 #include <ompl/base/PlannerData.h>
00054 #include <ompl/base/ScopedState.h>
00055 
00056 #include <ompl/config.h>
00057 
00058 // Custom validity checker that accounts for cost
00059 #include <ompl_visual_tools/costs/cost_map_2d_optimization_objective.h>
00060 
00061 // For converting OMPL state to a MoveIt robot state
00062 #include <moveit/ompl_interface/model_based_planning_context.h>
00063 #include <moveit/robot_state/robot_state.h>
00064 #include <moveit/macros/deprecation.h>
00065 
00066 namespace ob = ompl::base;
00067 namespace og = ompl::geometric;
00068 namespace bnu = boost::numeric::ublas;
00069 
00070 using namespace moveit_visual_tools;
00071 
00072 namespace ompl_visual_tools
00073 {
00074 
00075 OmplVisualTools::OmplVisualTools(const std::string& base_link, const std::string& marker_topic, robot_model::RobotModelConstPtr robot_model)
00076   : MoveItVisualTools(base_link, marker_topic, robot_model)
00077   , disable_3d_(false)
00078 {
00079 }
00080 
00081 void OmplVisualTools::setStateSpace(ompl::base::StateSpacePtr space)
00082 {
00083   si_.reset(new ompl::base::SpaceInformation(space));
00084 }
00085 
00086 void OmplVisualTools::setSpaceInformation(ompl::base::SpaceInformationPtr si)
00087 {
00088   si_ = si;
00089 }
00090 
00091 void OmplVisualTools::setCostMap(intMatrixPtr cost)
00092 {
00093   cost_ = cost;
00094 }
00095 
00099 bool OmplVisualTools::getDisable3D()
00100 {
00101   return disable_3d_;
00102 }
00103 
00107 void OmplVisualTools::setDisable3D(bool disable_3d)
00108 {
00109   disable_3d_ = disable_3d;
00110 }
00111 
00112 double OmplVisualTools::getCost(const geometry_msgs::Point &point)
00113 {
00114   // Check that a cost map has been passed in
00115   if (cost_)
00116   {
00117     return double((*cost_)( natRound(point.y), natRound(point.x) )) / 2.0;
00118   }
00119   else
00120     return 1;
00121 }
00122 
00123 double OmplVisualTools::getCostHeight(const geometry_msgs::Point &point)
00124 {
00125   if (disable_3d_)
00126     return 0.5; // all costs become almost zero in flat world
00127 
00128   // TODO make faster the following math
00129 
00130   // check if whole number
00131   if (floor(point.x) == point.x && floor(point.y) == point.y)
00132     return getCost(point) + COST_HEIGHT_OFFSET;
00133 
00134   // else do Bilinear Interpolation
00135 
00136   // top left
00137   geometry_msgs::Point a;
00138   a.x = floor(point.x);
00139   a.y = ceil(point.y);
00140   a.z = getCost(a);
00141 
00142   // bottom left
00143   geometry_msgs::Point b;
00144   b.x = floor(point.x);
00145   b.y = floor(point.y);
00146   b.z = getCost(b);
00147 
00148   // bottom right
00149   geometry_msgs::Point c;
00150   c.x = ceil(point.x);
00151   c.y = floor(point.y);
00152   c.z = getCost(c);
00153 
00154   // top right
00155   geometry_msgs::Point d;
00156   d.x = ceil(point.x);
00157   d.y = ceil(point.y);
00158   d.z = getCost(d);
00159 
00160   double R1;
00161   double R2;
00162 
00163   // check if our x axis is the same
00164   if (c.x == b.x)
00165   {
00166     // just choose either
00167     R1 = b.z;
00168     R2 = a.z;
00169   }
00170   else
00171   {
00172     R1 = ((c.x - point.x)/(c.x - b.x))*b.z + ((point.x - b.x)/(c.x - b.x))*c.z;
00173     R2 = ((c.x - point.x)/(c.x - b.x))*a.z + ((point.x - b.x)/(c.x - b.x))*d.z;
00174   }
00175 
00176   // After the two R values are calculated, the value of P can finally be calculated by a weighted average of R1 and R2.
00177   double val;
00178 
00179   // check if y axis is the same
00180   if ( a.y - b.y == 0) // division by zero
00181     val = R1;
00182   else
00183     val = ((a.y - point.y)/(a.y - b.y))*R1 + ((point.y - b.y)/(a.y - b.y))*R2;
00184 
00185   return val + COST_HEIGHT_OFFSET;
00186 }
00187 
00188 bool OmplVisualTools::publishCostMap(PPMImage *image, bool static_id)
00189 {
00190   visualization_msgs::Marker marker;
00191   // Set the frame ID and timestamp.  See the TF tutorials for information on these.
00192   marker.header.frame_id = base_frame_;
00193   marker.header.stamp = ros::Time::now();
00194 
00195   // Set the namespace and id for this marker.  This serves to create a unique ID
00196   marker.ns = "cost_map";
00197 
00198   // Set the marker type.
00199   marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
00200 
00201   // Set the marker action.  Options are ADD and DELETE
00202   marker.action = visualization_msgs::Marker::ADD;
00203 
00204   static std::size_t cost_map_id = 0;
00205   if (static_id)
00206   {
00207     marker.id = 0;
00208   }
00209   else
00210   {
00211     cost_map_id++;
00212     marker.id = cost_map_id;
00213   }
00214 
00215   marker.pose.position.x = 0;
00216   marker.pose.position.y = 0;
00217   marker.pose.position.z = 0;
00218   marker.pose.orientation.x = 0.0;
00219   marker.pose.orientation.y = 0.0;
00220   marker.pose.orientation.z = 0.0;
00221   marker.pose.orientation.w = 1.0;
00222   marker.scale.x = 1.0;
00223   marker.scale.y = 1.0;
00224   marker.scale.z = 1.0;
00225   marker.color = getColor( rviz_visual_tools::RED );
00226 
00227   // Visualize Results -------------------------------------------------------------------------------------------------
00228   for( size_t marker_id = 0; marker_id < image->getSize(); ++marker_id )
00229   {
00230 
00231     unsigned int x = marker_id % image->x;    // Map index back to coordinates
00232     unsigned int y = marker_id / image->x;    // Map index back to coordinates
00233 
00234     // Make right and down triangle
00235     // Check that we are not on the far right or bottom
00236     if( ! (x + 1 >= image->x ||  y + 1 >= image->y ) )
00237     {
00238       publishTriangle( x,   y, &marker, image);
00239       publishTriangle( x+1, y, &marker, image);
00240       publishTriangle( x,   y+1, &marker, image);
00241     }
00242 
00243     // Make back and down triangle
00244     // Check that we are not on the far left or bottom
00245     if( ! ( int(x) - 1 < 0 ||  y + 1 >= image->y ) )
00246     {
00247       publishTriangle( x,   y, &marker, image );
00248       publishTriangle( x,   y+1, &marker, image );
00249       publishTriangle( x-1, y+1, &marker, image );
00250     }
00251 
00252   }
00253 
00254   // Send to Rviz
00255   publishMarker( marker );
00256   ros::spinOnce();
00257 
00258   return true;
00259 }
00260 
00264 bool OmplVisualTools::publishTriangle( int x, int y, visualization_msgs::Marker* marker, PPMImage *image )
00265 {
00266   // Point
00267   temp_point_.x = x;
00268   temp_point_.y = y;
00269   if (disable_3d_)
00270     temp_point_.z = 0; // all costs become zero in flat world
00271   else
00272     temp_point_.z = getCost(temp_point_); // to speed things up, we know is always whole number
00273 
00274   marker->points.push_back( temp_point_ );
00275 
00276   std_msgs::ColorRGBA color;
00277   color.r = image->data[ image->getID( x, y ) ].red / 255.0;
00278   color.g = image->data[ image->getID( x, y ) ].green / 255.0;
00279   color.b = image->data[ image->getID( x, y ) ].blue / 255.0;
00280   color.a = 1.0;
00281 
00282   marker->colors.push_back( color );
00283 
00284   return true;
00285 }
00286 
00287 bool OmplVisualTools::interpolateLine( const geometry_msgs::Point &p1, const geometry_msgs::Point &p2, visualization_msgs::Marker* marker,
00288                                        const std_msgs::ColorRGBA color )
00289 {
00290   // Copy to non-const
00291   geometry_msgs::Point point_a = p1;
00292   geometry_msgs::Point point_b = p2;
00293 
00294   // Get the heights
00295   point_a.z = getCostHeight(point_a);
00296   point_b.z = getCostHeight(point_b);
00297 
00298   //ROS_INFO_STREAM("a is: " << point_a);
00299   //ROS_INFO_STREAM("b is: " << point_b);
00300 
00301   // Switch the coordinates such that x1 < x2
00302   if( point_a.x > point_b.x )
00303   {
00304     // Swap the coordinates
00305     geometry_msgs::Point point_temp = point_a;
00306     point_a = point_b;
00307     point_b = point_temp;
00308   }
00309 
00310   // Show the straight line --------------------------------------------------------------------
00311   if( false )
00312   {
00313     // Add the point pair to the line message
00314     marker->points.push_back( point_a );
00315     marker->points.push_back( point_b );
00316     marker->colors.push_back( color );
00317     marker->colors.push_back( color );
00318 
00319     // Show start and end point
00320     publishSphere(point_a, rviz_visual_tools::GREEN);
00321     publishSphere(point_b, rviz_visual_tools::RED);
00322   }
00323 
00324   // Interpolate the line ----------------------------------------------------------------------
00325 
00326   // Calculate slope between the lines
00327   double m = (point_b.y - point_a.y)/(point_b.x - point_a.x);
00328 
00329   // Calculate the y-intercep
00330   double b = point_a.y - m * point_a.x;
00331 
00332   // Define the interpolation interval
00333   double interval = 0.1; //0.5;
00334 
00335   // Make new locations
00336   geometry_msgs::Point temp_a = point_a; // remember the last point
00337   geometry_msgs::Point temp_b = point_a; // move along this point
00338 
00339   // Loop through the line adding segements along the cost map
00340   for( temp_b.x = point_a.x + interval; temp_b.x <= point_b.x; temp_b.x += interval )
00341   {
00342     //publishSphere(temp_b, color2);
00343 
00344     // Find the y coordinate
00345     temp_b.y = m*temp_b.x + b;
00346 
00347     // Add the new heights
00348     temp_a.z = getCostHeight(temp_a);
00349     temp_b.z = getCostHeight(temp_b);
00350 
00351     // Add the point pair to the line message
00352     marker->points.push_back( temp_a );
00353     marker->points.push_back( temp_b );
00354     // Add colors
00355     marker->colors.push_back( color );
00356     marker->colors.push_back( color );
00357 
00358     // Remember the last coordiante for next iteration
00359     temp_a = temp_b;
00360   }
00361 
00362   // Finish the line for non-even interval lengths
00363   marker->points.push_back( temp_a );
00364   marker->points.push_back( point_b );
00365   // Add colors
00366   marker->colors.push_back( color );
00367   marker->colors.push_back( color );
00368 
00369   return true;
00370 }
00371 
00372 bool OmplVisualTools::publishStartGoalSpheres(ob::PlannerDataPtr planner_data, const std::string& ns)
00373 {
00374   for (std::size_t i = 0; i < planner_data->numStartVertices(); ++i)
00375   {
00376     publishSphere( stateToPointMsg( planner_data->getStartVertex(i).getState() ), rviz_visual_tools::GREEN, rviz_visual_tools::REGULAR, ns);
00377   }
00378   for (std::size_t i = 0; i < planner_data->numGoalVertices(); ++i)
00379   {
00380     publishSphere( stateToPointMsg( planner_data->getGoalVertex(i).getState() ), rviz_visual_tools::RED, rviz_visual_tools::REGULAR, ns );
00381   }
00382 
00383   return true;
00384 }
00385 
00386 bool OmplVisualTools::publishGraph(ob::PlannerDataPtr planner_data, const rviz_visual_tools::colors& color,
00387                                    const double thickness, const std::string& ns)
00388 {
00389   visualization_msgs::Marker marker;
00390   // Set the frame ID and timestamp.  See the TF tutorials for information on these.
00391   marker.header.frame_id = base_frame_;
00392   marker.header.stamp = ros::Time();
00393 
00394   // Set the namespace and id for this marker.  This serves to create a unique ID
00395   marker.ns = ns;
00396 
00397   // Set the marker type.
00398   marker.type = visualization_msgs::Marker::LINE_LIST;
00399 
00400   // Set the marker action.  Options are ADD and DELETE
00401   marker.action = visualization_msgs::Marker::ADD;
00402   marker.id = 0;
00403 
00404   marker.pose.position.x = 0.0;
00405   marker.pose.position.y = 0.0;
00406   marker.pose.position.z = 0.0;
00407 
00408   marker.pose.orientation.x = 0.0;
00409   marker.pose.orientation.y = 0.0;
00410   marker.pose.orientation.z = 0.0;
00411   marker.pose.orientation.w = 1.0;
00412 
00413   marker.scale.x = thickness;
00414   marker.scale.y = 1.0;
00415   marker.scale.z = 1.0;
00416 
00417   marker.color = getColor( color );
00418 
00419   geometry_msgs::Point this_vertex;
00420   geometry_msgs::Point next_vertex;
00421 
00422   // Loop through all verticies
00423   for( std::size_t vertex_id = 0; vertex_id < planner_data->numVertices(); ++vertex_id )
00424   {
00425     this_vertex = stateToPointMsg( vertex_id, planner_data );
00426 
00427     // Get the out edges from the current vertex
00428     std::vector<unsigned int> edge_list;
00429     planner_data->getEdges( vertex_id, edge_list );
00430 
00431     // Now loop through each edge
00432     for( std::vector<unsigned int>::const_iterator edge_it = edge_list.begin();
00433          edge_it != edge_list.end(); ++edge_it)
00434     {
00435       // Convert vertex id to next coordinates
00436       next_vertex = stateToPointMsg( *edge_it, planner_data );
00437 
00438       interpolateLine( this_vertex, next_vertex, &marker, marker.color );
00439     }
00440 
00441   }
00442 
00443   // Send to Rviz
00444   publishMarker( marker );
00445   ros::spinOnce();;
00446 
00447   return true;
00448 }
00449 
00450 bool OmplVisualTools::publishEdge(const ob::State* stateA, const ob::State* stateB, const rviz_visual_tools::colors color, const rviz_visual_tools::scales scale)
00451 {
00452   return publishLine(
00453                      stateToPointMsg(stateA),
00454                      stateToPointMsg(stateB),
00455                      color, scale);
00456 }
00457 
00458 bool OmplVisualTools::publishSampleIDs(const og::PathGeometric& path, const rviz_visual_tools::colors color,
00459                                        const rviz_visual_tools::scales scale, const std::string& ns )
00460 {
00461   // Create a small scale for font size
00462   geometry_msgs::Vector3 scale_msg;
00463   if (cost_)
00464   {
00465     int size = ceil(cost_->size1() / 30.0);    // only z is required (size of an "A")
00466     scale_msg.x = size;
00467     scale_msg.y = size;
00468     scale_msg.z = size;
00469   }
00470   else
00471     scale_msg = getScale(scale); // TODO tune this
00472 
00473   std::string text;
00474   // Publish all
00475   for (std::size_t i = 0; i < path.getStateCount(); ++i)
00476   {
00477     text = boost::lexical_cast<std::string>(i+2);
00478 
00479     // send to moveit_visual_tools
00480     MoveItVisualTools::publishText( convertPointToPose(stateToPointMsg( path.getState(i) )),
00481                               text, color, scale_msg, false);
00482   }
00483 
00484   return true;
00485 }
00486 
00487 bool OmplVisualTools::publishSamples(const ob::PlannerDataPtr& planner_data, const rviz_visual_tools::colors color,
00488                                      const rviz_visual_tools::scales scale, const std::string& ns )
00489 {
00490   ROS_ERROR_STREAM_NAMED("ompl_visual_tools","Deprecated");
00491   og::PathGeometric path(si_);
00492   convertPlannerData(planner_data, path);
00493 
00494   return publishSpheres(path, color, scale, ns);
00495 }
00496 
00497 bool OmplVisualTools::publishSamples(const og::PathGeometric& path, const rviz_visual_tools::colors color,
00498                                      const rviz_visual_tools::scales scale, const std::string& ns )
00499 {
00500   ROS_ERROR_STREAM_NAMED("ompl_visual_tools","Deprecated");
00501   std::vector<geometry_msgs::Point> points;
00502   for (std::size_t i = 0; i < path.getStateCount(); ++i)
00503   {
00504     points.push_back( stateToPointMsg( path.getState(i) ) );
00505   }
00506 
00507   return MoveItVisualTools::publishSpheres(points, color, scale, ns);
00508 
00509   // Show the vertex ids
00510   //publishSampleIDs( path, rviz_visual_tools::rviz_visual_tools::BLACK );
00511 }
00512 
00513 bool OmplVisualTools::publishSpheres( const ob::PlannerDataPtr& planner_data, const rviz_visual_tools::colors color,
00514                                       const rviz_visual_tools::scales scale, const std::string& ns)
00515 {
00516   og::PathGeometric path(si_);
00517   convertPlannerData(planner_data, path);
00518 
00519   return publishSpheres(path, color, scale, ns);
00520 }
00521 
00522 bool OmplVisualTools::publishSpheres( const og::PathGeometric& path, const rviz_visual_tools::colors color,
00523                                       double scale, const std::string& ns)
00524 {
00525   geometry_msgs::Vector3 scale_vector;
00526   scale_vector.x = scale;
00527   scale_vector.y = scale;
00528   scale_vector.z = scale;
00529   publishSpheres( path, color, scale_vector, ns);
00530 }
00531 
00532 bool OmplVisualTools::publishSpheres( const og::PathGeometric& path, const rviz_visual_tools::colors color,
00533                                       const rviz_visual_tools::scales scale, const std::string& ns)
00534 {
00535   publishSpheres( path, color, getScale(scale, false, 0.25), ns);
00536 }
00537 
00538 bool OmplVisualTools::publishSpheres( const og::PathGeometric& path, const rviz_visual_tools::colors color,
00539                                       const geometry_msgs::Vector3 &scale, const std::string& ns)
00540 {
00541   std::vector<geometry_msgs::Point> points;
00542   for (std::size_t i = 0; i < path.getStateCount(); ++i)
00543     points.push_back( stateToPointMsg( path.getState(i) ) );
00544 
00545   return MoveItVisualTools::publishSpheres(points, color, scale, ns);
00546 
00547   // Show the vertex ids
00548   //publishSampleIDs( path, rviz_visual_tools::BLACK );
00549 }
00550 
00551 void OmplVisualTools::convertPlannerData(const ob::PlannerDataPtr planner_data, og::PathGeometric &path)
00552 {
00553   // Convert the planner data verticies into a vector of states
00554   for (std::size_t i = 0; i < planner_data->numVertices(); ++i)
00555   {
00556     path.append(planner_data->getVertex(i).getState());
00557   }
00558 }
00559 
00560 bool OmplVisualTools::publishStates(std::vector<const ompl::base::State*> states)
00561 {
00562   visualization_msgs::Marker marker;
00563   // Set the frame ID and timestamp.
00564   marker.header.frame_id = base_frame_;
00565   marker.header.stamp = ros::Time();
00566 
00567   // Set the namespace and id for this marker.  This serves to create a unique ID
00568   marker.ns = "states";
00569 
00570   // Set the marker type.
00571   marker.type = visualization_msgs::Marker::SPHERE_LIST;
00572 
00573   // Set the marker action.  Options are ADD and DELETE
00574   marker.action = visualization_msgs::Marker::ADD;
00575   marker.id = 0;
00576 
00577   marker.pose.position.x = 0.0;
00578   marker.pose.position.y = 0.0;
00579   marker.pose.position.z = 0.0;
00580 
00581   marker.pose.orientation.x = 0.0;
00582   marker.pose.orientation.y = 0.0;
00583   marker.pose.orientation.z = 0.0;
00584   marker.pose.orientation.w = 1.0;
00585 
00586   marker.scale.x = 0.4;
00587   marker.scale.y = 0.4;
00588   marker.scale.z = 0.4;
00589 
00590   marker.color = getColor( rviz_visual_tools::RED );
00591 
00592   // Make line color
00593   std_msgs::ColorRGBA color = getColor( rviz_visual_tools::RED );
00594 
00595   // Point
00596   geometry_msgs::Point point_a;
00597 
00598   // Loop through all verticies
00599   for( int vertex_id = 0; vertex_id < int( states.size() ); ++vertex_id )
00600   {
00601     // First point
00602     point_a = stateToPointMsg( states[vertex_id] );
00603 
00604     // Add the point pair to the line message
00605     marker.points.push_back( point_a );
00606     marker.colors.push_back( color );
00607   }
00608 
00609   // Send to Rviz
00610   publishMarker( marker );
00611   ros::spinOnce();;
00612   return true;
00613 }
00614 
00615 bool OmplVisualTools::publishRobotState( const ompl::base::State *state )
00616 {
00617   // Make sure a robot state is available
00618   loadSharedRobotState();
00619 
00620   ompl_interface::ModelBasedStateSpacePtr model_state_space =
00621     boost::static_pointer_cast<ompl_interface::ModelBasedStateSpace>(si_->getStateSpace());
00622 
00623   // Convert to robot state
00624   model_state_space->copyToRobotState( *shared_robot_state_, state );
00625   ROS_WARN_STREAM_NAMED("temp","updateStateWithFakeBase disabled");
00626   //shared_robot_state_->updateStateWithFakeBase();
00627 
00628   MoveItVisualTools::publishRobotState(shared_robot_state_);
00629 }
00630 
00631 bool OmplVisualTools::publishRobotPath( const ompl::base::PlannerDataPtr &path, robot_model::JointModelGroup* joint_model_group,
00632                                         const std::vector<const robot_model::LinkModel*> &tips, bool show_trajectory_animated)
00633 {
00634   // Make sure a robot state is available
00635   loadSharedRobotState();
00636 
00637   // Vars
00638   Eigen::Affine3d pose;
00639   std::vector< std::vector<geometry_msgs::Point> > paths_msgs(tips.size()); // each tip has its own path of points
00640   robot_trajectory::RobotTrajectoryPtr robot_trajectory;
00641 
00642   ompl_interface::ModelBasedStateSpacePtr model_state_space =
00643     boost::static_pointer_cast<ompl_interface::ModelBasedStateSpace>(si_->getStateSpace());
00644 
00645   // Optionally save the trajectory
00646   if (show_trajectory_animated)
00647   {
00648     robot_trajectory.reset(new robot_trajectory::RobotTrajectory(robot_model_, joint_model_group->getName()));
00649   }
00650 
00651   // Each state in the path
00652   for (std::size_t state_id = 0; state_id < path->numVertices(); ++state_id)
00653   {
00654     // Check if program is shutting down
00655     if (!ros::ok())
00656       return false;
00657 
00658     // Convert to robot state
00659     model_state_space->copyToRobotState( *shared_robot_state_, path->getVertex(state_id).getState() );
00660     ROS_WARN_STREAM_NAMED("temp","updateStateWithFakeBase disabled");
00661     //shared_robot_state_->updateStateWithFakeBase();
00662 
00663     MoveItVisualTools::publishRobotState(shared_robot_state_);
00664 
00665     // Each tip in the robot state
00666     for (std::size_t tip_id = 0; tip_id < tips.size(); ++tip_id)
00667     {
00668       // Forward kinematics
00669       pose = shared_robot_state_->getGlobalLinkTransform(tips[tip_id]);
00670 
00671       // Optionally save the trajectory
00672       if (show_trajectory_animated)
00673       {
00674         robot_state::RobotState robot_state_copy = *shared_robot_state_;
00675         robot_trajectory->addSuffixWayPoint(robot_state_copy, 0.05); // 1 second interval
00676       }
00677 
00678       // Debug pose
00679       //std::cout << "Pose: " << state_id << " of link " << tips[tip_id]->getName() << ": \n" << pose.translation() << std::endl;
00680 
00681       paths_msgs[tip_id].push_back( convertPose(pose).position );
00682 
00683       // Show goal state arrow
00684       if (state_id == path->numVertices() -1)
00685       {
00686         publishArrow( pose, rviz_visual_tools::BLACK );
00687       }
00688     }
00689   } // for each state
00690 
00691   for (std::size_t tip_id = 0; tip_id < tips.size(); ++tip_id)
00692   {
00693     MoveItVisualTools::publishPath( paths_msgs[tip_id], rviz_visual_tools::RAND, rviz_visual_tools::SMALL );
00694     ros::Duration(0.05).sleep();
00695     MoveItVisualTools::publishSpheres( paths_msgs[tip_id], rviz_visual_tools::ORANGE, rviz_visual_tools::SMALL );
00696     ros::Duration(0.05).sleep();
00697   }
00698 
00699   // Debugging - Convert to trajectory
00700   if (show_trajectory_animated)
00701   {
00702     publishTrajectoryPath(*robot_trajectory, true);
00703   }
00704 
00705   return true;
00706 }
00707 
00708 bool OmplVisualTools::publishRobotGraph( const ompl::base::PlannerDataPtr &graph,
00709                                          const std::vector<const robot_model::LinkModel*> &tips)
00710 {
00711   // Make sure a robot state is available
00712   loadSharedRobotState();
00713 
00714   // Turn into multiple graphs (one for each tip)
00715   std::vector<graph_msgs::GeometryGraph> graphs(tips.size());
00716 
00717   // Convert states to points
00718   std::vector< std::vector<geometry_msgs::Point> > vertex_tip_points;
00719   convertRobotStatesToTipPoints(graph, tips, vertex_tip_points);
00720 
00721   // Copy tip points to each resulting tip graph
00722   for (std::size_t vertex_id = 0; vertex_id < vertex_tip_points.size(); ++vertex_id)
00723   {
00724     for (std::size_t tip_id = 0; tip_id < tips.size(); ++tip_id)
00725     {
00726       graphs[tip_id].nodes.push_back(vertex_tip_points[vertex_id][tip_id]);
00727     }
00728   }
00729 
00730   // Now just copy the edges into each structure
00731   for (std::size_t vertex_id = 0; vertex_id < graph->numVertices(); ++vertex_id)
00732   {
00733     // Get the out edges from the current vertex
00734     std::vector<unsigned int> edge_list;
00735     graph->getEdges( vertex_id, edge_list );
00736 
00737     // Do for each tip
00738     for (std::size_t tip_id = 0; tip_id < tips.size(); ++tip_id)
00739     {
00740       // Create new edge with all the node ids listed
00741       graph_msgs::Edges edge;
00742       edge.node_ids = edge_list;
00743       graphs[tip_id].edges.push_back(edge);
00744 
00745     } // for each tip
00746   } // for each vertex
00747 
00748     // Now publish each tip graph
00749   for (std::size_t tip_id = 0; tip_id < tips.size(); ++tip_id)
00750   {
00751     const rviz_visual_tools::colors color = getRandColor();
00752     std::cout << "Color is  " << color << std::endl;
00753     MoveItVisualTools::publishGraph( graphs[tip_id], color, 0.005 );
00754     ros::Duration(0.1).sleep();
00755 
00756     MoveItVisualTools::publishSpheres( graphs[tip_id].nodes, rviz_visual_tools::ORANGE, rviz_visual_tools::SMALL );
00757     ros::Duration(0.1).sleep();
00758   }
00759 
00760   return true;
00761 }
00762 
00763 bool OmplVisualTools::publishPath( const ob::PlannerDataPtr& planner_data, const rviz_visual_tools::colors color,
00764                                    const double thickness, const std::string& ns )
00765 {
00766   og::PathGeometric path(si_);
00767   convertPlannerData(planner_data, path);
00768 
00769   return publishPath(path, color, thickness, ns);
00770 }
00771 
00772 bool OmplVisualTools::publishPath( const og::PathGeometric& path, const rviz_visual_tools::colors color, const double thickness, const std::string& ns )
00773 {
00774   visualization_msgs::Marker marker;
00775   // Set the frame ID and timestamp.
00776   marker.header.frame_id = base_frame_;
00777   marker.header.stamp = ros::Time();
00778 
00779   // Set the namespace and id for this marker.  This serves to create a unique ID
00780   marker.ns = ns;
00781 
00782   std_msgs::ColorRGBA this_color = getColor( color );
00783 
00784   // Set the marker type.
00785   marker.type = visualization_msgs::Marker::LINE_LIST;
00786 
00787   // Set the marker action.  Options are ADD and DELETE
00788   marker.action = visualization_msgs::Marker::ADD;
00789 
00790   // Provide a new id every call to this function
00791   static int result_id = 0;
00792   marker.id = result_id++;
00793 
00794   marker.pose.position.x = 0.0;
00795   marker.pose.position.y = 0.0;
00796   marker.pose.position.z = 0.0;
00797 
00798   marker.pose.orientation.x = 0.0;
00799   marker.pose.orientation.y = 0.0;
00800   marker.pose.orientation.z = 0.0;
00801   marker.pose.orientation.w = 1.0;
00802 
00803   marker.scale.x = thickness;
00804   marker.scale.y = 1.0;
00805   marker.scale.z = 1.0;
00806 
00807   marker.color = this_color;
00808 
00809 
00810   geometry_msgs::Point last_vertex;
00811   geometry_msgs::Point this_vertex;
00812 
00813   if (path.getStateCount() <= 0)
00814   {
00815     ROS_WARN_STREAM_NAMED("publishPath","No states found in path");
00816     return false;
00817   }
00818 
00819   // Initialize first vertex
00820   last_vertex = stateToPointMsg(path.getState(0));
00821 
00822   // Convert path coordinates
00823   for( std::size_t i = 1; i < path.getStateCount(); ++i )
00824   {
00825     // Get current coordinates
00826     this_vertex = stateToPointMsg(path.getState(i));
00827 
00828     // Publish line with interpolation
00829     interpolateLine( last_vertex, this_vertex, &marker, marker.color );
00830 
00831     // Save these coordinates for next line
00832     last_vertex = this_vertex;
00833   }
00834 
00835   // Send to Rviz
00836   publishMarker( marker );
00837   ros::spinOnce();
00838   return true;
00839 }
00840 
00841 geometry_msgs::Point OmplVisualTools::stateToPointMsg( int vertex_id, ob::PlannerDataPtr planner_data )
00842 {
00843   ob::PlannerDataVertex *vertex = &planner_data->getVertex( vertex_id );
00844 
00845   // Get this vertex's coordinates
00846   return stateToPointMsg(vertex->getState());
00847 }
00848 
00849 geometry_msgs::Point OmplVisualTools::stateToPointMsg( const ob::State *state )
00850 {
00851   if (!state)
00852   {
00853     ROS_FATAL("No state found for a vertex");
00854     exit(1);
00855   }
00856 
00857   // Convert to RealVectorStateSpace
00858   const ob::RealVectorStateSpace::StateType *real_state =
00859     static_cast<const ob::RealVectorStateSpace::StateType*>(state);
00860 
00861   // Create point
00862   temp_point_.x = real_state->values[0];
00863   temp_point_.y = real_state->values[1];
00864   temp_point_.z = getCostHeight(temp_point_);
00865   return temp_point_;
00866 }
00867 
00868 geometry_msgs::Point OmplVisualTools::stateToPointMsg( const ob::ScopedState<> state )
00869 {
00870   // Create point
00871   temp_point_.x = state[0];
00872   temp_point_.y = state[1];
00873   temp_point_.z = getCostHeight(temp_point_);
00874   return temp_point_;
00875 }
00876 
00877 int OmplVisualTools::natRound(double x)
00878 {
00879   return static_cast<int>(floor(x + 0.5f));
00880 }
00881 
00882 bool OmplVisualTools::publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors &color, const rviz_visual_tools::scales scale, const std::string& ns)
00883 {
00884   return publishSphere( convertPointToPose(stateToPointMsg( state )), color, scale, ns);
00885 }
00886 
00887 bool OmplVisualTools::publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors &color, double scale, const std::string& ns)
00888 {
00889   return publishSphere( convertPointToPose(stateToPointMsg( state )), color, scale, ns);
00890 }
00891 
00892 bool OmplVisualTools::publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors &color, const geometry_msgs::Vector3 &scale, const std::string& ns)
00893 {
00894   return publishSphere( convertPointToPose(stateToPointMsg( state )), color, scale, ns);
00895 }
00896 
00897 bool OmplVisualTools::publishSampleRegion(const ob::ScopedState<>& state_area, const double& distance)
00898 {
00899   temp_point_.x = state_area[0];
00900   temp_point_.y = state_area[1];
00901   temp_point_.z = getCostHeight(temp_point_);
00902 
00903   publishSphere(temp_point_, rviz_visual_tools::BLACK, rviz_visual_tools::REGULAR, "sample_region"); // mid point
00904   // outer sphere (x2 b/c its a radius, x0.1 to make it look nicer)
00905   return publishSphere(temp_point_, rviz_visual_tools::TRANSLUCENT, rviz_visual_tools::REGULAR, "sample_region");
00906 }
00907 
00908 bool OmplVisualTools::publishText(const geometry_msgs::Point &point, const std::string &text,
00909                                   const rviz_visual_tools::colors &color, bool static_id)
00910 {
00911   geometry_msgs::Pose text_pose;
00912   text_pose.position = point;
00913 
00914   return publishText(text_pose, text, color, static_id);
00915 }
00916 
00917 bool OmplVisualTools::publishText(const geometry_msgs::Pose &pose, const std::string &text, const rviz_visual_tools::colors &color,
00918                                   bool static_id)
00919 {
00920   geometry_msgs::Vector3 scale;
00921   if (cost_)
00922   {
00923     int size = ceil(cost_->size1() / 20.0);    // only z is required (size of an "A")
00924     scale.x = size;
00925     scale.y = size;
00926     scale.z = size;
00927   }
00928   else
00929     scale = getScale(rviz_visual_tools::REGULAR); // TODO tune this
00930 
00931   // send to moveit_visual_tools
00932   return RvizVisualTools::publishText( pose, text, color, scale, static_id);
00933 }
00934 
00935 bool OmplVisualTools::convertRobotStatesToTipPoints(const ompl::base::PlannerDataPtr &graph,
00936                                                     const std::vector<const robot_model::LinkModel*> &tips,
00937                                                     std::vector< std::vector<geometry_msgs::Point> > & vertex_tip_points)
00938 {
00939   // Make sure a robot state is available
00940   loadSharedRobotState();
00941 
00942   // Vars
00943   Eigen::Affine3d pose;
00944 
00945   // Load information about the robot's geometry
00946   ompl_interface::ModelBasedStateSpacePtr model_state_space =
00947     boost::static_pointer_cast<ompl_interface::ModelBasedStateSpace>(si_->getStateSpace());
00948 
00949   // Rows correspond to robot states
00950   vertex_tip_points.clear();
00951   vertex_tip_points.resize(graph->numVertices());
00952 
00953   // Each state in the path
00954   for (std::size_t state_id = 0; state_id < graph->numVertices(); ++state_id)
00955   {
00956     // Convert to robot state
00957     model_state_space->copyToRobotState( *shared_robot_state_, graph->getVertex(state_id).getState() );
00958     ROS_WARN_STREAM_NAMED("temp","updateStateWithFakeBase disabled");
00959     //shared_robot_state_->updateStateWithFakeBase();
00960 
00961     // Each tip in the robot state
00962     for (std::size_t tip_id = 0; tip_id < tips.size(); ++tip_id)
00963     {
00964       // Forward kinematics
00965       pose = shared_robot_state_->getGlobalLinkTransform(tips[tip_id]);
00966 
00967       vertex_tip_points[state_id].push_back( convertPose(pose).position );
00968     }
00969   }
00970 
00971   return true;
00972 }
00973 
00974 /*
00975 void OmplVisualTools::visualizationCallback(ompl::base::Planner *planner)
00976 {
00977   deleteAllMarkers();
00978   ros::spinOnce();
00979 
00980   ompl::base::PlannerDataPtr planner_data(new ompl::base::PlannerData(si_));
00981   planner->getPlannerData(*planner_data);
00982 
00983   std::cout << "visualizationCallback has states: " << planner_data->numVertices() << std::endl;
00984 
00985   // Show edges of graph
00986   publishGraph( planner_data, rviz_visual_tools::PURPLE );
00987 
00988   // Convert planner data to path
00989   og::PathGeometric path(si_);
00990   convertPlannerData(planner_data, path);
00991 
00992   // Show samples of graph
00993   OmplVisualTools::publishSpheres( path, rviz_visual_tools::ORANGE, rviz_visual_tools::SMALL);
00994 
00995   // Outline with circle
00996   //double nn_radius = 3.46482;
00997   //OmplVisualTools::publishSpheres( path, rviz_visual_tools::TRANSLUCENT2, nn_radius*2 );
00998 
00999   ros::Duration(0.1).sleep();
01000 
01001   ros::spinOnce();
01002 }
01003 
01004 void OmplVisualTools::visualizationStateCallback(ompl::base::State *state, std::size_t type, double neighborRadius)
01005 {
01006   geometry_msgs::Pose pose = convertPointToPose(stateToPointMsg( state ));
01007 
01008   switch (type)
01009   {
01010     case 1: // Candidate COEVERAGE node to be added
01011       publishSphere(pose, rviz_visual_tools::GREEN, rviz_visual_tools::SMALL);
01012       break;
01013     case 2: // Candidate CONNECTIVITY node to be added
01014       publishSphere(pose, rviz_visual_tools::BLUE, rviz_visual_tools::SMALL);
01015       break;
01016     case 3: // sampled nearby node
01017       publishSphere(pose, rviz_visual_tools::RED, rviz_visual_tools::SMALL);
01018       break;
01019     case 4: // Candidate node has already been added
01020       publishSphere(pose, rviz_visual_tools::PURPLE, rviz_visual_tools::REGULAR);
01021       // Outline with circle
01022       publishSphere(pose, rviz_visual_tools::TRANSLUCENT2, neighborRadius*2 );
01023       break;
01024     default:
01025       ROS_ERROR_STREAM_NAMED("visualizationStateCallback","Invalid state type value");
01026   }
01027   ros::spinOnce();
01028 }
01029 
01030 void OmplVisualTools::visualizationEdgeCallback(ompl::base::State *stateA, ompl::base::State *stateB)
01031 {
01032   publishEdge(stateA, stateB, rviz_visual_tools::ORANGE, rviz_visual_tools::SMALL);
01033   ros::spinOnce();
01034 }
01035 
01036 ompl::base::VisualizationCallback OmplVisualTools::getVisualizationCallback()
01037 {
01038   return boost::bind(&OmplVisualTools::visualizationCallback, this, _1);
01039 }
01040 
01041 ompl::base::VisualizationStateCallback OmplVisualTools::getVisualizationStateCallback()
01042 {
01043   return boost::bind(&OmplVisualTools::visualizationStateCallback, this, _1, _2, _3);
01044 }
01045 
01046 ompl::base::VisualizationEdgeCallback OmplVisualTools::getVisualizationEdgeCallback()
01047 {
01048   return boost::bind(&OmplVisualTools::visualizationEdgeCallback, this, _1, _2);
01049 }
01050 */
01051 
01052 } // end namespace


ompl_visual_tools
Author(s): Dave Coleman
autogenerated on Thu Feb 11 2016 23:58:14