ompl_planner_manager.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, Dave Coleman */
36 
43 
44 #include <dynamic_reconfigure/server.h>
45 #include "moveit_planners_ompl/OMPLDynamicReconfigureConfig.h"
46 
47 #include <moveit_msgs/DisplayRobotState.h>
48 #include <moveit_msgs/DisplayTrajectory.h>
49 
50 #include <memory>
51 
52 namespace ompl_interface
53 {
54 using namespace moveit_planners_ompl;
55 
57 {
58 public:
59  OMPLPlannerManager() : planning_interface::PlannerManager(), nh_("~"), display_random_valid_states_(false)
60  {
61  }
62 
63  virtual bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns)
64  {
65  if (!ns.empty())
66  nh_ = ros::NodeHandle(ns);
67  ompl_interface_.reset(new OMPLInterface(model, nh_));
68  std::string ompl_ns = ns.empty() ? "ompl" : ns + "/ompl";
69  dynamic_reconfigure_server_.reset(
70  new dynamic_reconfigure::Server<OMPLDynamicReconfigureConfig>(ros::NodeHandle(nh_, ompl_ns)));
71  dynamic_reconfigure_server_->setCallback(
72  boost::bind(&OMPLPlannerManager::dynamicReconfigureCallback, this, _1, _2));
73  config_settings_ = ompl_interface_->getPlannerConfigurations();
74  return true;
75  }
76 
77  virtual bool canServiceRequest(const moveit_msgs::MotionPlanRequest& req) const
78  {
79  return req.trajectory_constraints.constraints.empty();
80  }
81 
82  virtual std::string getDescription() const
83  {
84  return "OMPL";
85  }
86 
87  virtual void getPlanningAlgorithms(std::vector<std::string>& algs) const
88  {
89  const planning_interface::PlannerConfigurationMap& pconfig = ompl_interface_->getPlannerConfigurations();
90  algs.clear();
91  algs.reserve(pconfig.size());
92  for (planning_interface::PlannerConfigurationMap::const_iterator it = pconfig.begin(); it != pconfig.end(); ++it)
93  algs.push_back(it->first);
94  }
95 
97  {
98  // this call can add a few more configs than we pass in (adds defaults)
99  ompl_interface_->setPlannerConfigurations(pconfig);
100  // so we read the configs instead of just setting pconfig
101  PlannerManager::setPlannerConfigurations(ompl_interface_->getPlannerConfigurations());
102  }
103 
104  virtual planning_interface::PlanningContextPtr
105  getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
106  const planning_interface::MotionPlanRequest& req, moveit_msgs::MoveItErrorCodes& error_code) const
107  {
108  return ompl_interface_->getPlanningContext(planning_scene, req, error_code);
109  }
110 
111 private:
112  /*
113  bool r = ompl_interface_->solve(planning_scene, req, res);
114  if (!planner_data_link_name_.empty())
115  displayPlannerData(planning_scene, planner_data_link_name_);
116  return r;
117  */
118 
119  /*
120  bool r = ompl_interface_->solve(planning_scene, req, res);
121  if (!planner_data_link_name_.empty())
122  displayPlannerData(planning_scene, planner_data_link_name_);
123  return r;
124  */
125 
126  /*
127  void displayRandomValidStates()
128  {
129  ompl_interface::ModelBasedPlanningContextPtr pc = ompl_interface_->getLastPlanningContext();
130  if (!pc || !pc->getPlanningScene())
131  {
132  ROS_ERROR("No planning context to sample states for");
133  return;
134  }
135  ROS_INFO_STREAM("Displaying states for context " << pc->getName());
136  const og::SimpleSetup &ss = pc->getOMPLSimpleSetup();
137  ob::ValidStateSamplerPtr vss = ss.getSpaceInformation()->allocValidStateSampler();
138  robot_state::RobotState kstate = pc->getPlanningScene()->getCurrentState();
139  ob::ScopedState<> rstate1(ss.getStateSpace());
140  ob::ScopedState<> rstate2(ss.getStateSpace());
141  ros::WallDuration wait(2);
142  unsigned int n = 0;
143  std::vector<ob::State*> sts;
144  if (vss->sample(rstate2.get()))
145  while (display_random_valid_states_)
146  {
147  if (!vss->sampleNear(rstate1.get(), rstate2.get(), 10000000))
148  continue;
149  pc->getOMPLStateSpace()->copyToRobotState(kstate, rstate1.get());
150  kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
151  moveit_msgs::DisplayRobotState state_msg;
152  robot_state::robotStateToRobotStateMsg(kstate, state_msg.state);
153  pub_valid_states_.publish(state_msg);
154  n = (n + 1) % 2;
155  if (n == 0)
156  {
157  robot_trajectory::RobotTrajectory traj(pc->getRobotModel(), pc->getJointModelGroupName());
158  unsigned int g = ss.getSpaceInformation()->getMotionStates(rstate1.get(), rstate2.get(), sts, 10, true, true);
159  ROS_INFO("Generated a motion with %u states", g);
160  for (std::size_t i = 0 ; i < g ; ++i)
161  {
162  pc->getOMPLStateSpace()->copyToRobotState(kstate, sts[i]);
163  traj.addSuffixWayPoint(kstate, 0.0);
164  }
165  moveit_msgs::DisplayTrajectory msg;
166  msg.model_id = pc->getRobotModel()->getName();
167  msg.trajectory.resize(1);
168  traj.getRobotTrajectoryMsg(msg.trajectory[0]);
169  robot_state::robotStateToRobotStateMsg(traj.getFirstWayPoint(), msg.trajectory_start);
170  pub_valid_traj_.publish(msg);
171  }
172  rstate2 = rstate1;
173  wait.sleep();
174  }
175  }
176 
177  void displayPlannerData(const planning_scene::PlanningSceneConstPtr& planning_scene,
178  const std::string &link_name) const
179  {
180  ompl_interface::ModelBasedPlanningContextPtr pc = ompl_interface_->getLastPlanningContext();
181  if (pc)
182  {
183  ompl::base::PlannerData pd(pc->getOMPLSimpleSetup()->getSpaceInformation());
184  pc->getOMPLSimpleSetup()->getPlannerData(pd);
185  robot_state::RobotState kstate = planning_scene->getCurrentState();
186  visualization_msgs::MarkerArray arr;
187  std_msgs::ColorRGBA color;
188  color.r = 1.0f;
189  color.g = 0.25f;
190  color.b = 1.0f;
191  color.a = 1.0f;
192  unsigned int nv = pd.numVertices();
193  for (unsigned int i = 0 ; i < nv ; ++i)
194  {
195  pc->getOMPLStateSpace()->copyToRobotState(kstate, pd.getVertex(i).getState());
196  kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
197  const Eigen::Vector3d &pos = kstate.getLinkState(link_name)->getGlobalLinkTransform().translation();
198 
199  visualization_msgs::Marker mk;
200  mk.header.stamp = ros::Time::now();
201  mk.header.frame_id = planning_scene->getPlanningFrame();
202  mk.ns = "planner_data";
203  mk.id = i;
204  mk.type = visualization_msgs::Marker::SPHERE;
205  mk.action = visualization_msgs::Marker::ADD;
206  mk.pose.position.x = pos.x();
207  mk.pose.position.y = pos.y();
208  mk.pose.position.z = pos.z();
209  mk.pose.orientation.w = 1.0;
210  mk.scale.x = mk.scale.y = mk.scale.z = 0.025;
211  mk.color = color;
212  mk.lifetime = ros::Duration(30.0);
213  arr.markers.push_back(mk);
214  }
215  pub_markers_.publish(arr);
216  }
217  }
218  */
219 
220  void dynamicReconfigureCallback(OMPLDynamicReconfigureConfig& config, uint32_t level)
221  {
222  if (config.link_for_exploration_tree.empty() && !planner_data_link_name_.empty())
223  {
224  pub_markers_.shutdown();
225  planner_data_link_name_.clear();
226  ROS_INFO("Not displaying OMPL exploration data structures.");
227  }
228  else if (!config.link_for_exploration_tree.empty() && planner_data_link_name_.empty())
229  {
230  pub_markers_ = nh_.advertise<visualization_msgs::MarkerArray>("ompl_planner_data_marker_array", 5);
231  planner_data_link_name_ = config.link_for_exploration_tree;
232  ROS_INFO("Displaying OMPL exploration data structures for %s", planner_data_link_name_.c_str());
233  }
234 
235  ompl_interface_->simplifySolutions(config.simplify_solutions);
236  ompl_interface_->getPlanningContextManager().setMaximumSolutionSegmentLength(config.maximum_waypoint_distance);
237  ompl_interface_->getPlanningContextManager().setMinimumWaypointCount(config.minimum_waypoint_count);
238  if (display_random_valid_states_ && !config.display_random_valid_states)
239  {
240  display_random_valid_states_ = false;
241  if (pub_valid_states_thread_)
242  {
243  pub_valid_states_thread_->join();
244  pub_valid_states_thread_.reset();
245  }
246  pub_valid_states_.shutdown();
247  pub_valid_traj_.shutdown();
248  }
249  else if (!display_random_valid_states_ && config.display_random_valid_states)
250  {
251  pub_valid_states_ = nh_.advertise<moveit_msgs::DisplayRobotState>("ompl_planner_valid_states", 5);
252  pub_valid_traj_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("ompl_planner_valid_trajectories", 5);
253  display_random_valid_states_ = true;
254  // pub_valid_states_thread_.reset(new boost::thread(boost::bind(&OMPLPlannerManager::displayRandomValidStates,
255  // this)));
256  }
257  }
258 
260  std::unique_ptr<dynamic_reconfigure::Server<OMPLDynamicReconfigureConfig> > dynamic_reconfigure_server_;
261  std::unique_ptr<OMPLInterface> ompl_interface_;
262  std::unique_ptr<boost::thread> pub_valid_states_thread_;
268 };
269 
270 } // ompl_interface
271 
std::unique_ptr< boost::thread > pub_valid_states_thread_
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
The MoveIt interface to OMPL.
virtual void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap &pconfig)
std::unique_ptr< OMPLInterface > ompl_interface_
virtual std::string getDescription() const
#define ROS_INFO(...)
virtual bool initialize(const robot_model::RobotModelConstPtr &model, const std::string &ns)
moveit_msgs::MotionPlanRequest MotionPlanRequest
virtual planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, moveit_msgs::MoveItErrorCodes &error_code) const
void dynamicReconfigureCallback(OMPLDynamicReconfigureConfig &config, uint32_t level)
virtual bool canServiceRequest(const moveit_msgs::MotionPlanRequest &req) const
virtual void getPlanningAlgorithms(std::vector< std::string > &algs) const
std::unique_ptr< dynamic_reconfigure::Server< OMPLDynamicReconfigureConfig > > dynamic_reconfigure_server_
CLASS_LOADER_REGISTER_CLASS(ompl_interface::OMPLPlannerManager, planning_interface::PlannerManager)


ompl
Author(s): Ioan Sucan
autogenerated on Sun Jan 21 2018 03:55:31