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


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Sep 18 2022 02:26:52