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
42 {
43 // keep track of currently active contexts
44 struct ActiveContexts
45 {
46  boost::mutex mutex_;
47  std::set<planning_interface::PlanningContext*> contexts_;
48 };
49 
50 static ActiveContexts& getActiveContexts()
51 {
52  static ActiveContexts ac;
53  return ac;
54 }
55 }
56 
57 planning_interface::PlanningContext::PlanningContext(const std::string& name, const std::string& group)
58  : name_(name), group_(group)
59 {
60  ActiveContexts& ac = getActiveContexts();
61  boost::mutex::scoped_lock _(ac.mutex_);
62  ac.contexts_.insert(this);
63 }
64 
66 {
67  ActiveContexts& ac = getActiveContexts();
68  boost::mutex::scoped_lock _(ac.mutex_);
69  ac.contexts_.erase(this);
70 }
71 
72 void planning_interface::PlanningContext::setPlanningScene(const planning_scene::PlanningSceneConstPtr& planning_scene)
73 {
74  planning_scene_ = planning_scene;
75 }
76 
78 {
79  request_ = request;
80  if (request_.allowed_planning_time <= 0.0)
81  {
82  CONSOLE_BRIDGE_logInform("The timeout for planning must be positive (%lf specified). Assuming one second instead.",
83  request_.allowed_planning_time);
84  request_.allowed_planning_time = 1.0;
85  }
86  if (request_.num_planning_attempts < 0)
87  CONSOLE_BRIDGE_logError("The number of desired planning attempts should be positive. Assuming one attempt.");
88  request_.num_planning_attempts = std::max(1, request_.num_planning_attempts);
89 }
90 
91 bool planning_interface::PlannerManager::initialize(const robot_model::RobotModelConstPtr&, const std::string&)
92 {
93  return true;
94 }
95 
97 {
98  return "";
99 }
100 
101 planning_interface::PlanningContextPtr planning_interface::PlannerManager::getPlanningContext(
102  const planning_scene::PlanningSceneConstPtr& planning_scene, const MotionPlanRequest& req) const
103 {
104  moveit_msgs::MoveItErrorCodes dummy;
105  return getPlanningContext(planning_scene, req, dummy);
106 }
107 
108 void planning_interface::PlannerManager::getPlanningAlgorithms(std::vector<std::string>& algs) const
109 {
110  // nothing by default
111  algs.clear();
112 }
113 
115 {
116  config_settings_ = pcs;
117 }
118 
120 {
121  ActiveContexts& ac = getActiveContexts();
122  boost::mutex::scoped_lock _(ac.mutex_);
123  for (std::set<PlanningContext*>::iterator it = ac.contexts_.begin(); it != ac.contexts_.end(); ++it)
124  (*it)->terminate();
125 }
void setMotionPlanRequest(const MotionPlanRequest &request)
Set the planning request for this context.
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...
#define CONSOLE_BRIDGE_logError(fmt,...)
virtual std::string getDescription() const
Get.
PlanningContext(const std::string &name, const std::string &group)
Construct a planning context named name for the group group.
void terminate() const
Request termination, if a solve() function is currently computing plans.
MotionPlanRequest request_
The planning request for this context.
#define CONSOLE_BRIDGE_logInform(fmt,...)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Apr 21 2018 02:54:51