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 <geometry_msgs/PoseStamped.h>
52 #include <boost/shared_ptr.hpp>
53 #include <tf/tf.h>
54 
55 namespace moveit
56 {
58 namespace planning_interface
59 {
60 class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes
61 {
62 public:
64  {
65  val = 0;
66  }
67  MoveItErrorCode(int code)
68  {
69  val = code;
70  }
71  MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code)
72  {
73  val = code.val;
74  }
75  explicit operator bool() const
76  {
77  return val == moveit_msgs::MoveItErrorCodes::SUCCESS;
78  }
79  bool operator==(const int c) const
80  {
81  return val == c;
82  }
83  bool operator!=(const int c) const
84  {
85  return val != c;
86  }
87 };
88 
90 
97 {
98 public:
100  static const std::string ROBOT_DESCRIPTION;
101 
103  struct Options
104  {
105  Options(const std::string& group_name, const std::string& desc = ROBOT_DESCRIPTION,
106  const ros::NodeHandle& node_handle = ros::NodeHandle())
107  : group_name_(group_name), robot_description_(desc), node_handle_(node_handle)
108  {
109  }
110 
112  std::string group_name_;
113 
115  std::string robot_description_;
116 
118  robot_model::RobotModelConstPtr robot_model_;
119 
121  };
122 
124 
126  struct Plan
127  {
129  moveit_msgs::RobotState start_state_;
130 
132  moveit_msgs::RobotTrajectory trajectory_;
133 
136  };
137 
147  MoveGroupInterface(const Options& opt,
149  const ros::WallDuration& wait_for_servers = ros::WallDuration());
151  const ros::Duration& wait_for_servers);
152 
159  MoveGroupInterface(const std::string& group,
161  const ros::WallDuration& wait_for_servers = ros::WallDuration());
163  const ros::Duration& wait_for_servers);
164 
166 
172  MoveGroupInterface(const MoveGroupInterface&) = delete;
173  MoveGroupInterface& operator=(const MoveGroupInterface&) = delete;
174 
176  MoveGroupInterface& operator=(MoveGroupInterface&& other);
177 
179  const std::string& getName() const;
180 
183  const std::vector<std::string> getNamedTargets();
184 
186  robot_model::RobotModelConstPtr getRobotModel() const;
187 
189  const ros::NodeHandle& getNodeHandle() const;
190 
192  const std::string& getPlanningFrame() const;
193 
195  const std::vector<std::string>& getJointNames();
196 
198  const std::vector<std::string>& getLinkNames();
199 
201  std::map<std::string, double> getNamedTargetValues(const std::string& name);
202 
204  const std::vector<std::string>& getActiveJoints() const;
205 
207  const std::vector<std::string>& getJoints() const;
208 
211  unsigned int getVariableCount() const;
212 
214  bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc);
215 
217  std::map<std::string, std::string> getPlannerParams(const std::string& planner_id, const std::string& group = "");
218 
220  void setPlannerParams(const std::string& planner_id, const std::string& group,
221  const std::map<std::string, std::string>& params, bool bReplace = false);
222 
224  std::string getDefaultPlannerId(const std::string& group = "") const;
225 
227  void setPlannerId(const std::string& planner_id);
228 
230  const std::string& getPlannerId() const;
231 
233  void setPlanningTime(double seconds);
234 
237  void setNumPlanningAttempts(unsigned int num_planning_attempts);
238 
244  void setMaxVelocityScalingFactor(double max_velocity_scaling_factor);
245 
251  void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor);
252 
254  double getPlanningTime() const;
255 
258  double getGoalJointTolerance() const;
259 
262  double getGoalPositionTolerance() const;
263 
266  double getGoalOrientationTolerance() const;
267 
274  void setGoalTolerance(double tolerance);
275 
278  void setGoalJointTolerance(double tolerance);
279 
281  void setGoalPositionTolerance(double tolerance);
282 
284  void setGoalOrientationTolerance(double tolerance);
285 
290  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz);
291 
294  void setStartState(const moveit_msgs::RobotState& start_state);
295 
298  void setStartState(const robot_state::RobotState& start_state);
299 
301  void setStartStateToCurrentState();
302 
305  void setSupportSurfaceName(const std::string& name);
306 
337  bool setJointValueTarget(const std::vector<double>& group_variable_values);
338 
354  bool setJointValueTarget(const std::map<std::string, double>& variable_values);
355 
365  bool setJointValueTarget(const robot_state::RobotState& robot_state);
366 
378  bool setJointValueTarget(const std::string& joint_name, const std::vector<double>& values);
379 
391  bool setJointValueTarget(const std::string& joint_name, double value);
392 
403  bool setJointValueTarget(const sensor_msgs::JointState& state);
404 
416  bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = "");
417 
429  bool setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link = "");
430 
442  bool setJointValueTarget(const Eigen::Affine3d& eef_pose, const std::string& end_effector_link = "");
443 
454  bool setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = "");
455 
466  bool setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose,
467  const std::string& end_effector_link = "");
468 
479  bool setApproximateJointValueTarget(const Eigen::Affine3d& eef_pose, const std::string& end_effector_link = "");
480 
485  void setRandomTarget();
486 
489  bool setNamedTarget(const std::string& name);
490 
492  const robot_state::RobotState& getJointValueTarget() const;
493 
515  bool setPositionTarget(double x, double y, double z, const std::string& end_effector_link = "");
516 
524  bool setRPYTarget(double roll, double pitch, double yaw, const std::string& end_effector_link = "");
525 
534  bool setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link = "");
535 
543  bool setPoseTarget(const Eigen::Affine3d& end_effector_pose, const std::string& end_effector_link = "");
544 
552  bool setPoseTarget(const geometry_msgs::Pose& target, const std::string& end_effector_link = "");
553 
561  bool setPoseTarget(const geometry_msgs::PoseStamped& target, const std::string& end_effector_link = "");
562 
581  bool setPoseTargets(const EigenSTL::vector_Affine3d& end_effector_pose, const std::string& end_effector_link = "");
582 
601  bool setPoseTargets(const std::vector<geometry_msgs::Pose>& target, const std::string& end_effector_link = "");
602 
621  bool setPoseTargets(const std::vector<geometry_msgs::PoseStamped>& target, const std::string& end_effector_link = "");
622 
624  void setPoseReferenceFrame(const std::string& pose_reference_frame);
625 
629  bool setEndEffectorLink(const std::string& end_effector_link);
630 
633  bool setEndEffector(const std::string& eef_name);
634 
636  void clearPoseTarget(const std::string& end_effector_link = "");
637 
639  void clearPoseTargets();
640 
647  const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link = "") const;
648 
654  const std::vector<geometry_msgs::PoseStamped>& getPoseTargets(const std::string& end_effector_link = "") const;
655 
661  const std::string& getEndEffectorLink() const;
662 
668  const std::string& getEndEffector() const;
669 
672  const std::string& getPoseReferenceFrame() const;
673 
684  MoveItErrorCode asyncMove();
685 
690  MoveItErrorCode move();
691 
695  MoveItErrorCode plan(Plan& plan);
696 
698  MoveItErrorCode asyncExecute(const Plan& plan);
699 
701  MoveItErrorCode execute(const Plan& plan);
702 
712  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
713  moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true,
714  moveit_msgs::MoveItErrorCodes* error_code = NULL);
715 
728  double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints, double eef_step, double jump_threshold,
729  moveit_msgs::RobotTrajectory& trajectory,
730  const moveit_msgs::Constraints& path_constraints, bool avoid_collisions = true,
731  moveit_msgs::MoveItErrorCodes* error_code = NULL);
732 
734  void stop();
735 
738  void allowLooking(bool flag);
739 
741  void allowReplanning(bool flag);
742 
745  void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request);
746 
757  MoveItErrorCode pick(const std::string& object);
758 
760  MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp);
761 
765  MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps);
766 
770  MoveItErrorCode planGraspsAndPick(const std::string& object = "");
771 
775  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object);
776 
778  MoveItErrorCode place(const std::string& object);
779 
781  MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations);
782 
784  MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses);
785 
787  MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose);
788 
795  bool attachObject(const std::string& object, const std::string& link = "");
796 
805  bool attachObject(const std::string& object, const std::string& link, const std::vector<std::string>& touch_links);
806 
812  bool detachObject(const std::string& name = "");
813 
826  bool startStateMonitor(double wait = 1.0);
827 
829  std::vector<double> getCurrentJointValues();
830 
832  robot_state::RobotStatePtr getCurrentState(double wait = 1);
833 
837  geometry_msgs::PoseStamped getCurrentPose(const std::string& end_effector_link = "");
838 
842  std::vector<double> getCurrentRPY(const std::string& end_effector_link = "");
843 
845  std::vector<double> getRandomJointValues();
846 
850  geometry_msgs::PoseStamped getRandomPose(const std::string& end_effector_link = "");
851 
863  void rememberJointValues(const std::string& name);
864 
869  void rememberJointValues(const std::string& name, const std::vector<double>& values);
870 
872  const std::map<std::string, std::vector<double> >& getRememberedJointValues() const
873  {
874  return remembered_joint_values_;
875  }
876 
878  void forgetJointValues(const std::string& name);
879 
888  void setConstraintsDatabase(const std::string& host, unsigned int port);
889 
891  std::vector<std::string> getKnownConstraints() const;
892 
896  moveit_msgs::Constraints getPathConstraints() const;
897 
901  bool setPathConstraints(const std::string& constraint);
902 
906  void setPathConstraints(const moveit_msgs::Constraints& constraint);
907 
910  void clearPathConstraints();
911 
912  moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const;
913  void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint);
914  void clearTrajectoryConstraints();
915 
918 private:
919  std::map<std::string, std::vector<double> > remembered_joint_values_;
922 };
923 }
924 }
925 
926 #endif
MoveItErrorCode(const moveit_msgs::MoveItErrorCodes &code)
const std::map< std::string, std::vector< double > > & getRememberedJointValues() const
Get the currently remembered map of names to joint values.
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
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
moveit::planning_interface::MoveGroup MOVEIT_DEPRECATED
void attachObject(void)
Definition: demo.cpp:104
std::map< std::string, std::vector< double > > remembered_joint_values_
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)
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
#define MOVEIT_STRUCT_FORWARD(C)
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.
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 Fri Sep 21 2018 02:50:36