ompl_planner_manager.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan, Dave Coleman */
00036 
00037 #include <moveit/ompl_interface/ompl_interface.h>
00038 #include <moveit/planning_interface/planning_interface.h>
00039 #include <moveit/planning_scene/planning_scene.h>
00040 #include <moveit/robot_state/conversions.h>
00041 #include <moveit/profiler/profiler.h>
00042 #include <class_loader/class_loader.h>
00043 
00044 #include <dynamic_reconfigure/server.h>
00045 #include "moveit_planners_ompl/OMPLDynamicReconfigureConfig.h"
00046 
00047 #include <moveit_msgs/DisplayRobotState.h>
00048 #include <moveit_msgs/DisplayTrajectory.h>
00049 
00050 namespace ompl_interface
00051 {
00052 using namespace moveit_planners_ompl;
00053 
00054 class OMPLPlannerManager : public planning_interface::PlannerManager
00055 {
00056 public:
00057 
00058   OMPLPlannerManager() : planning_interface::PlannerManager(),
00059                          nh_("~"),
00060                          display_random_valid_states_(false)
00061   {
00062   }
00063 
00064   virtual bool initialize(const robot_model::RobotModelConstPtr& model, const std::string &ns)
00065   {
00066     if (!ns.empty())
00067       nh_ = ros::NodeHandle(ns);
00068     ompl_interface_.reset(new OMPLInterface(model, nh_));
00069     std::string ompl_ns = ns.empty() ? "ompl" : ns + "/ompl";
00070     dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server<OMPLDynamicReconfigureConfig>(ros::NodeHandle(nh_, ompl_ns)));
00071     dynamic_reconfigure_server_->setCallback(boost::bind(&OMPLPlannerManager::dynamicReconfigureCallback, this, _1, _2));
00072     return true;
00073   }
00074 
00075   virtual bool canServiceRequest(const moveit_msgs::MotionPlanRequest &req) const
00076   {
00077     return req.trajectory_constraints.constraints.empty();
00078   }
00079 
00080   virtual std::string getDescription() const
00081   {
00082     return "OMPL";
00083   }
00084 
00085   virtual void getPlanningAlgorithms(std::vector<std::string> &algs) const
00086   {
00087     const planning_interface::PlannerConfigurationMap &pconfig = ompl_interface_->getPlannerConfigurations();
00088     algs.clear();
00089     algs.reserve(pconfig.size());
00090     for (planning_interface::PlannerConfigurationMap::const_iterator it = pconfig.begin() ; it != pconfig.end() ; ++it)
00091       algs.push_back(it->first);
00092   }
00093 
00094   virtual void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap &pconfig)
00095   {
00096     // this call can add a few more configs than we pass in (adds defaults)
00097     ompl_interface_->setPlannerConfigurations(pconfig);
00098     // so we read the configs instead of just setting pconfig
00099     PlannerManager::setPlannerConfigurations(ompl_interface_->getPlannerConfigurations());
00100   }
00101 
00102   virtual planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
00103                                                                     const planning_interface::MotionPlanRequest &req,
00104                                                                     moveit_msgs::MoveItErrorCodes &error_code) const
00105   {
00106     return ompl_interface_->getPlanningContext(planning_scene, req, error_code);
00107   }
00108 
00109 private:
00110 
00111     /*
00112     bool r = ompl_interface_->solve(planning_scene, req, res);
00113     if (!planner_data_link_name_.empty())
00114       displayPlannerData(planning_scene, planner_data_link_name_);
00115     return r;
00116     */
00117 
00118     /*
00119     bool r = ompl_interface_->solve(planning_scene, req, res);
00120     if (!planner_data_link_name_.empty())
00121       displayPlannerData(planning_scene, planner_data_link_name_);
00122     return r;
00123     */
00124 
00125   /*
00126   void displayRandomValidStates()
00127   {
00128     ompl_interface::ModelBasedPlanningContextPtr pc = ompl_interface_->getLastPlanningContext();
00129     if (!pc || !pc->getPlanningScene())
00130     {
00131       ROS_ERROR("No planning context to sample states for");
00132       return;
00133     }
00134     ROS_INFO_STREAM("Displaying states for context " << pc->getName());
00135     const og::SimpleSetup &ss = pc->getOMPLSimpleSetup();
00136     ob::ValidStateSamplerPtr vss = ss.getSpaceInformation()->allocValidStateSampler();
00137     robot_state::RobotState kstate = pc->getPlanningScene()->getCurrentState();
00138     ob::ScopedState<> rstate1(ss.getStateSpace());
00139     ob::ScopedState<> rstate2(ss.getStateSpace());
00140     ros::WallDuration wait(2);
00141     unsigned int n = 0;
00142     std::vector<ob::State*> sts;
00143     if (vss->sample(rstate2.get()))
00144       while (display_random_valid_states_)
00145       {
00146         if (!vss->sampleNear(rstate1.get(), rstate2.get(), 10000000))
00147           continue;
00148         pc->getOMPLStateSpace()->copyToRobotState(kstate, rstate1.get());
00149         kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
00150         moveit_msgs::DisplayRobotState state_msg;
00151         robot_state::robotStateToRobotStateMsg(kstate, state_msg.state);
00152         pub_valid_states_.publish(state_msg);
00153         n = (n + 1) % 2;
00154         if (n == 0)
00155         {
00156           robot_trajectory::RobotTrajectory traj(pc->getRobotModel(), pc->getJointModelGroupName());
00157           unsigned int g = ss.getSpaceInformation()->getMotionStates(rstate1.get(), rstate2.get(), sts, 10, true, true);
00158           ROS_INFO("Generated a motion with %u states", g);
00159           for (std::size_t i = 0 ; i < g ; ++i)
00160           {
00161             pc->getOMPLStateSpace()->copyToRobotState(kstate, sts[i]);
00162             traj.addSuffixWayPoint(kstate, 0.0);
00163           }
00164           moveit_msgs::DisplayTrajectory msg;
00165           msg.model_id = pc->getRobotModel()->getName();
00166           msg.trajectory.resize(1);
00167           traj.getRobotTrajectoryMsg(msg.trajectory[0]);
00168           robot_state::robotStateToRobotStateMsg(traj.getFirstWayPoint(), msg.trajectory_start);
00169           pub_valid_traj_.publish(msg);
00170         }
00171         rstate2 = rstate1;
00172         wait.sleep();
00173       }
00174   }
00175 
00176   void displayPlannerData(const planning_scene::PlanningSceneConstPtr& planning_scene,
00177                           const std::string &link_name) const
00178   {
00179     ompl_interface::ModelBasedPlanningContextPtr pc = ompl_interface_->getLastPlanningContext();
00180     if (pc)
00181     {
00182       ompl::base::PlannerData pd(pc->getOMPLSimpleSetup()->getSpaceInformation());
00183       pc->getOMPLSimpleSetup()->getPlannerData(pd);
00184       robot_state::RobotState kstate = planning_scene->getCurrentState();
00185       visualization_msgs::MarkerArray arr;
00186       std_msgs::ColorRGBA color;
00187       color.r = 1.0f;
00188       color.g = 0.25f;
00189       color.b = 1.0f;
00190       color.a = 1.0f;
00191       unsigned int nv = pd.numVertices();
00192       for (unsigned int i = 0 ; i < nv ; ++i)
00193       {
00194         pc->getOMPLStateSpace()->copyToRobotState(kstate, pd.getVertex(i).getState());
00195         kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
00196         const Eigen::Vector3d &pos = kstate.getLinkState(link_name)->getGlobalLinkTransform().translation();
00197 
00198         visualization_msgs::Marker mk;
00199         mk.header.stamp = ros::Time::now();
00200         mk.header.frame_id = planning_scene->getPlanningFrame();
00201         mk.ns = "planner_data";
00202         mk.id = i;
00203         mk.type = visualization_msgs::Marker::SPHERE;
00204         mk.action = visualization_msgs::Marker::ADD;
00205         mk.pose.position.x = pos.x();
00206         mk.pose.position.y = pos.y();
00207         mk.pose.position.z = pos.z();
00208         mk.pose.orientation.w = 1.0;
00209         mk.scale.x = mk.scale.y = mk.scale.z = 0.025;
00210         mk.color = color;
00211         mk.lifetime = ros::Duration(30.0);
00212         arr.markers.push_back(mk);
00213       }
00214       pub_markers_.publish(arr);
00215     }
00216   }
00217   */
00218 
00219   void dynamicReconfigureCallback(OMPLDynamicReconfigureConfig &config, uint32_t level)
00220   {
00221     if (config.link_for_exploration_tree.empty() && !planner_data_link_name_.empty())
00222     {
00223       pub_markers_.shutdown();
00224       planner_data_link_name_.clear();
00225       ROS_INFO("Not displaying OMPL exploration data structures.");
00226     }
00227     else
00228       if (!config.link_for_exploration_tree.empty() && planner_data_link_name_.empty())
00229       {
00230         pub_markers_ = nh_.advertise<visualization_msgs::MarkerArray>("ompl_planner_data_marker_array", 5);
00231         planner_data_link_name_ = config.link_for_exploration_tree;
00232         ROS_INFO("Displaying OMPL exploration data structures for %s", planner_data_link_name_.c_str());
00233       }
00234 
00235     ompl_interface_->simplifySolutions(config.simplify_solutions);
00236     ompl_interface_->getPlanningContextManager().setMaximumSolutionSegmentLength(config.maximum_waypoint_distance);
00237     ompl_interface_->getPlanningContextManager().setMinimumWaypointCount(config.minimum_waypoint_count);
00238     if (display_random_valid_states_ && !config.display_random_valid_states)
00239     {
00240       display_random_valid_states_ = false;
00241       if (pub_valid_states_thread_)
00242       {
00243         pub_valid_states_thread_->join();
00244         pub_valid_states_thread_.reset();
00245       }
00246       pub_valid_states_.shutdown();
00247       pub_valid_traj_.shutdown();
00248     }
00249     else
00250       if (!display_random_valid_states_ && config.display_random_valid_states)
00251       {
00252         pub_valid_states_ = nh_.advertise<moveit_msgs::DisplayRobotState>("ompl_planner_valid_states", 5);
00253         pub_valid_traj_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("ompl_planner_valid_trajectories", 5);
00254     display_random_valid_states_ = true;
00255         //    pub_valid_states_thread_.reset(new boost::thread(boost::bind(&OMPLPlannerManager::displayRandomValidStates, this)));
00256       }
00257   }
00258 
00259   ros::NodeHandle nh_;
00260   boost::scoped_ptr<dynamic_reconfigure::Server<OMPLDynamicReconfigureConfig> > dynamic_reconfigure_server_;
00261   boost::scoped_ptr<OMPLInterface> ompl_interface_;
00262   boost::scoped_ptr<boost::thread> pub_valid_states_thread_;
00263   bool display_random_valid_states_;
00264   ros::Publisher pub_markers_;
00265   ros::Publisher pub_valid_states_;
00266   ros::Publisher pub_valid_traj_;
00267   std::string planner_data_link_name_;
00268 };
00269 
00270 } // ompl_interface
00271 
00272 CLASS_LOADER_REGISTER_CLASS(ompl_interface::OMPLPlannerManager, planning_interface::PlannerManager);


ompl
Author(s): Ioan Sucan
autogenerated on Wed Sep 16 2015 04:42:27