collision_plugin_loader.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014 Fetch Robotics 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 Fetch Robotics 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 
37 #include <memory>
38 
39 static const std::string LOGNAME = "collision_detection";
40 namespace collision_detection
41 {
43 {
44 public:
46  {
47  try
48  {
49  loader_.reset(new pluginlib::ClassLoader<CollisionPlugin>("moveit_core", "collision_detection::CollisionPlugin"));
50  }
52  {
53  ROS_ERROR("Unable to construct colllision plugin loader. Error: %s", e.what());
54  }
55  }
56 
57  CollisionPluginPtr load(const std::string& name)
58  {
59  CollisionPluginPtr plugin;
60  try
61  {
62  plugin = loader_->createUniqueInstance(name);
63  plugins_[name] = plugin;
64  }
66  {
67  ROS_ERROR_STREAM("Exception while loading " << name << ": " << ex.what());
68  }
69  return plugin;
70  }
71 
72  bool activate(const std::string& name, const planning_scene::PlanningScenePtr& scene, bool exclusive)
73  {
74  std::map<std::string, CollisionPluginPtr>::iterator it = plugins_.find(name);
75  if (it == plugins_.end())
76  {
77  CollisionPluginPtr plugin = load(name);
78  if (plugin)
79  {
80  return plugin->initialize(scene, exclusive);
81  }
82  return false;
83  }
84  if (it->second)
85  {
86  return it->second->initialize(scene, exclusive);
87  }
88  return false;
89  }
90 
91 private:
92  std::shared_ptr<pluginlib::ClassLoader<CollisionPlugin> > loader_;
93  std::map<std::string, CollisionPluginPtr> plugins_;
94 };
95 
97 {
99 }
100 
102 
103 bool CollisionPluginLoader::activate(const std::string& name, const planning_scene::PlanningScenePtr& scene,
104  bool exclusive)
105 {
106  return loader_->activate(name, scene, exclusive);
107 }
108 
109 void CollisionPluginLoader::setupScene(ros::NodeHandle& nh, const planning_scene::PlanningScenePtr& scene)
110 {
111  if (!scene)
112  {
113  ROS_WARN_NAMED(LOGNAME, "Cannot setup scene, PlanningScenePtr is null.");
114  return;
115  }
116 
117  std::string param_name;
118  std::string collision_detector_name;
119 
120  if (nh.searchParam("collision_detector", param_name))
121  {
122  nh.getParam(param_name, collision_detector_name);
123  }
124  else if (nh.hasParam("/move_group/collision_detector"))
125  {
126  // Check for existence in move_group namespace
127  // mainly for rviz plugins to get same collision detector.
128  nh.getParam("/move_group/collision_detector", collision_detector_name);
129  }
130  else
131  {
132  return;
133  }
134 
135  if (collision_detector_name.empty())
136  {
137  // This is not a valid name for a collision detector plugin
138  return;
139  }
140 
141  activate(collision_detector_name, scene, true);
142  ROS_INFO_STREAM("Using collision detector:" << scene->getActiveCollisionDetectorName().c_str());
143 }
144 
145 } // namespace collision_detection
std::shared_ptr< pluginlib::ClassLoader< CollisionPlugin > > loader_
#define ROS_WARN_NAMED(name,...)
static const std::string LOGNAME
bool activate(const std::string &name, const planning_scene::PlanningScenePtr &scene, bool exclusive)
Load a collision detection robot/world into a planning scene instance.
#define ROS_ERROR(...)
void setupScene(ros::NodeHandle &nh, const planning_scene::PlanningScenePtr &scene)
This can be called on a new planning scene to setup the collision detector.
constexpr char LOGNAME[]
bool getParam(const std::string &key, std::string &s) const
bool activate(const std::string &name, const planning_scene::PlanningScenePtr &scene, bool exclusive)
bool hasParam(const std::string &key) const
bool searchParam(const std::string &key, std::string &result) const
#define ROS_INFO_STREAM(args)
#define ROS_ERROR_STREAM(args)


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Oct 20 2021 02:52:05