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


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:17:33