planning_interface.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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 
38 #include <boost/thread/mutex.hpp>
39 #include <set>
40 
41 namespace planning_interface
42 {
43 namespace
44 {
45 // keep track of currently active contexts
46 struct ActiveContexts
47 {
48  boost::mutex mutex_;
49  std::set<PlanningContext*> contexts_;
50 };
51 
52 static ActiveContexts& getActiveContexts()
53 {
54  static ActiveContexts ac;
55  return ac;
56 }
57 } // namespace
58 
60 {
61  // Handle new configuration scheme with multiple pipelines configured in namespaces planning_pipelines/*
62  std::string default_pipeline;
63  if (nh.getParam("default_planning_pipeline", default_pipeline) &&
64  nh.hasParam("planning_pipelines/" + default_pipeline))
65  return ros::NodeHandle(nh, "planning_pipelines/" + default_pipeline);
66  return nh;
67 }
68 
69 PlanningContext::PlanningContext(const std::string& name, const std::string& group) : name_(name), group_(group)
70 {
71  ActiveContexts& ac = getActiveContexts();
72  boost::mutex::scoped_lock _(ac.mutex_);
73  ac.contexts_.insert(this);
74 }
75 
77 {
78  ActiveContexts& ac = getActiveContexts();
79  boost::mutex::scoped_lock _(ac.mutex_);
80  ac.contexts_.erase(this);
81 }
82 
83 void PlanningContext::setPlanningScene(const planning_scene::PlanningSceneConstPtr& planning_scene)
84 {
85  planning_scene_ = planning_scene;
86 }
87 
89 {
90  request_ = request;
91  if (request_.allowed_planning_time <= 0.0)
92  {
93  ROS_INFO_NAMED("planning_interface",
94  "The timeout for planning must be positive (%lf specified). Assuming one second instead.",
95  request_.allowed_planning_time);
96  request_.allowed_planning_time = 1.0;
97  }
98  if (request_.num_planning_attempts < 0)
99  ROS_ERROR_NAMED("planning_interface", "The number of desired planning attempts should be positive. "
100  "Assuming one attempt.");
101  request_.num_planning_attempts = std::max(1, request_.num_planning_attempts);
102 }
103 
104 bool PlannerManager::initialize(const robot_model::RobotModelConstPtr& /*unused*/, const std::string& /*unused*/)
105 {
106  return true;
107 }
108 
110 {
111  return "";
112 }
113 
114 PlanningContextPtr PlannerManager::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
115  const MotionPlanRequest& req) const
116 {
117  moveit_msgs::MoveItErrorCodes dummy;
118  return getPlanningContext(planning_scene, req, dummy);
119 }
120 
121 void PlannerManager::getPlanningAlgorithms(std::vector<std::string>& algs) const
122 {
123  // nothing by default
124  algs.clear();
125 }
126 
128 {
129  config_settings_ = pcs;
130 }
131 
133 {
134  ActiveContexts& ac = getActiveContexts();
135  boost::mutex::scoped_lock _(ac.mutex_);
136  for (std::set<PlanningContext*>::iterator it = ac.contexts_.begin(); it != ac.contexts_.end(); ++it)
137  (*it)->terminate();
138 }
139 
140 } // end of namespace planning_interface
void setMotionPlanRequest(const MotionPlanRequest &request)
Set the planning request for this context.
#define ROS_INFO_NAMED(name,...)
ros::NodeHandle getConfigNodeHandle(const ros::NodeHandle &nh=ros::NodeHandle("~"))
Retrieve NodeHandle/namespace defining the PlanningPipeline parameters Traditionally, these were directly defined in the private namespace of the move_group node. Since MoveIt 1.1.2 multiple pipeline configs are supported in parallel. In Melodic we support this new scheme by allowing to choose the default pipeline, specified via the parameter ~default_planning_pipeline.
std::set< PlanningContext * > contexts_
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings.
void terminate() const
Request termination, if a solve() function is currently computing plans.
virtual PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr &planning_scene, const MotionPlanRequest &req, moveit_msgs::MoveItErrorCodes &error_code) const =0
Construct a planning context given the current scene and a planning request. If a problem is encounte...
virtual void getPlanningAlgorithms(std::vector< std::string > &algs) const
Get the names of the known planning algorithms (values that can be filled as planner_id in the planni...
planning_scene::PlanningSceneConstPtr planning_scene_
The planning scene for this context.
bool getParam(const std::string &key, std::string &s) const
virtual bool initialize(const robot_model::RobotModelConstPtr &model, const std::string &ns)
virtual std::string getDescription() const
Get.
void setPlanningScene(const planning_scene::PlanningSceneConstPtr &planning_scene)
Set the planning scene for this context.
virtual void setPlannerConfigurations(const PlannerConfigurationMap &pcs)
Specify the settings to be used for specific algorithms.
bool hasParam(const std::string &key) const
This namespace includes the central class for representing planning contexts.
moveit_msgs::MotionPlanRequest MotionPlanRequest
#define ROS_ERROR_NAMED(name,...)
PlanningContext(const std::string &name, const std::string &group)
Construct a planning context named name for the group group.
This namespace includes the base class for MoveIt! planners.
MotionPlanRequest request_
The planning request for this context.
boost::mutex mutex_


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Mar 17 2022 02:51:04