moveit_cpp.h
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  Desc: A high level interface that does not require the use of ROS Actions, Services, and Messages to access the
37  core MoveIt functionality
38 */
39 
40 #pragma once
41 
46 #include <tf2_ros/buffer.h>
48 
49 namespace moveit
50 {
51 namespace planning_interface
52 {
53 MOVEIT_CLASS_FORWARD(MoveItCpp); // Defines MoveItCppPtr, ConstPtr, WeakPtr... etc
54 
55 class MoveItCpp
56 {
57 public:
59  struct PlanningSceneMonitorOptions
60  {
61  void load(const ros::NodeHandle& nh)
62  {
63  std::string ns = "planning_scene_monitor_options/";
64  nh.param<std::string>(ns + "name", name, "planning_scene_monitor");
65  nh.param<std::string>(ns + "robot_description", robot_description, "robot_description");
66  nh.param(ns + "joint_state_topic", joint_state_topic,
68  nh.param(ns + "attached_collision_object_topic", attached_collision_object_topic,
70  nh.param(ns + "monitored_planning_scene_topic", monitored_planning_scene_topic,
72  nh.param(ns + "publish_planning_scene_topic", publish_planning_scene_topic,
74  nh.param<double>(ns + "wait_for_initial_state_timeout", wait_for_initial_state_timeout, 0.0);
75  }
76  std::string name;
77  std::string robot_description;
78  std::string joint_state_topic;
81  std::string publish_planning_scene_topic;
83  };
84 
86  struct PlanningPipelineOptions
87  {
88  void load(const ros::NodeHandle& nh)
89  {
90  std::string ns = "planning_pipelines/";
91  nh.getParam(ns + "pipeline_names", pipeline_names);
92  nh.getParam(ns + "namespace", parent_namespace);
93  }
94  std::vector<std::string> pipeline_names;
95  std::string parent_namespace;
96  };
97 
99  struct Options
100  {
101  Options(const ros::NodeHandle& nh)
102  {
105  }
106 
107  PlanningSceneMonitorOptions planning_scene_monitor_options;
108  PlanningPipelineOptions planning_pipeline_options;
109  };
110 
112  MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = {});
113  MoveItCpp(const Options& options, const ros::NodeHandle& nh, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = {});
114 
120  MoveItCpp(const MoveItCpp&) = delete;
121  MoveItCpp& operator=(const MoveItCpp&) = delete;
122 
123  MoveItCpp(MoveItCpp&& other) = default;
124  MoveItCpp& operator=(MoveItCpp&& other) = default;
125 
127  ~MoveItCpp();
128 
130  moveit::core::RobotModelConstPtr getRobotModel() const;
131 
133  const ros::NodeHandle& getNodeHandle() const;
134 
137  bool getCurrentState(moveit::core::RobotStatePtr& current_state, double wait_seconds);
138 
141  moveit::core::RobotStatePtr getCurrentState(double wait_seconds = 0.0);
142 
144  const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& getPlanningPipelines() const;
145 
148  std::set<std::string> getPlanningPipelineNames(const std::string& group_name = "") const;
149 
151  const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const;
152  planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst();
153 
154  const std::shared_ptr<tf2_ros::Buffer>& getTFBuffer() const;
155 
157  const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const;
158  trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst();
159 
162  bool execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
163  bool blocking = true);
164 
165 protected:
166  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
167  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
168 
169 private:
170  // Core properties and instances
172  moveit::core::RobotModelConstPtr robot_model_;
173  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
174 
175  // Planning
176  std::map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
177  std::map<std::string, std::set<std::string>> groups_pipelines_map_;
178  std::map<std::string, std::set<std::string>> groups_algorithms_map_;
179 
180  // Execution
181  trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;
182 
184  void clearContents();
185 
188 
190  bool loadPlanningPipelines(const PlanningPipelineOptions& options);
191 };
192 } // namespace planning_interface
193 } // namespace moveit
moveit::planning_interface::MoveItCpp::PlanningSceneMonitorOptions::load
void load(const ros::NodeHandle &nh)
Definition: moveit_cpp.h:125
moveit::planning_interface::MoveItCpp::loadPlanningPipelines
bool loadPlanningPipelines(const PlanningPipelineOptions &options)
Initialize and setup the planning pipelines.
Definition: moveit_cpp.cpp:200
moveit::planning_interface::MoveItCpp::planning_scene_monitor_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
Definition: moveit_cpp.h:237
moveit::planning_interface::MoveItCpp::getPlanningPipelines
const std::map< std::string, planning_pipeline::PlanningPipelinePtr > & getPlanningPipelines() const
Get all loaded planning pipeline instances mapped to their reference names.
Definition: moveit_cpp.cpp:282
moveit::planning_interface::MoveItCpp::getPlanningPipelineNames
std::set< std::string > getPlanningPipelineNames(const std::string &group_name="") const
Get the names of all loaded planning pipelines. Specify group_name to filter the results by planning ...
Definition: moveit_cpp.cpp:287
moveit::planning_interface::MoveItCpp::getTFBuffer
const std::shared_ptr< tf2_ros::Buffer > & getTFBuffer() const
Definition: moveit_cpp.cpp:366
moveit::planning_interface::MoveItCpp::getNodeHandle
const ros::NodeHandle & getNodeHandle() const
Get the ROS node handle of this instance operates on.
Definition: moveit_cpp.cpp:255
moveit::planning_interface::MoveItCpp::groups_algorithms_map_
std::map< std::string, std::set< std::string > > groups_algorithms_map_
Definition: moveit_cpp.h:242
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
moveit::planning_interface::MoveItCpp::trajectory_execution_manager_
trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_
Definition: moveit_cpp.h:245
moveit::planning_interface::MoveItCpp::getCurrentState
bool getCurrentState(moveit::core::RobotStatePtr &current_state, double wait_seconds)
Get the current state queried from the current state monitor.
Definition: moveit_cpp.cpp:260
moveit::planning_interface::MoveItCpp::PlanningSceneMonitorOptions::wait_for_initial_state_timeout
double wait_for_initial_state_timeout
Definition: moveit_cpp.h:146
buffer.h
moveit::planning_interface::MoveItCpp::PlanningSceneMonitorOptions::joint_state_topic
std::string joint_state_topic
Definition: moveit_cpp.h:142
moveit::planning_interface::MoveItCpp::PlanningSceneMonitorOptions::publish_planning_scene_topic
std::string publish_planning_scene_topic
Definition: moveit_cpp.h:145
moveit::planning_interface::MoveItCpp
Definition: moveit_cpp.h:119
robot_state.h
moveit::planning_interface::MoveItCpp::clearContents
void clearContents()
Reset all member variables.
Definition: moveit_cpp.cpp:371
moveit::planning_interface::MoveItCpp::PlanningPipelineOptions
struct contains the the variables used for loading the planning pipeline
Definition: moveit_cpp.h:150
moveit::planning_interface::MoveItCpp::Options::Options
Options(const ros::NodeHandle &nh)
Definition: moveit_cpp.h:165
moveit::planning_interface::MoveItCpp::~MoveItCpp
~MoveItCpp()
Destructor.
Definition: moveit_cpp.cpp:163
moveit::planning_interface::MoveItCpp::Options::planning_pipeline_options
PlanningPipelineOptions planning_pipeline_options
Definition: moveit_cpp.h:172
moveit::planning_interface::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(MoveGroupInterface)
moveit::planning_interface::MoveItCpp::tf_listener_
std::shared_ptr< tf2_ros::TransformListener > tf_listener_
Definition: moveit_cpp.h:231
robot_trajectory
moveit::planning_interface::MoveItCpp::loadPlanningSceneMonitor
bool loadPlanningSceneMonitor(const PlanningSceneMonitorOptions &options)
Initialize and setup the planning scene monitor.
Definition: moveit_cpp.cpp:169
moveit::planning_interface::MoveItCpp::PlanningPipelineOptions::pipeline_names
std::vector< std::string > pipeline_names
Definition: moveit_cpp.h:158
planning_scene_monitor.h
trajectory_execution_manager.h
moveit::planning_interface::MoveItCpp::execute
bool execute(const std::string &group_name, const robot_trajectory::RobotTrajectoryPtr &robot_trajectory, bool blocking=true)
Execute a trajectory on the planning group specified by group_name using the trajectory execution man...
Definition: moveit_cpp.cpp:337
moveit::planning_interface::MoveItCpp::groups_pipelines_map_
std::map< std::string, std::set< std::string > > groups_pipelines_map_
Definition: moveit_cpp.h:241
moveit::planning_interface::MoveItCpp::getTrajectoryExecutionManagerNonConst
trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst()
Definition: moveit_cpp.cpp:332
moveit::planning_interface::MoveItCpp::Options::planning_scene_monitor_options
PlanningSceneMonitorOptions planning_scene_monitor_options
Definition: moveit_cpp.h:171
planning_pipeline.h
moveit::planning_interface::MoveItCpp::getPlanningSceneMonitor
const planning_scene_monitor::PlanningSceneMonitorPtr & getPlanningSceneMonitor() const
Get the stored instance of the planning scene monitor.
Definition: moveit_cpp.cpp:317
moveit::planning_interface::MoveItCpp::PlanningSceneMonitorOptions::robot_description
std::string robot_description
Definition: moveit_cpp.h:141
transform_listener.h
moveit::planning_interface::MoveItCpp::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: moveit_cpp.h:236
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC
static const std::string DEFAULT_JOINT_STATES_TOPIC
moveit::planning_interface::MoveItCpp::tf_buffer_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: moveit_cpp.h:230
moveit::planning_interface::MoveItCpp::getRobotModel
moveit::core::RobotModelConstPtr getRobotModel() const
Get the RobotModel object.
Definition: moveit_cpp.cpp:250
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC
static const std::string DEFAULT_PLANNING_SCENE_TOPIC
planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC
static const std::string MONITORED_PLANNING_SCENE_TOPIC
moveit
moveit::planning_interface::MoveItCpp::operator=
MoveItCpp & operator=(const MoveItCpp &)=delete
moveit::planning_interface::MoveItCpp::PlanningSceneMonitorOptions::name
std::string name
Definition: moveit_cpp.h:140
moveit::planning_interface::MoveItCpp::node_handle_
ros::NodeHandle node_handle_
Definition: moveit_cpp.h:235
moveit::planning_interface::MoveItCpp::PlanningSceneMonitorOptions
Specification of options to use when constructing the MoveItCpp class.
Definition: moveit_cpp.h:123
planning_interface
moveit::planning_interface::MoveItCpp::getPlanningSceneMonitorNonConst
planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst()
Definition: moveit_cpp.cpp:322
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
moveit::planning_interface::MoveItCpp::planning_pipelines_
std::map< std::string, planning_pipeline::PlanningPipelinePtr > planning_pipelines_
Definition: moveit_cpp.h:240
moveit::planning_interface::MoveItCpp::getTrajectoryExecutionManager
const trajectory_execution_manager::TrajectoryExecutionManagerPtr & getTrajectoryExecutionManager() const
Get the stored instance of the trajectory execution manager.
Definition: moveit_cpp.cpp:327
moveit::planning_interface::MoveItCpp::PlanningSceneMonitorOptions::monitored_planning_scene_topic
std::string monitored_planning_scene_topic
Definition: moveit_cpp.h:144
moveit::planning_interface::MoveItCpp::PlanningSceneMonitorOptions::attached_collision_object_topic
std::string attached_collision_object_topic
Definition: moveit_cpp.h:143
moveit::planning_interface::MoveItCpp::PlanningPipelineOptions::parent_namespace
std::string parent_namespace
Definition: moveit_cpp.h:159
moveit::planning_interface::MoveItCpp::PlanningPipelineOptions::load
void load(const ros::NodeHandle &nh)
Definition: moveit_cpp.h:152
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
ros::NodeHandle
planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC


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