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 <ompl/util/Console.h>
51 
52 #include <memory>
53 
54 namespace ompl_interface
55 {
56 using namespace moveit_planners_ompl;
57 
58 #define OMPL_ROS_LOG(ros_log_level) \
59  { \
60  ROSCONSOLE_DEFINE_LOCATION(true, ros_log_level, ROSCONSOLE_NAME_PREFIX ".ompl"); \
61  if (ROS_UNLIKELY(__rosconsole_define_location__enabled)) \
62  ::ros::console::print(0, __rosconsole_define_location__loc.logger_, __rosconsole_define_location__loc.level_, \
63  filename, line, __ROSCONSOLE_FUNCTION__, "%s", text.c_str()); \
64  }
65 
67 {
68 public:
69  OMPLPlannerManager() : planning_interface::PlannerManager(), nh_("~"), display_random_valid_states_(false)
70  {
71  class OutputHandler : public ompl::msg::OutputHandler
72  {
73  public:
74  void log(const std::string& text, ompl::msg::LogLevel level, const char* filename, int line) override
75  {
76  switch (level)
77  {
78  case ompl::msg::LOG_DEV2:
79  case ompl::msg::LOG_DEV1:
80  case ompl::msg::LOG_DEBUG:
82  break;
83  case ompl::msg::LOG_INFO:
85  break;
86  case ompl::msg::LOG_WARN:
88  break;
89  case ompl::msg::LOG_ERROR:
91  break;
92  case ompl::msg::LOG_NONE:
93  default:
94  /* ignore */
95  break;
96  }
97  }
98  };
99 
100  output_handler_.reset(new OutputHandler());
101  ompl::msg::useOutputHandler(output_handler_.get());
102  }
103 
104  virtual bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns)
105  {
106  if (!ns.empty())
107  nh_ = ros::NodeHandle(ns);
108  ompl_interface_.reset(new OMPLInterface(model, nh_));
109  std::string ompl_ns = ns.empty() ? "ompl" : ns + "/ompl";
110  dynamic_reconfigure_server_.reset(
111  new dynamic_reconfigure::Server<OMPLDynamicReconfigureConfig>(ros::NodeHandle(nh_, ompl_ns)));
112  dynamic_reconfigure_server_->setCallback(
113  boost::bind(&OMPLPlannerManager::dynamicReconfigureCallback, this, _1, _2));
114  config_settings_ = ompl_interface_->getPlannerConfigurations();
115  return true;
116  }
117 
118  virtual bool canServiceRequest(const moveit_msgs::MotionPlanRequest& req) const
119  {
120  return req.trajectory_constraints.constraints.empty();
121  }
122 
123  virtual std::string getDescription() const
124  {
125  return "OMPL";
126  }
127 
128  virtual void getPlanningAlgorithms(std::vector<std::string>& algs) const
129  {
130  const planning_interface::PlannerConfigurationMap& pconfig = ompl_interface_->getPlannerConfigurations();
131  algs.clear();
132  algs.reserve(pconfig.size());
133  for (planning_interface::PlannerConfigurationMap::const_iterator it = pconfig.begin(); it != pconfig.end(); ++it)
134  algs.push_back(it->first);
135  }
136 
138  {
139  // this call can add a few more configs than we pass in (adds defaults)
140  ompl_interface_->setPlannerConfigurations(pconfig);
141  // so we read the configs instead of just setting pconfig
142  PlannerManager::setPlannerConfigurations(ompl_interface_->getPlannerConfigurations());
143  }
144 
145  virtual planning_interface::PlanningContextPtr
146  getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
147  const planning_interface::MotionPlanRequest& req, moveit_msgs::MoveItErrorCodes& error_code) const
148  {
149  return ompl_interface_->getPlanningContext(planning_scene, req, error_code);
150  }
151 
152 private:
153  /*
154  bool r = ompl_interface_->solve(planning_scene, req, res);
155  if (!planner_data_link_name_.empty())
156  displayPlannerData(planning_scene, planner_data_link_name_);
157  return r;
158  */
159 
160  /*
161  bool r = ompl_interface_->solve(planning_scene, req, res);
162  if (!planner_data_link_name_.empty())
163  displayPlannerData(planning_scene, planner_data_link_name_);
164  return r;
165  */
166 
167  /*
168  void displayRandomValidStates()
169  {
170  ompl_interface::ModelBasedPlanningContextPtr pc = ompl_interface_->getLastPlanningContext();
171  if (!pc || !pc->getPlanningScene())
172  {
173  ROS_ERROR("No planning context to sample states for");
174  return;
175  }
176  ROS_INFO_STREAM("Displaying states for context " << pc->getName());
177  const og::SimpleSetup &ss = pc->getOMPLSimpleSetup();
178  ob::ValidStateSamplerPtr vss = ss.getSpaceInformation()->allocValidStateSampler();
179  robot_state::RobotState kstate = pc->getPlanningScene()->getCurrentState();
180  ob::ScopedState<> rstate1(ss.getStateSpace());
181  ob::ScopedState<> rstate2(ss.getStateSpace());
182  ros::WallDuration wait(2);
183  unsigned int n = 0;
184  std::vector<ob::State*> sts;
185  if (vss->sample(rstate2.get()))
186  while (display_random_valid_states_)
187  {
188  if (!vss->sampleNear(rstate1.get(), rstate2.get(), 10000000))
189  continue;
190  pc->getOMPLStateSpace()->copyToRobotState(kstate, rstate1.get());
191  kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
192  moveit_msgs::DisplayRobotState state_msg;
193  robot_state::robotStateToRobotStateMsg(kstate, state_msg.state);
194  pub_valid_states_.publish(state_msg);
195  n = (n + 1) % 2;
196  if (n == 0)
197  {
198  robot_trajectory::RobotTrajectory traj(pc->getRobotModel(), pc->getJointModelGroupName());
199  unsigned int g = ss.getSpaceInformation()->getMotionStates(rstate1.get(), rstate2.get(), sts, 10, true, true);
200  ROS_INFO("Generated a motion with %u states", g);
201  for (std::size_t i = 0 ; i < g ; ++i)
202  {
203  pc->getOMPLStateSpace()->copyToRobotState(kstate, sts[i]);
204  traj.addSuffixWayPoint(kstate, 0.0);
205  }
206  moveit_msgs::DisplayTrajectory msg;
207  msg.model_id = pc->getRobotModel()->getName();
208  msg.trajectory.resize(1);
209  traj.getRobotTrajectoryMsg(msg.trajectory[0]);
210  robot_state::robotStateToRobotStateMsg(traj.getFirstWayPoint(), msg.trajectory_start);
211  pub_valid_traj_.publish(msg);
212  }
213  rstate2 = rstate1;
214  wait.sleep();
215  }
216  }
217 
218  void displayPlannerData(const planning_scene::PlanningSceneConstPtr& planning_scene,
219  const std::string &link_name) const
220  {
221  ompl_interface::ModelBasedPlanningContextPtr pc = ompl_interface_->getLastPlanningContext();
222  if (pc)
223  {
224  ompl::base::PlannerData pd(pc->getOMPLSimpleSetup()->getSpaceInformation());
225  pc->getOMPLSimpleSetup()->getPlannerData(pd);
226  robot_state::RobotState kstate = planning_scene->getCurrentState();
227  visualization_msgs::MarkerArray arr;
228  std_msgs::ColorRGBA color;
229  color.r = 1.0f;
230  color.g = 0.25f;
231  color.b = 1.0f;
232  color.a = 1.0f;
233  unsigned int nv = pd.numVertices();
234  for (unsigned int i = 0 ; i < nv ; ++i)
235  {
236  pc->getOMPLStateSpace()->copyToRobotState(kstate, pd.getVertex(i).getState());
237  kstate.getJointStateGroup(pc->getJointModelGroupName())->updateLinkTransforms();
238  const Eigen::Vector3d &pos = kstate.getLinkState(link_name)->getGlobalLinkTransform().translation();
239 
240  visualization_msgs::Marker mk;
241  mk.header.stamp = ros::Time::now();
242  mk.header.frame_id = planning_scene->getPlanningFrame();
243  mk.ns = "planner_data";
244  mk.id = i;
245  mk.type = visualization_msgs::Marker::SPHERE;
246  mk.action = visualization_msgs::Marker::ADD;
247  mk.pose.position.x = pos.x();
248  mk.pose.position.y = pos.y();
249  mk.pose.position.z = pos.z();
250  mk.pose.orientation.w = 1.0;
251  mk.scale.x = mk.scale.y = mk.scale.z = 0.025;
252  mk.color = color;
253  mk.lifetime = ros::Duration(30.0);
254  arr.markers.push_back(mk);
255  }
256  pub_markers_.publish(arr);
257  }
258  }
259  */
260 
261  void dynamicReconfigureCallback(OMPLDynamicReconfigureConfig& config, uint32_t level)
262  {
263  if (config.link_for_exploration_tree.empty() && !planner_data_link_name_.empty())
264  {
265  pub_markers_.shutdown();
266  planner_data_link_name_.clear();
267  ROS_INFO("Not displaying OMPL exploration data structures.");
268  }
269  else if (!config.link_for_exploration_tree.empty() && planner_data_link_name_.empty())
270  {
271  pub_markers_ = nh_.advertise<visualization_msgs::MarkerArray>("ompl_planner_data_marker_array", 5);
272  planner_data_link_name_ = config.link_for_exploration_tree;
273  ROS_INFO("Displaying OMPL exploration data structures for %s", planner_data_link_name_.c_str());
274  }
275 
276  ompl_interface_->simplifySolutions(config.simplify_solutions);
277  ompl_interface_->getPlanningContextManager().setMaximumSolutionSegmentLength(config.maximum_waypoint_distance);
278  ompl_interface_->getPlanningContextManager().setMinimumWaypointCount(config.minimum_waypoint_count);
279  if (display_random_valid_states_ && !config.display_random_valid_states)
280  {
281  display_random_valid_states_ = false;
282  if (pub_valid_states_thread_)
283  {
284  pub_valid_states_thread_->join();
285  pub_valid_states_thread_.reset();
286  }
287  pub_valid_states_.shutdown();
288  pub_valid_traj_.shutdown();
289  }
290  else if (!display_random_valid_states_ && config.display_random_valid_states)
291  {
292  pub_valid_states_ = nh_.advertise<moveit_msgs::DisplayRobotState>("ompl_planner_valid_states", 5);
293  pub_valid_traj_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("ompl_planner_valid_trajectories", 5);
294  display_random_valid_states_ = true;
295  // pub_valid_states_thread_.reset(new boost::thread(boost::bind(&OMPLPlannerManager::displayRandomValidStates,
296  // this)));
297  }
298  }
299 
301  std::unique_ptr<dynamic_reconfigure::Server<OMPLDynamicReconfigureConfig> > dynamic_reconfigure_server_;
302  std::unique_ptr<OMPLInterface> ompl_interface_;
303  std::unique_ptr<boost::thread> pub_valid_states_thread_;
309  std::shared_ptr<ompl::msg::OutputHandler> output_handler_;
310 };
311 
312 } // ompl_interface
313 
std::unique_ptr< boost::thread > pub_valid_states_thread_
filename
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
#define OMPL_ROS_LOG(ros_log_level)
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)
std::shared_ptr< ompl::msg::OutputHandler > output_handler_
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 Mon Oct 15 2018 03:46:31