ompl_planner.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 */
36 
40 #include <tf/transform_listener.h>
41 #include <moveit_msgs/DisplayTrajectory.h>
43 #include <moveit_msgs/GetMotionPlan.h>
44 
45 static const std::string PLANNER_NODE_NAME = "ompl_planning"; // name of node
46 static const std::string PLANNER_SERVICE_NAME =
47  "plan_kinematic_path"; // name of the advertised service (within the ~ namespace)
48 static const std::string ROBOT_DESCRIPTION =
49  "robot_description"; // name of the robot description (a param name, so it can be changed externally)
50 
52 {
53 public:
55  : nh_("~"), psm_(psm), ompl_interface_(psm.getPlanningScene()->getRobotModel()), debug_(debug)
56  {
58  if (debug_)
59  {
60  pub_plan_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("display_motion_plan", 100);
61  pub_request_ = nh_.advertise<moveit_msgs::MotionPlanRequest>("motion_plan_request", 100);
62  }
63  }
64 
65  bool computePlan(moveit_msgs::GetMotionPlan::Request& req, moveit_msgs::GetMotionPlan::Response& res)
66  {
67  ROS_INFO("Received new planning request...");
68  if (debug_)
69  pub_request_.publish(req.motion_plan_request);
71 
72  ompl_interface::ModelBasedPlanningContextPtr context =
73  ompl_interface_.getPlanningContext(psm_.getPlanningScene(), req.motion_plan_request);
74  if (!context)
75  {
76  ROS_ERROR_STREAM_NAMED("computePlan", "No planning context found");
77  return false;
78  }
79  context->clear();
80 
81  bool result = context->solve(response);
82 
83  if (debug_)
84  {
85  if (result)
86  displaySolution(res.motion_plan_response);
87  std::stringstream ss;
88  ROS_INFO("%s", ss.str().c_str());
89  }
90  return result;
91  }
92 
93  void displaySolution(const moveit_msgs::MotionPlanResponse& mplan_res)
94  {
95  moveit_msgs::DisplayTrajectory d;
96  d.model_id = psm_.getPlanningScene()->getRobotModel()->getName();
97  d.trajectory_start = mplan_res.trajectory_start;
98  d.trajectory.resize(1, mplan_res.trajectory);
99  pub_plan_.publish(d);
100  }
101 
102  void status()
103  {
105  ROS_INFO("Responding to planning and bechmark requests");
106  if (debug_)
107  ROS_INFO("Publishing debug information");
108  }
109 
110 private:
118  bool debug_;
119 };
120 
121 int main(int argc, char** argv)
122 {
123  ros::init(argc, argv, PLANNER_NODE_NAME);
124 
125  bool debug = false;
126  for (int i = 1; i < argc; ++i)
127  if (strncmp(argv[i], "--debug", 7) == 0)
128  debug = true;
129 
131  spinner.start();
132 
135  if (psm.getPlanningScene())
136  {
138  psm.startSceneMonitor();
139  psm.startStateMonitor();
140 
141  OMPLPlannerService pservice(psm, debug);
142  pservice.status();
144  }
145  else
146  ROS_ERROR("Planning scene not configured");
147 
148  return 0;
149 }
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
d
void displaySolution(const moveit_msgs::MotionPlanResponse &mplan_res)
void publish(const boost::shared_ptr< M > &message) const
#define ROS_ERROR_STREAM_NAMED(name, args)
ros::ServiceServer plan_service_
ros::ServiceServer display_states_service_
static const std::string ROBOT_DESCRIPTION
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool computePlan(moveit_msgs::GetMotionPlan::Request &req, moveit_msgs::GetMotionPlan::Response &res)
ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req) const
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
OMPLPlannerService(planning_scene_monitor::PlanningSceneMonitor &psm, bool debug=false)
void spinner()
const planning_scene::PlanningScenePtr & getPlanningScene()
ros::NodeHandle nh_
ros::Publisher pub_request_
void startWorldGeometryMonitor(const std::string &collision_objects_topic=DEFAULT_COLLISION_OBJECT_TOPIC, const std::string &planning_scene_world_topic=DEFAULT_PLANNING_SCENE_WORLD_TOPIC, const bool load_octomap_monitor=true)
ompl_interface::OMPLInterface ompl_interface_
ros::Publisher pub_plan_
void printStatus()
Print the status of this node.
#define ROS_INFO(...)
static const std::string PLANNER_SERVICE_NAME
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void startStateMonitor(const std::string &joint_states_topic=DEFAULT_JOINT_STATES_TOPIC, const std::string &attached_objects_topic=DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC)
static const std::string PLANNER_NODE_NAME
planning_scene_monitor::PlanningSceneMonitor & psm_
#define ROS_ERROR(...)
int main(int argc, char **argv)
ROSCPP_DECL void waitForShutdown()


ompl
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:01