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 = loader_->createUniqueInstance(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 bool CollisionPluginLoader::activate(const std::string& name, const planning_scene::PlanningScenePtr& scene,
103  bool exclusive)
104 {
105  return loader_->activate(name, scene, exclusive);
106 }
107 
108 void CollisionPluginLoader::setupScene(ros::NodeHandle& nh, const planning_scene::PlanningScenePtr& scene)
109 {
110  if (!scene)
111  return;
112 
113  std::string param_name;
114  std::string collision_detector_name;
115 
116  if (nh.searchParam("collision_detector", param_name))
117  {
118  nh.getParam(param_name, collision_detector_name);
119  }
120  else if (nh.hasParam("/move_group/collision_detector"))
121  {
122  // Check for existence in move_group namespace
123  // mainly for rviz plugins to get same collision detector.
124  nh.getParam("/move_group/collision_detector", collision_detector_name);
125  }
126  else
127  {
128  return;
129  }
130 
131  if (collision_detector_name.empty())
132  {
133  // This is not a valid name for a collision detector plugin
134  return;
135  }
136 
137  activate(collision_detector_name, scene, true);
138  ROS_INFO_STREAM("Using collision detector:" << scene->getActiveCollisionDetectorName().c_str());
139 }
140 
141 } // 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.
#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.
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 Mon Nov 23 2020 03:53:16