ompl_visual_tools.h
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 #ifndef OMPL_VISUAL_TOOLS__OMPL_VISUAL_TOOLS_
00040 #define OMPL_VISUAL_TOOLS__OMPL_VISUAL_TOOLS_
00041 
00042 // ROS
00043 #include <ros/ros.h>
00044 #include <visualization_msgs/Marker.h>
00045 #include <geometry_msgs/Point.h>
00046 
00047 // OMPL
00048 #include <ompl/base/SpaceInformation.h>
00049 #include <ompl/base/Planner.h>
00050 #include <ompl/geometric/PathGeometric.h>
00051 #include <ompl/base/ScopedState.h>
00052 
00053 // Custom validity checker that accounts for cost
00054 #include <ompl_visual_tools/costs/cost_map_2d_optimization_objective.h>
00055 
00056 // MoveIt
00057 #include <moveit/robot_model/link_model.h>
00058 
00059 // Visualization
00060 #include <moveit_visual_tools/moveit_visual_tools.h>
00061 
00062 namespace ob = ompl::base;
00063 namespace og = ompl::geometric;
00064 namespace bnu = boost::numeric::ublas;
00065 
00066 // Forward Declartions
00067 namespace ompl
00068 {
00069 namespace base
00070 {
00071 class PlannerData;
00072 typedef boost::shared_ptr<PlannerData> PlannerDataPtr;
00073 }}
00074 
00075 namespace ompl_interface
00076 {
00077 class ModelBasedPlanningContext;
00078 typedef boost::shared_ptr<ModelBasedPlanningContext> ModelBasedPlanningContextPtr;
00079 }
00080 
00081 //namespace moveit
00082 //{
00083 //namespace core
00084 //{
00085 //class LinkModel;
00086 //{
00087 //}
00088 
00089 namespace ompl_visual_tools
00090 {
00091 
00092 // Default constants
00093 static const std::string RVIZ_MARKER_TOPIC = "/ompl_rviz_markers";
00094 static const double COST_HEIGHT_OFFSET = 0.5;
00095 
00096 typedef std::map< std::string, std::list<std::size_t> > MarkerList;
00097 
00098 class OmplVisualTools : public moveit_visual_tools::MoveItVisualTools
00099 {
00100 private:
00101 
00102   // Keep a pointer to an optional cost map
00103   intMatrixPtr cost_;
00104 
00105   // Remember what space we are working in
00106   ompl::base::SpaceInformationPtr si_;
00107 
00108   // Cached Point object to reduce memory loading
00109   geometry_msgs::Point temp_point_;
00110 
00111   // Mode that disables showing 3D in Rviz
00112   bool disable_3d_;
00113 
00114 public:
00115 
00119   OmplVisualTools(const std::string& base_link,
00120                   const std::string& marker_topic = ompl_visual_tools::RVIZ_MARKER_TOPIC,
00121                   robot_model::RobotModelConstPtr robot_model = robot_model::RobotModelConstPtr());
00122 
00126   void setStateSpace(ompl::base::StateSpacePtr space);
00127   void setSpaceInformation(ompl::base::SpaceInformationPtr si);
00128 
00132   void setCostMap(intMatrixPtr cost);
00133 
00137   bool getDisable3D();
00138 
00142   void setDisable3D(bool disable_3d);
00143 
00147   double getCost(const geometry_msgs::Point &point);
00148 
00153   double getCostHeight(const geometry_msgs::Point &point);
00154 
00158   bool publishCostMap(PPMImage *image, bool static_id = true);
00159 
00163   bool publishTriangle( int x, int y, visualization_msgs::Marker* marker, PPMImage *image );
00164 
00168   bool interpolateLine( const geometry_msgs::Point &p1, const geometry_msgs::Point &p2, visualization_msgs::Marker* marker,
00169                         const std_msgs::ColorRGBA color );
00170 
00174   bool publishStartGoalSpheres(ob::PlannerDataPtr planner_data, const std::string& ns);
00175 
00179   bool publishGraph(ob::PlannerDataPtr planner_data, const rviz_visual_tools::colors& color = rviz_visual_tools::BLUE, const double thickness = 0.2,
00180                     const std::string& ns = "space_exploration");
00181 
00185   MOVEIT_DEPRECATED bool publishSamples( const ob::PlannerDataPtr& planner_data, const rviz_visual_tools::colors color = rviz_visual_tools::RED,
00186                                          const rviz_visual_tools::scales scale = rviz_visual_tools::SMALL, const std::string& ns = "sample_locations" );
00187 
00188 
00189   MOVEIT_DEPRECATED bool publishSamples( const og::PathGeometric& path, const rviz_visual_tools::colors color = rviz_visual_tools::RED,
00190                                          const rviz_visual_tools::scales scale = rviz_visual_tools::SMALL, const std::string& ns = "sample_locations" );
00191 
00200   bool publishSpheres( const ob::PlannerDataPtr& planner_data, const rviz_visual_tools::colors color = rviz_visual_tools::RED, 
00201                        const rviz_visual_tools::scales scale = rviz_visual_tools::SMALL, const std::string& ns = "planner_data_spheres" );
00202   bool publishSpheres( const og::PathGeometric& path, const rviz_visual_tools::colors color = rviz_visual_tools::RED, 
00203                        double scale = 0.1, const std::string& ns = "path_spheres" );
00204   bool publishSpheres( const og::PathGeometric& path, const rviz_visual_tools::colors color = rviz_visual_tools::RED, 
00205                        const rviz_visual_tools::scales scale = rviz_visual_tools::SMALL, const std::string& ns = "path_spheres" );
00206   bool publishSpheres( const og::PathGeometric& path, const rviz_visual_tools::colors color, 
00207                        const geometry_msgs::Vector3 &scale, const std::string& ns = "path_spheres");
00208 
00215   bool publishEdge(const ob::State* stateA, const ob::State* stateB, const rviz_visual_tools::colors color = rviz_visual_tools::BLUE, 
00216                    const rviz_visual_tools::scales scale = rviz_visual_tools::REGULAR);
00217 
00221   bool publishSampleIDs( const og::PathGeometric& path, const rviz_visual_tools::colors color = rviz_visual_tools::RED,
00222                          const rviz_visual_tools::scales scale = rviz_visual_tools::SMALL, const std::string& ns = "sample_labels" );
00223   
00229   void convertPlannerData(const ob::PlannerDataPtr planner_data, og::PathGeometric &path);
00230 
00235   bool publishStates(std::vector<const ompl::base::State*> states);
00236 
00242   bool publishRobotState( const ompl::base::State *state );
00243 
00249   bool publishRobotPath( const ompl::base::PlannerDataPtr &path, robot_model::JointModelGroup* joint_model_group,
00250                          const std::vector<const robot_model::LinkModel*> &tips, bool show_trajectory_animated);
00251 
00257   bool publishRobotGraph( const ompl::base::PlannerDataPtr &graph, 
00258                           const std::vector<const robot_model::LinkModel*> &tips);
00264   bool publishPath( const ob::PlannerDataPtr& planner_data, const rviz_visual_tools::colors color,
00265                     const double thickness = 0.4, const std::string& ns = "result_path" );
00266 
00271   bool publishPath( const og::PathGeometric& path, const rviz_visual_tools::colors color, const double thickness = 0.4, const std::string& ns = "result_path" );
00272 
00279   geometry_msgs::Point stateToPointMsg( int vertex_id, ob::PlannerDataPtr planner_data );
00280 
00281   geometry_msgs::Point stateToPointMsg( const ob::State *state );
00282 
00283   geometry_msgs::Point stateToPointMsg( const ob::ScopedState<> state );
00284 
00290   static int natRound(double x);
00291 
00297   bool publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors &color,
00298                     const rviz_visual_tools::scales scale = rviz_visual_tools::REGULAR,
00299                     const std::string& ns = "state_sphere");
00300   bool publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors &color,
00301                     double scale = 0.1, const std::string& ns = "state_sphere");
00302   bool publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors &color,
00303                     const geometry_msgs::Vector3 &scale, const std::string& ns = "state_sphere");
00304                    
00310   bool publishSampleRegion(const ob::ScopedState<>& state_area, const double& distance);
00311 
00315   bool publishText(const geometry_msgs::Point &point, const std::string &text,
00316                    const rviz_visual_tools::colors &color = rviz_visual_tools::BLACK,
00317                    bool static_id = true);
00318 
00319   bool publishText(const geometry_msgs::Pose &pose, const std::string &text, 
00320                    const rviz_visual_tools::colors &color = rviz_visual_tools::BLACK,
00321                    bool static_id = true);
00322 
00329   bool convertRobotStatesToTipPoints(const ompl::base::PlannerDataPtr &graph, 
00330                                      const std::vector<const robot_model::LinkModel*> &tips, 
00331                                      std::vector< std::vector<geometry_msgs::Point> > & vertex_tip_points);
00332 
00350 }; // end class
00351 
00352 typedef boost::shared_ptr<OmplVisualTools> OmplVisualToolsPtr;
00353 typedef boost::shared_ptr<const OmplVisualTools> OmplVisualToolsConstPtr;
00354 
00355 } // end namespace
00356 
00357 #endif


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