moveit_cpp.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, PickNik LLC
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 PickNik LLC 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: Henning Kayser */
36 
37 #include <stdexcept>
41 
42 #include <tf2/utils.h>
44 #include <ros/console.h>
45 #include <ros/ros.h>
46 
47 namespace moveit
48 {
49 namespace planning_interface
50 {
51 constexpr char LOGNAME[] = "moveit_cpp";
52 constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin";
53 
54 MoveItCpp::MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
55  : MoveItCpp(Options(nh), nh, tf_buffer)
56 {
57 }
58 
59 MoveItCpp::MoveItCpp(const Options& options, const ros::NodeHandle& nh,
60  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
61  : tf_buffer_(tf_buffer), node_handle_(nh)
62 {
63  if (!tf_buffer_)
64  tf_buffer_.reset(new tf2_ros::Buffer());
65  tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_));
66 
67  // Configure planning scene monitor
68  if (!loadPlanningSceneMonitor(options.planning_scene_monitor_options))
69  {
70  const std::string error = "Unable to configure planning scene monitor";
72  throw std::runtime_error(error);
73  }
74 
75  robot_model_ = planning_scene_monitor_->getRobotModel();
76  if (!robot_model_)
77  {
78  const std::string error = "Unable to construct robot model. Please make sure all needed information is on the "
79  "parameter server.";
81  throw std::runtime_error(error);
82  }
83 
84  bool load_planning_pipelines = true;
85  if (load_planning_pipelines && !loadPlanningPipelines(options.planning_pipeline_options))
86  {
87  const std::string error = "Failed to load planning pipelines from parameter server";
89  throw std::runtime_error(error);
90  }
91 
92  // TODO(henningkayser): configure trajectory execution manager
93  trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager(
94  robot_model_, planning_scene_monitor_->getStateMonitor()));
95 
96  ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp running");
97 }
98 
99 MoveItCpp::~MoveItCpp()
100 {
101  ROS_INFO_NAMED(LOGNAME, "Deleting MoveItCpp");
102  clearContents();
103 }
104 
105 bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options)
106 {
107  planning_scene_monitor_.reset(
108  new planning_scene_monitor::PlanningSceneMonitor(options.robot_description, tf_buffer_, options.name));
109  // Allows us to sycronize to Rviz and also publish collision objects to ourselves
110  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Configuring Planning Scene Monitor");
111  if (planning_scene_monitor_->getPlanningScene())
112  {
113  // Start state and scene monitors
114  ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for joint states", options.joint_state_topic.c_str());
115  planning_scene_monitor_->startStateMonitor(options.joint_state_topic, options.attached_collision_object_topic);
116  planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
117  options.publish_planning_scene_topic);
118  planning_scene_monitor_->startSceneMonitor(options.monitored_planning_scene_topic);
119  }
120  else
121  {
122  ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning scene not configured");
123  return false;
124  }
125 
126  // Wait for complete state to be recieved
127  if (options.wait_for_initial_state_timeout > 0.0)
128  {
129  return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(),
130  options.wait_for_initial_state_timeout);
131  }
132 
133  return true;
134 }
135 
136 bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options)
137 {
138  ros::NodeHandle node_handle(options.parent_namespace.empty() ? "~" : options.parent_namespace);
139  for (const auto& planning_pipeline_name : options.pipeline_names)
140  {
141  if (planning_pipelines_.count(planning_pipeline_name) > 0)
142  {
143  ROS_WARN_NAMED(LOGNAME, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str());
144  continue;
145  }
146  ROS_INFO_NAMED(LOGNAME, "Loading planning pipeline '%s'", planning_pipeline_name.c_str());
147  ros::NodeHandle child_nh(node_handle, planning_pipeline_name);
148  planning_pipeline::PlanningPipelinePtr pipeline;
149  pipeline.reset(new planning_pipeline::PlanningPipeline(robot_model_, child_nh, PLANNING_PLUGIN_PARAM));
150 
151  if (!pipeline->getPlannerManager())
152  {
153  ROS_ERROR_NAMED(LOGNAME, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str());
154  continue;
155  }
156  planning_pipelines_[planning_pipeline_name] = pipeline;
157  }
158 
159  if (planning_pipelines_.empty())
160  {
161  return false;
162  ROS_ERROR_NAMED(LOGNAME, "Failed to load any planning pipelines.");
163  }
164 
165  // Retrieve group/pipeline mapping for faster lookup
166  std::vector<std::string> group_names = robot_model_->getJointModelGroupNames();
167  for (const auto& pipeline_entry : planning_pipelines_)
168  {
169  for (const auto& group_name : group_names)
170  {
171  groups_pipelines_map_[group_name] = {};
172  const auto& pipeline = pipeline_entry.second;
173  for (const auto& planner_configuration : pipeline->getPlannerManager()->getPlannerConfigurations())
174  {
175  if (planner_configuration.second.group == group_name)
176  {
177  groups_pipelines_map_[group_name].insert(pipeline_entry.first);
178  }
179  }
180  }
181  }
182 
183  return true;
184 }
185 
186 moveit::core::RobotModelConstPtr MoveItCpp::getRobotModel() const
187 {
188  return robot_model_;
189 }
190 
191 const ros::NodeHandle& MoveItCpp::getNodeHandle() const
192 {
193  return node_handle_;
194 }
195 
196 bool MoveItCpp::getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds)
197 {
198  if (wait_seconds > 0.0 &&
199  !planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), wait_seconds))
200  {
201  ROS_ERROR_NAMED(LOGNAME, "Did not receive robot state");
202  return false;
203  }
204  { // Lock planning scene
205  planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor_);
206  current_state.reset(new moveit::core::RobotState(scene->getCurrentState()));
207  } // Unlock planning scene
208  return true;
209 }
210 
211 moveit::core::RobotStatePtr MoveItCpp::getCurrentState(double wait)
212 {
213  moveit::core::RobotStatePtr current_state;
214  getCurrentState(current_state, wait);
215  return current_state;
216 }
217 
218 const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& MoveItCpp::getPlanningPipelines() const
219 {
220  return planning_pipelines_;
221 }
222 
223 std::set<std::string> MoveItCpp::getPlanningPipelineNames(const std::string& group_name) const
224 {
225  std::set<std::string> result_names;
226  if (!group_name.empty() && groups_pipelines_map_.count(group_name) == 0)
227  {
229  "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.",
230  group_name.c_str());
231  return result_names; // empty
232  }
233  for (const auto& pipeline_entry : planning_pipelines_)
234  {
235  const std::string& pipeline_name = pipeline_entry.first;
236  // If group_name is defined and valid, skip pipelines that don't belong to the planning group
237  if (!group_name.empty())
238  {
239  const auto& group_pipelines = groups_pipelines_map_.at(group_name);
240  if (group_pipelines.find(pipeline_name) == group_pipelines.end())
241  continue;
242  }
243  result_names.insert(pipeline_name);
244  }
245  // No valid planning pipelines
246  if (result_names.empty())
248  "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.",
249  group_name.c_str());
250  return result_names;
251 }
252 
253 const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const
254 {
255  return planning_scene_monitor_;
256 }
257 
258 planning_scene_monitor::PlanningSceneMonitorPtr MoveItCpp::getPlanningSceneMonitorNonConst()
259 {
260  return planning_scene_monitor_;
261 }
262 
263 const trajectory_execution_manager::TrajectoryExecutionManagerPtr& MoveItCpp::getTrajectoryExecutionManager() const
264 {
265  return trajectory_execution_manager_;
266 }
267 
268 trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveItCpp::getTrajectoryExecutionManagerNonConst()
269 {
270  return trajectory_execution_manager_;
271 }
272 
273 bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
274  bool blocking)
275 {
276  if (!robot_trajectory)
277  {
278  ROS_ERROR_NAMED(LOGNAME, "Robot trajectory is undefined");
279  return false;
280  }
281 
282  // Check if there are controllers that can handle the execution
283  if (!trajectory_execution_manager_->ensureActiveControllersForGroup(group_name))
284  {
285  ROS_ERROR_NAMED(LOGNAME, "Execution failed! No active controllers configured for group '%s'", group_name.c_str());
286  return false;
287  }
288 
289  // Execute trajectory
290  moveit_msgs::RobotTrajectory robot_trajectory_msg;
291  robot_trajectory->getRobotTrajectoryMsg(robot_trajectory_msg);
292  if (blocking)
293  {
294  trajectory_execution_manager_->push(robot_trajectory_msg);
295  trajectory_execution_manager_->execute();
296  return trajectory_execution_manager_->waitForExecution();
297  }
298  trajectory_execution_manager_->pushAndExecute(robot_trajectory_msg);
299  return true;
300 }
301 
302 const std::shared_ptr<tf2_ros::Buffer>& MoveItCpp::getTFBuffer() const
303 {
304  return tf_buffer_;
305 }
306 
307 void MoveItCpp::clearContents()
308 {
309  tf_listener_.reset();
310  tf_buffer_.reset();
311  planning_scene_monitor_.reset();
312  robot_model_.reset();
313  planning_pipelines_.clear();
314 }
315 } // namespace planning_interface
316 } // namespace moveit
utils.h
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
ros.h
moveit::planning_interface::LOGNAME
const std::string LOGNAME
Definition: move_group_interface.cpp:148
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
planning_scene_monitor::PlanningSceneMonitor
tf2_ros::TransformListener
moveit::planning_interface::PLANNING_PLUGIN_PARAM
constexpr char PLANNING_PLUGIN_PARAM[]
Definition: moveit_cpp.cpp:116
console.h
robot_trajectory
trajectory_execution_manager::TrajectoryExecutionManager
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
ROS_FATAL_STREAM_NAMED
#define ROS_FATAL_STREAM_NAMED(name, args)
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
tf2_ros::Buffer
moveit_cpp.h
common_objects.h
transform_listener.h
moveit
current_state_monitor.h
tf_buffer
tf2_ros::Buffer * tf_buffer
planning_scene_monitor::LockedPlanningSceneRO
planning_interface
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE
UPDATE_SCENE
moveit::planning_interface::MoveItCpp::MoveItCpp
MoveItCpp(const ros::NodeHandle &nh, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer={})
Constructor.
Definition: moveit_cpp.cpp:118
planning_pipeline::PlanningPipeline
ros::NodeHandle
ros::Time::now
static Time now()


planning_interface
Author(s): Ioan Sucan
autogenerated on Sat Dec 12 2020 03:27:08