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 }
58 
59 PlanningContext::PlanningContext(const std::string& name, const std::string& group) : name_(name), group_(group)
60 {
61  ActiveContexts& ac = getActiveContexts();
62  boost::mutex::scoped_lock _(ac.mutex_);
63  ac.contexts_.insert(this);
64 }
65 
67 {
68  ActiveContexts& ac = getActiveContexts();
69  boost::mutex::scoped_lock _(ac.mutex_);
70  ac.contexts_.erase(this);
71 }
72 
73 void PlanningContext::setPlanningScene(const planning_scene::PlanningSceneConstPtr& planning_scene)
74 {
75  planning_scene_ = planning_scene;
76 }
77 
79 {
80  request_ = request;
81  if (request_.allowed_planning_time <= 0.0)
82  {
83  ROS_INFO_NAMED("planning_interface",
84  "The timeout for planning must be positive (%lf specified). Assuming one second instead.",
85  request_.allowed_planning_time);
86  request_.allowed_planning_time = 1.0;
87  }
88  if (request_.num_planning_attempts < 0)
89  ROS_ERROR_NAMED("planning_interface", "The number of desired planning attempts should be positive. "
90  "Assuming one attempt.");
91  request_.num_planning_attempts = std::max(1, request_.num_planning_attempts);
92 }
93 
94 bool PlannerManager::initialize(const robot_model::RobotModelConstPtr& /*unused*/, const std::string& /*unused*/)
95 {
96  return true;
97 }
98 
99 std::string PlannerManager::getDescription() const
100 {
101  return "";
102 }
103 
104 PlanningContextPtr PlannerManager::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
105  const MotionPlanRequest& req) const
106 {
107  moveit_msgs::MoveItErrorCodes dummy;
108  return getPlanningContext(planning_scene, req, dummy);
109 }
110 
111 void PlannerManager::getPlanningAlgorithms(std::vector<std::string>& algs) const
112 {
113  // nothing by default
114  algs.clear();
115 }
116 
118 {
119  config_settings_ = pcs;
120 }
121 
123 {
124  ActiveContexts& ac = getActiveContexts();
125  boost::mutex::scoped_lock _(ac.mutex_);
126  for (std::set<PlanningContext*>::iterator it = ac.contexts_.begin(); it != ac.contexts_.end(); ++it)
127  (*it)->terminate();
128 }
129 
130 } // end of namespace planning_interface
void setMotionPlanRequest(const MotionPlanRequest &request)
Set the planning request for this context.
#define ROS_INFO_NAMED(name,...)
std::set< PlanningContext * > contexts_
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings.
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...
planning_scene::PlanningSceneConstPtr planning_scene_
The planning scene for this context.
virtual bool initialize(const robot_model::RobotModelConstPtr &model, const std::string &ns)
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.
This namespace includes the central class for representing planning contexts.
moveit_msgs::MotionPlanRequest MotionPlanRequest
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...
virtual std::string getDescription() const
Get.
#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.
void terminate() const
Request termination, if a solve() function is currently computing plans.
MotionPlanRequest request_
The planning request for this context.
boost::mutex mutex_


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05