move_group_interface.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, SRI International
5  * Copyright (c) 2012, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Ioan Sucan, Sachin Chitta */
37 
38 #ifndef MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_INTERFACE_
39 #define MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_INTERFACE_
40 
44 #include <moveit_msgs/RobotTrajectory.h>
45 #include <moveit_msgs/RobotState.h>
46 #include <moveit_msgs/PlannerInterfaceDescription.h>
47 #include <moveit_msgs/Constraints.h>
48 #include <moveit_msgs/Grasp.h>
49 #include <moveit_msgs/PlaceLocation.h>
50 #include <moveit_msgs/MotionPlanRequest.h>
51 #include <moveit_msgs/MoveGroupAction.h>
52 #include <geometry_msgs/PoseStamped.h>
54 #include <memory>
55 #include <tf2_ros/buffer.h>
56 
57 namespace moveit
58 {
60 namespace planning_interface
61 {
62 class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes
63 {
64 public:
66  {
67  val = 0;
68  }
69  MoveItErrorCode(int code)
70  {
71  val = code;
72  }
73  MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code)
74  {
75  val = code.val;
76  }
77  explicit operator bool() const
78  {
79  return val == moveit_msgs::MoveItErrorCodes::SUCCESS;
80  }
81  bool operator==(const int c) const
82  {
83  return val == c;
84  }
85  bool operator!=(const int c) const
86  {
87  return val != c;
88  }
89 };
90 
92 
99 {
100 public:
102  static const std::string ROBOT_DESCRIPTION;
103 
105  struct Options
106  {
107  Options(const std::string& group_name, const std::string& desc = ROBOT_DESCRIPTION,
108  const ros::NodeHandle& node_handle = ros::NodeHandle())
109  : group_name_(group_name), robot_description_(desc), node_handle_(node_handle)
110  {
111  }
112 
114  std::string group_name_;
115 
117  std::string robot_description_;
118 
120  robot_model::RobotModelConstPtr robot_model_;
121 
123  };
124 
126 
128  struct Plan
129  {
131  moveit_msgs::RobotState start_state_;
132 
134  moveit_msgs::RobotTrajectory trajectory_;
135 
138  };
139 
150  MoveGroupInterface(const Options& opt,
151  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
152  const ros::WallDuration& wait_for_servers = ros::WallDuration());
153  MOVEIT_DEPRECATED MoveGroupInterface(const Options& opt, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
154  const ros::Duration& wait_for_servers);
155 
163  MoveGroupInterface(const std::string& group,
164  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
165  const ros::WallDuration& wait_for_servers = ros::WallDuration());
166  MOVEIT_DEPRECATED MoveGroupInterface(const std::string& group, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
167  const ros::Duration& wait_for_servers);
168 
170 
176  MoveGroupInterface(const MoveGroupInterface&) = delete;
177  MoveGroupInterface& operator=(const MoveGroupInterface&) = delete;
178 
180  MoveGroupInterface& operator=(MoveGroupInterface&& other);
181 
183  const std::string& getName() const;
184 
187  const std::vector<std::string> getNamedTargets();
188 
190  robot_model::RobotModelConstPtr getRobotModel() const;
191 
193  const ros::NodeHandle& getNodeHandle() const;
194 
196  const std::string& getPlanningFrame() const;
197 
199  const std::vector<std::string>& getJointModelGroupNames() const;
200 
202  const std::vector<std::string>& getJointNames();
203 
205  const std::vector<std::string>& getLinkNames();
206 
208  std::map<std::string, double> getNamedTargetValues(const std::string& name);
209 
211  const std::vector<std::string>& getActiveJoints() const;
212 
214  const std::vector<std::string>& getJoints() const;
215 
218  unsigned int getVariableCount() const;
219 
221  bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc);
222 
224  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id, const std::string& group = "");
225 
227  void setPlannerParams(const std::string& planner_id, const std::string& group,
228  const std::map<std::string, std::string>& params, bool bReplace = false);
229 
231  std::string getDefaultPlannerId(const std::string& group = "") const;
232 
234  void setPlannerId(const std::string& planner_id);
235 
237  const std::string& getPlannerId() const;
238 
240  void setPlanningTime(double seconds);
241 
244  void setNumPlanningAttempts(unsigned int num_planning_attempts);
245 
251  void setMaxVelocityScalingFactor(double max_velocity_scaling_factor);
252 
258  void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor);
259 
261  double getPlanningTime() const;
262 
265  double getGoalJointTolerance() const;
266 
269  double getGoalPositionTolerance() const;
270 
273  double getGoalOrientationTolerance() const;
274 
281  void setGoalTolerance(double tolerance);
282 
285  void setGoalJointTolerance(double tolerance);
286 
288  void setGoalPositionTolerance(double tolerance);
289 
291  void setGoalOrientationTolerance(double tolerance);
292 
297  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz);
298 
301  void setStartState(const moveit_msgs::RobotState& start_state);
302 
305  void setStartState(const robot_state::RobotState& start_state);
306 
308  void setStartStateToCurrentState();
309 
312  void setSupportSurfaceName(const std::string& name);
313 
344  bool setJointValueTarget(const std::vector<double>& group_variable_values);
345 
361  bool setJointValueTarget(const std::map<std::string, double>& variable_values);
362 
372  bool setJointValueTarget(const robot_state::RobotState& robot_state);
373 
385  bool setJointValueTarget(const std::string& joint_name, const std::vector<double>& values);
386 
398  bool setJointValueTarget(const std::string& joint_name, double value);
399 
410  bool setJointValueTarget(const sensor_msgs::JointState& state);
411 
423  bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = "");
424 
436  bool setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link = "");
437 
449  bool setJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = "");
450 
461  bool setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = "");
462 
473  bool setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
474  const std::string& end_effector_link = "");
475 
486  bool setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = "");
487 
492  void setRandomTarget();
493 
496  bool setNamedTarget(const std::string& name);
497 
499  const robot_state::RobotState& getJointValueTarget() const;
500 
522  bool setPositionTarget(double x, double y, double z, const std::string& end_effector_link = "");
523 
531  bool setRPYTarget(double roll, double pitch, double yaw, const std::string& end_effector_link = "");
532 
541  bool setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link = "");
542 
550  bool setPoseTarget(const Eigen::Isometry3d& end_effector_pose, const std::string& end_effector_link = "");
551 
559  bool setPoseTarget(const geometry_msgs::Pose& target, const std::string& end_effector_link = "");
560 
568  bool setPoseTarget(const geometry_msgs::PoseStamped& target, const std::string& end_effector_link = "");
569 
588  bool setPoseTargets(const EigenSTL::vector_Isometry3d& end_effector_pose, const std::string& end_effector_link = "");
589 
608  bool setPoseTargets(const std::vector<geometry_msgs::Pose>& target, const std::string& end_effector_link = "");
609 
628  bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link = "");
629 
631  void setPoseReferenceFrame(const std::string& pose_reference_frame);
632 
636  bool setEndEffectorLink(const std::string& end_effector_link);
637 
640  bool setEndEffector(const std::string& eef_name);
641 
643  void clearPoseTarget(const std::string& end_effector_link = "");
644 
646  void clearPoseTargets();
647 
654  const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link = "") const;
655 
661  const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link = "") const;
662 
668  const std::string& getEndEffectorLink() const;
669 
675  const std::string& getEndEffector() const;
676 
679  const std::string& getPoseReferenceFrame() const;
680 
691  MoveItErrorCode asyncMove();
692 
697 
702  MoveItErrorCode move();
703 
707  MoveItErrorCode plan(Plan& plan);
708 
710  MoveItErrorCode asyncExecute(const Plan& plan);
711 
713  MoveItErrorCode execute(const Plan& plan);
714 
724  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
725  moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true,
726  moveit_msgs::MoveItErrorCodes* error_code = NULL);
727 
740  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
741  moveit_msgs::RobotTrajectory& trajectory,
742  const moveit_msgs::Constraints& path_constraints, bool avoid_collisions = true,
743  moveit_msgs::MoveItErrorCodes* error_code = NULL);
744 
746  void stop();
747 
750  void allowLooking(bool flag);
751 
753  void allowReplanning(bool flag);
754 
757  void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request);
758 
769  MoveItErrorCode pick(const std::string& object, bool plan_only = false);
770 
772  MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only = false);
773 
777  MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps,
778  bool plan_only = false);
779 
783  MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false);
784 
788  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false);
789 
791  MoveItErrorCode place(const std::string& object, bool plan_only = false);
792 
794  MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations,
795  bool plan_only = false);
796 
798  MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses,
799  bool plan_only = false);
800 
802  MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only = false);
803 
810  bool attachObject(const std::string& object, const std::string& link = "");
811 
820  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links);
821 
827  bool detachObject(const std::string& name = "");
828 
841  bool startStateMonitor(double wait = 1.0);
842 
844  std::vector<double> getCurrentJointValues();
845 
847  robot_state::RobotStatePtr getCurrentState(double wait = 1);
848 
852  geometry_msgs::PoseStamped getCurrentPose(const std::string& end_effector_link = "");
853 
857  std::vector<double> getCurrentRPY(const std::string& end_effector_link = "");
858 
860  std::vector<double> getRandomJointValues();
861 
865  geometry_msgs::PoseStamped getRandomPose(const std::string& end_effector_link = "");
866 
878  void rememberJointValues(const std::string& name);
879 
884  void rememberJointValues(const std::string& name, const std::vector<double>& values);
885 
887  const std::map<std::string, std::vector<double> >& getRememberedJointValues() const
888  {
889  return remembered_joint_values_;
890  }
891 
893  void forgetJointValues(const std::string& name);
894 
903  void setConstraintsDatabase(const std::string& host, unsigned int port);
904 
906  std::vector<std::string> getKnownConstraints() const;
907 
911  moveit_msgs::Constraints getPathConstraints() const;
912 
916  bool setPathConstraints(const std::string& constraint);
917 
921  void setPathConstraints(const moveit_msgs::Constraints& constraint);
922 
925  void clearPathConstraints();
926 
927  moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const;
928  void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint);
929  void clearTrajectoryConstraints();
930 
933 private:
934  std::map<std::string, std::vector<double> > remembered_joint_values_;
937 };
938 }
939 }
940 
941 #endif
MoveItErrorCode(const moveit_msgs::MoveItErrorCodes &code)
moveit_msgs::RobotTrajectory trajectory_
The trajectory of the robot (may not contain joints that are the same as for the start_state_) ...
static const std::string ROBOT_DESCRIPTION
Default ROS parameter name from where to read the robot&#39;s URDF. Set to &#39;robot_description&#39;.
desc
void attachObject(void)
Definition: demo.cpp:104
std::map< std::string, std::vector< double > > remembered_joint_values_
#define MOVEIT_DEPRECATED
std::string getName(void *handle)
Specification of options to use when constructing the MoveGroupInterface class.
std::string robot_description_
The robot description parameter name (if different from default)
MOVEIT_STRUCT_FORWARD(CollisionGeometryData)
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
Options(const std::string &group_name, const std::string &desc=ROBOT_DESCRIPTION, const ros::NodeHandle &node_handle=ros::NodeHandle())
Client class to conveniently use the ROS interfaces provided by the move_group node.
robot_model::RobotModelConstPtr robot_model_
Optionally, an instance of the RobotModel to use can be also specified.
moveit_msgs::RobotState start_state_
The full starting state used for planning.
const std::map< std::string, std::vector< double > > & getRememberedJointValues() const
Get the currently remembered map of names to joint values.
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
std::string group_name_
The group to construct the class instance for.
double planning_time_
The amount of time it took to generate the plan.
The representation of a motion plan (as ROS messasges)
MOVEIT_CLASS_FORWARD(MoveGroupInterface)


planning_interface
Author(s): Ioan Sucan
autogenerated on Tue Dec 11 2018 03:55:49