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   OMPLPlannerManager() : planning_interface::PlannerManager(), nh_("~"), display_random_valid_states_(false)
00058   {
00059   }
00060 
00061   virtual bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns)
00062   {
00063     if (!ns.empty())
00064       nh_ = ros::NodeHandle(ns);
00065     ompl_interface_.reset(new OMPLInterface(model, nh_));
00066     std::string ompl_ns = ns.empty() ? "ompl" : ns + "/ompl";
00067     dynamic_reconfigure_server_.reset(
00068         new dynamic_reconfigure::Server<OMPLDynamicReconfigureConfig>(ros::NodeHandle(nh_, ompl_ns)));
00069     dynamic_reconfigure_server_->setCallback(
00070         boost::bind(&OMPLPlannerManager::dynamicReconfigureCallback, this, _1, _2));
00071     config_settings_ = ompl_interface_->getPlannerConfigurations();
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
00103   getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
00104                      const planning_interface::MotionPlanRequest& req, moveit_msgs::MoveItErrorCodes& error_code) const
00105   {
00106     return ompl_interface_->getPlanningContext(planning_scene, req, error_code);
00107   }
00108 
00109 private:
00110   /*
00111   bool r = ompl_interface_->solve(planning_scene, req, res);
00112   if (!planner_data_link_name_.empty())
00113     displayPlannerData(planning_scene, planner_data_link_name_);
00114   return r;
00115   */
00116 
00117   /*
00118   bool r = ompl_interface_->solve(planning_scene, req, res);
00119   if (!planner_data_link_name_.empty())
00120     displayPlannerData(planning_scene, planner_data_link_name_);
00121   return r;
00122   */
00123 
00124   /*
00125   void displayRandomValidStates()
00126   {
00127     ompl_interface::ModelBasedPlanningContextPtr pc = ompl_interface_->getLastPlanningContext();
00128     if (!pc || !pc->getPlanningScene())
00129     {
00130       ROS_ERROR("No planning context to sample states for");
00131       return;
00132     }
00133     ROS_INFO_STREAM("Displaying states for context " << pc->getName());
00134     const og::SimpleSetup &ss = pc->getOMPLSimpleSetup();
00135     ob::ValidStateSamplerPtr vss = ss.getSpaceInformation()->allocValidStateSampler();
00136     robot_state::RobotState kstate = pc->getPlanningScene()->getCurrentState();
00137     ob::ScopedState<> rstate1(ss.getStateSpace());
00138     ob::ScopedState<> rstate2(ss.getStateSpace());
00139     ros::WallDuration wait(2);
00140     unsigned int n = 0;
00141     std::vector<ob::State*> sts;
00142     if (vss->sample(rstate2.get()))
00143       while (display_random_valid_states_)
00144       {
00145         if (!vss->sampleNear(rstate1.get(), rstate2.get(), 10000000))
00146           continue;
00147         pc->getOMPLStateSpace()->copyToRobotState(kstate, rstate1.get());
00148         kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
00149         moveit_msgs::DisplayRobotState state_msg;
00150         robot_state::robotStateToRobotStateMsg(kstate, state_msg.state);
00151         pub_valid_states_.publish(state_msg);
00152         n = (n + 1) % 2;
00153         if (n == 0)
00154         {
00155           robot_trajectory::RobotTrajectory traj(pc->getRobotModel(), pc->getJointModelGroupName());
00156           unsigned int g = ss.getSpaceInformation()->getMotionStates(rstate1.get(), rstate2.get(), sts, 10, true, true);
00157           ROS_INFO("Generated a motion with %u states", g);
00158           for (std::size_t i = 0 ; i < g ; ++i)
00159           {
00160             pc->getOMPLStateSpace()->copyToRobotState(kstate, sts[i]);
00161             traj.addSuffixWayPoint(kstate, 0.0);
00162           }
00163           moveit_msgs::DisplayTrajectory msg;
00164           msg.model_id = pc->getRobotModel()->getName();
00165           msg.trajectory.resize(1);
00166           traj.getRobotTrajectoryMsg(msg.trajectory[0]);
00167           robot_state::robotStateToRobotStateMsg(traj.getFirstWayPoint(), msg.trajectory_start);
00168           pub_valid_traj_.publish(msg);
00169         }
00170         rstate2 = rstate1;
00171         wait.sleep();
00172       }
00173   }
00174 
00175   void displayPlannerData(const planning_scene::PlanningSceneConstPtr& planning_scene,
00176                           const std::string &link_name) const
00177   {
00178     ompl_interface::ModelBasedPlanningContextPtr pc = ompl_interface_->getLastPlanningContext();
00179     if (pc)
00180     {
00181       ompl::base::PlannerData pd(pc->getOMPLSimpleSetup()->getSpaceInformation());
00182       pc->getOMPLSimpleSetup()->getPlannerData(pd);
00183       robot_state::RobotState kstate = planning_scene->getCurrentState();
00184       visualization_msgs::MarkerArray arr;
00185       std_msgs::ColorRGBA color;
00186       color.r = 1.0f;
00187       color.g = 0.25f;
00188       color.b = 1.0f;
00189       color.a = 1.0f;
00190       unsigned int nv = pd.numVertices();
00191       for (unsigned int i = 0 ; i < nv ; ++i)
00192       {
00193         pc->getOMPLStateSpace()->copyToRobotState(kstate, pd.getVertex(i).getState());
00194         kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
00195         const Eigen::Vector3d &pos = kstate.getLinkState(link_name)->getGlobalLinkTransform().translation();
00196 
00197         visualization_msgs::Marker mk;
00198         mk.header.stamp = ros::Time::now();
00199         mk.header.frame_id = planning_scene->getPlanningFrame();
00200         mk.ns = "planner_data";
00201         mk.id = i;
00202         mk.type = visualization_msgs::Marker::SPHERE;
00203         mk.action = visualization_msgs::Marker::ADD;
00204         mk.pose.position.x = pos.x();
00205         mk.pose.position.y = pos.y();
00206         mk.pose.position.z = pos.z();
00207         mk.pose.orientation.w = 1.0;
00208         mk.scale.x = mk.scale.y = mk.scale.z = 0.025;
00209         mk.color = color;
00210         mk.lifetime = ros::Duration(30.0);
00211         arr.markers.push_back(mk);
00212       }
00213       pub_markers_.publish(arr);
00214     }
00215   }
00216   */
00217 
00218   void dynamicReconfigureCallback(OMPLDynamicReconfigureConfig& config, uint32_t level)
00219   {
00220     if (config.link_for_exploration_tree.empty() && !planner_data_link_name_.empty())
00221     {
00222       pub_markers_.shutdown();
00223       planner_data_link_name_.clear();
00224       ROS_INFO("Not displaying OMPL exploration data structures.");
00225     }
00226     else if (!config.link_for_exploration_tree.empty() && planner_data_link_name_.empty())
00227     {
00228       pub_markers_ = nh_.advertise<visualization_msgs::MarkerArray>("ompl_planner_data_marker_array", 5);
00229       planner_data_link_name_ = config.link_for_exploration_tree;
00230       ROS_INFO("Displaying OMPL exploration data structures for %s", planner_data_link_name_.c_str());
00231     }
00232 
00233     ompl_interface_->simplifySolutions(config.simplify_solutions);
00234     ompl_interface_->getPlanningContextManager().setMaximumSolutionSegmentLength(config.maximum_waypoint_distance);
00235     ompl_interface_->getPlanningContextManager().setMinimumWaypointCount(config.minimum_waypoint_count);
00236     if (display_random_valid_states_ && !config.display_random_valid_states)
00237     {
00238       display_random_valid_states_ = false;
00239       if (pub_valid_states_thread_)
00240       {
00241         pub_valid_states_thread_->join();
00242         pub_valid_states_thread_.reset();
00243       }
00244       pub_valid_states_.shutdown();
00245       pub_valid_traj_.shutdown();
00246     }
00247     else if (!display_random_valid_states_ && config.display_random_valid_states)
00248     {
00249       pub_valid_states_ = nh_.advertise<moveit_msgs::DisplayRobotState>("ompl_planner_valid_states", 5);
00250       pub_valid_traj_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("ompl_planner_valid_trajectories", 5);
00251       display_random_valid_states_ = true;
00252       //    pub_valid_states_thread_.reset(new boost::thread(boost::bind(&OMPLPlannerManager::displayRandomValidStates,
00253       //    this)));
00254     }
00255   }
00256 
00257   ros::NodeHandle nh_;
00258   boost::scoped_ptr<dynamic_reconfigure::Server<OMPLDynamicReconfigureConfig> > dynamic_reconfigure_server_;
00259   boost::scoped_ptr<OMPLInterface> ompl_interface_;
00260   boost::scoped_ptr<boost::thread> pub_valid_states_thread_;
00261   bool display_random_valid_states_;
00262   ros::Publisher pub_markers_;
00263   ros::Publisher pub_valid_states_;
00264   ros::Publisher pub_valid_traj_;
00265   std::string planner_data_link_name_;
00266 };
00267 
00268 }  // ompl_interface
00269 
00270 CLASS_LOADER_REGISTER_CLASS(ompl_interface::OMPLPlannerManager, planning_interface::PlannerManager);


ompl
Author(s): Ioan Sucan
autogenerated on Mon Apr 23 2018 10:25:27