From 598a74aaef14a2f5027e3173953b75ea40c6c444 Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Tue, 30 Jul 2019 18:17:49 -0600 Subject: [PATCH 01/29] Copy made --- .../moveit_cpp/CMakeLists.txt | 26 + .../move_group_interface.h | 964 +++++++ .../moveit_cpp/src/demo.cpp | 121 + .../moveit_cpp/src/move_group_interface.cpp | 2245 +++++++++++++++++ .../moveit_cpp/src/wrap_python_move_group.cpp | 662 +++++ 5 files changed, 4018 insertions(+) create mode 100644 moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt create mode 100644 moveit_ros/planning_interface/moveit_cpp/include/moveit/move_group_interface/move_group_interface.h create mode 100644 moveit_ros/planning_interface/moveit_cpp/src/demo.cpp create mode 100644 moveit_ros/planning_interface/moveit_cpp/src/move_group_interface.cpp create mode 100644 moveit_ros/planning_interface/moveit_cpp/src/wrap_python_move_group.cpp diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt new file mode 100644 index 0000000000..eacfa2aecb --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -0,0 +1,26 @@ +set(MOVEIT_LIB_NAME moveit_move_group_interface) + +add_library(${MOVEIT_LIB_NAME} src/move_group_interface.cpp) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) + +add_library(${MOVEIT_LIB_NAME}_python src/wrap_python_move_group.cpp) +target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) +add_dependencies(${MOVEIT_LIB_NAME}_python ${catkin_EXPORTED_TARGETS}) +set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_move_group_interface PREFIX "") +set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") + +install(TARGETS ${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(TARGETS ${MOVEIT_LIB_NAME}_python + ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) + +install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + +add_executable(demo src/demo.cpp) +target_link_libraries(demo ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/move_group_interface/move_group_interface.h new file mode 100644 index 0000000000..9a8eb921e5 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/move_group_interface/move_group_interface.h @@ -0,0 +1,964 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, SRI International + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Ioan Sucan, Sachin Chitta */ + +#ifndef MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_INTERFACE_ +#define MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_INTERFACE_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +/** \brief Simple interface to MoveIt! components */ +namespace planning_interface +{ +class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes +{ +public: + MoveItErrorCode() + { + val = 0; + } + MoveItErrorCode(int code) + { + val = code; + } + MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code) + { + val = code.val; + } + explicit operator bool() const + { + return val == moveit_msgs::MoveItErrorCodes::SUCCESS; + } + bool operator==(const int c) const + { + return val == c; + } + bool operator!=(const int c) const + { + return val != c; + } +}; + +MOVEIT_CLASS_FORWARD(MoveGroupInterface); + +/** \class MoveGroupInterface move_group_interface.h moveit/planning_interface/move_group_interface.h + + \brief Client class to conveniently use the ROS interfaces provided by the move_group node. + + This class includes many default settings to make things easy to use. */ +class MoveGroupInterface +{ +public: + /** \brief Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description' */ + static const std::string ROBOT_DESCRIPTION; + + /** \brief Specification of options to use when constructing the MoveGroupInterface class */ + struct Options + { + Options(const std::string& group_name, const std::string& desc = ROBOT_DESCRIPTION, + const ros::NodeHandle& node_handle = ros::NodeHandle()) + : group_name_(group_name), robot_description_(desc), node_handle_(node_handle) + { + } + + /// The group to construct the class instance for + std::string group_name_; + + /// The robot description parameter name (if different from default) + std::string robot_description_; + + /// Optionally, an instance of the RobotModel to use can be also specified + robot_model::RobotModelConstPtr robot_model_; + + ros::NodeHandle node_handle_; + }; + + MOVEIT_STRUCT_FORWARD(Plan); + + /// The representation of a motion plan (as ROS messasges) + struct Plan + { + /// The full starting state used for planning + moveit_msgs::RobotState start_state_; + + /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) + moveit_msgs::RobotTrajectory trajectory_; + + /// The amount of time it took to generate the plan + double planning_time_; + }; + + /** + \brief Construct a MoveGroupInterface instance call using a specified set of options \e opt. + + \param opt. A MoveGroupInterface::Options structure, if you pass a ros::NodeHandle with a specific callback queue, + it has to be of type ros::CallbackQueue + (which is the default type of callback queues used in ROS) + \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, + one will be constructed internally along with an internal TF2_ROS TransformListener + \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. + */ + MoveGroupInterface(const Options& opt, + const std::shared_ptr& tf_buffer = std::shared_ptr(), + const ros::WallDuration& wait_for_servers = ros::WallDuration()); + MOVEIT_DEPRECATED MoveGroupInterface(const Options& opt, const std::shared_ptr& tf_buffer, + const ros::Duration& wait_for_servers); + + /** + \brief Construct a client for the MoveGroup action for a particular \e group. + + \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, + one will be constructed internally along with an internal TF2_ROS TransformListener + \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. + */ + MoveGroupInterface(const std::string& group, + const std::shared_ptr& tf_buffer = std::shared_ptr(), + const ros::WallDuration& wait_for_servers = ros::WallDuration()); + MOVEIT_DEPRECATED MoveGroupInterface(const std::string& group, const std::shared_ptr& tf_buffer, + const ros::Duration& wait_for_servers); + + ~MoveGroupInterface(); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + MoveGroupInterface(const MoveGroupInterface&) = delete; + MoveGroupInterface& operator=(const MoveGroupInterface&) = delete; + + MoveGroupInterface(MoveGroupInterface&& other); + MoveGroupInterface& operator=(MoveGroupInterface&& other); + + /** \brief Get the name of the group this instance operates on */ + const std::string& getName() const; + + /** \brief Get the names of the named robot states available as targets, both either remembered states or default + * states from srdf */ + const std::vector getNamedTargets(); + + /** \brief Get the RobotModel object. */ + robot_model::RobotModelConstPtr getRobotModel() const; + + /** \brief Get the ROS node handle of this instance operates on */ + const ros::NodeHandle& getNodeHandle() const; + + /** \brief Get the name of the frame in which the robot is planning */ + const std::string& getPlanningFrame() const; + + /** \brief Get the available planning group names */ + const std::vector& getJointModelGroupNames() const; + + /** \brief Get vector of names of joints available in move group */ + const std::vector& getJointNames(); + + /** \brief Get vector of names of links available in move group */ + const std::vector& getLinkNames(); + + /** \brief Get the joint angles for targets specified by name */ + std::map getNamedTargetValues(const std::string& name); + + /** \brief Get only the active (actuated) joints this instance operates on */ + const std::vector& getActiveJoints() const; + + /** \brief Get all the joints this instance operates on (including fixed joints)*/ + const std::vector& getJoints() const; + + /** \brief Get the number of variables used to describe the state of this group. This is larger or equal to the number + * of DOF. */ + unsigned int getVariableCount() const; + + /** \brief Get the description of the planning plugin loaded by the action server */ + bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc); + + /** \brief Get the planner parameters for given group and planner_id */ + std::map getPlannerParams(const std::string& planner_id, const std::string& group = ""); + + /** \brief Set the planner parameters for given group and planner_id */ + void setPlannerParams(const std::string& planner_id, const std::string& group, + const std::map& params, bool bReplace = false); + + /** \brief Get the default planner for a given group (or global default) */ + std::string getDefaultPlannerId(const std::string& group = "") const; + + /** \brief Specify a planner to be used for further planning */ + void setPlannerId(const std::string& planner_id); + + /** \brief Get the current planner_id */ + const std::string& getPlannerId() const; + + /** \brief Specify the maximum amount of time to use when planning */ + void setPlanningTime(double seconds); + + /** \brief Set the number of times the motion plan is to be computed from scratch before the shortest solution is + * returned. The default value is 1.*/ + void setNumPlanningAttempts(unsigned int num_planning_attempts); + + /** \brief Set a scaling factor for optionally reducing the maximum joint velocity. + Allowed values are in (0,1]. The maximum joint velocity specified + in the robot model is multiplied by the factor. If outside valid range + (imporantly, this includes it being set to 0.0), the factor is set to a + default value of 1.0 internally (i.e. maximum joint velocity) */ + void setMaxVelocityScalingFactor(double max_velocity_scaling_factor); + + /** \brief Set a scaling factor for optionally reducing the maximum joint acceleration. + Allowed values are in (0,1]. The maximum joint acceleration specified + in the robot model is multiplied by the factor. If outside valid range + (imporantly, this includes it being set to 0.0), the factor is set to a + default value of 1.0 internally (i.e. maximum joint acceleration) */ + void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor); + + /** \brief Get the number of seconds set by setPlanningTime() */ + double getPlanningTime() const; + + /** \brief Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configuration + * space */ + double getGoalJointTolerance() const; + + /** \brief Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where the + * end-effector must reach.*/ + double getGoalPositionTolerance() const; + + /** \brief Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll, pitch and + * yaw, in radians. */ + double getGoalOrientationTolerance() const; + + /** \brief Set the tolerance that is used for reaching the goal. For + joint state goals, this will be distance for each joint, in the + configuration space (radians or meters depending on joint type). For pose + goals this will be the radius of a sphere where the end-effector must + reach. This function simply triggers calls to setGoalPositionTolerance(), + setGoalOrientationTolerance() and setGoalJointTolerance(). */ + void setGoalTolerance(double tolerance); + + /** \brief Set the joint tolerance (for each joint) that is used for reaching the goal when moving to a joint value + * target. */ + void setGoalJointTolerance(double tolerance); + + /** \brief Set the position tolerance that is used for reaching the goal when moving to a pose. */ + void setGoalPositionTolerance(double tolerance); + + /** \brief Set the orientation tolerance that is used for reaching the goal when moving to a pose. */ + void setGoalOrientationTolerance(double tolerance); + + /** \brief Specify the workspace bounding box. + The box is specified in the planning frame (i.e. relative to the robot root link start position). + This is useful when the planning group contains the root joint of the robot -- i.e. when planning motion for the + robot relative to the world. */ + void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz); + + /** \brief If a different start state should be considered instead of the current state of the robot, this function + * sets that state */ + void setStartState(const moveit_msgs::RobotState& start_state); + + /** \brief If a different start state should be considered instead of the current state of the robot, this function + * sets that state */ + void setStartState(const robot_state::RobotState& start_state); + + /** \brief Set the starting state for planning to be that reported by the robot's joint state publication */ + void setStartStateToCurrentState(); + + /** \brief For pick/place operations, the name of the support surface is used to specify the fact that attached + * objects are allowed to touch the support surface */ + void setSupportSurfaceName(const std::string& name); + + /** + * \name Setting a joint state target (goal) + * + * There are 2 types of goal targets: + * \li a JointValueTarget (aka JointStateTarget) specifies an absolute value for each joint (angle for rotational + *joints or position for prismatic joints). + * \li a PoseTarget (Position, Orientation, or Pose) specifies the pose of one or more end effectors (and the planner + *can use any joint values that reaches the pose(s)). + * + * Only one or the other is used for planning. Calling any of the + * set*JointValueTarget() functions sets the current goal target to the + * JointValueTarget. Calling any of the setPoseTarget(), + * setOrientationTarget(), setRPYTarget(), setPositionTarget() functions sets + * the current goal target to the Pose target. + */ + /**@{*/ + + /** \brief Set the JointValueTarget and use it for future planning requests. + + \e group_variable_values MUST exactly match the variable order as returned by + getJointValueTarget(std::vector&). + + This always sets all of the group's joint values. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const std::vector& group_variable_values); + + /** \brief Set the JointValueTarget and use it for future planning requests. + + \e variable_values is a map of joint variable names to values. Joints in + the group are used to set the JointValueTarget. Joints in the model but + not in the group are ignored. An exception is thrown if a joint name is + not found in the model. Joint variables in the group that are missing + from \e variable_values remain unchanged (to reset all target variables + to their current values in the robot use + setJointValueTarget(getCurrentJointValues())). + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const std::map& variable_values); + + /** \brief Set the JointValueTarget and use it for future planning requests. + + \e variable_names are variable joint names and variable_values are + variable joint positions. Joints in the group are used to set the + JointValueTarget. Joints in the model but not in the group are ignored. + An exception is thrown if a joint name is not found in the model. + Joint variables in the group that are missing from \e variable_names + remain unchanged (to reset all target variables to their current values + in the robot use setJointValueTarget(getCurrentJointValues())). + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const std::vector& variable_names, const std::vector& variable_values); + + /** \brief Set the JointValueTarget and use it for future planning requests. + + The target for all joints in the group are set to the value in \e robot_state. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const robot_state::RobotState& robot_state); + + /** \brief Set the JointValueTarget and use it for future planning requests. + + \e values MUST have one value for each variable in joint \e joint_name. + \e values are set as the target for this joint. + Other joint targets remain unchanged. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const std::string& joint_name, const std::vector& values); + + /** \brief Set the JointValueTarget and use it for future planning requests. + + Joint \e joint_name must be a 1-DOF joint. + \e value is set as the target for this joint. + Other joint targets remain unchanged. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const std::string& joint_name, double value); + + /** \brief Set the JointValueTarget and use it for future planning requests. + + \e state is used to set the target joint state values. + Values not specified in \e state remain unchanged. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If these values are out of bounds then false is returned BUT THE VALUES + ARE STILL SET AS THE GOAL. */ + bool setJointValueTarget(const sensor_msgs::JointState& state); + + /** \brief Set the joint state goal for a particular joint by computing IK. + + This is different from setPoseTarget() in that a single IK state (aka + JointValueTarget) is computed using IK, and the resulting + JointValueTarget is used as the target for planning. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If IK fails to find a solution then false is returned BUT THE PARTIAL + RESULT OF IK IS STILL SET AS THE GOAL. */ + bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = ""); + + /** \brief Set the joint state goal for a particular joint by computing IK. + + This is different from setPoseTarget() in that a single IK state (aka + JointValueTarget) is computed using IK, and the resulting + JointValueTarget is used as the target for planning. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If IK fails to find a solution then false is returned BUT THE PARTIAL + RESULT OF IK IS STILL SET AS THE GOAL. */ + bool setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link = ""); + + /** \brief Set the joint state goal for a particular joint by computing IK. + + This is different from setPoseTarget() in that a single IK state (aka + JointValueTarget) is computed using IK, and the resulting + JointValueTarget is used as the target for planning. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If IK fails to find a solution then false is returned BUT THE PARTIAL + RESULT OF IK IS STILL SET AS THE GOAL. */ + bool setJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = ""); + + /** \brief Set the joint state goal for a particular joint by computing IK. + + This is different from setPoseTarget() in that a single IK state (aka + JointValueTarget) is computed using IK, and the resulting + JointValueTarget is used as the target for planning. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If IK fails to find a solution then an approximation is used. */ + bool setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = ""); + + /** \brief Set the joint state goal for a particular joint by computing IK. + + This is different from setPoseTarget() in that a single IK state (aka + JointValueTarget) is computed using IK, and the resulting + JointValueTarget is used as the target for planning. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If IK fails to find a solution then an approximation is used. */ + bool setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, + const std::string& end_effector_link = ""); + + /** \brief Set the joint state goal for a particular joint by computing IK. + + This is different from setPoseTarget() in that a single IK state (aka + JointValueTarget) is computed using IK, and the resulting + JointValueTarget is used as the target for planning. + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. + + If IK fails to find a solution then an approximation is used. */ + bool setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = ""); + + /** \brief Set the joint state goal to a random joint configuration + + After this call, the JointValueTarget is used \b instead of any + previously set Position, Orientation, or Pose targets. */ + void setRandomTarget(); + + /** \brief Set the current joint values to be ones previously remembered by rememberJointValues() or, if not found, + that are specified in the SRDF under the name \e name as a group state*/ + bool setNamedTarget(const std::string& name); + + /** \brief Get the current joint state goal in a form compatible to setJointValueTarget() */ + void getJointValueTarget(std::vector& group_variable_values) const; + + /// Get the currently set joint state goal, replaced by private getTargetRobotState() + MOVEIT_DEPRECATED const robot_state::RobotState& getJointValueTarget() const; + + /**@}*/ + + /** + * \name Setting a pose target (goal) + * + * Setting a Pose (or Position or Orientation) target disables any previously + * set JointValueTarget. + * + * For groups that have multiple end effectors, a pose can be set for each + * end effector in the group. End effectors which do not have a pose target + * set will end up in arbitrary positions. + */ + /**@{*/ + + /** \brief Set the goal position of the end-effector \e end_effector_link to be (\e x, \e y, \e z). + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + This new position target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target for this \e + end_effector_link. */ + bool setPositionTarget(double x, double y, double z, const std::string& end_effector_link = ""); + + /** \brief Set the goal orientation of the end-effector \e end_effector_link to be (\e roll,\e pitch,\e yaw) radians. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target for this \e + end_effector_link. */ + bool setRPYTarget(double roll, double pitch, double yaw, const std::string& end_effector_link = ""); + + /** \brief Set the goal orientation of the end-effector \e end_effector_link to be the quaternion (\e x,\e y,\e z,\e + w). + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target for this \e + end_effector_link. */ + bool setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link = ""); + + /** \brief Set the goal pose of the end-effector \e end_effector_link. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + This new pose target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target for this \e + end_effector_link. */ + bool setPoseTarget(const Eigen::Isometry3d& end_effector_pose, const std::string& end_effector_link = ""); + + /** \brief Set the goal pose of the end-effector \e end_effector_link. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target for this \e + end_effector_link. */ + bool setPoseTarget(const geometry_msgs::Pose& target, const std::string& end_effector_link = ""); + + /** \brief Set the goal pose of the end-effector \e end_effector_link. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target for this \e + end_effector_link. */ + bool setPoseTarget(const geometry_msgs::PoseStamped& target, const std::string& end_effector_link = ""); + + /** \brief Set goal poses for \e end_effector_link. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + When planning, the planner will find a path to one (arbitrarily chosen) + pose from the list. If this group contains multiple end effectors then + all end effectors in the group should have the same number of pose + targets. If planning is successful then the result of the plan will + place all end effectors at a pose from the same index in the list. (In + other words, if one end effector ends up at the 3rd pose in the list then + all end effectors in the group will end up at the 3rd pose in their + respective lists. End effectors which do not matter (i.e. can end up in + any position) can have their pose targets disabled by calling + clearPoseTarget() for that end_effector_link. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target(s) for this \e + end_effector_link. */ + bool setPoseTargets(const EigenSTL::vector_Isometry3d& end_effector_pose, const std::string& end_effector_link = ""); + + /** \brief Set goal poses for \e end_effector_link. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + When planning, the planner will find a path to one (arbitrarily chosen) + pose from the list. If this group contains multiple end effectors then + all end effectors in the group should have the same number of pose + targets. If planning is successful then the result of the plan will + place all end effectors at a pose from the same index in the list. (In + other words, if one end effector ends up at the 3rd pose in the list then + all end effectors in the group will end up at the 3rd pose in their + respective lists. End effectors which do not matter (i.e. can end up in + any position) can have their pose targets disabled by calling + clearPoseTarget() for that end_effector_link. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target(s) for this \e + end_effector_link. */ + bool setPoseTargets(const std::vector& target, const std::string& end_effector_link = ""); + + /** \brief Set goal poses for \e end_effector_link. + + If \e end_effector_link is empty then getEndEffectorLink() is used. + + When planning, the planner will find a path to one (arbitrarily chosen) + pose from the list. If this group contains multiple end effectors then + all end effectors in the group should have the same number of pose + targets. If planning is successful then the result of the plan will + place all end effectors at a pose from the same index in the list. (In + other words, if one end effector ends up at the 3rd pose in the list then + all end effectors in the group will end up at the 3rd pose in their + respective lists. End effectors which do not matter (i.e. can end up in + any position) can have their pose targets disabled by calling + clearPoseTarget() for that end_effector_link. + + This new orientation target replaces any pre-existing JointValueTarget or + pre-existing Position, Orientation, or Pose target(s) for this \e + end_effector_link. */ + bool setPoseTargets(const std::vector& target, const std::string& end_effector_link = ""); + + /// Specify which reference frame to assume for poses specified without a reference frame. + void setPoseReferenceFrame(const std::string& pose_reference_frame); + + /** \brief Specify the parent link of the end-effector. + This \e end_effector_link will be used in calls to pose target functions + when end_effector_link is not explicitly specified. */ + bool setEndEffectorLink(const std::string& end_effector_link); + + /** \brief Specify the name of the end-effector to use. + This is equivalent to setting the EndEffectorLink to the parent link of this end effector. */ + bool setEndEffector(const std::string& eef_name); + + /// Forget pose(s) specified for \e end_effector_link + void clearPoseTarget(const std::string& end_effector_link = ""); + + /// Forget any poses specified for all end-effectors. + void clearPoseTargets(); + + /** Get the currently set pose goal for the end-effector \e end_effector_link. + If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is + assumed. + If multiple targets are set for \e end_effector_link this will return the first one. + If no pose target is set for this \e end_effector_link then an empty pose will be returned (check for + orientation.xyzw == 0). */ + const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link = "") const; + + /** Get the currently set pose goal for the end-effector \e end_effector_link. The pose goal can consist of multiple + poses, + if corresponding setPoseTarget() calls were made. Otherwise, only one pose is returned in the vector. + If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is + assumed */ + const std::vector& getPoseTargets(const std::string& end_effector_link = "") const; + + /** \brief Get the current end-effector link. + This returns the value set by setEndEffectorLink() (or indirectly by setEndEffector()). + If setEndEffectorLink() was not called, this function reports the link name that serves as parent + of an end-effector attached to this group. If there are multiple end-effectors, one of them is returned. + If no such link is known, the empty string is returned. */ + const std::string& getEndEffectorLink() const; + + /** \brief Get the current end-effector name. + This returns the value set by setEndEffector() (or indirectly by setEndEffectorLink()). + If setEndEffector() was not called, this function reports an end-effector attached to this group. + If there are multiple end-effectors, one of them is returned. If no end-effector is known, the empty string is + returned. */ + const std::string& getEndEffector() const; + + /** \brief Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the robot + * model */ + const std::string& getPoseReferenceFrame() const; + + /**@}*/ + + /** + * \name Planning a path from the start position to the Target (goal) position, and executing that plan. + */ + /**@{*/ + + /** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified + target. + This call is not blocking (does not wait for the execution of the trajectory to complete). */ + MoveItErrorCode asyncMove(); + + /** \brief Get the move_group action client used by the \e MoveGroupInterface. + The client can be used for querying the execution state of the trajectory and abort trajectory execution + during asynchronous execution. */ + actionlib::SimpleActionClient& getMoveGroupClient() const; + + /** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified + target. + This call is always blocking (waits for the execution of the trajectory to complete) and requires an asynchronous + spinner to be started.*/ + MoveItErrorCode move(); + + /** \brief Compute a motion plan that takes the group declared in the constructor from the current state to the + specified + target. No execution is performed. The resulting plan is stored in \e plan*/ + MoveItErrorCode plan(Plan& plan); + + /** \brief Given a \e plan, execute it without waiting for completion. Return true on success. */ + MoveItErrorCode asyncExecute(const Plan& plan); + + /** \brief Given a \e plan, execute it while waiting for completion. Return true on success. */ + MoveItErrorCode execute(const Plan& plan); + + /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters + between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the + waypoints is that specified by setPoseReferenceFrame(). No more than \e jump_threshold + is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK + solutions). + Collisions are avoided if \e avoid_collisions is set to true. If collisions cannot be avoided, the function fails. + Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the + waypoints. + Return -1.0 in case of error. */ + double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, + moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true, + moveit_msgs::MoveItErrorCodes* error_code = NULL); + + /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters + between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the + waypoints is that specified by setPoseReferenceFrame(). No more than \e jump_threshold + is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK + solutions). + Kinematic constraints for the path given by \e path_constraints will be met for every point along the trajectory, + if they are not met, a partial solution will be returned. + Constraints are checked (collision and kinematic) if \e avoid_collisions is set to true. If constraints cannot be + met, the function fails. + Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the + waypoints. + Return -1.0 in case of error. */ + double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, + moveit_msgs::RobotTrajectory& trajectory, + const moveit_msgs::Constraints& path_constraints, bool avoid_collisions = true, + moveit_msgs::MoveItErrorCodes* error_code = NULL); + + /** \brief Stop any trajectory execution, if one is active */ + void stop(); + + /** \brief Specify whether the robot is allowed to look around before moving if it determines it should (default is + * true) */ + void allowLooking(bool flag); + + /** \brief Specify whether the robot is allowed to replan if it detects changes in the environment */ + void allowReplanning(bool flag); + + /** \brief Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it + in \e request */ + void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request); + + /**@}*/ + + /** + * \name High level actions that trigger a sequence of plans and actions. + */ + /**@{*/ + + /** \brief Pick up an object + + This applies a number of hard-coded default grasps */ + MoveItErrorCode pick(const std::string& object, bool plan_only = false); + + /** \brief Pick up an object given a grasp pose */ + MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only = false); + + /** \brief Pick up an object given possible grasp poses + + if the vector is left empty this behaves like pick(const std::string &object) */ + MoveItErrorCode pick(const std::string& object, const std::vector& grasps, + bool plan_only = false); + + /** \brief Pick up an object + + calls the external moveit_msgs::GraspPlanning service "plan_grasps" to compute possible grasps */ + MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false); + + /** \brief Pick up an object + + calls the external moveit_msgs::GraspPlanning service "plan_grasps" to compute possible grasps */ + MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false); + + /** \brief Place an object somewhere safe in the world (a safe location will be detected) */ + MoveItErrorCode place(const std::string& object, bool plan_only = false); + + /** \brief Place an object at one of the specified possible locations */ + MoveItErrorCode place(const std::string& object, const std::vector& locations, + bool plan_only = false); + + /** \brief Place an object at one of the specified possible locations */ + MoveItErrorCode place(const std::string& object, const std::vector& poses, + bool plan_only = false); + + /** \brief Place an object at one of the specified possible location */ + MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only = false); + + /** \brief Given the name of an object in the planning scene, make + the object attached to a link of the robot. If no link name is + specified, the end-effector is used. If there is no + end-effector, the first link in the group is used. If the object + name does not exist an error will be produced in move_group, but + the request made by this interface will succeed. */ + bool attachObject(const std::string& object, const std::string& link = ""); + + /** \brief Given the name of an object in the planning scene, make + the object attached to a link of the robot. The set of links the + object is allowed to touch without considering that a collision + is specified by \e touch_links. If \e link is empty, the + end-effector link is used. If there is no end-effector, the + first link in the group is used. If the object name does not + exist an error will be produced in move_group, but the request + made by this interface will succeed. */ + bool attachObject(const std::string& object, const std::string& link, const std::vector& touch_links); + + /** \brief Detach an object. \e name specifies the name of the + object attached to this group, or the name of the link the + object is attached to. If there is no name specified, and there + is only one attached object, that object is detached. An error + is produced if no object to detach is identified. */ + bool detachObject(const std::string& name = ""); + + /**@}*/ + + /** + * \name Query current robot state + */ + /**@{*/ + + /** \brief When reasoning about the current state of a robot, a + CurrentStateMonitor instance is automatically constructed. This + function allows triggering the construction of that object from + the beginning, so that future calls to functions such as + getCurrentState() will not take so long and are less likely to fail. */ + bool startStateMonitor(double wait = 1.0); + + /** \brief Get the current joint values for the joints planned for by this instance (see getJoints()) */ + std::vector getCurrentJointValues(); + + /** \brief Get the current state of the robot within the duration specified by wait. */ + robot_state::RobotStatePtr getCurrentState(double wait = 1); + + /** \brief Get the pose for the end-effector \e end_effector_link. + If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is + assumed */ + geometry_msgs::PoseStamped getCurrentPose(const std::string& end_effector_link = ""); + + /** \brief Get the roll-pitch-yaw (XYZ) for the end-effector \e end_effector_link. + If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is + assumed */ + std::vector getCurrentRPY(const std::string& end_effector_link = ""); + + /** \brief Get random joint values for the joints planned for by this instance (see getJoints()) */ + std::vector getRandomJointValues(); + + /** \brief Get a random reachable pose for the end-effector \e end_effector_link. + If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is + assumed */ + geometry_msgs::PoseStamped getRandomPose(const std::string& end_effector_link = ""); + + /**@}*/ + + /** + * \name Manage named joint configurations + */ + /**@{*/ + + /** \brief Remember the current joint values (of the robot being monitored) under \e name. + These can be used by setNamedTarget(). + These values are remembered locally in the client. Other clients will + not have access to them. */ + void rememberJointValues(const std::string& name); + + /** \brief Remember the specified joint values under \e name. + These can be used by setNamedTarget(). + These values are remembered locally in the client. Other clients will + not have access to them. */ + void rememberJointValues(const std::string& name, const std::vector& values); + + /** \brief Get the currently remembered map of names to joint values. */ + const std::map >& getRememberedJointValues() const + { + return remembered_joint_values_; + } + + /** \brief Forget the joint values remebered under \e name */ + void forgetJointValues(const std::string& name); + + /**@}*/ + + /** + * \name Manage planning constraints + */ + /**@{*/ + + /** \brief Specify where the database server that holds known constraints resides */ + void setConstraintsDatabase(const std::string& host, unsigned int port); + + /** \brief Get the names of the known constraints as read from the Mongo database, if a connection was achieved. */ + std::vector getKnownConstraints() const; + + /** \brief Get the actual set of constraints in use with this MoveGroupInterface. + @return A copy of the current path constraints set for this interface + */ + moveit_msgs::Constraints getPathConstraints() const; + + /** \brief Specify a set of path constraints to use. + The constraints are looked up by name from the Mongo database server. + This replaces any path constraints set in previous calls to setPathConstraints(). */ + bool setPathConstraints(const std::string& constraint); + + /** \brief Specify a set of path constraints to use. + This version does not require a database server. + This replaces any path constraints set in previous calls to setPathConstraints(). */ + void setPathConstraints(const moveit_msgs::Constraints& constraint); + + /** \brief Specify that no path constraints are to be used. + This removes any path constraints set in previous calls to setPathConstraints(). */ + void clearPathConstraints(); + + moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const; + void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint); + void clearTrajectoryConstraints(); + + /**@}*/ + +protected: + /** return the full RobotState of the joint-space target, only for internal use */ + const robot_state::RobotState& getTargetRobotState() const; + +private: + std::map > remembered_joint_values_; + class MoveGroupInterfaceImpl; + MoveGroupInterfaceImpl* impl_; +}; +} +} + +#endif diff --git a/moveit_ros/planning_interface/moveit_cpp/src/demo.cpp b/moveit_ros/planning_interface/moveit_cpp/src/demo.cpp new file mode 100644 index 0000000000..2cd370f906 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/src/demo.cpp @@ -0,0 +1,121 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Ioan Sucan */ + +#include +#include + +void demoPick(moveit::planning_interface::MoveGroupInterface& group) +{ + std::vector grasps; + for (std::size_t i = 0; i < 20; ++i) + { + geometry_msgs::PoseStamped p = group.getRandomPose(); + p.pose.orientation.x = 0; + p.pose.orientation.y = 0; + p.pose.orientation.z = 0; + p.pose.orientation.w = 1; + moveit_msgs::Grasp g; + g.grasp_pose = p; + g.pre_grasp_approach.direction.vector.x = 1.0; + g.post_grasp_retreat.direction.vector.z = 1.0; + g.post_grasp_retreat.direction.header = p.header; + g.pre_grasp_approach.min_distance = 0.2; + g.pre_grasp_approach.desired_distance = 0.4; + g.post_grasp_retreat.min_distance = 0.1; + g.post_grasp_retreat.desired_distance = 0.27; + g.pre_grasp_posture.joint_names.resize(1, "r_gripper_joint"); + g.pre_grasp_posture.points.resize(1); + g.pre_grasp_posture.points[0].positions.resize(1); + g.pre_grasp_posture.points[0].positions[0] = 1; + + g.grasp_posture.joint_names.resize(1, "r_gripper_joint"); + g.grasp_posture.points.resize(1); + g.grasp_posture.points[0].positions.resize(1); + g.grasp_posture.points[0].positions[0] = 0; + + grasps.push_back(g); + } + group.pick("bubu", grasps); +} + +void demoPlace(moveit::planning_interface::MoveGroupInterface& group) +{ + std::vector loc; + for (std::size_t i = 0; i < 20; ++i) + { + geometry_msgs::PoseStamped p = group.getRandomPose(); + p.pose.orientation.x = 0; + p.pose.orientation.y = 0; + p.pose.orientation.z = 0; + p.pose.orientation.w = 1; + moveit_msgs::PlaceLocation g; + g.place_pose = p; + g.pre_place_approach.direction.vector.x = 1.0; + g.post_place_retreat.direction.vector.z = 1.0; + g.post_place_retreat.direction.header = p.header; + g.pre_place_approach.min_distance = 0.2; + g.pre_place_approach.desired_distance = 0.4; + g.post_place_retreat.min_distance = 0.1; + g.post_place_retreat.desired_distance = 0.27; + + g.post_place_posture.joint_names.resize(1, "r_gripper_joint"); + g.post_place_posture.points.resize(1); + g.post_place_posture.points[0].positions.resize(1); + g.post_place_posture.points[0].positions[0] = 0; + + loc.push_back(g); + } + group.place("bubu", loc); +} + +void attachObject() +{ +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + moveit::planning_interface::MoveGroupInterface group(argc > 1 ? argv[1] : "right_arm"); + demoPlace(group); + + sleep(2); + + return 0; +} diff --git a/moveit_ros/planning_interface/moveit_cpp/src/move_group_interface.cpp b/moveit_ros/planning_interface/moveit_cpp/src/move_group_interface.cpp new file mode 100644 index 0000000000..f92b86e57c --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/src/move_group_interface.cpp @@ -0,0 +1,2245 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, SRI International + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Ioan Sucan, Sachin Chitta */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +const std::string MoveGroupInterface::ROBOT_DESCRIPTION = + "robot_description"; // name of the robot description (a param name, so it can be changed externally) + +const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps + +namespace +{ +enum ActiveTargetType +{ + JOINT, + POSE, + POSITION, + ORIENTATION +}; +} + +class MoveGroupInterface::MoveGroupInterfaceImpl +{ +public: + MoveGroupInterfaceImpl(const Options& opt, const std::shared_ptr& tf_buffer, + const ros::WallDuration& wait_for_servers) + : opt_(opt), node_handle_(opt.node_handle_), tf_buffer_(tf_buffer) + { + robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_); + if (!getRobotModel()) + { + std::string error = "Unable to construct robot model. Please make sure all needed information is on the " + "parameter server."; + ROS_FATAL_STREAM_NAMED("move_group_interface", error); + throw std::runtime_error(error); + } + + if (!getRobotModel()->hasJointModelGroup(opt.group_name_)) + { + std::string error = "Group '" + opt.group_name_ + "' was not found."; + ROS_FATAL_STREAM_NAMED("move_group_interface", error); + throw std::runtime_error(error); + } + + joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_); + + joint_state_target_.reset(new robot_state::RobotState(getRobotModel())); + joint_state_target_->setToDefaultValues(); + active_target_ = JOINT; + can_look_ = false; + can_replan_ = false; + replan_delay_ = 2.0; + goal_joint_tolerance_ = 1e-4; + goal_position_tolerance_ = 1e-4; // 0.1 mm + goal_orientation_tolerance_ = 1e-3; // ~0.1 deg + allowed_planning_time_ = 5.0; + num_planning_attempts_ = 1; + max_velocity_scaling_factor_ = 1.0; + max_acceleration_scaling_factor_ = 1.0; + initializing_constraints_ = false; + + if (joint_model_group_->isChain()) + end_effector_link_ = joint_model_group_->getLinkModelNames().back(); + pose_reference_frame_ = getRobotModel()->getModelFrame(); + + trajectory_event_publisher_ = node_handle_.advertise( + trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false); + attached_object_publisher_ = node_handle_.advertise( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1, false); + + current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_buffer_, node_handle_); + + ros::WallTime timeout_for_servers = ros::WallTime::now() + wait_for_servers; + if (wait_for_servers == ros::WallDuration()) + timeout_for_servers = ros::WallTime(); // wait for ever + double allotted_time = wait_for_servers.toSec(); + + move_action_client_.reset( + new actionlib::SimpleActionClient(node_handle_, move_group::MOVE_ACTION, false)); + waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time); + + pick_action_client_.reset( + new actionlib::SimpleActionClient(node_handle_, move_group::PICKUP_ACTION, false)); + waitForAction(pick_action_client_, move_group::PICKUP_ACTION, timeout_for_servers, allotted_time); + + place_action_client_.reset( + new actionlib::SimpleActionClient(node_handle_, move_group::PLACE_ACTION, false)); + waitForAction(place_action_client_, move_group::PLACE_ACTION, timeout_for_servers, allotted_time); + + execute_action_client_.reset(new actionlib::SimpleActionClient( + node_handle_, move_group::EXECUTE_ACTION_NAME, false)); + waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time); + + query_service_ = + node_handle_.serviceClient(move_group::QUERY_PLANNERS_SERVICE_NAME); + get_params_service_ = + node_handle_.serviceClient(move_group::GET_PLANNER_PARAMS_SERVICE_NAME); + set_params_service_ = + node_handle_.serviceClient(move_group::SET_PLANNER_PARAMS_SERVICE_NAME); + + cartesian_path_service_ = + node_handle_.serviceClient(move_group::CARTESIAN_PATH_SERVICE_NAME); + + plan_grasps_service_ = node_handle_.serviceClient(GRASP_PLANNING_SERVICE_NAME); + + ROS_INFO_STREAM_NAMED("move_group_interface", "Ready to take commands for planning group " << opt.group_name_ + << "."); + } + + template + void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) + { + ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str()); + + // wait for the server (and spin as needed) + if (timeout == ros::WallTime()) // wait forever + { + while (node_handle_.ok() && !action->isServerConnected()) + { + ros::WallDuration(0.001).sleep(); + // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client + ros::CallbackQueue* queue = dynamic_cast(node_handle_.getCallbackQueue()); + if (queue) + { + queue->callAvailable(); + } + else // in case of nodelets and specific callback queue implementations + { + ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " + "handling."); + } + } + } + else // wait with timeout + { + while (node_handle_.ok() && !action->isServerConnected() && timeout > ros::WallTime::now()) + { + ros::WallDuration(0.001).sleep(); + // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client + ros::CallbackQueue* queue = dynamic_cast(node_handle_.getCallbackQueue()); + if (queue) + { + queue->callAvailable(); + } + else // in case of nodelets and specific callback queue implementations + { + ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " + "handling."); + } + } + } + + if (!action->isServerConnected()) + { + std::stringstream error; + error << "Unable to connect to move_group action server '" << name << "' within allotted time (" << allotted_time + << "s)"; + throw std::runtime_error(error.str()); + } + else + { + ROS_DEBUG_NAMED("move_group_interface", "Connected to '%s'", name.c_str()); + } + } + + ~MoveGroupInterfaceImpl() + { + if (constraints_init_thread_) + constraints_init_thread_->join(); + } + + const std::shared_ptr& getTF() const + { + return tf_buffer_; + } + + const Options& getOptions() const + { + return opt_; + } + + const robot_model::RobotModelConstPtr& getRobotModel() const + { + return robot_model_; + } + + const robot_model::JointModelGroup* getJointModelGroup() const + { + return joint_model_group_; + } + + actionlib::SimpleActionClient& getMoveGroupClient() const + { + return *move_action_client_; + } + + bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) + { + moveit_msgs::QueryPlannerInterfaces::Request req; + moveit_msgs::QueryPlannerInterfaces::Response res; + if (query_service_.call(req, res)) + if (!res.planner_interfaces.empty()) + { + desc = res.planner_interfaces.front(); + return true; + } + return false; + } + + std::map getPlannerParams(const std::string& planner_id, const std::string& group = "") + { + moveit_msgs::GetPlannerParams::Request req; + moveit_msgs::GetPlannerParams::Response res; + req.planner_config = planner_id; + req.group = group; + std::map result; + if (get_params_service_.call(req, res)) + { + for (unsigned int i = 0, end = res.params.keys.size(); i < end; ++i) + result[res.params.keys[i]] = res.params.values[i]; + } + return result; + } + + void setPlannerParams(const std::string& planner_id, const std::string& group, + const std::map& params, bool replace = false) + { + moveit_msgs::SetPlannerParams::Request req; + moveit_msgs::SetPlannerParams::Response res; + req.planner_config = planner_id; + req.group = group; + req.replace = replace; + for (const std::pair& param : params) + { + req.params.keys.push_back(param.first); + req.params.values.push_back(param.second); + } + set_params_service_.call(req, res); + } + + std::string getDefaultPlannerId(const std::string& group) const + { + std::stringstream param_name; + param_name << "move_group"; + if (!group.empty()) + param_name << "/" << group; + param_name << "/default_planner_config"; + + std::string default_planner_config; + node_handle_.getParam(param_name.str(), default_planner_config); + return default_planner_config; + } + + void setPlannerId(const std::string& planner_id) + { + planner_id_ = planner_id; + } + + const std::string& getPlannerId() const + { + return planner_id_; + } + + void setNumPlanningAttempts(unsigned int num_planning_attempts) + { + num_planning_attempts_ = num_planning_attempts; + } + + void setMaxVelocityScalingFactor(double max_velocity_scaling_factor) + { + max_velocity_scaling_factor_ = max_velocity_scaling_factor; + } + + void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) + { + max_acceleration_scaling_factor_ = max_acceleration_scaling_factor; + } + + robot_state::RobotState& getTargetRobotState() + { + return *joint_state_target_; + } + + void setStartState(const robot_state::RobotState& start_state) + { + considered_start_state_.reset(new robot_state::RobotState(start_state)); + } + + void setStartStateToCurrentState() + { + considered_start_state_.reset(); + } + + robot_state::RobotStatePtr getStartState() + { + if (considered_start_state_) + return considered_start_state_; + else + { + robot_state::RobotStatePtr s; + getCurrentState(s); + return s; + } + } + + bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link, + const std::string& frame, bool approx) + { + const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; + // this only works if we have an end-effector + if (!eef.empty()) + { + // first we set the goal to be the same as the start state + moveit::core::RobotStatePtr c = getStartState(); + if (c) + { + setTargetType(JOINT); + c->enforceBounds(); + getTargetRobotState() = *c; + if (!getTargetRobotState().satisfiesBounds(getGoalJointTolerance())) + return false; + } + else + return false; + + // we may need to do approximate IK + kinematics::KinematicsQueryOptions o; + o.return_approximate_solution = approx; + + // if no frame transforms are needed, call IK directly + if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame())) + return getTargetRobotState().setFromIK(getJointModelGroup(), eef_pose, eef, 0.0, + moveit::core::GroupStateValidityCallbackFn(), o); + else + { + if (c->knowsFrameTransform(frame)) + { + // transform the pose first if possible, then do IK + const Eigen::Isometry3d& t = getTargetRobotState().getFrameTransform(frame); + Eigen::Isometry3d p; + tf2::fromMsg(eef_pose, p); + return getTargetRobotState().setFromIK(getJointModelGroup(), t * p, eef, 0.0, + moveit::core::GroupStateValidityCallbackFn(), o); + } + else + { + ROS_ERROR_NAMED("move_group_interface", "Unable to transform from frame '%s' to frame '%s'", frame.c_str(), + getRobotModel()->getModelFrame().c_str()); + return false; + } + } + } + else + return false; + } + + void setEndEffectorLink(const std::string& end_effector) + { + end_effector_link_ = end_effector; + } + + void clearPoseTarget(const std::string& end_effector_link) + { + pose_targets_.erase(end_effector_link); + } + + void clearPoseTargets() + { + pose_targets_.clear(); + } + + const std::string& getEndEffectorLink() const + { + return end_effector_link_; + } + + const std::string& getEndEffector() const + { + if (!end_effector_link_.empty()) + { + const std::vector& possible_eefs = + getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames(); + for (const std::string& possible_eef : possible_eefs) + if (getRobotModel()->getEndEffector(possible_eef)->hasLinkModel(end_effector_link_)) + return possible_eef; + } + static std::string empty; + return empty; + } + + bool setPoseTargets(const std::vector& poses, const std::string& end_effector_link) + { + const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; + if (eef.empty()) + { + ROS_ERROR_NAMED("move_group_interface", "No end-effector to set the pose for"); + return false; + } + else + { + pose_targets_[eef] = poses; + // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors + std::vector& stored_poses = pose_targets_[eef]; + for (geometry_msgs::PoseStamped& stored_pose : stored_poses) + stored_pose.header.stamp = ros::Time(0); + } + return true; + } + + bool hasPoseTarget(const std::string& end_effector_link) const + { + const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; + return pose_targets_.find(eef) != pose_targets_.end(); + } + + const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link) const + { + const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; + + // if multiple pose targets are set, return the first one + std::map >::const_iterator jt = pose_targets_.find(eef); + if (jt != pose_targets_.end()) + if (!jt->second.empty()) + return jt->second.at(0); + + // or return an error + static const geometry_msgs::PoseStamped UNKNOWN; + ROS_ERROR_NAMED("move_group_interface", "Pose for end-effector '%s' not known.", eef.c_str()); + return UNKNOWN; + } + + const std::vector& getPoseTargets(const std::string& end_effector_link) const + { + const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; + + std::map >::const_iterator jt = pose_targets_.find(eef); + if (jt != pose_targets_.end()) + if (!jt->second.empty()) + return jt->second; + + // or return an error + static const std::vector EMPTY; + ROS_ERROR_NAMED("move_group_interface", "Poses for end-effector '%s' are not known.", eef.c_str()); + return EMPTY; + } + + void setPoseReferenceFrame(const std::string& pose_reference_frame) + { + pose_reference_frame_ = pose_reference_frame; + } + + void setSupportSurfaceName(const std::string& support_surface) + { + support_surface_ = support_surface; + } + + const std::string& getPoseReferenceFrame() const + { + return pose_reference_frame_; + } + + void setTargetType(ActiveTargetType type) + { + active_target_ = type; + } + + ActiveTargetType getTargetType() const + { + return active_target_; + } + + bool startStateMonitor(double wait) + { + if (!current_state_monitor_) + { + ROS_ERROR_NAMED("move_group_interface", "Unable to monitor current robot state"); + return false; + } + + // if needed, start the monitor and wait up to 1 second for a full robot state + if (!current_state_monitor_->isActive()) + current_state_monitor_->startStateMonitor(); + + current_state_monitor_->waitForCompleteState(opt_.group_name_, wait); + return true; + } + + bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds = 1.0) + { + if (!current_state_monitor_) + { + ROS_ERROR_NAMED("move_group_interface", "Unable to get current robot state"); + return false; + } + + // if needed, start the monitor and wait up to 1 second for a full robot state + if (!current_state_monitor_->isActive()) + current_state_monitor_->startStateMonitor(); + + if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds)) + { + ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state"); + return false; + } + + current_state = current_state_monitor_->getCurrentState(); + return true; + } + + /** \brief Place an object at one of the specified possible locations */ + MoveItErrorCode place(const std::string& object, const std::vector& poses, + bool plan_only = false) + { + std::vector locations; + for (const geometry_msgs::PoseStamped& pose : poses) + { + moveit_msgs::PlaceLocation location; + location.pre_place_approach.direction.vector.z = -1.0; + location.post_place_retreat.direction.vector.x = -1.0; + location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame(); + location.post_place_retreat.direction.header.frame_id = end_effector_link_; + + location.pre_place_approach.min_distance = 0.1; + location.pre_place_approach.desired_distance = 0.2; + location.post_place_retreat.min_distance = 0.0; + location.post_place_retreat.desired_distance = 0.2; + // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody + + location.place_pose = pose; + locations.push_back(location); + } + ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations", + (unsigned int)locations.size()); + return place(object, locations, plan_only); + } + + MoveItErrorCode place(const std::string& object, const std::vector& locations, + bool plan_only = false) + { + if (!place_action_client_) + { + ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + if (!place_action_client_->isServerConnected()) + { + ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + moveit_msgs::PlaceGoal goal; + constructGoal(goal, object); + goal.place_locations = locations; + goal.planning_options.plan_only = plan_only; + goal.planning_options.look_around = can_look_; + goal.planning_options.replan = can_replan_; + goal.planning_options.replan_delay = replan_delay_; + goal.planning_options.planning_scene_diff.is_diff = true; + goal.planning_options.planning_scene_diff.robot_state.is_diff = true; + + place_action_client_->sendGoal(goal); + ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size()); + if (!place_action_client_->waitForResult()) + { + ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early"); + } + if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + return MoveItErrorCode(place_action_client_->getResult()->error_code); + } + else + { + ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": " + << place_action_client_->getState().getText()); + return MoveItErrorCode(place_action_client_->getResult()->error_code); + } + } + + MoveItErrorCode pick(const std::string& object, const std::vector& grasps, bool plan_only = false) + { + if (!pick_action_client_) + { + ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + if (!pick_action_client_->isServerConnected()) + { + ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + moveit_msgs::PickupGoal goal; + constructGoal(goal, object); + goal.possible_grasps = grasps; + goal.planning_options.plan_only = plan_only; + goal.planning_options.look_around = can_look_; + goal.planning_options.replan = can_replan_; + goal.planning_options.replan_delay = replan_delay_; + goal.planning_options.planning_scene_diff.is_diff = true; + goal.planning_options.planning_scene_diff.robot_state.is_diff = true; + + pick_action_client_->sendGoal(goal); + if (!pick_action_client_->waitForResult()) + { + ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early"); + } + if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + return MoveItErrorCode(pick_action_client_->getResult()->error_code); + } + else + { + ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": " + << pick_action_client_->getState().getText()); + return MoveItErrorCode(pick_action_client_->getResult()->error_code); + } + } + + MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false) + { + if (object.empty()) + { + return planGraspsAndPick(moveit_msgs::CollisionObject()); + } + moveit::planning_interface::PlanningSceneInterface psi; + + std::map objects = psi.getObjects(std::vector(1, object)); + + if (objects.empty()) + { + ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '" + << object << "', but the object could not be found"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME); + } + + return planGraspsAndPick(objects[object], plan_only); + } + + MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false) + { + if (!plan_grasps_service_) + { + ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '" + << GRASP_PLANNING_SERVICE_NAME + << "' is not available." + " This has to be implemented and started separately."); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + + moveit_msgs::GraspPlanning::Request request; + moveit_msgs::GraspPlanning::Response response; + + request.group_name = opt_.group_name_; + request.target = object; + request.support_surfaces.push_back(support_surface_); + + ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner..."); + if (!plan_grasps_service_.call(request, response) || + response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS) + { + ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick."); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + + return pick(object.id, response.grasps, plan_only); + } + + MoveItErrorCode plan(Plan& plan) + { + if (!move_action_client_) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + if (!move_action_client_->isServerConnected()) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + + moveit_msgs::MoveGroupGoal goal; + constructGoal(goal); + goal.planning_options.plan_only = true; + goal.planning_options.look_around = false; + goal.planning_options.replan = false; + goal.planning_options.planning_scene_diff.is_diff = true; + goal.planning_options.planning_scene_diff.robot_state.is_diff = true; + + move_action_client_->sendGoal(goal); + if (!move_action_client_->waitForResult()) + { + ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); + } + if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + plan.trajectory_ = move_action_client_->getResult()->planned_trajectory; + plan.start_state_ = move_action_client_->getResult()->trajectory_start; + plan.planning_time_ = move_action_client_->getResult()->planning_time; + return MoveItErrorCode(move_action_client_->getResult()->error_code); + } + else + { + ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": " + << move_action_client_->getState().getText()); + return MoveItErrorCode(move_action_client_->getResult()->error_code); + } + } + + MoveItErrorCode move(bool wait) + { + if (!move_action_client_) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + if (!move_action_client_->isServerConnected()) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + + moveit_msgs::MoveGroupGoal goal; + constructGoal(goal); + goal.planning_options.plan_only = false; + goal.planning_options.look_around = can_look_; + goal.planning_options.replan = can_replan_; + goal.planning_options.replan_delay = replan_delay_; + goal.planning_options.planning_scene_diff.is_diff = true; + goal.planning_options.planning_scene_diff.robot_state.is_diff = true; + + move_action_client_->sendGoal(goal); + if (!wait) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); + } + + if (!move_action_client_->waitForResult()) + { + ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); + } + + if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + return MoveItErrorCode(move_action_client_->getResult()->error_code); + } + else + { + ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString() + << ": " << move_action_client_->getState().getText()); + return MoveItErrorCode(move_action_client_->getResult()->error_code); + } + } + + MoveItErrorCode execute(const Plan& plan, bool wait) + { + if (!execute_action_client_->isServerConnected()) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + + moveit_msgs::ExecuteTrajectoryGoal goal; + goal.trajectory = plan.trajectory_; + + execute_action_client_->sendGoal(goal); + if (!wait) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); + } + + if (!execute_action_client_->waitForResult()) + { + ROS_INFO_STREAM_NAMED("move_group_interface", "ExecuteTrajectory action returned early"); + } + + if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + return MoveItErrorCode(execute_action_client_->getResult()->error_code); + } + else + { + ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString() + << ": " << execute_action_client_->getState().getText()); + return MoveItErrorCode(execute_action_client_->getResult()->error_code); + } + } + + double computeCartesianPath(const std::vector& waypoints, double step, double jump_threshold, + moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints, + bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code) + { + moveit_msgs::GetCartesianPath::Request req; + moveit_msgs::GetCartesianPath::Response res; + + if (considered_start_state_) + robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state); + else + req.start_state.is_diff = true; + + req.group_name = opt_.group_name_; + req.header.frame_id = getPoseReferenceFrame(); + req.header.stamp = ros::Time::now(); + req.waypoints = waypoints; + req.max_step = step; + req.jump_threshold = jump_threshold; + req.path_constraints = path_constraints; + req.avoid_collisions = avoid_collisions; + req.link_name = getEndEffectorLink(); + + if (cartesian_path_service_.call(req, res)) + { + error_code = res.error_code; + if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) + { + msg = res.solution; + return res.fraction; + } + else + return -1.0; + } + else + { + error_code.val = error_code.FAILURE; + return -1.0; + } + } + + void stop() + { + if (trajectory_event_publisher_) + { + std_msgs::String event; + event.data = "stop"; + trajectory_event_publisher_.publish(event); + } + } + + bool attachObject(const std::string& object, const std::string& link, const std::vector& touch_links) + { + std::string l = link.empty() ? getEndEffectorLink() : link; + if (l.empty()) + { + const std::vector& links = joint_model_group_->getLinkModelNames(); + if (!links.empty()) + l = links[0]; + } + if (l.empty()) + { + ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str()); + return false; + } + moveit_msgs::AttachedCollisionObject aco; + aco.object.id = object; + aco.link_name.swap(l); + if (touch_links.empty()) + aco.touch_links.push_back(aco.link_name); + else + aco.touch_links = touch_links; + aco.object.operation = moveit_msgs::CollisionObject::ADD; + attached_object_publisher_.publish(aco); + return true; + } + + bool detachObject(const std::string& name) + { + moveit_msgs::AttachedCollisionObject aco; + // if name is a link + if (!name.empty() && joint_model_group_->hasLinkModel(name)) + aco.link_name = name; + else + aco.object.id = name; + aco.object.operation = moveit_msgs::CollisionObject::REMOVE; + if (aco.link_name.empty() && aco.object.id.empty()) + { + // we only want to detach objects for this group + const std::vector& lnames = joint_model_group_->getLinkModelNames(); + for (const std::string& lname : lnames) + { + aco.link_name = lname; + attached_object_publisher_.publish(aco); + } + } + else + attached_object_publisher_.publish(aco); + return true; + } + + double getGoalPositionTolerance() const + { + return goal_position_tolerance_; + } + + double getGoalOrientationTolerance() const + { + return goal_orientation_tolerance_; + } + + double getGoalJointTolerance() const + { + return goal_joint_tolerance_; + } + + void setGoalJointTolerance(double tolerance) + { + goal_joint_tolerance_ = tolerance; + } + + void setGoalPositionTolerance(double tolerance) + { + goal_position_tolerance_ = tolerance; + } + + void setGoalOrientationTolerance(double tolerance) + { + goal_orientation_tolerance_ = tolerance; + } + + void setPlanningTime(double seconds) + { + if (seconds > 0.0) + allowed_planning_time_ = seconds; + } + + double getPlanningTime() const + { + return allowed_planning_time_; + } + + void allowLooking(bool flag) + { + can_look_ = flag; + ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no"); + } + + void allowReplanning(bool flag) + { + can_replan_ = flag; + ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no"); + } + + void setReplanningDelay(double delay) + { + if (delay >= 0.0) + replan_delay_ = delay; + } + + double getReplanningDelay() const + { + return replan_delay_; + } + + void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request) + { + request.group_name = opt_.group_name_; + request.num_planning_attempts = num_planning_attempts_; + request.max_velocity_scaling_factor = max_velocity_scaling_factor_; + request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_; + request.allowed_planning_time = allowed_planning_time_; + request.planner_id = planner_id_; + request.workspace_parameters = workspace_parameters_; + + if (considered_start_state_) + robot_state::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); + else + request.start_state.is_diff = true; + + if (active_target_ == JOINT) + { + request.goal_constraints.resize(1); + request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( + getTargetRobotState(), joint_model_group_, goal_joint_tolerance_); + } + else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION) + { + // find out how many goals are specified + std::size_t goal_count = 0; + for (std::map >::const_iterator it = pose_targets_.begin(); + it != pose_targets_.end(); ++it) + goal_count = std::max(goal_count, it->second.size()); + + // start filling the goals; + // each end effector has a number of possible poses (K) as valid goals + // but there could be multiple end effectors specified, so we want each end effector + // to reach the goal that corresponds to the goals of the other end effectors + request.goal_constraints.resize(goal_count); + + for (std::map >::const_iterator it = pose_targets_.begin(); + it != pose_targets_.end(); ++it) + { + for (std::size_t i = 0; i < it->second.size(); ++i) + { + moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints( + it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_); + if (active_target_ == ORIENTATION) + c.position_constraints.clear(); + if (active_target_ == POSITION) + c.orientation_constraints.clear(); + request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c); + } + } + } + else + ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation"); + + if (path_constraints_) + request.path_constraints = *path_constraints_; + if (trajectory_constraints_) + request.trajectory_constraints = *trajectory_constraints_; + } + + void constructGoal(moveit_msgs::MoveGroupGoal& goal) + { + constructMotionPlanRequest(goal.request); + } + + void constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object) + { + moveit_msgs::PickupGoal goal; + goal.target_name = object; + goal.group_name = opt_.group_name_; + goal.end_effector = getEndEffector(); + goal.allowed_planning_time = allowed_planning_time_; + goal.support_surface_name = support_surface_; + goal.planner_id = planner_id_; + if (!support_surface_.empty()) + goal.allow_gripper_support_collision = true; + + if (path_constraints_) + goal.path_constraints = *path_constraints_; + + goal_out = goal; + } + + void constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object) + { + moveit_msgs::PlaceGoal goal; + goal.attached_object_name = object; + goal.group_name = opt_.group_name_; + goal.allowed_planning_time = allowed_planning_time_; + goal.support_surface_name = support_surface_; + goal.planner_id = planner_id_; + if (!support_surface_.empty()) + goal.allow_gripper_support_collision = true; + + if (path_constraints_) + goal.path_constraints = *path_constraints_; + + goal_out = goal; + } + + void setPathConstraints(const moveit_msgs::Constraints& constraint) + { + path_constraints_.reset(new moveit_msgs::Constraints(constraint)); + } + + bool setPathConstraints(const std::string& constraint) + { + if (constraints_storage_) + { + moveit_warehouse::ConstraintsWithMetadata msg_m; + if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_)) + { + path_constraints_.reset(new moveit_msgs::Constraints(static_cast(*msg_m))); + return true; + } + else + return false; + } + else + return false; + } + + void clearPathConstraints() + { + path_constraints_.reset(); + } + + void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint) + { + trajectory_constraints_.reset(new moveit_msgs::TrajectoryConstraints(constraint)); + } + + void clearTrajectoryConstraints() + { + trajectory_constraints_.reset(); + } + + std::vector getKnownConstraints() const + { + while (initializing_constraints_) + { + static ros::WallDuration d(0.01); + d.sleep(); + } + + std::vector c; + if (constraints_storage_) + constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_); + + return c; + } + + moveit_msgs::Constraints getPathConstraints() const + { + if (path_constraints_) + return *path_constraints_; + else + return moveit_msgs::Constraints(); + } + + moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const + { + if (trajectory_constraints_) + return *trajectory_constraints_; + else + return moveit_msgs::TrajectoryConstraints(); + } + + void initializeConstraintsStorage(const std::string& host, unsigned int port) + { + initializing_constraints_ = true; + if (constraints_init_thread_) + constraints_init_thread_->join(); + constraints_init_thread_.reset( + new boost::thread(boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port))); + } + + void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) + { + workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame(); + workspace_parameters_.header.stamp = ros::Time::now(); + workspace_parameters_.min_corner.x = minx; + workspace_parameters_.min_corner.y = miny; + workspace_parameters_.min_corner.z = minz; + workspace_parameters_.max_corner.x = maxx; + workspace_parameters_.max_corner.y = maxy; + workspace_parameters_.max_corner.z = maxz; + } + +private: + void initializeConstraintsStorageThread(const std::string& host, unsigned int port) + { + // Set up db + try + { + warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase(); + conn->setParams(host, port); + if (conn->connect()) + { + constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(conn)); + } + } + catch (std::exception& ex) + { + ROS_ERROR_NAMED("move_group_interface", "%s", ex.what()); + } + initializing_constraints_ = false; + } + + Options opt_; + ros::NodeHandle node_handle_; + std::shared_ptr tf_buffer_; + robot_model::RobotModelConstPtr robot_model_; + planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; + std::unique_ptr > move_action_client_; + std::unique_ptr > execute_action_client_; + std::unique_ptr > pick_action_client_; + std::unique_ptr > place_action_client_; + + // general planning params + robot_state::RobotStatePtr considered_start_state_; + moveit_msgs::WorkspaceParameters workspace_parameters_; + double allowed_planning_time_; + std::string planner_id_; + unsigned int num_planning_attempts_; + double max_velocity_scaling_factor_; + double max_acceleration_scaling_factor_; + double goal_joint_tolerance_; + double goal_position_tolerance_; + double goal_orientation_tolerance_; + bool can_look_; + bool can_replan_; + double replan_delay_; + + // joint state goal + robot_state::RobotStatePtr joint_state_target_; + const robot_model::JointModelGroup* joint_model_group_; + + // pose goal; + // for each link we have a set of possible goal locations; + std::map > pose_targets_; + + // common properties for goals + ActiveTargetType active_target_; + std::unique_ptr path_constraints_; + std::unique_ptr trajectory_constraints_; + std::string end_effector_link_; + std::string pose_reference_frame_; + std::string support_surface_; + + // ROS communication + ros::Publisher trajectory_event_publisher_; + ros::Publisher attached_object_publisher_; + ros::ServiceClient query_service_; + ros::ServiceClient get_params_service_; + ros::ServiceClient set_params_service_; + ros::ServiceClient cartesian_path_service_; + ros::ServiceClient plan_grasps_service_; + std::unique_ptr constraints_storage_; + std::unique_ptr constraints_init_thread_; + bool initializing_constraints_; +}; +} // namespace planning_interface +} // namespace moveit + +moveit::planning_interface::MoveGroupInterface::MoveGroupInterface(const std::string& group_name, + const std::shared_ptr& tf_buffer, + const ros::WallDuration& wait_for_servers) +{ + if (!ros::ok()) + throw std::runtime_error("ROS does not seem to be running"); + impl_ = new MoveGroupInterfaceImpl(Options(group_name), tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers); +} + +moveit::planning_interface::MoveGroupInterface::MoveGroupInterface(const std::string& group, + const std::shared_ptr& tf_buffer, + const ros::Duration& wait_for_servers) + : MoveGroupInterface(group, tf_buffer, ros::WallDuration(wait_for_servers.toSec())) +{ +} + +moveit::planning_interface::MoveGroupInterface::MoveGroupInterface(const Options& opt, + const std::shared_ptr& tf_buffer, + const ros::WallDuration& wait_for_servers) +{ + impl_ = new MoveGroupInterfaceImpl(opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers); +} + +moveit::planning_interface::MoveGroupInterface::MoveGroupInterface( + const moveit::planning_interface::MoveGroupInterface::Options& opt, + const std::shared_ptr& tf_buffer, const ros::Duration& wait_for_servers) + : MoveGroupInterface(opt, tf_buffer, ros::WallDuration(wait_for_servers.toSec())) +{ +} + +moveit::planning_interface::MoveGroupInterface::~MoveGroupInterface() +{ + delete impl_; +} + +moveit::planning_interface::MoveGroupInterface::MoveGroupInterface(MoveGroupInterface&& other) + : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_) +{ + other.impl_ = nullptr; +} + +moveit::planning_interface::MoveGroupInterface& moveit::planning_interface::MoveGroupInterface:: +operator=(MoveGroupInterface&& other) +{ + if (this != &other) + { + delete impl_; + impl_ = other.impl_; + remembered_joint_values_ = std::move(other.remembered_joint_values_); + other.impl_ = nullptr; + } + + return *this; +} + +const std::string& moveit::planning_interface::MoveGroupInterface::getName() const +{ + return impl_->getOptions().group_name_; +} + +const std::vector moveit::planning_interface::MoveGroupInterface::getNamedTargets() +{ + const robot_model::RobotModelConstPtr& robot = getRobotModel(); + const std::string& group = getName(); + const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group); + + if (joint_group) + { + return joint_group->getDefaultStateNames(); + } + + std::vector empty; + return empty; +} + +robot_model::RobotModelConstPtr moveit::planning_interface::MoveGroupInterface::getRobotModel() const +{ + return impl_->getRobotModel(); +} + +const ros::NodeHandle& moveit::planning_interface::MoveGroupInterface::getNodeHandle() const +{ + return impl_->getOptions().node_handle_; +} + +bool moveit::planning_interface::MoveGroupInterface::getInterfaceDescription( + moveit_msgs::PlannerInterfaceDescription& desc) +{ + return impl_->getInterfaceDescription(desc); +} + +std::map moveit::planning_interface::MoveGroupInterface::getPlannerParams( + const std::string& planner_id, const std::string& group) +{ + return impl_->getPlannerParams(planner_id, group); +} + +void moveit::planning_interface::MoveGroupInterface::setPlannerParams(const std::string& planner_id, + const std::string& group, + const std::map& params, + bool replace) +{ + impl_->setPlannerParams(planner_id, group, params, replace); +} + +std::string moveit::planning_interface::MoveGroupInterface::getDefaultPlannerId(const std::string& group) const +{ + return impl_->getDefaultPlannerId(group); +} + +void moveit::planning_interface::MoveGroupInterface::setPlannerId(const std::string& planner_id) +{ + impl_->setPlannerId(planner_id); +} + +const std::string& moveit::planning_interface::MoveGroupInterface::getPlannerId() const +{ + return impl_->getPlannerId(); +} + +void moveit::planning_interface::MoveGroupInterface::setNumPlanningAttempts(unsigned int num_planning_attempts) +{ + impl_->setNumPlanningAttempts(num_planning_attempts); +} + +void moveit::planning_interface::MoveGroupInterface::setMaxVelocityScalingFactor(double max_velocity_scaling_factor) +{ + impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor); +} + +void moveit::planning_interface::MoveGroupInterface::setMaxAccelerationScalingFactor( + double max_acceleration_scaling_factor) +{ + impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor); +} + +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::asyncMove() +{ + return impl_->move(false); +} + +actionlib::SimpleActionClient& +moveit::planning_interface::MoveGroupInterface::getMoveGroupClient() const +{ + return impl_->getMoveGroupClient(); +} + +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::move() +{ + return impl_->move(true); +} + +moveit::planning_interface::MoveItErrorCode +moveit::planning_interface::MoveGroupInterface::asyncExecute(const Plan& plan) +{ + return impl_->execute(plan, false); +} + +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::execute(const Plan& plan) +{ + return impl_->execute(plan, true); +} + +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::plan(Plan& plan) +{ + return impl_->plan(plan); +} + +moveit::planning_interface::MoveItErrorCode +moveit::planning_interface::MoveGroupInterface::pick(const std::string& object, bool plan_only) +{ + return impl_->pick(object, std::vector(), plan_only); +} + +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::pick( + const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only) +{ + return impl_->pick(object, std::vector(1, grasp), plan_only); +} + +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::pick( + const std::string& object, const std::vector& grasps, bool plan_only) +{ + return impl_->pick(object, grasps, plan_only); +} + +moveit::planning_interface::MoveItErrorCode +moveit::planning_interface::MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only) +{ + return impl_->planGraspsAndPick(object, plan_only); +} + +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::planGraspsAndPick( + const moveit_msgs::CollisionObject& object, bool plan_only) +{ + return impl_->planGraspsAndPick(object, plan_only); +} + +moveit::planning_interface::MoveItErrorCode +moveit::planning_interface::MoveGroupInterface::place(const std::string& object, bool plan_only) +{ + return impl_->place(object, std::vector(), plan_only); +} + +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::place( + const std::string& object, const std::vector& locations, bool plan_only) +{ + return impl_->place(object, locations, plan_only); +} + +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::place( + const std::string& object, const std::vector& poses, bool plan_only) +{ + return impl_->place(object, poses, plan_only); +} + +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::place( + const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only) +{ + return impl_->place(object, std::vector(1, pose), plan_only); +} + +double moveit::planning_interface::MoveGroupInterface::computeCartesianPath( + const std::vector& waypoints, double eef_step, double jump_threshold, + moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code) +{ + moveit_msgs::Constraints path_constraints_tmp; + return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions, + error_code); +} + +double moveit::planning_interface::MoveGroupInterface::computeCartesianPath( + const std::vector& waypoints, double eef_step, double jump_threshold, + moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions, + moveit_msgs::MoveItErrorCodes* error_code) +{ + if (error_code) + { + return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, + avoid_collisions, *error_code); + } + else + { + moveit_msgs::MoveItErrorCodes error_code_tmp; + return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, + avoid_collisions, error_code_tmp); + } +} + +void moveit::planning_interface::MoveGroupInterface::stop() +{ + impl_->stop(); +} + +void moveit::planning_interface::MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state) +{ + robot_state::RobotStatePtr rs; + impl_->getCurrentState(rs); + robot_state::robotStateMsgToRobotState(start_state, *rs); + setStartState(*rs); +} + +void moveit::planning_interface::MoveGroupInterface::setStartState(const robot_state::RobotState& start_state) +{ + impl_->setStartState(start_state); +} + +void moveit::planning_interface::MoveGroupInterface::setStartStateToCurrentState() +{ + impl_->setStartStateToCurrentState(); +} + +void moveit::planning_interface::MoveGroupInterface::setRandomTarget() +{ + impl_->getTargetRobotState().setToRandomPositions(); + impl_->setTargetType(JOINT); +} + +const std::vector& moveit::planning_interface::MoveGroupInterface::getJointNames() +{ + return impl_->getJointModelGroup()->getVariableNames(); +} + +const std::vector& moveit::planning_interface::MoveGroupInterface::getLinkNames() +{ + return impl_->getJointModelGroup()->getLinkModelNames(); +} + +std::map +moveit::planning_interface::MoveGroupInterface::getNamedTargetValues(const std::string& name) +{ + std::map >::const_iterator it = remembered_joint_values_.find(name); + std::map positions; + + if (it != remembered_joint_values_.end()) + { + std::vector names = impl_->getJointModelGroup()->getVariableNames(); + for (size_t x = 0; x < names.size(); ++x) + { + positions[names[x]] = it->second[x]; + } + } + else + { + impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions); + } + return positions; +} + +bool moveit::planning_interface::MoveGroupInterface::setNamedTarget(const std::string& name) +{ + std::map >::const_iterator it = remembered_joint_values_.find(name); + if (it != remembered_joint_values_.end()) + { + return setJointValueTarget(it->second); + } + else + { + if (impl_->getTargetRobotState().setToDefaultValues(impl_->getJointModelGroup(), name)) + { + impl_->setTargetType(JOINT); + return true; + } + ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str()); + return false; + } +} + +void moveit::planning_interface::MoveGroupInterface::getJointValueTarget( + std::vector& group_variable_values) const +{ + impl_->getTargetRobotState().copyJointGroupPositions(impl_->getJointModelGroup(), group_variable_values); +} + +bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector& joint_values) +{ + if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount()) + return false; + impl_->setTargetType(JOINT); + impl_->getTargetRobotState().setJointGroupPositions(impl_->getJointModelGroup(), joint_values); + return impl_->getTargetRobotState().satisfiesBounds(impl_->getJointModelGroup(), impl_->getGoalJointTolerance()); +} + +bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget( + const std::map& variable_values) +{ + const auto& allowed = impl_->getJointModelGroup()->getVariableNames(); + for (const auto& pair : variable_values) + { + if (std::find(allowed.begin(), allowed.end(), pair.first) == allowed.end()) + { + ROS_ERROR_STREAM("joint variable " << pair.first << " is not part of group " + << impl_->getJointModelGroup()->getName()); + return false; + } + } + + impl_->setTargetType(JOINT); + impl_->getTargetRobotState().setVariablePositions(variable_values); + return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance()); +} + +bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector& variable_names, + const std::vector& variable_values) +{ + const auto& allowed = impl_->getJointModelGroup()->getVariableNames(); + for (const auto& variable_name : variable_names) + { + if (std::find(allowed.begin(), allowed.end(), variable_name) == allowed.end()) + { + ROS_ERROR_STREAM("joint variable " << variable_name << " is not part of group " + << impl_->getJointModelGroup()->getName()); + return false; + } + } + + impl_->setTargetType(JOINT); + impl_->getTargetRobotState().setVariablePositions(variable_names, variable_values); + return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance()); +} + +bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const robot_state::RobotState& rstate) +{ + impl_->setTargetType(JOINT); + impl_->getTargetRobotState() = rstate; + return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance()); +} + +bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::string& joint_name, double value) +{ + std::vector values(1, value); + return setJointValueTarget(joint_name, values); +} + +bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::string& joint_name, + const std::vector& values) +{ + impl_->setTargetType(JOINT); + const robot_model::JointModel* jm = impl_->getJointModelGroup()->getJointModel(joint_name); + if (jm && jm->getVariableCount() == values.size()) + { + impl_->getTargetRobotState().setJointPositions(jm, values); + return impl_->getTargetRobotState().satisfiesBounds(jm, impl_->getGoalJointTolerance()); + } + + ROS_ERROR_STREAM("joint " << joint_name << " is not part of group " << impl_->getJointModelGroup()->getName()); + return false; +} + +bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const sensor_msgs::JointState& state) +{ + return setJointValueTarget(state.name, state.position); +} + +bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const geometry_msgs::Pose& eef_pose, + const std::string& end_effector_link) +{ + return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false); +} + +bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, + const std::string& end_effector_link) +{ + return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false); +} + +bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const Eigen::Isometry3d& eef_pose, + const std::string& end_effector_link) +{ + geometry_msgs::Pose msg = tf2::toMsg(eef_pose); + return setJointValueTarget(msg, end_effector_link); +} + +bool moveit::planning_interface::MoveGroupInterface::setApproximateJointValueTarget( + const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link) +{ + return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true); +} + +bool moveit::planning_interface::MoveGroupInterface::setApproximateJointValueTarget( + const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link) +{ + return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true); +} + +bool moveit::planning_interface::MoveGroupInterface::setApproximateJointValueTarget( + const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link) +{ + geometry_msgs::Pose msg = tf2::toMsg(eef_pose); + return setApproximateJointValueTarget(msg, end_effector_link); +} + +const robot_state::RobotState& moveit::planning_interface::MoveGroupInterface::getJointValueTarget() const +{ + return impl_->getTargetRobotState(); +} + +const robot_state::RobotState& moveit::planning_interface::MoveGroupInterface::getTargetRobotState() const +{ + return impl_->getTargetRobotState(); +} + +const std::string& moveit::planning_interface::MoveGroupInterface::getEndEffectorLink() const +{ + return impl_->getEndEffectorLink(); +} + +const std::string& moveit::planning_interface::MoveGroupInterface::getEndEffector() const +{ + return impl_->getEndEffector(); +} + +bool moveit::planning_interface::MoveGroupInterface::setEndEffectorLink(const std::string& link_name) +{ + if (impl_->getEndEffectorLink().empty() || link_name.empty()) + return false; + impl_->setEndEffectorLink(link_name); + impl_->setTargetType(POSE); + return true; +} + +bool moveit::planning_interface::MoveGroupInterface::setEndEffector(const std::string& eef_name) +{ + const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name); + if (jmg) + return setEndEffectorLink(jmg->getEndEffectorParentGroup().second); + return false; +} + +void moveit::planning_interface::MoveGroupInterface::clearPoseTarget(const std::string& end_effector_link) +{ + impl_->clearPoseTarget(end_effector_link); +} + +void moveit::planning_interface::MoveGroupInterface::clearPoseTargets() +{ + impl_->clearPoseTargets(); +} + +bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const Eigen::Isometry3d& pose, + const std::string& end_effector_link) +{ + std::vector pose_msg(1); + pose_msg[0].pose = tf2::toMsg(pose); + pose_msg[0].header.frame_id = getPoseReferenceFrame(); + pose_msg[0].header.stamp = ros::Time::now(); + return setPoseTargets(pose_msg, end_effector_link); +} + +bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const geometry_msgs::Pose& target, + const std::string& end_effector_link) +{ + std::vector pose_msg(1); + pose_msg[0].pose = target; + pose_msg[0].header.frame_id = getPoseReferenceFrame(); + pose_msg[0].header.stamp = ros::Time::now(); + return setPoseTargets(pose_msg, end_effector_link); +} + +bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const geometry_msgs::PoseStamped& target, + const std::string& end_effector_link) +{ + std::vector targets(1, target); + return setPoseTargets(targets, end_effector_link); +} + +bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const EigenSTL::vector_Isometry3d& target, + const std::string& end_effector_link) +{ + std::vector pose_out(target.size()); + ros::Time tm = ros::Time::now(); + const std::string& frame_id = getPoseReferenceFrame(); + for (std::size_t i = 0; i < target.size(); ++i) + { + pose_out[i].pose = tf2::toMsg(target[i]); + pose_out[i].header.stamp = tm; + pose_out[i].header.frame_id = frame_id; + } + return setPoseTargets(pose_out, end_effector_link); +} + +bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const std::vector& target, + const std::string& end_effector_link) +{ + std::vector target_stamped(target.size()); + ros::Time tm = ros::Time::now(); + const std::string& frame_id = getPoseReferenceFrame(); + for (std::size_t i = 0; i < target.size(); ++i) + { + target_stamped[i].pose = target[i]; + target_stamped[i].header.stamp = tm; + target_stamped[i].header.frame_id = frame_id; + } + return setPoseTargets(target_stamped, end_effector_link); +} + +bool moveit::planning_interface::MoveGroupInterface::setPoseTargets( + const std::vector& target, const std::string& end_effector_link) +{ + if (target.empty()) + { + ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target"); + return false; + } + else + { + impl_->setTargetType(POSE); + return impl_->setPoseTargets(target, end_effector_link); + } +} + +const geometry_msgs::PoseStamped& +moveit::planning_interface::MoveGroupInterface::getPoseTarget(const std::string& end_effector_link) const +{ + return impl_->getPoseTarget(end_effector_link); +} + +const std::vector& +moveit::planning_interface::MoveGroupInterface::getPoseTargets(const std::string& end_effector_link) const +{ + return impl_->getPoseTargets(end_effector_link); +} + +namespace +{ +inline void transformPose(const tf2_ros::Buffer& tf_buffer, const std::string& desired_frame, + geometry_msgs::PoseStamped& target) +{ + if (desired_frame != target.header.frame_id) + { + geometry_msgs::PoseStamped target_in(target); + tf_buffer.transform(target_in, target, desired_frame); + // we leave the stamp to ros::Time(0) on purpose + target.header.stamp = ros::Time(0); + } +} +} // namespace + +bool moveit::planning_interface::MoveGroupInterface::setPositionTarget(double x, double y, double z, + const std::string& end_effector_link) +{ + geometry_msgs::PoseStamped target; + if (impl_->hasPoseTarget(end_effector_link)) + { + target = getPoseTarget(end_effector_link); + transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target); + } + else + { + target.pose.orientation.x = 0.0; + target.pose.orientation.y = 0.0; + target.pose.orientation.z = 0.0; + target.pose.orientation.w = 1.0; + target.header.frame_id = impl_->getPoseReferenceFrame(); + } + + target.pose.position.x = x; + target.pose.position.y = y; + target.pose.position.z = z; + bool result = setPoseTarget(target, end_effector_link); + impl_->setTargetType(POSITION); + return result; +} + +bool moveit::planning_interface::MoveGroupInterface::setRPYTarget(double r, double p, double y, + const std::string& end_effector_link) +{ + geometry_msgs::PoseStamped target; + if (impl_->hasPoseTarget(end_effector_link)) + { + target = getPoseTarget(end_effector_link); + transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target); + } + else + { + target.pose.position.x = 0.0; + target.pose.position.y = 0.0; + target.pose.position.z = 0.0; + target.header.frame_id = impl_->getPoseReferenceFrame(); + } + tf2::Quaternion q; + q.setRPY(r, p, y); + target.pose.orientation = tf2::toMsg(q); + bool result = setPoseTarget(target, end_effector_link); + impl_->setTargetType(ORIENTATION); + return result; +} + +bool moveit::planning_interface::MoveGroupInterface::setOrientationTarget(double x, double y, double z, double w, + const std::string& end_effector_link) +{ + geometry_msgs::PoseStamped target; + if (impl_->hasPoseTarget(end_effector_link)) + { + target = getPoseTarget(end_effector_link); + transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target); + } + else + { + target.pose.position.x = 0.0; + target.pose.position.y = 0.0; + target.pose.position.z = 0.0; + target.header.frame_id = impl_->getPoseReferenceFrame(); + } + + target.pose.orientation.x = x; + target.pose.orientation.y = y; + target.pose.orientation.z = z; + target.pose.orientation.w = w; + bool result = setPoseTarget(target, end_effector_link); + impl_->setTargetType(ORIENTATION); + return result; +} + +void moveit::planning_interface::MoveGroupInterface::setPoseReferenceFrame(const std::string& pose_reference_frame) +{ + impl_->setPoseReferenceFrame(pose_reference_frame); +} + +const std::string& moveit::planning_interface::MoveGroupInterface::getPoseReferenceFrame() const +{ + return impl_->getPoseReferenceFrame(); +} + +double moveit::planning_interface::MoveGroupInterface::getGoalJointTolerance() const +{ + return impl_->getGoalJointTolerance(); +} + +double moveit::planning_interface::MoveGroupInterface::getGoalPositionTolerance() const +{ + return impl_->getGoalPositionTolerance(); +} + +double moveit::planning_interface::MoveGroupInterface::getGoalOrientationTolerance() const +{ + return impl_->getGoalOrientationTolerance(); +} + +void moveit::planning_interface::MoveGroupInterface::setGoalTolerance(double tolerance) +{ + setGoalJointTolerance(tolerance); + setGoalPositionTolerance(tolerance); + setGoalOrientationTolerance(tolerance); +} + +void moveit::planning_interface::MoveGroupInterface::setGoalJointTolerance(double tolerance) +{ + impl_->setGoalJointTolerance(tolerance); +} + +void moveit::planning_interface::MoveGroupInterface::setGoalPositionTolerance(double tolerance) +{ + impl_->setGoalPositionTolerance(tolerance); +} + +void moveit::planning_interface::MoveGroupInterface::setGoalOrientationTolerance(double tolerance) +{ + impl_->setGoalOrientationTolerance(tolerance); +} + +void moveit::planning_interface::MoveGroupInterface::rememberJointValues(const std::string& name) +{ + rememberJointValues(name, getCurrentJointValues()); +} + +bool moveit::planning_interface::MoveGroupInterface::startStateMonitor(double wait) +{ + return impl_->startStateMonitor(wait); +} + +std::vector moveit::planning_interface::MoveGroupInterface::getCurrentJointValues() +{ + robot_state::RobotStatePtr current_state; + std::vector values; + if (impl_->getCurrentState(current_state)) + current_state->copyJointGroupPositions(getName(), values); + return values; +} + +std::vector moveit::planning_interface::MoveGroupInterface::getRandomJointValues() +{ + std::vector r; + impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getTargetRobotState().getRandomNumberGenerator(), r); + return r; +} + +geometry_msgs::PoseStamped +moveit::planning_interface::MoveGroupInterface::getRandomPose(const std::string& end_effector_link) +{ + const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; + Eigen::Isometry3d pose; + pose.setIdentity(); + if (eef.empty()) + ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); + else + { + robot_state::RobotStatePtr current_state; + if (impl_->getCurrentState(current_state)) + { + current_state->setToRandomPositions(impl_->getJointModelGroup()); + const robot_model::LinkModel* lm = current_state->getLinkModel(eef); + if (lm) + pose = current_state->getGlobalLinkTransform(lm); + } + } + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.stamp = ros::Time::now(); + pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame(); + pose_msg.pose = tf2::toMsg(pose); + return pose_msg; +} + +geometry_msgs::PoseStamped +moveit::planning_interface::MoveGroupInterface::getCurrentPose(const std::string& end_effector_link) +{ + const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; + Eigen::Isometry3d pose; + pose.setIdentity(); + if (eef.empty()) + ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); + else + { + robot_state::RobotStatePtr current_state; + if (impl_->getCurrentState(current_state)) + { + const robot_model::LinkModel* lm = current_state->getLinkModel(eef); + if (lm) + pose = current_state->getGlobalLinkTransform(lm); + } + } + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.stamp = ros::Time::now(); + pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame(); + pose_msg.pose = tf2::toMsg(pose); + return pose_msg; +} + +std::vector moveit::planning_interface::MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link) +{ + std::vector result; + const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; + if (eef.empty()) + ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); + else + { + robot_state::RobotStatePtr current_state; + if (impl_->getCurrentState(current_state)) + { + const robot_model::LinkModel* lm = current_state->getLinkModel(eef); + if (lm) + { + result.resize(3); + geometry_msgs::TransformStamped tfs = tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm)); + double pitch, roll, yaw; + tf2::getEulerYPR(tfs.transform.rotation, yaw, pitch, roll); + result[0] = roll; + result[1] = pitch; + result[2] = yaw; + } + } + } + return result; +} + +const std::vector& moveit::planning_interface::MoveGroupInterface::getActiveJoints() const +{ + return impl_->getJointModelGroup()->getActiveJointModelNames(); +} + +const std::vector& moveit::planning_interface::MoveGroupInterface::getJoints() const +{ + return impl_->getJointModelGroup()->getJointModelNames(); +} + +unsigned int moveit::planning_interface::MoveGroupInterface::getVariableCount() const +{ + return impl_->getJointModelGroup()->getVariableCount(); +} + +robot_state::RobotStatePtr moveit::planning_interface::MoveGroupInterface::getCurrentState(double wait) +{ + robot_state::RobotStatePtr current_state; + impl_->getCurrentState(current_state, wait); + return current_state; +} + +void moveit::planning_interface::MoveGroupInterface::rememberJointValues(const std::string& name, + const std::vector& values) +{ + remembered_joint_values_[name] = values; +} + +void moveit::planning_interface::MoveGroupInterface::forgetJointValues(const std::string& name) +{ + remembered_joint_values_.erase(name); +} + +void moveit::planning_interface::MoveGroupInterface::allowLooking(bool flag) +{ + impl_->allowLooking(flag); +} + +void moveit::planning_interface::MoveGroupInterface::allowReplanning(bool flag) +{ + impl_->allowReplanning(flag); +} + +std::vector moveit::planning_interface::MoveGroupInterface::getKnownConstraints() const +{ + return impl_->getKnownConstraints(); +} + +moveit_msgs::Constraints moveit::planning_interface::MoveGroupInterface::getPathConstraints() const +{ + return impl_->getPathConstraints(); +} + +bool moveit::planning_interface::MoveGroupInterface::setPathConstraints(const std::string& constraint) +{ + return impl_->setPathConstraints(constraint); +} + +void moveit::planning_interface::MoveGroupInterface::setPathConstraints(const moveit_msgs::Constraints& constraint) +{ + impl_->setPathConstraints(constraint); +} + +void moveit::planning_interface::MoveGroupInterface::clearPathConstraints() +{ + impl_->clearPathConstraints(); +} + +moveit_msgs::TrajectoryConstraints moveit::planning_interface::MoveGroupInterface::getTrajectoryConstraints() const +{ + return impl_->getTrajectoryConstraints(); +} + +void moveit::planning_interface::MoveGroupInterface::setTrajectoryConstraints( + const moveit_msgs::TrajectoryConstraints& constraint) +{ + impl_->setTrajectoryConstraints(constraint); +} + +void moveit::planning_interface::MoveGroupInterface::clearTrajectoryConstraints() +{ + impl_->clearTrajectoryConstraints(); +} + +void moveit::planning_interface::MoveGroupInterface::setConstraintsDatabase(const std::string& host, unsigned int port) +{ + impl_->initializeConstraintsStorage(host, port); +} + +void moveit::planning_interface::MoveGroupInterface::setWorkspace(double minx, double miny, double minz, double maxx, + double maxy, double maxz) +{ + impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz); +} + +/** \brief Set time allowed to planner to solve problem before aborting */ +void moveit::planning_interface::MoveGroupInterface::setPlanningTime(double seconds) +{ + impl_->setPlanningTime(seconds); +} + +/** \brief Get time allowed to planner to solve problem before aborting */ +double moveit::planning_interface::MoveGroupInterface::getPlanningTime() const +{ + return impl_->getPlanningTime(); +} + +void moveit::planning_interface::MoveGroupInterface::setSupportSurfaceName(const std::string& name) +{ + impl_->setSupportSurfaceName(name); +} + +const std::string& moveit::planning_interface::MoveGroupInterface::getPlanningFrame() const +{ + return impl_->getRobotModel()->getModelFrame(); +} + +const std::vector& moveit::planning_interface::MoveGroupInterface::getJointModelGroupNames() const +{ + return impl_->getRobotModel()->getJointModelGroupNames(); +} + +bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link) +{ + return attachObject(object, link, std::vector()); +} + +bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link, + const std::vector& touch_links) +{ + return impl_->attachObject(object, link, touch_links); +} + +bool moveit::planning_interface::MoveGroupInterface::detachObject(const std::string& name) +{ + return impl_->detachObject(name); +} + +void moveit::planning_interface::MoveGroupInterface::constructMotionPlanRequest( + moveit_msgs::MotionPlanRequest& goal_out) +{ + impl_->constructMotionPlanRequest(goal_out); +} diff --git a/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_move_group.cpp new file mode 100644 index 0000000000..36fcc36422 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_move_group.cpp @@ -0,0 +1,662 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Ioan Sucan */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +/** @cond IGNORE */ + +namespace bp = boost::python; + +namespace moveit +{ +namespace planning_interface +{ +class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer, public MoveGroupInterface +{ +public: + // ROSInitializer is constructed first, and ensures ros::init() was called, if + // needed + MoveGroupInterfaceWrapper(const std::string& group_name, const std::string& robot_description, + const std::string& ns = "", double wait_for_servers = 5.0) + : py_bindings_tools::ROScppInitializer() + , MoveGroupInterface(Options(group_name, robot_description, ros::NodeHandle(ns)), + std::shared_ptr(), ros::WallDuration(wait_for_servers)) + { + } + + bool setJointValueTargetPerJointPythonList(const std::string& joint, bp::list& values) + { + return setJointValueTarget(joint, py_bindings_tools::doubleFromList(values)); + } + + bool setJointValueTargetPythonIterable(bp::object& values) + { + return setJointValueTarget(py_bindings_tools::doubleFromList(values)); + } + + bool setJointValueTargetPythonDict(bp::dict& values) + { + bp::list k = values.keys(); + int l = bp::len(k); + std::map v; + for (int i = 0; i < l; ++i) + v[bp::extract(k[i])] = bp::extract(values[k[i]]); + return setJointValueTarget(v); + } + + bool setJointValueTargetFromPosePython(const std::string& pose_str, const std::string& eef, bool approx) + { + geometry_msgs::Pose pose_msg; + py_bindings_tools::deserializeMsg(pose_str, pose_msg); + return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef); + } + + bool setJointValueTargetFromPoseStampedPython(const std::string& pose_str, const std::string& eef, bool approx) + { + geometry_msgs::PoseStamped pose_msg; + py_bindings_tools::deserializeMsg(pose_str, pose_msg); + return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef); + } + + bool setJointValueTargetFromJointStatePython(const std::string& js_str) + { + sensor_msgs::JointState js_msg; + py_bindings_tools::deserializeMsg(js_str, js_msg); + return setJointValueTarget(js_msg); + } + + bp::list getJointValueTargetPythonList() + { + std::vector values; + MoveGroupInterface::getJointValueTarget(values); + bp::list l; + for (const double value : values) + l.append(value); + return l; + } + + std::string getJointValueTarget() + { + moveit_msgs::RobotState msg; + const robot_state::RobotState state = moveit::planning_interface::MoveGroupInterface::getTargetRobotState(); + moveit::core::robotStateToRobotStateMsg(state, msg); + return py_bindings_tools::serializeMsg(msg); + } + + void rememberJointValuesFromPythonList(const std::string& string, bp::list& values) + { + rememberJointValues(string, py_bindings_tools::doubleFromList(values)); + } + + const char* getPlanningFrameCStr() const + { + return getPlanningFrame().c_str(); + } + + std::string getInterfaceDescriptionPython() + { + moveit_msgs::PlannerInterfaceDescription msg; + getInterfaceDescription(msg); + return py_bindings_tools::serializeMsg(msg); + } + + bp::list getActiveJointsList() const + { + return py_bindings_tools::listFromString(getActiveJoints()); + } + + bp::list getJointsList() const + { + return py_bindings_tools::listFromString(getJoints()); + } + + bp::list getCurrentJointValuesList() + { + return py_bindings_tools::listFromDouble(getCurrentJointValues()); + } + + bp::list getRandomJointValuesList() + { + return py_bindings_tools::listFromDouble(getRandomJointValues()); + } + + bp::dict getRememberedJointValuesPython() const + { + const std::map>& rv = getRememberedJointValues(); + bp::dict d; + for (const std::pair>& it : rv) + d[it.first] = py_bindings_tools::listFromDouble(it.second); + return d; + } + + bp::list convertPoseToList(const geometry_msgs::Pose& pose) const + { + std::vector v(7); + v[0] = pose.position.x; + v[1] = pose.position.y; + v[2] = pose.position.z; + v[3] = pose.orientation.x; + v[4] = pose.orientation.y; + v[5] = pose.orientation.z; + v[6] = pose.orientation.w; + return moveit::py_bindings_tools::listFromDouble(v); + } + + bp::list convertTransformToList(const geometry_msgs::Transform& tr) const + { + std::vector v(7); + v[0] = tr.translation.x; + v[1] = tr.translation.y; + v[2] = tr.translation.z; + v[3] = tr.rotation.x; + v[4] = tr.rotation.y; + v[5] = tr.rotation.z; + v[6] = tr.rotation.w; + return py_bindings_tools::listFromDouble(v); + } + + void convertListToTransform(const bp::list& l, geometry_msgs::Transform& tr) const + { + std::vector v = py_bindings_tools::doubleFromList(l); + tr.translation.x = v[0]; + tr.translation.y = v[1]; + tr.translation.z = v[2]; + tr.rotation.x = v[3]; + tr.rotation.y = v[4]; + tr.rotation.z = v[5]; + tr.rotation.w = v[6]; + } + + void convertListToPose(const bp::list& l, geometry_msgs::Pose& p) const + { + std::vector v = py_bindings_tools::doubleFromList(l); + p.position.x = v[0]; + p.position.y = v[1]; + p.position.z = v[2]; + p.orientation.x = v[3]; + p.orientation.y = v[4]; + p.orientation.z = v[5]; + p.orientation.w = v[6]; + } + + bp::list getCurrentRPYPython(const std::string& end_effector_link = "") + { + return py_bindings_tools::listFromDouble(getCurrentRPY(end_effector_link)); + } + + bp::list getCurrentPosePython(const std::string& end_effector_link = "") + { + geometry_msgs::PoseStamped pose = getCurrentPose(end_effector_link); + return convertPoseToList(pose.pose); + } + + bp::list getRandomPosePython(const std::string& end_effector_link = "") + { + geometry_msgs::PoseStamped pose = getRandomPose(end_effector_link); + return convertPoseToList(pose.pose); + } + + bp::list getKnownConstraintsList() const + { + return py_bindings_tools::listFromString(getKnownConstraints()); + } + + bool placePose(const std::string& object_name, const bp::list& pose, bool plan_only = false) + { + geometry_msgs::PoseStamped msg; + convertListToPose(pose, msg.pose); + msg.header.frame_id = getPoseReferenceFrame(); + msg.header.stamp = ros::Time::now(); + return place(object_name, msg, plan_only) == MoveItErrorCode::SUCCESS; + } + + bool placeLocation(const std::string& object_name, const std::string& location_str, bool plan_only = false) + { + std::vector locations(1); + py_bindings_tools::deserializeMsg(location_str, locations[0]); + return place(object_name, locations, plan_only) == MoveItErrorCode::SUCCESS; + } + + bool placeAnywhere(const std::string& object_name, bool plan_only = false) + { + return place(object_name, plan_only) == MoveItErrorCode::SUCCESS; + } + + void convertListToArrayOfPoses(const bp::list& poses, std::vector& msg) + { + int l = bp::len(poses); + for (int i = 0; i < l; ++i) + { + const bp::list& pose = bp::extract(poses[i]); + std::vector v = py_bindings_tools::doubleFromList(pose); + if (v.size() == 6 || v.size() == 7) + { + Eigen::Isometry3d p; + if (v.size() == 6) + { + tf2::Quaternion tq; + tq.setRPY(v[3], v[4], v[5]); + Eigen::Quaterniond eq; + tf2::convert(tq, eq); + p = Eigen::Isometry3d(eq); + } + else + p = Eigen::Isometry3d(Eigen::Quaterniond(v[6], v[3], v[4], v[5])); + p.translation() = Eigen::Vector3d(v[0], v[1], v[2]); + geometry_msgs::Pose pm = tf2::toMsg(p); + msg.push_back(pm); + } + else + ROS_WARN("Incorrect number of values for a pose: %u", (unsigned int)v.size()); + } + } + + bp::dict getCurrentStateBoundedPython() + { + robot_state::RobotStatePtr current = getCurrentState(); + current->enforceBounds(); + moveit_msgs::RobotState rsmv; + robot_state::robotStateToRobotStateMsg(*current, rsmv); + bp::dict output; + for (size_t x = 0; x < rsmv.joint_state.name.size(); ++x) + output[rsmv.joint_state.name[x]] = rsmv.joint_state.position[x]; + return output; + } + + void setStartStatePython(const std::string& msg_str) + { + moveit_msgs::RobotState msg; + py_bindings_tools::deserializeMsg(msg_str, msg); + setStartState(msg); + } + + bool setPoseTargetsPython(bp::list& poses, const std::string& end_effector_link = "") + { + std::vector msg; + convertListToArrayOfPoses(poses, msg); + return setPoseTargets(msg, end_effector_link); + } + std::string getPoseTargetPython(const std::string& end_effector_link) + { + geometry_msgs::PoseStamped pose = moveit::planning_interface::MoveGroupInterface::getPoseTarget(end_effector_link); + return py_bindings_tools::serializeMsg(pose); + } + + bool setPoseTargetPython(bp::list& pose, const std::string& end_effector_link = "") + { + std::vector v = py_bindings_tools::doubleFromList(pose); + geometry_msgs::Pose msg; + if (v.size() == 6) + { + tf2::Quaternion q; + q.setRPY(v[3], v[4], v[5]); + tf2::convert(q, msg.orientation); + } + else if (v.size() == 7) + { + msg.orientation.x = v[3]; + msg.orientation.y = v[4]; + msg.orientation.z = v[5]; + msg.orientation.w = v[6]; + } + else + { + ROS_ERROR("Pose description expected to consist of either 6 or 7 values"); + return false; + } + msg.position.x = v[0]; + msg.position.y = v[1]; + msg.position.z = v[2]; + return setPoseTarget(msg, end_effector_link); + } + + const char* getEndEffectorLinkCStr() const + { + return getEndEffectorLink().c_str(); + } + + const char* getPoseReferenceFrameCStr() const + { + return getPoseReferenceFrame().c_str(); + } + + const char* getNameCStr() const + { + return getName().c_str(); + } + + bp::dict getNamedTargetValuesPython(const std::string& name) + { + bp::dict output; + std::map positions = getNamedTargetValues(name); + std::map::iterator iterator; + + for (iterator = positions.begin(); iterator != positions.end(); ++iterator) + output[iterator->first] = iterator->second; + return output; + } + + bp::list getNamedTargetsPython() + { + return py_bindings_tools::listFromString(getNamedTargets()); + } + + bool movePython() + { + return move() == MoveItErrorCode::SUCCESS; + } + + bool asyncMovePython() + { + return asyncMove() == MoveItErrorCode::SUCCESS; + } + + bool attachObjectPython(const std::string& object_name, const std::string& link_name, const bp::list& touch_links) + { + return attachObject(object_name, link_name, py_bindings_tools::stringFromList(touch_links)); + } + + bool executePython(const std::string& plan_str) + { + MoveGroupInterface::Plan plan; + py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); + return execute(plan) == MoveItErrorCode::SUCCESS; + } + + bool asyncExecutePython(const std::string& plan_str) + { + MoveGroupInterface::Plan plan; + py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); + return asyncExecute(plan) == MoveItErrorCode::SUCCESS; + } + + bp::tuple planPython() + { + MoveGroupInterface::Plan plan; + moveit_msgs::MoveItErrorCodes res = MoveGroupInterface::plan(plan); + return bp::make_tuple(py_bindings_tools::serializeMsg(res), py_bindings_tools::serializeMsg(plan.trajectory_), + plan.planning_time_); + } + + bp::tuple computeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, + bool avoid_collisions) + { + moveit_msgs::Constraints path_constraints_tmp; + return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints_tmp); + } + + bp::tuple computeCartesianPathConstrainedPython(const bp::list& waypoints, double eef_step, double jump_threshold, + bool avoid_collisions, const std::string& path_constraints_str) + { + moveit_msgs::Constraints path_constraints; + py_bindings_tools::deserializeMsg(path_constraints_str, path_constraints); + return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints); + } + + bp::tuple doComputeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, + bool avoid_collisions, const moveit_msgs::Constraints& path_constraints) + { + std::vector poses; + convertListToArrayOfPoses(waypoints, poses); + moveit_msgs::RobotTrajectory trajectory; + double fraction = + computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions); + return bp::make_tuple(py_bindings_tools::serializeMsg(trajectory), fraction); + } + + int pickGrasp(const std::string& object, const std::string& grasp_str, bool plan_only = false) + { + moveit_msgs::Grasp grasp; + py_bindings_tools::deserializeMsg(grasp_str, grasp); + return pick(object, grasp, plan_only).val; + } + + int pickGrasps(const std::string& object, const bp::list& grasp_list, bool plan_only = false) + { + int l = bp::len(grasp_list); + std::vector grasps(l); + for (int i = 0; i < l; ++i) + py_bindings_tools::deserializeMsg(bp::extract(grasp_list[i]), grasps[i]); + return pick(object, grasps, plan_only).val; + } + + void setPathConstraintsFromMsg(const std::string& constraints_str) + { + moveit_msgs::Constraints constraints_msg; + py_bindings_tools::deserializeMsg(constraints_str, constraints_msg); + setPathConstraints(constraints_msg); + } + + std::string getPathConstraintsPython() + { + moveit_msgs::Constraints constraints_msg(getPathConstraints()); + std::string constraints_str = py_bindings_tools::serializeMsg(constraints_msg); + return constraints_str; + } + + std::string retimeTrajectory(const std::string& ref_state_str, const std::string& traj_str, + double velocity_scaling_factor) + { + // Convert reference state message to object + moveit_msgs::RobotState ref_state_msg; + py_bindings_tools::deserializeMsg(ref_state_str, ref_state_msg); + moveit::core::RobotState ref_state_obj(getRobotModel()); + if (moveit::core::robotStateMsgToRobotState(ref_state_msg, ref_state_obj, true)) + { + // Convert trajectory message to object + moveit_msgs::RobotTrajectory traj_msg; + py_bindings_tools::deserializeMsg(traj_str, traj_msg); + robot_trajectory::RobotTrajectory traj_obj(getRobotModel(), getName()); + traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg); + + // Do the actual retiming + trajectory_processing::IterativeParabolicTimeParameterization time_param; + time_param.computeTimeStamps(traj_obj, velocity_scaling_factor); + + // Convert the retimed trajectory back into a message + traj_obj.getRobotTrajectoryMsg(traj_msg); + std::string traj_str = py_bindings_tools::serializeMsg(traj_msg); + + // Return it. + return traj_str; + } + else + { + ROS_ERROR("Unable to convert RobotState message to RobotState instance."); + return ""; + } + } +}; + +static void wrap_move_group_interface() +{ + bp::class_ move_group_interface_class( + "MoveGroupInterface", bp::init>()); + + move_group_interface_class.def("async_move", &MoveGroupInterfaceWrapper::asyncMovePython); + move_group_interface_class.def("move", &MoveGroupInterfaceWrapper::movePython); + move_group_interface_class.def("execute", &MoveGroupInterfaceWrapper::executePython); + move_group_interface_class.def("async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython); + moveit::planning_interface::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) = + &MoveGroupInterfaceWrapper::pick; + move_group_interface_class.def("pick", pick_1); + move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasp); + move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasps); + move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placePose); + move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeLocation); + move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeAnywhere); + move_group_interface_class.def("stop", &MoveGroupInterfaceWrapper::stop); + + move_group_interface_class.def("get_name", &MoveGroupInterfaceWrapper::getNameCStr); + move_group_interface_class.def("get_planning_frame", &MoveGroupInterfaceWrapper::getPlanningFrameCStr); + move_group_interface_class.def("get_interface_description", + &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython); + + move_group_interface_class.def("get_active_joints", &MoveGroupInterfaceWrapper::getActiveJointsList); + move_group_interface_class.def("get_joints", &MoveGroupInterfaceWrapper::getJointsList); + move_group_interface_class.def("get_variable_count", &MoveGroupInterfaceWrapper::getVariableCount); + move_group_interface_class.def("allow_looking", &MoveGroupInterfaceWrapper::allowLooking); + move_group_interface_class.def("allow_replanning", &MoveGroupInterfaceWrapper::allowReplanning); + + move_group_interface_class.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame); + + move_group_interface_class.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame); + move_group_interface_class.def("set_end_effector_link", &MoveGroupInterfaceWrapper::setEndEffectorLink); + move_group_interface_class.def("get_end_effector_link", &MoveGroupInterfaceWrapper::getEndEffectorLinkCStr); + move_group_interface_class.def("get_pose_reference_frame", &MoveGroupInterfaceWrapper::getPoseReferenceFrameCStr); + + move_group_interface_class.def("set_pose_target", &MoveGroupInterfaceWrapper::setPoseTargetPython); + + move_group_interface_class.def("set_pose_targets", &MoveGroupInterfaceWrapper::setPoseTargetsPython); + + move_group_interface_class.def("set_position_target", &MoveGroupInterfaceWrapper::setPositionTarget); + move_group_interface_class.def("set_rpy_target", &MoveGroupInterfaceWrapper::setRPYTarget); + move_group_interface_class.def("set_orientation_target", &MoveGroupInterfaceWrapper::setOrientationTarget); + + move_group_interface_class.def("get_current_pose", &MoveGroupInterfaceWrapper::getCurrentPosePython); + move_group_interface_class.def("get_current_rpy", &MoveGroupInterfaceWrapper::getCurrentRPYPython); + + move_group_interface_class.def("get_random_pose", &MoveGroupInterfaceWrapper::getRandomPosePython); + + move_group_interface_class.def("clear_pose_target", &MoveGroupInterfaceWrapper::clearPoseTarget); + move_group_interface_class.def("clear_pose_targets", &MoveGroupInterfaceWrapper::clearPoseTargets); + + move_group_interface_class.def("set_joint_value_target", + &MoveGroupInterfaceWrapper::setJointValueTargetPythonIterable); + move_group_interface_class.def("set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonDict); + + move_group_interface_class.def("set_joint_value_target", + &MoveGroupInterfaceWrapper::setJointValueTargetPerJointPythonList); + bool (MoveGroupInterfaceWrapper::*set_joint_value_target_4)(const std::string&, double) = + &MoveGroupInterfaceWrapper::setJointValueTarget; + move_group_interface_class.def("set_joint_value_target", set_joint_value_target_4); + + move_group_interface_class.def("set_joint_value_target_from_pose", + &MoveGroupInterfaceWrapper::setJointValueTargetFromPosePython); + move_group_interface_class.def("set_joint_value_target_from_pose_stamped", + &MoveGroupInterfaceWrapper::setJointValueTargetFromPoseStampedPython); + move_group_interface_class.def("set_joint_value_target_from_joint_state_message", + &MoveGroupInterfaceWrapper::setJointValueTargetFromJointStatePython); + + move_group_interface_class.def("get_joint_value_target", &MoveGroupInterfaceWrapper::getJointValueTargetPythonList); + + move_group_interface_class.def("set_named_target", &MoveGroupInterfaceWrapper::setNamedTarget); + move_group_interface_class.def("set_random_target", &MoveGroupInterfaceWrapper::setRandomTarget); + + void (MoveGroupInterfaceWrapper::*remember_joint_values_2)(const std::string&) = + &MoveGroupInterfaceWrapper::rememberJointValues; + move_group_interface_class.def("remember_joint_values", remember_joint_values_2); + + move_group_interface_class.def("remember_joint_values", + &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList); + + move_group_interface_class.def("start_state_monitor", &MoveGroupInterfaceWrapper::startStateMonitor); + move_group_interface_class.def("get_current_joint_values", &MoveGroupInterfaceWrapper::getCurrentJointValuesList); + move_group_interface_class.def("get_random_joint_values", &MoveGroupInterfaceWrapper::getRandomJointValuesList); + move_group_interface_class.def("get_remembered_joint_values", + &MoveGroupInterfaceWrapper::getRememberedJointValuesPython); + + move_group_interface_class.def("forget_joint_values", &MoveGroupInterfaceWrapper::forgetJointValues); + + move_group_interface_class.def("get_goal_joint_tolerance", &MoveGroupInterfaceWrapper::getGoalJointTolerance); + move_group_interface_class.def("get_goal_position_tolerance", &MoveGroupInterfaceWrapper::getGoalPositionTolerance); + move_group_interface_class.def("get_goal_orientation_tolerance", + &MoveGroupInterfaceWrapper::getGoalOrientationTolerance); + + move_group_interface_class.def("set_goal_joint_tolerance", &MoveGroupInterfaceWrapper::setGoalJointTolerance); + move_group_interface_class.def("set_goal_position_tolerance", &MoveGroupInterfaceWrapper::setGoalPositionTolerance); + move_group_interface_class.def("set_goal_orientation_tolerance", + &MoveGroupInterfaceWrapper::setGoalOrientationTolerance); + move_group_interface_class.def("set_goal_tolerance", &MoveGroupInterfaceWrapper::setGoalTolerance); + + move_group_interface_class.def("set_start_state_to_current_state", + &MoveGroupInterfaceWrapper::setStartStateToCurrentState); + move_group_interface_class.def("set_start_state", &MoveGroupInterfaceWrapper::setStartStatePython); + + bool (MoveGroupInterfaceWrapper::*set_path_constraints_1)(const std::string&) = + &MoveGroupInterfaceWrapper::setPathConstraints; + move_group_interface_class.def("set_path_constraints", set_path_constraints_1); + move_group_interface_class.def("set_path_constraints_from_msg", + &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg); + move_group_interface_class.def("get_path_constraints", &MoveGroupInterfaceWrapper::getPathConstraintsPython); + move_group_interface_class.def("clear_path_constraints", &MoveGroupInterfaceWrapper::clearPathConstraints); + move_group_interface_class.def("get_known_constraints", &MoveGroupInterfaceWrapper::getKnownConstraintsList); + move_group_interface_class.def("set_constraints_database", &MoveGroupInterfaceWrapper::setConstraintsDatabase); + move_group_interface_class.def("set_workspace", &MoveGroupInterfaceWrapper::setWorkspace); + move_group_interface_class.def("set_planning_time", &MoveGroupInterfaceWrapper::setPlanningTime); + move_group_interface_class.def("get_planning_time", &MoveGroupInterfaceWrapper::getPlanningTime); + move_group_interface_class.def("set_max_velocity_scaling_factor", + &MoveGroupInterfaceWrapper::setMaxVelocityScalingFactor); + move_group_interface_class.def("set_max_acceleration_scaling_factor", + &MoveGroupInterfaceWrapper::setMaxAccelerationScalingFactor); + move_group_interface_class.def("set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId); + move_group_interface_class.def("set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts); + move_group_interface_class.def("plan", &MoveGroupInterfaceWrapper::planPython); + move_group_interface_class.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython); + move_group_interface_class.def("compute_cartesian_path", + &MoveGroupInterfaceWrapper::computeCartesianPathConstrainedPython); + move_group_interface_class.def("set_support_surface_name", &MoveGroupInterfaceWrapper::setSupportSurfaceName); + move_group_interface_class.def("attach_object", &MoveGroupInterfaceWrapper::attachObjectPython); + move_group_interface_class.def("detach_object", &MoveGroupInterfaceWrapper::detachObject); + move_group_interface_class.def("retime_trajectory", &MoveGroupInterfaceWrapper::retimeTrajectory); + move_group_interface_class.def("get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython); + move_group_interface_class.def("get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython); + move_group_interface_class.def("get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython); +} +} // namespace planning_interface +} // namespace moveit + +BOOST_PYTHON_MODULE(_moveit_move_group_interface) +{ + using namespace moveit::planning_interface; + wrap_move_group_interface(); +} + +/** @endcond */ From dd5e646b877c91b1f80cb4227596bf9b7575ad13 Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Tue, 30 Jul 2019 18:44:18 -0600 Subject: [PATCH 02/29] Renamed and added to Cmake --- moveit_ros/planning_interface/CMakeLists.txt | 3 + .../moveit_cpp/CMakeLists.txt | 12 +- .../moveit_cpp.h} | 42 +-- .../moveit_cpp/src/demo.cpp | 10 +- ...ove_group_interface.cpp => moveit_cpp.cpp} | 356 +++++++++--------- ...e_group.cpp => wrap_python_moveit_cpp.cpp} | 292 +++++++------- 6 files changed, 359 insertions(+), 356 deletions(-) rename moveit_ros/planning_interface/moveit_cpp/include/moveit/{move_group_interface/move_group_interface.h => moveit_cpp/moveit_cpp.h} (97%) rename moveit_ros/planning_interface/moveit_cpp/src/{move_group_interface.cpp => moveit_cpp.cpp} (80%) rename moveit_ros/planning_interface/moveit_cpp/src/{wrap_python_move_group.cpp => wrap_python_moveit_cpp.cpp} (61%) diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index d1b5594b18..ae9647c7c7 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -51,6 +51,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS common_planning_interface_objects/include planning_scene_interface/include move_group_interface/include + moveit_cpp/include ) catkin_python_setup() @@ -60,6 +61,7 @@ catkin_package( moveit_common_planning_interface_objects moveit_planning_scene_interface moveit_move_group_interface + moveit_moveit_cpp INCLUDE_DIRS ${THIS_PACKAGE_INCLUDE_DIRS} CATKIN_DEPENDS @@ -89,3 +91,4 @@ add_subdirectory(planning_scene_interface) add_subdirectory(move_group_interface) add_subdirectory(robot_interface) add_subdirectory(test) +add_subdirectory(moveit_cpp) diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt index eacfa2aecb..e045478964 100644 --- a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -1,15 +1,15 @@ -set(MOVEIT_LIB_NAME moveit_move_group_interface) +set(MOVEIT_LIB_NAME moveit_moveit_cpp) -add_library(${MOVEIT_LIB_NAME} src/move_group_interface.cpp) +add_library(${MOVEIT_LIB_NAME} src/moveit_cpp.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) -add_library(${MOVEIT_LIB_NAME}_python src/wrap_python_move_group.cpp) +add_library(${MOVEIT_LIB_NAME}_python src/wrap_python_moveit_cpp.cpp) target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) add_dependencies(${MOVEIT_LIB_NAME}_python ${catkin_EXPORTED_TARGETS}) set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_move_group_interface PREFIX "") +set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_moveit_cpp PREFIX "") set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") install(TARGETS ${MOVEIT_LIB_NAME} @@ -22,5 +22,5 @@ install(TARGETS ${MOVEIT_LIB_NAME}_python install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) -add_executable(demo src/demo.cpp) -target_link_libraries(demo ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +#add_executable(demo src/demo.cpp) +#target_link_libraries(demo ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h similarity index 97% rename from moveit_ros/planning_interface/moveit_cpp/include/moveit/move_group_interface/move_group_interface.h rename to moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 9a8eb921e5..d9fce76f78 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -35,8 +35,8 @@ /* Author: Ioan Sucan, Sachin Chitta */ -#ifndef MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_INTERFACE_ -#define MOVEIT_MOVE_GROUP_INTERFACE_MOVE_GROUP_INTERFACE_ +#ifndef MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ +#define MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ #include #include @@ -88,20 +88,20 @@ class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes } }; -MOVEIT_CLASS_FORWARD(MoveGroupInterface); +MOVEIT_CLASS_FORWARD(MoveItCpp); -/** \class MoveGroupInterface move_group_interface.h moveit/planning_interface/move_group_interface.h +/** \class MoveItCpp moveit_cpp.h moveit/planning_interface/moveit_cpp.h \brief Client class to conveniently use the ROS interfaces provided by the move_group node. This class includes many default settings to make things easy to use. */ -class MoveGroupInterface +class MoveItCpp { public: /** \brief Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description' */ static const std::string ROBOT_DESCRIPTION; - /** \brief Specification of options to use when constructing the MoveGroupInterface class */ + /** \brief Specification of options to use when constructing the MoveItCpp class */ struct Options { Options(const std::string& group_name, const std::string& desc = ROBOT_DESCRIPTION, @@ -138,19 +138,19 @@ class MoveGroupInterface }; /** - \brief Construct a MoveGroupInterface instance call using a specified set of options \e opt. + \brief Construct a MoveItCpp instance call using a specified set of options \e opt. - \param opt. A MoveGroupInterface::Options structure, if you pass a ros::NodeHandle with a specific callback queue, + \param opt. A MoveItCpp::Options structure, if you pass a ros::NodeHandle with a specific callback queue, it has to be of type ros::CallbackQueue (which is the default type of callback queues used in ROS) \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, one will be constructed internally along with an internal TF2_ROS TransformListener \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. */ - MoveGroupInterface(const Options& opt, + MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer = std::shared_ptr(), const ros::WallDuration& wait_for_servers = ros::WallDuration()); - MOVEIT_DEPRECATED MoveGroupInterface(const Options& opt, const std::shared_ptr& tf_buffer, + MOVEIT_DEPRECATED MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer, const ros::Duration& wait_for_servers); /** @@ -160,24 +160,24 @@ class MoveGroupInterface one will be constructed internally along with an internal TF2_ROS TransformListener \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. */ - MoveGroupInterface(const std::string& group, + MoveItCpp(const std::string& group, const std::shared_ptr& tf_buffer = std::shared_ptr(), const ros::WallDuration& wait_for_servers = ros::WallDuration()); - MOVEIT_DEPRECATED MoveGroupInterface(const std::string& group, const std::shared_ptr& tf_buffer, + MOVEIT_DEPRECATED MoveItCpp(const std::string& group, const std::shared_ptr& tf_buffer, const ros::Duration& wait_for_servers); - ~MoveGroupInterface(); + ~MoveItCpp(); /** * @brief This class owns unique resources (e.g. action clients, threads) and its not very * meaningful to copy. Pass by references, move it, or simply create multiple instances where * required. */ - MoveGroupInterface(const MoveGroupInterface&) = delete; - MoveGroupInterface& operator=(const MoveGroupInterface&) = delete; + MoveItCpp(const MoveItCpp&) = delete; + MoveItCpp& operator=(const MoveItCpp&) = delete; - MoveGroupInterface(MoveGroupInterface&& other); - MoveGroupInterface& operator=(MoveGroupInterface&& other); + MoveItCpp(MoveItCpp&& other); + MoveItCpp& operator=(MoveItCpp&& other); /** \brief Get the name of the group this instance operates on */ const std::string& getName() const; @@ -709,7 +709,7 @@ class MoveGroupInterface This call is not blocking (does not wait for the execution of the trajectory to complete). */ MoveItErrorCode asyncMove(); - /** \brief Get the move_group action client used by the \e MoveGroupInterface. + /** \brief Get the move_group action client used by the \e MoveItCpp. The client can be used for querying the execution state of the trajectory and abort trajectory execution during asynchronous execution. */ actionlib::SimpleActionClient& getMoveGroupClient() const; @@ -924,7 +924,7 @@ class MoveGroupInterface /** \brief Get the names of the known constraints as read from the Mongo database, if a connection was achieved. */ std::vector getKnownConstraints() const; - /** \brief Get the actual set of constraints in use with this MoveGroupInterface. + /** \brief Get the actual set of constraints in use with this MoveItCpp. @return A copy of the current path constraints set for this interface */ moveit_msgs::Constraints getPathConstraints() const; @@ -955,8 +955,8 @@ class MoveGroupInterface private: std::map > remembered_joint_values_; - class MoveGroupInterfaceImpl; - MoveGroupInterfaceImpl* impl_; + class MoveItCppImpl; + MoveItCppImpl* impl_; }; } } diff --git a/moveit_ros/planning_interface/moveit_cpp/src/demo.cpp b/moveit_ros/planning_interface/moveit_cpp/src/demo.cpp index 2cd370f906..30ce1f221a 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/demo.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/demo.cpp @@ -34,10 +34,10 @@ /* Author: Ioan Sucan */ -#include +#include #include -void demoPick(moveit::planning_interface::MoveGroupInterface& group) +void demoPick(moveit::planning_interface::MoveItCpp& group) { std::vector grasps; for (std::size_t i = 0; i < 20; ++i) @@ -71,7 +71,7 @@ void demoPick(moveit::planning_interface::MoveGroupInterface& group) group.pick("bubu", grasps); } -void demoPlace(moveit::planning_interface::MoveGroupInterface& group) +void demoPlace(moveit::planning_interface::MoveItCpp& group) { std::vector loc; for (std::size_t i = 0; i < 20; ++i) @@ -107,12 +107,12 @@ void attachObject() int main(int argc, char** argv) { - ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName); + ros::init(argc, argv, "moveit_cpp_demo", ros::init_options::AnonymousName); ros::AsyncSpinner spinner(1); spinner.start(); - moveit::planning_interface::MoveGroupInterface group(argc > 1 ? argv[1] : "right_arm"); + moveit::planning_interface::MoveItCpp group(argc > 1 ? argv[1] : "right_arm"); demoPlace(group); sleep(2); diff --git a/moveit_ros/planning_interface/moveit_cpp/src/move_group_interface.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp similarity index 80% rename from moveit_ros/planning_interface/moveit_cpp/src/move_group_interface.cpp rename to moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index f92b86e57c..8f2957837e 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include #include @@ -72,7 +72,7 @@ namespace moveit { namespace planning_interface { -const std::string MoveGroupInterface::ROBOT_DESCRIPTION = +const std::string MoveItCpp::ROBOT_DESCRIPTION = "robot_description"; // name of the robot description (a param name, so it can be changed externally) const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps @@ -88,10 +88,10 @@ enum ActiveTargetType }; } -class MoveGroupInterface::MoveGroupInterfaceImpl +class MoveItCpp::MoveItCppImpl { public: - MoveGroupInterfaceImpl(const Options& opt, const std::shared_ptr& tf_buffer, + MoveItCppImpl(const Options& opt, const std::shared_ptr& tf_buffer, const ros::WallDuration& wait_for_servers) : opt_(opt), node_handle_(opt.node_handle_), tf_buffer_(tf_buffer) { @@ -100,14 +100,14 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { std::string error = "Unable to construct robot model. Please make sure all needed information is on the " "parameter server."; - ROS_FATAL_STREAM_NAMED("move_group_interface", error); + ROS_FATAL_STREAM_NAMED("moveit_cpp", error); throw std::runtime_error(error); } if (!getRobotModel()->hasJointModelGroup(opt.group_name_)) { std::string error = "Group '" + opt.group_name_ + "' was not found."; - ROS_FATAL_STREAM_NAMED("move_group_interface", error); + ROS_FATAL_STREAM_NAMED("moveit_cpp", error); throw std::runtime_error(error); } @@ -172,14 +172,14 @@ class MoveGroupInterface::MoveGroupInterfaceImpl plan_grasps_service_ = node_handle_.serviceClient(GRASP_PLANNING_SERVICE_NAME); - ROS_INFO_STREAM_NAMED("move_group_interface", "Ready to take commands for planning group " << opt.group_name_ + ROS_INFO_STREAM_NAMED("moveit_cpp", "Ready to take commands for planning group " << opt.group_name_ << "."); } template void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) { - ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str()); + ROS_DEBUG_NAMED("moveit_cpp", "Waiting for move_group action server (%s)...", name.c_str()); // wait for the server (and spin as needed) if (timeout == ros::WallTime()) // wait forever @@ -195,7 +195,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else // in case of nodelets and specific callback queue implementations { - ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " + ROS_WARN_ONCE_NAMED("moveit_cpp", "Non-default CallbackQueue: Waiting for external queue " "handling."); } } @@ -213,7 +213,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else // in case of nodelets and specific callback queue implementations { - ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " + ROS_WARN_ONCE_NAMED("moveit_cpp", "Non-default CallbackQueue: Waiting for external queue " "handling."); } } @@ -228,11 +228,11 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_DEBUG_NAMED("move_group_interface", "Connected to '%s'", name.c_str()); + ROS_DEBUG_NAMED("moveit_cpp", "Connected to '%s'", name.c_str()); } } - ~MoveGroupInterfaceImpl() + ~MoveItCppImpl() { if (constraints_init_thread_) constraints_init_thread_->join(); @@ -413,7 +413,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_ERROR_NAMED("move_group_interface", "Unable to transform from frame '%s' to frame '%s'", frame.c_str(), + ROS_ERROR_NAMED("moveit_cpp", "Unable to transform from frame '%s' to frame '%s'", frame.c_str(), getRobotModel()->getModelFrame().c_str()); return false; } @@ -462,7 +462,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; if (eef.empty()) { - ROS_ERROR_NAMED("move_group_interface", "No end-effector to set the pose for"); + ROS_ERROR_NAMED("moveit_cpp", "No end-effector to set the pose for"); return false; } else @@ -494,7 +494,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // or return an error static const geometry_msgs::PoseStamped UNKNOWN; - ROS_ERROR_NAMED("move_group_interface", "Pose for end-effector '%s' not known.", eef.c_str()); + ROS_ERROR_NAMED("moveit_cpp", "Pose for end-effector '%s' not known.", eef.c_str()); return UNKNOWN; } @@ -509,7 +509,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // or return an error static const std::vector EMPTY; - ROS_ERROR_NAMED("move_group_interface", "Poses for end-effector '%s' are not known.", eef.c_str()); + ROS_ERROR_NAMED("moveit_cpp", "Poses for end-effector '%s' are not known.", eef.c_str()); return EMPTY; } @@ -542,7 +542,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!current_state_monitor_) { - ROS_ERROR_NAMED("move_group_interface", "Unable to monitor current robot state"); + ROS_ERROR_NAMED("moveit_cpp", "Unable to monitor current robot state"); return false; } @@ -558,7 +558,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!current_state_monitor_) { - ROS_ERROR_NAMED("move_group_interface", "Unable to get current robot state"); + ROS_ERROR_NAMED("moveit_cpp", "Unable to get current robot state"); return false; } @@ -568,7 +568,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds)) { - ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state"); + ROS_ERROR_NAMED("moveit_cpp", "Failed to fetch current robot state"); return false; } @@ -598,7 +598,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl location.place_pose = pose; locations.push_back(location); } - ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations", + ROS_DEBUG_NAMED("moveit_cpp", "Move group interface has %u place locations", (unsigned int)locations.size()); return place(object, locations, plan_only); } @@ -608,12 +608,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!place_action_client_) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found"); + ROS_ERROR_STREAM_NAMED("moveit_cpp", "Place action client not found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } if (!place_action_client_->isServerConnected()) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected"); + ROS_ERROR_STREAM_NAMED("moveit_cpp", "Place action server not connected"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } moveit_msgs::PlaceGoal goal; @@ -627,10 +627,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl goal.planning_options.planning_scene_diff.robot_state.is_diff = true; place_action_client_->sendGoal(goal); - ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size()); + ROS_DEBUG_NAMED("moveit_cpp", "Sent place goal with %d locations", (int)goal.place_locations.size()); if (!place_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early"); + ROS_INFO_STREAM_NAMED("moveit_cpp", "Place action returned early"); } if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { @@ -638,7 +638,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": " + ROS_WARN_STREAM_NAMED("moveit_cpp", "Fail: " << place_action_client_->getState().toString() << ": " << place_action_client_->getState().getText()); return MoveItErrorCode(place_action_client_->getResult()->error_code); } @@ -648,12 +648,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!pick_action_client_) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found"); + ROS_ERROR_STREAM_NAMED("moveit_cpp", "Pick action client not found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } if (!pick_action_client_->isServerConnected()) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected"); + ROS_ERROR_STREAM_NAMED("moveit_cpp", "Pick action server not connected"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } moveit_msgs::PickupGoal goal; @@ -669,7 +669,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl pick_action_client_->sendGoal(goal); if (!pick_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early"); + ROS_INFO_STREAM_NAMED("moveit_cpp", "Pickup action returned early"); } if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { @@ -677,7 +677,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": " + ROS_WARN_STREAM_NAMED("moveit_cpp", "Fail: " << pick_action_client_->getState().toString() << ": " << pick_action_client_->getState().getText()); return MoveItErrorCode(pick_action_client_->getResult()->error_code); } @@ -695,7 +695,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (objects.empty()) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '" + ROS_ERROR_STREAM_NAMED("moveit_cpp", "Asked for grasps for the object '" << object << "', but the object could not be found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME); } @@ -707,7 +707,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl { if (!plan_grasps_service_) { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '" + ROS_ERROR_STREAM_NAMED("moveit_cpp", "Grasp planning service '" << GRASP_PLANNING_SERVICE_NAME << "' is not available." " This has to be implemented and started separately."); @@ -721,11 +721,11 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.target = object; request.support_surfaces.push_back(support_surface_); - ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner..."); + ROS_DEBUG_NAMED("moveit_cpp", "Calling grasp planner..."); if (!plan_grasps_service_.call(request, response) || response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { - ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick."); + ROS_ERROR_NAMED("moveit_cpp", "Grasp planning failed. Unable to pick."); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } @@ -754,7 +754,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl move_action_client_->sendGoal(goal); if (!move_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); + ROS_INFO_STREAM_NAMED("moveit_cpp", "MoveGroup action returned early"); } if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { @@ -765,7 +765,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": " + ROS_WARN_STREAM_NAMED("moveit_cpp", "Fail: " << move_action_client_->getState().toString() << ": " << move_action_client_->getState().getText()); return MoveItErrorCode(move_action_client_->getResult()->error_code); } @@ -799,7 +799,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!move_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); + ROS_INFO_STREAM_NAMED("moveit_cpp", "MoveGroup action returned early"); } if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) @@ -808,7 +808,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString() + ROS_INFO_STREAM_NAMED("moveit_cpp", move_action_client_->getState().toString() << ": " << move_action_client_->getState().getText()); return MoveItErrorCode(move_action_client_->getResult()->error_code); } @@ -832,7 +832,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (!execute_action_client_->waitForResult()) { - ROS_INFO_STREAM_NAMED("move_group_interface", "ExecuteTrajectory action returned early"); + ROS_INFO_STREAM_NAMED("moveit_cpp", "ExecuteTrajectory action returned early"); } if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) @@ -841,7 +841,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } else { - ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString() + ROS_INFO_STREAM_NAMED("moveit_cpp", execute_action_client_->getState().toString() << ": " << execute_action_client_->getState().getText()); return MoveItErrorCode(execute_action_client_->getResult()->error_code); } @@ -908,7 +908,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } if (l.empty()) { - ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str()); + ROS_ERROR_NAMED("moveit_cpp", "No known link to attach object '%s' to", object.c_str()); return false; } moveit_msgs::AttachedCollisionObject aco; @@ -991,13 +991,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl void allowLooking(bool flag) { can_look_ = flag; - ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no"); + ROS_INFO_NAMED("moveit_cpp", "Looking around: %s", can_look_ ? "yes" : "no"); } void allowReplanning(bool flag) { can_replan_ = flag; - ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no"); + ROS_INFO_NAMED("moveit_cpp", "Replanning: %s", can_replan_ ? "yes" : "no"); } void setReplanningDelay(double delay) @@ -1062,7 +1062,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } } else - ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation"); + ROS_ERROR_NAMED("moveit_cpp", "Unable to construct MotionPlanRequest representation"); if (path_constraints_) request.path_constraints = *path_constraints_; @@ -1184,7 +1184,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (constraints_init_thread_) constraints_init_thread_->join(); constraints_init_thread_.reset( - new boost::thread(boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port))); + new boost::thread(boost::bind(&MoveItCppImpl::initializeConstraintsStorageThread, this, host, port))); } void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) @@ -1214,7 +1214,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } catch (std::exception& ex) { - ROS_ERROR_NAMED("move_group_interface", "%s", ex.what()); + ROS_ERROR_NAMED("moveit_cpp", "%s", ex.what()); } initializing_constraints_ = false; } @@ -1275,49 +1275,49 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } // namespace planning_interface } // namespace moveit -moveit::planning_interface::MoveGroupInterface::MoveGroupInterface(const std::string& group_name, +moveit::planning_interface::MoveItCpp::MoveItCpp(const std::string& group_name, const std::shared_ptr& tf_buffer, const ros::WallDuration& wait_for_servers) { if (!ros::ok()) throw std::runtime_error("ROS does not seem to be running"); - impl_ = new MoveGroupInterfaceImpl(Options(group_name), tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers); + impl_ = new MoveItCppImpl(Options(group_name), tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers); } -moveit::planning_interface::MoveGroupInterface::MoveGroupInterface(const std::string& group, +moveit::planning_interface::MoveItCpp::MoveItCpp(const std::string& group, const std::shared_ptr& tf_buffer, const ros::Duration& wait_for_servers) - : MoveGroupInterface(group, tf_buffer, ros::WallDuration(wait_for_servers.toSec())) + : MoveItCpp(group, tf_buffer, ros::WallDuration(wait_for_servers.toSec())) { } -moveit::planning_interface::MoveGroupInterface::MoveGroupInterface(const Options& opt, +moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer, const ros::WallDuration& wait_for_servers) { - impl_ = new MoveGroupInterfaceImpl(opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers); + impl_ = new MoveItCppImpl(opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers); } -moveit::planning_interface::MoveGroupInterface::MoveGroupInterface( - const moveit::planning_interface::MoveGroupInterface::Options& opt, +moveit::planning_interface::MoveItCpp::MoveItCpp( + const moveit::planning_interface::MoveItCpp::Options& opt, const std::shared_ptr& tf_buffer, const ros::Duration& wait_for_servers) - : MoveGroupInterface(opt, tf_buffer, ros::WallDuration(wait_for_servers.toSec())) + : MoveItCpp(opt, tf_buffer, ros::WallDuration(wait_for_servers.toSec())) { } -moveit::planning_interface::MoveGroupInterface::~MoveGroupInterface() +moveit::planning_interface::MoveItCpp::~MoveItCpp() { delete impl_; } -moveit::planning_interface::MoveGroupInterface::MoveGroupInterface(MoveGroupInterface&& other) +moveit::planning_interface::MoveItCpp::MoveItCpp(MoveItCpp&& other) : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_) { other.impl_ = nullptr; } -moveit::planning_interface::MoveGroupInterface& moveit::planning_interface::MoveGroupInterface:: -operator=(MoveGroupInterface&& other) +moveit::planning_interface::MoveItCpp& moveit::planning_interface::MoveItCpp:: +operator=(MoveItCpp&& other) { if (this != &other) { @@ -1330,12 +1330,12 @@ operator=(MoveGroupInterface&& other) return *this; } -const std::string& moveit::planning_interface::MoveGroupInterface::getName() const +const std::string& moveit::planning_interface::MoveItCpp::getName() const { return impl_->getOptions().group_name_; } -const std::vector moveit::planning_interface::MoveGroupInterface::getNamedTargets() +const std::vector moveit::planning_interface::MoveItCpp::getNamedTargets() { const robot_model::RobotModelConstPtr& robot = getRobotModel(); const std::string& group = getName(); @@ -1350,29 +1350,29 @@ const std::vector moveit::planning_interface::MoveGroupInterface::g return empty; } -robot_model::RobotModelConstPtr moveit::planning_interface::MoveGroupInterface::getRobotModel() const +robot_model::RobotModelConstPtr moveit::planning_interface::MoveItCpp::getRobotModel() const { return impl_->getRobotModel(); } -const ros::NodeHandle& moveit::planning_interface::MoveGroupInterface::getNodeHandle() const +const ros::NodeHandle& moveit::planning_interface::MoveItCpp::getNodeHandle() const { return impl_->getOptions().node_handle_; } -bool moveit::planning_interface::MoveGroupInterface::getInterfaceDescription( +bool moveit::planning_interface::MoveItCpp::getInterfaceDescription( moveit_msgs::PlannerInterfaceDescription& desc) { return impl_->getInterfaceDescription(desc); } -std::map moveit::planning_interface::MoveGroupInterface::getPlannerParams( +std::map moveit::planning_interface::MoveItCpp::getPlannerParams( const std::string& planner_id, const std::string& group) { return impl_->getPlannerParams(planner_id, group); } -void moveit::planning_interface::MoveGroupInterface::setPlannerParams(const std::string& planner_id, +void moveit::planning_interface::MoveItCpp::setPlannerParams(const std::string& planner_id, const std::string& group, const std::map& params, bool replace) @@ -1380,124 +1380,124 @@ void moveit::planning_interface::MoveGroupInterface::setPlannerParams(const std: impl_->setPlannerParams(planner_id, group, params, replace); } -std::string moveit::planning_interface::MoveGroupInterface::getDefaultPlannerId(const std::string& group) const +std::string moveit::planning_interface::MoveItCpp::getDefaultPlannerId(const std::string& group) const { return impl_->getDefaultPlannerId(group); } -void moveit::planning_interface::MoveGroupInterface::setPlannerId(const std::string& planner_id) +void moveit::planning_interface::MoveItCpp::setPlannerId(const std::string& planner_id) { impl_->setPlannerId(planner_id); } -const std::string& moveit::planning_interface::MoveGroupInterface::getPlannerId() const +const std::string& moveit::planning_interface::MoveItCpp::getPlannerId() const { return impl_->getPlannerId(); } -void moveit::planning_interface::MoveGroupInterface::setNumPlanningAttempts(unsigned int num_planning_attempts) +void moveit::planning_interface::MoveItCpp::setNumPlanningAttempts(unsigned int num_planning_attempts) { impl_->setNumPlanningAttempts(num_planning_attempts); } -void moveit::planning_interface::MoveGroupInterface::setMaxVelocityScalingFactor(double max_velocity_scaling_factor) +void moveit::planning_interface::MoveItCpp::setMaxVelocityScalingFactor(double max_velocity_scaling_factor) { impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor); } -void moveit::planning_interface::MoveGroupInterface::setMaxAccelerationScalingFactor( +void moveit::planning_interface::MoveItCpp::setMaxAccelerationScalingFactor( double max_acceleration_scaling_factor) { impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::asyncMove() +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::asyncMove() { return impl_->move(false); } actionlib::SimpleActionClient& -moveit::planning_interface::MoveGroupInterface::getMoveGroupClient() const +moveit::planning_interface::MoveItCpp::getMoveGroupClient() const { return impl_->getMoveGroupClient(); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::move() +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::move() { return impl_->move(true); } moveit::planning_interface::MoveItErrorCode -moveit::planning_interface::MoveGroupInterface::asyncExecute(const Plan& plan) +moveit::planning_interface::MoveItCpp::asyncExecute(const Plan& plan) { return impl_->execute(plan, false); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::execute(const Plan& plan) +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::execute(const Plan& plan) { return impl_->execute(plan, true); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::plan(Plan& plan) +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::plan(Plan& plan) { return impl_->plan(plan); } moveit::planning_interface::MoveItErrorCode -moveit::planning_interface::MoveGroupInterface::pick(const std::string& object, bool plan_only) +moveit::planning_interface::MoveItCpp::pick(const std::string& object, bool plan_only) { return impl_->pick(object, std::vector(), plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::pick( +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::pick( const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only) { return impl_->pick(object, std::vector(1, grasp), plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::pick( +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::pick( const std::string& object, const std::vector& grasps, bool plan_only) { return impl_->pick(object, grasps, plan_only); } moveit::planning_interface::MoveItErrorCode -moveit::planning_interface::MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only) +moveit::planning_interface::MoveItCpp::planGraspsAndPick(const std::string& object, bool plan_only) { return impl_->planGraspsAndPick(object, plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::planGraspsAndPick( +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::planGraspsAndPick( const moveit_msgs::CollisionObject& object, bool plan_only) { return impl_->planGraspsAndPick(object, plan_only); } moveit::planning_interface::MoveItErrorCode -moveit::planning_interface::MoveGroupInterface::place(const std::string& object, bool plan_only) +moveit::planning_interface::MoveItCpp::place(const std::string& object, bool plan_only) { return impl_->place(object, std::vector(), plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::place( +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::place( const std::string& object, const std::vector& locations, bool plan_only) { return impl_->place(object, locations, plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::place( +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::place( const std::string& object, const std::vector& poses, bool plan_only) { return impl_->place(object, poses, plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::place( +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::place( const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only) { return impl_->place(object, std::vector(1, pose), plan_only); } -double moveit::planning_interface::MoveGroupInterface::computeCartesianPath( +double moveit::planning_interface::MoveItCpp::computeCartesianPath( const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code) { @@ -1506,7 +1506,7 @@ double moveit::planning_interface::MoveGroupInterface::computeCartesianPath( error_code); } -double moveit::planning_interface::MoveGroupInterface::computeCartesianPath( +double moveit::planning_interface::MoveItCpp::computeCartesianPath( const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code) @@ -1524,12 +1524,12 @@ double moveit::planning_interface::MoveGroupInterface::computeCartesianPath( } } -void moveit::planning_interface::MoveGroupInterface::stop() +void moveit::planning_interface::MoveItCpp::stop() { impl_->stop(); } -void moveit::planning_interface::MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_state) +void moveit::planning_interface::MoveItCpp::setStartState(const moveit_msgs::RobotState& start_state) { robot_state::RobotStatePtr rs; impl_->getCurrentState(rs); @@ -1537,34 +1537,34 @@ void moveit::planning_interface::MoveGroupInterface::setStartState(const moveit_ setStartState(*rs); } -void moveit::planning_interface::MoveGroupInterface::setStartState(const robot_state::RobotState& start_state) +void moveit::planning_interface::MoveItCpp::setStartState(const robot_state::RobotState& start_state) { impl_->setStartState(start_state); } -void moveit::planning_interface::MoveGroupInterface::setStartStateToCurrentState() +void moveit::planning_interface::MoveItCpp::setStartStateToCurrentState() { impl_->setStartStateToCurrentState(); } -void moveit::planning_interface::MoveGroupInterface::setRandomTarget() +void moveit::planning_interface::MoveItCpp::setRandomTarget() { impl_->getTargetRobotState().setToRandomPositions(); impl_->setTargetType(JOINT); } -const std::vector& moveit::planning_interface::MoveGroupInterface::getJointNames() +const std::vector& moveit::planning_interface::MoveItCpp::getJointNames() { return impl_->getJointModelGroup()->getVariableNames(); } -const std::vector& moveit::planning_interface::MoveGroupInterface::getLinkNames() +const std::vector& moveit::planning_interface::MoveItCpp::getLinkNames() { return impl_->getJointModelGroup()->getLinkModelNames(); } std::map -moveit::planning_interface::MoveGroupInterface::getNamedTargetValues(const std::string& name) +moveit::planning_interface::MoveItCpp::getNamedTargetValues(const std::string& name) { std::map >::const_iterator it = remembered_joint_values_.find(name); std::map positions; @@ -1584,7 +1584,7 @@ moveit::planning_interface::MoveGroupInterface::getNamedTargetValues(const std:: return positions; } -bool moveit::planning_interface::MoveGroupInterface::setNamedTarget(const std::string& name) +bool moveit::planning_interface::MoveItCpp::setNamedTarget(const std::string& name) { std::map >::const_iterator it = remembered_joint_values_.find(name); if (it != remembered_joint_values_.end()) @@ -1598,18 +1598,18 @@ bool moveit::planning_interface::MoveGroupInterface::setNamedTarget(const std::s impl_->setTargetType(JOINT); return true; } - ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str()); + ROS_ERROR_NAMED("moveit_cpp", "The requested named target '%s' does not exist", name.c_str()); return false; } } -void moveit::planning_interface::MoveGroupInterface::getJointValueTarget( +void moveit::planning_interface::MoveItCpp::getJointValueTarget( std::vector& group_variable_values) const { impl_->getTargetRobotState().copyJointGroupPositions(impl_->getJointModelGroup(), group_variable_values); } -bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector& joint_values) +bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::vector& joint_values) { if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount()) return false; @@ -1618,7 +1618,7 @@ bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const s return impl_->getTargetRobotState().satisfiesBounds(impl_->getJointModelGroup(), impl_->getGoalJointTolerance()); } -bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget( +bool moveit::planning_interface::MoveItCpp::setJointValueTarget( const std::map& variable_values) { const auto& allowed = impl_->getJointModelGroup()->getVariableNames(); @@ -1637,7 +1637,7 @@ bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget( return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance()); } -bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector& variable_names, +bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::vector& variable_names, const std::vector& variable_values) { const auto& allowed = impl_->getJointModelGroup()->getVariableNames(); @@ -1656,20 +1656,20 @@ bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const s return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance()); } -bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const robot_state::RobotState& rstate) +bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const robot_state::RobotState& rstate) { impl_->setTargetType(JOINT); impl_->getTargetRobotState() = rstate; return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance()); } -bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::string& joint_name, double value) +bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::string& joint_name, double value) { std::vector values(1, value); return setJointValueTarget(joint_name, values); } -bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::string& joint_name, +bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::string& joint_name, const std::vector& values) { impl_->setTargetType(JOINT); @@ -1684,70 +1684,70 @@ bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const s return false; } -bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const sensor_msgs::JointState& state) +bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const sensor_msgs::JointState& state) { return setJointValueTarget(state.name, state.position); } -bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const geometry_msgs::Pose& eef_pose, +bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link) { return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false); } -bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, +bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link) { return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false); } -bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const Eigen::Isometry3d& eef_pose, +bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link) { geometry_msgs::Pose msg = tf2::toMsg(eef_pose); return setJointValueTarget(msg, end_effector_link); } -bool moveit::planning_interface::MoveGroupInterface::setApproximateJointValueTarget( +bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget( const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link) { return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true); } -bool moveit::planning_interface::MoveGroupInterface::setApproximateJointValueTarget( +bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget( const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link) { return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true); } -bool moveit::planning_interface::MoveGroupInterface::setApproximateJointValueTarget( +bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget( const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link) { geometry_msgs::Pose msg = tf2::toMsg(eef_pose); return setApproximateJointValueTarget(msg, end_effector_link); } -const robot_state::RobotState& moveit::planning_interface::MoveGroupInterface::getJointValueTarget() const +const robot_state::RobotState& moveit::planning_interface::MoveItCpp::getJointValueTarget() const { return impl_->getTargetRobotState(); } -const robot_state::RobotState& moveit::planning_interface::MoveGroupInterface::getTargetRobotState() const +const robot_state::RobotState& moveit::planning_interface::MoveItCpp::getTargetRobotState() const { return impl_->getTargetRobotState(); } -const std::string& moveit::planning_interface::MoveGroupInterface::getEndEffectorLink() const +const std::string& moveit::planning_interface::MoveItCpp::getEndEffectorLink() const { return impl_->getEndEffectorLink(); } -const std::string& moveit::planning_interface::MoveGroupInterface::getEndEffector() const +const std::string& moveit::planning_interface::MoveItCpp::getEndEffector() const { return impl_->getEndEffector(); } -bool moveit::planning_interface::MoveGroupInterface::setEndEffectorLink(const std::string& link_name) +bool moveit::planning_interface::MoveItCpp::setEndEffectorLink(const std::string& link_name) { if (impl_->getEndEffectorLink().empty() || link_name.empty()) return false; @@ -1756,7 +1756,7 @@ bool moveit::planning_interface::MoveGroupInterface::setEndEffectorLink(const st return true; } -bool moveit::planning_interface::MoveGroupInterface::setEndEffector(const std::string& eef_name) +bool moveit::planning_interface::MoveItCpp::setEndEffector(const std::string& eef_name) { const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name); if (jmg) @@ -1764,17 +1764,17 @@ bool moveit::planning_interface::MoveGroupInterface::setEndEffector(const std::s return false; } -void moveit::planning_interface::MoveGroupInterface::clearPoseTarget(const std::string& end_effector_link) +void moveit::planning_interface::MoveItCpp::clearPoseTarget(const std::string& end_effector_link) { impl_->clearPoseTarget(end_effector_link); } -void moveit::planning_interface::MoveGroupInterface::clearPoseTargets() +void moveit::planning_interface::MoveItCpp::clearPoseTargets() { impl_->clearPoseTargets(); } -bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const Eigen::Isometry3d& pose, +bool moveit::planning_interface::MoveItCpp::setPoseTarget(const Eigen::Isometry3d& pose, const std::string& end_effector_link) { std::vector pose_msg(1); @@ -1784,7 +1784,7 @@ bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const Eigen:: return setPoseTargets(pose_msg, end_effector_link); } -bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const geometry_msgs::Pose& target, +bool moveit::planning_interface::MoveItCpp::setPoseTarget(const geometry_msgs::Pose& target, const std::string& end_effector_link) { std::vector pose_msg(1); @@ -1794,14 +1794,14 @@ bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const geometr return setPoseTargets(pose_msg, end_effector_link); } -bool moveit::planning_interface::MoveGroupInterface::setPoseTarget(const geometry_msgs::PoseStamped& target, +bool moveit::planning_interface::MoveItCpp::setPoseTarget(const geometry_msgs::PoseStamped& target, const std::string& end_effector_link) { std::vector targets(1, target); return setPoseTargets(targets, end_effector_link); } -bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const EigenSTL::vector_Isometry3d& target, +bool moveit::planning_interface::MoveItCpp::setPoseTargets(const EigenSTL::vector_Isometry3d& target, const std::string& end_effector_link) { std::vector pose_out(target.size()); @@ -1816,7 +1816,7 @@ bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const EigenS return setPoseTargets(pose_out, end_effector_link); } -bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const std::vector& target, +bool moveit::planning_interface::MoveItCpp::setPoseTargets(const std::vector& target, const std::string& end_effector_link) { std::vector target_stamped(target.size()); @@ -1831,12 +1831,12 @@ bool moveit::planning_interface::MoveGroupInterface::setPoseTargets(const std::v return setPoseTargets(target_stamped, end_effector_link); } -bool moveit::planning_interface::MoveGroupInterface::setPoseTargets( +bool moveit::planning_interface::MoveItCpp::setPoseTargets( const std::vector& target, const std::string& end_effector_link) { if (target.empty()) { - ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target"); + ROS_ERROR_NAMED("moveit_cpp", "No pose specified as goal target"); return false; } else @@ -1847,13 +1847,13 @@ bool moveit::planning_interface::MoveGroupInterface::setPoseTargets( } const geometry_msgs::PoseStamped& -moveit::planning_interface::MoveGroupInterface::getPoseTarget(const std::string& end_effector_link) const +moveit::planning_interface::MoveItCpp::getPoseTarget(const std::string& end_effector_link) const { return impl_->getPoseTarget(end_effector_link); } const std::vector& -moveit::planning_interface::MoveGroupInterface::getPoseTargets(const std::string& end_effector_link) const +moveit::planning_interface::MoveItCpp::getPoseTargets(const std::string& end_effector_link) const { return impl_->getPoseTargets(end_effector_link); } @@ -1873,7 +1873,7 @@ inline void transformPose(const tf2_ros::Buffer& tf_buffer, const std::string& d } } // namespace -bool moveit::planning_interface::MoveGroupInterface::setPositionTarget(double x, double y, double z, +bool moveit::planning_interface::MoveItCpp::setPositionTarget(double x, double y, double z, const std::string& end_effector_link) { geometry_msgs::PoseStamped target; @@ -1899,7 +1899,7 @@ bool moveit::planning_interface::MoveGroupInterface::setPositionTarget(double x, return result; } -bool moveit::planning_interface::MoveGroupInterface::setRPYTarget(double r, double p, double y, +bool moveit::planning_interface::MoveItCpp::setRPYTarget(double r, double p, double y, const std::string& end_effector_link) { geometry_msgs::PoseStamped target; @@ -1923,7 +1923,7 @@ bool moveit::planning_interface::MoveGroupInterface::setRPYTarget(double r, doub return result; } -bool moveit::planning_interface::MoveGroupInterface::setOrientationTarget(double x, double y, double z, double w, +bool moveit::planning_interface::MoveItCpp::setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link) { geometry_msgs::PoseStamped target; @@ -1949,64 +1949,64 @@ bool moveit::planning_interface::MoveGroupInterface::setOrientationTarget(double return result; } -void moveit::planning_interface::MoveGroupInterface::setPoseReferenceFrame(const std::string& pose_reference_frame) +void moveit::planning_interface::MoveItCpp::setPoseReferenceFrame(const std::string& pose_reference_frame) { impl_->setPoseReferenceFrame(pose_reference_frame); } -const std::string& moveit::planning_interface::MoveGroupInterface::getPoseReferenceFrame() const +const std::string& moveit::planning_interface::MoveItCpp::getPoseReferenceFrame() const { return impl_->getPoseReferenceFrame(); } -double moveit::planning_interface::MoveGroupInterface::getGoalJointTolerance() const +double moveit::planning_interface::MoveItCpp::getGoalJointTolerance() const { return impl_->getGoalJointTolerance(); } -double moveit::planning_interface::MoveGroupInterface::getGoalPositionTolerance() const +double moveit::planning_interface::MoveItCpp::getGoalPositionTolerance() const { return impl_->getGoalPositionTolerance(); } -double moveit::planning_interface::MoveGroupInterface::getGoalOrientationTolerance() const +double moveit::planning_interface::MoveItCpp::getGoalOrientationTolerance() const { return impl_->getGoalOrientationTolerance(); } -void moveit::planning_interface::MoveGroupInterface::setGoalTolerance(double tolerance) +void moveit::planning_interface::MoveItCpp::setGoalTolerance(double tolerance) { setGoalJointTolerance(tolerance); setGoalPositionTolerance(tolerance); setGoalOrientationTolerance(tolerance); } -void moveit::planning_interface::MoveGroupInterface::setGoalJointTolerance(double tolerance) +void moveit::planning_interface::MoveItCpp::setGoalJointTolerance(double tolerance) { impl_->setGoalJointTolerance(tolerance); } -void moveit::planning_interface::MoveGroupInterface::setGoalPositionTolerance(double tolerance) +void moveit::planning_interface::MoveItCpp::setGoalPositionTolerance(double tolerance) { impl_->setGoalPositionTolerance(tolerance); } -void moveit::planning_interface::MoveGroupInterface::setGoalOrientationTolerance(double tolerance) +void moveit::planning_interface::MoveItCpp::setGoalOrientationTolerance(double tolerance) { impl_->setGoalOrientationTolerance(tolerance); } -void moveit::planning_interface::MoveGroupInterface::rememberJointValues(const std::string& name) +void moveit::planning_interface::MoveItCpp::rememberJointValues(const std::string& name) { rememberJointValues(name, getCurrentJointValues()); } -bool moveit::planning_interface::MoveGroupInterface::startStateMonitor(double wait) +bool moveit::planning_interface::MoveItCpp::startStateMonitor(double wait) { return impl_->startStateMonitor(wait); } -std::vector moveit::planning_interface::MoveGroupInterface::getCurrentJointValues() +std::vector moveit::planning_interface::MoveItCpp::getCurrentJointValues() { robot_state::RobotStatePtr current_state; std::vector values; @@ -2015,7 +2015,7 @@ std::vector moveit::planning_interface::MoveGroupInterface::getCurrentJo return values; } -std::vector moveit::planning_interface::MoveGroupInterface::getRandomJointValues() +std::vector moveit::planning_interface::MoveItCpp::getRandomJointValues() { std::vector r; impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getTargetRobotState().getRandomNumberGenerator(), r); @@ -2023,13 +2023,13 @@ std::vector moveit::planning_interface::MoveGroupInterface::getRandomJoi } geometry_msgs::PoseStamped -moveit::planning_interface::MoveGroupInterface::getRandomPose(const std::string& end_effector_link) +moveit::planning_interface::MoveItCpp::getRandomPose(const std::string& end_effector_link) { const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; Eigen::Isometry3d pose; pose.setIdentity(); if (eef.empty()) - ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); + ROS_ERROR_NAMED("moveit_cpp", "No end-effector specified"); else { robot_state::RobotStatePtr current_state; @@ -2049,13 +2049,13 @@ moveit::planning_interface::MoveGroupInterface::getRandomPose(const std::string& } geometry_msgs::PoseStamped -moveit::planning_interface::MoveGroupInterface::getCurrentPose(const std::string& end_effector_link) +moveit::planning_interface::MoveItCpp::getCurrentPose(const std::string& end_effector_link) { const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; Eigen::Isometry3d pose; pose.setIdentity(); if (eef.empty()) - ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); + ROS_ERROR_NAMED("moveit_cpp", "No end-effector specified"); else { robot_state::RobotStatePtr current_state; @@ -2073,12 +2073,12 @@ moveit::planning_interface::MoveGroupInterface::getCurrentPose(const std::string return pose_msg; } -std::vector moveit::planning_interface::MoveGroupInterface::getCurrentRPY(const std::string& end_effector_link) +std::vector moveit::planning_interface::MoveItCpp::getCurrentRPY(const std::string& end_effector_link) { std::vector result; const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; if (eef.empty()) - ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); + ROS_ERROR_NAMED("moveit_cpp", "No end-effector specified"); else { robot_state::RobotStatePtr current_state; @@ -2100,145 +2100,145 @@ std::vector moveit::planning_interface::MoveGroupInterface::getCurrentRP return result; } -const std::vector& moveit::planning_interface::MoveGroupInterface::getActiveJoints() const +const std::vector& moveit::planning_interface::MoveItCpp::getActiveJoints() const { return impl_->getJointModelGroup()->getActiveJointModelNames(); } -const std::vector& moveit::planning_interface::MoveGroupInterface::getJoints() const +const std::vector& moveit::planning_interface::MoveItCpp::getJoints() const { return impl_->getJointModelGroup()->getJointModelNames(); } -unsigned int moveit::planning_interface::MoveGroupInterface::getVariableCount() const +unsigned int moveit::planning_interface::MoveItCpp::getVariableCount() const { return impl_->getJointModelGroup()->getVariableCount(); } -robot_state::RobotStatePtr moveit::planning_interface::MoveGroupInterface::getCurrentState(double wait) +robot_state::RobotStatePtr moveit::planning_interface::MoveItCpp::getCurrentState(double wait) { robot_state::RobotStatePtr current_state; impl_->getCurrentState(current_state, wait); return current_state; } -void moveit::planning_interface::MoveGroupInterface::rememberJointValues(const std::string& name, +void moveit::planning_interface::MoveItCpp::rememberJointValues(const std::string& name, const std::vector& values) { remembered_joint_values_[name] = values; } -void moveit::planning_interface::MoveGroupInterface::forgetJointValues(const std::string& name) +void moveit::planning_interface::MoveItCpp::forgetJointValues(const std::string& name) { remembered_joint_values_.erase(name); } -void moveit::planning_interface::MoveGroupInterface::allowLooking(bool flag) +void moveit::planning_interface::MoveItCpp::allowLooking(bool flag) { impl_->allowLooking(flag); } -void moveit::planning_interface::MoveGroupInterface::allowReplanning(bool flag) +void moveit::planning_interface::MoveItCpp::allowReplanning(bool flag) { impl_->allowReplanning(flag); } -std::vector moveit::planning_interface::MoveGroupInterface::getKnownConstraints() const +std::vector moveit::planning_interface::MoveItCpp::getKnownConstraints() const { return impl_->getKnownConstraints(); } -moveit_msgs::Constraints moveit::planning_interface::MoveGroupInterface::getPathConstraints() const +moveit_msgs::Constraints moveit::planning_interface::MoveItCpp::getPathConstraints() const { return impl_->getPathConstraints(); } -bool moveit::planning_interface::MoveGroupInterface::setPathConstraints(const std::string& constraint) +bool moveit::planning_interface::MoveItCpp::setPathConstraints(const std::string& constraint) { return impl_->setPathConstraints(constraint); } -void moveit::planning_interface::MoveGroupInterface::setPathConstraints(const moveit_msgs::Constraints& constraint) +void moveit::planning_interface::MoveItCpp::setPathConstraints(const moveit_msgs::Constraints& constraint) { impl_->setPathConstraints(constraint); } -void moveit::planning_interface::MoveGroupInterface::clearPathConstraints() +void moveit::planning_interface::MoveItCpp::clearPathConstraints() { impl_->clearPathConstraints(); } -moveit_msgs::TrajectoryConstraints moveit::planning_interface::MoveGroupInterface::getTrajectoryConstraints() const +moveit_msgs::TrajectoryConstraints moveit::planning_interface::MoveItCpp::getTrajectoryConstraints() const { return impl_->getTrajectoryConstraints(); } -void moveit::planning_interface::MoveGroupInterface::setTrajectoryConstraints( +void moveit::planning_interface::MoveItCpp::setTrajectoryConstraints( const moveit_msgs::TrajectoryConstraints& constraint) { impl_->setTrajectoryConstraints(constraint); } -void moveit::planning_interface::MoveGroupInterface::clearTrajectoryConstraints() +void moveit::planning_interface::MoveItCpp::clearTrajectoryConstraints() { impl_->clearTrajectoryConstraints(); } -void moveit::planning_interface::MoveGroupInterface::setConstraintsDatabase(const std::string& host, unsigned int port) +void moveit::planning_interface::MoveItCpp::setConstraintsDatabase(const std::string& host, unsigned int port) { impl_->initializeConstraintsStorage(host, port); } -void moveit::planning_interface::MoveGroupInterface::setWorkspace(double minx, double miny, double minz, double maxx, +void moveit::planning_interface::MoveItCpp::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) { impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz); } /** \brief Set time allowed to planner to solve problem before aborting */ -void moveit::planning_interface::MoveGroupInterface::setPlanningTime(double seconds) +void moveit::planning_interface::MoveItCpp::setPlanningTime(double seconds) { impl_->setPlanningTime(seconds); } /** \brief Get time allowed to planner to solve problem before aborting */ -double moveit::planning_interface::MoveGroupInterface::getPlanningTime() const +double moveit::planning_interface::MoveItCpp::getPlanningTime() const { return impl_->getPlanningTime(); } -void moveit::planning_interface::MoveGroupInterface::setSupportSurfaceName(const std::string& name) +void moveit::planning_interface::MoveItCpp::setSupportSurfaceName(const std::string& name) { impl_->setSupportSurfaceName(name); } -const std::string& moveit::planning_interface::MoveGroupInterface::getPlanningFrame() const +const std::string& moveit::planning_interface::MoveItCpp::getPlanningFrame() const { return impl_->getRobotModel()->getModelFrame(); } -const std::vector& moveit::planning_interface::MoveGroupInterface::getJointModelGroupNames() const +const std::vector& moveit::planning_interface::MoveItCpp::getJointModelGroupNames() const { return impl_->getRobotModel()->getJointModelGroupNames(); } -bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link) +bool moveit::planning_interface::MoveItCpp::attachObject(const std::string& object, const std::string& link) { return attachObject(object, link, std::vector()); } -bool moveit::planning_interface::MoveGroupInterface::attachObject(const std::string& object, const std::string& link, +bool moveit::planning_interface::MoveItCpp::attachObject(const std::string& object, const std::string& link, const std::vector& touch_links) { return impl_->attachObject(object, link, touch_links); } -bool moveit::planning_interface::MoveGroupInterface::detachObject(const std::string& name) +bool moveit::planning_interface::MoveItCpp::detachObject(const std::string& name) { return impl_->detachObject(name); } -void moveit::planning_interface::MoveGroupInterface::constructMotionPlanRequest( +void moveit::planning_interface::MoveItCpp::constructMotionPlanRequest( moveit_msgs::MotionPlanRequest& goal_out) { impl_->constructMotionPlanRequest(goal_out); diff --git a/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_moveit_cpp.cpp similarity index 61% rename from moveit_ros/planning_interface/moveit_cpp/src/wrap_python_move_group.cpp rename to moveit_ros/planning_interface/moveit_cpp/src/wrap_python_moveit_cpp.cpp index 36fcc36422..49b74361db 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_moveit_cpp.cpp @@ -34,7 +34,7 @@ /* Author: Ioan Sucan */ -#include +#include #include #include #include @@ -58,15 +58,15 @@ namespace moveit { namespace planning_interface { -class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer, public MoveGroupInterface +class MoveItCppWrapper : protected py_bindings_tools::ROScppInitializer, public MoveItCpp { public: // ROSInitializer is constructed first, and ensures ros::init() was called, if // needed - MoveGroupInterfaceWrapper(const std::string& group_name, const std::string& robot_description, + MoveItCppWrapper(const std::string& group_name, const std::string& robot_description, const std::string& ns = "", double wait_for_servers = 5.0) : py_bindings_tools::ROScppInitializer() - , MoveGroupInterface(Options(group_name, robot_description, ros::NodeHandle(ns)), + , MoveItCpp(Options(group_name, robot_description, ros::NodeHandle(ns)), std::shared_ptr(), ros::WallDuration(wait_for_servers)) { } @@ -115,7 +115,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer bp::list getJointValueTargetPythonList() { std::vector values; - MoveGroupInterface::getJointValueTarget(values); + MoveItCpp::getJointValueTarget(values); bp::list l; for (const double value : values) l.append(value); @@ -125,7 +125,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer std::string getJointValueTarget() { moveit_msgs::RobotState msg; - const robot_state::RobotState state = moveit::planning_interface::MoveGroupInterface::getTargetRobotState(); + const robot_state::RobotState state = moveit::planning_interface::MoveItCpp::getTargetRobotState(); moveit::core::robotStateToRobotStateMsg(state, msg); return py_bindings_tools::serializeMsg(msg); } @@ -325,7 +325,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer } std::string getPoseTargetPython(const std::string& end_effector_link) { - geometry_msgs::PoseStamped pose = moveit::planning_interface::MoveGroupInterface::getPoseTarget(end_effector_link); + geometry_msgs::PoseStamped pose = moveit::planning_interface::MoveItCpp::getPoseTarget(end_effector_link); return py_bindings_tools::serializeMsg(pose); } @@ -405,22 +405,22 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer bool executePython(const std::string& plan_str) { - MoveGroupInterface::Plan plan; + MoveItCpp::Plan plan; py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); return execute(plan) == MoveItErrorCode::SUCCESS; } bool asyncExecutePython(const std::string& plan_str) { - MoveGroupInterface::Plan plan; + MoveItCpp::Plan plan; py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); return asyncExecute(plan) == MoveItErrorCode::SUCCESS; } bp::tuple planPython() { - MoveGroupInterface::Plan plan; - moveit_msgs::MoveItErrorCodes res = MoveGroupInterface::plan(plan); + MoveItCpp::Plan plan; + moveit_msgs::MoveItErrorCodes res = MoveItCpp::plan(plan); return bp::make_tuple(py_bindings_tools::serializeMsg(res), py_bindings_tools::serializeMsg(plan.trajectory_), plan.planning_time_); } @@ -515,148 +515,148 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer } }; -static void wrap_move_group_interface() +static void wrap_moveit_cpp() { - bp::class_ move_group_interface_class( - "MoveGroupInterface", bp::init>()); - - move_group_interface_class.def("async_move", &MoveGroupInterfaceWrapper::asyncMovePython); - move_group_interface_class.def("move", &MoveGroupInterfaceWrapper::movePython); - move_group_interface_class.def("execute", &MoveGroupInterfaceWrapper::executePython); - move_group_interface_class.def("async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython); - moveit::planning_interface::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) = - &MoveGroupInterfaceWrapper::pick; - move_group_interface_class.def("pick", pick_1); - move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasp); - move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasps); - move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placePose); - move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeLocation); - move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeAnywhere); - move_group_interface_class.def("stop", &MoveGroupInterfaceWrapper::stop); - - move_group_interface_class.def("get_name", &MoveGroupInterfaceWrapper::getNameCStr); - move_group_interface_class.def("get_planning_frame", &MoveGroupInterfaceWrapper::getPlanningFrameCStr); - move_group_interface_class.def("get_interface_description", - &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython); - - move_group_interface_class.def("get_active_joints", &MoveGroupInterfaceWrapper::getActiveJointsList); - move_group_interface_class.def("get_joints", &MoveGroupInterfaceWrapper::getJointsList); - move_group_interface_class.def("get_variable_count", &MoveGroupInterfaceWrapper::getVariableCount); - move_group_interface_class.def("allow_looking", &MoveGroupInterfaceWrapper::allowLooking); - move_group_interface_class.def("allow_replanning", &MoveGroupInterfaceWrapper::allowReplanning); - - move_group_interface_class.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame); - - move_group_interface_class.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame); - move_group_interface_class.def("set_end_effector_link", &MoveGroupInterfaceWrapper::setEndEffectorLink); - move_group_interface_class.def("get_end_effector_link", &MoveGroupInterfaceWrapper::getEndEffectorLinkCStr); - move_group_interface_class.def("get_pose_reference_frame", &MoveGroupInterfaceWrapper::getPoseReferenceFrameCStr); - - move_group_interface_class.def("set_pose_target", &MoveGroupInterfaceWrapper::setPoseTargetPython); - - move_group_interface_class.def("set_pose_targets", &MoveGroupInterfaceWrapper::setPoseTargetsPython); - - move_group_interface_class.def("set_position_target", &MoveGroupInterfaceWrapper::setPositionTarget); - move_group_interface_class.def("set_rpy_target", &MoveGroupInterfaceWrapper::setRPYTarget); - move_group_interface_class.def("set_orientation_target", &MoveGroupInterfaceWrapper::setOrientationTarget); - - move_group_interface_class.def("get_current_pose", &MoveGroupInterfaceWrapper::getCurrentPosePython); - move_group_interface_class.def("get_current_rpy", &MoveGroupInterfaceWrapper::getCurrentRPYPython); - - move_group_interface_class.def("get_random_pose", &MoveGroupInterfaceWrapper::getRandomPosePython); - - move_group_interface_class.def("clear_pose_target", &MoveGroupInterfaceWrapper::clearPoseTarget); - move_group_interface_class.def("clear_pose_targets", &MoveGroupInterfaceWrapper::clearPoseTargets); - - move_group_interface_class.def("set_joint_value_target", - &MoveGroupInterfaceWrapper::setJointValueTargetPythonIterable); - move_group_interface_class.def("set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonDict); - - move_group_interface_class.def("set_joint_value_target", - &MoveGroupInterfaceWrapper::setJointValueTargetPerJointPythonList); - bool (MoveGroupInterfaceWrapper::*set_joint_value_target_4)(const std::string&, double) = - &MoveGroupInterfaceWrapper::setJointValueTarget; - move_group_interface_class.def("set_joint_value_target", set_joint_value_target_4); - - move_group_interface_class.def("set_joint_value_target_from_pose", - &MoveGroupInterfaceWrapper::setJointValueTargetFromPosePython); - move_group_interface_class.def("set_joint_value_target_from_pose_stamped", - &MoveGroupInterfaceWrapper::setJointValueTargetFromPoseStampedPython); - move_group_interface_class.def("set_joint_value_target_from_joint_state_message", - &MoveGroupInterfaceWrapper::setJointValueTargetFromJointStatePython); - - move_group_interface_class.def("get_joint_value_target", &MoveGroupInterfaceWrapper::getJointValueTargetPythonList); - - move_group_interface_class.def("set_named_target", &MoveGroupInterfaceWrapper::setNamedTarget); - move_group_interface_class.def("set_random_target", &MoveGroupInterfaceWrapper::setRandomTarget); - - void (MoveGroupInterfaceWrapper::*remember_joint_values_2)(const std::string&) = - &MoveGroupInterfaceWrapper::rememberJointValues; - move_group_interface_class.def("remember_joint_values", remember_joint_values_2); - - move_group_interface_class.def("remember_joint_values", - &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList); - - move_group_interface_class.def("start_state_monitor", &MoveGroupInterfaceWrapper::startStateMonitor); - move_group_interface_class.def("get_current_joint_values", &MoveGroupInterfaceWrapper::getCurrentJointValuesList); - move_group_interface_class.def("get_random_joint_values", &MoveGroupInterfaceWrapper::getRandomJointValuesList); - move_group_interface_class.def("get_remembered_joint_values", - &MoveGroupInterfaceWrapper::getRememberedJointValuesPython); - - move_group_interface_class.def("forget_joint_values", &MoveGroupInterfaceWrapper::forgetJointValues); - - move_group_interface_class.def("get_goal_joint_tolerance", &MoveGroupInterfaceWrapper::getGoalJointTolerance); - move_group_interface_class.def("get_goal_position_tolerance", &MoveGroupInterfaceWrapper::getGoalPositionTolerance); - move_group_interface_class.def("get_goal_orientation_tolerance", - &MoveGroupInterfaceWrapper::getGoalOrientationTolerance); - - move_group_interface_class.def("set_goal_joint_tolerance", &MoveGroupInterfaceWrapper::setGoalJointTolerance); - move_group_interface_class.def("set_goal_position_tolerance", &MoveGroupInterfaceWrapper::setGoalPositionTolerance); - move_group_interface_class.def("set_goal_orientation_tolerance", - &MoveGroupInterfaceWrapper::setGoalOrientationTolerance); - move_group_interface_class.def("set_goal_tolerance", &MoveGroupInterfaceWrapper::setGoalTolerance); - - move_group_interface_class.def("set_start_state_to_current_state", - &MoveGroupInterfaceWrapper::setStartStateToCurrentState); - move_group_interface_class.def("set_start_state", &MoveGroupInterfaceWrapper::setStartStatePython); - - bool (MoveGroupInterfaceWrapper::*set_path_constraints_1)(const std::string&) = - &MoveGroupInterfaceWrapper::setPathConstraints; - move_group_interface_class.def("set_path_constraints", set_path_constraints_1); - move_group_interface_class.def("set_path_constraints_from_msg", - &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg); - move_group_interface_class.def("get_path_constraints", &MoveGroupInterfaceWrapper::getPathConstraintsPython); - move_group_interface_class.def("clear_path_constraints", &MoveGroupInterfaceWrapper::clearPathConstraints); - move_group_interface_class.def("get_known_constraints", &MoveGroupInterfaceWrapper::getKnownConstraintsList); - move_group_interface_class.def("set_constraints_database", &MoveGroupInterfaceWrapper::setConstraintsDatabase); - move_group_interface_class.def("set_workspace", &MoveGroupInterfaceWrapper::setWorkspace); - move_group_interface_class.def("set_planning_time", &MoveGroupInterfaceWrapper::setPlanningTime); - move_group_interface_class.def("get_planning_time", &MoveGroupInterfaceWrapper::getPlanningTime); - move_group_interface_class.def("set_max_velocity_scaling_factor", - &MoveGroupInterfaceWrapper::setMaxVelocityScalingFactor); - move_group_interface_class.def("set_max_acceleration_scaling_factor", - &MoveGroupInterfaceWrapper::setMaxAccelerationScalingFactor); - move_group_interface_class.def("set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId); - move_group_interface_class.def("set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts); - move_group_interface_class.def("plan", &MoveGroupInterfaceWrapper::planPython); - move_group_interface_class.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython); - move_group_interface_class.def("compute_cartesian_path", - &MoveGroupInterfaceWrapper::computeCartesianPathConstrainedPython); - move_group_interface_class.def("set_support_surface_name", &MoveGroupInterfaceWrapper::setSupportSurfaceName); - move_group_interface_class.def("attach_object", &MoveGroupInterfaceWrapper::attachObjectPython); - move_group_interface_class.def("detach_object", &MoveGroupInterfaceWrapper::detachObject); - move_group_interface_class.def("retime_trajectory", &MoveGroupInterfaceWrapper::retimeTrajectory); - move_group_interface_class.def("get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython); - move_group_interface_class.def("get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython); - move_group_interface_class.def("get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython); + bp::class_ moveit_cpp_class( + "MoveItCpp", bp::init>()); + + moveit_cpp_class.def("async_move", &MoveItCppWrapper::asyncMovePython); + moveit_cpp_class.def("move", &MoveItCppWrapper::movePython); + moveit_cpp_class.def("execute", &MoveItCppWrapper::executePython); + moveit_cpp_class.def("async_execute", &MoveItCppWrapper::asyncExecutePython); + moveit::planning_interface::MoveItErrorCode (MoveItCppWrapper::*pick_1)(const std::string&, bool) = + &MoveItCppWrapper::pick; + moveit_cpp_class.def("pick", pick_1); + moveit_cpp_class.def("pick", &MoveItCppWrapper::pickGrasp); + moveit_cpp_class.def("pick", &MoveItCppWrapper::pickGrasps); + moveit_cpp_class.def("place", &MoveItCppWrapper::placePose); + moveit_cpp_class.def("place", &MoveItCppWrapper::placeLocation); + moveit_cpp_class.def("place", &MoveItCppWrapper::placeAnywhere); + moveit_cpp_class.def("stop", &MoveItCppWrapper::stop); + + moveit_cpp_class.def("get_name", &MoveItCppWrapper::getNameCStr); + moveit_cpp_class.def("get_planning_frame", &MoveItCppWrapper::getPlanningFrameCStr); + moveit_cpp_class.def("get_interface_description", + &MoveItCppWrapper::getInterfaceDescriptionPython); + + moveit_cpp_class.def("get_active_joints", &MoveItCppWrapper::getActiveJointsList); + moveit_cpp_class.def("get_joints", &MoveItCppWrapper::getJointsList); + moveit_cpp_class.def("get_variable_count", &MoveItCppWrapper::getVariableCount); + moveit_cpp_class.def("allow_looking", &MoveItCppWrapper::allowLooking); + moveit_cpp_class.def("allow_replanning", &MoveItCppWrapper::allowReplanning); + + moveit_cpp_class.def("set_pose_reference_frame", &MoveItCppWrapper::setPoseReferenceFrame); + + moveit_cpp_class.def("set_pose_reference_frame", &MoveItCppWrapper::setPoseReferenceFrame); + moveit_cpp_class.def("set_end_effector_link", &MoveItCppWrapper::setEndEffectorLink); + moveit_cpp_class.def("get_end_effector_link", &MoveItCppWrapper::getEndEffectorLinkCStr); + moveit_cpp_class.def("get_pose_reference_frame", &MoveItCppWrapper::getPoseReferenceFrameCStr); + + moveit_cpp_class.def("set_pose_target", &MoveItCppWrapper::setPoseTargetPython); + + moveit_cpp_class.def("set_pose_targets", &MoveItCppWrapper::setPoseTargetsPython); + + moveit_cpp_class.def("set_position_target", &MoveItCppWrapper::setPositionTarget); + moveit_cpp_class.def("set_rpy_target", &MoveItCppWrapper::setRPYTarget); + moveit_cpp_class.def("set_orientation_target", &MoveItCppWrapper::setOrientationTarget); + + moveit_cpp_class.def("get_current_pose", &MoveItCppWrapper::getCurrentPosePython); + moveit_cpp_class.def("get_current_rpy", &MoveItCppWrapper::getCurrentRPYPython); + + moveit_cpp_class.def("get_random_pose", &MoveItCppWrapper::getRandomPosePython); + + moveit_cpp_class.def("clear_pose_target", &MoveItCppWrapper::clearPoseTarget); + moveit_cpp_class.def("clear_pose_targets", &MoveItCppWrapper::clearPoseTargets); + + moveit_cpp_class.def("set_joint_value_target", + &MoveItCppWrapper::setJointValueTargetPythonIterable); + moveit_cpp_class.def("set_joint_value_target", &MoveItCppWrapper::setJointValueTargetPythonDict); + + moveit_cpp_class.def("set_joint_value_target", + &MoveItCppWrapper::setJointValueTargetPerJointPythonList); + bool (MoveItCppWrapper::*set_joint_value_target_4)(const std::string&, double) = + &MoveItCppWrapper::setJointValueTarget; + moveit_cpp_class.def("set_joint_value_target", set_joint_value_target_4); + + moveit_cpp_class.def("set_joint_value_target_from_pose", + &MoveItCppWrapper::setJointValueTargetFromPosePython); + moveit_cpp_class.def("set_joint_value_target_from_pose_stamped", + &MoveItCppWrapper::setJointValueTargetFromPoseStampedPython); + moveit_cpp_class.def("set_joint_value_target_from_joint_state_message", + &MoveItCppWrapper::setJointValueTargetFromJointStatePython); + + moveit_cpp_class.def("get_joint_value_target", &MoveItCppWrapper::getJointValueTargetPythonList); + + moveit_cpp_class.def("set_named_target", &MoveItCppWrapper::setNamedTarget); + moveit_cpp_class.def("set_random_target", &MoveItCppWrapper::setRandomTarget); + + void (MoveItCppWrapper::*remember_joint_values_2)(const std::string&) = + &MoveItCppWrapper::rememberJointValues; + moveit_cpp_class.def("remember_joint_values", remember_joint_values_2); + + moveit_cpp_class.def("remember_joint_values", + &MoveItCppWrapper::rememberJointValuesFromPythonList); + + moveit_cpp_class.def("start_state_monitor", &MoveItCppWrapper::startStateMonitor); + moveit_cpp_class.def("get_current_joint_values", &MoveItCppWrapper::getCurrentJointValuesList); + moveit_cpp_class.def("get_random_joint_values", &MoveItCppWrapper::getRandomJointValuesList); + moveit_cpp_class.def("get_remembered_joint_values", + &MoveItCppWrapper::getRememberedJointValuesPython); + + moveit_cpp_class.def("forget_joint_values", &MoveItCppWrapper::forgetJointValues); + + moveit_cpp_class.def("get_goal_joint_tolerance", &MoveItCppWrapper::getGoalJointTolerance); + moveit_cpp_class.def("get_goal_position_tolerance", &MoveItCppWrapper::getGoalPositionTolerance); + moveit_cpp_class.def("get_goal_orientation_tolerance", + &MoveItCppWrapper::getGoalOrientationTolerance); + + moveit_cpp_class.def("set_goal_joint_tolerance", &MoveItCppWrapper::setGoalJointTolerance); + moveit_cpp_class.def("set_goal_position_tolerance", &MoveItCppWrapper::setGoalPositionTolerance); + moveit_cpp_class.def("set_goal_orientation_tolerance", + &MoveItCppWrapper::setGoalOrientationTolerance); + moveit_cpp_class.def("set_goal_tolerance", &MoveItCppWrapper::setGoalTolerance); + + moveit_cpp_class.def("set_start_state_to_current_state", + &MoveItCppWrapper::setStartStateToCurrentState); + moveit_cpp_class.def("set_start_state", &MoveItCppWrapper::setStartStatePython); + + bool (MoveItCppWrapper::*set_path_constraints_1)(const std::string&) = + &MoveItCppWrapper::setPathConstraints; + moveit_cpp_class.def("set_path_constraints", set_path_constraints_1); + moveit_cpp_class.def("set_path_constraints_from_msg", + &MoveItCppWrapper::setPathConstraintsFromMsg); + moveit_cpp_class.def("get_path_constraints", &MoveItCppWrapper::getPathConstraintsPython); + moveit_cpp_class.def("clear_path_constraints", &MoveItCppWrapper::clearPathConstraints); + moveit_cpp_class.def("get_known_constraints", &MoveItCppWrapper::getKnownConstraintsList); + moveit_cpp_class.def("set_constraints_database", &MoveItCppWrapper::setConstraintsDatabase); + moveit_cpp_class.def("set_workspace", &MoveItCppWrapper::setWorkspace); + moveit_cpp_class.def("set_planning_time", &MoveItCppWrapper::setPlanningTime); + moveit_cpp_class.def("get_planning_time", &MoveItCppWrapper::getPlanningTime); + moveit_cpp_class.def("set_max_velocity_scaling_factor", + &MoveItCppWrapper::setMaxVelocityScalingFactor); + moveit_cpp_class.def("set_max_acceleration_scaling_factor", + &MoveItCppWrapper::setMaxAccelerationScalingFactor); + moveit_cpp_class.def("set_planner_id", &MoveItCppWrapper::setPlannerId); + moveit_cpp_class.def("set_num_planning_attempts", &MoveItCppWrapper::setNumPlanningAttempts); + moveit_cpp_class.def("plan", &MoveItCppWrapper::planPython); + moveit_cpp_class.def("compute_cartesian_path", &MoveItCppWrapper::computeCartesianPathPython); + moveit_cpp_class.def("compute_cartesian_path", + &MoveItCppWrapper::computeCartesianPathConstrainedPython); + moveit_cpp_class.def("set_support_surface_name", &MoveItCppWrapper::setSupportSurfaceName); + moveit_cpp_class.def("attach_object", &MoveItCppWrapper::attachObjectPython); + moveit_cpp_class.def("detach_object", &MoveItCppWrapper::detachObject); + moveit_cpp_class.def("retime_trajectory", &MoveItCppWrapper::retimeTrajectory); + moveit_cpp_class.def("get_named_targets", &MoveItCppWrapper::getNamedTargetsPython); + moveit_cpp_class.def("get_named_target_values", &MoveItCppWrapper::getNamedTargetValuesPython); + moveit_cpp_class.def("get_current_state_bounded", &MoveItCppWrapper::getCurrentStateBoundedPython); } } // namespace planning_interface } // namespace moveit -BOOST_PYTHON_MODULE(_moveit_move_group_interface) +BOOST_PYTHON_MODULE(_moveit_moveit_cpp) { using namespace moveit::planning_interface; - wrap_move_group_interface(); + wrap_moveit_cpp(); } /** @endcond */ From 2ad3c9476e1dc9ea85a68543824e939d16de95dc Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Tue, 30 Jul 2019 18:55:59 -0600 Subject: [PATCH 03/29] Remove impl --- .../include/moveit/moveit_cpp/moveit_cpp.h | 152 +- .../moveit_cpp/src/moveit_cpp.cpp | 2398 +++++++---------- 2 files changed, 1186 insertions(+), 1364 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index d9fce76f78..06668eb8e9 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -33,13 +33,16 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan, Sachin Chitta */ +/* Author: Ioan Sucan, Sachin Chitta, simon Goldstein */ #ifndef MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ #define MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ +#include #include #include +#include +#include #include #include #include @@ -49,16 +52,53 @@ #include #include #include +#include +#include +#include #include +#include #include #include #include +namespace planning_scene_monitor +{ +MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); +} + +namespace planning_pipeline +{ +MOVEIT_CLASS_FORWARD(PlanningPipeline); +} + +namespace plan_execution +{ +MOVEIT_CLASS_FORWARD(PlanExecution); +MOVEIT_CLASS_FORWARD(PlanWithSensing); +} + +namespace trajectory_execution_manager +{ +MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); +} + +namespace +{ +enum ActiveTargetType +{ + JOINT, + POSE, + POSITION, + ORIENTATION +}; +} + namespace moveit { /** \brief Simple interface to MoveIt! components */ namespace planning_interface { + class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes { public: @@ -90,7 +130,7 @@ class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes MOVEIT_CLASS_FORWARD(MoveItCpp); -/** \class MoveItCpp moveit_cpp.h moveit/planning_interface/moveit_cpp.h +/** \class MoveItCpp move_group_interface.h moveit/planning_interface/move_group_interface.h \brief Client class to conveniently use the ROS interfaces provided by the move_group node. @@ -104,7 +144,7 @@ class MoveItCpp /** \brief Specification of options to use when constructing the MoveItCpp class */ struct Options { - Options(const std::string& group_name, const std::string& desc = ROBOT_DESCRIPTION, + Options(const std::string& group_name="FAKE", const std::string& desc = ROBOT_DESCRIPTION, const ros::NodeHandle& node_handle = ros::NodeHandle()) : group_name_(group_name), robot_description_(desc), node_handle_(node_handle) { @@ -957,8 +997,110 @@ class MoveItCpp std::map > remembered_joint_values_; class MoveItCppImpl; MoveItCppImpl* impl_; + + bool status() const; + + // New stuff + template + void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time); + MoveItErrorCode move(bool wait); + void constructGoal(moveit_msgs::MoveGroupGoal& goal); + void constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object); + void constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object); + MoveItErrorCode execute(const Plan& plan, bool wait); + bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds); + bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link, + const std::string& frame, bool approx); + robot_state::RobotStatePtr getStartState(); + bool hasPoseTarget(const std::string& end_effector_link) const; + void clearContents(); + //void initializeConstraintsStorage(const std::string& host, unsigned int port); + + // context contents + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; + planning_pipeline::PlanningPipelinePtr planning_pipeline_; + plan_execution::PlanExecutionPtr plan_execution_; + plan_execution::PlanWithSensingPtr plan_with_sensing_; + bool allow_trajectory_execution_; + bool debug_; + + // impl contents + void initializeConstraintsStorageThread(const std::string& host, unsigned int port); + /*{ + // Set up db + try + { + warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase(); + conn->setParams(host, port); + if (conn->connect()) + { + constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(conn)); + } + } + catch (std::exception& ex) + { + ROS_ERROR_NAMED("move_group_interface", "%s", ex.what()); + } + initializing_constraints_ = false; + } */ + + // Options + std::string group_name_; + std::string robot_description_; + ros::NodeHandle node_handle_; + std::shared_ptr tf_buffer_; + robot_model::RobotModelConstPtr robot_model_; + planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; + std::unique_ptr > move_action_client_; + std::unique_ptr > execute_action_client_; + std::unique_ptr > pick_action_client_; + std::unique_ptr > place_action_client_; + + // general planning params + robot_state::RobotStatePtr considered_start_state_; + moveit_msgs::WorkspaceParameters workspace_parameters_; + double allowed_planning_time_; + std::string planner_id_; + unsigned int num_planning_attempts_; + double max_velocity_scaling_factor_; + double max_acceleration_scaling_factor_; + double goal_joint_tolerance_; + double goal_position_tolerance_; + double goal_orientation_tolerance_; + bool can_look_; + bool can_replan_; + double replan_delay_; + + // joint state goal + robot_state::RobotStatePtr joint_state_target_; + const robot_model::JointModelGroup* joint_model_group_; + + // pose goal; + // for each link we have a set of possible goal locations; + std::map > pose_targets_; + + // common properties for goals + ActiveTargetType active_target_; + std::unique_ptr path_constraints_; + std::unique_ptr trajectory_constraints_; + std::string end_effector_link_; + std::string pose_reference_frame_; + std::string support_surface_; + + // ROS communication + ros::Publisher trajectory_event_publisher_; + ros::Publisher attached_object_publisher_; + ros::ServiceClient query_service_; + ros::ServiceClient get_params_service_; + ros::ServiceClient set_params_service_; + ros::ServiceClient cartesian_path_service_; + ros::ServiceClient plan_grasps_service_; + std::unique_ptr constraints_storage_; + std::unique_ptr constraints_init_thread_; + bool initializing_constraints_; }; -} -} +} // namespace planning_interface +} // namespace moveit #endif diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index 8f2957837e..dd454a85a1 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -1,6 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * + * Copyright (c) 2019, PickNik Robotics * Copyright (c) 2014, SRI International * Copyright (c) 2013, Ioan A. Sucan * Copyright (c) 2012, Willow Garage, Inc. @@ -34,7 +35,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan, Sachin Chitta */ +/* Author: Ioan Sucan, Sachin Chitta, Simon Goldstien */ #include #include @@ -72,1230 +73,113 @@ namespace moveit { namespace planning_interface { -const std::string MoveItCpp::ROBOT_DESCRIPTION = - "robot_description"; // name of the robot description (a param name, so it can be changed externally) - -const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps - -namespace -{ -enum ActiveTargetType -{ - JOINT, - POSE, - POSITION, - ORIENTATION -}; -} - -class MoveItCpp::MoveItCppImpl -{ -public: - MoveItCppImpl(const Options& opt, const std::shared_ptr& tf_buffer, - const ros::WallDuration& wait_for_servers) - : opt_(opt), node_handle_(opt.node_handle_), tf_buffer_(tf_buffer) - { - robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_); - if (!getRobotModel()) - { - std::string error = "Unable to construct robot model. Please make sure all needed information is on the " - "parameter server."; - ROS_FATAL_STREAM_NAMED("moveit_cpp", error); - throw std::runtime_error(error); - } - - if (!getRobotModel()->hasJointModelGroup(opt.group_name_)) - { - std::string error = "Group '" + opt.group_name_ + "' was not found."; - ROS_FATAL_STREAM_NAMED("moveit_cpp", error); - throw std::runtime_error(error); - } - - joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_); - - joint_state_target_.reset(new robot_state::RobotState(getRobotModel())); - joint_state_target_->setToDefaultValues(); - active_target_ = JOINT; - can_look_ = false; - can_replan_ = false; - replan_delay_ = 2.0; - goal_joint_tolerance_ = 1e-4; - goal_position_tolerance_ = 1e-4; // 0.1 mm - goal_orientation_tolerance_ = 1e-3; // ~0.1 deg - allowed_planning_time_ = 5.0; - num_planning_attempts_ = 1; - max_velocity_scaling_factor_ = 1.0; - max_acceleration_scaling_factor_ = 1.0; - initializing_constraints_ = false; - - if (joint_model_group_->isChain()) - end_effector_link_ = joint_model_group_->getLinkModelNames().back(); - pose_reference_frame_ = getRobotModel()->getModelFrame(); - - trajectory_event_publisher_ = node_handle_.advertise( - trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false); - attached_object_publisher_ = node_handle_.advertise( - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1, false); - - current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_buffer_, node_handle_); - - ros::WallTime timeout_for_servers = ros::WallTime::now() + wait_for_servers; - if (wait_for_servers == ros::WallDuration()) - timeout_for_servers = ros::WallTime(); // wait for ever - double allotted_time = wait_for_servers.toSec(); - - move_action_client_.reset( - new actionlib::SimpleActionClient(node_handle_, move_group::MOVE_ACTION, false)); - waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time); - - pick_action_client_.reset( - new actionlib::SimpleActionClient(node_handle_, move_group::PICKUP_ACTION, false)); - waitForAction(pick_action_client_, move_group::PICKUP_ACTION, timeout_for_servers, allotted_time); - - place_action_client_.reset( - new actionlib::SimpleActionClient(node_handle_, move_group::PLACE_ACTION, false)); - waitForAction(place_action_client_, move_group::PLACE_ACTION, timeout_for_servers, allotted_time); - - execute_action_client_.reset(new actionlib::SimpleActionClient( - node_handle_, move_group::EXECUTE_ACTION_NAME, false)); - waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time); - - query_service_ = - node_handle_.serviceClient(move_group::QUERY_PLANNERS_SERVICE_NAME); - get_params_service_ = - node_handle_.serviceClient(move_group::GET_PLANNER_PARAMS_SERVICE_NAME); - set_params_service_ = - node_handle_.serviceClient(move_group::SET_PLANNER_PARAMS_SERVICE_NAME); - - cartesian_path_service_ = - node_handle_.serviceClient(move_group::CARTESIAN_PATH_SERVICE_NAME); - - plan_grasps_service_ = node_handle_.serviceClient(GRASP_PLANNING_SERVICE_NAME); - - ROS_INFO_STREAM_NAMED("moveit_cpp", "Ready to take commands for planning group " << opt.group_name_ - << "."); - } - - template - void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) - { - ROS_DEBUG_NAMED("moveit_cpp", "Waiting for move_group action server (%s)...", name.c_str()); - - // wait for the server (and spin as needed) - if (timeout == ros::WallTime()) // wait forever - { - while (node_handle_.ok() && !action->isServerConnected()) - { - ros::WallDuration(0.001).sleep(); - // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client - ros::CallbackQueue* queue = dynamic_cast(node_handle_.getCallbackQueue()); - if (queue) - { - queue->callAvailable(); - } - else // in case of nodelets and specific callback queue implementations - { - ROS_WARN_ONCE_NAMED("moveit_cpp", "Non-default CallbackQueue: Waiting for external queue " - "handling."); - } - } - } - else // wait with timeout - { - while (node_handle_.ok() && !action->isServerConnected() && timeout > ros::WallTime::now()) - { - ros::WallDuration(0.001).sleep(); - // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client - ros::CallbackQueue* queue = dynamic_cast(node_handle_.getCallbackQueue()); - if (queue) - { - queue->callAvailable(); - } - else // in case of nodelets and specific callback queue implementations - { - ROS_WARN_ONCE_NAMED("moveit_cpp", "Non-default CallbackQueue: Waiting for external queue " - "handling."); - } - } - } - - if (!action->isServerConnected()) - { - std::stringstream error; - error << "Unable to connect to move_group action server '" << name << "' within allotted time (" << allotted_time - << "s)"; - throw std::runtime_error(error.str()); - } - else - { - ROS_DEBUG_NAMED("moveit_cpp", "Connected to '%s'", name.c_str()); - } - } - - ~MoveItCppImpl() - { - if (constraints_init_thread_) - constraints_init_thread_->join(); - } - - const std::shared_ptr& getTF() const - { - return tf_buffer_; - } - - const Options& getOptions() const - { - return opt_; - } - - const robot_model::RobotModelConstPtr& getRobotModel() const - { - return robot_model_; - } - - const robot_model::JointModelGroup* getJointModelGroup() const - { - return joint_model_group_; - } - - actionlib::SimpleActionClient& getMoveGroupClient() const - { - return *move_action_client_; - } - - bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) - { - moveit_msgs::QueryPlannerInterfaces::Request req; - moveit_msgs::QueryPlannerInterfaces::Response res; - if (query_service_.call(req, res)) - if (!res.planner_interfaces.empty()) - { - desc = res.planner_interfaces.front(); - return true; - } - return false; - } - - std::map getPlannerParams(const std::string& planner_id, const std::string& group = "") - { - moveit_msgs::GetPlannerParams::Request req; - moveit_msgs::GetPlannerParams::Response res; - req.planner_config = planner_id; - req.group = group; - std::map result; - if (get_params_service_.call(req, res)) - { - for (unsigned int i = 0, end = res.params.keys.size(); i < end; ++i) - result[res.params.keys[i]] = res.params.values[i]; - } - return result; - } - - void setPlannerParams(const std::string& planner_id, const std::string& group, - const std::map& params, bool replace = false) - { - moveit_msgs::SetPlannerParams::Request req; - moveit_msgs::SetPlannerParams::Response res; - req.planner_config = planner_id; - req.group = group; - req.replace = replace; - for (const std::pair& param : params) - { - req.params.keys.push_back(param.first); - req.params.values.push_back(param.second); - } - set_params_service_.call(req, res); - } - - std::string getDefaultPlannerId(const std::string& group) const - { - std::stringstream param_name; - param_name << "move_group"; - if (!group.empty()) - param_name << "/" << group; - param_name << "/default_planner_config"; - - std::string default_planner_config; - node_handle_.getParam(param_name.str(), default_planner_config); - return default_planner_config; - } - - void setPlannerId(const std::string& planner_id) - { - planner_id_ = planner_id; - } - - const std::string& getPlannerId() const - { - return planner_id_; - } - - void setNumPlanningAttempts(unsigned int num_planning_attempts) - { - num_planning_attempts_ = num_planning_attempts; - } - - void setMaxVelocityScalingFactor(double max_velocity_scaling_factor) - { - max_velocity_scaling_factor_ = max_velocity_scaling_factor; - } - - void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) - { - max_acceleration_scaling_factor_ = max_acceleration_scaling_factor; - } - - robot_state::RobotState& getTargetRobotState() - { - return *joint_state_target_; - } - - void setStartState(const robot_state::RobotState& start_state) - { - considered_start_state_.reset(new robot_state::RobotState(start_state)); - } - - void setStartStateToCurrentState() - { - considered_start_state_.reset(); - } - - robot_state::RobotStatePtr getStartState() - { - if (considered_start_state_) - return considered_start_state_; - else - { - robot_state::RobotStatePtr s; - getCurrentState(s); - return s; - } - } - - bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link, - const std::string& frame, bool approx) - { - const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; - // this only works if we have an end-effector - if (!eef.empty()) - { - // first we set the goal to be the same as the start state - moveit::core::RobotStatePtr c = getStartState(); - if (c) - { - setTargetType(JOINT); - c->enforceBounds(); - getTargetRobotState() = *c; - if (!getTargetRobotState().satisfiesBounds(getGoalJointTolerance())) - return false; - } - else - return false; - - // we may need to do approximate IK - kinematics::KinematicsQueryOptions o; - o.return_approximate_solution = approx; - - // if no frame transforms are needed, call IK directly - if (frame.empty() || moveit::core::Transforms::sameFrame(frame, getRobotModel()->getModelFrame())) - return getTargetRobotState().setFromIK(getJointModelGroup(), eef_pose, eef, 0.0, - moveit::core::GroupStateValidityCallbackFn(), o); - else - { - if (c->knowsFrameTransform(frame)) - { - // transform the pose first if possible, then do IK - const Eigen::Isometry3d& t = getTargetRobotState().getFrameTransform(frame); - Eigen::Isometry3d p; - tf2::fromMsg(eef_pose, p); - return getTargetRobotState().setFromIK(getJointModelGroup(), t * p, eef, 0.0, - moveit::core::GroupStateValidityCallbackFn(), o); - } - else - { - ROS_ERROR_NAMED("moveit_cpp", "Unable to transform from frame '%s' to frame '%s'", frame.c_str(), - getRobotModel()->getModelFrame().c_str()); - return false; - } - } - } - else - return false; - } - - void setEndEffectorLink(const std::string& end_effector) - { - end_effector_link_ = end_effector; - } - - void clearPoseTarget(const std::string& end_effector_link) - { - pose_targets_.erase(end_effector_link); - } - - void clearPoseTargets() - { - pose_targets_.clear(); - } - - const std::string& getEndEffectorLink() const - { - return end_effector_link_; - } - - const std::string& getEndEffector() const - { - if (!end_effector_link_.empty()) - { - const std::vector& possible_eefs = - getRobotModel()->getJointModelGroup(opt_.group_name_)->getAttachedEndEffectorNames(); - for (const std::string& possible_eef : possible_eefs) - if (getRobotModel()->getEndEffector(possible_eef)->hasLinkModel(end_effector_link_)) - return possible_eef; - } - static std::string empty; - return empty; - } - - bool setPoseTargets(const std::vector& poses, const std::string& end_effector_link) - { - const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; - if (eef.empty()) - { - ROS_ERROR_NAMED("moveit_cpp", "No end-effector to set the pose for"); - return false; - } - else - { - pose_targets_[eef] = poses; - // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors - std::vector& stored_poses = pose_targets_[eef]; - for (geometry_msgs::PoseStamped& stored_pose : stored_poses) - stored_pose.header.stamp = ros::Time(0); - } - return true; - } - - bool hasPoseTarget(const std::string& end_effector_link) const - { - const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; - return pose_targets_.find(eef) != pose_targets_.end(); - } - - const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link) const - { - const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; - - // if multiple pose targets are set, return the first one - std::map >::const_iterator jt = pose_targets_.find(eef); - if (jt != pose_targets_.end()) - if (!jt->second.empty()) - return jt->second.at(0); - - // or return an error - static const geometry_msgs::PoseStamped UNKNOWN; - ROS_ERROR_NAMED("moveit_cpp", "Pose for end-effector '%s' not known.", eef.c_str()); - return UNKNOWN; - } - - const std::vector& getPoseTargets(const std::string& end_effector_link) const - { - const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; - - std::map >::const_iterator jt = pose_targets_.find(eef); - if (jt != pose_targets_.end()) - if (!jt->second.empty()) - return jt->second; - - // or return an error - static const std::vector EMPTY; - ROS_ERROR_NAMED("moveit_cpp", "Poses for end-effector '%s' are not known.", eef.c_str()); - return EMPTY; - } - - void setPoseReferenceFrame(const std::string& pose_reference_frame) - { - pose_reference_frame_ = pose_reference_frame; - } - - void setSupportSurfaceName(const std::string& support_surface) - { - support_surface_ = support_surface; - } - - const std::string& getPoseReferenceFrame() const - { - return pose_reference_frame_; - } - - void setTargetType(ActiveTargetType type) - { - active_target_ = type; - } - - ActiveTargetType getTargetType() const - { - return active_target_; - } - - bool startStateMonitor(double wait) - { - if (!current_state_monitor_) - { - ROS_ERROR_NAMED("moveit_cpp", "Unable to monitor current robot state"); - return false; - } - - // if needed, start the monitor and wait up to 1 second for a full robot state - if (!current_state_monitor_->isActive()) - current_state_monitor_->startStateMonitor(); - - current_state_monitor_->waitForCompleteState(opt_.group_name_, wait); - return true; - } - - bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds = 1.0) - { - if (!current_state_monitor_) - { - ROS_ERROR_NAMED("moveit_cpp", "Unable to get current robot state"); - return false; - } - - // if needed, start the monitor and wait up to 1 second for a full robot state - if (!current_state_monitor_->isActive()) - current_state_monitor_->startStateMonitor(); - - if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds)) - { - ROS_ERROR_NAMED("moveit_cpp", "Failed to fetch current robot state"); - return false; - } - - current_state = current_state_monitor_->getCurrentState(); - return true; - } - - /** \brief Place an object at one of the specified possible locations */ - MoveItErrorCode place(const std::string& object, const std::vector& poses, - bool plan_only = false) - { - std::vector locations; - for (const geometry_msgs::PoseStamped& pose : poses) - { - moveit_msgs::PlaceLocation location; - location.pre_place_approach.direction.vector.z = -1.0; - location.post_place_retreat.direction.vector.x = -1.0; - location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame(); - location.post_place_retreat.direction.header.frame_id = end_effector_link_; - - location.pre_place_approach.min_distance = 0.1; - location.pre_place_approach.desired_distance = 0.2; - location.post_place_retreat.min_distance = 0.0; - location.post_place_retreat.desired_distance = 0.2; - // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody - - location.place_pose = pose; - locations.push_back(location); - } - ROS_DEBUG_NAMED("moveit_cpp", "Move group interface has %u place locations", - (unsigned int)locations.size()); - return place(object, locations, plan_only); - } - - MoveItErrorCode place(const std::string& object, const std::vector& locations, - bool plan_only = false) - { - if (!place_action_client_) - { - ROS_ERROR_STREAM_NAMED("moveit_cpp", "Place action client not found"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - if (!place_action_client_->isServerConnected()) - { - ROS_ERROR_STREAM_NAMED("moveit_cpp", "Place action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - moveit_msgs::PlaceGoal goal; - constructGoal(goal, object); - goal.place_locations = locations; - goal.planning_options.plan_only = plan_only; - goal.planning_options.look_around = can_look_; - goal.planning_options.replan = can_replan_; - goal.planning_options.replan_delay = replan_delay_; - goal.planning_options.planning_scene_diff.is_diff = true; - goal.planning_options.planning_scene_diff.robot_state.is_diff = true; - - place_action_client_->sendGoal(goal); - ROS_DEBUG_NAMED("moveit_cpp", "Sent place goal with %d locations", (int)goal.place_locations.size()); - if (!place_action_client_->waitForResult()) - { - ROS_INFO_STREAM_NAMED("moveit_cpp", "Place action returned early"); - } - if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) - { - return MoveItErrorCode(place_action_client_->getResult()->error_code); - } - else - { - ROS_WARN_STREAM_NAMED("moveit_cpp", "Fail: " << place_action_client_->getState().toString() << ": " - << place_action_client_->getState().getText()); - return MoveItErrorCode(place_action_client_->getResult()->error_code); - } - } - - MoveItErrorCode pick(const std::string& object, const std::vector& grasps, bool plan_only = false) - { - if (!pick_action_client_) - { - ROS_ERROR_STREAM_NAMED("moveit_cpp", "Pick action client not found"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - if (!pick_action_client_->isServerConnected()) - { - ROS_ERROR_STREAM_NAMED("moveit_cpp", "Pick action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - moveit_msgs::PickupGoal goal; - constructGoal(goal, object); - goal.possible_grasps = grasps; - goal.planning_options.plan_only = plan_only; - goal.planning_options.look_around = can_look_; - goal.planning_options.replan = can_replan_; - goal.planning_options.replan_delay = replan_delay_; - goal.planning_options.planning_scene_diff.is_diff = true; - goal.planning_options.planning_scene_diff.robot_state.is_diff = true; - - pick_action_client_->sendGoal(goal); - if (!pick_action_client_->waitForResult()) - { - ROS_INFO_STREAM_NAMED("moveit_cpp", "Pickup action returned early"); - } - if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) - { - return MoveItErrorCode(pick_action_client_->getResult()->error_code); - } - else - { - ROS_WARN_STREAM_NAMED("moveit_cpp", "Fail: " << pick_action_client_->getState().toString() << ": " - << pick_action_client_->getState().getText()); - return MoveItErrorCode(pick_action_client_->getResult()->error_code); - } - } - - MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false) - { - if (object.empty()) - { - return planGraspsAndPick(moveit_msgs::CollisionObject()); - } - moveit::planning_interface::PlanningSceneInterface psi; - - std::map objects = psi.getObjects(std::vector(1, object)); - - if (objects.empty()) - { - ROS_ERROR_STREAM_NAMED("moveit_cpp", "Asked for grasps for the object '" - << object << "', but the object could not be found"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME); - } - - return planGraspsAndPick(objects[object], plan_only); - } - - MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false) - { - if (!plan_grasps_service_) - { - ROS_ERROR_STREAM_NAMED("moveit_cpp", "Grasp planning service '" - << GRASP_PLANNING_SERVICE_NAME - << "' is not available." - " This has to be implemented and started separately."); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - - moveit_msgs::GraspPlanning::Request request; - moveit_msgs::GraspPlanning::Response response; - - request.group_name = opt_.group_name_; - request.target = object; - request.support_surfaces.push_back(support_surface_); - - ROS_DEBUG_NAMED("moveit_cpp", "Calling grasp planner..."); - if (!plan_grasps_service_.call(request, response) || - response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS) - { - ROS_ERROR_NAMED("moveit_cpp", "Grasp planning failed. Unable to pick."); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - - return pick(object.id, response.grasps, plan_only); - } - - MoveItErrorCode plan(Plan& plan) - { - if (!move_action_client_) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - if (!move_action_client_->isServerConnected()) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - - moveit_msgs::MoveGroupGoal goal; - constructGoal(goal); - goal.planning_options.plan_only = true; - goal.planning_options.look_around = false; - goal.planning_options.replan = false; - goal.planning_options.planning_scene_diff.is_diff = true; - goal.planning_options.planning_scene_diff.robot_state.is_diff = true; - - move_action_client_->sendGoal(goal); - if (!move_action_client_->waitForResult()) - { - ROS_INFO_STREAM_NAMED("moveit_cpp", "MoveGroup action returned early"); - } - if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) - { - plan.trajectory_ = move_action_client_->getResult()->planned_trajectory; - plan.start_state_ = move_action_client_->getResult()->trajectory_start; - plan.planning_time_ = move_action_client_->getResult()->planning_time; - return MoveItErrorCode(move_action_client_->getResult()->error_code); - } - else - { - ROS_WARN_STREAM_NAMED("moveit_cpp", "Fail: " << move_action_client_->getState().toString() << ": " - << move_action_client_->getState().getText()); - return MoveItErrorCode(move_action_client_->getResult()->error_code); - } - } - - MoveItErrorCode move(bool wait) - { - if (!move_action_client_) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - if (!move_action_client_->isServerConnected()) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - - moveit_msgs::MoveGroupGoal goal; - constructGoal(goal); - goal.planning_options.plan_only = false; - goal.planning_options.look_around = can_look_; - goal.planning_options.replan = can_replan_; - goal.planning_options.replan_delay = replan_delay_; - goal.planning_options.planning_scene_diff.is_diff = true; - goal.planning_options.planning_scene_diff.robot_state.is_diff = true; - - move_action_client_->sendGoal(goal); - if (!wait) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); - } - - if (!move_action_client_->waitForResult()) - { - ROS_INFO_STREAM_NAMED("moveit_cpp", "MoveGroup action returned early"); - } - - if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) - { - return MoveItErrorCode(move_action_client_->getResult()->error_code); - } - else - { - ROS_INFO_STREAM_NAMED("moveit_cpp", move_action_client_->getState().toString() - << ": " << move_action_client_->getState().getText()); - return MoveItErrorCode(move_action_client_->getResult()->error_code); - } - } - - MoveItErrorCode execute(const Plan& plan, bool wait) - { - if (!execute_action_client_->isServerConnected()) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - - moveit_msgs::ExecuteTrajectoryGoal goal; - goal.trajectory = plan.trajectory_; - - execute_action_client_->sendGoal(goal); - if (!wait) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); - } - - if (!execute_action_client_->waitForResult()) - { - ROS_INFO_STREAM_NAMED("moveit_cpp", "ExecuteTrajectory action returned early"); - } - - if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) - { - return MoveItErrorCode(execute_action_client_->getResult()->error_code); - } - else - { - ROS_INFO_STREAM_NAMED("moveit_cpp", execute_action_client_->getState().toString() - << ": " << execute_action_client_->getState().getText()); - return MoveItErrorCode(execute_action_client_->getResult()->error_code); - } - } - - double computeCartesianPath(const std::vector& waypoints, double step, double jump_threshold, - moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints, - bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code) - { - moveit_msgs::GetCartesianPath::Request req; - moveit_msgs::GetCartesianPath::Response res; - - if (considered_start_state_) - robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state); - else - req.start_state.is_diff = true; - - req.group_name = opt_.group_name_; - req.header.frame_id = getPoseReferenceFrame(); - req.header.stamp = ros::Time::now(); - req.waypoints = waypoints; - req.max_step = step; - req.jump_threshold = jump_threshold; - req.path_constraints = path_constraints; - req.avoid_collisions = avoid_collisions; - req.link_name = getEndEffectorLink(); - - if (cartesian_path_service_.call(req, res)) - { - error_code = res.error_code; - if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) - { - msg = res.solution; - return res.fraction; - } - else - return -1.0; - } - else - { - error_code.val = error_code.FAILURE; - return -1.0; - } - } - - void stop() - { - if (trajectory_event_publisher_) - { - std_msgs::String event; - event.data = "stop"; - trajectory_event_publisher_.publish(event); - } - } - - bool attachObject(const std::string& object, const std::string& link, const std::vector& touch_links) - { - std::string l = link.empty() ? getEndEffectorLink() : link; - if (l.empty()) - { - const std::vector& links = joint_model_group_->getLinkModelNames(); - if (!links.empty()) - l = links[0]; - } - if (l.empty()) - { - ROS_ERROR_NAMED("moveit_cpp", "No known link to attach object '%s' to", object.c_str()); - return false; - } - moveit_msgs::AttachedCollisionObject aco; - aco.object.id = object; - aco.link_name.swap(l); - if (touch_links.empty()) - aco.touch_links.push_back(aco.link_name); - else - aco.touch_links = touch_links; - aco.object.operation = moveit_msgs::CollisionObject::ADD; - attached_object_publisher_.publish(aco); - return true; - } - - bool detachObject(const std::string& name) - { - moveit_msgs::AttachedCollisionObject aco; - // if name is a link - if (!name.empty() && joint_model_group_->hasLinkModel(name)) - aco.link_name = name; - else - aco.object.id = name; - aco.object.operation = moveit_msgs::CollisionObject::REMOVE; - if (aco.link_name.empty() && aco.object.id.empty()) - { - // we only want to detach objects for this group - const std::vector& lnames = joint_model_group_->getLinkModelNames(); - for (const std::string& lname : lnames) - { - aco.link_name = lname; - attached_object_publisher_.publish(aco); - } - } - else - attached_object_publisher_.publish(aco); - return true; - } - - double getGoalPositionTolerance() const - { - return goal_position_tolerance_; - } - - double getGoalOrientationTolerance() const - { - return goal_orientation_tolerance_; - } - - double getGoalJointTolerance() const - { - return goal_joint_tolerance_; - } - - void setGoalJointTolerance(double tolerance) - { - goal_joint_tolerance_ = tolerance; - } - - void setGoalPositionTolerance(double tolerance) - { - goal_position_tolerance_ = tolerance; - } - - void setGoalOrientationTolerance(double tolerance) - { - goal_orientation_tolerance_ = tolerance; - } - - void setPlanningTime(double seconds) - { - if (seconds > 0.0) - allowed_planning_time_ = seconds; - } - - double getPlanningTime() const - { - return allowed_planning_time_; - } - - void allowLooking(bool flag) - { - can_look_ = flag; - ROS_INFO_NAMED("moveit_cpp", "Looking around: %s", can_look_ ? "yes" : "no"); - } - - void allowReplanning(bool flag) - { - can_replan_ = flag; - ROS_INFO_NAMED("moveit_cpp", "Replanning: %s", can_replan_ ? "yes" : "no"); - } - - void setReplanningDelay(double delay) - { - if (delay >= 0.0) - replan_delay_ = delay; - } - - double getReplanningDelay() const - { - return replan_delay_; - } - - void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request) - { - request.group_name = opt_.group_name_; - request.num_planning_attempts = num_planning_attempts_; - request.max_velocity_scaling_factor = max_velocity_scaling_factor_; - request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_; - request.allowed_planning_time = allowed_planning_time_; - request.planner_id = planner_id_; - request.workspace_parameters = workspace_parameters_; - - if (considered_start_state_) - robot_state::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); - else - request.start_state.is_diff = true; - - if (active_target_ == JOINT) - { - request.goal_constraints.resize(1); - request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( - getTargetRobotState(), joint_model_group_, goal_joint_tolerance_); - } - else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION) - { - // find out how many goals are specified - std::size_t goal_count = 0; - for (std::map >::const_iterator it = pose_targets_.begin(); - it != pose_targets_.end(); ++it) - goal_count = std::max(goal_count, it->second.size()); - - // start filling the goals; - // each end effector has a number of possible poses (K) as valid goals - // but there could be multiple end effectors specified, so we want each end effector - // to reach the goal that corresponds to the goals of the other end effectors - request.goal_constraints.resize(goal_count); - - for (std::map >::const_iterator it = pose_targets_.begin(); - it != pose_targets_.end(); ++it) - { - for (std::size_t i = 0; i < it->second.size(); ++i) - { - moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints( - it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_); - if (active_target_ == ORIENTATION) - c.position_constraints.clear(); - if (active_target_ == POSITION) - c.orientation_constraints.clear(); - request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c); - } - } - } - else - ROS_ERROR_NAMED("moveit_cpp", "Unable to construct MotionPlanRequest representation"); - - if (path_constraints_) - request.path_constraints = *path_constraints_; - if (trajectory_constraints_) - request.trajectory_constraints = *trajectory_constraints_; - } - - void constructGoal(moveit_msgs::MoveGroupGoal& goal) - { - constructMotionPlanRequest(goal.request); - } - - void constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object) - { - moveit_msgs::PickupGoal goal; - goal.target_name = object; - goal.group_name = opt_.group_name_; - goal.end_effector = getEndEffector(); - goal.allowed_planning_time = allowed_planning_time_; - goal.support_surface_name = support_surface_; - goal.planner_id = planner_id_; - if (!support_surface_.empty()) - goal.allow_gripper_support_collision = true; - - if (path_constraints_) - goal.path_constraints = *path_constraints_; - - goal_out = goal; - } - void constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object) - { - moveit_msgs::PlaceGoal goal; - goal.attached_object_name = object; - goal.group_name = opt_.group_name_; - goal.allowed_planning_time = allowed_planning_time_; - goal.support_surface_name = support_surface_; - goal.planner_id = planner_id_; - if (!support_surface_.empty()) - goal.allow_gripper_support_collision = true; +const std::string MoveItCpp::ROBOT_DESCRIPTION = + "robot_description"; // name of the robot description (a param name, so it can be changed externally) - if (path_constraints_) - goal.path_constraints = *path_constraints_; +const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps +} // planning_interface +} // moveit +moveit::planning_interface::MoveItCpp::MoveItCpp(const std::string& group_name, + const std::shared_ptr& tf_buffer, + const ros::WallDuration& wait_for_servers) + : MoveItCpp(Options(group_name), tf_buffer, wait_for_servers) +{ +} - goal_out = goal; - } +moveit::planning_interface::MoveItCpp::MoveItCpp(const std::string& group, + const std::shared_ptr& tf_buffer, + const ros::Duration& wait_for_servers) + : MoveItCpp(Options(group), tf_buffer, ros::WallDuration(wait_for_servers.toSec())) +{ +} - void setPathConstraints(const moveit_msgs::Constraints& constraint) +moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, + const std::shared_ptr& tf_buffer, + const ros::WallDuration& wait_for_servers) + : node_handle_(opt.node_handle_), tf_buffer_(tf_buffer) +{ + group_name_ = opt.group_name_; + robot_description_ = opt.robot_description_; + robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_); + if (!getRobotModel()) { - path_constraints_.reset(new moveit_msgs::Constraints(constraint)); + std::string error = "Unable to construct robot model. Please make sure all needed information is on the " + "parameter server."; + ROS_FATAL_STREAM_NAMED("move_group_interface", error); + throw std::runtime_error(error); } - bool setPathConstraints(const std::string& constraint) + if (!getRobotModel()->hasJointModelGroup(opt.group_name_)) { - if (constraints_storage_) - { - moveit_warehouse::ConstraintsWithMetadata msg_m; - if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_)) - { - path_constraints_.reset(new moveit_msgs::Constraints(static_cast(*msg_m))); - return true; - } - else - return false; - } - else - return false; + std::string error = "Group '" + opt.group_name_ + "' was not found."; + ROS_FATAL_STREAM_NAMED("move_group_interface", error); + throw std::runtime_error(error); } - void clearPathConstraints() - { - path_constraints_.reset(); - } + joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_); - void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint) - { - trajectory_constraints_.reset(new moveit_msgs::TrajectoryConstraints(constraint)); - } + joint_state_target_.reset(new robot_state::RobotState(getRobotModel())); + joint_state_target_->setToDefaultValues(); + active_target_ = JOINT; + can_look_ = false; + can_replan_ = false; + replan_delay_ = 2.0; + goal_joint_tolerance_ = 1e-4; + goal_position_tolerance_ = 1e-4; // 0.1 mm + goal_orientation_tolerance_ = 1e-3; // ~0.1 deg + allowed_planning_time_ = 5.0; + num_planning_attempts_ = 1; + max_velocity_scaling_factor_ = 1.0; + max_acceleration_scaling_factor_ = 1.0; + initializing_constraints_ = false; - void clearTrajectoryConstraints() - { - trajectory_constraints_.reset(); - } + if (joint_model_group_->isChain()) + end_effector_link_ = joint_model_group_->getLinkModelNames().back(); + pose_reference_frame_ = getRobotModel()->getModelFrame(); - std::vector getKnownConstraints() const - { - while (initializing_constraints_) - { - static ros::WallDuration d(0.01); - d.sleep(); - } + trajectory_event_publisher_ = node_handle_.advertise(trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false); + attached_object_publisher_ = node_handle_.advertise(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1, false); - std::vector c; - if (constraints_storage_) - constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_); + current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_buffer_, node_handle_); - return c; - } + ros::WallTime timeout_for_servers = ros::WallTime::now() + wait_for_servers; + if (wait_for_servers == ros::WallDuration()) + timeout_for_servers = ros::WallTime(); // wait for ever + double allotted_time = wait_for_servers.toSec(); - moveit_msgs::Constraints getPathConstraints() const - { - if (path_constraints_) - return *path_constraints_; - else - return moveit_msgs::Constraints(); - } + move_action_client_.reset( + new actionlib::SimpleActionClient(node_handle_, move_group::MOVE_ACTION, false)); + waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time); - moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const - { - if (trajectory_constraints_) - return *trajectory_constraints_; - else - return moveit_msgs::TrajectoryConstraints(); - } + pick_action_client_.reset( + new actionlib::SimpleActionClient(node_handle_, move_group::PICKUP_ACTION, false)); + waitForAction(pick_action_client_, move_group::PICKUP_ACTION, timeout_for_servers, allotted_time); - void initializeConstraintsStorage(const std::string& host, unsigned int port) - { - initializing_constraints_ = true; - if (constraints_init_thread_) - constraints_init_thread_->join(); - constraints_init_thread_.reset( - new boost::thread(boost::bind(&MoveItCppImpl::initializeConstraintsStorageThread, this, host, port))); - } + place_action_client_.reset( + new actionlib::SimpleActionClient(node_handle_, move_group::PLACE_ACTION, false)); + waitForAction(place_action_client_, move_group::PLACE_ACTION, timeout_for_servers, allotted_time); - void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) - { - workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame(); - workspace_parameters_.header.stamp = ros::Time::now(); - workspace_parameters_.min_corner.x = minx; - workspace_parameters_.min_corner.y = miny; - workspace_parameters_.min_corner.z = minz; - workspace_parameters_.max_corner.x = maxx; - workspace_parameters_.max_corner.y = maxy; - workspace_parameters_.max_corner.z = maxz; - } + execute_action_client_.reset(new actionlib::SimpleActionClient( + node_handle_, move_group::EXECUTE_ACTION_NAME, false)); + waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time); -private: - void initializeConstraintsStorageThread(const std::string& host, unsigned int port) - { - // Set up db - try - { - warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase(); - conn->setParams(host, port); - if (conn->connect()) - { - constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(conn)); - } - } - catch (std::exception& ex) - { - ROS_ERROR_NAMED("moveit_cpp", "%s", ex.what()); - } - initializing_constraints_ = false; - } - - Options opt_; - ros::NodeHandle node_handle_; - std::shared_ptr tf_buffer_; - robot_model::RobotModelConstPtr robot_model_; - planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; - std::unique_ptr > move_action_client_; - std::unique_ptr > execute_action_client_; - std::unique_ptr > pick_action_client_; - std::unique_ptr > place_action_client_; - - // general planning params - robot_state::RobotStatePtr considered_start_state_; - moveit_msgs::WorkspaceParameters workspace_parameters_; - double allowed_planning_time_; - std::string planner_id_; - unsigned int num_planning_attempts_; - double max_velocity_scaling_factor_; - double max_acceleration_scaling_factor_; - double goal_joint_tolerance_; - double goal_position_tolerance_; - double goal_orientation_tolerance_; - bool can_look_; - bool can_replan_; - double replan_delay_; - - // joint state goal - robot_state::RobotStatePtr joint_state_target_; - const robot_model::JointModelGroup* joint_model_group_; - - // pose goal; - // for each link we have a set of possible goal locations; - std::map > pose_targets_; - - // common properties for goals - ActiveTargetType active_target_; - std::unique_ptr path_constraints_; - std::unique_ptr trajectory_constraints_; - std::string end_effector_link_; - std::string pose_reference_frame_; - std::string support_surface_; - - // ROS communication - ros::Publisher trajectory_event_publisher_; - ros::Publisher attached_object_publisher_; - ros::ServiceClient query_service_; - ros::ServiceClient get_params_service_; - ros::ServiceClient set_params_service_; - ros::ServiceClient cartesian_path_service_; - ros::ServiceClient plan_grasps_service_; - std::unique_ptr constraints_storage_; - std::unique_ptr constraints_init_thread_; - bool initializing_constraints_; -}; -} // namespace planning_interface -} // namespace moveit + query_service_ = + node_handle_.serviceClient(move_group::QUERY_PLANNERS_SERVICE_NAME); + get_params_service_ = + node_handle_.serviceClient(move_group::GET_PLANNER_PARAMS_SERVICE_NAME); + set_params_service_ = + node_handle_.serviceClient(move_group::SET_PLANNER_PARAMS_SERVICE_NAME); -moveit::planning_interface::MoveItCpp::MoveItCpp(const std::string& group_name, - const std::shared_ptr& tf_buffer, - const ros::WallDuration& wait_for_servers) -{ - if (!ros::ok()) - throw std::runtime_error("ROS does not seem to be running"); - impl_ = new MoveItCppImpl(Options(group_name), tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers); -} + cartesian_path_service_ = + node_handle_.serviceClient(move_group::CARTESIAN_PATH_SERVICE_NAME); -moveit::planning_interface::MoveItCpp::MoveItCpp(const std::string& group, - const std::shared_ptr& tf_buffer, - const ros::Duration& wait_for_servers) - : MoveItCpp(group, tf_buffer, ros::WallDuration(wait_for_servers.toSec())) -{ -} + plan_grasps_service_ = node_handle_.serviceClient(GRASP_PLANNING_SERVICE_NAME); -moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, - const std::shared_ptr& tf_buffer, - const ros::WallDuration& wait_for_servers) -{ - impl_ = new MoveItCppImpl(opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers); + ROS_INFO_STREAM_NAMED("move_group_interface", "Ready to take commands for planning group " << opt.group_name_ + << "."); + group_name_ = opt.group_name_; + robot_description_ = opt.robot_description_; } moveit::planning_interface::MoveItCpp::MoveItCpp( @@ -1307,13 +191,11 @@ moveit::planning_interface::MoveItCpp::MoveItCpp( moveit::planning_interface::MoveItCpp::~MoveItCpp() { - delete impl_; } moveit::planning_interface::MoveItCpp::MoveItCpp(MoveItCpp&& other) - : remembered_joint_values_(std::move(other.remembered_joint_values_)), impl_(other.impl_) { - other.impl_ = nullptr; + other.clearContents(); } moveit::planning_interface::MoveItCpp& moveit::planning_interface::MoveItCpp:: @@ -1321,10 +203,56 @@ operator=(MoveItCpp&& other) { if (this != &other) { - delete impl_; - impl_ = other.impl_; + this->group_name_ = other.group_name_; + this->robot_description_ = other.robot_description_; + this->node_handle_ = other.node_handle_; + this->tf_buffer_ = other.tf_buffer_; + this->robot_model_ = other.robot_model_; + this->current_state_monitor_ = other.current_state_monitor_; + swap(this->move_action_client_, other.move_action_client_); + swap(this->execute_action_client_, other.execute_action_client_); + swap(this->pick_action_client_, other.pick_action_client_); + swap(this->place_action_client_, other.place_action_client_); + + this->considered_start_state_ = other.considered_start_state_; + this->workspace_parameters_ = other.workspace_parameters_; + this->allowed_planning_time_ = other.allowed_planning_time_; + this->planner_id_ = other.planner_id_; + this->num_planning_attempts_ = other.num_planning_attempts_; + this->max_velocity_scaling_factor_ = other.max_velocity_scaling_factor_; + this->max_acceleration_scaling_factor_ = other.max_acceleration_scaling_factor_; + this->goal_joint_tolerance_ = other.goal_joint_tolerance_; + this->goal_position_tolerance_ = other.goal_position_tolerance_; + this->goal_orientation_tolerance_ = other.goal_orientation_tolerance_; + this->can_look_ = other.can_look_; + this->can_replan_ = other.can_replan_; + this->replan_delay_ = other.replan_delay_; + + this->joint_state_target_ = other.joint_state_target_; + this->joint_model_group_ = other.joint_model_group_; + + this->pose_targets_ = other.pose_targets_; + + this->active_target_ = other.active_target_; + swap(this->path_constraints_, other.path_constraints_); + swap(this->trajectory_constraints_, other.trajectory_constraints_); + this->end_effector_link_ = other.end_effector_link_; + this->pose_reference_frame_ = other.pose_reference_frame_; + this->support_surface_ = other.support_surface_; + + this->trajectory_event_publisher_ = other.trajectory_event_publisher_; + this->attached_object_publisher_ = other.attached_object_publisher_; + this->query_service_ = other.query_service_; + this->get_params_service_ = other.get_params_service_; + this->set_params_service_ = other.set_params_service_; + this->cartesian_path_service_ = other.cartesian_path_service_; + this->plan_grasps_service_ = other.plan_grasps_service_; + swap(this->constraints_storage_, other.constraints_storage_); + swap(this->constraints_init_thread_, other.constraints_init_thread_); + this->initializing_constraints_ = other.initializing_constraints_; + remembered_joint_values_ = std::move(other.remembered_joint_values_); - other.impl_ = nullptr; + other.clearContents(); } return *this; @@ -1332,7 +260,7 @@ operator=(MoveItCpp&& other) const std::string& moveit::planning_interface::MoveItCpp::getName() const { - return impl_->getOptions().group_name_; + return group_name_; } const std::vector moveit::planning_interface::MoveItCpp::getNamedTargets() @@ -1352,24 +280,42 @@ const std::vector moveit::planning_interface::MoveItCpp::getNamedTa robot_model::RobotModelConstPtr moveit::planning_interface::MoveItCpp::getRobotModel() const { - return impl_->getRobotModel(); + return robot_model_; } const ros::NodeHandle& moveit::planning_interface::MoveItCpp::getNodeHandle() const { - return impl_->getOptions().node_handle_; + return node_handle_; } bool moveit::planning_interface::MoveItCpp::getInterfaceDescription( moveit_msgs::PlannerInterfaceDescription& desc) { - return impl_->getInterfaceDescription(desc); + moveit_msgs::QueryPlannerInterfaces::Request req; + moveit_msgs::QueryPlannerInterfaces::Response res; + if (query_service_.call(req, res)) + if (!res.planner_interfaces.empty()) + { + desc = res.planner_interfaces.front(); + return true; + } + return false; } std::map moveit::planning_interface::MoveItCpp::getPlannerParams( const std::string& planner_id, const std::string& group) { - return impl_->getPlannerParams(planner_id, group); + moveit_msgs::GetPlannerParams::Request req; + moveit_msgs::GetPlannerParams::Response res; + req.planner_config = planner_id; + req.group = group; + std::map result; + if (get_params_service_.call(req, res)) + { + for (unsigned int i = 0, end = res.params.keys.size(); i < end; ++i) + result[res.params.keys[i]] = res.params.values[i]; + } + return result; } void moveit::planning_interface::MoveItCpp::setPlannerParams(const std::string& planner_id, @@ -1377,124 +323,304 @@ void moveit::planning_interface::MoveItCpp::setPlannerParams(const std::string& const std::map& params, bool replace) { - impl_->setPlannerParams(planner_id, group, params, replace); + moveit_msgs::SetPlannerParams::Request req; + moveit_msgs::SetPlannerParams::Response res; + req.planner_config = planner_id; + req.group = group; + req.replace = replace; + for (const std::pair& param : params) + { + req.params.keys.push_back(param.first); + req.params.values.push_back(param.second); + } + set_params_service_.call(req, res); } std::string moveit::planning_interface::MoveItCpp::getDefaultPlannerId(const std::string& group) const { - return impl_->getDefaultPlannerId(group); + std::stringstream param_name; + param_name << "move_group"; + if (!group.empty()) + param_name << "/" << group; + param_name << "/default_planner_config"; + + std::string default_planner_config; + node_handle_.getParam(param_name.str(), default_planner_config); + return default_planner_config; } void moveit::planning_interface::MoveItCpp::setPlannerId(const std::string& planner_id) { - impl_->setPlannerId(planner_id); + planner_id_ = planner_id; } const std::string& moveit::planning_interface::MoveItCpp::getPlannerId() const { - return impl_->getPlannerId(); + return planner_id_; } void moveit::planning_interface::MoveItCpp::setNumPlanningAttempts(unsigned int num_planning_attempts) { - impl_->setNumPlanningAttempts(num_planning_attempts); + num_planning_attempts_ = num_planning_attempts; } void moveit::planning_interface::MoveItCpp::setMaxVelocityScalingFactor(double max_velocity_scaling_factor) { - impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor); + max_velocity_scaling_factor_ = max_velocity_scaling_factor; } void moveit::planning_interface::MoveItCpp::setMaxAccelerationScalingFactor( double max_acceleration_scaling_factor) { - impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor); + max_acceleration_scaling_factor_ = max_acceleration_scaling_factor; } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::asyncMove() { - return impl_->move(false); + return move(false); } actionlib::SimpleActionClient& moveit::planning_interface::MoveItCpp::getMoveGroupClient() const { - return impl_->getMoveGroupClient(); + return *move_action_client_; } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::move() { - return impl_->move(true); + return move(true); } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::asyncExecute(const Plan& plan) { - return impl_->execute(plan, false); + return execute(plan, false); } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::execute(const Plan& plan) { - return impl_->execute(plan, true); + return execute(plan, true); } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::plan(Plan& plan) { - return impl_->plan(plan); + if (!move_action_client_) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + if (!move_action_client_->isServerConnected()) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + + moveit_msgs::MoveGroupGoal goal; + constructGoal(goal); + goal.planning_options.plan_only = true; + goal.planning_options.look_around = false; + goal.planning_options.replan = false; + goal.planning_options.planning_scene_diff.is_diff = true; + goal.planning_options.planning_scene_diff.robot_state.is_diff = true; + + move_action_client_->sendGoal(goal); + if (!move_action_client_->waitForResult()) + { + ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); + } + if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + plan.trajectory_ = move_action_client_->getResult()->planned_trajectory; + plan.start_state_ = move_action_client_->getResult()->trajectory_start; + plan.planning_time_ = move_action_client_->getResult()->planning_time; + return MoveItErrorCode(move_action_client_->getResult()->error_code); + } + else + { + ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": " + << move_action_client_->getState().getText()); + return MoveItErrorCode(move_action_client_->getResult()->error_code); + } } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::pick(const std::string& object, bool plan_only) { - return impl_->pick(object, std::vector(), plan_only); + return pick(object, std::vector(), plan_only); } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::pick( const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only) { - return impl_->pick(object, std::vector(1, grasp), plan_only); + return pick(object, std::vector(1, grasp), plan_only); } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::pick( const std::string& object, const std::vector& grasps, bool plan_only) { - return impl_->pick(object, grasps, plan_only); + if (!pick_action_client_) + { + ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + if (!pick_action_client_->isServerConnected()) + { + ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + moveit_msgs::PickupGoal goal; + constructGoal(goal, object); + goal.possible_grasps = grasps; + goal.planning_options.plan_only = plan_only; + goal.planning_options.look_around = can_look_; + goal.planning_options.replan = can_replan_; + goal.planning_options.replan_delay = replan_delay_; + goal.planning_options.planning_scene_diff.is_diff = true; + goal.planning_options.planning_scene_diff.robot_state.is_diff = true; + + pick_action_client_->sendGoal(goal); + if (!pick_action_client_->waitForResult()) + { + ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early"); + } + if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + return MoveItErrorCode(pick_action_client_->getResult()->error_code); + } + else + { + ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": " + << pick_action_client_->getState().getText()); + return MoveItErrorCode(pick_action_client_->getResult()->error_code); + } } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::planGraspsAndPick(const std::string& object, bool plan_only) { - return impl_->planGraspsAndPick(object, plan_only); + if (object.empty()) + { + return planGraspsAndPick(moveit_msgs::CollisionObject()); + } + moveit::planning_interface::PlanningSceneInterface psi; + + std::map objects = psi.getObjects(std::vector(1, object)); + + if (objects.empty()) + { + ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '" + << object << "', but the object could not be found"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME); + } + + return planGraspsAndPick(objects[object], plan_only); } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::planGraspsAndPick( const moveit_msgs::CollisionObject& object, bool plan_only) { - return impl_->planGraspsAndPick(object, plan_only); + if (!plan_grasps_service_) + { + ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '" + << GRASP_PLANNING_SERVICE_NAME + << "' is not available." + " This has to be implemented and started seperatly."); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + + moveit_msgs::GraspPlanning::Request request; + moveit_msgs::GraspPlanning::Response response; + + request.group_name = group_name_; + request.target = object; + request.support_surfaces.push_back(support_surface_); + + ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner..."); + if (!plan_grasps_service_.call(request, response) || + response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS) + { + ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick."); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + + return pick(object.id, response.grasps, plan_only); } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::place(const std::string& object, bool plan_only) { - return impl_->place(object, std::vector(), plan_only); + return place(object, std::vector(), plan_only); } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::place( const std::string& object, const std::vector& locations, bool plan_only) { - return impl_->place(object, locations, plan_only); + if (!place_action_client_) + { + ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + if (!place_action_client_->isServerConnected()) + { + ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected"); + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + moveit_msgs::PlaceGoal goal; + constructGoal(goal, object); + goal.place_locations = locations; + goal.planning_options.plan_only = plan_only; + goal.planning_options.look_around = can_look_; + goal.planning_options.replan = can_replan_; + goal.planning_options.replan_delay = replan_delay_; + goal.planning_options.planning_scene_diff.is_diff = true; + goal.planning_options.planning_scene_diff.robot_state.is_diff = true; + + place_action_client_->sendGoal(goal); + ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size()); + if (!place_action_client_->waitForResult()) + { + ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early"); + } + if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + return MoveItErrorCode(place_action_client_->getResult()->error_code); + } + else + { + ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": " + << place_action_client_->getState().getText()); + return MoveItErrorCode(place_action_client_->getResult()->error_code); + } } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::place( const std::string& object, const std::vector& poses, bool plan_only) { - return impl_->place(object, poses, plan_only); + std::vector locations; + for (const geometry_msgs::PoseStamped& pose : poses) + { + moveit_msgs::PlaceLocation location; + location.pre_place_approach.direction.vector.z = -1.0; + location.post_place_retreat.direction.vector.x = -1.0; + location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame(); + location.post_place_retreat.direction.header.frame_id = end_effector_link_; + + location.pre_place_approach.min_distance = 0.1; + location.pre_place_approach.desired_distance = 0.2; + location.post_place_retreat.min_distance = 0.0; + location.post_place_retreat.desired_distance = 0.2; + // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody + + location.place_pose = pose; + locations.push_back(location); + } + ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations", + (unsigned int)locations.size()); + return place(object, locations, plan_only); } moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::place( const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only) { - return impl_->place(object, std::vector(1, pose), plan_only); + return place(object, std::vector(1, pose), plan_only); } double moveit::planning_interface::MoveItCpp::computeCartesianPath( @@ -1511,56 +637,87 @@ double moveit::planning_interface::MoveItCpp::computeCartesianPath( moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code) { - if (error_code) + moveit_msgs::GetCartesianPath::Request req; + moveit_msgs::GetCartesianPath::Response res; + + if (considered_start_state_) + robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state); + else + req.start_state.is_diff = true; + + req.group_name = group_name_; + req.header.frame_id = pose_reference_frame_; + req.header.stamp = ros::Time::now(); + req.waypoints = waypoints; + req.max_step = eef_step; + req.jump_threshold = jump_threshold; + req.path_constraints = path_constraints; + req.avoid_collisions = avoid_collisions; + req.link_name = end_effector_link_; + + if (cartesian_path_service_.call(req, res)) { - return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, - avoid_collisions, *error_code); + if (error_code) + *error_code = res.error_code; + if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) + { + trajectory = res.solution; + return res.fraction; + } + else + { + return -1.0; + } } else { - moveit_msgs::MoveItErrorCodes error_code_tmp; - return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, - avoid_collisions, error_code_tmp); + error_code->val = error_code->FAILURE; + return -1.0; } } void moveit::planning_interface::MoveItCpp::stop() { - impl_->stop(); + if (trajectory_event_publisher_) + { + std_msgs::String event; + event.data = "stop"; + trajectory_event_publisher_.publish(event); + } } void moveit::planning_interface::MoveItCpp::setStartState(const moveit_msgs::RobotState& start_state) { robot_state::RobotStatePtr rs; - impl_->getCurrentState(rs); + getCurrentState(rs, 1.0); robot_state::robotStateMsgToRobotState(start_state, *rs); setStartState(*rs); } void moveit::planning_interface::MoveItCpp::setStartState(const robot_state::RobotState& start_state) { - impl_->setStartState(start_state); + considered_start_state_.reset(new robot_state::RobotState(start_state)); } void moveit::planning_interface::MoveItCpp::setStartStateToCurrentState() { - impl_->setStartStateToCurrentState(); + considered_start_state_.reset(); } void moveit::planning_interface::MoveItCpp::setRandomTarget() { - impl_->getTargetRobotState().setToRandomPositions(); - impl_->setTargetType(JOINT); + joint_state_target_->setToRandomPositions(); + active_target_=JOINT; } const std::vector& moveit::planning_interface::MoveItCpp::getJointNames() { - return impl_->getJointModelGroup()->getVariableNames(); + return joint_model_group_->getVariableNames(); } const std::vector& moveit::planning_interface::MoveItCpp::getLinkNames() { - return impl_->getJointModelGroup()->getLinkModelNames(); + return joint_model_group_->getLinkModelNames(); } std::map @@ -1571,7 +728,7 @@ moveit::planning_interface::MoveItCpp::getNamedTargetValues(const std::string& n if (it != remembered_joint_values_.end()) { - std::vector names = impl_->getJointModelGroup()->getVariableNames(); + std::vector names = joint_model_group_->getVariableNames(); for (size_t x = 0; x < names.size(); ++x) { positions[names[x]] = it->second[x]; @@ -1579,7 +736,7 @@ moveit::planning_interface::MoveItCpp::getNamedTargetValues(const std::string& n } else { - impl_->getJointModelGroup()->getVariableDefaultPositions(name, positions); + joint_model_group_->getVariableDefaultPositions(name, positions); } return positions; } @@ -1593,12 +750,12 @@ bool moveit::planning_interface::MoveItCpp::setNamedTarget(const std::string& na } else { - if (impl_->getTargetRobotState().setToDefaultValues(impl_->getJointModelGroup(), name)) + if (joint_state_target_->setToDefaultValues(joint_model_group_, name)) { - impl_->setTargetType(JOINT); + active_target_ = JOINT; return true; } - ROS_ERROR_NAMED("moveit_cpp", "The requested named target '%s' does not exist", name.c_str()); + ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str()); return false; } } @@ -1606,61 +763,61 @@ bool moveit::planning_interface::MoveItCpp::setNamedTarget(const std::string& na void moveit::planning_interface::MoveItCpp::getJointValueTarget( std::vector& group_variable_values) const { - impl_->getTargetRobotState().copyJointGroupPositions(impl_->getJointModelGroup(), group_variable_values); + joint_state_target_->copyJointGroupPositions(joint_model_group_, group_variable_values); } bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::vector& joint_values) { - if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount()) + if (joint_values.size() != joint_model_group_->getVariableCount()) return false; - impl_->setTargetType(JOINT); - impl_->getTargetRobotState().setJointGroupPositions(impl_->getJointModelGroup(), joint_values); - return impl_->getTargetRobotState().satisfiesBounds(impl_->getJointModelGroup(), impl_->getGoalJointTolerance()); + active_target_ = JOINT; + joint_state_target_->setJointGroupPositions(joint_model_group_, joint_values); + return joint_state_target_->satisfiesBounds(joint_model_group_, goal_joint_tolerance_); } bool moveit::planning_interface::MoveItCpp::setJointValueTarget( const std::map& variable_values) { - const auto& allowed = impl_->getJointModelGroup()->getVariableNames(); + const auto& allowed = joint_model_group_->getVariableNames(); for (const auto& pair : variable_values) { if (std::find(allowed.begin(), allowed.end(), pair.first) == allowed.end()) { ROS_ERROR_STREAM("joint variable " << pair.first << " is not part of group " - << impl_->getJointModelGroup()->getName()); + << joint_model_group_->getName()); return false; } } - impl_->setTargetType(JOINT); - impl_->getTargetRobotState().setVariablePositions(variable_values); - return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance()); + active_target_ = JOINT; + joint_state_target_->setVariablePositions(variable_values); + return joint_state_target_->satisfiesBounds(goal_joint_tolerance_); } bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::vector& variable_names, const std::vector& variable_values) { - const auto& allowed = impl_->getJointModelGroup()->getVariableNames(); + const auto& allowed = joint_model_group_->getVariableNames(); for (const auto& variable_name : variable_names) { if (std::find(allowed.begin(), allowed.end(), variable_name) == allowed.end()) { ROS_ERROR_STREAM("joint variable " << variable_name << " is not part of group " - << impl_->getJointModelGroup()->getName()); + << joint_model_group_->getName()); return false; } } - impl_->setTargetType(JOINT); - impl_->getTargetRobotState().setVariablePositions(variable_names, variable_values); - return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance()); + active_target_ = JOINT; + joint_state_target_->setVariablePositions(variable_names, variable_values); + return joint_state_target_->satisfiesBounds(goal_joint_tolerance_); } bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const robot_state::RobotState& rstate) { - impl_->setTargetType(JOINT); - impl_->getTargetRobotState() = rstate; - return impl_->getTargetRobotState().satisfiesBounds(impl_->getGoalJointTolerance()); + active_target_ = JOINT; + *joint_state_target_ = rstate; + return joint_state_target_->satisfiesBounds(goal_joint_tolerance_); } bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::string& joint_name, double value) @@ -1672,15 +829,15 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::strin bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::string& joint_name, const std::vector& values) { - impl_->setTargetType(JOINT); - const robot_model::JointModel* jm = impl_->getJointModelGroup()->getJointModel(joint_name); + active_target_ = JOINT; + const robot_model::JointModel* jm = joint_model_group_->getJointModel(joint_name); if (jm && jm->getVariableCount() == values.size()) { - impl_->getTargetRobotState().setJointPositions(jm, values); - return impl_->getTargetRobotState().satisfiesBounds(jm, impl_->getGoalJointTolerance()); + joint_state_target_->setJointPositions(jm, values); + return joint_state_target_->satisfiesBounds(jm, goal_joint_tolerance_); } - ROS_ERROR_STREAM("joint " << joint_name << " is not part of group " << impl_->getJointModelGroup()->getName()); + ROS_ERROR_STREAM("joint " << joint_name << " is not part of group " << joint_model_group_->getName()); return false; } @@ -1692,13 +849,13 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const sensor_msg bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link) { - return impl_->setJointValueTarget(eef_pose, end_effector_link, "", false); + return setJointValueTarget(eef_pose, end_effector_link, "", false); } bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link) { - return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false); + return setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false); } bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const Eigen::Isometry3d& eef_pose, @@ -1711,13 +868,13 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const Eigen::Iso bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget( const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link) { - return impl_->setJointValueTarget(eef_pose, end_effector_link, "", true); + return setJointValueTarget(eef_pose, end_effector_link, "", true); } bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget( const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link) { - return impl_->setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true); + return setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true); } bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget( @@ -1729,36 +886,45 @@ bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget( const robot_state::RobotState& moveit::planning_interface::MoveItCpp::getJointValueTarget() const { - return impl_->getTargetRobotState(); + return *joint_state_target_; } const robot_state::RobotState& moveit::planning_interface::MoveItCpp::getTargetRobotState() const { - return impl_->getTargetRobotState(); + return *joint_state_target_; } const std::string& moveit::planning_interface::MoveItCpp::getEndEffectorLink() const { - return impl_->getEndEffectorLink(); + return end_effector_link_; } const std::string& moveit::planning_interface::MoveItCpp::getEndEffector() const { - return impl_->getEndEffector(); + if (!end_effector_link_.empty()) + { + const std::vector& possible_eefs = + getRobotModel()->getJointModelGroup(group_name_)->getAttachedEndEffectorNames(); + for (const std::string& possible_eef : possible_eefs) + if (getRobotModel()->getEndEffector(possible_eef)->hasLinkModel(end_effector_link_)) + return possible_eef; + } + static std::string empty; + return empty; } bool moveit::planning_interface::MoveItCpp::setEndEffectorLink(const std::string& link_name) { - if (impl_->getEndEffectorLink().empty() || link_name.empty()) + if (end_effector_link_.empty() || link_name.empty()) return false; - impl_->setEndEffectorLink(link_name); - impl_->setTargetType(POSE); + end_effector_link_ = link_name; + active_target_ = POSE; return true; } bool moveit::planning_interface::MoveItCpp::setEndEffector(const std::string& eef_name) { - const robot_model::JointModelGroup* jmg = impl_->getRobotModel()->getEndEffector(eef_name); + const robot_model::JointModelGroup* jmg = robot_model_->getEndEffector(eef_name); if (jmg) return setEndEffectorLink(jmg->getEndEffectorParentGroup().second); return false; @@ -1766,12 +932,12 @@ bool moveit::planning_interface::MoveItCpp::setEndEffector(const std::string& ee void moveit::planning_interface::MoveItCpp::clearPoseTarget(const std::string& end_effector_link) { - impl_->clearPoseTarget(end_effector_link); + pose_targets_.erase(end_effector_link); } void moveit::planning_interface::MoveItCpp::clearPoseTargets() { - impl_->clearPoseTargets(); + pose_targets_.clear(); } bool moveit::planning_interface::MoveItCpp::setPoseTarget(const Eigen::Isometry3d& pose, @@ -1831,31 +997,63 @@ bool moveit::planning_interface::MoveItCpp::setPoseTargets(const std::vector& target, const std::string& end_effector_link) +bool moveit::planning_interface::MoveItCpp::setPoseTargets(const std::vector& target, const std::string& end_effector_link) { if (target.empty()) { - ROS_ERROR_NAMED("moveit_cpp", "No pose specified as goal target"); + ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target"); return false; } else { - impl_->setTargetType(POSE); - return impl_->setPoseTargets(target, end_effector_link); + active_target_ = POSE; + const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; + if (eef.empty()) + { + ROS_ERROR_NAMED("move_group_interface", "No end-effector to set the pose for"); + return false; + } + else + { + pose_targets_[eef] = target; + // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors + std::vector& stored_poses = pose_targets_[eef]; + for (geometry_msgs::PoseStamped& stored_pose : stored_poses) + stored_pose.header.stamp = ros::Time(0); + } + return true; } } -const geometry_msgs::PoseStamped& -moveit::planning_interface::MoveItCpp::getPoseTarget(const std::string& end_effector_link) const +const geometry_msgs::PoseStamped& moveit::planning_interface::MoveItCpp::getPoseTarget(const std::string& end_effector_link) const { - return impl_->getPoseTarget(end_effector_link); + const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; + + // if multiple pose targets are set, return the first one + std::map >::const_iterator jt = pose_targets_.find(eef); + if (jt != pose_targets_.end()) + if (!jt->second.empty()) + return jt->second.at(0); + + // or return an error + static const geometry_msgs::PoseStamped UNKNOWN; + ROS_ERROR_NAMED("move_group_interface", "Pose for end-effector '%s' not known.", eef.c_str()); + return UNKNOWN; } -const std::vector& -moveit::planning_interface::MoveItCpp::getPoseTargets(const std::string& end_effector_link) const +const std::vector& moveit::planning_interface::MoveItCpp::getPoseTargets(const std::string& end_effector_link) const { - return impl_->getPoseTargets(end_effector_link); + const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; + + std::map >::const_iterator jt = pose_targets_.find(eef); + if (jt != pose_targets_.end()) + if (!jt->second.empty()) + return jt->second; + + // or return an error + static const std::vector EMPTY; + ROS_ERROR_NAMED("move_group_interface", "Poses for end-effector '%s' are not known.", eef.c_str()); + return EMPTY; } namespace @@ -1877,10 +1075,10 @@ bool moveit::planning_interface::MoveItCpp::setPositionTarget(double x, double y const std::string& end_effector_link) { geometry_msgs::PoseStamped target; - if (impl_->hasPoseTarget(end_effector_link)) + if (hasPoseTarget(end_effector_link)) { target = getPoseTarget(end_effector_link); - transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target); + transformPose(*tf_buffer_, pose_reference_frame_, target); } else { @@ -1888,14 +1086,14 @@ bool moveit::planning_interface::MoveItCpp::setPositionTarget(double x, double y target.pose.orientation.y = 0.0; target.pose.orientation.z = 0.0; target.pose.orientation.w = 1.0; - target.header.frame_id = impl_->getPoseReferenceFrame(); + target.header.frame_id = pose_reference_frame_; } target.pose.position.x = x; target.pose.position.y = y; target.pose.position.z = z; bool result = setPoseTarget(target, end_effector_link); - impl_->setTargetType(POSITION); + active_target_ = POSITION; return result; } @@ -1903,23 +1101,23 @@ bool moveit::planning_interface::MoveItCpp::setRPYTarget(double r, double p, dou const std::string& end_effector_link) { geometry_msgs::PoseStamped target; - if (impl_->hasPoseTarget(end_effector_link)) + if (hasPoseTarget(end_effector_link)) { target = getPoseTarget(end_effector_link); - transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target); + transformPose(*tf_buffer_, pose_reference_frame_, target); } else { target.pose.position.x = 0.0; target.pose.position.y = 0.0; target.pose.position.z = 0.0; - target.header.frame_id = impl_->getPoseReferenceFrame(); + target.header.frame_id = pose_reference_frame_; } tf2::Quaternion q; q.setRPY(r, p, y); target.pose.orientation = tf2::toMsg(q); bool result = setPoseTarget(target, end_effector_link); - impl_->setTargetType(ORIENTATION); + active_target_ = ORIENTATION; return result; } @@ -1927,17 +1125,17 @@ bool moveit::planning_interface::MoveItCpp::setOrientationTarget(double x, doubl const std::string& end_effector_link) { geometry_msgs::PoseStamped target; - if (impl_->hasPoseTarget(end_effector_link)) + if (hasPoseTarget(end_effector_link)) { target = getPoseTarget(end_effector_link); - transformPose(*impl_->getTF(), impl_->getPoseReferenceFrame(), target); + transformPose(*tf_buffer_, pose_reference_frame_, target); } else { target.pose.position.x = 0.0; target.pose.position.y = 0.0; target.pose.position.z = 0.0; - target.header.frame_id = impl_->getPoseReferenceFrame(); + target.header.frame_id = pose_reference_frame_; } target.pose.orientation.x = x; @@ -1945,33 +1143,33 @@ bool moveit::planning_interface::MoveItCpp::setOrientationTarget(double x, doubl target.pose.orientation.z = z; target.pose.orientation.w = w; bool result = setPoseTarget(target, end_effector_link); - impl_->setTargetType(ORIENTATION); + active_target_ = ORIENTATION; return result; } void moveit::planning_interface::MoveItCpp::setPoseReferenceFrame(const std::string& pose_reference_frame) { - impl_->setPoseReferenceFrame(pose_reference_frame); + pose_reference_frame_ = pose_reference_frame; } const std::string& moveit::planning_interface::MoveItCpp::getPoseReferenceFrame() const { - return impl_->getPoseReferenceFrame(); + return pose_reference_frame_; } double moveit::planning_interface::MoveItCpp::getGoalJointTolerance() const { - return impl_->getGoalJointTolerance(); + return goal_joint_tolerance_; } double moveit::planning_interface::MoveItCpp::getGoalPositionTolerance() const { - return impl_->getGoalPositionTolerance(); + return goal_position_tolerance_; } double moveit::planning_interface::MoveItCpp::getGoalOrientationTolerance() const { - return impl_->getGoalOrientationTolerance(); + return goal_orientation_tolerance_; } void moveit::planning_interface::MoveItCpp::setGoalTolerance(double tolerance) @@ -1983,17 +1181,17 @@ void moveit::planning_interface::MoveItCpp::setGoalTolerance(double tolerance) void moveit::planning_interface::MoveItCpp::setGoalJointTolerance(double tolerance) { - impl_->setGoalJointTolerance(tolerance); + goal_joint_tolerance_ = tolerance; } void moveit::planning_interface::MoveItCpp::setGoalPositionTolerance(double tolerance) { - impl_->setGoalPositionTolerance(tolerance); + goal_position_tolerance_ = tolerance; } void moveit::planning_interface::MoveItCpp::setGoalOrientationTolerance(double tolerance) { - impl_->setGoalOrientationTolerance(tolerance); + goal_orientation_tolerance_ = tolerance; } void moveit::planning_interface::MoveItCpp::rememberJointValues(const std::string& name) @@ -2003,14 +1201,25 @@ void moveit::planning_interface::MoveItCpp::rememberJointValues(const std::strin bool moveit::planning_interface::MoveItCpp::startStateMonitor(double wait) { - return impl_->startStateMonitor(wait); + if (!current_state_monitor_) + { + ROS_ERROR_NAMED("move_group_interface", "Unable to monitor current robot state"); + return false; + } + + // if needed, start the monitor and wait up to 1 second for a full robot state + if (!current_state_monitor_->isActive()) + current_state_monitor_->startStateMonitor(); + + current_state_monitor_->waitForCompleteState(group_name_, wait); + return true; } std::vector moveit::planning_interface::MoveItCpp::getCurrentJointValues() { robot_state::RobotStatePtr current_state; std::vector values; - if (impl_->getCurrentState(current_state)) + if (getCurrentState(current_state, 1.0)) current_state->copyJointGroupPositions(getName(), values); return values; } @@ -2018,7 +1227,7 @@ std::vector moveit::planning_interface::MoveItCpp::getCurrentJointValues std::vector moveit::planning_interface::MoveItCpp::getRandomJointValues() { std::vector r; - impl_->getJointModelGroup()->getVariableRandomPositions(impl_->getTargetRobotState().getRandomNumberGenerator(), r); + joint_model_group_->getVariableRandomPositions(joint_state_target_->getRandomNumberGenerator(), r); return r; } @@ -2029,13 +1238,13 @@ moveit::planning_interface::MoveItCpp::getRandomPose(const std::string& end_effe Eigen::Isometry3d pose; pose.setIdentity(); if (eef.empty()) - ROS_ERROR_NAMED("moveit_cpp", "No end-effector specified"); + ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); else { robot_state::RobotStatePtr current_state; - if (impl_->getCurrentState(current_state)) + if (getCurrentState(current_state, 1.0)) { - current_state->setToRandomPositions(impl_->getJointModelGroup()); + current_state->setToRandomPositions(joint_model_group_); const robot_model::LinkModel* lm = current_state->getLinkModel(eef); if (lm) pose = current_state->getGlobalLinkTransform(lm); @@ -2043,7 +1252,7 @@ moveit::planning_interface::MoveItCpp::getRandomPose(const std::string& end_effe } geometry_msgs::PoseStamped pose_msg; pose_msg.header.stamp = ros::Time::now(); - pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame(); + pose_msg.header.frame_id = robot_model_->getModelFrame(); pose_msg.pose = tf2::toMsg(pose); return pose_msg; } @@ -2055,11 +1264,11 @@ moveit::planning_interface::MoveItCpp::getCurrentPose(const std::string& end_eff Eigen::Isometry3d pose; pose.setIdentity(); if (eef.empty()) - ROS_ERROR_NAMED("moveit_cpp", "No end-effector specified"); + ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); else { robot_state::RobotStatePtr current_state; - if (impl_->getCurrentState(current_state)) + if (getCurrentState(current_state, 1.0)) { const robot_model::LinkModel* lm = current_state->getLinkModel(eef); if (lm) @@ -2068,7 +1277,7 @@ moveit::planning_interface::MoveItCpp::getCurrentPose(const std::string& end_eff } geometry_msgs::PoseStamped pose_msg; pose_msg.header.stamp = ros::Time::now(); - pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame(); + pose_msg.header.frame_id = robot_model_->getModelFrame(); pose_msg.pose = tf2::toMsg(pose); return pose_msg; } @@ -2078,11 +1287,11 @@ std::vector moveit::planning_interface::MoveItCpp::getCurrentRPY(const s std::vector result; const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; if (eef.empty()) - ROS_ERROR_NAMED("moveit_cpp", "No end-effector specified"); + ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); else { robot_state::RobotStatePtr current_state; - if (impl_->getCurrentState(current_state)) + if (getCurrentState(current_state, 1.0)) { const robot_model::LinkModel* lm = current_state->getLinkModel(eef); if (lm) @@ -2102,23 +1311,23 @@ std::vector moveit::planning_interface::MoveItCpp::getCurrentRPY(const s const std::vector& moveit::planning_interface::MoveItCpp::getActiveJoints() const { - return impl_->getJointModelGroup()->getActiveJointModelNames(); + return joint_model_group_->getActiveJointModelNames(); } const std::vector& moveit::planning_interface::MoveItCpp::getJoints() const { - return impl_->getJointModelGroup()->getJointModelNames(); + return joint_model_group_->getJointModelNames(); } unsigned int moveit::planning_interface::MoveItCpp::getVariableCount() const { - return impl_->getJointModelGroup()->getVariableCount(); + return joint_model_group_->getVariableCount(); } robot_state::RobotStatePtr moveit::planning_interface::MoveItCpp::getCurrentState(double wait) { robot_state::RobotStatePtr current_state; - impl_->getCurrentState(current_state, wait); + getCurrentState(current_state, wait); return current_state; } @@ -2135,91 +1344,132 @@ void moveit::planning_interface::MoveItCpp::forgetJointValues(const std::string& void moveit::planning_interface::MoveItCpp::allowLooking(bool flag) { - impl_->allowLooking(flag); + can_look_ = flag; + ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no"); } void moveit::planning_interface::MoveItCpp::allowReplanning(bool flag) { - impl_->allowReplanning(flag); + can_replan_ = flag; + ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no"); } std::vector moveit::planning_interface::MoveItCpp::getKnownConstraints() const { - return impl_->getKnownConstraints(); + while (initializing_constraints_) + { + static ros::WallDuration d(0.01); + d.sleep(); + } + + std::vector c; + if (constraints_storage_) + constraints_storage_->getKnownConstraints(c, robot_model_->getName(), group_name_); + + return c; } moveit_msgs::Constraints moveit::planning_interface::MoveItCpp::getPathConstraints() const { - return impl_->getPathConstraints(); + if (path_constraints_) + return *path_constraints_; + else + return moveit_msgs::Constraints(); } bool moveit::planning_interface::MoveItCpp::setPathConstraints(const std::string& constraint) { - return impl_->setPathConstraints(constraint); + if (constraints_storage_) + { + moveit_warehouse::ConstraintsWithMetadata msg_m; + if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), group_name_)) + { + path_constraints_.reset(new moveit_msgs::Constraints(static_cast(*msg_m))); + return true; + } + else + return false; + } + else + return false; } void moveit::planning_interface::MoveItCpp::setPathConstraints(const moveit_msgs::Constraints& constraint) { - impl_->setPathConstraints(constraint); + path_constraints_.reset(new moveit_msgs::Constraints(constraint)); } void moveit::planning_interface::MoveItCpp::clearPathConstraints() { - impl_->clearPathConstraints(); + path_constraints_.reset(); } moveit_msgs::TrajectoryConstraints moveit::planning_interface::MoveItCpp::getTrajectoryConstraints() const { - return impl_->getTrajectoryConstraints(); + if (trajectory_constraints_) + return *trajectory_constraints_; + else + return moveit_msgs::TrajectoryConstraints(); } void moveit::planning_interface::MoveItCpp::setTrajectoryConstraints( const moveit_msgs::TrajectoryConstraints& constraint) { - impl_->setTrajectoryConstraints(constraint); + trajectory_constraints_.reset(new moveit_msgs::TrajectoryConstraints(constraint)); } void moveit::planning_interface::MoveItCpp::clearTrajectoryConstraints() { - impl_->clearTrajectoryConstraints(); + trajectory_constraints_.reset(); } void moveit::planning_interface::MoveItCpp::setConstraintsDatabase(const std::string& host, unsigned int port) { - impl_->initializeConstraintsStorage(host, port); + initializing_constraints_ = true; + if (constraints_init_thread_) + constraints_init_thread_->join(); + constraints_init_thread_.reset(new boost::thread(boost::bind(&MoveItCpp::initializeConstraintsStorageThread, this, host, port))); } void moveit::planning_interface::MoveItCpp::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) { - impl_->setWorkspace(minx, miny, minz, maxx, maxy, maxz); + workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame(); + workspace_parameters_.header.stamp = ros::Time::now(); + workspace_parameters_.min_corner.x = minx; + workspace_parameters_.min_corner.y = miny; + workspace_parameters_.min_corner.z = minz; + workspace_parameters_.max_corner.x = maxx; + workspace_parameters_.max_corner.y = maxy; + workspace_parameters_.max_corner.z = maxz; } /** \brief Set time allowed to planner to solve problem before aborting */ void moveit::planning_interface::MoveItCpp::setPlanningTime(double seconds) { - impl_->setPlanningTime(seconds); + if (seconds > 0.0) + allowed_planning_time_ = seconds; } /** \brief Get time allowed to planner to solve problem before aborting */ double moveit::planning_interface::MoveItCpp::getPlanningTime() const { - return impl_->getPlanningTime(); + return allowed_planning_time_; } void moveit::planning_interface::MoveItCpp::setSupportSurfaceName(const std::string& name) { - impl_->setSupportSurfaceName(name); + support_surface_ = name; } const std::string& moveit::planning_interface::MoveItCpp::getPlanningFrame() const { - return impl_->getRobotModel()->getModelFrame(); + return robot_model_->getModelFrame(); } const std::vector& moveit::planning_interface::MoveItCpp::getJointModelGroupNames() const { - return impl_->getRobotModel()->getJointModelGroupNames(); + return robot_model_->getJointModelGroupNames(); } bool moveit::planning_interface::MoveItCpp::attachObject(const std::string& object, const std::string& link) @@ -2230,16 +1480,446 @@ bool moveit::planning_interface::MoveItCpp::attachObject(const std::string& obje bool moveit::planning_interface::MoveItCpp::attachObject(const std::string& object, const std::string& link, const std::vector& touch_links) { - return impl_->attachObject(object, link, touch_links); + std::string l = link.empty() ? getEndEffectorLink() : link; + if (l.empty()) + { + const std::vector& links = joint_model_group_->getLinkModelNames(); + if (!links.empty()) + l = links[0]; + } + if (l.empty()) + { + ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str()); + return false; + } + moveit_msgs::AttachedCollisionObject aco; + aco.object.id = object; + aco.link_name.swap(l); + if (touch_links.empty()) + aco.touch_links.push_back(aco.link_name); + else + aco.touch_links = touch_links; + aco.object.operation = moveit_msgs::CollisionObject::ADD; + attached_object_publisher_.publish(aco); + return true; } bool moveit::planning_interface::MoveItCpp::detachObject(const std::string& name) { - return impl_->detachObject(name); + moveit_msgs::AttachedCollisionObject aco; + // if name is a link + if (!name.empty() && joint_model_group_->hasLinkModel(name)) + aco.link_name = name; + else + aco.object.id = name; + aco.object.operation = moveit_msgs::CollisionObject::REMOVE; + if (aco.link_name.empty() && aco.object.id.empty()) + { + // we only want to detach objects for this group + const std::vector& lnames = joint_model_group_->getLinkModelNames(); + for (const std::string& lname : lnames) + { + aco.link_name = lname; + attached_object_publisher_.publish(aco); + } + } + else + attached_object_publisher_.publish(aco); + return true; } void moveit::planning_interface::MoveItCpp::constructMotionPlanRequest( - moveit_msgs::MotionPlanRequest& goal_out) + moveit_msgs::MotionPlanRequest& request) +{ + request.group_name = group_name_; + request.num_planning_attempts = num_planning_attempts_; + request.max_velocity_scaling_factor = max_velocity_scaling_factor_; + request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_; + request.allowed_planning_time = allowed_planning_time_; + request.planner_id = planner_id_; + request.workspace_parameters = workspace_parameters_; + + if (considered_start_state_) + robot_state::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); + else + request.start_state.is_diff = true; + + if (active_target_ == JOINT) + { + request.goal_constraints.resize(1); + request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( + getTargetRobotState(), joint_model_group_, goal_joint_tolerance_); + } + else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION) + { + // find out how many goals are specified + std::size_t goal_count = 0; + for (std::map >::const_iterator it = pose_targets_.begin(); it != pose_targets_.end(); ++it) + goal_count = std::max(goal_count, it->second.size()); + + // start filling the goals; + // each end effector has a number of possible poses (K) as valid goals + // but there could be multiple end effectors specified, so we want each end effector + // to reach the goal that corresponds to the goals of the other end effectors + request.goal_constraints.resize(goal_count); + + for (std::map >::const_iterator it = pose_targets_.begin(); it != pose_targets_.end(); ++it) + { + for (std::size_t i = 0; i < it->second.size(); ++i) + { + moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints( + it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_); + if (active_target_ == ORIENTATION) + c.position_constraints.clear(); + if (active_target_ == POSITION) + c.orientation_constraints.clear(); + request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c); + } + } + } + else + ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation"); + + if (path_constraints_) + request.path_constraints = *path_constraints_; + if (trajectory_constraints_) + request.trajectory_constraints = *trajectory_constraints_; +} + +template +void moveit::planning_interface::MoveItCpp::waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) +{ + ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str()); + + // wait for the server (and spin as needed) + if (timeout == ros::WallTime()) // wait forever + { + while (node_handle_.ok() && !action->isServerConnected()) + { + ros::WallDuration(0.001).sleep(); + // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client + ros::CallbackQueue* queue = dynamic_cast(node_handle_.getCallbackQueue()); + if (queue) + { + queue->callAvailable(); + } + else // in case of nodelets and specific callback queue implementations + { + ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " + "handling."); + } + } + } + else // wait with timeout + { + while (node_handle_.ok() && !action->isServerConnected() && timeout > ros::WallTime::now()) + { + ros::WallDuration(0.001).sleep(); + // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client + ros::CallbackQueue* queue = dynamic_cast(node_handle_.getCallbackQueue()); + if (queue) + { + queue->callAvailable(); + } + else // in case of nodelets and specific callback queue implementations + { + ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " + "handling."); + } + } + } + + if (!action->isServerConnected()) + { + std::stringstream error; + error << "Unable to connect to move_group action server '" << name << "' within allotted time (" << allotted_time + << "s)"; + throw std::runtime_error(error.str()); + } + else + { + ROS_DEBUG_NAMED("move_group_interface", "Connected to '%s'", name.c_str()); + } +} + +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::move(bool wait) +{ + if (!move_action_client_) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + if (!move_action_client_->isServerConnected()) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + + moveit_msgs::MoveGroupGoal goal; + constructGoal(goal); + goal.planning_options.plan_only = false; + goal.planning_options.look_around = can_look_; + goal.planning_options.replan = can_replan_; + goal.planning_options.replan_delay = replan_delay_; + goal.planning_options.planning_scene_diff.is_diff = true; + goal.planning_options.planning_scene_diff.robot_state.is_diff = true; + + move_action_client_->sendGoal(goal); + if (!wait) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); + } + + if (!move_action_client_->waitForResult()) + { + ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); + } + if (!move_action_client_->waitForResult()) + { + ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); + } + + if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + return MoveItErrorCode(move_action_client_->getResult()->error_code); + } + else + { + ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString() + << ": " << move_action_client_->getState().getText()); + return MoveItErrorCode(move_action_client_->getResult()->error_code); + } + +} + +void moveit::planning_interface::MoveItCpp::constructGoal(moveit_msgs::MoveGroupGoal& goal) +{ + constructMotionPlanRequest(goal.request); +} + + +void moveit::planning_interface::MoveItCpp::constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object) +{ + moveit_msgs::PickupGoal goal; + goal.target_name = object; + goal.group_name = group_name_; + goal.end_effector = getEndEffector(); + goal.allowed_planning_time = allowed_planning_time_; + goal.support_surface_name = support_surface_; + goal.planner_id = planner_id_; + if (!support_surface_.empty()) + goal.allow_gripper_support_collision = true; + + if (path_constraints_) + goal.path_constraints = *path_constraints_; + + goal_out = goal; +} + +void moveit::planning_interface::MoveItCpp::constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object) +{ + moveit_msgs::PlaceGoal goal; + goal.attached_object_name = object; + goal.group_name = group_name_; + goal.allowed_planning_time = allowed_planning_time_; + goal.support_surface_name = support_surface_; + goal.planner_id = planner_id_; + if (!support_surface_.empty()) + goal.allow_gripper_support_collision = true; + + if (path_constraints_) + goal.path_constraints = *path_constraints_; + + goal_out = goal; +} + +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::execute(const Plan& plan, bool wait) +{ + if (!execute_action_client_->isServerConnected()) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + } + + moveit_msgs::ExecuteTrajectoryGoal goal; + goal.trajectory = plan.trajectory_; + + execute_action_client_->sendGoal(goal); + if (!wait) + { + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); + } + + if (!execute_action_client_->waitForResult()) + { + ROS_INFO_STREAM_NAMED("move_group_interface", "ExecuteTrajectory action returned early"); + } + + if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + return MoveItErrorCode(execute_action_client_->getResult()->error_code); + } + else + { + ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString() + << ": " << execute_action_client_->getState().getText()); + return MoveItErrorCode(execute_action_client_->getResult()->error_code); + } +} + +bool moveit::planning_interface::MoveItCpp::getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds) +{ + if (!current_state_monitor_) + { + ROS_ERROR_NAMED("move_group_interface", "Unable to get current robot state"); + return false; + } + + // if needed, start the monitor and wait up to 1 second for a full robot state + if (!current_state_monitor_->isActive()) + current_state_monitor_->startStateMonitor(); + + if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds)) + { + ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state"); + return false; + } + + current_state = current_state_monitor_->getCurrentState(); + return true; +} + +bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link, const std::string& frame, bool approx) +{ + const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; + // this only works if we have an end-effector + if (!eef.empty()) + { + // first we set the goal to be the same as the start state + moveit::core::RobotStatePtr c = getStartState(); + if (c) + { + active_target_ = JOINT; + c->enforceBounds(); + *joint_state_target_ = *c; + if (!joint_state_target_->satisfiesBounds(goal_joint_tolerance_)) + return false; + } + else + return false; + + // we may need to do approximate IK + kinematics::KinematicsQueryOptions o; + o.return_approximate_solution = approx; + + // if no frame transforms are needed, call IK directly + if (frame.empty() || moveit::core::Transforms::sameFrame(frame, robot_model_->getModelFrame())) + return joint_state_target_->setFromIK(joint_model_group_, eef_pose, eef, 0.0, + moveit::core::GroupStateValidityCallbackFn(), o); + else + { + if (c->knowsFrameTransform(frame)) + { + // transform the pose first if possible, then do IK + const Eigen::Isometry3d& t = joint_state_target_->getFrameTransform(frame); + Eigen::Isometry3d p; + tf2::fromMsg(eef_pose, p); + return joint_state_target_->setFromIK(joint_model_group_, t * p, eef, 0.0, + moveit::core::GroupStateValidityCallbackFn(), o); + } + else + { + ROS_ERROR_NAMED("move_group_interface", "Unable to transform from frame '%s' to frame '%s'", frame.c_str(), + robot_model_->getModelFrame().c_str()); + return false; + } + } + } + else + return false; +} + +robot_state::RobotStatePtr moveit::planning_interface::MoveItCpp::getStartState() +{ + if (considered_start_state_) + return considered_start_state_; + else + { + robot_state::RobotStatePtr s; + getCurrentState(s, 1.0); + return s; + } +} + +bool moveit::planning_interface::MoveItCpp::hasPoseTarget(const std::string& end_effector_link) const +{ + const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; + return pose_targets_.find(eef) != pose_targets_.end(); +} + +void moveit::planning_interface::MoveItCpp::initializeConstraintsStorageThread(const std::string& host, unsigned int port) +{ + // Set up db + try + { + warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase(); + conn->setParams(host, port); + if (conn->connect()) + { + constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(conn)); + } + } + catch (std::exception& ex) + { + ROS_ERROR_NAMED("move_group_interface", "%s", ex.what()); + } + initializing_constraints_ = false; +} + +void moveit::planning_interface::MoveItCpp::clearContents() { - impl_->constructMotionPlanRequest(goal_out); + //TODO figure out what else needs to be removed/cleaned. + group_name_.clear(); + robot_description_.clear(); + //node_handle_; + tf_buffer_ = nullptr; + //robot_model_ + current_state_monitor_ = nullptr; + execute_action_client_ = nullptr; + pick_action_client_ = nullptr; + place_action_client_ = nullptr; + + considered_start_state_ = nullptr; + //workspace_parameters_ + //allowed_planning_time_ + planner_id_.clear(); + //num_planning_attempts_ + //max_velocity_scaling_factor_ + //max_acceleration_scaling_factor_ + //goal_joint_tolerance_; + //goal_position_tolerance_ + //goal_orientation_tolerance_ + //can_look_ + //can_replan_ + //replan_delay_ + + joint_state_target_ = nullptr; + joint_model_group_ = nullptr; + + pose_targets_.clear(); + + //active_target_ + path_constraints_ = nullptr; + trajectory_constraints_ = nullptr; + end_effector_link_.clear(); + pose_reference_frame_.clear(); + support_surface_.clear(); + + //trajectory_event_publisher_ + //attached_object_publisher_ + //query_service_ + //get_params_service_ + //set_params_service_ + //cartesian_path_service_ + //plan_grasps_service_ + constraints_storage_ = nullptr; + constraints_init_thread_ = nullptr; + //initializing_constraints_ } From 7c2378dc7e225aafb81acd4b6f1a60f5ea6d922b Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Wed, 31 Jul 2019 09:12:51 -0600 Subject: [PATCH 04/29] Added tests --- .../include/moveit/moveit_cpp/moveit_cpp.h | 5 - .../moveit_cpp/src/moveit_cpp.cpp | 28 +++-- .../test/python_moveit_cpp.py | 76 ++++++++++++ .../test/python_moveit_cpp.test | 9 ++ .../test/python_moveit_cpp_ns.py | 114 ++++++++++++++++++ .../test/python_moveit_cpp_ns.test | 9 ++ 6 files changed, 223 insertions(+), 18 deletions(-) create mode 100755 moveit_ros/planning_interface/test/python_moveit_cpp.py create mode 100644 moveit_ros/planning_interface/test/python_moveit_cpp.test create mode 100755 moveit_ros/planning_interface/test/python_moveit_cpp_ns.py create mode 100644 moveit_ros/planning_interface/test/python_moveit_cpp_ns.test diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 06668eb8e9..7df3035f1b 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -190,9 +190,6 @@ class MoveItCpp MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer = std::shared_ptr(), const ros::WallDuration& wait_for_servers = ros::WallDuration()); - MOVEIT_DEPRECATED MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer, - const ros::Duration& wait_for_servers); - /** \brief Construct a client for the MoveGroup action for a particular \e group. @@ -203,8 +200,6 @@ class MoveItCpp MoveItCpp(const std::string& group, const std::shared_ptr& tf_buffer = std::shared_ptr(), const ros::WallDuration& wait_for_servers = ros::WallDuration()); - MOVEIT_DEPRECATED MoveItCpp(const std::string& group, const std::shared_ptr& tf_buffer, - const ros::Duration& wait_for_servers); ~MoveItCpp(); diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index dd454a85a1..77fed97696 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -102,7 +102,7 @@ moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, group_name_ = opt.group_name_; robot_description_ = opt.robot_description_; robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_); - if (!getRobotModel()) + if (!robot_model_) { std::string error = "Unable to construct robot model. Please make sure all needed information is on the " "parameter server."; @@ -110,16 +110,16 @@ moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, throw std::runtime_error(error); } - if (!getRobotModel()->hasJointModelGroup(opt.group_name_)) + if (!robot_model_->hasJointModelGroup(opt.group_name_)) { std::string error = "Group '" + opt.group_name_ + "' was not found."; ROS_FATAL_STREAM_NAMED("move_group_interface", error); throw std::runtime_error(error); } - joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_); + joint_model_group_ = robot_model_->getJointModelGroup(opt.group_name_); - joint_state_target_.reset(new robot_state::RobotState(getRobotModel())); + joint_state_target_.reset(new robot_state::RobotState(robot_model_)); joint_state_target_->setToDefaultValues(); active_target_ = JOINT; can_look_ = false; @@ -136,7 +136,7 @@ moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, if (joint_model_group_->isChain()) end_effector_link_ = joint_model_group_->getLinkModelNames().back(); - pose_reference_frame_ = getRobotModel()->getModelFrame(); + pose_reference_frame_ = robot_model_->getModelFrame(); trajectory_event_publisher_ = node_handle_.advertise(trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false); attached_object_publisher_ = node_handle_.advertise(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1, false); @@ -191,6 +191,7 @@ moveit::planning_interface::MoveItCpp::MoveItCpp( moveit::planning_interface::MoveItCpp::~MoveItCpp() { + clearContents(); } moveit::planning_interface::MoveItCpp::MoveItCpp(MoveItCpp&& other) @@ -265,8 +266,8 @@ const std::string& moveit::planning_interface::MoveItCpp::getName() const const std::vector moveit::planning_interface::MoveItCpp::getNamedTargets() { - const robot_model::RobotModelConstPtr& robot = getRobotModel(); - const std::string& group = getName(); + const robot_model::RobotModelConstPtr& robot = robot_model_; + const std::string& group = group_name_; const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group); if (joint_group) @@ -600,7 +601,7 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp moveit_msgs::PlaceLocation location; location.pre_place_approach.direction.vector.z = -1.0; location.post_place_retreat.direction.vector.x = -1.0; - location.pre_place_approach.direction.header.frame_id = getRobotModel()->getModelFrame(); + location.pre_place_approach.direction.header.frame_id = robot_model_->getModelFrame(); location.post_place_retreat.direction.header.frame_id = end_effector_link_; location.pre_place_approach.min_distance = 0.1; @@ -904,9 +905,9 @@ const std::string& moveit::planning_interface::MoveItCpp::getEndEffector() const if (!end_effector_link_.empty()) { const std::vector& possible_eefs = - getRobotModel()->getJointModelGroup(group_name_)->getAttachedEndEffectorNames(); + robot_model_->getJointModelGroup(group_name_)->getAttachedEndEffectorNames(); for (const std::string& possible_eef : possible_eefs) - if (getRobotModel()->getEndEffector(possible_eef)->hasLinkModel(end_effector_link_)) + if (robot_model_->getEndEffector(possible_eef)->hasLinkModel(end_effector_link_)) return possible_eef; } static std::string empty; @@ -1220,7 +1221,7 @@ std::vector moveit::planning_interface::MoveItCpp::getCurrentJointValues robot_state::RobotStatePtr current_state; std::vector values; if (getCurrentState(current_state, 1.0)) - current_state->copyJointGroupPositions(getName(), values); + current_state->copyJointGroupPositions(group_name_, values); return values; } @@ -1434,7 +1435,7 @@ void moveit::planning_interface::MoveItCpp::setConstraintsDatabase(const std::st void moveit::planning_interface::MoveItCpp::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) { - workspace_parameters_.header.frame_id = getRobotModel()->getModelFrame(); + workspace_parameters_.header.frame_id = robot_model_->getModelFrame(); workspace_parameters_.header.stamp = ros::Time::now(); workspace_parameters_.min_corner.x = minx; workspace_parameters_.min_corner.y = miny; @@ -1875,7 +1876,8 @@ void moveit::planning_interface::MoveItCpp::initializeConstraintsStorageThread(c void moveit::planning_interface::MoveItCpp::clearContents() { - //TODO figure out what else needs to be removed/cleaned. + // TODO Instead of setting to nullptrs actually delete + // TODO Set values to "default values" group_name_.clear(); robot_description_.clear(); //node_handle_; diff --git a/moveit_ros/planning_interface/test/python_moveit_cpp.py b/moveit_ros/planning_interface/test/python_moveit_cpp.py new file mode 100755 index 0000000000..9629851ba9 --- /dev/null +++ b/moveit_ros/planning_interface/test/python_moveit_cpp.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python + +import unittest +import numpy as np +import rospy +import rostest +import os + +from moveit_ros_planning_interface._moveit_moveit_cpp_interface import MoveItCpp +from moveit_msgs.msg import MoveItErrorCodes + + +class PythonMoveItCppTest(unittest.TestCase): + PLANNING_GROUP = "manipulator" + + @classmethod + def setUpClass(self): + self.group = MoveItCpp(self.PLANNING_GROUP, "robot_description", rospy.get_namespace()) + + @classmethod + def tearDown(self): + pass + + def check_target_setting(self, expect, *args): + if len(args) == 0: + args = [expect] + self.group.set_joint_value_target(*args) + res = self.group.get_joint_value_target() + self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)), + "Setting failed for %s, values: %s" % (type(args[0]), res)) + + def test_target_setting(self): + n = self.group.get_variable_count() + self.check_target_setting([0.1] * n) + self.check_target_setting((0.2,) * n) + self.check_target_setting(np.zeros(n)) + self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}) + self.check_target_setting([0.5] + [0.3]*(n-1), "joint_1", 0.5) + + def plan(self, target): + self.group.set_joint_value_target(target) + return self.group.plan() + + def test_validation(self): + current = np.asarray(self.group.get_current_joint_values()) + + error_code1, plan1, time = self.plan(current + 0.2) + error_code2, plan2, time = self.plan(current + 0.2) + + # both plans should have succeeded: + error_code = MoveItErrorCodes() + error_code.deserialize(error_code1) + self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) + error_code = MoveItErrorCodes() + error_code.deserialize(error_code2) + self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) + + # first plan should execute + self.assertTrue(self.group.execute(plan1)) + + # second plan should be invalid now (due to modified start point) and rejected + self.assertFalse(self.group.execute(plan2)) + + # newly planned trajectory should execute again + error_code3, plan3, time = self.plan(current) + error_code = MoveItErrorCodes() + error_code.deserialize(error_code3) + self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) + self.assertTrue(self.group.execute(plan3)) + + +if __name__ == '__main__': + PKGNAME = 'moveit_ros_planning_interface' + NODENAME = 'moveit_test_python_moveit_cpp' + rospy.init_node(NODENAME) + rostest.rosrun(PKGNAME, NODENAME, PythonMoveItCppTest) diff --git a/moveit_ros/planning_interface/test/python_moveit_cpp.test b/moveit_ros/planning_interface/test/python_moveit_cpp.test new file mode 100644 index 0000000000..9dc5afe28f --- /dev/null +++ b/moveit_ros/planning_interface/test/python_moveit_cpp.test @@ -0,0 +1,9 @@ + + + + + + + + diff --git a/moveit_ros/planning_interface/test/python_moveit_cpp_ns.py b/moveit_ros/planning_interface/test/python_moveit_cpp_ns.py new file mode 100755 index 0000000000..3e431012fe --- /dev/null +++ b/moveit_ros/planning_interface/test/python_moveit_cpp_ns.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python + +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: William Baker +# +# This test is used to ensure planning with a MoveGroupInterface is +# possbile if the robot's move_group node is in a different namespace + +import unittest +import numpy as np +import rospy +import rostest +import os + +from moveit_ros_planning_interface._moveit_moveit_cpp_interface import MoveItCpp +from moveit_msgs.msg import MoveItErrorCodes + + +class PythonMoveItCppNsTest(unittest.TestCase): + PLANNING_GROUP = "manipulator" + PLANNING_NS = "test_ns/" + + @classmethod + def setUpClass(self): + self.group = MoveItCpp(self.PLANNING_GROUP, "%srobot_description" % self.PLANNING_NS, self.PLANNING_NS) + + @classmethod + def tearDown(self): + pass + + def check_target_setting(self, expect, *args): + if len(args) == 0: + args = [expect] + self.group.set_joint_value_target(*args) + res = self.group.get_joint_value_target() + self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)), + "Setting failed for %s, values: %s" % (type(args[0]), res)) + + def test_target_setting(self): + n = self.group.get_variable_count() + self.check_target_setting([0.1] * n) + self.check_target_setting((0.2,) * n) + self.check_target_setting(np.zeros(n)) + self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}) + self.check_target_setting([0.5] + [0.3]*(n-1), "joint_1", 0.5) + + def plan(self, target): + self.group.set_joint_value_target(target) + return self.group.plan() + + def test_validation(self): + current = np.asarray(self.group.get_current_joint_values()) + + error_code1, plan1, time = self.plan(current + 0.2) + error_code2, plan2, time = self.plan(current + 0.2) + + # both plans should have succeeded: + error_code = MoveItErrorCodes() + error_code.deserialize(error_code1) + self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) + error_code = MoveItErrorCodes() + error_code.deserialize(error_code2) + self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) + + # first plan should execute + self.assertTrue(self.group.execute(plan1)) + + # second plan should be invalid now (due to modified start point) and rejected + self.assertFalse(self.group.execute(plan2)) + + # newly planned trajectory should execute again + error_code3, plan3, time = self.plan(current) + self.assertTrue(self.group.execute(plan3)) + error_code = MoveItErrorCodes() + error_code.deserialize(error_code3) + self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) + + +if __name__ == '__main__': + PKGNAME = 'moveit_ros_planning_interface' + NODENAME = 'moveit_test_python_moveit_cpp' + rospy.init_node(NODENAME) + rostest.rosrun(PKGNAME, NODENAME, PythonMoveItCppNsTest) diff --git a/moveit_ros/planning_interface/test/python_moveit_cpp_ns.test b/moveit_ros/planning_interface/test/python_moveit_cpp_ns.test new file mode 100644 index 0000000000..188ab6632a --- /dev/null +++ b/moveit_ros/planning_interface/test/python_moveit_cpp_ns.test @@ -0,0 +1,9 @@ + + + + + + + From 0a83ab0cd63b38ac0b761a2f49e1519d2e363eb9 Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Wed, 31 Jul 2019 18:56:19 -0600 Subject: [PATCH 05/29] Clang Format --- .../include/moveit/moveit_cpp/moveit_cpp.h | 14 +- .../moveit_cpp/src/moveit_cpp.cpp | 310 +++++++++--------- .../moveit_cpp/src/wrap_python_moveit_cpp.cpp | 57 ++-- 3 files changed, 182 insertions(+), 199 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 7df3035f1b..141cb5f462 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -98,7 +98,6 @@ namespace moveit /** \brief Simple interface to MoveIt! components */ namespace planning_interface { - class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes { public: @@ -144,7 +143,7 @@ class MoveItCpp /** \brief Specification of options to use when constructing the MoveItCpp class */ struct Options { - Options(const std::string& group_name="FAKE", const std::string& desc = ROBOT_DESCRIPTION, + Options(const std::string& group_name = "FAKE", const std::string& desc = ROBOT_DESCRIPTION, const ros::NodeHandle& node_handle = ros::NodeHandle()) : group_name_(group_name), robot_description_(desc), node_handle_(node_handle) { @@ -187,9 +186,8 @@ class MoveItCpp one will be constructed internally along with an internal TF2_ROS TransformListener \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. */ - MoveItCpp(const Options& opt, - const std::shared_ptr& tf_buffer = std::shared_ptr(), - const ros::WallDuration& wait_for_servers = ros::WallDuration()); + MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer = std::shared_ptr(), + const ros::WallDuration& wait_for_servers = ros::WallDuration()); /** \brief Construct a client for the MoveGroup action for a particular \e group. @@ -198,8 +196,8 @@ class MoveItCpp \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. */ MoveItCpp(const std::string& group, - const std::shared_ptr& tf_buffer = std::shared_ptr(), - const ros::WallDuration& wait_for_servers = ros::WallDuration()); + const std::shared_ptr& tf_buffer = std::shared_ptr(), + const ros::WallDuration& wait_for_servers = ros::WallDuration()); ~MoveItCpp(); @@ -1009,7 +1007,7 @@ class MoveItCpp robot_state::RobotStatePtr getStartState(); bool hasPoseTarget(const std::string& end_effector_link) const; void clearContents(); - //void initializeConstraintsStorage(const std::string& host, unsigned int port); + // void initializeConstraintsStorage(const std::string& host, unsigned int port); // context contents planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index 77fed97696..e4638a6594 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -73,9 +73,8 @@ namespace moveit { namespace planning_interface { - const std::string MoveItCpp::ROBOT_DESCRIPTION = - "robot_description"; // name of the robot description (a param name, so it can be changed externally) + "robot_description"; // name of the robot description (a param name, so it can be changed externally) const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps } // planning_interface @@ -94,8 +93,7 @@ moveit::planning_interface::MoveItCpp::MoveItCpp(const std::string& group, { } -moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, - const std::shared_ptr& tf_buffer, +moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer, const ros::WallDuration& wait_for_servers) : node_handle_(opt.node_handle_), tf_buffer_(tf_buffer) { @@ -105,7 +103,7 @@ moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, if (!robot_model_) { std::string error = "Unable to construct robot model. Please make sure all needed information is on the " - "parameter server."; + "parameter server."; ROS_FATAL_STREAM_NAMED("move_group_interface", error); throw std::runtime_error(error); } @@ -138,8 +136,10 @@ moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, end_effector_link_ = joint_model_group_->getLinkModelNames().back(); pose_reference_frame_ = robot_model_->getModelFrame(); - trajectory_event_publisher_ = node_handle_.advertise(trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false); - attached_object_publisher_ = node_handle_.advertise(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1, false); + trajectory_event_publisher_ = node_handle_.advertise( + trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false); + attached_object_publisher_ = node_handle_.advertise( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1, false); current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_buffer_, node_handle_); @@ -149,42 +149,41 @@ moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, double allotted_time = wait_for_servers.toSec(); move_action_client_.reset( - new actionlib::SimpleActionClient(node_handle_, move_group::MOVE_ACTION, false)); + new actionlib::SimpleActionClient(node_handle_, move_group::MOVE_ACTION, false)); waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time); pick_action_client_.reset( - new actionlib::SimpleActionClient(node_handle_, move_group::PICKUP_ACTION, false)); + new actionlib::SimpleActionClient(node_handle_, move_group::PICKUP_ACTION, false)); waitForAction(pick_action_client_, move_group::PICKUP_ACTION, timeout_for_servers, allotted_time); place_action_client_.reset( - new actionlib::SimpleActionClient(node_handle_, move_group::PLACE_ACTION, false)); + new actionlib::SimpleActionClient(node_handle_, move_group::PLACE_ACTION, false)); waitForAction(place_action_client_, move_group::PLACE_ACTION, timeout_for_servers, allotted_time); execute_action_client_.reset(new actionlib::SimpleActionClient( - node_handle_, move_group::EXECUTE_ACTION_NAME, false)); + node_handle_, move_group::EXECUTE_ACTION_NAME, false)); waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time); query_service_ = - node_handle_.serviceClient(move_group::QUERY_PLANNERS_SERVICE_NAME); + node_handle_.serviceClient(move_group::QUERY_PLANNERS_SERVICE_NAME); get_params_service_ = - node_handle_.serviceClient(move_group::GET_PLANNER_PARAMS_SERVICE_NAME); + node_handle_.serviceClient(move_group::GET_PLANNER_PARAMS_SERVICE_NAME); set_params_service_ = - node_handle_.serviceClient(move_group::SET_PLANNER_PARAMS_SERVICE_NAME); + node_handle_.serviceClient(move_group::SET_PLANNER_PARAMS_SERVICE_NAME); cartesian_path_service_ = - node_handle_.serviceClient(move_group::CARTESIAN_PATH_SERVICE_NAME); + node_handle_.serviceClient(move_group::CARTESIAN_PATH_SERVICE_NAME); plan_grasps_service_ = node_handle_.serviceClient(GRASP_PLANNING_SERVICE_NAME); - ROS_INFO_STREAM_NAMED("move_group_interface", "Ready to take commands for planning group " << opt.group_name_ - << "."); + ROS_INFO_STREAM_NAMED("move_group_interface", "Ready to take commands for planning group " << opt.group_name_ << "."); group_name_ = opt.group_name_; robot_description_ = opt.robot_description_; } -moveit::planning_interface::MoveItCpp::MoveItCpp( - const moveit::planning_interface::MoveItCpp::Options& opt, - const std::shared_ptr& tf_buffer, const ros::Duration& wait_for_servers) +moveit::planning_interface::MoveItCpp::MoveItCpp(const moveit::planning_interface::MoveItCpp::Options& opt, + const std::shared_ptr& tf_buffer, + const ros::Duration& wait_for_servers) : MoveItCpp(opt, tf_buffer, ros::WallDuration(wait_for_servers.toSec())) { } @@ -199,8 +198,7 @@ moveit::planning_interface::MoveItCpp::MoveItCpp(MoveItCpp&& other) other.clearContents(); } -moveit::planning_interface::MoveItCpp& moveit::planning_interface::MoveItCpp:: -operator=(MoveItCpp&& other) +moveit::planning_interface::MoveItCpp& moveit::planning_interface::MoveItCpp::operator=(MoveItCpp&& other) { if (this != &other) { @@ -289,8 +287,7 @@ const ros::NodeHandle& moveit::planning_interface::MoveItCpp::getNodeHandle() co return node_handle_; } -bool moveit::planning_interface::MoveItCpp::getInterfaceDescription( - moveit_msgs::PlannerInterfaceDescription& desc) +bool moveit::planning_interface::MoveItCpp::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) { moveit_msgs::QueryPlannerInterfaces::Request req; moveit_msgs::QueryPlannerInterfaces::Response res; @@ -303,8 +300,8 @@ bool moveit::planning_interface::MoveItCpp::getInterfaceDescription( return false; } -std::map moveit::planning_interface::MoveItCpp::getPlannerParams( - const std::string& planner_id, const std::string& group) +std::map +moveit::planning_interface::MoveItCpp::getPlannerParams(const std::string& planner_id, const std::string& group) { moveit_msgs::GetPlannerParams::Request req; moveit_msgs::GetPlannerParams::Response res; @@ -319,10 +316,9 @@ std::map moveit::planning_interface::MoveItCpp::getPla return result; } -void moveit::planning_interface::MoveItCpp::setPlannerParams(const std::string& planner_id, - const std::string& group, - const std::map& params, - bool replace) +void moveit::planning_interface::MoveItCpp::setPlannerParams(const std::string& planner_id, const std::string& group, + const std::map& params, + bool replace) { moveit_msgs::SetPlannerParams::Request req; moveit_msgs::SetPlannerParams::Response res; @@ -370,8 +366,7 @@ void moveit::planning_interface::MoveItCpp::setMaxVelocityScalingFactor(double m max_velocity_scaling_factor_ = max_velocity_scaling_factor; } -void moveit::planning_interface::MoveItCpp::setMaxAccelerationScalingFactor( - double max_acceleration_scaling_factor) +void moveit::planning_interface::MoveItCpp::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) { max_acceleration_scaling_factor_ = max_acceleration_scaling_factor; } @@ -392,8 +387,7 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp return move(true); } -moveit::planning_interface::MoveItErrorCode -moveit::planning_interface::MoveItCpp::asyncExecute(const Plan& plan) +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::asyncExecute(const Plan& plan) { return execute(plan, false); } @@ -437,19 +431,20 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp else { ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": " - << move_action_client_->getState().getText()); + << move_action_client_->getState().getText()); return MoveItErrorCode(move_action_client_->getResult()->error_code); } } -moveit::planning_interface::MoveItErrorCode -moveit::planning_interface::MoveItCpp::pick(const std::string& object, bool plan_only) +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::pick(const std::string& object, + bool plan_only) { return pick(object, std::vector(), plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::pick( - const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only) +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::pick(const std::string& object, + const moveit_msgs::Grasp& grasp, + bool plan_only) { return pick(object, std::vector(1, grasp), plan_only); } @@ -489,7 +484,7 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp else { ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": " - << pick_action_client_->getState().getText()); + << pick_action_client_->getState().getText()); return MoveItErrorCode(pick_action_client_->getResult()->error_code); } } @@ -508,22 +503,22 @@ moveit::planning_interface::MoveItCpp::planGraspsAndPick(const std::string& obje if (objects.empty()) { ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '" - << object << "', but the object could not be found"); + << object << "', but the object could not be found"); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME); } return planGraspsAndPick(objects[object], plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::planGraspsAndPick( - const moveit_msgs::CollisionObject& object, bool plan_only) +moveit::planning_interface::MoveItErrorCode +moveit::planning_interface::MoveItCpp::planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only) { if (!plan_grasps_service_) { ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '" - << GRASP_PLANNING_SERVICE_NAME - << "' is not available." - " This has to be implemented and started seperatly."); + << GRASP_PLANNING_SERVICE_NAME + << "' is not available." + " This has to be implemented and started seperatly."); return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } @@ -545,8 +540,8 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp return pick(object.id, response.grasps, plan_only); } -moveit::planning_interface::MoveItErrorCode -moveit::planning_interface::MoveItCpp::place(const std::string& object, bool plan_only) +moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::place(const std::string& object, + bool plan_only) { return place(object, std::vector(), plan_only); } @@ -587,7 +582,7 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp else { ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": " - << place_action_client_->getState().getText()); + << place_action_client_->getState().getText()); return MoveItErrorCode(place_action_client_->getResult()->error_code); } } @@ -624,19 +619,23 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp return place(object, std::vector(1, pose), plan_only); } -double moveit::planning_interface::MoveItCpp::computeCartesianPath( - const std::vector& waypoints, double eef_step, double jump_threshold, - moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code) +double moveit::planning_interface::MoveItCpp::computeCartesianPath(const std::vector& waypoints, + double eef_step, double jump_threshold, + moveit_msgs::RobotTrajectory& trajectory, + bool avoid_collisions, + moveit_msgs::MoveItErrorCodes* error_code) { moveit_msgs::Constraints path_constraints_tmp; return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions, error_code); } -double moveit::planning_interface::MoveItCpp::computeCartesianPath( - const std::vector& waypoints, double eef_step, double jump_threshold, - moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions, - moveit_msgs::MoveItErrorCodes* error_code) +double moveit::planning_interface::MoveItCpp::computeCartesianPath(const std::vector& waypoints, + double eef_step, double jump_threshold, + moveit_msgs::RobotTrajectory& trajectory, + const moveit_msgs::Constraints& path_constraints, + bool avoid_collisions, + moveit_msgs::MoveItErrorCodes* error_code) { moveit_msgs::GetCartesianPath::Request req; moveit_msgs::GetCartesianPath::Response res; @@ -666,9 +665,9 @@ double moveit::planning_interface::MoveItCpp::computeCartesianPath( return res.fraction; } else - { + { return -1.0; - } + } } else { @@ -708,7 +707,7 @@ void moveit::planning_interface::MoveItCpp::setStartStateToCurrentState() void moveit::planning_interface::MoveItCpp::setRandomTarget() { joint_state_target_->setToRandomPositions(); - active_target_=JOINT; + active_target_ = JOINT; } const std::vector& moveit::planning_interface::MoveItCpp::getJointNames() @@ -721,8 +720,7 @@ const std::vector& moveit::planning_interface::MoveItCpp::getLinkNa return joint_model_group_->getLinkModelNames(); } -std::map -moveit::planning_interface::MoveItCpp::getNamedTargetValues(const std::string& name) +std::map moveit::planning_interface::MoveItCpp::getNamedTargetValues(const std::string& name) { std::map >::const_iterator it = remembered_joint_values_.find(name); std::map positions; @@ -761,8 +759,7 @@ bool moveit::planning_interface::MoveItCpp::setNamedTarget(const std::string& na } } -void moveit::planning_interface::MoveItCpp::getJointValueTarget( - std::vector& group_variable_values) const +void moveit::planning_interface::MoveItCpp::getJointValueTarget(std::vector& group_variable_values) const { joint_state_target_->copyJointGroupPositions(joint_model_group_, group_variable_values); } @@ -776,16 +773,14 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::vecto return joint_state_target_->satisfiesBounds(joint_model_group_, goal_joint_tolerance_); } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget( - const std::map& variable_values) +bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::map& variable_values) { const auto& allowed = joint_model_group_->getVariableNames(); for (const auto& pair : variable_values) { if (std::find(allowed.begin(), allowed.end(), pair.first) == allowed.end()) { - ROS_ERROR_STREAM("joint variable " << pair.first << " is not part of group " - << joint_model_group_->getName()); + ROS_ERROR_STREAM("joint variable " << pair.first << " is not part of group " << joint_model_group_->getName()); return false; } } @@ -796,15 +791,14 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget( } bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::vector& variable_names, - const std::vector& variable_values) + const std::vector& variable_values) { const auto& allowed = joint_model_group_->getVariableNames(); for (const auto& variable_name : variable_names) { if (std::find(allowed.begin(), allowed.end(), variable_name) == allowed.end()) { - ROS_ERROR_STREAM("joint variable " << variable_name << " is not part of group " - << joint_model_group_->getName()); + ROS_ERROR_STREAM("joint variable " << variable_name << " is not part of group " << joint_model_group_->getName()); return false; } } @@ -828,7 +822,7 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::strin } bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::string& joint_name, - const std::vector& values) + const std::vector& values) { active_target_ = JOINT; const robot_model::JointModel* jm = joint_model_group_->getJointModel(joint_name); @@ -848,38 +842,38 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const sensor_msg } bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_msgs::Pose& eef_pose, - const std::string& end_effector_link) + const std::string& end_effector_link) { return setJointValueTarget(eef_pose, end_effector_link, "", false); } bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, - const std::string& end_effector_link) + const std::string& end_effector_link) { return setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false); } bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const Eigen::Isometry3d& eef_pose, - const std::string& end_effector_link) + const std::string& end_effector_link) { geometry_msgs::Pose msg = tf2::toMsg(eef_pose); return setJointValueTarget(msg, end_effector_link); } -bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget( - const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link) +bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose, + const std::string& end_effector_link) { return setJointValueTarget(eef_pose, end_effector_link, "", true); } -bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget( - const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link) +bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, + const std::string& end_effector_link) { return setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true); } -bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget( - const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link) +bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose, + const std::string& end_effector_link) { geometry_msgs::Pose msg = tf2::toMsg(eef_pose); return setApproximateJointValueTarget(msg, end_effector_link); @@ -905,7 +899,7 @@ const std::string& moveit::planning_interface::MoveItCpp::getEndEffector() const if (!end_effector_link_.empty()) { const std::vector& possible_eefs = - robot_model_->getJointModelGroup(group_name_)->getAttachedEndEffectorNames(); + robot_model_->getJointModelGroup(group_name_)->getAttachedEndEffectorNames(); for (const std::string& possible_eef : possible_eefs) if (robot_model_->getEndEffector(possible_eef)->hasLinkModel(end_effector_link_)) return possible_eef; @@ -942,7 +936,7 @@ void moveit::planning_interface::MoveItCpp::clearPoseTargets() } bool moveit::planning_interface::MoveItCpp::setPoseTarget(const Eigen::Isometry3d& pose, - const std::string& end_effector_link) + const std::string& end_effector_link) { std::vector pose_msg(1); pose_msg[0].pose = tf2::toMsg(pose); @@ -952,7 +946,7 @@ bool moveit::planning_interface::MoveItCpp::setPoseTarget(const Eigen::Isometry3 } bool moveit::planning_interface::MoveItCpp::setPoseTarget(const geometry_msgs::Pose& target, - const std::string& end_effector_link) + const std::string& end_effector_link) { std::vector pose_msg(1); pose_msg[0].pose = target; @@ -962,14 +956,14 @@ bool moveit::planning_interface::MoveItCpp::setPoseTarget(const geometry_msgs::P } bool moveit::planning_interface::MoveItCpp::setPoseTarget(const geometry_msgs::PoseStamped& target, - const std::string& end_effector_link) + const std::string& end_effector_link) { std::vector targets(1, target); return setPoseTargets(targets, end_effector_link); } bool moveit::planning_interface::MoveItCpp::setPoseTargets(const EigenSTL::vector_Isometry3d& target, - const std::string& end_effector_link) + const std::string& end_effector_link) { std::vector pose_out(target.size()); ros::Time tm = ros::Time::now(); @@ -984,7 +978,7 @@ bool moveit::planning_interface::MoveItCpp::setPoseTargets(const EigenSTL::vecto } bool moveit::planning_interface::MoveItCpp::setPoseTargets(const std::vector& target, - const std::string& end_effector_link) + const std::string& end_effector_link) { std::vector target_stamped(target.size()); ros::Time tm = ros::Time::now(); @@ -998,7 +992,8 @@ bool moveit::planning_interface::MoveItCpp::setPoseTargets(const std::vector& target, const std::string& end_effector_link) +bool moveit::planning_interface::MoveItCpp::setPoseTargets(const std::vector& target, + const std::string& end_effector_link) { if (target.empty()) { @@ -1026,7 +1021,8 @@ bool moveit::planning_interface::MoveItCpp::setPoseTargets(const std::vector& moveit::planning_interface::MoveItCpp::getPoseTargets(const std::string& end_effector_link) const +const std::vector& +moveit::planning_interface::MoveItCpp::getPoseTargets(const std::string& end_effector_link) const { const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; @@ -1073,7 +1070,7 @@ inline void transformPose(const tf2_ros::Buffer& tf_buffer, const std::string& d } // namespace bool moveit::planning_interface::MoveItCpp::setPositionTarget(double x, double y, double z, - const std::string& end_effector_link) + const std::string& end_effector_link) { geometry_msgs::PoseStamped target; if (hasPoseTarget(end_effector_link)) @@ -1099,7 +1096,7 @@ bool moveit::planning_interface::MoveItCpp::setPositionTarget(double x, double y } bool moveit::planning_interface::MoveItCpp::setRPYTarget(double r, double p, double y, - const std::string& end_effector_link) + const std::string& end_effector_link) { geometry_msgs::PoseStamped target; if (hasPoseTarget(end_effector_link)) @@ -1123,7 +1120,7 @@ bool moveit::planning_interface::MoveItCpp::setRPYTarget(double r, double p, dou } bool moveit::planning_interface::MoveItCpp::setOrientationTarget(double x, double y, double z, double w, - const std::string& end_effector_link) + const std::string& end_effector_link) { geometry_msgs::PoseStamped target; if (hasPoseTarget(end_effector_link)) @@ -1232,8 +1229,7 @@ std::vector moveit::planning_interface::MoveItCpp::getRandomJointValues( return r; } -geometry_msgs::PoseStamped -moveit::planning_interface::MoveItCpp::getRandomPose(const std::string& end_effector_link) +geometry_msgs::PoseStamped moveit::planning_interface::MoveItCpp::getRandomPose(const std::string& end_effector_link) { const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; Eigen::Isometry3d pose; @@ -1258,8 +1254,7 @@ moveit::planning_interface::MoveItCpp::getRandomPose(const std::string& end_effe return pose_msg; } -geometry_msgs::PoseStamped -moveit::planning_interface::MoveItCpp::getCurrentPose(const std::string& end_effector_link) +geometry_msgs::PoseStamped moveit::planning_interface::MoveItCpp::getCurrentPose(const std::string& end_effector_link) { const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; Eigen::Isometry3d pose; @@ -1333,7 +1328,7 @@ robot_state::RobotStatePtr moveit::planning_interface::MoveItCpp::getCurrentStat } void moveit::planning_interface::MoveItCpp::rememberJointValues(const std::string& name, - const std::vector& values) + const std::vector& values) { remembered_joint_values_[name] = values; } @@ -1429,11 +1424,12 @@ void moveit::planning_interface::MoveItCpp::setConstraintsDatabase(const std::st initializing_constraints_ = true; if (constraints_init_thread_) constraints_init_thread_->join(); - constraints_init_thread_.reset(new boost::thread(boost::bind(&MoveItCpp::initializeConstraintsStorageThread, this, host, port))); + constraints_init_thread_.reset( + new boost::thread(boost::bind(&MoveItCpp::initializeConstraintsStorageThread, this, host, port))); } void moveit::planning_interface::MoveItCpp::setWorkspace(double minx, double miny, double minz, double maxx, - double maxy, double maxz) + double maxy, double maxz) { workspace_parameters_.header.frame_id = robot_model_->getModelFrame(); workspace_parameters_.header.stamp = ros::Time::now(); @@ -1479,7 +1475,7 @@ bool moveit::planning_interface::MoveItCpp::attachObject(const std::string& obje } bool moveit::planning_interface::MoveItCpp::attachObject(const std::string& object, const std::string& link, - const std::vector& touch_links) + const std::vector& touch_links) { std::string l = link.empty() ? getEndEffectorLink() : link; if (l.empty()) @@ -1529,8 +1525,7 @@ bool moveit::planning_interface::MoveItCpp::detachObject(const std::string& name return true; } -void moveit::planning_interface::MoveItCpp::constructMotionPlanRequest( - moveit_msgs::MotionPlanRequest& request) +void moveit::planning_interface::MoveItCpp::constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request) { request.group_name = group_name_; request.num_planning_attempts = num_planning_attempts_; @@ -1549,13 +1544,14 @@ void moveit::planning_interface::MoveItCpp::constructMotionPlanRequest( { request.goal_constraints.resize(1); request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( - getTargetRobotState(), joint_model_group_, goal_joint_tolerance_); + getTargetRobotState(), joint_model_group_, goal_joint_tolerance_); } else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION) { // find out how many goals are specified std::size_t goal_count = 0; - for (std::map >::const_iterator it = pose_targets_.begin(); it != pose_targets_.end(); ++it) + for (std::map >::const_iterator it = pose_targets_.begin(); + it != pose_targets_.end(); ++it) goal_count = std::max(goal_count, it->second.size()); // start filling the goals; @@ -1564,31 +1560,33 @@ void moveit::planning_interface::MoveItCpp::constructMotionPlanRequest( // to reach the goal that corresponds to the goals of the other end effectors request.goal_constraints.resize(goal_count); - for (std::map >::const_iterator it = pose_targets_.begin(); it != pose_targets_.end(); ++it) + for (std::map >::const_iterator it = pose_targets_.begin(); + it != pose_targets_.end(); ++it) + { + for (std::size_t i = 0; i < it->second.size(); ++i) { - for (std::size_t i = 0; i < it->second.size(); ++i) - { - moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints( - it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_); - if (active_target_ == ORIENTATION) - c.position_constraints.clear(); - if (active_target_ == POSITION) - c.orientation_constraints.clear(); - request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c); - } + moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints( + it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_); + if (active_target_ == ORIENTATION) + c.position_constraints.clear(); + if (active_target_ == POSITION) + c.orientation_constraints.clear(); + request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c); } } - else - ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation"); + } + else + ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation"); - if (path_constraints_) - request.path_constraints = *path_constraints_; - if (trajectory_constraints_) - request.trajectory_constraints = *trajectory_constraints_; + if (path_constraints_) + request.path_constraints = *path_constraints_; + if (trajectory_constraints_) + request.trajectory_constraints = *trajectory_constraints_; } template -void moveit::planning_interface::MoveItCpp::waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) +void moveit::planning_interface::MoveItCpp::waitForAction(const T& action, const std::string& name, + const ros::WallTime& timeout, double allotted_time) { ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str()); @@ -1607,7 +1605,7 @@ void moveit::planning_interface::MoveItCpp::waitForAction(const T& action, const else // in case of nodelets and specific callback queue implementations { ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " - "handling."); + "handling."); } } } @@ -1625,7 +1623,7 @@ void moveit::planning_interface::MoveItCpp::waitForAction(const T& action, const else // in case of nodelets and specific callback queue implementations { ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " - "handling."); + "handling."); } } } @@ -1685,10 +1683,9 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp else { ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString() - << ": " << move_action_client_->getState().getText()); + << ": " << move_action_client_->getState().getText()); return MoveItErrorCode(move_action_client_->getResult()->error_code); } - } void moveit::planning_interface::MoveItCpp::constructGoal(moveit_msgs::MoveGroupGoal& goal) @@ -1696,7 +1693,6 @@ void moveit::planning_interface::MoveItCpp::constructGoal(moveit_msgs::MoveGroup constructMotionPlanRequest(goal.request); } - void moveit::planning_interface::MoveItCpp::constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object) { moveit_msgs::PickupGoal goal; @@ -1760,12 +1756,13 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp else { ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString() - << ": " << execute_action_client_->getState().getText()); + << ": " << execute_action_client_->getState().getText()); return MoveItErrorCode(execute_action_client_->getResult()->error_code); } } -bool moveit::planning_interface::MoveItCpp::getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds) +bool moveit::planning_interface::MoveItCpp::getCurrentState(robot_state::RobotStatePtr& current_state, + double wait_seconds) { if (!current_state_monitor_) { @@ -1787,7 +1784,9 @@ bool moveit::planning_interface::MoveItCpp::getCurrentState(robot_state::RobotSt return true; } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link, const std::string& frame, bool approx) +bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_msgs::Pose& eef_pose, + const std::string& end_effector_link, + const std::string& frame, bool approx) { const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; // this only works if we have an end-effector @@ -1813,7 +1812,7 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_m // if no frame transforms are needed, call IK directly if (frame.empty() || moveit::core::Transforms::sameFrame(frame, robot_model_->getModelFrame())) return joint_state_target_->setFromIK(joint_model_group_, eef_pose, eef, 0.0, - moveit::core::GroupStateValidityCallbackFn(), o); + moveit::core::GroupStateValidityCallbackFn(), o); else { if (c->knowsFrameTransform(frame)) @@ -1823,7 +1822,7 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_m Eigen::Isometry3d p; tf2::fromMsg(eef_pose, p); return joint_state_target_->setFromIK(joint_model_group_, t * p, eef, 0.0, - moveit::core::GroupStateValidityCallbackFn(), o); + moveit::core::GroupStateValidityCallbackFn(), o); } else { @@ -1855,7 +1854,8 @@ bool moveit::planning_interface::MoveItCpp::hasPoseTarget(const std::string& end return pose_targets_.find(eef) != pose_targets_.end(); } -void moveit::planning_interface::MoveItCpp::initializeConstraintsStorageThread(const std::string& host, unsigned int port) +void moveit::planning_interface::MoveItCpp::initializeConstraintsStorageThread(const std::string& host, + unsigned int port) { // Set up db try @@ -1880,48 +1880,48 @@ void moveit::planning_interface::MoveItCpp::clearContents() // TODO Set values to "default values" group_name_.clear(); robot_description_.clear(); - //node_handle_; + // node_handle_; tf_buffer_ = nullptr; - //robot_model_ + // robot_model_ current_state_monitor_ = nullptr; execute_action_client_ = nullptr; pick_action_client_ = nullptr; place_action_client_ = nullptr; considered_start_state_ = nullptr; - //workspace_parameters_ - //allowed_planning_time_ + // workspace_parameters_ + // allowed_planning_time_ planner_id_.clear(); - //num_planning_attempts_ - //max_velocity_scaling_factor_ - //max_acceleration_scaling_factor_ - //goal_joint_tolerance_; - //goal_position_tolerance_ - //goal_orientation_tolerance_ - //can_look_ - //can_replan_ - //replan_delay_ + // num_planning_attempts_ + // max_velocity_scaling_factor_ + // max_acceleration_scaling_factor_ + // goal_joint_tolerance_; + // goal_position_tolerance_ + // goal_orientation_tolerance_ + // can_look_ + // can_replan_ + // replan_delay_ joint_state_target_ = nullptr; joint_model_group_ = nullptr; pose_targets_.clear(); - //active_target_ + // active_target_ path_constraints_ = nullptr; trajectory_constraints_ = nullptr; end_effector_link_.clear(); pose_reference_frame_.clear(); support_surface_.clear(); - //trajectory_event_publisher_ - //attached_object_publisher_ - //query_service_ - //get_params_service_ - //set_params_service_ - //cartesian_path_service_ - //plan_grasps_service_ + // trajectory_event_publisher_ + // attached_object_publisher_ + // query_service_ + // get_params_service_ + // set_params_service_ + // cartesian_path_service_ + // plan_grasps_service_ constraints_storage_ = nullptr; constraints_init_thread_ = nullptr; - //initializing_constraints_ + // initializing_constraints_ } diff --git a/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_moveit_cpp.cpp index 49b74361db..9b205a2a00 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_moveit_cpp.cpp @@ -63,11 +63,11 @@ class MoveItCppWrapper : protected py_bindings_tools::ROScppInitializer, public public: // ROSInitializer is constructed first, and ensures ros::init() was called, if // needed - MoveItCppWrapper(const std::string& group_name, const std::string& robot_description, - const std::string& ns = "", double wait_for_servers = 5.0) + MoveItCppWrapper(const std::string& group_name, const std::string& robot_description, const std::string& ns = "", + double wait_for_servers = 5.0) : py_bindings_tools::ROScppInitializer() - , MoveItCpp(Options(group_name, robot_description, ros::NodeHandle(ns)), - std::shared_ptr(), ros::WallDuration(wait_for_servers)) + , MoveItCpp(Options(group_name, robot_description, ros::NodeHandle(ns)), std::shared_ptr(), + ros::WallDuration(wait_for_servers)) { } @@ -536,8 +536,7 @@ static void wrap_moveit_cpp() moveit_cpp_class.def("get_name", &MoveItCppWrapper::getNameCStr); moveit_cpp_class.def("get_planning_frame", &MoveItCppWrapper::getPlanningFrameCStr); - moveit_cpp_class.def("get_interface_description", - &MoveItCppWrapper::getInterfaceDescriptionPython); + moveit_cpp_class.def("get_interface_description", &MoveItCppWrapper::getInterfaceDescriptionPython); moveit_cpp_class.def("get_active_joints", &MoveItCppWrapper::getActiveJointsList); moveit_cpp_class.def("get_joints", &MoveItCppWrapper::getJointsList); @@ -568,63 +567,52 @@ static void wrap_moveit_cpp() moveit_cpp_class.def("clear_pose_target", &MoveItCppWrapper::clearPoseTarget); moveit_cpp_class.def("clear_pose_targets", &MoveItCppWrapper::clearPoseTargets); - moveit_cpp_class.def("set_joint_value_target", - &MoveItCppWrapper::setJointValueTargetPythonIterable); + moveit_cpp_class.def("set_joint_value_target", &MoveItCppWrapper::setJointValueTargetPythonIterable); moveit_cpp_class.def("set_joint_value_target", &MoveItCppWrapper::setJointValueTargetPythonDict); - moveit_cpp_class.def("set_joint_value_target", - &MoveItCppWrapper::setJointValueTargetPerJointPythonList); + moveit_cpp_class.def("set_joint_value_target", &MoveItCppWrapper::setJointValueTargetPerJointPythonList); bool (MoveItCppWrapper::*set_joint_value_target_4)(const std::string&, double) = &MoveItCppWrapper::setJointValueTarget; moveit_cpp_class.def("set_joint_value_target", set_joint_value_target_4); - moveit_cpp_class.def("set_joint_value_target_from_pose", - &MoveItCppWrapper::setJointValueTargetFromPosePython); + moveit_cpp_class.def("set_joint_value_target_from_pose", &MoveItCppWrapper::setJointValueTargetFromPosePython); moveit_cpp_class.def("set_joint_value_target_from_pose_stamped", - &MoveItCppWrapper::setJointValueTargetFromPoseStampedPython); + &MoveItCppWrapper::setJointValueTargetFromPoseStampedPython); moveit_cpp_class.def("set_joint_value_target_from_joint_state_message", - &MoveItCppWrapper::setJointValueTargetFromJointStatePython); + &MoveItCppWrapper::setJointValueTargetFromJointStatePython); moveit_cpp_class.def("get_joint_value_target", &MoveItCppWrapper::getJointValueTargetPythonList); moveit_cpp_class.def("set_named_target", &MoveItCppWrapper::setNamedTarget); moveit_cpp_class.def("set_random_target", &MoveItCppWrapper::setRandomTarget); - void (MoveItCppWrapper::*remember_joint_values_2)(const std::string&) = - &MoveItCppWrapper::rememberJointValues; + void (MoveItCppWrapper::*remember_joint_values_2)(const std::string&) = &MoveItCppWrapper::rememberJointValues; moveit_cpp_class.def("remember_joint_values", remember_joint_values_2); - moveit_cpp_class.def("remember_joint_values", - &MoveItCppWrapper::rememberJointValuesFromPythonList); + moveit_cpp_class.def("remember_joint_values", &MoveItCppWrapper::rememberJointValuesFromPythonList); moveit_cpp_class.def("start_state_monitor", &MoveItCppWrapper::startStateMonitor); moveit_cpp_class.def("get_current_joint_values", &MoveItCppWrapper::getCurrentJointValuesList); moveit_cpp_class.def("get_random_joint_values", &MoveItCppWrapper::getRandomJointValuesList); - moveit_cpp_class.def("get_remembered_joint_values", - &MoveItCppWrapper::getRememberedJointValuesPython); + moveit_cpp_class.def("get_remembered_joint_values", &MoveItCppWrapper::getRememberedJointValuesPython); moveit_cpp_class.def("forget_joint_values", &MoveItCppWrapper::forgetJointValues); moveit_cpp_class.def("get_goal_joint_tolerance", &MoveItCppWrapper::getGoalJointTolerance); moveit_cpp_class.def("get_goal_position_tolerance", &MoveItCppWrapper::getGoalPositionTolerance); - moveit_cpp_class.def("get_goal_orientation_tolerance", - &MoveItCppWrapper::getGoalOrientationTolerance); + moveit_cpp_class.def("get_goal_orientation_tolerance", &MoveItCppWrapper::getGoalOrientationTolerance); moveit_cpp_class.def("set_goal_joint_tolerance", &MoveItCppWrapper::setGoalJointTolerance); moveit_cpp_class.def("set_goal_position_tolerance", &MoveItCppWrapper::setGoalPositionTolerance); - moveit_cpp_class.def("set_goal_orientation_tolerance", - &MoveItCppWrapper::setGoalOrientationTolerance); + moveit_cpp_class.def("set_goal_orientation_tolerance", &MoveItCppWrapper::setGoalOrientationTolerance); moveit_cpp_class.def("set_goal_tolerance", &MoveItCppWrapper::setGoalTolerance); - moveit_cpp_class.def("set_start_state_to_current_state", - &MoveItCppWrapper::setStartStateToCurrentState); + moveit_cpp_class.def("set_start_state_to_current_state", &MoveItCppWrapper::setStartStateToCurrentState); moveit_cpp_class.def("set_start_state", &MoveItCppWrapper::setStartStatePython); - bool (MoveItCppWrapper::*set_path_constraints_1)(const std::string&) = - &MoveItCppWrapper::setPathConstraints; + bool (MoveItCppWrapper::*set_path_constraints_1)(const std::string&) = &MoveItCppWrapper::setPathConstraints; moveit_cpp_class.def("set_path_constraints", set_path_constraints_1); - moveit_cpp_class.def("set_path_constraints_from_msg", - &MoveItCppWrapper::setPathConstraintsFromMsg); + moveit_cpp_class.def("set_path_constraints_from_msg", &MoveItCppWrapper::setPathConstraintsFromMsg); moveit_cpp_class.def("get_path_constraints", &MoveItCppWrapper::getPathConstraintsPython); moveit_cpp_class.def("clear_path_constraints", &MoveItCppWrapper::clearPathConstraints); moveit_cpp_class.def("get_known_constraints", &MoveItCppWrapper::getKnownConstraintsList); @@ -632,16 +620,13 @@ static void wrap_moveit_cpp() moveit_cpp_class.def("set_workspace", &MoveItCppWrapper::setWorkspace); moveit_cpp_class.def("set_planning_time", &MoveItCppWrapper::setPlanningTime); moveit_cpp_class.def("get_planning_time", &MoveItCppWrapper::getPlanningTime); - moveit_cpp_class.def("set_max_velocity_scaling_factor", - &MoveItCppWrapper::setMaxVelocityScalingFactor); - moveit_cpp_class.def("set_max_acceleration_scaling_factor", - &MoveItCppWrapper::setMaxAccelerationScalingFactor); + moveit_cpp_class.def("set_max_velocity_scaling_factor", &MoveItCppWrapper::setMaxVelocityScalingFactor); + moveit_cpp_class.def("set_max_acceleration_scaling_factor", &MoveItCppWrapper::setMaxAccelerationScalingFactor); moveit_cpp_class.def("set_planner_id", &MoveItCppWrapper::setPlannerId); moveit_cpp_class.def("set_num_planning_attempts", &MoveItCppWrapper::setNumPlanningAttempts); moveit_cpp_class.def("plan", &MoveItCppWrapper::planPython); moveit_cpp_class.def("compute_cartesian_path", &MoveItCppWrapper::computeCartesianPathPython); - moveit_cpp_class.def("compute_cartesian_path", - &MoveItCppWrapper::computeCartesianPathConstrainedPython); + moveit_cpp_class.def("compute_cartesian_path", &MoveItCppWrapper::computeCartesianPathConstrainedPython); moveit_cpp_class.def("set_support_surface_name", &MoveItCppWrapper::setSupportSurfaceName); moveit_cpp_class.def("attach_object", &MoveItCppWrapper::attachObjectPython); moveit_cpp_class.def("detach_object", &MoveItCppWrapper::detachObject); From 9ccf83e87b9227b0e9e6e3ef773ad3cebe6fd36c Mon Sep 17 00:00:00 2001 From: simonGoldstein Date: Thu, 1 Aug 2019 13:08:11 -0600 Subject: [PATCH 06/29] Remove depreciated constructor funtions --- .../moveit_cpp/src/moveit_cpp.cpp | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index e4638a6594..ebd310a571 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -86,13 +86,6 @@ moveit::planning_interface::MoveItCpp::MoveItCpp(const std::string& group_name, { } -moveit::planning_interface::MoveItCpp::MoveItCpp(const std::string& group, - const std::shared_ptr& tf_buffer, - const ros::Duration& wait_for_servers) - : MoveItCpp(Options(group), tf_buffer, ros::WallDuration(wait_for_servers.toSec())) -{ -} - moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer, const ros::WallDuration& wait_for_servers) : node_handle_(opt.node_handle_), tf_buffer_(tf_buffer) @@ -181,13 +174,6 @@ moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, const std:: robot_description_ = opt.robot_description_; } -moveit::planning_interface::MoveItCpp::MoveItCpp(const moveit::planning_interface::MoveItCpp::Options& opt, - const std::shared_ptr& tf_buffer, - const ros::Duration& wait_for_servers) - : MoveItCpp(opt, tf_buffer, ros::WallDuration(wait_for_servers.toSec())) -{ -} - moveit::planning_interface::MoveItCpp::~MoveItCpp() { clearContents(); From 1c5c5114de1fa84be8bf85b89fb6180b9899afdc Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 16 Aug 2019 19:47:33 +0200 Subject: [PATCH 07/29] Remove MoveItCpp python wrapper --- moveit_ros/planning_interface/CMakeLists.txt | 2 +- .../moveit_cpp/CMakeLists.txt | 13 +- .../moveit_cpp/src/wrap_python_moveit_cpp.cpp | 647 ------------------ .../test/python_moveit_cpp.py | 76 -- .../test/python_moveit_cpp.test | 9 - .../test/python_moveit_cpp_ns.py | 114 --- .../test/python_moveit_cpp_ns.test | 9 - 7 files changed, 2 insertions(+), 868 deletions(-) delete mode 100644 moveit_ros/planning_interface/moveit_cpp/src/wrap_python_moveit_cpp.cpp delete mode 100755 moveit_ros/planning_interface/test/python_moveit_cpp.py delete mode 100644 moveit_ros/planning_interface/test/python_moveit_cpp.test delete mode 100755 moveit_ros/planning_interface/test/python_moveit_cpp_ns.py delete mode 100644 moveit_ros/planning_interface/test/python_moveit_cpp_ns.test diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index ae9647c7c7..d8073f37b2 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -61,7 +61,7 @@ catkin_package( moveit_common_planning_interface_objects moveit_planning_scene_interface moveit_move_group_interface - moveit_moveit_cpp + moveit_cpp INCLUDE_DIRS ${THIS_PACKAGE_INCLUDE_DIRS} CATKIN_DEPENDS diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt index e045478964..fd86913dc2 100644 --- a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -1,25 +1,14 @@ -set(MOVEIT_LIB_NAME moveit_moveit_cpp) +set(MOVEIT_LIB_NAME moveit_cpp) add_library(${MOVEIT_LIB_NAME} src/moveit_cpp.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) -add_library(${MOVEIT_LIB_NAME}_python src/wrap_python_moveit_cpp.cpp) -target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) -add_dependencies(${MOVEIT_LIB_NAME}_python ${catkin_EXPORTED_TARGETS}) -set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_moveit_cpp PREFIX "") -set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") - install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -install(TARGETS ${MOVEIT_LIB_NAME}_python - ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) - install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) #add_executable(demo src/demo.cpp) diff --git a/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_moveit_cpp.cpp deleted file mode 100644 index 9b205a2a00..0000000000 --- a/moveit_ros/planning_interface/moveit_cpp/src/wrap_python_moveit_cpp.cpp +++ /dev/null @@ -1,647 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -/** @cond IGNORE */ - -namespace bp = boost::python; - -namespace moveit -{ -namespace planning_interface -{ -class MoveItCppWrapper : protected py_bindings_tools::ROScppInitializer, public MoveItCpp -{ -public: - // ROSInitializer is constructed first, and ensures ros::init() was called, if - // needed - MoveItCppWrapper(const std::string& group_name, const std::string& robot_description, const std::string& ns = "", - double wait_for_servers = 5.0) - : py_bindings_tools::ROScppInitializer() - , MoveItCpp(Options(group_name, robot_description, ros::NodeHandle(ns)), std::shared_ptr(), - ros::WallDuration(wait_for_servers)) - { - } - - bool setJointValueTargetPerJointPythonList(const std::string& joint, bp::list& values) - { - return setJointValueTarget(joint, py_bindings_tools::doubleFromList(values)); - } - - bool setJointValueTargetPythonIterable(bp::object& values) - { - return setJointValueTarget(py_bindings_tools::doubleFromList(values)); - } - - bool setJointValueTargetPythonDict(bp::dict& values) - { - bp::list k = values.keys(); - int l = bp::len(k); - std::map v; - for (int i = 0; i < l; ++i) - v[bp::extract(k[i])] = bp::extract(values[k[i]]); - return setJointValueTarget(v); - } - - bool setJointValueTargetFromPosePython(const std::string& pose_str, const std::string& eef, bool approx) - { - geometry_msgs::Pose pose_msg; - py_bindings_tools::deserializeMsg(pose_str, pose_msg); - return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef); - } - - bool setJointValueTargetFromPoseStampedPython(const std::string& pose_str, const std::string& eef, bool approx) - { - geometry_msgs::PoseStamped pose_msg; - py_bindings_tools::deserializeMsg(pose_str, pose_msg); - return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef); - } - - bool setJointValueTargetFromJointStatePython(const std::string& js_str) - { - sensor_msgs::JointState js_msg; - py_bindings_tools::deserializeMsg(js_str, js_msg); - return setJointValueTarget(js_msg); - } - - bp::list getJointValueTargetPythonList() - { - std::vector values; - MoveItCpp::getJointValueTarget(values); - bp::list l; - for (const double value : values) - l.append(value); - return l; - } - - std::string getJointValueTarget() - { - moveit_msgs::RobotState msg; - const robot_state::RobotState state = moveit::planning_interface::MoveItCpp::getTargetRobotState(); - moveit::core::robotStateToRobotStateMsg(state, msg); - return py_bindings_tools::serializeMsg(msg); - } - - void rememberJointValuesFromPythonList(const std::string& string, bp::list& values) - { - rememberJointValues(string, py_bindings_tools::doubleFromList(values)); - } - - const char* getPlanningFrameCStr() const - { - return getPlanningFrame().c_str(); - } - - std::string getInterfaceDescriptionPython() - { - moveit_msgs::PlannerInterfaceDescription msg; - getInterfaceDescription(msg); - return py_bindings_tools::serializeMsg(msg); - } - - bp::list getActiveJointsList() const - { - return py_bindings_tools::listFromString(getActiveJoints()); - } - - bp::list getJointsList() const - { - return py_bindings_tools::listFromString(getJoints()); - } - - bp::list getCurrentJointValuesList() - { - return py_bindings_tools::listFromDouble(getCurrentJointValues()); - } - - bp::list getRandomJointValuesList() - { - return py_bindings_tools::listFromDouble(getRandomJointValues()); - } - - bp::dict getRememberedJointValuesPython() const - { - const std::map>& rv = getRememberedJointValues(); - bp::dict d; - for (const std::pair>& it : rv) - d[it.first] = py_bindings_tools::listFromDouble(it.second); - return d; - } - - bp::list convertPoseToList(const geometry_msgs::Pose& pose) const - { - std::vector v(7); - v[0] = pose.position.x; - v[1] = pose.position.y; - v[2] = pose.position.z; - v[3] = pose.orientation.x; - v[4] = pose.orientation.y; - v[5] = pose.orientation.z; - v[6] = pose.orientation.w; - return moveit::py_bindings_tools::listFromDouble(v); - } - - bp::list convertTransformToList(const geometry_msgs::Transform& tr) const - { - std::vector v(7); - v[0] = tr.translation.x; - v[1] = tr.translation.y; - v[2] = tr.translation.z; - v[3] = tr.rotation.x; - v[4] = tr.rotation.y; - v[5] = tr.rotation.z; - v[6] = tr.rotation.w; - return py_bindings_tools::listFromDouble(v); - } - - void convertListToTransform(const bp::list& l, geometry_msgs::Transform& tr) const - { - std::vector v = py_bindings_tools::doubleFromList(l); - tr.translation.x = v[0]; - tr.translation.y = v[1]; - tr.translation.z = v[2]; - tr.rotation.x = v[3]; - tr.rotation.y = v[4]; - tr.rotation.z = v[5]; - tr.rotation.w = v[6]; - } - - void convertListToPose(const bp::list& l, geometry_msgs::Pose& p) const - { - std::vector v = py_bindings_tools::doubleFromList(l); - p.position.x = v[0]; - p.position.y = v[1]; - p.position.z = v[2]; - p.orientation.x = v[3]; - p.orientation.y = v[4]; - p.orientation.z = v[5]; - p.orientation.w = v[6]; - } - - bp::list getCurrentRPYPython(const std::string& end_effector_link = "") - { - return py_bindings_tools::listFromDouble(getCurrentRPY(end_effector_link)); - } - - bp::list getCurrentPosePython(const std::string& end_effector_link = "") - { - geometry_msgs::PoseStamped pose = getCurrentPose(end_effector_link); - return convertPoseToList(pose.pose); - } - - bp::list getRandomPosePython(const std::string& end_effector_link = "") - { - geometry_msgs::PoseStamped pose = getRandomPose(end_effector_link); - return convertPoseToList(pose.pose); - } - - bp::list getKnownConstraintsList() const - { - return py_bindings_tools::listFromString(getKnownConstraints()); - } - - bool placePose(const std::string& object_name, const bp::list& pose, bool plan_only = false) - { - geometry_msgs::PoseStamped msg; - convertListToPose(pose, msg.pose); - msg.header.frame_id = getPoseReferenceFrame(); - msg.header.stamp = ros::Time::now(); - return place(object_name, msg, plan_only) == MoveItErrorCode::SUCCESS; - } - - bool placeLocation(const std::string& object_name, const std::string& location_str, bool plan_only = false) - { - std::vector locations(1); - py_bindings_tools::deserializeMsg(location_str, locations[0]); - return place(object_name, locations, plan_only) == MoveItErrorCode::SUCCESS; - } - - bool placeAnywhere(const std::string& object_name, bool plan_only = false) - { - return place(object_name, plan_only) == MoveItErrorCode::SUCCESS; - } - - void convertListToArrayOfPoses(const bp::list& poses, std::vector& msg) - { - int l = bp::len(poses); - for (int i = 0; i < l; ++i) - { - const bp::list& pose = bp::extract(poses[i]); - std::vector v = py_bindings_tools::doubleFromList(pose); - if (v.size() == 6 || v.size() == 7) - { - Eigen::Isometry3d p; - if (v.size() == 6) - { - tf2::Quaternion tq; - tq.setRPY(v[3], v[4], v[5]); - Eigen::Quaterniond eq; - tf2::convert(tq, eq); - p = Eigen::Isometry3d(eq); - } - else - p = Eigen::Isometry3d(Eigen::Quaterniond(v[6], v[3], v[4], v[5])); - p.translation() = Eigen::Vector3d(v[0], v[1], v[2]); - geometry_msgs::Pose pm = tf2::toMsg(p); - msg.push_back(pm); - } - else - ROS_WARN("Incorrect number of values for a pose: %u", (unsigned int)v.size()); - } - } - - bp::dict getCurrentStateBoundedPython() - { - robot_state::RobotStatePtr current = getCurrentState(); - current->enforceBounds(); - moveit_msgs::RobotState rsmv; - robot_state::robotStateToRobotStateMsg(*current, rsmv); - bp::dict output; - for (size_t x = 0; x < rsmv.joint_state.name.size(); ++x) - output[rsmv.joint_state.name[x]] = rsmv.joint_state.position[x]; - return output; - } - - void setStartStatePython(const std::string& msg_str) - { - moveit_msgs::RobotState msg; - py_bindings_tools::deserializeMsg(msg_str, msg); - setStartState(msg); - } - - bool setPoseTargetsPython(bp::list& poses, const std::string& end_effector_link = "") - { - std::vector msg; - convertListToArrayOfPoses(poses, msg); - return setPoseTargets(msg, end_effector_link); - } - std::string getPoseTargetPython(const std::string& end_effector_link) - { - geometry_msgs::PoseStamped pose = moveit::planning_interface::MoveItCpp::getPoseTarget(end_effector_link); - return py_bindings_tools::serializeMsg(pose); - } - - bool setPoseTargetPython(bp::list& pose, const std::string& end_effector_link = "") - { - std::vector v = py_bindings_tools::doubleFromList(pose); - geometry_msgs::Pose msg; - if (v.size() == 6) - { - tf2::Quaternion q; - q.setRPY(v[3], v[4], v[5]); - tf2::convert(q, msg.orientation); - } - else if (v.size() == 7) - { - msg.orientation.x = v[3]; - msg.orientation.y = v[4]; - msg.orientation.z = v[5]; - msg.orientation.w = v[6]; - } - else - { - ROS_ERROR("Pose description expected to consist of either 6 or 7 values"); - return false; - } - msg.position.x = v[0]; - msg.position.y = v[1]; - msg.position.z = v[2]; - return setPoseTarget(msg, end_effector_link); - } - - const char* getEndEffectorLinkCStr() const - { - return getEndEffectorLink().c_str(); - } - - const char* getPoseReferenceFrameCStr() const - { - return getPoseReferenceFrame().c_str(); - } - - const char* getNameCStr() const - { - return getName().c_str(); - } - - bp::dict getNamedTargetValuesPython(const std::string& name) - { - bp::dict output; - std::map positions = getNamedTargetValues(name); - std::map::iterator iterator; - - for (iterator = positions.begin(); iterator != positions.end(); ++iterator) - output[iterator->first] = iterator->second; - return output; - } - - bp::list getNamedTargetsPython() - { - return py_bindings_tools::listFromString(getNamedTargets()); - } - - bool movePython() - { - return move() == MoveItErrorCode::SUCCESS; - } - - bool asyncMovePython() - { - return asyncMove() == MoveItErrorCode::SUCCESS; - } - - bool attachObjectPython(const std::string& object_name, const std::string& link_name, const bp::list& touch_links) - { - return attachObject(object_name, link_name, py_bindings_tools::stringFromList(touch_links)); - } - - bool executePython(const std::string& plan_str) - { - MoveItCpp::Plan plan; - py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); - return execute(plan) == MoveItErrorCode::SUCCESS; - } - - bool asyncExecutePython(const std::string& plan_str) - { - MoveItCpp::Plan plan; - py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); - return asyncExecute(plan) == MoveItErrorCode::SUCCESS; - } - - bp::tuple planPython() - { - MoveItCpp::Plan plan; - moveit_msgs::MoveItErrorCodes res = MoveItCpp::plan(plan); - return bp::make_tuple(py_bindings_tools::serializeMsg(res), py_bindings_tools::serializeMsg(plan.trajectory_), - plan.planning_time_); - } - - bp::tuple computeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions) - { - moveit_msgs::Constraints path_constraints_tmp; - return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints_tmp); - } - - bp::tuple computeCartesianPathConstrainedPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions, const std::string& path_constraints_str) - { - moveit_msgs::Constraints path_constraints; - py_bindings_tools::deserializeMsg(path_constraints_str, path_constraints); - return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints); - } - - bp::tuple doComputeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions, const moveit_msgs::Constraints& path_constraints) - { - std::vector poses; - convertListToArrayOfPoses(waypoints, poses); - moveit_msgs::RobotTrajectory trajectory; - double fraction = - computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions); - return bp::make_tuple(py_bindings_tools::serializeMsg(trajectory), fraction); - } - - int pickGrasp(const std::string& object, const std::string& grasp_str, bool plan_only = false) - { - moveit_msgs::Grasp grasp; - py_bindings_tools::deserializeMsg(grasp_str, grasp); - return pick(object, grasp, plan_only).val; - } - - int pickGrasps(const std::string& object, const bp::list& grasp_list, bool plan_only = false) - { - int l = bp::len(grasp_list); - std::vector grasps(l); - for (int i = 0; i < l; ++i) - py_bindings_tools::deserializeMsg(bp::extract(grasp_list[i]), grasps[i]); - return pick(object, grasps, plan_only).val; - } - - void setPathConstraintsFromMsg(const std::string& constraints_str) - { - moveit_msgs::Constraints constraints_msg; - py_bindings_tools::deserializeMsg(constraints_str, constraints_msg); - setPathConstraints(constraints_msg); - } - - std::string getPathConstraintsPython() - { - moveit_msgs::Constraints constraints_msg(getPathConstraints()); - std::string constraints_str = py_bindings_tools::serializeMsg(constraints_msg); - return constraints_str; - } - - std::string retimeTrajectory(const std::string& ref_state_str, const std::string& traj_str, - double velocity_scaling_factor) - { - // Convert reference state message to object - moveit_msgs::RobotState ref_state_msg; - py_bindings_tools::deserializeMsg(ref_state_str, ref_state_msg); - moveit::core::RobotState ref_state_obj(getRobotModel()); - if (moveit::core::robotStateMsgToRobotState(ref_state_msg, ref_state_obj, true)) - { - // Convert trajectory message to object - moveit_msgs::RobotTrajectory traj_msg; - py_bindings_tools::deserializeMsg(traj_str, traj_msg); - robot_trajectory::RobotTrajectory traj_obj(getRobotModel(), getName()); - traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg); - - // Do the actual retiming - trajectory_processing::IterativeParabolicTimeParameterization time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor); - - // Convert the retimed trajectory back into a message - traj_obj.getRobotTrajectoryMsg(traj_msg); - std::string traj_str = py_bindings_tools::serializeMsg(traj_msg); - - // Return it. - return traj_str; - } - else - { - ROS_ERROR("Unable to convert RobotState message to RobotState instance."); - return ""; - } - } -}; - -static void wrap_moveit_cpp() -{ - bp::class_ moveit_cpp_class( - "MoveItCpp", bp::init>()); - - moveit_cpp_class.def("async_move", &MoveItCppWrapper::asyncMovePython); - moveit_cpp_class.def("move", &MoveItCppWrapper::movePython); - moveit_cpp_class.def("execute", &MoveItCppWrapper::executePython); - moveit_cpp_class.def("async_execute", &MoveItCppWrapper::asyncExecutePython); - moveit::planning_interface::MoveItErrorCode (MoveItCppWrapper::*pick_1)(const std::string&, bool) = - &MoveItCppWrapper::pick; - moveit_cpp_class.def("pick", pick_1); - moveit_cpp_class.def("pick", &MoveItCppWrapper::pickGrasp); - moveit_cpp_class.def("pick", &MoveItCppWrapper::pickGrasps); - moveit_cpp_class.def("place", &MoveItCppWrapper::placePose); - moveit_cpp_class.def("place", &MoveItCppWrapper::placeLocation); - moveit_cpp_class.def("place", &MoveItCppWrapper::placeAnywhere); - moveit_cpp_class.def("stop", &MoveItCppWrapper::stop); - - moveit_cpp_class.def("get_name", &MoveItCppWrapper::getNameCStr); - moveit_cpp_class.def("get_planning_frame", &MoveItCppWrapper::getPlanningFrameCStr); - moveit_cpp_class.def("get_interface_description", &MoveItCppWrapper::getInterfaceDescriptionPython); - - moveit_cpp_class.def("get_active_joints", &MoveItCppWrapper::getActiveJointsList); - moveit_cpp_class.def("get_joints", &MoveItCppWrapper::getJointsList); - moveit_cpp_class.def("get_variable_count", &MoveItCppWrapper::getVariableCount); - moveit_cpp_class.def("allow_looking", &MoveItCppWrapper::allowLooking); - moveit_cpp_class.def("allow_replanning", &MoveItCppWrapper::allowReplanning); - - moveit_cpp_class.def("set_pose_reference_frame", &MoveItCppWrapper::setPoseReferenceFrame); - - moveit_cpp_class.def("set_pose_reference_frame", &MoveItCppWrapper::setPoseReferenceFrame); - moveit_cpp_class.def("set_end_effector_link", &MoveItCppWrapper::setEndEffectorLink); - moveit_cpp_class.def("get_end_effector_link", &MoveItCppWrapper::getEndEffectorLinkCStr); - moveit_cpp_class.def("get_pose_reference_frame", &MoveItCppWrapper::getPoseReferenceFrameCStr); - - moveit_cpp_class.def("set_pose_target", &MoveItCppWrapper::setPoseTargetPython); - - moveit_cpp_class.def("set_pose_targets", &MoveItCppWrapper::setPoseTargetsPython); - - moveit_cpp_class.def("set_position_target", &MoveItCppWrapper::setPositionTarget); - moveit_cpp_class.def("set_rpy_target", &MoveItCppWrapper::setRPYTarget); - moveit_cpp_class.def("set_orientation_target", &MoveItCppWrapper::setOrientationTarget); - - moveit_cpp_class.def("get_current_pose", &MoveItCppWrapper::getCurrentPosePython); - moveit_cpp_class.def("get_current_rpy", &MoveItCppWrapper::getCurrentRPYPython); - - moveit_cpp_class.def("get_random_pose", &MoveItCppWrapper::getRandomPosePython); - - moveit_cpp_class.def("clear_pose_target", &MoveItCppWrapper::clearPoseTarget); - moveit_cpp_class.def("clear_pose_targets", &MoveItCppWrapper::clearPoseTargets); - - moveit_cpp_class.def("set_joint_value_target", &MoveItCppWrapper::setJointValueTargetPythonIterable); - moveit_cpp_class.def("set_joint_value_target", &MoveItCppWrapper::setJointValueTargetPythonDict); - - moveit_cpp_class.def("set_joint_value_target", &MoveItCppWrapper::setJointValueTargetPerJointPythonList); - bool (MoveItCppWrapper::*set_joint_value_target_4)(const std::string&, double) = - &MoveItCppWrapper::setJointValueTarget; - moveit_cpp_class.def("set_joint_value_target", set_joint_value_target_4); - - moveit_cpp_class.def("set_joint_value_target_from_pose", &MoveItCppWrapper::setJointValueTargetFromPosePython); - moveit_cpp_class.def("set_joint_value_target_from_pose_stamped", - &MoveItCppWrapper::setJointValueTargetFromPoseStampedPython); - moveit_cpp_class.def("set_joint_value_target_from_joint_state_message", - &MoveItCppWrapper::setJointValueTargetFromJointStatePython); - - moveit_cpp_class.def("get_joint_value_target", &MoveItCppWrapper::getJointValueTargetPythonList); - - moveit_cpp_class.def("set_named_target", &MoveItCppWrapper::setNamedTarget); - moveit_cpp_class.def("set_random_target", &MoveItCppWrapper::setRandomTarget); - - void (MoveItCppWrapper::*remember_joint_values_2)(const std::string&) = &MoveItCppWrapper::rememberJointValues; - moveit_cpp_class.def("remember_joint_values", remember_joint_values_2); - - moveit_cpp_class.def("remember_joint_values", &MoveItCppWrapper::rememberJointValuesFromPythonList); - - moveit_cpp_class.def("start_state_monitor", &MoveItCppWrapper::startStateMonitor); - moveit_cpp_class.def("get_current_joint_values", &MoveItCppWrapper::getCurrentJointValuesList); - moveit_cpp_class.def("get_random_joint_values", &MoveItCppWrapper::getRandomJointValuesList); - moveit_cpp_class.def("get_remembered_joint_values", &MoveItCppWrapper::getRememberedJointValuesPython); - - moveit_cpp_class.def("forget_joint_values", &MoveItCppWrapper::forgetJointValues); - - moveit_cpp_class.def("get_goal_joint_tolerance", &MoveItCppWrapper::getGoalJointTolerance); - moveit_cpp_class.def("get_goal_position_tolerance", &MoveItCppWrapper::getGoalPositionTolerance); - moveit_cpp_class.def("get_goal_orientation_tolerance", &MoveItCppWrapper::getGoalOrientationTolerance); - - moveit_cpp_class.def("set_goal_joint_tolerance", &MoveItCppWrapper::setGoalJointTolerance); - moveit_cpp_class.def("set_goal_position_tolerance", &MoveItCppWrapper::setGoalPositionTolerance); - moveit_cpp_class.def("set_goal_orientation_tolerance", &MoveItCppWrapper::setGoalOrientationTolerance); - moveit_cpp_class.def("set_goal_tolerance", &MoveItCppWrapper::setGoalTolerance); - - moveit_cpp_class.def("set_start_state_to_current_state", &MoveItCppWrapper::setStartStateToCurrentState); - moveit_cpp_class.def("set_start_state", &MoveItCppWrapper::setStartStatePython); - - bool (MoveItCppWrapper::*set_path_constraints_1)(const std::string&) = &MoveItCppWrapper::setPathConstraints; - moveit_cpp_class.def("set_path_constraints", set_path_constraints_1); - moveit_cpp_class.def("set_path_constraints_from_msg", &MoveItCppWrapper::setPathConstraintsFromMsg); - moveit_cpp_class.def("get_path_constraints", &MoveItCppWrapper::getPathConstraintsPython); - moveit_cpp_class.def("clear_path_constraints", &MoveItCppWrapper::clearPathConstraints); - moveit_cpp_class.def("get_known_constraints", &MoveItCppWrapper::getKnownConstraintsList); - moveit_cpp_class.def("set_constraints_database", &MoveItCppWrapper::setConstraintsDatabase); - moveit_cpp_class.def("set_workspace", &MoveItCppWrapper::setWorkspace); - moveit_cpp_class.def("set_planning_time", &MoveItCppWrapper::setPlanningTime); - moveit_cpp_class.def("get_planning_time", &MoveItCppWrapper::getPlanningTime); - moveit_cpp_class.def("set_max_velocity_scaling_factor", &MoveItCppWrapper::setMaxVelocityScalingFactor); - moveit_cpp_class.def("set_max_acceleration_scaling_factor", &MoveItCppWrapper::setMaxAccelerationScalingFactor); - moveit_cpp_class.def("set_planner_id", &MoveItCppWrapper::setPlannerId); - moveit_cpp_class.def("set_num_planning_attempts", &MoveItCppWrapper::setNumPlanningAttempts); - moveit_cpp_class.def("plan", &MoveItCppWrapper::planPython); - moveit_cpp_class.def("compute_cartesian_path", &MoveItCppWrapper::computeCartesianPathPython); - moveit_cpp_class.def("compute_cartesian_path", &MoveItCppWrapper::computeCartesianPathConstrainedPython); - moveit_cpp_class.def("set_support_surface_name", &MoveItCppWrapper::setSupportSurfaceName); - moveit_cpp_class.def("attach_object", &MoveItCppWrapper::attachObjectPython); - moveit_cpp_class.def("detach_object", &MoveItCppWrapper::detachObject); - moveit_cpp_class.def("retime_trajectory", &MoveItCppWrapper::retimeTrajectory); - moveit_cpp_class.def("get_named_targets", &MoveItCppWrapper::getNamedTargetsPython); - moveit_cpp_class.def("get_named_target_values", &MoveItCppWrapper::getNamedTargetValuesPython); - moveit_cpp_class.def("get_current_state_bounded", &MoveItCppWrapper::getCurrentStateBoundedPython); -} -} // namespace planning_interface -} // namespace moveit - -BOOST_PYTHON_MODULE(_moveit_moveit_cpp) -{ - using namespace moveit::planning_interface; - wrap_moveit_cpp(); -} - -/** @endcond */ diff --git a/moveit_ros/planning_interface/test/python_moveit_cpp.py b/moveit_ros/planning_interface/test/python_moveit_cpp.py deleted file mode 100755 index 9629851ba9..0000000000 --- a/moveit_ros/planning_interface/test/python_moveit_cpp.py +++ /dev/null @@ -1,76 +0,0 @@ -#!/usr/bin/env python - -import unittest -import numpy as np -import rospy -import rostest -import os - -from moveit_ros_planning_interface._moveit_moveit_cpp_interface import MoveItCpp -from moveit_msgs.msg import MoveItErrorCodes - - -class PythonMoveItCppTest(unittest.TestCase): - PLANNING_GROUP = "manipulator" - - @classmethod - def setUpClass(self): - self.group = MoveItCpp(self.PLANNING_GROUP, "robot_description", rospy.get_namespace()) - - @classmethod - def tearDown(self): - pass - - def check_target_setting(self, expect, *args): - if len(args) == 0: - args = [expect] - self.group.set_joint_value_target(*args) - res = self.group.get_joint_value_target() - self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)), - "Setting failed for %s, values: %s" % (type(args[0]), res)) - - def test_target_setting(self): - n = self.group.get_variable_count() - self.check_target_setting([0.1] * n) - self.check_target_setting((0.2,) * n) - self.check_target_setting(np.zeros(n)) - self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}) - self.check_target_setting([0.5] + [0.3]*(n-1), "joint_1", 0.5) - - def plan(self, target): - self.group.set_joint_value_target(target) - return self.group.plan() - - def test_validation(self): - current = np.asarray(self.group.get_current_joint_values()) - - error_code1, plan1, time = self.plan(current + 0.2) - error_code2, plan2, time = self.plan(current + 0.2) - - # both plans should have succeeded: - error_code = MoveItErrorCodes() - error_code.deserialize(error_code1) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code2) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - - # first plan should execute - self.assertTrue(self.group.execute(plan1)) - - # second plan should be invalid now (due to modified start point) and rejected - self.assertFalse(self.group.execute(plan2)) - - # newly planned trajectory should execute again - error_code3, plan3, time = self.plan(current) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code3) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - self.assertTrue(self.group.execute(plan3)) - - -if __name__ == '__main__': - PKGNAME = 'moveit_ros_planning_interface' - NODENAME = 'moveit_test_python_moveit_cpp' - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, PythonMoveItCppTest) diff --git a/moveit_ros/planning_interface/test/python_moveit_cpp.test b/moveit_ros/planning_interface/test/python_moveit_cpp.test deleted file mode 100644 index 9dc5afe28f..0000000000 --- a/moveit_ros/planning_interface/test/python_moveit_cpp.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - diff --git a/moveit_ros/planning_interface/test/python_moveit_cpp_ns.py b/moveit_ros/planning_interface/test/python_moveit_cpp_ns.py deleted file mode 100755 index 3e431012fe..0000000000 --- a/moveit_ros/planning_interface/test/python_moveit_cpp_ns.py +++ /dev/null @@ -1,114 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: William Baker -# -# This test is used to ensure planning with a MoveGroupInterface is -# possbile if the robot's move_group node is in a different namespace - -import unittest -import numpy as np -import rospy -import rostest -import os - -from moveit_ros_planning_interface._moveit_moveit_cpp_interface import MoveItCpp -from moveit_msgs.msg import MoveItErrorCodes - - -class PythonMoveItCppNsTest(unittest.TestCase): - PLANNING_GROUP = "manipulator" - PLANNING_NS = "test_ns/" - - @classmethod - def setUpClass(self): - self.group = MoveItCpp(self.PLANNING_GROUP, "%srobot_description" % self.PLANNING_NS, self.PLANNING_NS) - - @classmethod - def tearDown(self): - pass - - def check_target_setting(self, expect, *args): - if len(args) == 0: - args = [expect] - self.group.set_joint_value_target(*args) - res = self.group.get_joint_value_target() - self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)), - "Setting failed for %s, values: %s" % (type(args[0]), res)) - - def test_target_setting(self): - n = self.group.get_variable_count() - self.check_target_setting([0.1] * n) - self.check_target_setting((0.2,) * n) - self.check_target_setting(np.zeros(n)) - self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}) - self.check_target_setting([0.5] + [0.3]*(n-1), "joint_1", 0.5) - - def plan(self, target): - self.group.set_joint_value_target(target) - return self.group.plan() - - def test_validation(self): - current = np.asarray(self.group.get_current_joint_values()) - - error_code1, plan1, time = self.plan(current + 0.2) - error_code2, plan2, time = self.plan(current + 0.2) - - # both plans should have succeeded: - error_code = MoveItErrorCodes() - error_code.deserialize(error_code1) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code2) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - - # first plan should execute - self.assertTrue(self.group.execute(plan1)) - - # second plan should be invalid now (due to modified start point) and rejected - self.assertFalse(self.group.execute(plan2)) - - # newly planned trajectory should execute again - error_code3, plan3, time = self.plan(current) - self.assertTrue(self.group.execute(plan3)) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code3) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - - -if __name__ == '__main__': - PKGNAME = 'moveit_ros_planning_interface' - NODENAME = 'moveit_test_python_moveit_cpp' - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, PythonMoveItCppNsTest) diff --git a/moveit_ros/planning_interface/test/python_moveit_cpp_ns.test b/moveit_ros/planning_interface/test/python_moveit_cpp_ns.test deleted file mode 100644 index 188ab6632a..0000000000 --- a/moveit_ros/planning_interface/test/python_moveit_cpp_ns.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - From a2b5a399c27d031f1c5fced838c0f7f1843265e2 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 16 Aug 2019 19:48:55 +0200 Subject: [PATCH 08/29] Rename moveit_cpp.cpp to moveit.cpp --- moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt | 2 +- .../moveit_cpp/src/{moveit_cpp.cpp => moveit.cpp} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename moveit_ros/planning_interface/moveit_cpp/src/{moveit_cpp.cpp => moveit.cpp} (100%) diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt index fd86913dc2..4f3b77e45e 100644 --- a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -1,6 +1,6 @@ set(MOVEIT_LIB_NAME moveit_cpp) -add_library(${MOVEIT_LIB_NAME} src/moveit_cpp.cpp) +add_library(${MOVEIT_LIB_NAME} src/moveit.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp similarity index 100% rename from moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp rename to moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp From 0a9dd92fa012c662845d2d87829d59935bc0c1c9 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 16 Aug 2019 20:15:28 +0200 Subject: [PATCH 09/29] Remove previous authors --- moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp index ebd310a571..97ca186228 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp @@ -2,9 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2019, PickNik Robotics - * Copyright (c) 2014, SRI International - * Copyright (c) 2013, Ioan A. Sucan - * Copyright (c) 2012, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -17,7 +14,7 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * Neither the name of Willow Garage nor the names of its + * * Neither the name of PickNik Robotics nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -35,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan, Sachin Chitta, Simon Goldstien */ +/* Author: Simon Goldstien, Henning Kayser */ #include #include From 38885161c8d6c517da1e26e33dfa300cf35f19a3 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 16 Aug 2019 20:15:53 +0200 Subject: [PATCH 10/29] Remove non-functional demo --- .../moveit_cpp/CMakeLists.txt | 3 - .../moveit_cpp/src/demo.cpp | 121 ------------------ 2 files changed, 124 deletions(-) delete mode 100644 moveit_ros/planning_interface/moveit_cpp/src/demo.cpp diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt index 4f3b77e45e..7e1e422dd2 100644 --- a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -10,6 +10,3 @@ install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) - -#add_executable(demo src/demo.cpp) -#target_link_libraries(demo ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) diff --git a/moveit_ros/planning_interface/moveit_cpp/src/demo.cpp b/moveit_ros/planning_interface/moveit_cpp/src/demo.cpp deleted file mode 100644 index 30ce1f221a..0000000000 --- a/moveit_ros/planning_interface/moveit_cpp/src/demo.cpp +++ /dev/null @@ -1,121 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include - -void demoPick(moveit::planning_interface::MoveItCpp& group) -{ - std::vector grasps; - for (std::size_t i = 0; i < 20; ++i) - { - geometry_msgs::PoseStamped p = group.getRandomPose(); - p.pose.orientation.x = 0; - p.pose.orientation.y = 0; - p.pose.orientation.z = 0; - p.pose.orientation.w = 1; - moveit_msgs::Grasp g; - g.grasp_pose = p; - g.pre_grasp_approach.direction.vector.x = 1.0; - g.post_grasp_retreat.direction.vector.z = 1.0; - g.post_grasp_retreat.direction.header = p.header; - g.pre_grasp_approach.min_distance = 0.2; - g.pre_grasp_approach.desired_distance = 0.4; - g.post_grasp_retreat.min_distance = 0.1; - g.post_grasp_retreat.desired_distance = 0.27; - g.pre_grasp_posture.joint_names.resize(1, "r_gripper_joint"); - g.pre_grasp_posture.points.resize(1); - g.pre_grasp_posture.points[0].positions.resize(1); - g.pre_grasp_posture.points[0].positions[0] = 1; - - g.grasp_posture.joint_names.resize(1, "r_gripper_joint"); - g.grasp_posture.points.resize(1); - g.grasp_posture.points[0].positions.resize(1); - g.grasp_posture.points[0].positions[0] = 0; - - grasps.push_back(g); - } - group.pick("bubu", grasps); -} - -void demoPlace(moveit::planning_interface::MoveItCpp& group) -{ - std::vector loc; - for (std::size_t i = 0; i < 20; ++i) - { - geometry_msgs::PoseStamped p = group.getRandomPose(); - p.pose.orientation.x = 0; - p.pose.orientation.y = 0; - p.pose.orientation.z = 0; - p.pose.orientation.w = 1; - moveit_msgs::PlaceLocation g; - g.place_pose = p; - g.pre_place_approach.direction.vector.x = 1.0; - g.post_place_retreat.direction.vector.z = 1.0; - g.post_place_retreat.direction.header = p.header; - g.pre_place_approach.min_distance = 0.2; - g.pre_place_approach.desired_distance = 0.4; - g.post_place_retreat.min_distance = 0.1; - g.post_place_retreat.desired_distance = 0.27; - - g.post_place_posture.joint_names.resize(1, "r_gripper_joint"); - g.post_place_posture.points.resize(1); - g.post_place_posture.points[0].positions.resize(1); - g.post_place_posture.points[0].positions[0] = 0; - - loc.push_back(g); - } - group.place("bubu", loc); -} - -void attachObject() -{ -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "moveit_cpp_demo", ros::init_options::AnonymousName); - - ros::AsyncSpinner spinner(1); - spinner.start(); - - moveit::planning_interface::MoveItCpp group(argc > 1 ? argv[1] : "right_arm"); - demoPlace(group); - - sleep(2); - - return 0; -} From 8e0946c35f32d8cdd1774f0e4eea12112770cdd1 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 16 Aug 2019 20:22:13 +0200 Subject: [PATCH 11/29] Update license headers --- .../moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h | 7 +++---- moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp | 5 +++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 141cb5f462..05c7c95ee5 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -1,8 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2014, SRI International - * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2019, PickNik LLC * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -15,7 +14,7 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * Neither the name of Willow Garage nor the names of its + * * Neither the name of PickNik LLC nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan, Sachin Chitta, simon Goldstein */ +/* Author: Simon Goldstein, Henning Kayser */ #ifndef MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ #define MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp index 97ca186228..68eadc215a 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2019, PickNik Robotics + * Copyright (c) 2019, PickNik LLC * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +14,7 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * Neither the name of PickNik Robotics nor the names of its + * * Neither the name of PickNik LLC nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * @@ -32,6 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ + /* Author: Simon Goldstien, Henning Kayser */ #include From 136dc5f43b8e0bf9f42adec3d829ead0daa1db82 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 16 Aug 2019 20:23:45 +0200 Subject: [PATCH 12/29] Remove ! from MoveIt --- .../moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 05c7c95ee5..c5f82b7b19 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -94,7 +94,7 @@ enum ActiveTargetType namespace moveit { -/** \brief Simple interface to MoveIt! components */ +/** \brief Simple interface to MoveIt components */ namespace planning_interface { class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes From f7d213e1091d481d0ef3bf844b309425d752489c Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 16 Aug 2019 20:38:13 +0200 Subject: [PATCH 13/29] Add TODOs --- .../moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index c5f82b7b19..55b095ee4c 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -97,6 +97,7 @@ namespace moveit /** \brief Simple interface to MoveIt components */ namespace planning_interface { +//TODO(henningkayser): Move to own header and share with MoveGroup class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes { public: @@ -950,6 +951,7 @@ class MoveItCpp */ /**@{*/ + //TODO(henningkayser): Remove this for now /** \brief Specify where the database server that holds known constraints resides */ void setConstraintsDatabase(const std::string& host, unsigned int port); @@ -987,6 +989,7 @@ class MoveItCpp private: std::map > remembered_joint_values_; + // TODO(henningkayser): Refactor class and possibly remove impl concept class MoveItCppImpl; MoveItCppImpl* impl_; From 6a4d6f645dbada2a6c9f199d30ab2d3520b3b1b3 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 16 Aug 2019 20:43:14 +0200 Subject: [PATCH 14/29] Wrap moveit_cpp function definitions in namespace --- .../moveit_cpp/src/moveit.cpp | 279 +++++++++--------- 1 file changed, 140 insertions(+), 139 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp index 68eadc215a..d62868d068 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp @@ -71,20 +71,19 @@ namespace moveit { namespace planning_interface { -const std::string MoveItCpp::ROBOT_DESCRIPTION = - "robot_description"; // name of the robot description (a param name, so it can be changed externally) +/// Name of the robot description (a param name, so it can be changed externally) +const std::string MoveItCpp::ROBOT_DESCRIPTION = "robot_description"; const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps -} // planning_interface -} // moveit -moveit::planning_interface::MoveItCpp::MoveItCpp(const std::string& group_name, + +MoveItCpp::MoveItCpp(const std::string& group_name, const std::shared_ptr& tf_buffer, const ros::WallDuration& wait_for_servers) : MoveItCpp(Options(group_name), tf_buffer, wait_for_servers) { } -moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer, +MoveItCpp::MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer, const ros::WallDuration& wait_for_servers) : node_handle_(opt.node_handle_), tf_buffer_(tf_buffer) { @@ -172,17 +171,17 @@ moveit::planning_interface::MoveItCpp::MoveItCpp(const Options& opt, const std:: robot_description_ = opt.robot_description_; } -moveit::planning_interface::MoveItCpp::~MoveItCpp() +MoveItCpp::~MoveItCpp() { clearContents(); } -moveit::planning_interface::MoveItCpp::MoveItCpp(MoveItCpp&& other) +MoveItCpp::MoveItCpp(MoveItCpp&& other) { other.clearContents(); } -moveit::planning_interface::MoveItCpp& moveit::planning_interface::MoveItCpp::operator=(MoveItCpp&& other) +MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) { if (this != &other) { @@ -241,12 +240,12 @@ moveit::planning_interface::MoveItCpp& moveit::planning_interface::MoveItCpp::op return *this; } -const std::string& moveit::planning_interface::MoveItCpp::getName() const +const std::string& MoveItCpp::getName() const { return group_name_; } -const std::vector moveit::planning_interface::MoveItCpp::getNamedTargets() +const std::vector MoveItCpp::getNamedTargets() { const robot_model::RobotModelConstPtr& robot = robot_model_; const std::string& group = group_name_; @@ -261,17 +260,17 @@ const std::vector moveit::planning_interface::MoveItCpp::getNamedTa return empty; } -robot_model::RobotModelConstPtr moveit::planning_interface::MoveItCpp::getRobotModel() const +robot_model::RobotModelConstPtr MoveItCpp::getRobotModel() const { return robot_model_; } -const ros::NodeHandle& moveit::planning_interface::MoveItCpp::getNodeHandle() const +const ros::NodeHandle& MoveItCpp::getNodeHandle() const { return node_handle_; } -bool moveit::planning_interface::MoveItCpp::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) +bool MoveItCpp::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) { moveit_msgs::QueryPlannerInterfaces::Request req; moveit_msgs::QueryPlannerInterfaces::Response res; @@ -285,7 +284,7 @@ bool moveit::planning_interface::MoveItCpp::getInterfaceDescription(moveit_msgs: } std::map -moveit::planning_interface::MoveItCpp::getPlannerParams(const std::string& planner_id, const std::string& group) +MoveItCpp::getPlannerParams(const std::string& planner_id, const std::string& group) { moveit_msgs::GetPlannerParams::Request req; moveit_msgs::GetPlannerParams::Response res; @@ -300,7 +299,7 @@ moveit::planning_interface::MoveItCpp::getPlannerParams(const std::string& plann return result; } -void moveit::planning_interface::MoveItCpp::setPlannerParams(const std::string& planner_id, const std::string& group, +void MoveItCpp::setPlannerParams(const std::string& planner_id, const std::string& group, const std::map& params, bool replace) { @@ -317,7 +316,7 @@ void moveit::planning_interface::MoveItCpp::setPlannerParams(const std::string& set_params_service_.call(req, res); } -std::string moveit::planning_interface::MoveItCpp::getDefaultPlannerId(const std::string& group) const +std::string MoveItCpp::getDefaultPlannerId(const std::string& group) const { std::stringstream param_name; param_name << "move_group"; @@ -330,58 +329,58 @@ std::string moveit::planning_interface::MoveItCpp::getDefaultPlannerId(const std return default_planner_config; } -void moveit::planning_interface::MoveItCpp::setPlannerId(const std::string& planner_id) +void MoveItCpp::setPlannerId(const std::string& planner_id) { planner_id_ = planner_id; } -const std::string& moveit::planning_interface::MoveItCpp::getPlannerId() const +const std::string& MoveItCpp::getPlannerId() const { return planner_id_; } -void moveit::planning_interface::MoveItCpp::setNumPlanningAttempts(unsigned int num_planning_attempts) +void MoveItCpp::setNumPlanningAttempts(unsigned int num_planning_attempts) { num_planning_attempts_ = num_planning_attempts; } -void moveit::planning_interface::MoveItCpp::setMaxVelocityScalingFactor(double max_velocity_scaling_factor) +void MoveItCpp::setMaxVelocityScalingFactor(double max_velocity_scaling_factor) { max_velocity_scaling_factor_ = max_velocity_scaling_factor; } -void moveit::planning_interface::MoveItCpp::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) +void MoveItCpp::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) { max_acceleration_scaling_factor_ = max_acceleration_scaling_factor; } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::asyncMove() +MoveItErrorCode MoveItCpp::asyncMove() { return move(false); } actionlib::SimpleActionClient& -moveit::planning_interface::MoveItCpp::getMoveGroupClient() const +MoveItCpp::getMoveGroupClient() const { return *move_action_client_; } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::move() +MoveItErrorCode MoveItCpp::move() { return move(true); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::asyncExecute(const Plan& plan) +MoveItErrorCode MoveItCpp::asyncExecute(const Plan& plan) { return execute(plan, false); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::execute(const Plan& plan) +MoveItErrorCode MoveItCpp::execute(const Plan& plan) { return execute(plan, true); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::plan(Plan& plan) +MoveItErrorCode MoveItCpp::plan(Plan& plan) { if (!move_action_client_) { @@ -420,20 +419,20 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp } } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::pick(const std::string& object, +MoveItErrorCode MoveItCpp::pick(const std::string& object, bool plan_only) { return pick(object, std::vector(), plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::pick(const std::string& object, +MoveItErrorCode MoveItCpp::pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only) { return pick(object, std::vector(1, grasp), plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::pick( +MoveItErrorCode MoveItCpp::pick( const std::string& object, const std::vector& grasps, bool plan_only) { if (!pick_action_client_) @@ -473,14 +472,14 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp } } -moveit::planning_interface::MoveItErrorCode -moveit::planning_interface::MoveItCpp::planGraspsAndPick(const std::string& object, bool plan_only) +MoveItErrorCode +MoveItCpp::planGraspsAndPick(const std::string& object, bool plan_only) { if (object.empty()) { return planGraspsAndPick(moveit_msgs::CollisionObject()); } - moveit::planning_interface::PlanningSceneInterface psi; + PlanningSceneInterface psi; std::map objects = psi.getObjects(std::vector(1, object)); @@ -494,8 +493,8 @@ moveit::planning_interface::MoveItCpp::planGraspsAndPick(const std::string& obje return planGraspsAndPick(objects[object], plan_only); } -moveit::planning_interface::MoveItErrorCode -moveit::planning_interface::MoveItCpp::planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only) +MoveItErrorCode +MoveItCpp::planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only) { if (!plan_grasps_service_) { @@ -524,13 +523,13 @@ moveit::planning_interface::MoveItCpp::planGraspsAndPick(const moveit_msgs::Coll return pick(object.id, response.grasps, plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::place(const std::string& object, +MoveItErrorCode MoveItCpp::place(const std::string& object, bool plan_only) { return place(object, std::vector(), plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::place( +MoveItErrorCode MoveItCpp::place( const std::string& object, const std::vector& locations, bool plan_only) { if (!place_action_client_) @@ -571,7 +570,7 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp } } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::place( +MoveItErrorCode MoveItCpp::place( const std::string& object, const std::vector& poses, bool plan_only) { std::vector locations; @@ -597,13 +596,13 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp return place(object, locations, plan_only); } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::place( +MoveItErrorCode MoveItCpp::place( const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only) { return place(object, std::vector(1, pose), plan_only); } -double moveit::planning_interface::MoveItCpp::computeCartesianPath(const std::vector& waypoints, +double MoveItCpp::computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions, @@ -614,7 +613,7 @@ double moveit::planning_interface::MoveItCpp::computeCartesianPath(const std::ve error_code); } -double moveit::planning_interface::MoveItCpp::computeCartesianPath(const std::vector& waypoints, +double MoveItCpp::computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, @@ -660,7 +659,7 @@ double moveit::planning_interface::MoveItCpp::computeCartesianPath(const std::ve } } -void moveit::planning_interface::MoveItCpp::stop() +void MoveItCpp::stop() { if (trajectory_event_publisher_) { @@ -670,7 +669,7 @@ void moveit::planning_interface::MoveItCpp::stop() } } -void moveit::planning_interface::MoveItCpp::setStartState(const moveit_msgs::RobotState& start_state) +void MoveItCpp::setStartState(const moveit_msgs::RobotState& start_state) { robot_state::RobotStatePtr rs; getCurrentState(rs, 1.0); @@ -678,33 +677,33 @@ void moveit::planning_interface::MoveItCpp::setStartState(const moveit_msgs::Rob setStartState(*rs); } -void moveit::planning_interface::MoveItCpp::setStartState(const robot_state::RobotState& start_state) +void MoveItCpp::setStartState(const robot_state::RobotState& start_state) { considered_start_state_.reset(new robot_state::RobotState(start_state)); } -void moveit::planning_interface::MoveItCpp::setStartStateToCurrentState() +void MoveItCpp::setStartStateToCurrentState() { considered_start_state_.reset(); } -void moveit::planning_interface::MoveItCpp::setRandomTarget() +void MoveItCpp::setRandomTarget() { joint_state_target_->setToRandomPositions(); active_target_ = JOINT; } -const std::vector& moveit::planning_interface::MoveItCpp::getJointNames() +const std::vector& MoveItCpp::getJointNames() { return joint_model_group_->getVariableNames(); } -const std::vector& moveit::planning_interface::MoveItCpp::getLinkNames() +const std::vector& MoveItCpp::getLinkNames() { return joint_model_group_->getLinkModelNames(); } -std::map moveit::planning_interface::MoveItCpp::getNamedTargetValues(const std::string& name) +std::map MoveItCpp::getNamedTargetValues(const std::string& name) { std::map >::const_iterator it = remembered_joint_values_.find(name); std::map positions; @@ -724,7 +723,7 @@ std::map moveit::planning_interface::MoveItCpp::getNamedTar return positions; } -bool moveit::planning_interface::MoveItCpp::setNamedTarget(const std::string& name) +bool MoveItCpp::setNamedTarget(const std::string& name) { std::map >::const_iterator it = remembered_joint_values_.find(name); if (it != remembered_joint_values_.end()) @@ -743,12 +742,12 @@ bool moveit::planning_interface::MoveItCpp::setNamedTarget(const std::string& na } } -void moveit::planning_interface::MoveItCpp::getJointValueTarget(std::vector& group_variable_values) const +void MoveItCpp::getJointValueTarget(std::vector& group_variable_values) const { joint_state_target_->copyJointGroupPositions(joint_model_group_, group_variable_values); } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::vector& joint_values) +bool MoveItCpp::setJointValueTarget(const std::vector& joint_values) { if (joint_values.size() != joint_model_group_->getVariableCount()) return false; @@ -757,7 +756,7 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::vecto return joint_state_target_->satisfiesBounds(joint_model_group_, goal_joint_tolerance_); } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::map& variable_values) +bool MoveItCpp::setJointValueTarget(const std::map& variable_values) { const auto& allowed = joint_model_group_->getVariableNames(); for (const auto& pair : variable_values) @@ -774,7 +773,7 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::mapsatisfiesBounds(goal_joint_tolerance_); } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::vector& variable_names, +bool MoveItCpp::setJointValueTarget(const std::vector& variable_names, const std::vector& variable_values) { const auto& allowed = joint_model_group_->getVariableNames(); @@ -792,20 +791,20 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::vecto return joint_state_target_->satisfiesBounds(goal_joint_tolerance_); } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const robot_state::RobotState& rstate) +bool MoveItCpp::setJointValueTarget(const robot_state::RobotState& rstate) { active_target_ = JOINT; *joint_state_target_ = rstate; return joint_state_target_->satisfiesBounds(goal_joint_tolerance_); } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::string& joint_name, double value) +bool MoveItCpp::setJointValueTarget(const std::string& joint_name, double value) { std::vector values(1, value); return setJointValueTarget(joint_name, values); } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::string& joint_name, +bool MoveItCpp::setJointValueTarget(const std::string& joint_name, const std::vector& values) { active_target_ = JOINT; @@ -820,65 +819,65 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const std::strin return false; } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const sensor_msgs::JointState& state) +bool MoveItCpp::setJointValueTarget(const sensor_msgs::JointState& state) { return setJointValueTarget(state.name, state.position); } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_msgs::Pose& eef_pose, +bool MoveItCpp::setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link) { return setJointValueTarget(eef_pose, end_effector_link, "", false); } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, +bool MoveItCpp::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link) { return setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false); } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const Eigen::Isometry3d& eef_pose, +bool MoveItCpp::setJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link) { geometry_msgs::Pose msg = tf2::toMsg(eef_pose); return setJointValueTarget(msg, end_effector_link); } -bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose, +bool MoveItCpp::setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link) { return setJointValueTarget(eef_pose, end_effector_link, "", true); } -bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, +bool MoveItCpp::setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link) { return setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true); } -bool moveit::planning_interface::MoveItCpp::setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose, +bool MoveItCpp::setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link) { geometry_msgs::Pose msg = tf2::toMsg(eef_pose); return setApproximateJointValueTarget(msg, end_effector_link); } -const robot_state::RobotState& moveit::planning_interface::MoveItCpp::getJointValueTarget() const +const robot_state::RobotState& MoveItCpp::getJointValueTarget() const { return *joint_state_target_; } -const robot_state::RobotState& moveit::planning_interface::MoveItCpp::getTargetRobotState() const +const robot_state::RobotState& MoveItCpp::getTargetRobotState() const { return *joint_state_target_; } -const std::string& moveit::planning_interface::MoveItCpp::getEndEffectorLink() const +const std::string& MoveItCpp::getEndEffectorLink() const { return end_effector_link_; } -const std::string& moveit::planning_interface::MoveItCpp::getEndEffector() const +const std::string& MoveItCpp::getEndEffector() const { if (!end_effector_link_.empty()) { @@ -892,7 +891,7 @@ const std::string& moveit::planning_interface::MoveItCpp::getEndEffector() const return empty; } -bool moveit::planning_interface::MoveItCpp::setEndEffectorLink(const std::string& link_name) +bool MoveItCpp::setEndEffectorLink(const std::string& link_name) { if (end_effector_link_.empty() || link_name.empty()) return false; @@ -901,7 +900,7 @@ bool moveit::planning_interface::MoveItCpp::setEndEffectorLink(const std::string return true; } -bool moveit::planning_interface::MoveItCpp::setEndEffector(const std::string& eef_name) +bool MoveItCpp::setEndEffector(const std::string& eef_name) { const robot_model::JointModelGroup* jmg = robot_model_->getEndEffector(eef_name); if (jmg) @@ -909,17 +908,17 @@ bool moveit::planning_interface::MoveItCpp::setEndEffector(const std::string& ee return false; } -void moveit::planning_interface::MoveItCpp::clearPoseTarget(const std::string& end_effector_link) +void MoveItCpp::clearPoseTarget(const std::string& end_effector_link) { pose_targets_.erase(end_effector_link); } -void moveit::planning_interface::MoveItCpp::clearPoseTargets() +void MoveItCpp::clearPoseTargets() { pose_targets_.clear(); } -bool moveit::planning_interface::MoveItCpp::setPoseTarget(const Eigen::Isometry3d& pose, +bool MoveItCpp::setPoseTarget(const Eigen::Isometry3d& pose, const std::string& end_effector_link) { std::vector pose_msg(1); @@ -929,7 +928,7 @@ bool moveit::planning_interface::MoveItCpp::setPoseTarget(const Eigen::Isometry3 return setPoseTargets(pose_msg, end_effector_link); } -bool moveit::planning_interface::MoveItCpp::setPoseTarget(const geometry_msgs::Pose& target, +bool MoveItCpp::setPoseTarget(const geometry_msgs::Pose& target, const std::string& end_effector_link) { std::vector pose_msg(1); @@ -939,14 +938,14 @@ bool moveit::planning_interface::MoveItCpp::setPoseTarget(const geometry_msgs::P return setPoseTargets(pose_msg, end_effector_link); } -bool moveit::planning_interface::MoveItCpp::setPoseTarget(const geometry_msgs::PoseStamped& target, +bool MoveItCpp::setPoseTarget(const geometry_msgs::PoseStamped& target, const std::string& end_effector_link) { std::vector targets(1, target); return setPoseTargets(targets, end_effector_link); } -bool moveit::planning_interface::MoveItCpp::setPoseTargets(const EigenSTL::vector_Isometry3d& target, +bool MoveItCpp::setPoseTargets(const EigenSTL::vector_Isometry3d& target, const std::string& end_effector_link) { std::vector pose_out(target.size()); @@ -961,7 +960,7 @@ bool moveit::planning_interface::MoveItCpp::setPoseTargets(const EigenSTL::vecto return setPoseTargets(pose_out, end_effector_link); } -bool moveit::planning_interface::MoveItCpp::setPoseTargets(const std::vector& target, +bool MoveItCpp::setPoseTargets(const std::vector& target, const std::string& end_effector_link) { std::vector target_stamped(target.size()); @@ -976,7 +975,7 @@ bool moveit::planning_interface::MoveItCpp::setPoseTargets(const std::vector& target, +bool MoveItCpp::setPoseTargets(const std::vector& target, const std::string& end_effector_link) { if (target.empty()) @@ -1006,7 +1005,7 @@ bool moveit::planning_interface::MoveItCpp::setPoseTargets(const std::vector& -moveit::planning_interface::MoveItCpp::getPoseTargets(const std::string& end_effector_link) const +MoveItCpp::getPoseTargets(const std::string& end_effector_link) const { const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; @@ -1053,7 +1052,7 @@ inline void transformPose(const tf2_ros::Buffer& tf_buffer, const std::string& d } } // namespace -bool moveit::planning_interface::MoveItCpp::setPositionTarget(double x, double y, double z, +bool MoveItCpp::setPositionTarget(double x, double y, double z, const std::string& end_effector_link) { geometry_msgs::PoseStamped target; @@ -1079,7 +1078,7 @@ bool moveit::planning_interface::MoveItCpp::setPositionTarget(double x, double y return result; } -bool moveit::planning_interface::MoveItCpp::setRPYTarget(double r, double p, double y, +bool MoveItCpp::setRPYTarget(double r, double p, double y, const std::string& end_effector_link) { geometry_msgs::PoseStamped target; @@ -1103,7 +1102,7 @@ bool moveit::planning_interface::MoveItCpp::setRPYTarget(double r, double p, dou return result; } -bool moveit::planning_interface::MoveItCpp::setOrientationTarget(double x, double y, double z, double w, +bool MoveItCpp::setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link) { geometry_msgs::PoseStamped target; @@ -1129,59 +1128,59 @@ bool moveit::planning_interface::MoveItCpp::setOrientationTarget(double x, doubl return result; } -void moveit::planning_interface::MoveItCpp::setPoseReferenceFrame(const std::string& pose_reference_frame) +void MoveItCpp::setPoseReferenceFrame(const std::string& pose_reference_frame) { pose_reference_frame_ = pose_reference_frame; } -const std::string& moveit::planning_interface::MoveItCpp::getPoseReferenceFrame() const +const std::string& MoveItCpp::getPoseReferenceFrame() const { return pose_reference_frame_; } -double moveit::planning_interface::MoveItCpp::getGoalJointTolerance() const +double MoveItCpp::getGoalJointTolerance() const { return goal_joint_tolerance_; } -double moveit::planning_interface::MoveItCpp::getGoalPositionTolerance() const +double MoveItCpp::getGoalPositionTolerance() const { return goal_position_tolerance_; } -double moveit::planning_interface::MoveItCpp::getGoalOrientationTolerance() const +double MoveItCpp::getGoalOrientationTolerance() const { return goal_orientation_tolerance_; } -void moveit::planning_interface::MoveItCpp::setGoalTolerance(double tolerance) +void MoveItCpp::setGoalTolerance(double tolerance) { setGoalJointTolerance(tolerance); setGoalPositionTolerance(tolerance); setGoalOrientationTolerance(tolerance); } -void moveit::planning_interface::MoveItCpp::setGoalJointTolerance(double tolerance) +void MoveItCpp::setGoalJointTolerance(double tolerance) { goal_joint_tolerance_ = tolerance; } -void moveit::planning_interface::MoveItCpp::setGoalPositionTolerance(double tolerance) +void MoveItCpp::setGoalPositionTolerance(double tolerance) { goal_position_tolerance_ = tolerance; } -void moveit::planning_interface::MoveItCpp::setGoalOrientationTolerance(double tolerance) +void MoveItCpp::setGoalOrientationTolerance(double tolerance) { goal_orientation_tolerance_ = tolerance; } -void moveit::planning_interface::MoveItCpp::rememberJointValues(const std::string& name) +void MoveItCpp::rememberJointValues(const std::string& name) { rememberJointValues(name, getCurrentJointValues()); } -bool moveit::planning_interface::MoveItCpp::startStateMonitor(double wait) +bool MoveItCpp::startStateMonitor(double wait) { if (!current_state_monitor_) { @@ -1197,7 +1196,7 @@ bool moveit::planning_interface::MoveItCpp::startStateMonitor(double wait) return true; } -std::vector moveit::planning_interface::MoveItCpp::getCurrentJointValues() +std::vector MoveItCpp::getCurrentJointValues() { robot_state::RobotStatePtr current_state; std::vector values; @@ -1206,14 +1205,14 @@ std::vector moveit::planning_interface::MoveItCpp::getCurrentJointValues return values; } -std::vector moveit::planning_interface::MoveItCpp::getRandomJointValues() +std::vector MoveItCpp::getRandomJointValues() { std::vector r; joint_model_group_->getVariableRandomPositions(joint_state_target_->getRandomNumberGenerator(), r); return r; } -geometry_msgs::PoseStamped moveit::planning_interface::MoveItCpp::getRandomPose(const std::string& end_effector_link) +geometry_msgs::PoseStamped MoveItCpp::getRandomPose(const std::string& end_effector_link) { const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; Eigen::Isometry3d pose; @@ -1238,7 +1237,7 @@ geometry_msgs::PoseStamped moveit::planning_interface::MoveItCpp::getRandomPose( return pose_msg; } -geometry_msgs::PoseStamped moveit::planning_interface::MoveItCpp::getCurrentPose(const std::string& end_effector_link) +geometry_msgs::PoseStamped MoveItCpp::getCurrentPose(const std::string& end_effector_link) { const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; Eigen::Isometry3d pose; @@ -1262,7 +1261,7 @@ geometry_msgs::PoseStamped moveit::planning_interface::MoveItCpp::getCurrentPose return pose_msg; } -std::vector moveit::planning_interface::MoveItCpp::getCurrentRPY(const std::string& end_effector_link) +std::vector MoveItCpp::getCurrentRPY(const std::string& end_effector_link) { std::vector result; const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; @@ -1289,52 +1288,52 @@ std::vector moveit::planning_interface::MoveItCpp::getCurrentRPY(const s return result; } -const std::vector& moveit::planning_interface::MoveItCpp::getActiveJoints() const +const std::vector& MoveItCpp::getActiveJoints() const { return joint_model_group_->getActiveJointModelNames(); } -const std::vector& moveit::planning_interface::MoveItCpp::getJoints() const +const std::vector& MoveItCpp::getJoints() const { return joint_model_group_->getJointModelNames(); } -unsigned int moveit::planning_interface::MoveItCpp::getVariableCount() const +unsigned int MoveItCpp::getVariableCount() const { return joint_model_group_->getVariableCount(); } -robot_state::RobotStatePtr moveit::planning_interface::MoveItCpp::getCurrentState(double wait) +robot_state::RobotStatePtr MoveItCpp::getCurrentState(double wait) { robot_state::RobotStatePtr current_state; getCurrentState(current_state, wait); return current_state; } -void moveit::planning_interface::MoveItCpp::rememberJointValues(const std::string& name, +void MoveItCpp::rememberJointValues(const std::string& name, const std::vector& values) { remembered_joint_values_[name] = values; } -void moveit::planning_interface::MoveItCpp::forgetJointValues(const std::string& name) +void MoveItCpp::forgetJointValues(const std::string& name) { remembered_joint_values_.erase(name); } -void moveit::planning_interface::MoveItCpp::allowLooking(bool flag) +void MoveItCpp::allowLooking(bool flag) { can_look_ = flag; ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no"); } -void moveit::planning_interface::MoveItCpp::allowReplanning(bool flag) +void MoveItCpp::allowReplanning(bool flag) { can_replan_ = flag; ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no"); } -std::vector moveit::planning_interface::MoveItCpp::getKnownConstraints() const +std::vector MoveItCpp::getKnownConstraints() const { while (initializing_constraints_) { @@ -1349,7 +1348,7 @@ std::vector moveit::planning_interface::MoveItCpp::getKnownConstrai return c; } -moveit_msgs::Constraints moveit::planning_interface::MoveItCpp::getPathConstraints() const +moveit_msgs::Constraints MoveItCpp::getPathConstraints() const { if (path_constraints_) return *path_constraints_; @@ -1357,7 +1356,7 @@ moveit_msgs::Constraints moveit::planning_interface::MoveItCpp::getPathConstrain return moveit_msgs::Constraints(); } -bool moveit::planning_interface::MoveItCpp::setPathConstraints(const std::string& constraint) +bool MoveItCpp::setPathConstraints(const std::string& constraint) { if (constraints_storage_) { @@ -1374,17 +1373,17 @@ bool moveit::planning_interface::MoveItCpp::setPathConstraints(const std::string return false; } -void moveit::planning_interface::MoveItCpp::setPathConstraints(const moveit_msgs::Constraints& constraint) +void MoveItCpp::setPathConstraints(const moveit_msgs::Constraints& constraint) { path_constraints_.reset(new moveit_msgs::Constraints(constraint)); } -void moveit::planning_interface::MoveItCpp::clearPathConstraints() +void MoveItCpp::clearPathConstraints() { path_constraints_.reset(); } -moveit_msgs::TrajectoryConstraints moveit::planning_interface::MoveItCpp::getTrajectoryConstraints() const +moveit_msgs::TrajectoryConstraints MoveItCpp::getTrajectoryConstraints() const { if (trajectory_constraints_) return *trajectory_constraints_; @@ -1392,18 +1391,18 @@ moveit_msgs::TrajectoryConstraints moveit::planning_interface::MoveItCpp::getTra return moveit_msgs::TrajectoryConstraints(); } -void moveit::planning_interface::MoveItCpp::setTrajectoryConstraints( +void MoveItCpp::setTrajectoryConstraints( const moveit_msgs::TrajectoryConstraints& constraint) { trajectory_constraints_.reset(new moveit_msgs::TrajectoryConstraints(constraint)); } -void moveit::planning_interface::MoveItCpp::clearTrajectoryConstraints() +void MoveItCpp::clearTrajectoryConstraints() { trajectory_constraints_.reset(); } -void moveit::planning_interface::MoveItCpp::setConstraintsDatabase(const std::string& host, unsigned int port) +void MoveItCpp::setConstraintsDatabase(const std::string& host, unsigned int port) { initializing_constraints_ = true; if (constraints_init_thread_) @@ -1412,7 +1411,7 @@ void moveit::planning_interface::MoveItCpp::setConstraintsDatabase(const std::st new boost::thread(boost::bind(&MoveItCpp::initializeConstraintsStorageThread, this, host, port))); } -void moveit::planning_interface::MoveItCpp::setWorkspace(double minx, double miny, double minz, double maxx, +void MoveItCpp::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) { workspace_parameters_.header.frame_id = robot_model_->getModelFrame(); @@ -1426,39 +1425,39 @@ void moveit::planning_interface::MoveItCpp::setWorkspace(double minx, double min } /** \brief Set time allowed to planner to solve problem before aborting */ -void moveit::planning_interface::MoveItCpp::setPlanningTime(double seconds) +void MoveItCpp::setPlanningTime(double seconds) { if (seconds > 0.0) allowed_planning_time_ = seconds; } /** \brief Get time allowed to planner to solve problem before aborting */ -double moveit::planning_interface::MoveItCpp::getPlanningTime() const +double MoveItCpp::getPlanningTime() const { return allowed_planning_time_; } -void moveit::planning_interface::MoveItCpp::setSupportSurfaceName(const std::string& name) +void MoveItCpp::setSupportSurfaceName(const std::string& name) { support_surface_ = name; } -const std::string& moveit::planning_interface::MoveItCpp::getPlanningFrame() const +const std::string& MoveItCpp::getPlanningFrame() const { return robot_model_->getModelFrame(); } -const std::vector& moveit::planning_interface::MoveItCpp::getJointModelGroupNames() const +const std::vector& MoveItCpp::getJointModelGroupNames() const { return robot_model_->getJointModelGroupNames(); } -bool moveit::planning_interface::MoveItCpp::attachObject(const std::string& object, const std::string& link) +bool MoveItCpp::attachObject(const std::string& object, const std::string& link) { return attachObject(object, link, std::vector()); } -bool moveit::planning_interface::MoveItCpp::attachObject(const std::string& object, const std::string& link, +bool MoveItCpp::attachObject(const std::string& object, const std::string& link, const std::vector& touch_links) { std::string l = link.empty() ? getEndEffectorLink() : link; @@ -1485,7 +1484,7 @@ bool moveit::planning_interface::MoveItCpp::attachObject(const std::string& obje return true; } -bool moveit::planning_interface::MoveItCpp::detachObject(const std::string& name) +bool MoveItCpp::detachObject(const std::string& name) { moveit_msgs::AttachedCollisionObject aco; // if name is a link @@ -1509,7 +1508,7 @@ bool moveit::planning_interface::MoveItCpp::detachObject(const std::string& name return true; } -void moveit::planning_interface::MoveItCpp::constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request) +void MoveItCpp::constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request) { request.group_name = group_name_; request.num_planning_attempts = num_planning_attempts_; @@ -1569,7 +1568,7 @@ void moveit::planning_interface::MoveItCpp::constructMotionPlanRequest(moveit_ms } template -void moveit::planning_interface::MoveItCpp::waitForAction(const T& action, const std::string& name, +void MoveItCpp::waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time) { ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str()); @@ -1625,7 +1624,7 @@ void moveit::planning_interface::MoveItCpp::waitForAction(const T& action, const } } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::move(bool wait) +MoveItErrorCode MoveItCpp::move(bool wait) { if (!move_action_client_) { @@ -1672,12 +1671,12 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp } } -void moveit::planning_interface::MoveItCpp::constructGoal(moveit_msgs::MoveGroupGoal& goal) +void MoveItCpp::constructGoal(moveit_msgs::MoveGroupGoal& goal) { constructMotionPlanRequest(goal.request); } -void moveit::planning_interface::MoveItCpp::constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object) +void MoveItCpp::constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object) { moveit_msgs::PickupGoal goal; goal.target_name = object; @@ -1695,7 +1694,7 @@ void moveit::planning_interface::MoveItCpp::constructGoal(moveit_msgs::PickupGoa goal_out = goal; } -void moveit::planning_interface::MoveItCpp::constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object) +void MoveItCpp::constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object) { moveit_msgs::PlaceGoal goal; goal.attached_object_name = object; @@ -1712,7 +1711,7 @@ void moveit::planning_interface::MoveItCpp::constructGoal(moveit_msgs::PlaceGoal goal_out = goal; } -moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCpp::execute(const Plan& plan, bool wait) +MoveItErrorCode MoveItCpp::execute(const Plan& plan, bool wait) { if (!execute_action_client_->isServerConnected()) { @@ -1745,7 +1744,7 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveItCp } } -bool moveit::planning_interface::MoveItCpp::getCurrentState(robot_state::RobotStatePtr& current_state, +bool MoveItCpp::getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds) { if (!current_state_monitor_) @@ -1768,7 +1767,7 @@ bool moveit::planning_interface::MoveItCpp::getCurrentState(robot_state::RobotSt return true; } -bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_msgs::Pose& eef_pose, +bool MoveItCpp::setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link, const std::string& frame, bool approx) { @@ -1820,7 +1819,7 @@ bool moveit::planning_interface::MoveItCpp::setJointValueTarget(const geometry_m return false; } -robot_state::RobotStatePtr moveit::planning_interface::MoveItCpp::getStartState() +robot_state::RobotStatePtr MoveItCpp::getStartState() { if (considered_start_state_) return considered_start_state_; @@ -1832,13 +1831,13 @@ robot_state::RobotStatePtr moveit::planning_interface::MoveItCpp::getStartState( } } -bool moveit::planning_interface::MoveItCpp::hasPoseTarget(const std::string& end_effector_link) const +bool MoveItCpp::hasPoseTarget(const std::string& end_effector_link) const { const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; return pose_targets_.find(eef) != pose_targets_.end(); } -void moveit::planning_interface::MoveItCpp::initializeConstraintsStorageThread(const std::string& host, +void MoveItCpp::initializeConstraintsStorageThread(const std::string& host, unsigned int port) { // Set up db @@ -1858,7 +1857,7 @@ void moveit::planning_interface::MoveItCpp::initializeConstraintsStorageThread(c initializing_constraints_ = false; } -void moveit::planning_interface::MoveItCpp::clearContents() +void MoveItCpp::clearContents() { // TODO Instead of setting to nullptrs actually delete // TODO Set values to "default values" @@ -1909,3 +1908,5 @@ void moveit::planning_interface::MoveItCpp::clearContents() constraints_init_thread_ = nullptr; // initializing_constraints_ } +} // planning_interface +} // moveit From 9c282aadd4f46e960c94be7c8e3a245f14c49934 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 20 Aug 2019 11:24:46 +0200 Subject: [PATCH 15/29] Remove old pick/place actions --- .../include/moveit/moveit_cpp/moveit_cpp.h | 48 ---- .../moveit_cpp/src/moveit.cpp | 236 ------------------ 2 files changed, 284 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 55b095ee4c..2051afd4aa 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -48,10 +48,8 @@ #include #include #include -#include #include #include -#include #include #include #include @@ -340,10 +338,6 @@ class MoveItCpp /** \brief Set the starting state for planning to be that reported by the robot's joint state publication */ void setStartStateToCurrentState(); - /** \brief For pick/place operations, the name of the support surface is used to specify the fact that attached - * objects are allowed to touch the support surface */ - void setSupportSurfaceName(const std::string& name); - /** * \name Setting a joint state target (goal) * @@ -815,44 +809,6 @@ class MoveItCpp */ /**@{*/ - /** \brief Pick up an object - - This applies a number of hard-coded default grasps */ - MoveItErrorCode pick(const std::string& object, bool plan_only = false); - - /** \brief Pick up an object given a grasp pose */ - MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only = false); - - /** \brief Pick up an object given possible grasp poses - - if the vector is left empty this behaves like pick(const std::string &object) */ - MoveItErrorCode pick(const std::string& object, const std::vector& grasps, - bool plan_only = false); - - /** \brief Pick up an object - - calls the external moveit_msgs::GraspPlanning service "plan_grasps" to compute possible grasps */ - MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false); - - /** \brief Pick up an object - - calls the external moveit_msgs::GraspPlanning service "plan_grasps" to compute possible grasps */ - MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false); - - /** \brief Place an object somewhere safe in the world (a safe location will be detected) */ - MoveItErrorCode place(const std::string& object, bool plan_only = false); - - /** \brief Place an object at one of the specified possible locations */ - MoveItErrorCode place(const std::string& object, const std::vector& locations, - bool plan_only = false); - - /** \brief Place an object at one of the specified possible locations */ - MoveItErrorCode place(const std::string& object, const std::vector& poses, - bool plan_only = false); - - /** \brief Place an object at one of the specified possible location */ - MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only = false); - /** \brief Given the name of an object in the planning scene, make the object attached to a link of the robot. If no link name is specified, the end-effector is used. If there is no @@ -1000,8 +956,6 @@ class MoveItCpp void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time); MoveItErrorCode move(bool wait); void constructGoal(moveit_msgs::MoveGroupGoal& goal); - void constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object); - void constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object); MoveItErrorCode execute(const Plan& plan, bool wait); bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds); bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link, @@ -1049,8 +1003,6 @@ class MoveItCpp planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; std::unique_ptr > move_action_client_; std::unique_ptr > execute_action_client_; - std::unique_ptr > pick_action_client_; - std::unique_ptr > place_action_client_; // general planning params robot_state::RobotStatePtr considered_start_state_; diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp index d62868d068..b3be17ac6b 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp @@ -41,7 +41,6 @@ #include #include #include -#include #include #include #include @@ -142,14 +141,6 @@ MoveItCpp::MoveItCpp(const Options& opt, const std::shared_ptr& new actionlib::SimpleActionClient(node_handle_, move_group::MOVE_ACTION, false)); waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time); - pick_action_client_.reset( - new actionlib::SimpleActionClient(node_handle_, move_group::PICKUP_ACTION, false)); - waitForAction(pick_action_client_, move_group::PICKUP_ACTION, timeout_for_servers, allotted_time); - - place_action_client_.reset( - new actionlib::SimpleActionClient(node_handle_, move_group::PLACE_ACTION, false)); - waitForAction(place_action_client_, move_group::PLACE_ACTION, timeout_for_servers, allotted_time); - execute_action_client_.reset(new actionlib::SimpleActionClient( node_handle_, move_group::EXECUTE_ACTION_NAME, false)); waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time); @@ -193,8 +184,6 @@ MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) this->current_state_monitor_ = other.current_state_monitor_; swap(this->move_action_client_, other.move_action_client_); swap(this->execute_action_client_, other.execute_action_client_); - swap(this->pick_action_client_, other.pick_action_client_); - swap(this->place_action_client_, other.place_action_client_); this->considered_start_state_ = other.considered_start_state_; this->workspace_parameters_ = other.workspace_parameters_; @@ -419,189 +408,6 @@ MoveItErrorCode MoveItCpp::plan(Plan& plan) } } -MoveItErrorCode MoveItCpp::pick(const std::string& object, - bool plan_only) -{ - return pick(object, std::vector(), plan_only); -} - -MoveItErrorCode MoveItCpp::pick(const std::string& object, - const moveit_msgs::Grasp& grasp, - bool plan_only) -{ - return pick(object, std::vector(1, grasp), plan_only); -} - -MoveItErrorCode MoveItCpp::pick( - const std::string& object, const std::vector& grasps, bool plan_only) -{ - if (!pick_action_client_) - { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action client not found"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - if (!pick_action_client_->isServerConnected()) - { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Pick action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - moveit_msgs::PickupGoal goal; - constructGoal(goal, object); - goal.possible_grasps = grasps; - goal.planning_options.plan_only = plan_only; - goal.planning_options.look_around = can_look_; - goal.planning_options.replan = can_replan_; - goal.planning_options.replan_delay = replan_delay_; - goal.planning_options.planning_scene_diff.is_diff = true; - goal.planning_options.planning_scene_diff.robot_state.is_diff = true; - - pick_action_client_->sendGoal(goal); - if (!pick_action_client_->waitForResult()) - { - ROS_INFO_STREAM_NAMED("move_group_interface", "Pickup action returned early"); - } - if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) - { - return MoveItErrorCode(pick_action_client_->getResult()->error_code); - } - else - { - ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << pick_action_client_->getState().toString() << ": " - << pick_action_client_->getState().getText()); - return MoveItErrorCode(pick_action_client_->getResult()->error_code); - } -} - -MoveItErrorCode -MoveItCpp::planGraspsAndPick(const std::string& object, bool plan_only) -{ - if (object.empty()) - { - return planGraspsAndPick(moveit_msgs::CollisionObject()); - } - PlanningSceneInterface psi; - - std::map objects = psi.getObjects(std::vector(1, object)); - - if (objects.empty()) - { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Asked for grasps for the object '" - << object << "', but the object could not be found"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME); - } - - return planGraspsAndPick(objects[object], plan_only); -} - -MoveItErrorCode -MoveItCpp::planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only) -{ - if (!plan_grasps_service_) - { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Grasp planning service '" - << GRASP_PLANNING_SERVICE_NAME - << "' is not available." - " This has to be implemented and started seperatly."); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - - moveit_msgs::GraspPlanning::Request request; - moveit_msgs::GraspPlanning::Response response; - - request.group_name = group_name_; - request.target = object; - request.support_surfaces.push_back(support_surface_); - - ROS_DEBUG_NAMED("move_group_interface", "Calling grasp planner..."); - if (!plan_grasps_service_.call(request, response) || - response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS) - { - ROS_ERROR_NAMED("move_group_interface", "Grasp planning failed. Unable to pick."); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - - return pick(object.id, response.grasps, plan_only); -} - -MoveItErrorCode MoveItCpp::place(const std::string& object, - bool plan_only) -{ - return place(object, std::vector(), plan_only); -} - -MoveItErrorCode MoveItCpp::place( - const std::string& object, const std::vector& locations, bool plan_only) -{ - if (!place_action_client_) - { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action client not found"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - if (!place_action_client_->isServerConnected()) - { - ROS_ERROR_STREAM_NAMED("move_group_interface", "Place action server not connected"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - moveit_msgs::PlaceGoal goal; - constructGoal(goal, object); - goal.place_locations = locations; - goal.planning_options.plan_only = plan_only; - goal.planning_options.look_around = can_look_; - goal.planning_options.replan = can_replan_; - goal.planning_options.replan_delay = replan_delay_; - goal.planning_options.planning_scene_diff.is_diff = true; - goal.planning_options.planning_scene_diff.robot_state.is_diff = true; - - place_action_client_->sendGoal(goal); - ROS_DEBUG_NAMED("move_group_interface", "Sent place goal with %d locations", (int)goal.place_locations.size()); - if (!place_action_client_->waitForResult()) - { - ROS_INFO_STREAM_NAMED("move_group_interface", "Place action returned early"); - } - if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) - { - return MoveItErrorCode(place_action_client_->getResult()->error_code); - } - else - { - ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << place_action_client_->getState().toString() << ": " - << place_action_client_->getState().getText()); - return MoveItErrorCode(place_action_client_->getResult()->error_code); - } -} - -MoveItErrorCode MoveItCpp::place( - const std::string& object, const std::vector& poses, bool plan_only) -{ - std::vector locations; - for (const geometry_msgs::PoseStamped& pose : poses) - { - moveit_msgs::PlaceLocation location; - location.pre_place_approach.direction.vector.z = -1.0; - location.post_place_retreat.direction.vector.x = -1.0; - location.pre_place_approach.direction.header.frame_id = robot_model_->getModelFrame(); - location.post_place_retreat.direction.header.frame_id = end_effector_link_; - - location.pre_place_approach.min_distance = 0.1; - location.pre_place_approach.desired_distance = 0.2; - location.post_place_retreat.min_distance = 0.0; - location.post_place_retreat.desired_distance = 0.2; - // location.post_place_posture is filled by the pick&place lib with the getDetachPosture from the AttachedBody - - location.place_pose = pose; - locations.push_back(location); - } - ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations", - (unsigned int)locations.size()); - return place(object, locations, plan_only); -} - -MoveItErrorCode MoveItCpp::place( - const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only) -{ - return place(object, std::vector(1, pose), plan_only); -} - double MoveItCpp::computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, @@ -1437,11 +1243,6 @@ double MoveItCpp::getPlanningTime() const return allowed_planning_time_; } -void MoveItCpp::setSupportSurfaceName(const std::string& name) -{ - support_surface_ = name; -} - const std::string& MoveItCpp::getPlanningFrame() const { return robot_model_->getModelFrame(); @@ -1676,41 +1477,6 @@ void MoveItCpp::constructGoal(moveit_msgs::MoveGroupGoal& goal) constructMotionPlanRequest(goal.request); } -void MoveItCpp::constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object) -{ - moveit_msgs::PickupGoal goal; - goal.target_name = object; - goal.group_name = group_name_; - goal.end_effector = getEndEffector(); - goal.allowed_planning_time = allowed_planning_time_; - goal.support_surface_name = support_surface_; - goal.planner_id = planner_id_; - if (!support_surface_.empty()) - goal.allow_gripper_support_collision = true; - - if (path_constraints_) - goal.path_constraints = *path_constraints_; - - goal_out = goal; -} - -void MoveItCpp::constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object) -{ - moveit_msgs::PlaceGoal goal; - goal.attached_object_name = object; - goal.group_name = group_name_; - goal.allowed_planning_time = allowed_planning_time_; - goal.support_surface_name = support_surface_; - goal.planner_id = planner_id_; - if (!support_surface_.empty()) - goal.allow_gripper_support_collision = true; - - if (path_constraints_) - goal.path_constraints = *path_constraints_; - - goal_out = goal; -} - MoveItErrorCode MoveItCpp::execute(const Plan& plan, bool wait) { if (!execute_action_client_->isServerConnected()) @@ -1868,8 +1634,6 @@ void MoveItCpp::clearContents() // robot_model_ current_state_monitor_ = nullptr; execute_action_client_ = nullptr; - pick_action_client_ = nullptr; - place_action_client_ = nullptr; considered_start_state_ = nullptr; // workspace_parameters_ From 4b63fb4ab3f790ce40554fb5c8b911d580dab855 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 20 Aug 2019 11:32:01 +0200 Subject: [PATCH 16/29] Remove constraints storage --- .../include/moveit/moveit_cpp/moveit_cpp.h | 35 ---------- .../moveit_cpp/src/moveit.cpp | 68 ------------------- 2 files changed, 103 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 2051afd4aa..44ded62b2b 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -907,23 +907,11 @@ class MoveItCpp */ /**@{*/ - //TODO(henningkayser): Remove this for now - /** \brief Specify where the database server that holds known constraints resides */ - void setConstraintsDatabase(const std::string& host, unsigned int port); - - /** \brief Get the names of the known constraints as read from the Mongo database, if a connection was achieved. */ - std::vector getKnownConstraints() const; - /** \brief Get the actual set of constraints in use with this MoveItCpp. @return A copy of the current path constraints set for this interface */ moveit_msgs::Constraints getPathConstraints() const; - /** \brief Specify a set of path constraints to use. - The constraints are looked up by name from the Mongo database server. - This replaces any path constraints set in previous calls to setPathConstraints(). */ - bool setPathConstraints(const std::string& constraint); - /** \brief Specify a set of path constraints to use. This version does not require a database server. This replaces any path constraints set in previous calls to setPathConstraints(). */ @@ -974,26 +962,6 @@ class MoveItCpp bool allow_trajectory_execution_; bool debug_; - // impl contents - void initializeConstraintsStorageThread(const std::string& host, unsigned int port); - /*{ - // Set up db - try - { - warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase(); - conn->setParams(host, port); - if (conn->connect()) - { - constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(conn)); - } - } - catch (std::exception& ex) - { - ROS_ERROR_NAMED("move_group_interface", "%s", ex.what()); - } - initializing_constraints_ = false; - } */ - // Options std::string group_name_; std::string robot_description_; @@ -1043,9 +1011,6 @@ class MoveItCpp ros::ServiceClient set_params_service_; ros::ServiceClient cartesian_path_service_; ros::ServiceClient plan_grasps_service_; - std::unique_ptr constraints_storage_; - std::unique_ptr constraints_init_thread_; - bool initializing_constraints_; }; } // namespace planning_interface } // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp index b3be17ac6b..ee5c8baeec 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp @@ -38,7 +38,6 @@ #include #include #include -#include #include #include #include @@ -119,7 +118,6 @@ MoveItCpp::MoveItCpp(const Options& opt, const std::shared_ptr& num_planning_attempts_ = 1; max_velocity_scaling_factor_ = 1.0; max_acceleration_scaling_factor_ = 1.0; - initializing_constraints_ = false; if (joint_model_group_->isChain()) end_effector_link_ = joint_model_group_->getLinkModelNames().back(); @@ -218,9 +216,6 @@ MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) this->set_params_service_ = other.set_params_service_; this->cartesian_path_service_ = other.cartesian_path_service_; this->plan_grasps_service_ = other.plan_grasps_service_; - swap(this->constraints_storage_, other.constraints_storage_); - swap(this->constraints_init_thread_, other.constraints_init_thread_); - this->initializing_constraints_ = other.initializing_constraints_; remembered_joint_values_ = std::move(other.remembered_joint_values_); other.clearContents(); @@ -1139,21 +1134,6 @@ void MoveItCpp::allowReplanning(bool flag) ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no"); } -std::vector MoveItCpp::getKnownConstraints() const -{ - while (initializing_constraints_) - { - static ros::WallDuration d(0.01); - d.sleep(); - } - - std::vector c; - if (constraints_storage_) - constraints_storage_->getKnownConstraints(c, robot_model_->getName(), group_name_); - - return c; -} - moveit_msgs::Constraints MoveItCpp::getPathConstraints() const { if (path_constraints_) @@ -1162,23 +1142,6 @@ moveit_msgs::Constraints MoveItCpp::getPathConstraints() const return moveit_msgs::Constraints(); } -bool MoveItCpp::setPathConstraints(const std::string& constraint) -{ - if (constraints_storage_) - { - moveit_warehouse::ConstraintsWithMetadata msg_m; - if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), group_name_)) - { - path_constraints_.reset(new moveit_msgs::Constraints(static_cast(*msg_m))); - return true; - } - else - return false; - } - else - return false; -} - void MoveItCpp::setPathConstraints(const moveit_msgs::Constraints& constraint) { path_constraints_.reset(new moveit_msgs::Constraints(constraint)); @@ -1208,15 +1171,6 @@ void MoveItCpp::clearTrajectoryConstraints() trajectory_constraints_.reset(); } -void MoveItCpp::setConstraintsDatabase(const std::string& host, unsigned int port) -{ - initializing_constraints_ = true; - if (constraints_init_thread_) - constraints_init_thread_->join(); - constraints_init_thread_.reset( - new boost::thread(boost::bind(&MoveItCpp::initializeConstraintsStorageThread, this, host, port))); -} - void MoveItCpp::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) { @@ -1603,26 +1557,6 @@ bool MoveItCpp::hasPoseTarget(const std::string& end_effector_link) const return pose_targets_.find(eef) != pose_targets_.end(); } -void MoveItCpp::initializeConstraintsStorageThread(const std::string& host, - unsigned int port) -{ - // Set up db - try - { - warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase(); - conn->setParams(host, port); - if (conn->connect()) - { - constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(conn)); - } - } - catch (std::exception& ex) - { - ROS_ERROR_NAMED("move_group_interface", "%s", ex.what()); - } - initializing_constraints_ = false; -} - void MoveItCpp::clearContents() { // TODO Instead of setting to nullptrs actually delete @@ -1668,8 +1602,6 @@ void MoveItCpp::clearContents() // set_params_service_ // cartesian_path_service_ // plan_grasps_service_ - constraints_storage_ = nullptr; - constraints_init_thread_ = nullptr; // initializing_constraints_ } } // planning_interface From fbb5f0cf3f57df09c39e484b40bb664313d49dda Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 20 Aug 2019 13:22:06 +0200 Subject: [PATCH 17/29] Reduce moveit_cpp to minimal core class --- .../moveit_cpp/CMakeLists.txt | 2 +- .../include/moveit/moveit_cpp/moveit_core.h | 239 +++ .../include/moveit/moveit_cpp/moveit_cpp.h | 1018 ----------- .../moveit_cpp/src/moveit.cpp | 1608 ----------------- .../moveit_cpp/src/moveit_core.cpp | 186 ++ 5 files changed, 426 insertions(+), 2627 deletions(-) create mode 100644 moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h delete mode 100644 moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h delete mode 100644 moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp create mode 100644 moveit_ros/planning_interface/moveit_cpp/src/moveit_core.cpp diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt index 7e1e422dd2..a039bfc398 100644 --- a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -1,6 +1,6 @@ set(MOVEIT_LIB_NAME moveit_cpp) -add_library(${MOVEIT_LIB_NAME} src/moveit.cpp) +add_library(${MOVEIT_LIB_NAME} src/moveit_core.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h new file mode 100644 index 0000000000..098fddf120 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h @@ -0,0 +1,239 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Simon Goldstein, Henning Kayser */ + +#ifndef MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ +#define MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace planning_scene_monitor +{ +MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); +} + +namespace planning_pipeline +{ +MOVEIT_CLASS_FORWARD(PlanningPipeline); +} + +namespace plan_execution +{ +MOVEIT_CLASS_FORWARD(PlanExecution); +MOVEIT_CLASS_FORWARD(PlanWithSensing); +} + +namespace trajectory_execution_manager +{ +MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); +} + +namespace +{ +enum ActiveTargetType +{ + JOINT, + POSE, + POSITION, + ORIENTATION +}; +} + +namespace moveit +{ +/** \brief Simple interface to MoveIt components */ +namespace planning_interface +{ +//TODO(henningkayser): Move to own header and share with MoveGroup +class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes +{ +public: + MoveItErrorCode() + { + val = 0; + } + MoveItErrorCode(int code) + { + val = code; + } + MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code) + { + val = code.val; + } + explicit operator bool() const + { + return val == moveit_msgs::MoveItErrorCodes::SUCCESS; + } + bool operator==(const int c) const + { + return val == c; + } + bool operator!=(const int c) const + { + return val != c; + } +}; + +MOVEIT_CLASS_FORWARD(MoveItCpp); + +/** \class MoveItCpp move_group_interface.h moveit/planning_interface/move_group_interface.h + + \brief Client class to conveniently use the ROS interfaces provided by the move_group node. + + This class includes many default settings to make things easy to use. */ +class MoveItCpp +{ +public: + /** \brief Specification of options to use when constructing the MoveItCpp class */ + struct Options + { + Options(const std::string& group_name = "FAKE", const std::string& desc = "robot_description", + const ros::NodeHandle& node_handle = ros::NodeHandle()) + : default_group_name_(group_name), robot_description_(desc), node_handle_(node_handle) + { + } + + /// The group to construct the class instance for + std::string default_group_name_; + + /// The robot description parameter name (if different from default) + std::string robot_description_; + + /// Optionally, an instance of the RobotModel to use can be also specified + robot_model::RobotModelConstPtr robot_model_; + + ros::NodeHandle node_handle_; + }; + + MOVEIT_STRUCT_FORWARD(Plan); + + /// The representation of a motion plan (as ROS messages) + struct Plan + { + /// The full starting state used for planning + moveit_msgs::RobotState start_state_; + + /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) + moveit_msgs::RobotTrajectory trajectory_; + + /// The amount of time it took to generate the plan + double planning_time_; + }; + + /** + \brief Construct a MoveItCpp instance call using a specified set of options \e opt. + + \param opt. A MoveItCpp::Options structure, if you pass a ros::NodeHandle with a specific callback queue, + it has to be of type ros::CallbackQueue + (which is the default type of callback queues used in ROS) + \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, + one will be constructed internally along with an internal TF2_ROS TransformListener + \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. + */ + MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer = std::shared_ptr()); + /** + \brief Construct a client for the MoveGroup action for a particular \e group. + + \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, + one will be constructed internally along with an internal TF2_ROS TransformListener + \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. + */ + MoveItCpp(const std::string& group, + const std::shared_ptr& tf_buffer = std::shared_ptr()); + + ~MoveItCpp(); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + MoveItCpp(const MoveItCpp&) = delete; + MoveItCpp& operator=(const MoveItCpp&) = delete; + + MoveItCpp(MoveItCpp&& other); + MoveItCpp& operator=(MoveItCpp&& other); + + /** \brief Get the names of the named robot states available as targets, both either remembered states or default + * states from srdf */ + const std::vector getNamedTargets(); + + /** \brief Get the RobotModel object. */ + robot_model::RobotModelConstPtr getRobotModel() const; + + /** \brief Get the name of the default planning group */ + const std::string& getDefaultGroup() const; + + /** \brief Set the name of the default planning group */ + bool setDefaultGroup(const std::string& group_name); + + /** \brief Get the ROS node handle of this instance operates on */ + const ros::NodeHandle& getNodeHandle() const; + +private: + // Core properties + ros::NodeHandle node_handle_; + std::shared_ptr tf_buffer_; + std::string robot_description_; + std::string default_group_name_; + robot_model::RobotModelConstPtr robot_model_; + planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + void clearContents(); +}; +} // namespace planning_interface +} // namespace moveit + +#endif diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h deleted file mode 100644 index 44ded62b2b..0000000000 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ /dev/null @@ -1,1018 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, PickNik LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik LLC nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Simon Goldstein, Henning Kayser */ - -#ifndef MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ -#define MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace planning_scene_monitor -{ -MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); -} - -namespace planning_pipeline -{ -MOVEIT_CLASS_FORWARD(PlanningPipeline); -} - -namespace plan_execution -{ -MOVEIT_CLASS_FORWARD(PlanExecution); -MOVEIT_CLASS_FORWARD(PlanWithSensing); -} - -namespace trajectory_execution_manager -{ -MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); -} - -namespace -{ -enum ActiveTargetType -{ - JOINT, - POSE, - POSITION, - ORIENTATION -}; -} - -namespace moveit -{ -/** \brief Simple interface to MoveIt components */ -namespace planning_interface -{ -//TODO(henningkayser): Move to own header and share with MoveGroup -class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes -{ -public: - MoveItErrorCode() - { - val = 0; - } - MoveItErrorCode(int code) - { - val = code; - } - MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code) - { - val = code.val; - } - explicit operator bool() const - { - return val == moveit_msgs::MoveItErrorCodes::SUCCESS; - } - bool operator==(const int c) const - { - return val == c; - } - bool operator!=(const int c) const - { - return val != c; - } -}; - -MOVEIT_CLASS_FORWARD(MoveItCpp); - -/** \class MoveItCpp move_group_interface.h moveit/planning_interface/move_group_interface.h - - \brief Client class to conveniently use the ROS interfaces provided by the move_group node. - - This class includes many default settings to make things easy to use. */ -class MoveItCpp -{ -public: - /** \brief Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description' */ - static const std::string ROBOT_DESCRIPTION; - - /** \brief Specification of options to use when constructing the MoveItCpp class */ - struct Options - { - Options(const std::string& group_name = "FAKE", const std::string& desc = ROBOT_DESCRIPTION, - const ros::NodeHandle& node_handle = ros::NodeHandle()) - : group_name_(group_name), robot_description_(desc), node_handle_(node_handle) - { - } - - /// The group to construct the class instance for - std::string group_name_; - - /// The robot description parameter name (if different from default) - std::string robot_description_; - - /// Optionally, an instance of the RobotModel to use can be also specified - robot_model::RobotModelConstPtr robot_model_; - - ros::NodeHandle node_handle_; - }; - - MOVEIT_STRUCT_FORWARD(Plan); - - /// The representation of a motion plan (as ROS messasges) - struct Plan - { - /// The full starting state used for planning - moveit_msgs::RobotState start_state_; - - /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) - moveit_msgs::RobotTrajectory trajectory_; - - /// The amount of time it took to generate the plan - double planning_time_; - }; - - /** - \brief Construct a MoveItCpp instance call using a specified set of options \e opt. - - \param opt. A MoveItCpp::Options structure, if you pass a ros::NodeHandle with a specific callback queue, - it has to be of type ros::CallbackQueue - (which is the default type of callback queues used in ROS) - \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, - one will be constructed internally along with an internal TF2_ROS TransformListener - \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. - */ - MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer = std::shared_ptr(), - const ros::WallDuration& wait_for_servers = ros::WallDuration()); - /** - \brief Construct a client for the MoveGroup action for a particular \e group. - - \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, - one will be constructed internally along with an internal TF2_ROS TransformListener - \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. - */ - MoveItCpp(const std::string& group, - const std::shared_ptr& tf_buffer = std::shared_ptr(), - const ros::WallDuration& wait_for_servers = ros::WallDuration()); - - ~MoveItCpp(); - - /** - * @brief This class owns unique resources (e.g. action clients, threads) and its not very - * meaningful to copy. Pass by references, move it, or simply create multiple instances where - * required. - */ - MoveItCpp(const MoveItCpp&) = delete; - MoveItCpp& operator=(const MoveItCpp&) = delete; - - MoveItCpp(MoveItCpp&& other); - MoveItCpp& operator=(MoveItCpp&& other); - - /** \brief Get the name of the group this instance operates on */ - const std::string& getName() const; - - /** \brief Get the names of the named robot states available as targets, both either remembered states or default - * states from srdf */ - const std::vector getNamedTargets(); - - /** \brief Get the RobotModel object. */ - robot_model::RobotModelConstPtr getRobotModel() const; - - /** \brief Get the ROS node handle of this instance operates on */ - const ros::NodeHandle& getNodeHandle() const; - - /** \brief Get the name of the frame in which the robot is planning */ - const std::string& getPlanningFrame() const; - - /** \brief Get the available planning group names */ - const std::vector& getJointModelGroupNames() const; - - /** \brief Get vector of names of joints available in move group */ - const std::vector& getJointNames(); - - /** \brief Get vector of names of links available in move group */ - const std::vector& getLinkNames(); - - /** \brief Get the joint angles for targets specified by name */ - std::map getNamedTargetValues(const std::string& name); - - /** \brief Get only the active (actuated) joints this instance operates on */ - const std::vector& getActiveJoints() const; - - /** \brief Get all the joints this instance operates on (including fixed joints)*/ - const std::vector& getJoints() const; - - /** \brief Get the number of variables used to describe the state of this group. This is larger or equal to the number - * of DOF. */ - unsigned int getVariableCount() const; - - /** \brief Get the description of the planning plugin loaded by the action server */ - bool getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc); - - /** \brief Get the planner parameters for given group and planner_id */ - std::map getPlannerParams(const std::string& planner_id, const std::string& group = ""); - - /** \brief Set the planner parameters for given group and planner_id */ - void setPlannerParams(const std::string& planner_id, const std::string& group, - const std::map& params, bool bReplace = false); - - /** \brief Get the default planner for a given group (or global default) */ - std::string getDefaultPlannerId(const std::string& group = "") const; - - /** \brief Specify a planner to be used for further planning */ - void setPlannerId(const std::string& planner_id); - - /** \brief Get the current planner_id */ - const std::string& getPlannerId() const; - - /** \brief Specify the maximum amount of time to use when planning */ - void setPlanningTime(double seconds); - - /** \brief Set the number of times the motion plan is to be computed from scratch before the shortest solution is - * returned. The default value is 1.*/ - void setNumPlanningAttempts(unsigned int num_planning_attempts); - - /** \brief Set a scaling factor for optionally reducing the maximum joint velocity. - Allowed values are in (0,1]. The maximum joint velocity specified - in the robot model is multiplied by the factor. If outside valid range - (imporantly, this includes it being set to 0.0), the factor is set to a - default value of 1.0 internally (i.e. maximum joint velocity) */ - void setMaxVelocityScalingFactor(double max_velocity_scaling_factor); - - /** \brief Set a scaling factor for optionally reducing the maximum joint acceleration. - Allowed values are in (0,1]. The maximum joint acceleration specified - in the robot model is multiplied by the factor. If outside valid range - (imporantly, this includes it being set to 0.0), the factor is set to a - default value of 1.0 internally (i.e. maximum joint acceleration) */ - void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor); - - /** \brief Get the number of seconds set by setPlanningTime() */ - double getPlanningTime() const; - - /** \brief Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configuration - * space */ - double getGoalJointTolerance() const; - - /** \brief Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where the - * end-effector must reach.*/ - double getGoalPositionTolerance() const; - - /** \brief Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll, pitch and - * yaw, in radians. */ - double getGoalOrientationTolerance() const; - - /** \brief Set the tolerance that is used for reaching the goal. For - joint state goals, this will be distance for each joint, in the - configuration space (radians or meters depending on joint type). For pose - goals this will be the radius of a sphere where the end-effector must - reach. This function simply triggers calls to setGoalPositionTolerance(), - setGoalOrientationTolerance() and setGoalJointTolerance(). */ - void setGoalTolerance(double tolerance); - - /** \brief Set the joint tolerance (for each joint) that is used for reaching the goal when moving to a joint value - * target. */ - void setGoalJointTolerance(double tolerance); - - /** \brief Set the position tolerance that is used for reaching the goal when moving to a pose. */ - void setGoalPositionTolerance(double tolerance); - - /** \brief Set the orientation tolerance that is used for reaching the goal when moving to a pose. */ - void setGoalOrientationTolerance(double tolerance); - - /** \brief Specify the workspace bounding box. - The box is specified in the planning frame (i.e. relative to the robot root link start position). - This is useful when the planning group contains the root joint of the robot -- i.e. when planning motion for the - robot relative to the world. */ - void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz); - - /** \brief If a different start state should be considered instead of the current state of the robot, this function - * sets that state */ - void setStartState(const moveit_msgs::RobotState& start_state); - - /** \brief If a different start state should be considered instead of the current state of the robot, this function - * sets that state */ - void setStartState(const robot_state::RobotState& start_state); - - /** \brief Set the starting state for planning to be that reported by the robot's joint state publication */ - void setStartStateToCurrentState(); - - /** - * \name Setting a joint state target (goal) - * - * There are 2 types of goal targets: - * \li a JointValueTarget (aka JointStateTarget) specifies an absolute value for each joint (angle for rotational - *joints or position for prismatic joints). - * \li a PoseTarget (Position, Orientation, or Pose) specifies the pose of one or more end effectors (and the planner - *can use any joint values that reaches the pose(s)). - * - * Only one or the other is used for planning. Calling any of the - * set*JointValueTarget() functions sets the current goal target to the - * JointValueTarget. Calling any of the setPoseTarget(), - * setOrientationTarget(), setRPYTarget(), setPositionTarget() functions sets - * the current goal target to the Pose target. - */ - /**@{*/ - - /** \brief Set the JointValueTarget and use it for future planning requests. - - \e group_variable_values MUST exactly match the variable order as returned by - getJointValueTarget(std::vector&). - - This always sets all of the group's joint values. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const std::vector& group_variable_values); - - /** \brief Set the JointValueTarget and use it for future planning requests. - - \e variable_values is a map of joint variable names to values. Joints in - the group are used to set the JointValueTarget. Joints in the model but - not in the group are ignored. An exception is thrown if a joint name is - not found in the model. Joint variables in the group that are missing - from \e variable_values remain unchanged (to reset all target variables - to their current values in the robot use - setJointValueTarget(getCurrentJointValues())). - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const std::map& variable_values); - - /** \brief Set the JointValueTarget and use it for future planning requests. - - \e variable_names are variable joint names and variable_values are - variable joint positions. Joints in the group are used to set the - JointValueTarget. Joints in the model but not in the group are ignored. - An exception is thrown if a joint name is not found in the model. - Joint variables in the group that are missing from \e variable_names - remain unchanged (to reset all target variables to their current values - in the robot use setJointValueTarget(getCurrentJointValues())). - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const std::vector& variable_names, const std::vector& variable_values); - - /** \brief Set the JointValueTarget and use it for future planning requests. - - The target for all joints in the group are set to the value in \e robot_state. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const robot_state::RobotState& robot_state); - - /** \brief Set the JointValueTarget and use it for future planning requests. - - \e values MUST have one value for each variable in joint \e joint_name. - \e values are set as the target for this joint. - Other joint targets remain unchanged. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const std::string& joint_name, const std::vector& values); - - /** \brief Set the JointValueTarget and use it for future planning requests. - - Joint \e joint_name must be a 1-DOF joint. - \e value is set as the target for this joint. - Other joint targets remain unchanged. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const std::string& joint_name, double value); - - /** \brief Set the JointValueTarget and use it for future planning requests. - - \e state is used to set the target joint state values. - Values not specified in \e state remain unchanged. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If these values are out of bounds then false is returned BUT THE VALUES - ARE STILL SET AS THE GOAL. */ - bool setJointValueTarget(const sensor_msgs::JointState& state); - - /** \brief Set the joint state goal for a particular joint by computing IK. - - This is different from setPoseTarget() in that a single IK state (aka - JointValueTarget) is computed using IK, and the resulting - JointValueTarget is used as the target for planning. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If IK fails to find a solution then false is returned BUT THE PARTIAL - RESULT OF IK IS STILL SET AS THE GOAL. */ - bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = ""); - - /** \brief Set the joint state goal for a particular joint by computing IK. - - This is different from setPoseTarget() in that a single IK state (aka - JointValueTarget) is computed using IK, and the resulting - JointValueTarget is used as the target for planning. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If IK fails to find a solution then false is returned BUT THE PARTIAL - RESULT OF IK IS STILL SET AS THE GOAL. */ - bool setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, const std::string& end_effector_link = ""); - - /** \brief Set the joint state goal for a particular joint by computing IK. - - This is different from setPoseTarget() in that a single IK state (aka - JointValueTarget) is computed using IK, and the resulting - JointValueTarget is used as the target for planning. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If IK fails to find a solution then false is returned BUT THE PARTIAL - RESULT OF IK IS STILL SET AS THE GOAL. */ - bool setJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = ""); - - /** \brief Set the joint state goal for a particular joint by computing IK. - - This is different from setPoseTarget() in that a single IK state (aka - JointValueTarget) is computed using IK, and the resulting - JointValueTarget is used as the target for planning. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If IK fails to find a solution then an approximation is used. */ - bool setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link = ""); - - /** \brief Set the joint state goal for a particular joint by computing IK. - - This is different from setPoseTarget() in that a single IK state (aka - JointValueTarget) is computed using IK, and the resulting - JointValueTarget is used as the target for planning. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If IK fails to find a solution then an approximation is used. */ - bool setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, - const std::string& end_effector_link = ""); - - /** \brief Set the joint state goal for a particular joint by computing IK. - - This is different from setPoseTarget() in that a single IK state (aka - JointValueTarget) is computed using IK, and the resulting - JointValueTarget is used as the target for planning. - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. - - If IK fails to find a solution then an approximation is used. */ - bool setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose, const std::string& end_effector_link = ""); - - /** \brief Set the joint state goal to a random joint configuration - - After this call, the JointValueTarget is used \b instead of any - previously set Position, Orientation, or Pose targets. */ - void setRandomTarget(); - - /** \brief Set the current joint values to be ones previously remembered by rememberJointValues() or, if not found, - that are specified in the SRDF under the name \e name as a group state*/ - bool setNamedTarget(const std::string& name); - - /** \brief Get the current joint state goal in a form compatible to setJointValueTarget() */ - void getJointValueTarget(std::vector& group_variable_values) const; - - /// Get the currently set joint state goal, replaced by private getTargetRobotState() - MOVEIT_DEPRECATED const robot_state::RobotState& getJointValueTarget() const; - - /**@}*/ - - /** - * \name Setting a pose target (goal) - * - * Setting a Pose (or Position or Orientation) target disables any previously - * set JointValueTarget. - * - * For groups that have multiple end effectors, a pose can be set for each - * end effector in the group. End effectors which do not have a pose target - * set will end up in arbitrary positions. - */ - /**@{*/ - - /** \brief Set the goal position of the end-effector \e end_effector_link to be (\e x, \e y, \e z). - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - This new position target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target for this \e - end_effector_link. */ - bool setPositionTarget(double x, double y, double z, const std::string& end_effector_link = ""); - - /** \brief Set the goal orientation of the end-effector \e end_effector_link to be (\e roll,\e pitch,\e yaw) radians. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target for this \e - end_effector_link. */ - bool setRPYTarget(double roll, double pitch, double yaw, const std::string& end_effector_link = ""); - - /** \brief Set the goal orientation of the end-effector \e end_effector_link to be the quaternion (\e x,\e y,\e z,\e - w). - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target for this \e - end_effector_link. */ - bool setOrientationTarget(double x, double y, double z, double w, const std::string& end_effector_link = ""); - - /** \brief Set the goal pose of the end-effector \e end_effector_link. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - This new pose target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target for this \e - end_effector_link. */ - bool setPoseTarget(const Eigen::Isometry3d& end_effector_pose, const std::string& end_effector_link = ""); - - /** \brief Set the goal pose of the end-effector \e end_effector_link. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target for this \e - end_effector_link. */ - bool setPoseTarget(const geometry_msgs::Pose& target, const std::string& end_effector_link = ""); - - /** \brief Set the goal pose of the end-effector \e end_effector_link. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target for this \e - end_effector_link. */ - bool setPoseTarget(const geometry_msgs::PoseStamped& target, const std::string& end_effector_link = ""); - - /** \brief Set goal poses for \e end_effector_link. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - When planning, the planner will find a path to one (arbitrarily chosen) - pose from the list. If this group contains multiple end effectors then - all end effectors in the group should have the same number of pose - targets. If planning is successful then the result of the plan will - place all end effectors at a pose from the same index in the list. (In - other words, if one end effector ends up at the 3rd pose in the list then - all end effectors in the group will end up at the 3rd pose in their - respective lists. End effectors which do not matter (i.e. can end up in - any position) can have their pose targets disabled by calling - clearPoseTarget() for that end_effector_link. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target(s) for this \e - end_effector_link. */ - bool setPoseTargets(const EigenSTL::vector_Isometry3d& end_effector_pose, const std::string& end_effector_link = ""); - - /** \brief Set goal poses for \e end_effector_link. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - When planning, the planner will find a path to one (arbitrarily chosen) - pose from the list. If this group contains multiple end effectors then - all end effectors in the group should have the same number of pose - targets. If planning is successful then the result of the plan will - place all end effectors at a pose from the same index in the list. (In - other words, if one end effector ends up at the 3rd pose in the list then - all end effectors in the group will end up at the 3rd pose in their - respective lists. End effectors which do not matter (i.e. can end up in - any position) can have their pose targets disabled by calling - clearPoseTarget() for that end_effector_link. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target(s) for this \e - end_effector_link. */ - bool setPoseTargets(const std::vector& target, const std::string& end_effector_link = ""); - - /** \brief Set goal poses for \e end_effector_link. - - If \e end_effector_link is empty then getEndEffectorLink() is used. - - When planning, the planner will find a path to one (arbitrarily chosen) - pose from the list. If this group contains multiple end effectors then - all end effectors in the group should have the same number of pose - targets. If planning is successful then the result of the plan will - place all end effectors at a pose from the same index in the list. (In - other words, if one end effector ends up at the 3rd pose in the list then - all end effectors in the group will end up at the 3rd pose in their - respective lists. End effectors which do not matter (i.e. can end up in - any position) can have their pose targets disabled by calling - clearPoseTarget() for that end_effector_link. - - This new orientation target replaces any pre-existing JointValueTarget or - pre-existing Position, Orientation, or Pose target(s) for this \e - end_effector_link. */ - bool setPoseTargets(const std::vector& target, const std::string& end_effector_link = ""); - - /// Specify which reference frame to assume for poses specified without a reference frame. - void setPoseReferenceFrame(const std::string& pose_reference_frame); - - /** \brief Specify the parent link of the end-effector. - This \e end_effector_link will be used in calls to pose target functions - when end_effector_link is not explicitly specified. */ - bool setEndEffectorLink(const std::string& end_effector_link); - - /** \brief Specify the name of the end-effector to use. - This is equivalent to setting the EndEffectorLink to the parent link of this end effector. */ - bool setEndEffector(const std::string& eef_name); - - /// Forget pose(s) specified for \e end_effector_link - void clearPoseTarget(const std::string& end_effector_link = ""); - - /// Forget any poses specified for all end-effectors. - void clearPoseTargets(); - - /** Get the currently set pose goal for the end-effector \e end_effector_link. - If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is - assumed. - If multiple targets are set for \e end_effector_link this will return the first one. - If no pose target is set for this \e end_effector_link then an empty pose will be returned (check for - orientation.xyzw == 0). */ - const geometry_msgs::PoseStamped& getPoseTarget(const std::string& end_effector_link = "") const; - - /** Get the currently set pose goal for the end-effector \e end_effector_link. The pose goal can consist of multiple - poses, - if corresponding setPoseTarget() calls were made. Otherwise, only one pose is returned in the vector. - If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is - assumed */ - const std::vector& getPoseTargets(const std::string& end_effector_link = "") const; - - /** \brief Get the current end-effector link. - This returns the value set by setEndEffectorLink() (or indirectly by setEndEffector()). - If setEndEffectorLink() was not called, this function reports the link name that serves as parent - of an end-effector attached to this group. If there are multiple end-effectors, one of them is returned. - If no such link is known, the empty string is returned. */ - const std::string& getEndEffectorLink() const; - - /** \brief Get the current end-effector name. - This returns the value set by setEndEffector() (or indirectly by setEndEffectorLink()). - If setEndEffector() was not called, this function reports an end-effector attached to this group. - If there are multiple end-effectors, one of them is returned. If no end-effector is known, the empty string is - returned. */ - const std::string& getEndEffector() const; - - /** \brief Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the robot - * model */ - const std::string& getPoseReferenceFrame() const; - - /**@}*/ - - /** - * \name Planning a path from the start position to the Target (goal) position, and executing that plan. - */ - /**@{*/ - - /** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified - target. - This call is not blocking (does not wait for the execution of the trajectory to complete). */ - MoveItErrorCode asyncMove(); - - /** \brief Get the move_group action client used by the \e MoveItCpp. - The client can be used for querying the execution state of the trajectory and abort trajectory execution - during asynchronous execution. */ - actionlib::SimpleActionClient& getMoveGroupClient() const; - - /** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified - target. - This call is always blocking (waits for the execution of the trajectory to complete) and requires an asynchronous - spinner to be started.*/ - MoveItErrorCode move(); - - /** \brief Compute a motion plan that takes the group declared in the constructor from the current state to the - specified - target. No execution is performed. The resulting plan is stored in \e plan*/ - MoveItErrorCode plan(Plan& plan); - - /** \brief Given a \e plan, execute it without waiting for completion. Return true on success. */ - MoveItErrorCode asyncExecute(const Plan& plan); - - /** \brief Given a \e plan, execute it while waiting for completion. Return true on success. */ - MoveItErrorCode execute(const Plan& plan); - - /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters - between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the - waypoints is that specified by setPoseReferenceFrame(). No more than \e jump_threshold - is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK - solutions). - Collisions are avoided if \e avoid_collisions is set to true. If collisions cannot be avoided, the function fails. - Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the - waypoints. - Return -1.0 in case of error. */ - double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, - moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true, - moveit_msgs::MoveItErrorCodes* error_code = NULL); - - /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters - between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the - waypoints is that specified by setPoseReferenceFrame(). No more than \e jump_threshold - is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK - solutions). - Kinematic constraints for the path given by \e path_constraints will be met for every point along the trajectory, - if they are not met, a partial solution will be returned. - Constraints are checked (collision and kinematic) if \e avoid_collisions is set to true. If constraints cannot be - met, the function fails. - Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the - waypoints. - Return -1.0 in case of error. */ - double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, - moveit_msgs::RobotTrajectory& trajectory, - const moveit_msgs::Constraints& path_constraints, bool avoid_collisions = true, - moveit_msgs::MoveItErrorCodes* error_code = NULL); - - /** \brief Stop any trajectory execution, if one is active */ - void stop(); - - /** \brief Specify whether the robot is allowed to look around before moving if it determines it should (default is - * true) */ - void allowLooking(bool flag); - - /** \brief Specify whether the robot is allowed to replan if it detects changes in the environment */ - void allowReplanning(bool flag); - - /** \brief Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it - in \e request */ - void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request); - - /**@}*/ - - /** - * \name High level actions that trigger a sequence of plans and actions. - */ - /**@{*/ - - /** \brief Given the name of an object in the planning scene, make - the object attached to a link of the robot. If no link name is - specified, the end-effector is used. If there is no - end-effector, the first link in the group is used. If the object - name does not exist an error will be produced in move_group, but - the request made by this interface will succeed. */ - bool attachObject(const std::string& object, const std::string& link = ""); - - /** \brief Given the name of an object in the planning scene, make - the object attached to a link of the robot. The set of links the - object is allowed to touch without considering that a collision - is specified by \e touch_links. If \e link is empty, the - end-effector link is used. If there is no end-effector, the - first link in the group is used. If the object name does not - exist an error will be produced in move_group, but the request - made by this interface will succeed. */ - bool attachObject(const std::string& object, const std::string& link, const std::vector& touch_links); - - /** \brief Detach an object. \e name specifies the name of the - object attached to this group, or the name of the link the - object is attached to. If there is no name specified, and there - is only one attached object, that object is detached. An error - is produced if no object to detach is identified. */ - bool detachObject(const std::string& name = ""); - - /**@}*/ - - /** - * \name Query current robot state - */ - /**@{*/ - - /** \brief When reasoning about the current state of a robot, a - CurrentStateMonitor instance is automatically constructed. This - function allows triggering the construction of that object from - the beginning, so that future calls to functions such as - getCurrentState() will not take so long and are less likely to fail. */ - bool startStateMonitor(double wait = 1.0); - - /** \brief Get the current joint values for the joints planned for by this instance (see getJoints()) */ - std::vector getCurrentJointValues(); - - /** \brief Get the current state of the robot within the duration specified by wait. */ - robot_state::RobotStatePtr getCurrentState(double wait = 1); - - /** \brief Get the pose for the end-effector \e end_effector_link. - If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is - assumed */ - geometry_msgs::PoseStamped getCurrentPose(const std::string& end_effector_link = ""); - - /** \brief Get the roll-pitch-yaw (XYZ) for the end-effector \e end_effector_link. - If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is - assumed */ - std::vector getCurrentRPY(const std::string& end_effector_link = ""); - - /** \brief Get random joint values for the joints planned for by this instance (see getJoints()) */ - std::vector getRandomJointValues(); - - /** \brief Get a random reachable pose for the end-effector \e end_effector_link. - If \e end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is - assumed */ - geometry_msgs::PoseStamped getRandomPose(const std::string& end_effector_link = ""); - - /**@}*/ - - /** - * \name Manage named joint configurations - */ - /**@{*/ - - /** \brief Remember the current joint values (of the robot being monitored) under \e name. - These can be used by setNamedTarget(). - These values are remembered locally in the client. Other clients will - not have access to them. */ - void rememberJointValues(const std::string& name); - - /** \brief Remember the specified joint values under \e name. - These can be used by setNamedTarget(). - These values are remembered locally in the client. Other clients will - not have access to them. */ - void rememberJointValues(const std::string& name, const std::vector& values); - - /** \brief Get the currently remembered map of names to joint values. */ - const std::map >& getRememberedJointValues() const - { - return remembered_joint_values_; - } - - /** \brief Forget the joint values remebered under \e name */ - void forgetJointValues(const std::string& name); - - /**@}*/ - - /** - * \name Manage planning constraints - */ - /**@{*/ - - /** \brief Get the actual set of constraints in use with this MoveItCpp. - @return A copy of the current path constraints set for this interface - */ - moveit_msgs::Constraints getPathConstraints() const; - - /** \brief Specify a set of path constraints to use. - This version does not require a database server. - This replaces any path constraints set in previous calls to setPathConstraints(). */ - void setPathConstraints(const moveit_msgs::Constraints& constraint); - - /** \brief Specify that no path constraints are to be used. - This removes any path constraints set in previous calls to setPathConstraints(). */ - void clearPathConstraints(); - - moveit_msgs::TrajectoryConstraints getTrajectoryConstraints() const; - void setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& constraint); - void clearTrajectoryConstraints(); - - /**@}*/ - -protected: - /** return the full RobotState of the joint-space target, only for internal use */ - const robot_state::RobotState& getTargetRobotState() const; - -private: - std::map > remembered_joint_values_; - // TODO(henningkayser): Refactor class and possibly remove impl concept - class MoveItCppImpl; - MoveItCppImpl* impl_; - - bool status() const; - - // New stuff - template - void waitForAction(const T& action, const std::string& name, const ros::WallTime& timeout, double allotted_time); - MoveItErrorCode move(bool wait); - void constructGoal(moveit_msgs::MoveGroupGoal& goal); - MoveItErrorCode execute(const Plan& plan, bool wait); - bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds); - bool setJointValueTarget(const geometry_msgs::Pose& eef_pose, const std::string& end_effector_link, - const std::string& frame, bool approx); - robot_state::RobotStatePtr getStartState(); - bool hasPoseTarget(const std::string& end_effector_link) const; - void clearContents(); - // void initializeConstraintsStorage(const std::string& host, unsigned int port); - - // context contents - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; - planning_pipeline::PlanningPipelinePtr planning_pipeline_; - plan_execution::PlanExecutionPtr plan_execution_; - plan_execution::PlanWithSensingPtr plan_with_sensing_; - bool allow_trajectory_execution_; - bool debug_; - - // Options - std::string group_name_; - std::string robot_description_; - ros::NodeHandle node_handle_; - std::shared_ptr tf_buffer_; - robot_model::RobotModelConstPtr robot_model_; - planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; - std::unique_ptr > move_action_client_; - std::unique_ptr > execute_action_client_; - - // general planning params - robot_state::RobotStatePtr considered_start_state_; - moveit_msgs::WorkspaceParameters workspace_parameters_; - double allowed_planning_time_; - std::string planner_id_; - unsigned int num_planning_attempts_; - double max_velocity_scaling_factor_; - double max_acceleration_scaling_factor_; - double goal_joint_tolerance_; - double goal_position_tolerance_; - double goal_orientation_tolerance_; - bool can_look_; - bool can_replan_; - double replan_delay_; - - // joint state goal - robot_state::RobotStatePtr joint_state_target_; - const robot_model::JointModelGroup* joint_model_group_; - - // pose goal; - // for each link we have a set of possible goal locations; - std::map > pose_targets_; - - // common properties for goals - ActiveTargetType active_target_; - std::unique_ptr path_constraints_; - std::unique_ptr trajectory_constraints_; - std::string end_effector_link_; - std::string pose_reference_frame_; - std::string support_surface_; - - // ROS communication - ros::Publisher trajectory_event_publisher_; - ros::Publisher attached_object_publisher_; - ros::ServiceClient query_service_; - ros::ServiceClient get_params_service_; - ros::ServiceClient set_params_service_; - ros::ServiceClient cartesian_path_service_; - ros::ServiceClient plan_grasps_service_; -}; -} // namespace planning_interface -} // namespace moveit - -#endif diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp deleted file mode 100644 index ee5c8baeec..0000000000 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ /dev/null @@ -1,1608 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, PickNik LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik LLC nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - - -/* Author: Simon Goldstien, Henning Kayser */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace moveit -{ -namespace planning_interface -{ -/// Name of the robot description (a param name, so it can be changed externally) -const std::string MoveItCpp::ROBOT_DESCRIPTION = "robot_description"; - -const std::string GRASP_PLANNING_SERVICE_NAME = "plan_grasps"; // name of the service that can be used to plan grasps - -MoveItCpp::MoveItCpp(const std::string& group_name, - const std::shared_ptr& tf_buffer, - const ros::WallDuration& wait_for_servers) - : MoveItCpp(Options(group_name), tf_buffer, wait_for_servers) -{ -} - -MoveItCpp::MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer, - const ros::WallDuration& wait_for_servers) - : node_handle_(opt.node_handle_), tf_buffer_(tf_buffer) -{ - group_name_ = opt.group_name_; - robot_description_ = opt.robot_description_; - robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_); - if (!robot_model_) - { - std::string error = "Unable to construct robot model. Please make sure all needed information is on the " - "parameter server."; - ROS_FATAL_STREAM_NAMED("move_group_interface", error); - throw std::runtime_error(error); - } - - if (!robot_model_->hasJointModelGroup(opt.group_name_)) - { - std::string error = "Group '" + opt.group_name_ + "' was not found."; - ROS_FATAL_STREAM_NAMED("move_group_interface", error); - throw std::runtime_error(error); - } - - joint_model_group_ = robot_model_->getJointModelGroup(opt.group_name_); - - joint_state_target_.reset(new robot_state::RobotState(robot_model_)); - joint_state_target_->setToDefaultValues(); - active_target_ = JOINT; - can_look_ = false; - can_replan_ = false; - replan_delay_ = 2.0; - goal_joint_tolerance_ = 1e-4; - goal_position_tolerance_ = 1e-4; // 0.1 mm - goal_orientation_tolerance_ = 1e-3; // ~0.1 deg - allowed_planning_time_ = 5.0; - num_planning_attempts_ = 1; - max_velocity_scaling_factor_ = 1.0; - max_acceleration_scaling_factor_ = 1.0; - - if (joint_model_group_->isChain()) - end_effector_link_ = joint_model_group_->getLinkModelNames().back(); - pose_reference_frame_ = robot_model_->getModelFrame(); - - trajectory_event_publisher_ = node_handle_.advertise( - trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false); - attached_object_publisher_ = node_handle_.advertise( - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1, false); - - current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_buffer_, node_handle_); - - ros::WallTime timeout_for_servers = ros::WallTime::now() + wait_for_servers; - if (wait_for_servers == ros::WallDuration()) - timeout_for_servers = ros::WallTime(); // wait for ever - double allotted_time = wait_for_servers.toSec(); - - move_action_client_.reset( - new actionlib::SimpleActionClient(node_handle_, move_group::MOVE_ACTION, false)); - waitForAction(move_action_client_, move_group::MOVE_ACTION, timeout_for_servers, allotted_time); - - execute_action_client_.reset(new actionlib::SimpleActionClient( - node_handle_, move_group::EXECUTE_ACTION_NAME, false)); - waitForAction(execute_action_client_, move_group::EXECUTE_ACTION_NAME, timeout_for_servers, allotted_time); - - query_service_ = - node_handle_.serviceClient(move_group::QUERY_PLANNERS_SERVICE_NAME); - get_params_service_ = - node_handle_.serviceClient(move_group::GET_PLANNER_PARAMS_SERVICE_NAME); - set_params_service_ = - node_handle_.serviceClient(move_group::SET_PLANNER_PARAMS_SERVICE_NAME); - - cartesian_path_service_ = - node_handle_.serviceClient(move_group::CARTESIAN_PATH_SERVICE_NAME); - - plan_grasps_service_ = node_handle_.serviceClient(GRASP_PLANNING_SERVICE_NAME); - - ROS_INFO_STREAM_NAMED("move_group_interface", "Ready to take commands for planning group " << opt.group_name_ << "."); - group_name_ = opt.group_name_; - robot_description_ = opt.robot_description_; -} - -MoveItCpp::~MoveItCpp() -{ - clearContents(); -} - -MoveItCpp::MoveItCpp(MoveItCpp&& other) -{ - other.clearContents(); -} - -MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) -{ - if (this != &other) - { - this->group_name_ = other.group_name_; - this->robot_description_ = other.robot_description_; - this->node_handle_ = other.node_handle_; - this->tf_buffer_ = other.tf_buffer_; - this->robot_model_ = other.robot_model_; - this->current_state_monitor_ = other.current_state_monitor_; - swap(this->move_action_client_, other.move_action_client_); - swap(this->execute_action_client_, other.execute_action_client_); - - this->considered_start_state_ = other.considered_start_state_; - this->workspace_parameters_ = other.workspace_parameters_; - this->allowed_planning_time_ = other.allowed_planning_time_; - this->planner_id_ = other.planner_id_; - this->num_planning_attempts_ = other.num_planning_attempts_; - this->max_velocity_scaling_factor_ = other.max_velocity_scaling_factor_; - this->max_acceleration_scaling_factor_ = other.max_acceleration_scaling_factor_; - this->goal_joint_tolerance_ = other.goal_joint_tolerance_; - this->goal_position_tolerance_ = other.goal_position_tolerance_; - this->goal_orientation_tolerance_ = other.goal_orientation_tolerance_; - this->can_look_ = other.can_look_; - this->can_replan_ = other.can_replan_; - this->replan_delay_ = other.replan_delay_; - - this->joint_state_target_ = other.joint_state_target_; - this->joint_model_group_ = other.joint_model_group_; - - this->pose_targets_ = other.pose_targets_; - - this->active_target_ = other.active_target_; - swap(this->path_constraints_, other.path_constraints_); - swap(this->trajectory_constraints_, other.trajectory_constraints_); - this->end_effector_link_ = other.end_effector_link_; - this->pose_reference_frame_ = other.pose_reference_frame_; - this->support_surface_ = other.support_surface_; - - this->trajectory_event_publisher_ = other.trajectory_event_publisher_; - this->attached_object_publisher_ = other.attached_object_publisher_; - this->query_service_ = other.query_service_; - this->get_params_service_ = other.get_params_service_; - this->set_params_service_ = other.set_params_service_; - this->cartesian_path_service_ = other.cartesian_path_service_; - this->plan_grasps_service_ = other.plan_grasps_service_; - - remembered_joint_values_ = std::move(other.remembered_joint_values_); - other.clearContents(); - } - - return *this; -} - -const std::string& MoveItCpp::getName() const -{ - return group_name_; -} - -const std::vector MoveItCpp::getNamedTargets() -{ - const robot_model::RobotModelConstPtr& robot = robot_model_; - const std::string& group = group_name_; - const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group); - - if (joint_group) - { - return joint_group->getDefaultStateNames(); - } - - std::vector empty; - return empty; -} - -robot_model::RobotModelConstPtr MoveItCpp::getRobotModel() const -{ - return robot_model_; -} - -const ros::NodeHandle& MoveItCpp::getNodeHandle() const -{ - return node_handle_; -} - -bool MoveItCpp::getInterfaceDescription(moveit_msgs::PlannerInterfaceDescription& desc) -{ - moveit_msgs::QueryPlannerInterfaces::Request req; - moveit_msgs::QueryPlannerInterfaces::Response res; - if (query_service_.call(req, res)) - if (!res.planner_interfaces.empty()) - { - desc = res.planner_interfaces.front(); - return true; - } - return false; -} - -std::map -MoveItCpp::getPlannerParams(const std::string& planner_id, const std::string& group) -{ - moveit_msgs::GetPlannerParams::Request req; - moveit_msgs::GetPlannerParams::Response res; - req.planner_config = planner_id; - req.group = group; - std::map result; - if (get_params_service_.call(req, res)) - { - for (unsigned int i = 0, end = res.params.keys.size(); i < end; ++i) - result[res.params.keys[i]] = res.params.values[i]; - } - return result; -} - -void MoveItCpp::setPlannerParams(const std::string& planner_id, const std::string& group, - const std::map& params, - bool replace) -{ - moveit_msgs::SetPlannerParams::Request req; - moveit_msgs::SetPlannerParams::Response res; - req.planner_config = planner_id; - req.group = group; - req.replace = replace; - for (const std::pair& param : params) - { - req.params.keys.push_back(param.first); - req.params.values.push_back(param.second); - } - set_params_service_.call(req, res); -} - -std::string MoveItCpp::getDefaultPlannerId(const std::string& group) const -{ - std::stringstream param_name; - param_name << "move_group"; - if (!group.empty()) - param_name << "/" << group; - param_name << "/default_planner_config"; - - std::string default_planner_config; - node_handle_.getParam(param_name.str(), default_planner_config); - return default_planner_config; -} - -void MoveItCpp::setPlannerId(const std::string& planner_id) -{ - planner_id_ = planner_id; -} - -const std::string& MoveItCpp::getPlannerId() const -{ - return planner_id_; -} - -void MoveItCpp::setNumPlanningAttempts(unsigned int num_planning_attempts) -{ - num_planning_attempts_ = num_planning_attempts; -} - -void MoveItCpp::setMaxVelocityScalingFactor(double max_velocity_scaling_factor) -{ - max_velocity_scaling_factor_ = max_velocity_scaling_factor; -} - -void MoveItCpp::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) -{ - max_acceleration_scaling_factor_ = max_acceleration_scaling_factor; -} - -MoveItErrorCode MoveItCpp::asyncMove() -{ - return move(false); -} - -actionlib::SimpleActionClient& -MoveItCpp::getMoveGroupClient() const -{ - return *move_action_client_; -} - -MoveItErrorCode MoveItCpp::move() -{ - return move(true); -} - -MoveItErrorCode MoveItCpp::asyncExecute(const Plan& plan) -{ - return execute(plan, false); -} - -MoveItErrorCode MoveItCpp::execute(const Plan& plan) -{ - return execute(plan, true); -} - -MoveItErrorCode MoveItCpp::plan(Plan& plan) -{ - if (!move_action_client_) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - if (!move_action_client_->isServerConnected()) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - - moveit_msgs::MoveGroupGoal goal; - constructGoal(goal); - goal.planning_options.plan_only = true; - goal.planning_options.look_around = false; - goal.planning_options.replan = false; - goal.planning_options.planning_scene_diff.is_diff = true; - goal.planning_options.planning_scene_diff.robot_state.is_diff = true; - - move_action_client_->sendGoal(goal); - if (!move_action_client_->waitForResult()) - { - ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); - } - if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) - { - plan.trajectory_ = move_action_client_->getResult()->planned_trajectory; - plan.start_state_ = move_action_client_->getResult()->trajectory_start; - plan.planning_time_ = move_action_client_->getResult()->planning_time; - return MoveItErrorCode(move_action_client_->getResult()->error_code); - } - else - { - ROS_WARN_STREAM_NAMED("move_group_interface", "Fail: " << move_action_client_->getState().toString() << ": " - << move_action_client_->getState().getText()); - return MoveItErrorCode(move_action_client_->getResult()->error_code); - } -} - -double MoveItCpp::computeCartesianPath(const std::vector& waypoints, - double eef_step, double jump_threshold, - moveit_msgs::RobotTrajectory& trajectory, - bool avoid_collisions, - moveit_msgs::MoveItErrorCodes* error_code) -{ - moveit_msgs::Constraints path_constraints_tmp; - return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions, - error_code); -} - -double MoveItCpp::computeCartesianPath(const std::vector& waypoints, - double eef_step, double jump_threshold, - moveit_msgs::RobotTrajectory& trajectory, - const moveit_msgs::Constraints& path_constraints, - bool avoid_collisions, - moveit_msgs::MoveItErrorCodes* error_code) -{ - moveit_msgs::GetCartesianPath::Request req; - moveit_msgs::GetCartesianPath::Response res; - - if (considered_start_state_) - robot_state::robotStateToRobotStateMsg(*considered_start_state_, req.start_state); - else - req.start_state.is_diff = true; - - req.group_name = group_name_; - req.header.frame_id = pose_reference_frame_; - req.header.stamp = ros::Time::now(); - req.waypoints = waypoints; - req.max_step = eef_step; - req.jump_threshold = jump_threshold; - req.path_constraints = path_constraints; - req.avoid_collisions = avoid_collisions; - req.link_name = end_effector_link_; - - if (cartesian_path_service_.call(req, res)) - { - if (error_code) - *error_code = res.error_code; - if (res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) - { - trajectory = res.solution; - return res.fraction; - } - else - { - return -1.0; - } - } - else - { - error_code->val = error_code->FAILURE; - return -1.0; - } -} - -void MoveItCpp::stop() -{ - if (trajectory_event_publisher_) - { - std_msgs::String event; - event.data = "stop"; - trajectory_event_publisher_.publish(event); - } -} - -void MoveItCpp::setStartState(const moveit_msgs::RobotState& start_state) -{ - robot_state::RobotStatePtr rs; - getCurrentState(rs, 1.0); - robot_state::robotStateMsgToRobotState(start_state, *rs); - setStartState(*rs); -} - -void MoveItCpp::setStartState(const robot_state::RobotState& start_state) -{ - considered_start_state_.reset(new robot_state::RobotState(start_state)); -} - -void MoveItCpp::setStartStateToCurrentState() -{ - considered_start_state_.reset(); -} - -void MoveItCpp::setRandomTarget() -{ - joint_state_target_->setToRandomPositions(); - active_target_ = JOINT; -} - -const std::vector& MoveItCpp::getJointNames() -{ - return joint_model_group_->getVariableNames(); -} - -const std::vector& MoveItCpp::getLinkNames() -{ - return joint_model_group_->getLinkModelNames(); -} - -std::map MoveItCpp::getNamedTargetValues(const std::string& name) -{ - std::map >::const_iterator it = remembered_joint_values_.find(name); - std::map positions; - - if (it != remembered_joint_values_.end()) - { - std::vector names = joint_model_group_->getVariableNames(); - for (size_t x = 0; x < names.size(); ++x) - { - positions[names[x]] = it->second[x]; - } - } - else - { - joint_model_group_->getVariableDefaultPositions(name, positions); - } - return positions; -} - -bool MoveItCpp::setNamedTarget(const std::string& name) -{ - std::map >::const_iterator it = remembered_joint_values_.find(name); - if (it != remembered_joint_values_.end()) - { - return setJointValueTarget(it->second); - } - else - { - if (joint_state_target_->setToDefaultValues(joint_model_group_, name)) - { - active_target_ = JOINT; - return true; - } - ROS_ERROR_NAMED("move_group_interface", "The requested named target '%s' does not exist", name.c_str()); - return false; - } -} - -void MoveItCpp::getJointValueTarget(std::vector& group_variable_values) const -{ - joint_state_target_->copyJointGroupPositions(joint_model_group_, group_variable_values); -} - -bool MoveItCpp::setJointValueTarget(const std::vector& joint_values) -{ - if (joint_values.size() != joint_model_group_->getVariableCount()) - return false; - active_target_ = JOINT; - joint_state_target_->setJointGroupPositions(joint_model_group_, joint_values); - return joint_state_target_->satisfiesBounds(joint_model_group_, goal_joint_tolerance_); -} - -bool MoveItCpp::setJointValueTarget(const std::map& variable_values) -{ - const auto& allowed = joint_model_group_->getVariableNames(); - for (const auto& pair : variable_values) - { - if (std::find(allowed.begin(), allowed.end(), pair.first) == allowed.end()) - { - ROS_ERROR_STREAM("joint variable " << pair.first << " is not part of group " << joint_model_group_->getName()); - return false; - } - } - - active_target_ = JOINT; - joint_state_target_->setVariablePositions(variable_values); - return joint_state_target_->satisfiesBounds(goal_joint_tolerance_); -} - -bool MoveItCpp::setJointValueTarget(const std::vector& variable_names, - const std::vector& variable_values) -{ - const auto& allowed = joint_model_group_->getVariableNames(); - for (const auto& variable_name : variable_names) - { - if (std::find(allowed.begin(), allowed.end(), variable_name) == allowed.end()) - { - ROS_ERROR_STREAM("joint variable " << variable_name << " is not part of group " << joint_model_group_->getName()); - return false; - } - } - - active_target_ = JOINT; - joint_state_target_->setVariablePositions(variable_names, variable_values); - return joint_state_target_->satisfiesBounds(goal_joint_tolerance_); -} - -bool MoveItCpp::setJointValueTarget(const robot_state::RobotState& rstate) -{ - active_target_ = JOINT; - *joint_state_target_ = rstate; - return joint_state_target_->satisfiesBounds(goal_joint_tolerance_); -} - -bool MoveItCpp::setJointValueTarget(const std::string& joint_name, double value) -{ - std::vector values(1, value); - return setJointValueTarget(joint_name, values); -} - -bool MoveItCpp::setJointValueTarget(const std::string& joint_name, - const std::vector& values) -{ - active_target_ = JOINT; - const robot_model::JointModel* jm = joint_model_group_->getJointModel(joint_name); - if (jm && jm->getVariableCount() == values.size()) - { - joint_state_target_->setJointPositions(jm, values); - return joint_state_target_->satisfiesBounds(jm, goal_joint_tolerance_); - } - - ROS_ERROR_STREAM("joint " << joint_name << " is not part of group " << joint_model_group_->getName()); - return false; -} - -bool MoveItCpp::setJointValueTarget(const sensor_msgs::JointState& state) -{ - return setJointValueTarget(state.name, state.position); -} - -bool MoveItCpp::setJointValueTarget(const geometry_msgs::Pose& eef_pose, - const std::string& end_effector_link) -{ - return setJointValueTarget(eef_pose, end_effector_link, "", false); -} - -bool MoveItCpp::setJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, - const std::string& end_effector_link) -{ - return setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, false); -} - -bool MoveItCpp::setJointValueTarget(const Eigen::Isometry3d& eef_pose, - const std::string& end_effector_link) -{ - geometry_msgs::Pose msg = tf2::toMsg(eef_pose); - return setJointValueTarget(msg, end_effector_link); -} - -bool MoveItCpp::setApproximateJointValueTarget(const geometry_msgs::Pose& eef_pose, - const std::string& end_effector_link) -{ - return setJointValueTarget(eef_pose, end_effector_link, "", true); -} - -bool MoveItCpp::setApproximateJointValueTarget(const geometry_msgs::PoseStamped& eef_pose, - const std::string& end_effector_link) -{ - return setJointValueTarget(eef_pose.pose, end_effector_link, eef_pose.header.frame_id, true); -} - -bool MoveItCpp::setApproximateJointValueTarget(const Eigen::Isometry3d& eef_pose, - const std::string& end_effector_link) -{ - geometry_msgs::Pose msg = tf2::toMsg(eef_pose); - return setApproximateJointValueTarget(msg, end_effector_link); -} - -const robot_state::RobotState& MoveItCpp::getJointValueTarget() const -{ - return *joint_state_target_; -} - -const robot_state::RobotState& MoveItCpp::getTargetRobotState() const -{ - return *joint_state_target_; -} - -const std::string& MoveItCpp::getEndEffectorLink() const -{ - return end_effector_link_; -} - -const std::string& MoveItCpp::getEndEffector() const -{ - if (!end_effector_link_.empty()) - { - const std::vector& possible_eefs = - robot_model_->getJointModelGroup(group_name_)->getAttachedEndEffectorNames(); - for (const std::string& possible_eef : possible_eefs) - if (robot_model_->getEndEffector(possible_eef)->hasLinkModel(end_effector_link_)) - return possible_eef; - } - static std::string empty; - return empty; -} - -bool MoveItCpp::setEndEffectorLink(const std::string& link_name) -{ - if (end_effector_link_.empty() || link_name.empty()) - return false; - end_effector_link_ = link_name; - active_target_ = POSE; - return true; -} - -bool MoveItCpp::setEndEffector(const std::string& eef_name) -{ - const robot_model::JointModelGroup* jmg = robot_model_->getEndEffector(eef_name); - if (jmg) - return setEndEffectorLink(jmg->getEndEffectorParentGroup().second); - return false; -} - -void MoveItCpp::clearPoseTarget(const std::string& end_effector_link) -{ - pose_targets_.erase(end_effector_link); -} - -void MoveItCpp::clearPoseTargets() -{ - pose_targets_.clear(); -} - -bool MoveItCpp::setPoseTarget(const Eigen::Isometry3d& pose, - const std::string& end_effector_link) -{ - std::vector pose_msg(1); - pose_msg[0].pose = tf2::toMsg(pose); - pose_msg[0].header.frame_id = getPoseReferenceFrame(); - pose_msg[0].header.stamp = ros::Time::now(); - return setPoseTargets(pose_msg, end_effector_link); -} - -bool MoveItCpp::setPoseTarget(const geometry_msgs::Pose& target, - const std::string& end_effector_link) -{ - std::vector pose_msg(1); - pose_msg[0].pose = target; - pose_msg[0].header.frame_id = getPoseReferenceFrame(); - pose_msg[0].header.stamp = ros::Time::now(); - return setPoseTargets(pose_msg, end_effector_link); -} - -bool MoveItCpp::setPoseTarget(const geometry_msgs::PoseStamped& target, - const std::string& end_effector_link) -{ - std::vector targets(1, target); - return setPoseTargets(targets, end_effector_link); -} - -bool MoveItCpp::setPoseTargets(const EigenSTL::vector_Isometry3d& target, - const std::string& end_effector_link) -{ - std::vector pose_out(target.size()); - ros::Time tm = ros::Time::now(); - const std::string& frame_id = getPoseReferenceFrame(); - for (std::size_t i = 0; i < target.size(); ++i) - { - pose_out[i].pose = tf2::toMsg(target[i]); - pose_out[i].header.stamp = tm; - pose_out[i].header.frame_id = frame_id; - } - return setPoseTargets(pose_out, end_effector_link); -} - -bool MoveItCpp::setPoseTargets(const std::vector& target, - const std::string& end_effector_link) -{ - std::vector target_stamped(target.size()); - ros::Time tm = ros::Time::now(); - const std::string& frame_id = getPoseReferenceFrame(); - for (std::size_t i = 0; i < target.size(); ++i) - { - target_stamped[i].pose = target[i]; - target_stamped[i].header.stamp = tm; - target_stamped[i].header.frame_id = frame_id; - } - return setPoseTargets(target_stamped, end_effector_link); -} - -bool MoveItCpp::setPoseTargets(const std::vector& target, - const std::string& end_effector_link) -{ - if (target.empty()) - { - ROS_ERROR_NAMED("move_group_interface", "No pose specified as goal target"); - return false; - } - else - { - active_target_ = POSE; - const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; - if (eef.empty()) - { - ROS_ERROR_NAMED("move_group_interface", "No end-effector to set the pose for"); - return false; - } - else - { - pose_targets_[eef] = target; - // make sure we don't store an actual stamp, since that will become stale can potentially cause tf errors - std::vector& stored_poses = pose_targets_[eef]; - for (geometry_msgs::PoseStamped& stored_pose : stored_poses) - stored_pose.header.stamp = ros::Time(0); - } - return true; - } -} - -const geometry_msgs::PoseStamped& -MoveItCpp::getPoseTarget(const std::string& end_effector_link) const -{ - const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; - - // if multiple pose targets are set, return the first one - std::map >::const_iterator jt = pose_targets_.find(eef); - if (jt != pose_targets_.end()) - if (!jt->second.empty()) - return jt->second.at(0); - - // or return an error - static const geometry_msgs::PoseStamped UNKNOWN; - ROS_ERROR_NAMED("move_group_interface", "Pose for end-effector '%s' not known.", eef.c_str()); - return UNKNOWN; -} - -const std::vector& -MoveItCpp::getPoseTargets(const std::string& end_effector_link) const -{ - const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; - - std::map >::const_iterator jt = pose_targets_.find(eef); - if (jt != pose_targets_.end()) - if (!jt->second.empty()) - return jt->second; - - // or return an error - static const std::vector EMPTY; - ROS_ERROR_NAMED("move_group_interface", "Poses for end-effector '%s' are not known.", eef.c_str()); - return EMPTY; -} - -namespace -{ -inline void transformPose(const tf2_ros::Buffer& tf_buffer, const std::string& desired_frame, - geometry_msgs::PoseStamped& target) -{ - if (desired_frame != target.header.frame_id) - { - geometry_msgs::PoseStamped target_in(target); - tf_buffer.transform(target_in, target, desired_frame); - // we leave the stamp to ros::Time(0) on purpose - target.header.stamp = ros::Time(0); - } -} -} // namespace - -bool MoveItCpp::setPositionTarget(double x, double y, double z, - const std::string& end_effector_link) -{ - geometry_msgs::PoseStamped target; - if (hasPoseTarget(end_effector_link)) - { - target = getPoseTarget(end_effector_link); - transformPose(*tf_buffer_, pose_reference_frame_, target); - } - else - { - target.pose.orientation.x = 0.0; - target.pose.orientation.y = 0.0; - target.pose.orientation.z = 0.0; - target.pose.orientation.w = 1.0; - target.header.frame_id = pose_reference_frame_; - } - - target.pose.position.x = x; - target.pose.position.y = y; - target.pose.position.z = z; - bool result = setPoseTarget(target, end_effector_link); - active_target_ = POSITION; - return result; -} - -bool MoveItCpp::setRPYTarget(double r, double p, double y, - const std::string& end_effector_link) -{ - geometry_msgs::PoseStamped target; - if (hasPoseTarget(end_effector_link)) - { - target = getPoseTarget(end_effector_link); - transformPose(*tf_buffer_, pose_reference_frame_, target); - } - else - { - target.pose.position.x = 0.0; - target.pose.position.y = 0.0; - target.pose.position.z = 0.0; - target.header.frame_id = pose_reference_frame_; - } - tf2::Quaternion q; - q.setRPY(r, p, y); - target.pose.orientation = tf2::toMsg(q); - bool result = setPoseTarget(target, end_effector_link); - active_target_ = ORIENTATION; - return result; -} - -bool MoveItCpp::setOrientationTarget(double x, double y, double z, double w, - const std::string& end_effector_link) -{ - geometry_msgs::PoseStamped target; - if (hasPoseTarget(end_effector_link)) - { - target = getPoseTarget(end_effector_link); - transformPose(*tf_buffer_, pose_reference_frame_, target); - } - else - { - target.pose.position.x = 0.0; - target.pose.position.y = 0.0; - target.pose.position.z = 0.0; - target.header.frame_id = pose_reference_frame_; - } - - target.pose.orientation.x = x; - target.pose.orientation.y = y; - target.pose.orientation.z = z; - target.pose.orientation.w = w; - bool result = setPoseTarget(target, end_effector_link); - active_target_ = ORIENTATION; - return result; -} - -void MoveItCpp::setPoseReferenceFrame(const std::string& pose_reference_frame) -{ - pose_reference_frame_ = pose_reference_frame; -} - -const std::string& MoveItCpp::getPoseReferenceFrame() const -{ - return pose_reference_frame_; -} - -double MoveItCpp::getGoalJointTolerance() const -{ - return goal_joint_tolerance_; -} - -double MoveItCpp::getGoalPositionTolerance() const -{ - return goal_position_tolerance_; -} - -double MoveItCpp::getGoalOrientationTolerance() const -{ - return goal_orientation_tolerance_; -} - -void MoveItCpp::setGoalTolerance(double tolerance) -{ - setGoalJointTolerance(tolerance); - setGoalPositionTolerance(tolerance); - setGoalOrientationTolerance(tolerance); -} - -void MoveItCpp::setGoalJointTolerance(double tolerance) -{ - goal_joint_tolerance_ = tolerance; -} - -void MoveItCpp::setGoalPositionTolerance(double tolerance) -{ - goal_position_tolerance_ = tolerance; -} - -void MoveItCpp::setGoalOrientationTolerance(double tolerance) -{ - goal_orientation_tolerance_ = tolerance; -} - -void MoveItCpp::rememberJointValues(const std::string& name) -{ - rememberJointValues(name, getCurrentJointValues()); -} - -bool MoveItCpp::startStateMonitor(double wait) -{ - if (!current_state_monitor_) - { - ROS_ERROR_NAMED("move_group_interface", "Unable to monitor current robot state"); - return false; - } - - // if needed, start the monitor and wait up to 1 second for a full robot state - if (!current_state_monitor_->isActive()) - current_state_monitor_->startStateMonitor(); - - current_state_monitor_->waitForCompleteState(group_name_, wait); - return true; -} - -std::vector MoveItCpp::getCurrentJointValues() -{ - robot_state::RobotStatePtr current_state; - std::vector values; - if (getCurrentState(current_state, 1.0)) - current_state->copyJointGroupPositions(group_name_, values); - return values; -} - -std::vector MoveItCpp::getRandomJointValues() -{ - std::vector r; - joint_model_group_->getVariableRandomPositions(joint_state_target_->getRandomNumberGenerator(), r); - return r; -} - -geometry_msgs::PoseStamped MoveItCpp::getRandomPose(const std::string& end_effector_link) -{ - const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; - Eigen::Isometry3d pose; - pose.setIdentity(); - if (eef.empty()) - ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); - else - { - robot_state::RobotStatePtr current_state; - if (getCurrentState(current_state, 1.0)) - { - current_state->setToRandomPositions(joint_model_group_); - const robot_model::LinkModel* lm = current_state->getLinkModel(eef); - if (lm) - pose = current_state->getGlobalLinkTransform(lm); - } - } - geometry_msgs::PoseStamped pose_msg; - pose_msg.header.stamp = ros::Time::now(); - pose_msg.header.frame_id = robot_model_->getModelFrame(); - pose_msg.pose = tf2::toMsg(pose); - return pose_msg; -} - -geometry_msgs::PoseStamped MoveItCpp::getCurrentPose(const std::string& end_effector_link) -{ - const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; - Eigen::Isometry3d pose; - pose.setIdentity(); - if (eef.empty()) - ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); - else - { - robot_state::RobotStatePtr current_state; - if (getCurrentState(current_state, 1.0)) - { - const robot_model::LinkModel* lm = current_state->getLinkModel(eef); - if (lm) - pose = current_state->getGlobalLinkTransform(lm); - } - } - geometry_msgs::PoseStamped pose_msg; - pose_msg.header.stamp = ros::Time::now(); - pose_msg.header.frame_id = robot_model_->getModelFrame(); - pose_msg.pose = tf2::toMsg(pose); - return pose_msg; -} - -std::vector MoveItCpp::getCurrentRPY(const std::string& end_effector_link) -{ - std::vector result; - const std::string& eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link; - if (eef.empty()) - ROS_ERROR_NAMED("move_group_interface", "No end-effector specified"); - else - { - robot_state::RobotStatePtr current_state; - if (getCurrentState(current_state, 1.0)) - { - const robot_model::LinkModel* lm = current_state->getLinkModel(eef); - if (lm) - { - result.resize(3); - geometry_msgs::TransformStamped tfs = tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm)); - double pitch, roll, yaw; - tf2::getEulerYPR(tfs.transform.rotation, yaw, pitch, roll); - result[0] = roll; - result[1] = pitch; - result[2] = yaw; - } - } - } - return result; -} - -const std::vector& MoveItCpp::getActiveJoints() const -{ - return joint_model_group_->getActiveJointModelNames(); -} - -const std::vector& MoveItCpp::getJoints() const -{ - return joint_model_group_->getJointModelNames(); -} - -unsigned int MoveItCpp::getVariableCount() const -{ - return joint_model_group_->getVariableCount(); -} - -robot_state::RobotStatePtr MoveItCpp::getCurrentState(double wait) -{ - robot_state::RobotStatePtr current_state; - getCurrentState(current_state, wait); - return current_state; -} - -void MoveItCpp::rememberJointValues(const std::string& name, - const std::vector& values) -{ - remembered_joint_values_[name] = values; -} - -void MoveItCpp::forgetJointValues(const std::string& name) -{ - remembered_joint_values_.erase(name); -} - -void MoveItCpp::allowLooking(bool flag) -{ - can_look_ = flag; - ROS_INFO_NAMED("move_group_interface", "Looking around: %s", can_look_ ? "yes" : "no"); -} - -void MoveItCpp::allowReplanning(bool flag) -{ - can_replan_ = flag; - ROS_INFO_NAMED("move_group_interface", "Replanning: %s", can_replan_ ? "yes" : "no"); -} - -moveit_msgs::Constraints MoveItCpp::getPathConstraints() const -{ - if (path_constraints_) - return *path_constraints_; - else - return moveit_msgs::Constraints(); -} - -void MoveItCpp::setPathConstraints(const moveit_msgs::Constraints& constraint) -{ - path_constraints_.reset(new moveit_msgs::Constraints(constraint)); -} - -void MoveItCpp::clearPathConstraints() -{ - path_constraints_.reset(); -} - -moveit_msgs::TrajectoryConstraints MoveItCpp::getTrajectoryConstraints() const -{ - if (trajectory_constraints_) - return *trajectory_constraints_; - else - return moveit_msgs::TrajectoryConstraints(); -} - -void MoveItCpp::setTrajectoryConstraints( - const moveit_msgs::TrajectoryConstraints& constraint) -{ - trajectory_constraints_.reset(new moveit_msgs::TrajectoryConstraints(constraint)); -} - -void MoveItCpp::clearTrajectoryConstraints() -{ - trajectory_constraints_.reset(); -} - -void MoveItCpp::setWorkspace(double minx, double miny, double minz, double maxx, - double maxy, double maxz) -{ - workspace_parameters_.header.frame_id = robot_model_->getModelFrame(); - workspace_parameters_.header.stamp = ros::Time::now(); - workspace_parameters_.min_corner.x = minx; - workspace_parameters_.min_corner.y = miny; - workspace_parameters_.min_corner.z = minz; - workspace_parameters_.max_corner.x = maxx; - workspace_parameters_.max_corner.y = maxy; - workspace_parameters_.max_corner.z = maxz; -} - -/** \brief Set time allowed to planner to solve problem before aborting */ -void MoveItCpp::setPlanningTime(double seconds) -{ - if (seconds > 0.0) - allowed_planning_time_ = seconds; -} - -/** \brief Get time allowed to planner to solve problem before aborting */ -double MoveItCpp::getPlanningTime() const -{ - return allowed_planning_time_; -} - -const std::string& MoveItCpp::getPlanningFrame() const -{ - return robot_model_->getModelFrame(); -} - -const std::vector& MoveItCpp::getJointModelGroupNames() const -{ - return robot_model_->getJointModelGroupNames(); -} - -bool MoveItCpp::attachObject(const std::string& object, const std::string& link) -{ - return attachObject(object, link, std::vector()); -} - -bool MoveItCpp::attachObject(const std::string& object, const std::string& link, - const std::vector& touch_links) -{ - std::string l = link.empty() ? getEndEffectorLink() : link; - if (l.empty()) - { - const std::vector& links = joint_model_group_->getLinkModelNames(); - if (!links.empty()) - l = links[0]; - } - if (l.empty()) - { - ROS_ERROR_NAMED("move_group_interface", "No known link to attach object '%s' to", object.c_str()); - return false; - } - moveit_msgs::AttachedCollisionObject aco; - aco.object.id = object; - aco.link_name.swap(l); - if (touch_links.empty()) - aco.touch_links.push_back(aco.link_name); - else - aco.touch_links = touch_links; - aco.object.operation = moveit_msgs::CollisionObject::ADD; - attached_object_publisher_.publish(aco); - return true; -} - -bool MoveItCpp::detachObject(const std::string& name) -{ - moveit_msgs::AttachedCollisionObject aco; - // if name is a link - if (!name.empty() && joint_model_group_->hasLinkModel(name)) - aco.link_name = name; - else - aco.object.id = name; - aco.object.operation = moveit_msgs::CollisionObject::REMOVE; - if (aco.link_name.empty() && aco.object.id.empty()) - { - // we only want to detach objects for this group - const std::vector& lnames = joint_model_group_->getLinkModelNames(); - for (const std::string& lname : lnames) - { - aco.link_name = lname; - attached_object_publisher_.publish(aco); - } - } - else - attached_object_publisher_.publish(aco); - return true; -} - -void MoveItCpp::constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request) -{ - request.group_name = group_name_; - request.num_planning_attempts = num_planning_attempts_; - request.max_velocity_scaling_factor = max_velocity_scaling_factor_; - request.max_acceleration_scaling_factor = max_acceleration_scaling_factor_; - request.allowed_planning_time = allowed_planning_time_; - request.planner_id = planner_id_; - request.workspace_parameters = workspace_parameters_; - - if (considered_start_state_) - robot_state::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); - else - request.start_state.is_diff = true; - - if (active_target_ == JOINT) - { - request.goal_constraints.resize(1); - request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( - getTargetRobotState(), joint_model_group_, goal_joint_tolerance_); - } - else if (active_target_ == POSE || active_target_ == POSITION || active_target_ == ORIENTATION) - { - // find out how many goals are specified - std::size_t goal_count = 0; - for (std::map >::const_iterator it = pose_targets_.begin(); - it != pose_targets_.end(); ++it) - goal_count = std::max(goal_count, it->second.size()); - - // start filling the goals; - // each end effector has a number of possible poses (K) as valid goals - // but there could be multiple end effectors specified, so we want each end effector - // to reach the goal that corresponds to the goals of the other end effectors - request.goal_constraints.resize(goal_count); - - for (std::map >::const_iterator it = pose_targets_.begin(); - it != pose_targets_.end(); ++it) - { - for (std::size_t i = 0; i < it->second.size(); ++i) - { - moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints( - it->first, it->second[i], goal_position_tolerance_, goal_orientation_tolerance_); - if (active_target_ == ORIENTATION) - c.position_constraints.clear(); - if (active_target_ == POSITION) - c.orientation_constraints.clear(); - request.goal_constraints[i] = kinematic_constraints::mergeConstraints(request.goal_constraints[i], c); - } - } - } - else - ROS_ERROR_NAMED("move_group_interface", "Unable to construct MotionPlanRequest representation"); - - if (path_constraints_) - request.path_constraints = *path_constraints_; - if (trajectory_constraints_) - request.trajectory_constraints = *trajectory_constraints_; -} - -template -void MoveItCpp::waitForAction(const T& action, const std::string& name, - const ros::WallTime& timeout, double allotted_time) -{ - ROS_DEBUG_NAMED("move_group_interface", "Waiting for move_group action server (%s)...", name.c_str()); - - // wait for the server (and spin as needed) - if (timeout == ros::WallTime()) // wait forever - { - while (node_handle_.ok() && !action->isServerConnected()) - { - ros::WallDuration(0.001).sleep(); - // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client - ros::CallbackQueue* queue = dynamic_cast(node_handle_.getCallbackQueue()); - if (queue) - { - queue->callAvailable(); - } - else // in case of nodelets and specific callback queue implementations - { - ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " - "handling."); - } - } - } - else // wait with timeout - { - while (node_handle_.ok() && !action->isServerConnected() && timeout > ros::WallTime::now()) - { - ros::WallDuration(0.001).sleep(); - // explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client - ros::CallbackQueue* queue = dynamic_cast(node_handle_.getCallbackQueue()); - if (queue) - { - queue->callAvailable(); - } - else // in case of nodelets and specific callback queue implementations - { - ROS_WARN_ONCE_NAMED("move_group_interface", "Non-default CallbackQueue: Waiting for external queue " - "handling."); - } - } - } - - if (!action->isServerConnected()) - { - std::stringstream error; - error << "Unable to connect to move_group action server '" << name << "' within allotted time (" << allotted_time - << "s)"; - throw std::runtime_error(error.str()); - } - else - { - ROS_DEBUG_NAMED("move_group_interface", "Connected to '%s'", name.c_str()); - } -} - -MoveItErrorCode MoveItCpp::move(bool wait) -{ - if (!move_action_client_) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - if (!move_action_client_->isServerConnected()) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - - moveit_msgs::MoveGroupGoal goal; - constructGoal(goal); - goal.planning_options.plan_only = false; - goal.planning_options.look_around = can_look_; - goal.planning_options.replan = can_replan_; - goal.planning_options.replan_delay = replan_delay_; - goal.planning_options.planning_scene_diff.is_diff = true; - goal.planning_options.planning_scene_diff.robot_state.is_diff = true; - - move_action_client_->sendGoal(goal); - if (!wait) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); - } - - if (!move_action_client_->waitForResult()) - { - ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); - } - if (!move_action_client_->waitForResult()) - { - ROS_INFO_STREAM_NAMED("move_group_interface", "MoveGroup action returned early"); - } - - if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) - { - return MoveItErrorCode(move_action_client_->getResult()->error_code); - } - else - { - ROS_INFO_STREAM_NAMED("move_group_interface", move_action_client_->getState().toString() - << ": " << move_action_client_->getState().getText()); - return MoveItErrorCode(move_action_client_->getResult()->error_code); - } -} - -void MoveItCpp::constructGoal(moveit_msgs::MoveGroupGoal& goal) -{ - constructMotionPlanRequest(goal.request); -} - -MoveItErrorCode MoveItCpp::execute(const Plan& plan, bool wait) -{ - if (!execute_action_client_->isServerConnected()) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); - } - - moveit_msgs::ExecuteTrajectoryGoal goal; - goal.trajectory = plan.trajectory_; - - execute_action_client_->sendGoal(goal); - if (!wait) - { - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); - } - - if (!execute_action_client_->waitForResult()) - { - ROS_INFO_STREAM_NAMED("move_group_interface", "ExecuteTrajectory action returned early"); - } - - if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) - { - return MoveItErrorCode(execute_action_client_->getResult()->error_code); - } - else - { - ROS_INFO_STREAM_NAMED("move_group_interface", execute_action_client_->getState().toString() - << ": " << execute_action_client_->getState().getText()); - return MoveItErrorCode(execute_action_client_->getResult()->error_code); - } -} - -bool MoveItCpp::getCurrentState(robot_state::RobotStatePtr& current_state, - double wait_seconds) -{ - if (!current_state_monitor_) - { - ROS_ERROR_NAMED("move_group_interface", "Unable to get current robot state"); - return false; - } - - // if needed, start the monitor and wait up to 1 second for a full robot state - if (!current_state_monitor_->isActive()) - current_state_monitor_->startStateMonitor(); - - if (!current_state_monitor_->waitForCurrentState(ros::Time::now(), wait_seconds)) - { - ROS_ERROR_NAMED("move_group_interface", "Failed to fetch current robot state"); - return false; - } - - current_state = current_state_monitor_->getCurrentState(); - return true; -} - -bool MoveItCpp::setJointValueTarget(const geometry_msgs::Pose& eef_pose, - const std::string& end_effector_link, - const std::string& frame, bool approx) -{ - const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; - // this only works if we have an end-effector - if (!eef.empty()) - { - // first we set the goal to be the same as the start state - moveit::core::RobotStatePtr c = getStartState(); - if (c) - { - active_target_ = JOINT; - c->enforceBounds(); - *joint_state_target_ = *c; - if (!joint_state_target_->satisfiesBounds(goal_joint_tolerance_)) - return false; - } - else - return false; - - // we may need to do approximate IK - kinematics::KinematicsQueryOptions o; - o.return_approximate_solution = approx; - - // if no frame transforms are needed, call IK directly - if (frame.empty() || moveit::core::Transforms::sameFrame(frame, robot_model_->getModelFrame())) - return joint_state_target_->setFromIK(joint_model_group_, eef_pose, eef, 0.0, - moveit::core::GroupStateValidityCallbackFn(), o); - else - { - if (c->knowsFrameTransform(frame)) - { - // transform the pose first if possible, then do IK - const Eigen::Isometry3d& t = joint_state_target_->getFrameTransform(frame); - Eigen::Isometry3d p; - tf2::fromMsg(eef_pose, p); - return joint_state_target_->setFromIK(joint_model_group_, t * p, eef, 0.0, - moveit::core::GroupStateValidityCallbackFn(), o); - } - else - { - ROS_ERROR_NAMED("move_group_interface", "Unable to transform from frame '%s' to frame '%s'", frame.c_str(), - robot_model_->getModelFrame().c_str()); - return false; - } - } - } - else - return false; -} - -robot_state::RobotStatePtr MoveItCpp::getStartState() -{ - if (considered_start_state_) - return considered_start_state_; - else - { - robot_state::RobotStatePtr s; - getCurrentState(s, 1.0); - return s; - } -} - -bool MoveItCpp::hasPoseTarget(const std::string& end_effector_link) const -{ - const std::string& eef = end_effector_link.empty() ? end_effector_link_ : end_effector_link; - return pose_targets_.find(eef) != pose_targets_.end(); -} - -void MoveItCpp::clearContents() -{ - // TODO Instead of setting to nullptrs actually delete - // TODO Set values to "default values" - group_name_.clear(); - robot_description_.clear(); - // node_handle_; - tf_buffer_ = nullptr; - // robot_model_ - current_state_monitor_ = nullptr; - execute_action_client_ = nullptr; - - considered_start_state_ = nullptr; - // workspace_parameters_ - // allowed_planning_time_ - planner_id_.clear(); - // num_planning_attempts_ - // max_velocity_scaling_factor_ - // max_acceleration_scaling_factor_ - // goal_joint_tolerance_; - // goal_position_tolerance_ - // goal_orientation_tolerance_ - // can_look_ - // can_replan_ - // replan_delay_ - - joint_state_target_ = nullptr; - joint_model_group_ = nullptr; - - pose_targets_.clear(); - - // active_target_ - path_constraints_ = nullptr; - trajectory_constraints_ = nullptr; - end_effector_link_.clear(); - pose_reference_frame_.clear(); - support_surface_.clear(); - - // trajectory_event_publisher_ - // attached_object_publisher_ - // query_service_ - // get_params_service_ - // set_params_service_ - // cartesian_path_service_ - // plan_grasps_service_ - // initializing_constraints_ -} -} // planning_interface -} // moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_core.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_core.cpp new file mode 100644 index 0000000000..987eef84a7 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_core.cpp @@ -0,0 +1,186 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + + +/* Author: Simon Goldstien, Henning Kayser */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +constexpr char LOGNAME[] = "moveit_cpp"; +MoveItCpp::MoveItCpp(const std::string& group_name, + const std::shared_ptr& tf_buffer) + : MoveItCpp(Options(group_name), tf_buffer) +{ +} + +MoveItCpp::MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer) + : node_handle_(opt.node_handle_), tf_buffer_(tf_buffer) +{ + // Initialize robot model + robot_description_ = opt.robot_description_; + robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_); + if (!robot_model_) + { + std::string error = "Unable to construct robot model. Please make sure all needed information is on the " + "parameter server."; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + // Check if default planning group is specified + if (!opt.default_group_name_.empty()) + { + setDefaultGroup(opt.default_group_name_); + } + else + { + ROS_INFO_NAMED(LOGNAME, "No default planning group specified"); + } + + // Init current state monitor + current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_buffer_, node_handle_); +} + +MoveItCpp::~MoveItCpp() +{ + clearContents(); +} + +MoveItCpp::MoveItCpp(MoveItCpp&& other) +{ + other.clearContents(); +} + +MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) +{ + if (this != &other) + { + this->default_group_name_ = other.default_group_name_; + this->robot_description_ = other.robot_description_; + this->node_handle_ = other.node_handle_; + this->tf_buffer_ = other.tf_buffer_; + this->robot_model_ = other.robot_model_; + this->current_state_monitor_ = other.current_state_monitor_; + other.clearContents(); + } + + return *this; +} + +const std::string& MoveItCpp::getDefaultGroup() const +{ + return default_group_name_; +} + +bool MoveItCpp::setDefaultGroup(const std::string& group_name) +{ + if(!robot_model_->hasJointModelGroup(group_name)) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Group '" << group_name << "' was not found. Nothing changed."); + return false; + } + + default_group_name_ = group_name; + ROS_INFO_STREAM_NAMED(LOGNAME, "Ready to take commands for planning group " << group_name << "."); + return true; +} + +const std::vector MoveItCpp::getNamedTargets() +{ + const robot_model::RobotModelConstPtr& robot = robot_model_; + const std::string& group = default_group_name_; + const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group); + + if (joint_group) + { + return joint_group->getDefaultStateNames(); + } + + std::vector empty; + return empty; +} + +robot_model::RobotModelConstPtr MoveItCpp::getRobotModel() const +{ + return robot_model_; +} + +const ros::NodeHandle& MoveItCpp::getNodeHandle() const +{ + return node_handle_; +} + + +void MoveItCpp::clearContents() +{ + default_group_name_.clear(); + robot_description_.clear(); + robot_model_.reset(); + tf_buffer_.reset(); + current_state_monitor_.reset(); +} +} // planning_interface +} // moveit From 45e55ee5fed61d3af458b31fdda63afa3819d13b Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 20 Aug 2019 13:32:04 +0200 Subject: [PATCH 18/29] Remove unused headers --- .../include/moveit/moveit_cpp/moveit_core.h | 13 ------------- .../moveit_cpp/src/moveit_core.cpp | 19 ------------------- 2 files changed, 32 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h index 098fddf120..aa30492a5c 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h @@ -37,24 +37,11 @@ #ifndef MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ #define MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ -#include #include -#include #include -#include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_core.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_core.cpp index 987eef84a7..242f6957e6 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_core.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_core.cpp @@ -36,31 +36,12 @@ /* Author: Simon Goldstien, Henning Kayser */ #include -#include -#include -#include -#include #include #include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include -#include #include -#include #include #include #include From 2a46db9c6004f89567a8b909f3e84fa1fe17509e Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 20 Aug 2019 13:35:12 +0200 Subject: [PATCH 19/29] Remove unused forwards and structs --- .../include/moveit/moveit_cpp/moveit_core.h | 43 ------------------- 1 file changed, 43 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h index aa30492a5c..2dcbbdcf8a 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h @@ -50,33 +50,6 @@ namespace planning_scene_monitor MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); } -namespace planning_pipeline -{ -MOVEIT_CLASS_FORWARD(PlanningPipeline); -} - -namespace plan_execution -{ -MOVEIT_CLASS_FORWARD(PlanExecution); -MOVEIT_CLASS_FORWARD(PlanWithSensing); -} - -namespace trajectory_execution_manager -{ -MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); -} - -namespace -{ -enum ActiveTargetType -{ - JOINT, - POSE, - POSITION, - ORIENTATION -}; -} - namespace moveit { /** \brief Simple interface to MoveIt components */ @@ -143,21 +116,6 @@ class MoveItCpp ros::NodeHandle node_handle_; }; - MOVEIT_STRUCT_FORWARD(Plan); - - /// The representation of a motion plan (as ROS messages) - struct Plan - { - /// The full starting state used for planning - moveit_msgs::RobotState start_state_; - - /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) - moveit_msgs::RobotTrajectory trajectory_; - - /// The amount of time it took to generate the plan - double planning_time_; - }; - /** \brief Construct a MoveItCpp instance call using a specified set of options \e opt. @@ -217,7 +175,6 @@ class MoveItCpp robot_model::RobotModelConstPtr robot_model_; planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - void clearContents(); }; } // namespace planning_interface From 1af8f53c766ac0f60dcdf950c4e2f0e90fd1920c Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 22 Aug 2019 16:14:16 +0200 Subject: [PATCH 20/29] Implement plan/execution interface --- .../moveit_cpp/CMakeLists.txt | 2 +- .../include/moveit/moveit_cpp/moveit_core.h | 183 ---------- .../include/moveit/moveit_cpp/moveit_cpp.h | 193 ++++++++++ .../moveit/moveit_cpp/planning_component.h | 173 +++++++++ .../moveit_cpp/src/moveit.cpp | 336 ++++++++++++++++++ .../moveit_cpp/src/moveit_core.cpp | 167 --------- .../moveit_cpp/src/planning_component.cpp | 330 +++++++++++++++++ 7 files changed, 1033 insertions(+), 351 deletions(-) delete mode 100644 moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h create mode 100644 moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h create mode 100644 moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h create mode 100644 moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp delete mode 100644 moveit_ros/planning_interface/moveit_cpp/src/moveit_core.cpp create mode 100644 moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt index a039bfc398..45ccc22b0e 100644 --- a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -1,6 +1,6 @@ set(MOVEIT_LIB_NAME moveit_cpp) -add_library(${MOVEIT_LIB_NAME} src/moveit_core.cpp) +add_library(${MOVEIT_LIB_NAME} src/moveit.cpp src/planning_component.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h deleted file mode 100644 index 2dcbbdcf8a..0000000000 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_core.h +++ /dev/null @@ -1,183 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, PickNik LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik LLC nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Simon Goldstein, Henning Kayser */ - -#ifndef MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ -#define MOVEIT_MOVEIT_CPP_MOVEIT_CPP_ - -#include -#include -#include -#include -#include -#include -#include - -namespace planning_scene_monitor -{ -MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); -} - -namespace moveit -{ -/** \brief Simple interface to MoveIt components */ -namespace planning_interface -{ -//TODO(henningkayser): Move to own header and share with MoveGroup -class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes -{ -public: - MoveItErrorCode() - { - val = 0; - } - MoveItErrorCode(int code) - { - val = code; - } - MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code) - { - val = code.val; - } - explicit operator bool() const - { - return val == moveit_msgs::MoveItErrorCodes::SUCCESS; - } - bool operator==(const int c) const - { - return val == c; - } - bool operator!=(const int c) const - { - return val != c; - } -}; - -MOVEIT_CLASS_FORWARD(MoveItCpp); - -/** \class MoveItCpp move_group_interface.h moveit/planning_interface/move_group_interface.h - - \brief Client class to conveniently use the ROS interfaces provided by the move_group node. - - This class includes many default settings to make things easy to use. */ -class MoveItCpp -{ -public: - /** \brief Specification of options to use when constructing the MoveItCpp class */ - struct Options - { - Options(const std::string& group_name = "FAKE", const std::string& desc = "robot_description", - const ros::NodeHandle& node_handle = ros::NodeHandle()) - : default_group_name_(group_name), robot_description_(desc), node_handle_(node_handle) - { - } - - /// The group to construct the class instance for - std::string default_group_name_; - - /// The robot description parameter name (if different from default) - std::string robot_description_; - - /// Optionally, an instance of the RobotModel to use can be also specified - robot_model::RobotModelConstPtr robot_model_; - - ros::NodeHandle node_handle_; - }; - - /** - \brief Construct a MoveItCpp instance call using a specified set of options \e opt. - - \param opt. A MoveItCpp::Options structure, if you pass a ros::NodeHandle with a specific callback queue, - it has to be of type ros::CallbackQueue - (which is the default type of callback queues used in ROS) - \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, - one will be constructed internally along with an internal TF2_ROS TransformListener - \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. - */ - MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer = std::shared_ptr()); - /** - \brief Construct a client for the MoveGroup action for a particular \e group. - - \param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified, - one will be constructed internally along with an internal TF2_ROS TransformListener - \param wait_for_servers. Timeout for connecting to action servers. Zero time means unlimited waiting. - */ - MoveItCpp(const std::string& group, - const std::shared_ptr& tf_buffer = std::shared_ptr()); - - ~MoveItCpp(); - - /** - * @brief This class owns unique resources (e.g. action clients, threads) and its not very - * meaningful to copy. Pass by references, move it, or simply create multiple instances where - * required. - */ - MoveItCpp(const MoveItCpp&) = delete; - MoveItCpp& operator=(const MoveItCpp&) = delete; - - MoveItCpp(MoveItCpp&& other); - MoveItCpp& operator=(MoveItCpp&& other); - - /** \brief Get the names of the named robot states available as targets, both either remembered states or default - * states from srdf */ - const std::vector getNamedTargets(); - - /** \brief Get the RobotModel object. */ - robot_model::RobotModelConstPtr getRobotModel() const; - - /** \brief Get the name of the default planning group */ - const std::string& getDefaultGroup() const; - - /** \brief Set the name of the default planning group */ - bool setDefaultGroup(const std::string& group_name); - - /** \brief Get the ROS node handle of this instance operates on */ - const ros::NodeHandle& getNodeHandle() const; - -private: - // Core properties - ros::NodeHandle node_handle_; - std::shared_ptr tf_buffer_; - std::string robot_description_; - std::string default_group_name_; - robot_model::RobotModelConstPtr robot_model_; - planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - void clearContents(); -}; -} // namespace planning_interface -} // namespace moveit - -#endif diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h new file mode 100644 index 0000000000..6c2ab539b9 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -0,0 +1,193 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Simon Goldstein, Henning Kayser */ + +#pragma once + +#include +#include +#include +#include +#include + +//namespace planning_scene_monitor +//{ +//MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); +//} + +namespace moveit +{ +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(MoveitCpp); + +class MoveitCpp +{ +public: + /// Specification of options to use when constructing the MoveitCpp class + struct PlanningSceneMonitorOptions + { + void load(const ros::NodeHandle& nh) + { + std::string ns = "planning_scene_monitor_options/"; + nh.param(ns + "name", name, "planning_scene_monitor"); + nh.param(ns + "robot_description", robot_description, "robot_description"); + nh.param(ns + "joint_state_topic", joint_state_topic, planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC); + nh.param(ns + "attached_collision_object_topic", attached_collision_object_topic, planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC); + nh.param(ns + "monitored_planning_scene_topic", monitored_planning_scene_topic, planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC); + nh.param(ns + "publish_planning_scene_topic", publish_planning_scene_topic, planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC); + }; + std::string name; + std::string robot_description; + std::string joint_state_topic; + std::string attached_collision_object_topic; + std::string monitored_planning_scene_topic; + std::string publish_planning_scene_topic; + }; + + /// Parameter container for initializing MoveitCpp + struct Options + { + Options(const ros::NodeHandle& nh) + { + planning_scene_monitor_options.load(nh); + default_planner_options.load(nh); + planning_pipeline_options.load(nh); + } + PlanningSceneMonitorOptions planning_scene_monitor_options; + + struct PlanningPipelineOptions + { + void load(const ros::NodeHandle& nh) + { + std::string ns = "planning_pipeline_options/"; + nh.getParam(ns + "pipeline_names", pipeline_names); + }; + std::vector pipeline_names; + }; + PlanningPipelineOptions planning_pipeline_options; + struct PlannerOptions + { + void load(const ros::NodeHandle& nh) + { + std::string ns = "default_planner_options/"; + nh.getParam(ns + "planning_attempts", planning_attempts); + nh.getParam(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor); + nh.getParam(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor); + }; + int planning_attempts; + double planning_time; + double max_velocity_scaling_factor; + double max_acceleration_scaling_factor; + }; + PlannerOptions default_planner_options; + }; + + /** \brief Constructor */ + MoveitCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = std::shared_ptr()); + MoveitCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = std::shared_ptr()); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + MoveitCpp(const MoveitCpp&) = delete; + MoveitCpp& operator=(const MoveitCpp&) = delete; + + MoveitCpp(MoveitCpp&& other); + MoveitCpp& operator=(MoveitCpp&& other); + + /** \brief Destructor */ + ~MoveitCpp(); + + /** \brief Get the RobotModel object. */ + robot_model::RobotModelConstPtr getRobotModel() const; + + /** \brief Get the ROS node handle of this instance operates on */ + const ros::NodeHandle& getNodeHandle() const; + + /** \brief Get the current state queried from the current state monitor */ + bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds); + + /** \brief Get the current state queried from the current state monitor */ + robot_state::RobotStatePtr getCurrentState(double wait_seconds=0.0); + + /** \brief Get all loaded planning pipeline instances mapped to their reference names */ + const std::map& getPlanningPipelines() const; + + /** \brief Get the names of all loaded planning pipelines. Specify group_name to filter the results by planning group */ + std::set getPlanningPipelineNames(const std::string& group_name = "") const; + + /** \brief Get the stored instance of the planning scene monitor */ + const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const; + planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst(); + + /** \brief Get the stored instance of the trajectory execution manager */ + const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const; + trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst(); + + /** \brief Execute a trajectory on the planning group specified by group_name using the trajectory execution manager. If blocking is set to false, the execution is run in background and the function returns immediately. */ + bool execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, bool blocking=true); + +protected: + std::shared_ptr tf_buffer_; + +private: + // Core properties and instances + ros::NodeHandle node_handle_; + std::string robot_description_; + robot_model::RobotModelConstPtr robot_model_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // Planning + std::map planning_pipelines_; + std::map> groups_pipelines_map_; + std::map> groups_algorithms_map_; + + // Execution + trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; + + /** \brief Reset all member variables */ + void clearContents(); + + /** \brief Initialize and setup the planning scene monitor */ + bool loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt); + + /** \brief Initialize and setup the planning pipelines */ + bool loadPlanningPipelines(std::vector pipeline_names); +}; +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h new file mode 100644 index 0000000000..f337c1e212 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -0,0 +1,173 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Henning Kayser */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace planning_pipeline +{ +MOVEIT_CLASS_FORWARD(PlanningPipeline); +} + +namespace moveit +{ +/** \brief Simple interface to MoveIt planning components */ +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(PlanningComponent); + +class PlanningComponent +{ +public: + MOVEIT_STRUCT_FORWARD(PlanSolution); + + /// The representation of a plan solution + struct PlanSolution + { + /// The full starting state used for planning + moveit_msgs::RobotState start_state; + + /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) + robot_trajectory::RobotTrajectoryPtr trajectory; + }; + + /// Planner parameters provided with the MotionPlanRequest + struct PlanRequestParameters + { + std::string planner_id; + std::string planning_pipeline; + size_t planning_attempts; + double planning_time; + double max_velocity_scaling_factor; + double max_acceleration_scaling_factor; + }; + + PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh); + PlanningComponent(const std::string& group_name, const MoveitCppPtr& moveit_context); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + PlanningComponent(const PlanningComponent&) = delete; + PlanningComponent& operator=(const PlanningComponent&) = delete; + + PlanningComponent(PlanningComponent&& other); + PlanningComponent& operator=(PlanningComponent&& other); + ~PlanningComponent(); + + /** \brief Get the name of the planning group */ + const std::string& getName() const; + + /** \brief Get the names of the named robot states available as targets */ + const std::vector getNamedTargets(); + + /** \brief Get the joint values for targets specified by name */ + std::map getNamedTargetValues(const std::string& name); + + /** \brief Specify the workspace bounding box. + The box is specified in the planning frame (i.e. relative to the robot root link start position). + This is useful when the planning group contains the root joint of the robot -- i.e. when planning motion for the + robot relative to the world. */ + void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz); + + /** \brief Remove the workspace bounding box from planning */ + void unsetWorkspace(); + + /** \brief Get the considered start state if defined, otherwise get the current state */ + robot_state::RobotStatePtr getStartState(); + + /** \brief Set the robot state that should be considered as start state for planning */ + void setStartState(const robot_state::RobotState& start_state); + /** \brief Set the named robot state that should be considered as start state for planning */ + void setStartState(const std::string& named_state); + + /** \brief Set the start state for planning to be the current state of the robot */ + void setStartStateToCurrentState(); + + /** \brief Set the goal constraints used for planning */ + bool setGoal(const std::vector& goal_constraints); + /** \brief Set the goal constraints generated from a target state */ + bool setGoal(const robot_state::RobotState& goal_state); + /** \brief Set the goal constraints generated from target pose and robot link */ + bool setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name); + /** \brief Set the goal constraints generated from a named target state */ + bool setGoal(const std::string& named_target); + + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using default parameters. */ + bool plan(); + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the provided PlanRequestParameters. */ + bool plan(const PlanRequestParameters& parameters); + + /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates after the execution is complete. The execution can be run in background by setting blocking to false. */ + bool execute(bool blocking=true); +private: + // Core properties and instances + ros::NodeHandle nh_; + MoveitCppPtr moveit_cpp_; + const std::string group_name_; + moveit::core::JointModelGroupConstPtr joint_model_group_; + + // Planning + std::set planning_pipeline_names_; + robot_state::RobotStatePtr considered_start_state_; + std::vector current_goal_constraints_; + PlanRequestParameters plan_request_parameters_; + moveit_msgs::WorkspaceParameters workspace_parameters_; + bool workspace_parameters_set_ = false; + PlanSolutionPtr last_plan_solution_; + + // common properties for goals + // TODO(henningkayser): support goal tolerances + //double goal_joint_tolerance_; + //double goal_position_tolerance_; + //double goal_orientation_tolerance_; + // TODO(henningkayser): implment path/trajectory constraints + //std::unique_ptr path_constraints_; + //std::unique_ptr trajectory_constraints_; + + /** \brief Reset all member variables */ + void clearContents(); +}; +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp new file mode 100644 index 0000000000..ccc9b43a20 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp @@ -0,0 +1,336 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + + +/* Author: Simon Goldstein, Henning Kayser */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +constexpr char LOGNAME[] = "moveit_cpp"; +constexpr char PLANNING_SCENE_MONITOR_NAME[] = "moveit_cpp_planning_scene"; +constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin"; + +MoveitCpp::MoveitCpp(const ros::NodeHandle& nh, + const std::shared_ptr& tf_buffer) + : MoveitCpp(Options(nh), nh, tf_buffer) +{ +} + +MoveitCpp::MoveitCpp(const Options& opt, const ros::NodeHandle& nh, + const std::shared_ptr& tf_buffer) + : tf_buffer_(tf_buffer) +{ + // Configure planning scene monitor + if (!loadPlanningSceneMonitor(opt.planning_scene_monitor_options)) + { + std::string error = "Unable to configure planning scene monitor"; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + robot_model_ = planning_scene_monitor_->getRobotModel(); + if (!robot_model_) + { + std::string error = "Unable to construct robot model. Please make sure all needed information is on the " + "parameter server."; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + bool load_planning_pipelines = true; + if (load_planning_pipelines && !loadPlanningPipelines(opt.planning_pipeline_options.pipeline_names)) + { + std::string error = "Failed to load planning pipelines from parameter server"; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + // TODO(henningkayser): configure trajectory execution manager + trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager(robot_model_, + planning_scene_monitor_->getStateMonitor())); + + ROS_INFO_NAMED(LOGNAME, "MoveitCpp running"); +} + +MoveitCpp::MoveitCpp(MoveitCpp&& other) +{ + other.clearContents(); +} + +MoveitCpp::~MoveitCpp() +{ + ROS_INFO_NAMED(LOGNAME, "Deleting MoveitCpp"); + clearContents(); +} + +MoveitCpp& MoveitCpp::operator=(MoveitCpp&& other) +{ + if (this != &other) + { + this->robot_description_ = other.robot_description_; + this->node_handle_ = other.node_handle_; + this->tf_buffer_ = other.tf_buffer_; + this->robot_model_ = other.robot_model_; + this->planning_scene_monitor_ = other.planning_scene_monitor_; + other.clearContents(); + } + + return *this; +} + +bool MoveitCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt) +{ + planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(opt.robot_description, tf_buffer_, + opt.name)); + // Allows us to sycronize to Rviz and also publish collision objects to ourselves + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Configuring Planning Scene Monitor"); + if (planning_scene_monitor_->getPlanningScene()) + { + // Start state and scene monitors + ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for joint states", opt.joint_state_topic.c_str()); + planning_scene_monitor_->startStateMonitor(opt.joint_state_topic, opt.attached_collision_object_topic); + planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, opt.publish_planning_scene_topic); + planning_scene_monitor_->startSceneMonitor(opt.monitored_planning_scene_topic); + } + else + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning scene not configured"); + return false; + } + // TODO(henningkayser): fix and remove lines below + ros::spinOnce(); + ros::Duration(0.5).sleep(); // when at 0.1, i believe sometimes vjoint not properly loaded + + // Wait for complete state to be recieved + // TODO(henningkayser): parameterize + double wait_for_complete_state_timeout = 10.0; + // Break early + if (wait_for_complete_state_timeout > 0.0) + { + return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(); + } + + return true; +} + +bool MoveitCpp::loadPlanningPipelines(std::vector pipeline_names) +{ + ros::NodeHandle pnh("~"); + for (const auto& planning_pipeline_name : pipeline_names) + { + if (planning_pipelines_.count(planning_pipeline_name) > 0) + { + ROS_WARN_NAMED(LOGNAME, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); + continue; + } + ROS_INFO_NAMED(LOGNAME, "Loading planning pipeline '%s'", planning_pipeline_name.c_str()); + ros::NodeHandle child_nh(pnh, planning_pipeline_name); + planning_pipeline::PlanningPipelinePtr pipeline; + pipeline.reset( + new planning_pipeline::PlanningPipeline(robot_model_, child_nh, PLANNING_PLUGIN_PARAM)); + + if (!pipeline->getPlannerManager()) + { + + ROS_ERROR_NAMED(LOGNAME, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); + continue; + } + planning_pipelines_[planning_pipeline_name] = pipeline; + } + + if (planning_pipelines_.empty()) + { + return false; + ROS_ERROR_NAMED(LOGNAME, "Failed to load any planning pipelines."); + } + + // Retrieve group/pipeline mapping for faster lookup + std::vector group_names = robot_model_->getJointModelGroupNames(); + for (const auto& pipeline_entry : planning_pipelines_) + { + for (const auto& group_name : group_names) + { + groups_pipelines_map_[group_name] = {}; + const auto& pipeline = pipeline_entry.second; + for (const auto& planner_configuration : pipeline->getPlannerManager()->getPlannerConfigurations()) + { + if (planner_configuration.second.group == group_name) + { + groups_pipelines_map_[group_name].insert(pipeline_entry.first); + } + } + } + } + + return true; +} + + +robot_model::RobotModelConstPtr MoveitCpp::getRobotModel() const +{ + ROS_DEBUG_NAMED(LOGNAME, "MoveitCpp::getRobotModel()"); + return robot_model_; +} + +const ros::NodeHandle& MoveitCpp::getNodeHandle() const +{ + ROS_DEBUG_NAMED(LOGNAME, "MoveitCpp::getNodeHandle()"); + return node_handle_; +} + +bool MoveitCpp::getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds) +{ + if (wait_seconds > 0.0 && !planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), wait_seconds)) + { + ROS_ERROR_NAMED(LOGNAME, "Did not receive robot state"); + return false; + } + { // Lock planning scene + planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor_); + current_state.reset(new moveit::core::RobotState(scene->getCurrentState())); + } // Unlock planning scene + return true; +} + +robot_state::RobotStatePtr MoveitCpp::getCurrentState(double wait) +{ + robot_state::RobotStatePtr current_state; + getCurrentState(current_state, wait); + return current_state; +} + +const std::map& MoveitCpp::getPlanningPipelines() const +{ + return planning_pipelines_; +} + +std::set MoveitCpp::getPlanningPipelineNames(const std::string& group_name) const +{ + std::set result_names; + if (!group_name.empty() && groups_pipelines_map_.count(group_name) == 0) + { + ROS_ERROR_NAMED(LOGNAME, "There are no planning pipelines loaded for group '%s'.", group_name.c_str()); + return result_names; // empty + } + for (const auto& pipeline_entry : planning_pipelines_) + { + const std::string& pipeline_name = pipeline_entry.first; + // If group_name is defined and valid, skip pipelines that don't belong to the planning group + if (!group_name.empty()) + { + const auto& group_pipelines = groups_pipelines_map_.at(group_name); + if (group_pipelines.find(pipeline_name) == group_pipelines.end()) + continue; + } + result_names.insert(pipeline_name); + } + return result_names; +} + +const planning_scene_monitor::PlanningSceneMonitorPtr& MoveitCpp::getPlanningSceneMonitor() const +{ + return planning_scene_monitor_; +} +planning_scene_monitor::PlanningSceneMonitorPtr MoveitCpp::getPlanningSceneMonitorNonConst() +{ + return planning_scene_monitor_; +} + +const trajectory_execution_manager::TrajectoryExecutionManagerPtr& MoveitCpp::getTrajectoryExecutionManager() const +{ + return trajectory_execution_manager_; +} + +trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveitCpp::getTrajectoryExecutionManagerNonConst() +{ + return trajectory_execution_manager_; +} + +bool MoveitCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, bool blocking) +{ + if (!robot_trajectory) + { + ROS_ERROR_NAMED(LOGNAME, "Robot trajectory is undefined"); + return false; + } + + // Check if there are controllers that can handle the execution + if (!trajectory_execution_manager_->ensureActiveControllersForGroup(group_name)) + { + ROS_ERROR_NAMED(LOGNAME, "Execution failed! No active controllers configured for group '%s'", group_name.c_str()); + return false; + } + + // Execute trajectory + moveit_msgs::RobotTrajectory robot_trajectory_msg; + robot_trajectory->getRobotTrajectoryMsg(robot_trajectory_msg); + if (blocking) + { + trajectory_execution_manager_->push(robot_trajectory_msg); + trajectory_execution_manager_->execute(); + return trajectory_execution_manager_->waitForExecution(); + } + trajectory_execution_manager_->pushAndExecute(robot_trajectory_msg); + return true; +} + + +void MoveitCpp::clearContents() +{ + ROS_INFO_NAMED(LOGNAME, "robot_description"); + robot_description_.clear(); + ROS_INFO_NAMED(LOGNAME, "tf_buffer_"); + tf_buffer_.reset(); + ROS_INFO_NAMED(LOGNAME, "planning_scene_monitor"); + planning_scene_monitor_.reset(); + ROS_INFO_NAMED(LOGNAME, "robot_model"); + robot_model_.reset(); + ROS_INFO_NAMED(LOGNAME, "pipelines"); + planning_pipelines_.clear(); +} +} // planning_interface +} // moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_core.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_core.cpp deleted file mode 100644 index 242f6957e6..0000000000 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit_core.cpp +++ /dev/null @@ -1,167 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, PickNik LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik LLC nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - - -/* Author: Simon Goldstien, Henning Kayser */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace moveit -{ -namespace planning_interface -{ -constexpr char LOGNAME[] = "moveit_cpp"; -MoveItCpp::MoveItCpp(const std::string& group_name, - const std::shared_ptr& tf_buffer) - : MoveItCpp(Options(group_name), tf_buffer) -{ -} - -MoveItCpp::MoveItCpp(const Options& opt, const std::shared_ptr& tf_buffer) - : node_handle_(opt.node_handle_), tf_buffer_(tf_buffer) -{ - // Initialize robot model - robot_description_ = opt.robot_description_; - robot_model_ = opt.robot_model_ ? opt.robot_model_ : getSharedRobotModel(opt.robot_description_); - if (!robot_model_) - { - std::string error = "Unable to construct robot model. Please make sure all needed information is on the " - "parameter server."; - ROS_FATAL_STREAM_NAMED(LOGNAME, error); - throw std::runtime_error(error); - } - - // Check if default planning group is specified - if (!opt.default_group_name_.empty()) - { - setDefaultGroup(opt.default_group_name_); - } - else - { - ROS_INFO_NAMED(LOGNAME, "No default planning group specified"); - } - - // Init current state monitor - current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_buffer_, node_handle_); -} - -MoveItCpp::~MoveItCpp() -{ - clearContents(); -} - -MoveItCpp::MoveItCpp(MoveItCpp&& other) -{ - other.clearContents(); -} - -MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) -{ - if (this != &other) - { - this->default_group_name_ = other.default_group_name_; - this->robot_description_ = other.robot_description_; - this->node_handle_ = other.node_handle_; - this->tf_buffer_ = other.tf_buffer_; - this->robot_model_ = other.robot_model_; - this->current_state_monitor_ = other.current_state_monitor_; - other.clearContents(); - } - - return *this; -} - -const std::string& MoveItCpp::getDefaultGroup() const -{ - return default_group_name_; -} - -bool MoveItCpp::setDefaultGroup(const std::string& group_name) -{ - if(!robot_model_->hasJointModelGroup(group_name)) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Group '" << group_name << "' was not found. Nothing changed."); - return false; - } - - default_group_name_ = group_name; - ROS_INFO_STREAM_NAMED(LOGNAME, "Ready to take commands for planning group " << group_name << "."); - return true; -} - -const std::vector MoveItCpp::getNamedTargets() -{ - const robot_model::RobotModelConstPtr& robot = robot_model_; - const std::string& group = default_group_name_; - const robot_model::JointModelGroup* joint_group = robot->getJointModelGroup(group); - - if (joint_group) - { - return joint_group->getDefaultStateNames(); - } - - std::vector empty; - return empty; -} - -robot_model::RobotModelConstPtr MoveItCpp::getRobotModel() const -{ - return robot_model_; -} - -const ros::NodeHandle& MoveItCpp::getNodeHandle() const -{ - return node_handle_; -} - - -void MoveItCpp::clearContents() -{ - default_group_name_.clear(); - robot_description_.clear(); - robot_model_.reset(); - tf_buffer_.reset(); - current_state_monitor_.reset(); -} -} // planning_interface -} // moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp new file mode 100644 index 0000000000..9ed39d6590 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -0,0 +1,330 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + + +/* Author: Henning Kayser */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +constexpr char LOGNAME[] = "planning_component"; + +PlanningComponent::PlanningComponent(const std::string& group_name, const MoveitCppPtr& moveit_context) + : group_name_(group_name), nh_(moveit_context->getNodeHandle()), moveit_cpp_(moveit_context) +{ + joint_model_group_.reset(moveit_cpp_->getRobotModel()->getJointModelGroup(group_name)); + if (!joint_model_group_) + { + std::string error = "Could not find joint model group '" + group_name + "'."; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name); +} + +PlanningComponent::PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh) + : group_name_(group_name), nh_(nh), moveit_cpp_(new MoveitCpp(nh)) +{ +} + +PlanningComponent::~PlanningComponent() +{ + ROS_INFO_NAMED(LOGNAME, "Deleting PlanningComponent '%s'", group_name_.c_str()); + clearContents(); +} + +PlanningComponent& PlanningComponent::operator=(PlanningComponent&& other) +{ + if (this != &other) + { + this->considered_start_state_ = other.considered_start_state_; + this->workspace_parameters_ = other.workspace_parameters_; + this->last_plan_solution_ = other.last_plan_solution_; + other.clearContents(); + } + return *this; +} + +const std::vector PlanningComponent::getNamedTargets() +{ + if (joint_model_group_) + { + return joint_model_group_->getDefaultStateNames(); + } + else + { + ROS_WARN_NAMED(LOGNAME, "Unable to find joint group with name '%s'.", group_name_.c_str()); + } + + std::vector empty; + return empty; +} + +const std::string& PlanningComponent::getName() const +{ + return group_name_; +} + +bool PlanningComponent::plan(const PlanRequestParameters& parameters) +{ + if (!joint_model_group_) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); + return false; + } + + // Clone current planning scene + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitorNonConst(); + planning_scene_monitor->lockSceneRead(); // LOCK planning scene + planning_scene::PlanningScenePtr planning_scene = + planning_scene::PlanningScene::clone(planning_scene_monitor->getPlanningScene()); + planning_scene_monitor->unlockSceneRead(); // UNLOCK planning scene + planning_scene_monitor.reset(); // release this pointer + + // Init MotionPlanRequest + ::planning_interface::MotionPlanRequest req; + req.group_name = group_name_; + req.planner_id = parameters.planner_id; + req.allowed_planning_time = parameters.planning_time; + req.max_velocity_scaling_factor = parameters.max_velocity_scaling_factor; + req.max_acceleration_scaling_factor = parameters.max_acceleration_scaling_factor; + if (workspace_parameters_set_) + req.workspace_parameters = workspace_parameters_; + + // Set start state + moveit::core::RobotStatePtr start_state = considered_start_state_; + if (!start_state) + start_state = moveit_cpp_->getCurrentState(); + moveit::core::robotStateToRobotStateMsg(*start_state, req.start_state); + planning_scene->setCurrentState(*start_state); + + // Set goal constraints + if (current_goal_constraints_.empty()) + { + ROS_ERROR_NAMED(LOGNAME, "No goal constraints set for planning request"); + return false; + } + req.goal_constraints = current_goal_constraints_; + + // Run planning attempt + ::planning_interface::MotionPlanResponse res; + if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end()) + { + ROS_ERROR_NAMED(LOGNAME, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); + return false; + } + const planning_pipeline::PlanningPipelinePtr pipeline = + moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline); + pipeline->generatePlan(planning_scene, req, res); + if (res.error_code_.val != res.error_code_.SUCCESS) + { + ROS_ERROR("Could not compute plan successfully"); + return false; + } + last_plan_solution_.reset(new PlanSolution()); + last_plan_solution_->start_state = req.start_state; + last_plan_solution_->trajectory = res.trajectory_; + // TODO(henningkayser): Visualize trajectory + //std::vector eef_links; + //if (joint_model_group->getEndEffectorTips(eef_links)) + //{ + // for (const auto& eef_link : eef_links) + // { + // ROS_INFO_STREAM("Publishing trajectory for end effector " << eef_link->getName()); + // visual_tools_->publishTrajectoryLine(last_solution_trajectory_, eef_link); + // visual_tools_->publishTrajectoryPath(last_solution_trajectory_, false); + // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT); + // } + //} + return true; +} + +bool PlanningComponent::plan() +{ + PlanRequestParameters default_parameters; + default_parameters.planning_attempts = 1; + default_parameters.planning_time = 5.0; + default_parameters.max_velocity_scaling_factor = 1.0; + default_parameters.max_acceleration_scaling_factor = 1.0; + if (!planning_pipeline_names_.empty()) + default_parameters.planning_pipeline = *planning_pipeline_names_.begin(); + return plan(default_parameters); +} + +void PlanningComponent::setStartState(const robot_state::RobotState& start_state) +{ + considered_start_state_.reset(new robot_state::RobotState(start_state)); +} + +robot_state::RobotStatePtr PlanningComponent::getStartState() +{ + if (considered_start_state_) + return considered_start_state_; + else + { + robot_state::RobotStatePtr s; + moveit_cpp_->getCurrentState(s, 1.0); + return s; + } +} + +void PlanningComponent::setStartStateToCurrentState() +{ + considered_start_state_.reset(); +} + +std::map PlanningComponent::getNamedTargetValues(const std::string& name) +{ + // TODO(henningkayser): verify result + std::map positions; + joint_model_group_->getVariableDefaultPositions(name, positions); + return positions; +} + +void PlanningComponent::setWorkspace(double minx, double miny, double minz, double maxx, + double maxy, double maxz) +{ + workspace_parameters_.header.frame_id = moveit_cpp_->getRobotModel()->getModelFrame(); + workspace_parameters_.header.stamp = ros::Time::now(); + workspace_parameters_.min_corner.x = minx; + workspace_parameters_.min_corner.y = miny; + workspace_parameters_.min_corner.z = minz; + workspace_parameters_.max_corner.x = maxx; + workspace_parameters_.max_corner.y = maxy; + workspace_parameters_.max_corner.z = maxz; + workspace_parameters_set_ = true; +} + +void PlanningComponent::unsetWorkspace() +{ + workspace_parameters_set_ = false; +} + +bool PlanningComponent::setGoal(const std::vector& goal_constraints) +{ + current_goal_constraints_ = goal_constraints; + return true; +} + +bool PlanningComponent::setGoal(const robot_state::RobotState& goal_state) +{ + current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_.get()) }; + return true; +} + +bool PlanningComponent::setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name) +{ + const auto& joint_names = joint_model_group_->getLinkModelNames(); + if (std::find(joint_names.begin(), joint_names.end(), link_name) == joint_names.end()) + { + ROS_ERROR_NAMED(LOGNAME, "Link '%s' is not part of joint model group '%s'.", link_name.c_str(), group_name_.c_str()); + return false; + } + current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(link_name, goal_pose) }; + return true; +} + +bool PlanningComponent::setGoal(const std::string& goal_state_name) +{ + const auto& named_targets = getNamedTargets(); + if (std::find(named_targets.begin(), named_targets.end(), goal_state_name) == named_targets.end()) + { + ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", goal_state_name.c_str()); + return false; + } + robot_state::RobotState goal_state(moveit_cpp_->getRobotModel()); + goal_state.setToDefaultValues(joint_model_group_.get(), goal_state_name); + return setGoal(goal_state); +} + +bool PlanningComponent::execute(bool blocking) +{ + if (!last_plan_solution_) + { + ROS_ERROR_NAMED(LOGNAME, "There is no successfull plan to execute"); + return false; + } + + // TODO(henningkayser): parameterize timestamps if required + //trajectory_processing::TimeOptimalTrajectoryGeneration totg; + //if (!totg.computeTimeStamps(*last_solution_trajectory_, max_velocity_scaling_factor_, max_acceleration_scaling_factor_)) + //{ + // ROS_ERROR("Failed to parameterize trajectory"); + // return false; + //} + moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking); +} + +void PlanningComponent::clearContents() +{ + considered_start_state_.reset(); + last_plan_solution_.reset(); + current_goal_constraints_.clear(); + joint_model_group_.reset(); + moveit_cpp_.reset(); + planning_pipeline_names_.clear(); +} +} // planning_interface +} // moveit From 7eb1e980bdad50b615b13df33a90e0a68f14c10d Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 22 Aug 2019 19:54:28 +0200 Subject: [PATCH 21/29] Change author in headers --- .../moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h | 2 +- moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 6c2ab539b9..b2d95c2ded 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Simon Goldstein, Henning Kayser */ +/* Author: Henning Kayser */ #pragma once diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp index ccc9b43a20..1905e3575e 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp @@ -33,7 +33,7 @@ *********************************************************************/ -/* Author: Simon Goldstein, Henning Kayser */ +/* Author: Henning Kayser */ #include #include From 1f96e86e1edec22c9beb89552441adbfa53d85a6 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 22 Aug 2019 20:50:32 +0200 Subject: [PATCH 22/29] Cleanup + clang --- .../include/moveit/moveit_cpp/moveit_cpp.h | 60 ++++++++++--------- .../moveit/moveit_cpp/planning_component.h | 31 +++++----- .../moveit_cpp/src/moveit.cpp | 30 ++++------ .../moveit_cpp/src/planning_component.cpp | 25 ++++---- 4 files changed, 74 insertions(+), 72 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index b2d95c2ded..c0af077a34 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -42,11 +42,6 @@ #include #include -//namespace planning_scene_monitor -//{ -//MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); -//} - namespace moveit { namespace planning_interface @@ -64,10 +59,14 @@ class MoveitCpp std::string ns = "planning_scene_monitor_options/"; nh.param(ns + "name", name, "planning_scene_monitor"); nh.param(ns + "robot_description", robot_description, "robot_description"); - nh.param(ns + "joint_state_topic", joint_state_topic, planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC); - nh.param(ns + "attached_collision_object_topic", attached_collision_object_topic, planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC); - nh.param(ns + "monitored_planning_scene_topic", monitored_planning_scene_topic, planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC); - nh.param(ns + "publish_planning_scene_topic", publish_planning_scene_topic, planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC); + nh.param(ns + "joint_state_topic", joint_state_topic, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC); + nh.param(ns + "attached_collision_object_topic", attached_collision_object_topic, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC); + nh.param(ns + "monitored_planning_scene_topic", monitored_planning_scene_topic, + planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC); + nh.param(ns + "publish_planning_scene_topic", publish_planning_scene_topic, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC); }; std::string name; std::string robot_description; @@ -90,23 +89,23 @@ class MoveitCpp struct PlanningPipelineOptions { - void load(const ros::NodeHandle& nh) - { - std::string ns = "planning_pipeline_options/"; - nh.getParam(ns + "pipeline_names", pipeline_names); - }; - std::vector pipeline_names; + void load(const ros::NodeHandle& nh) + { + std::string ns = "planning_pipeline_options/"; + nh.getParam(ns + "pipeline_names", pipeline_names); + }; + std::vector pipeline_names; }; PlanningPipelineOptions planning_pipeline_options; struct PlannerOptions { - void load(const ros::NodeHandle& nh) - { - std::string ns = "default_planner_options/"; - nh.getParam(ns + "planning_attempts", planning_attempts); - nh.getParam(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor); - nh.getParam(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor); - }; + void load(const ros::NodeHandle& nh) + { + std::string ns = "default_planner_options/"; + nh.getParam(ns + "planning_attempts", planning_attempts); + nh.getParam(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor); + nh.getParam(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor); + }; int planning_attempts; double planning_time; double max_velocity_scaling_factor; @@ -116,8 +115,10 @@ class MoveitCpp }; /** \brief Constructor */ - MoveitCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = std::shared_ptr()); - MoveitCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = std::shared_ptr()); + MoveitCpp(const ros::NodeHandle& nh, + const std::shared_ptr& tf_buffer = std::shared_ptr()); + MoveitCpp(const Options& opt, const ros::NodeHandle& nh, + const std::shared_ptr& tf_buffer = std::shared_ptr()); /** * @brief This class owns unique resources (e.g. action clients, threads) and its not very @@ -143,12 +144,13 @@ class MoveitCpp bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds); /** \brief Get the current state queried from the current state monitor */ - robot_state::RobotStatePtr getCurrentState(double wait_seconds=0.0); + robot_state::RobotStatePtr getCurrentState(double wait_seconds = 0.0); /** \brief Get all loaded planning pipeline instances mapped to their reference names */ const std::map& getPlanningPipelines() const; - /** \brief Get the names of all loaded planning pipelines. Specify group_name to filter the results by planning group */ + /** \brief Get the names of all loaded planning pipelines. Specify group_name to filter the results by planning group + */ std::set getPlanningPipelineNames(const std::string& group_name = "") const; /** \brief Get the stored instance of the planning scene monitor */ @@ -159,8 +161,10 @@ class MoveitCpp const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const; trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst(); - /** \brief Execute a trajectory on the planning group specified by group_name using the trajectory execution manager. If blocking is set to false, the execution is run in background and the function returns immediately. */ - bool execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, bool blocking=true); + /** \brief Execute a trajectory on the planning group specified by group_name using the trajectory execution manager. + * If blocking is set to false, the execution is run in background and the function returns immediately. */ + bool execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + bool blocking = true); protected: std::shared_ptr tf_buffer_; diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index f337c1e212..8595763668 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -43,14 +43,8 @@ #include #include -namespace planning_pipeline -{ -MOVEIT_CLASS_FORWARD(PlanningPipeline); -} - namespace moveit { -/** \brief Simple interface to MoveIt planning components */ namespace planning_interface { MOVEIT_CLASS_FORWARD(PlanningComponent); @@ -81,6 +75,7 @@ class PlanningComponent double max_acceleration_scaling_factor; }; + /** \brief Constructor */ PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh); PlanningComponent(const std::string& group_name, const MoveitCppPtr& moveit_context); @@ -94,6 +89,8 @@ class PlanningComponent PlanningComponent(PlanningComponent&& other); PlanningComponent& operator=(PlanningComponent&& other); + + /** \brief Destructor */ ~PlanningComponent(); /** \brief Get the name of the planning group */ @@ -134,13 +131,17 @@ class PlanningComponent /** \brief Set the goal constraints generated from a named target state */ bool setGoal(const std::string& named_target); - /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using default parameters. */ + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using + * default parameters. */ bool plan(); - /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the provided PlanRequestParameters. */ + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the + * provided PlanRequestParameters. */ bool plan(const PlanRequestParameters& parameters); - /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates after the execution is complete. The execution can be run in background by setting blocking to false. */ - bool execute(bool blocking=true); + /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates + * after the execution is complete. The execution can be run in background by setting blocking to false. */ + bool execute(bool blocking = true); + private: // Core properties and instances ros::NodeHandle nh_; @@ -159,12 +160,12 @@ class PlanningComponent // common properties for goals // TODO(henningkayser): support goal tolerances - //double goal_joint_tolerance_; - //double goal_position_tolerance_; - //double goal_orientation_tolerance_; + // double goal_joint_tolerance_; + // double goal_position_tolerance_; + // double goal_orientation_tolerance_; // TODO(henningkayser): implment path/trajectory constraints - //std::unique_ptr path_constraints_; - //std::unique_ptr trajectory_constraints_; + // std::unique_ptr path_constraints_; + // std::unique_ptr trajectory_constraints_; /** \brief Reset all member variables */ void clearContents(); diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp index 1905e3575e..1bbf1e4ebe 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp @@ -32,7 +32,6 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ - /* Author: Henning Kayser */ #include @@ -54,14 +53,12 @@ constexpr char LOGNAME[] = "moveit_cpp"; constexpr char PLANNING_SCENE_MONITOR_NAME[] = "moveit_cpp_planning_scene"; constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin"; -MoveitCpp::MoveitCpp(const ros::NodeHandle& nh, - const std::shared_ptr& tf_buffer) +MoveitCpp::MoveitCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) : MoveitCpp(Options(nh), nh, tf_buffer) { } -MoveitCpp::MoveitCpp(const Options& opt, const ros::NodeHandle& nh, - const std::shared_ptr& tf_buffer) +MoveitCpp::MoveitCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) : tf_buffer_(tf_buffer) { // Configure planning scene monitor @@ -90,8 +87,8 @@ MoveitCpp::MoveitCpp(const Options& opt, const ros::NodeHandle& nh, } // TODO(henningkayser): configure trajectory execution manager - trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager(robot_model_, - planning_scene_monitor_->getStateMonitor())); + trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager( + robot_model_, planning_scene_monitor_->getStateMonitor())); ROS_INFO_NAMED(LOGNAME, "MoveitCpp running"); } @@ -124,8 +121,8 @@ MoveitCpp& MoveitCpp::operator=(MoveitCpp&& other) bool MoveitCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt) { - planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(opt.robot_description, tf_buffer_, - opt.name)); + planning_scene_monitor_.reset( + new planning_scene_monitor::PlanningSceneMonitor(opt.robot_description, tf_buffer_, opt.name)); // Allows us to sycronize to Rviz and also publish collision objects to ourselves ROS_DEBUG_STREAM_NAMED(LOGNAME, "Configuring Planning Scene Monitor"); if (planning_scene_monitor_->getPlanningScene()) @@ -133,7 +130,8 @@ bool MoveitCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt) // Start state and scene monitors ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for joint states", opt.joint_state_topic.c_str()); planning_scene_monitor_->startStateMonitor(opt.joint_state_topic, opt.attached_collision_object_topic); - planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, opt.publish_planning_scene_topic); + planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, + opt.publish_planning_scene_topic); planning_scene_monitor_->startSceneMonitor(opt.monitored_planning_scene_topic); } else @@ -170,12 +168,10 @@ bool MoveitCpp::loadPlanningPipelines(std::vector pipeline_names) ROS_INFO_NAMED(LOGNAME, "Loading planning pipeline '%s'", planning_pipeline_name.c_str()); ros::NodeHandle child_nh(pnh, planning_pipeline_name); planning_pipeline::PlanningPipelinePtr pipeline; - pipeline.reset( - new planning_pipeline::PlanningPipeline(robot_model_, child_nh, PLANNING_PLUGIN_PARAM)); + pipeline.reset(new planning_pipeline::PlanningPipeline(robot_model_, child_nh, PLANNING_PLUGIN_PARAM)); if (!pipeline->getPlannerManager()) { - ROS_ERROR_NAMED(LOGNAME, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); continue; } @@ -209,7 +205,6 @@ bool MoveitCpp::loadPlanningPipelines(std::vector pipeline_names) return true; } - robot_model::RobotModelConstPtr MoveitCpp::getRobotModel() const { ROS_DEBUG_NAMED(LOGNAME, "MoveitCpp::getRobotModel()"); @@ -224,7 +219,8 @@ const ros::NodeHandle& MoveitCpp::getNodeHandle() const bool MoveitCpp::getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds) { - if (wait_seconds > 0.0 && !planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), wait_seconds)) + if (wait_seconds > 0.0 && + !planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), wait_seconds)) { ROS_ERROR_NAMED(LOGNAME, "Did not receive robot state"); return false; @@ -290,7 +286,8 @@ trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveitCpp::getTrajec return trajectory_execution_manager_; } -bool MoveitCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, bool blocking) +bool MoveitCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + bool blocking) { if (!robot_trajectory) { @@ -318,7 +315,6 @@ bool MoveitCpp::execute(const std::string& group_name, const robot_trajectory::R return true; } - void MoveitCpp::clearContents() { ROS_INFO_NAMED(LOGNAME, "robot_description"); diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp index 9ed39d6590..87eef45de7 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -32,7 +32,6 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ - /* Author: Henning Kayser */ #include @@ -136,12 +135,13 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) } // Clone current planning scene - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitorNonConst(); + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = + moveit_cpp_->getPlanningSceneMonitorNonConst(); planning_scene_monitor->lockSceneRead(); // LOCK planning scene planning_scene::PlanningScenePtr planning_scene = - planning_scene::PlanningScene::clone(planning_scene_monitor->getPlanningScene()); + planning_scene::PlanningScene::clone(planning_scene_monitor->getPlanningScene()); planning_scene_monitor->unlockSceneRead(); // UNLOCK planning scene - planning_scene_monitor.reset(); // release this pointer + planning_scene_monitor.reset(); // release this pointer // Init MotionPlanRequest ::planning_interface::MotionPlanRequest req; @@ -176,7 +176,7 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) return false; } const planning_pipeline::PlanningPipelinePtr pipeline = - moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline); + moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline); pipeline->generatePlan(planning_scene, req, res); if (res.error_code_.val != res.error_code_.SUCCESS) { @@ -187,8 +187,8 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) last_plan_solution_->start_state = req.start_state; last_plan_solution_->trajectory = res.trajectory_; // TODO(henningkayser): Visualize trajectory - //std::vector eef_links; - //if (joint_model_group->getEndEffectorTips(eef_links)) + // std::vector eef_links; + // if (joint_model_group->getEndEffectorTips(eef_links)) //{ // for (const auto& eef_link : eef_links) // { @@ -243,8 +243,7 @@ std::map PlanningComponent::getNamedTargetValues(const std: return positions; } -void PlanningComponent::setWorkspace(double minx, double miny, double minz, double maxx, - double maxy, double maxz) +void PlanningComponent::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) { workspace_parameters_.header.frame_id = moveit_cpp_->getRobotModel()->getModelFrame(); workspace_parameters_.header.stamp = ros::Time::now(); @@ -279,7 +278,8 @@ bool PlanningComponent::setGoal(const geometry_msgs::PoseStamped& goal_pose, con const auto& joint_names = joint_model_group_->getLinkModelNames(); if (std::find(joint_names.begin(), joint_names.end(), link_name) == joint_names.end()) { - ROS_ERROR_NAMED(LOGNAME, "Link '%s' is not part of joint model group '%s'.", link_name.c_str(), group_name_.c_str()); + ROS_ERROR_NAMED(LOGNAME, "Link '%s' is not part of joint model group '%s'.", link_name.c_str(), + group_name_.c_str()); return false; } current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(link_name, goal_pose) }; @@ -308,8 +308,9 @@ bool PlanningComponent::execute(bool blocking) } // TODO(henningkayser): parameterize timestamps if required - //trajectory_processing::TimeOptimalTrajectoryGeneration totg; - //if (!totg.computeTimeStamps(*last_solution_trajectory_, max_velocity_scaling_factor_, max_acceleration_scaling_factor_)) + // trajectory_processing::TimeOptimalTrajectoryGeneration totg; + // if (!totg.computeTimeStamps(*last_solution_trajectory_, max_velocity_scaling_factor_, + // max_acceleration_scaling_factor_)) //{ // ROS_ERROR("Failed to parameterize trajectory"); // return false; From 32bdaaf080b130689c39f6f77f32c438ba38932a Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 22 Aug 2019 21:03:05 +0200 Subject: [PATCH 23/29] Remove debug messages --- moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp index 1bbf1e4ebe..5ccff35abb 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp @@ -317,15 +317,10 @@ bool MoveitCpp::execute(const std::string& group_name, const robot_trajectory::R void MoveitCpp::clearContents() { - ROS_INFO_NAMED(LOGNAME, "robot_description"); robot_description_.clear(); - ROS_INFO_NAMED(LOGNAME, "tf_buffer_"); tf_buffer_.reset(); - ROS_INFO_NAMED(LOGNAME, "planning_scene_monitor"); planning_scene_monitor_.reset(); - ROS_INFO_NAMED(LOGNAME, "robot_model"); robot_model_.reset(); - ROS_INFO_NAMED(LOGNAME, "pipelines"); planning_pipelines_.clear(); } } // planning_interface From 5e1f4d0c519aeb8f00d7d21225b51fb0074c3927 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 30 Aug 2019 17:40:35 +0200 Subject: [PATCH 24/29] Make options accessible + fix TF --- .../include/moveit/moveit_cpp/moveit_cpp.h | 62 ++++++++++--------- .../moveit_cpp/src/moveit.cpp | 20 ++++-- .../moveit_cpp/src/planning_component.cpp | 13 ++-- 3 files changed, 54 insertions(+), 41 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index c0af077a34..7a36ae39a0 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -41,6 +41,7 @@ #include #include #include +#include namespace moveit { @@ -75,6 +76,31 @@ class MoveitCpp std::string monitored_planning_scene_topic; std::string publish_planning_scene_topic; }; + struct PlanningPipelineOptions + { + void load(const ros::NodeHandle& nh) + { + std::string ns = "planning_pipelines/"; + nh.getParam(ns + "pipeline_names", pipeline_names); + nh.getParam(ns + "namespace", parent_namespace); + }; + std::vector pipeline_names; + std::string parent_namespace; + }; + struct PlannerOptions + { + void load(const ros::NodeHandle& nh) + { + std::string ns = "default_planner_options/"; + nh.getParam(ns + "planning_attempts", planning_attempts); + nh.getParam(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor); + nh.getParam(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor); + }; + int planning_attempts; + double planning_time; + double max_velocity_scaling_factor; + double max_acceleration_scaling_factor; + }; /// Parameter container for initializing MoveitCpp struct Options @@ -85,40 +111,15 @@ class MoveitCpp default_planner_options.load(nh); planning_pipeline_options.load(nh); } - PlanningSceneMonitorOptions planning_scene_monitor_options; - struct PlanningPipelineOptions - { - void load(const ros::NodeHandle& nh) - { - std::string ns = "planning_pipeline_options/"; - nh.getParam(ns + "pipeline_names", pipeline_names); - }; - std::vector pipeline_names; - }; + PlanningSceneMonitorOptions planning_scene_monitor_options; PlanningPipelineOptions planning_pipeline_options; - struct PlannerOptions - { - void load(const ros::NodeHandle& nh) - { - std::string ns = "default_planner_options/"; - nh.getParam(ns + "planning_attempts", planning_attempts); - nh.getParam(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor); - nh.getParam(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor); - }; - int planning_attempts; - double planning_time; - double max_velocity_scaling_factor; - double max_acceleration_scaling_factor; - }; PlannerOptions default_planner_options; }; /** \brief Constructor */ - MoveitCpp(const ros::NodeHandle& nh, - const std::shared_ptr& tf_buffer = std::shared_ptr()); - MoveitCpp(const Options& opt, const ros::NodeHandle& nh, - const std::shared_ptr& tf_buffer = std::shared_ptr()); + MoveitCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); + MoveitCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); /** * @brief This class owns unique resources (e.g. action clients, threads) and its not very @@ -157,6 +158,8 @@ class MoveitCpp const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const; planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst(); + const std::shared_ptr& getTFBuffer() const; + /** \brief Get the stored instance of the trajectory execution manager */ const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const; trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst(); @@ -168,6 +171,7 @@ class MoveitCpp protected: std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; private: // Core properties and instances @@ -191,7 +195,7 @@ class MoveitCpp bool loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt); /** \brief Initialize and setup the planning pipelines */ - bool loadPlanningPipelines(std::vector pipeline_names); + bool loadPlanningPipelines(const PlanningPipelineOptions& opt); }; } // namespace planning_interface } // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp index 5ccff35abb..38699be121 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp @@ -59,8 +59,11 @@ MoveitCpp::MoveitCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) - : tf_buffer_(tf_buffer) { + if (!tf_buffer_) + tf_buffer_.reset(new tf2_ros::Buffer()); + tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_)); + // Configure planning scene monitor if (!loadPlanningSceneMonitor(opt.planning_scene_monitor_options)) { @@ -79,7 +82,7 @@ MoveitCpp::MoveitCpp(const Options& opt, const ros::NodeHandle& nh, const std::s } bool load_planning_pipelines = true; - if (load_planning_pipelines && !loadPlanningPipelines(opt.planning_pipeline_options.pipeline_names)) + if (load_planning_pipelines && !loadPlanningPipelines(opt.planning_pipeline_options)) { std::string error = "Failed to load planning pipelines from parameter server"; ROS_FATAL_STREAM_NAMED(LOGNAME, error); @@ -155,10 +158,10 @@ bool MoveitCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt) return true; } -bool MoveitCpp::loadPlanningPipelines(std::vector pipeline_names) +bool MoveitCpp::loadPlanningPipelines(const PlanningPipelineOptions& opt) { - ros::NodeHandle pnh("~"); - for (const auto& planning_pipeline_name : pipeline_names) + ros::NodeHandle node_handle(opt.parent_namespace.empty() ? "~" : opt.parent_namespace); + for (const auto& planning_pipeline_name : opt.pipeline_names) { if (planning_pipelines_.count(planning_pipeline_name) > 0) { @@ -166,7 +169,7 @@ bool MoveitCpp::loadPlanningPipelines(std::vector pipeline_names) continue; } ROS_INFO_NAMED(LOGNAME, "Loading planning pipeline '%s'", planning_pipeline_name.c_str()); - ros::NodeHandle child_nh(pnh, planning_pipeline_name); + ros::NodeHandle child_nh(node_handle, planning_pipeline_name); planning_pipeline::PlanningPipelinePtr pipeline; pipeline.reset(new planning_pipeline::PlanningPipeline(robot_model_, child_nh, PLANNING_PLUGIN_PARAM)); @@ -315,6 +318,11 @@ bool MoveitCpp::execute(const std::string& group_name, const robot_trajectory::R return true; } +const std::shared_ptr& MoveitCpp::getTFBuffer() const +{ + return tf_buffer_; +} + void MoveitCpp::clearContents() { robot_description_.clear(); diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp index 87eef45de7..620a113f8e 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -137,6 +137,7 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) // Clone current planning scene planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitorNonConst(); + planning_scene_monitor->updateFrameTransforms(); planning_scene_monitor->lockSceneRead(); // LOCK planning scene planning_scene::PlanningScenePtr planning_scene = planning_scene::PlanningScene::clone(planning_scene_monitor->getPlanningScene()); @@ -276,12 +277,12 @@ bool PlanningComponent::setGoal(const robot_state::RobotState& goal_state) bool PlanningComponent::setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name) { const auto& joint_names = joint_model_group_->getLinkModelNames(); - if (std::find(joint_names.begin(), joint_names.end(), link_name) == joint_names.end()) - { - ROS_ERROR_NAMED(LOGNAME, "Link '%s' is not part of joint model group '%s'.", link_name.c_str(), - group_name_.c_str()); - return false; - } + // if (std::find(joint_names.begin(), joint_names.end(), link_name) == joint_names.end()) + //{ + // ROS_ERROR_NAMED(LOGNAME, "Link '%s' is not part of joint model group '%s'.", link_name.c_str(), + // group_name_.c_str()); + // return false; + //} current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(link_name, goal_pose) }; return true; } From 4180151bbabb9ba5512f529edc9d29835497b08f Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Wed, 25 Sep 2019 14:05:30 +0300 Subject: [PATCH 25/29] Initial unit tests for MoveItCpp (#4) --- .../include/moveit/moveit_cpp/moveit_cpp.h | 8 +- .../moveit/moveit_cpp/planning_component.h | 48 ++++- .../moveit_cpp/src/moveit.cpp | 8 +- .../moveit_cpp/src/planning_component.cpp | 34 ++-- .../planning_interface/test/CMakeLists.txt | 2 + .../planning_interface/test/moveit_cpp.yaml | 17 ++ .../test/moveit_cpp_test.cpp | 178 ++++++++++++++++++ .../test/moveit_cpp_test.test | 34 ++++ 8 files changed, 305 insertions(+), 24 deletions(-) create mode 100644 moveit_ros/planning_interface/test/moveit_cpp.yaml create mode 100644 moveit_ros/planning_interface/test/moveit_cpp_test.cpp create mode 100644 moveit_ros/planning_interface/test/moveit_cpp_test.test diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 7a36ae39a0..7d87109ca8 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -68,13 +68,15 @@ class MoveitCpp planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC); nh.param(ns + "publish_planning_scene_topic", publish_planning_scene_topic, planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC); - }; + nh.param(ns + "wait_for_initial_state_timeout", wait_for_initial_state_timeout, 0.0); + } std::string name; std::string robot_description; std::string joint_state_topic; std::string attached_collision_object_topic; std::string monitored_planning_scene_topic; std::string publish_planning_scene_topic; + double wait_for_initial_state_timeout; }; struct PlanningPipelineOptions { @@ -83,7 +85,7 @@ class MoveitCpp std::string ns = "planning_pipelines/"; nh.getParam(ns + "pipeline_names", pipeline_names); nh.getParam(ns + "namespace", parent_namespace); - }; + } std::vector pipeline_names; std::string parent_namespace; }; @@ -95,7 +97,7 @@ class MoveitCpp nh.getParam(ns + "planning_attempts", planning_attempts); nh.getParam(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor); nh.getParam(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor); - }; + } int planning_attempts; double planning_time; double max_velocity_scaling_factor; diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 8595763668..fb99033ecd 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -42,6 +42,7 @@ #include #include #include +#include namespace moveit { @@ -54,6 +55,35 @@ class PlanningComponent public: MOVEIT_STRUCT_FORWARD(PlanSolution); + class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes + { + public: + MoveItErrorCode() + { + val = 0; + } + MoveItErrorCode(int code) + { + val = code; + } + MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code) + { + val = code.val; + } + explicit operator bool() const + { + return val == moveit_msgs::MoveItErrorCodes::SUCCESS; + } + bool operator==(const int c) const + { + return val == c; + } + bool operator!=(const int c) const + { + return val != c; + } + }; + /// The representation of a plan solution struct PlanSolution { @@ -62,6 +92,14 @@ class PlanningComponent /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) robot_trajectory::RobotTrajectoryPtr trajectory; + + /// Error code + MoveItErrorCode error_code; + + explicit operator bool() const + { + return bool(error_code); + } }; /// Planner parameters provided with the MotionPlanRequest @@ -133,21 +171,25 @@ class PlanningComponent /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using * default parameters. */ - bool plan(); + PlanSolution plan(); /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the * provided PlanRequestParameters. */ - bool plan(const PlanRequestParameters& parameters); + PlanSolution plan(const PlanRequestParameters& parameters); /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates * after the execution is complete. The execution can be run in background by setting blocking to false. */ bool execute(bool blocking = true); + /** \brief Return the last plan solution*/ + const PlanSolutionPtr getLastPlanSolution(); + private: // Core properties and instances ros::NodeHandle nh_; MoveitCppPtr moveit_cpp_; const std::string group_name_; - moveit::core::JointModelGroupConstPtr joint_model_group_; + // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources + const moveit::core::JointModelGroup* joint_model_group_; // Planning std::set planning_pipeline_names_; diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp index 38699be121..8ec4eb5a32 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp @@ -147,12 +147,10 @@ bool MoveitCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt) ros::Duration(0.5).sleep(); // when at 0.1, i believe sometimes vjoint not properly loaded // Wait for complete state to be recieved - // TODO(henningkayser): parameterize - double wait_for_complete_state_timeout = 10.0; - // Break early - if (wait_for_complete_state_timeout > 0.0) + if (opt.wait_for_initial_state_timeout > 0.0) { - return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(); + return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), + opt.wait_for_initial_state_timeout); } return true; diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp index 620a113f8e..e490f81694 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -73,7 +73,7 @@ constexpr char LOGNAME[] = "planning_component"; PlanningComponent::PlanningComponent(const std::string& group_name, const MoveitCppPtr& moveit_context) : group_name_(group_name), nh_(moveit_context->getNodeHandle()), moveit_cpp_(moveit_context) { - joint_model_group_.reset(moveit_cpp_->getRobotModel()->getJointModelGroup(group_name)); + joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name); if (!joint_model_group_) { std::string error = "Could not find joint model group '" + group_name + "'."; @@ -126,12 +126,14 @@ const std::string& PlanningComponent::getName() const return group_name_; } -bool PlanningComponent::plan(const PlanRequestParameters& parameters) +PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParameters& parameters) { + last_plan_solution_.reset(new PlanSolution()); if (!joint_model_group_) { ROS_ERROR_NAMED(LOGNAME, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); - return false; + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); + return *last_plan_solution_; } // Clone current planning scene @@ -165,7 +167,8 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) if (current_goal_constraints_.empty()) { ROS_ERROR_NAMED(LOGNAME, "No goal constraints set for planning request"); - return false; + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + return *last_plan_solution_; } req.goal_constraints = current_goal_constraints_; @@ -174,17 +177,18 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end()) { ROS_ERROR_NAMED(LOGNAME, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); - return false; + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return *last_plan_solution_; } const planning_pipeline::PlanningPipelinePtr pipeline = moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline); pipeline->generatePlan(planning_scene, req, res); + last_plan_solution_->error_code = res.error_code_.val; if (res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); - return false; + return *last_plan_solution_; } - last_plan_solution_.reset(new PlanSolution()); last_plan_solution_->start_state = req.start_state; last_plan_solution_->trajectory = res.trajectory_; // TODO(henningkayser): Visualize trajectory @@ -199,10 +203,10 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT); // } //} - return true; + return *last_plan_solution_; } -bool PlanningComponent::plan() +PlanningComponent::PlanSolution PlanningComponent::plan() { PlanRequestParameters default_parameters; default_parameters.planning_attempts = 1; @@ -270,7 +274,7 @@ bool PlanningComponent::setGoal(const std::vector& goa bool PlanningComponent::setGoal(const robot_state::RobotState& goal_state) { - current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_.get()) }; + current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_) }; return true; } @@ -296,7 +300,7 @@ bool PlanningComponent::setGoal(const std::string& goal_state_name) return false; } robot_state::RobotState goal_state(moveit_cpp_->getRobotModel()); - goal_state.setToDefaultValues(joint_model_group_.get(), goal_state_name); + goal_state.setToDefaultValues(joint_model_group_, goal_state_name); return setGoal(goal_state); } @@ -316,7 +320,12 @@ bool PlanningComponent::execute(bool blocking) // ROS_ERROR("Failed to parameterize trajectory"); // return false; //} - moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking); + return moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking); +} + +const PlanningComponent::PlanSolutionPtr PlanningComponent::getLastPlanSolution() +{ + return last_plan_solution_; } void PlanningComponent::clearContents() @@ -324,7 +333,6 @@ void PlanningComponent::clearContents() considered_start_state_.reset(); last_plan_solution_.reset(); current_goal_constraints_.clear(); - joint_model_group_.reset(); moveit_cpp_.reset(); planning_pipeline_names_.clear(); } diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 979b4e1f92..7228694591 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -5,6 +5,8 @@ if (CATKIN_ENABLE_TESTING) add_executable(test_cleanup test_cleanup.cpp) target_link_libraries(test_cleanup moveit_move_group_interface) + add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) + target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) add_rostest(python_move_group.test) add_rostest(python_move_group_ns.test) add_rostest(robot_state_update.test) diff --git a/moveit_ros/planning_interface/test/moveit_cpp.yaml b/moveit_ros/planning_interface/test/moveit_cpp.yaml new file mode 100644 index 0000000000..910b85c700 --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp.yaml @@ -0,0 +1,17 @@ +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/planning_scene_monitor" + publish_planning_scene_topic: "/publish_planning_scene" + monitored_planning_scene_topic: "/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +planning_pipelines: + pipeline_names: + - ompl + +default_planner_options: + planning_attempts: 1 + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 \ No newline at end of file diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp new file mode 100644 index 0000000000..0b956ac074 --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp @@ -0,0 +1,178 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jafar Abdi + Desc: Test the MoveItCpp and PlanningComponent interfaces +*/ + +// ROS +#include + +// Testing +#include + +// Main class +#include +#include +// Msgs +#include + +namespace moveit +{ +namespace planning_interface +{ +class MoveItCppTest : public ::testing::Test +{ +public: + void SetUp() override + { + nh_ = ros::NodeHandle("/moveit_cpp_test"); + moveit_cpp_ptr = std::make_shared(nh_); + planning_component_ptr = std::make_shared(PLANNING_GROUP, moveit_cpp_ptr); + robot_model_ptr = moveit_cpp_ptr->getRobotModel(); + jmg_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); + + target_pose1.header.frame_id = "panda_link0"; + target_pose1.pose.orientation.w = 1.0; + target_pose1.pose.position.x = 0.28; + target_pose1.pose.position.y = -0.2; + target_pose1.pose.position.z = 0.5; + + start_pose.orientation.w = 1.0; + start_pose.position.x = 0.55; + start_pose.position.y = 0.0; + start_pose.position.z = 0.6; + + target_pose2.orientation.w = 1.0; + target_pose2.position.x = 0.55; + target_pose2.position.y = -0.05; + target_pose2.position.z = 0.8; + } + +protected: + ros::NodeHandle nh_; + MoveitCppPtr moveit_cpp_ptr; + PlanningComponentPtr planning_component_ptr; + robot_model::RobotModelConstPtr robot_model_ptr; + const moveit::core::JointModelGroup* jmg_ptr; + const std::string PLANNING_GROUP = "panda_arm"; + geometry_msgs::PoseStamped target_pose1; + geometry_msgs::Pose target_pose2; + geometry_msgs::Pose start_pose; +}; + +// Test the current and the initial state of the Panda robot +TEST_F(MoveItCppTest, GetCurrentStateTest) +{ + ros::Duration(1.0).sleep(); // Otherwise joint_states will result in an invalid robot state + auto robot_model = moveit_cpp_ptr->getRobotModel(); + auto robot_state = std::make_shared(robot_model); + EXPECT_TRUE(moveit_cpp_ptr->getCurrentState(robot_state, 0.0)); + // Make sure the Panda robot is in "ready" state which is loaded from fake_controller.yaml + std::vector joints_vals; + robot_state->copyJointGroupPositions(PLANNING_GROUP, joints_vals); + EXPECT_NEAR(joints_vals[0], 0.0, 0.001); // panda_joint1 + EXPECT_NEAR(joints_vals[1], -0.785, 0.001); // panda_joint2 + EXPECT_NEAR(joints_vals[2], 0.0, 0.001); // panda_joint3 + EXPECT_NEAR(joints_vals[3], -2.356, 0.001); // panda_joint4 + EXPECT_NEAR(joints_vals[4], 0.0, 0.001); // panda_joint5 + EXPECT_NEAR(joints_vals[5], 1.571, 0.001); // panda_joint6 + EXPECT_NEAR(joints_vals[6], 0.785, 0.001); // panda_joint7 +} + +// Test the name of the planning group used by PlanningComponent for the Panda robot +TEST_F(MoveItCppTest, NameOfPlanningGroupTest) +{ + EXPECT_STREQ(planning_component_ptr->getName().c_str(), "panda_arm"); +} + +// Test setting the start state of the plan to the current state of the robot +TEST_F(MoveItCppTest, TestSetStartStateToCurrentState) +{ + planning_component_ptr->setStartStateToCurrentState(); + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(planning_component_ptr->plan()); + // TODO(JafarAbdi) adding testing to the soln state +} + +// Test setting the goal using geometry_msgs::PoseStamped and a robot's link name +TEST_F(MoveItCppTest, TestSetGoalFromPoseStamped) +{ + planning_component_ptr->setStartStateToCurrentState(); + + geometry_msgs::PoseStamped target_pose1; + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(planning_component_ptr->plan()); +} + +// Test setting the plan start state using robot_state::RobotState +TEST_F(MoveItCppTest, TestSetStartStateFromRobotState) +{ + auto start_state = *(moveit_cpp_ptr->getCurrentState()); + start_state.setFromIK(jmg_ptr, start_pose); + + planning_component_ptr->setStartState(start_state); + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(planning_component_ptr->plan()); +} + +// Test settting the goal of the plan using a robot_state::RobotState +TEST_F(MoveItCppTest, TestSetGoalFromRobotState) +{ + auto target_state = *(moveit_cpp_ptr->getCurrentState()); + + target_state.setFromIK(jmg_ptr, target_pose2); + + planning_component_ptr->setGoal(target_state); + + ASSERT_TRUE(planning_component_ptr->plan()); +} +} // namespace planning_interface +} // namespace moveit + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "moveit_cpp_test"); + + ros::AsyncSpinner spinner(4); + spinner.start(); + + int result = RUN_ALL_TESTS(); + + return result; +} diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.test b/moveit_ros/planning_interface/test/moveit_cpp_test.test new file mode 100644 index 0000000000..265fb337dc --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.test @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + ["/moveit_cpp_test/fake_controller_joint_states"] + + + + + + + + From 907b7136fcac1db35f3c5253201d0a0403a1e3aa Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 25 Sep 2019 13:32:42 +0200 Subject: [PATCH 26/29] Set start state from named target --- .../moveit/moveit_cpp/planning_component.h | 4 ++-- .../moveit_cpp/src/planning_component.cpp | 16 +++++++++++++++- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index fb99033ecd..a9c994c538 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -153,9 +153,9 @@ class PlanningComponent robot_state::RobotStatePtr getStartState(); /** \brief Set the robot state that should be considered as start state for planning */ - void setStartState(const robot_state::RobotState& start_state); + bool setStartState(const robot_state::RobotState& start_state); /** \brief Set the named robot state that should be considered as start state for planning */ - void setStartState(const std::string& named_state); + bool setStartState(const std::string& named_state); /** \brief Set the start state for planning to be the current state of the robot */ void setStartStateToCurrentState(); diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp index e490f81694..e81933267d 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -218,9 +218,10 @@ PlanningComponent::PlanSolution PlanningComponent::plan() return plan(default_parameters); } -void PlanningComponent::setStartState(const robot_state::RobotState& start_state) +bool PlanningComponent::setStartState(const robot_state::RobotState& start_state) { considered_start_state_.reset(new robot_state::RobotState(start_state)); + return true; } robot_state::RobotStatePtr PlanningComponent::getStartState() @@ -235,6 +236,19 @@ robot_state::RobotStatePtr PlanningComponent::getStartState() } } +bool PlanningComponent::setStartState(const std::string& start_state_name) +{ + const auto& named_targets = getNamedTargets(); + if (std::find(named_targets.begin(), named_targets.end(), start_state_name) == named_targets.end()) + { + ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", start_state_name.c_str()); + return false; + } + robot_state::RobotState start_state(moveit_cpp_->getRobotModel()); + start_state.setToDefaultValues(joint_model_group_, start_state_name); + return setStartState(start_state); +} + void PlanningComponent::setStartStateToCurrentState() { considered_start_state_.reset(); From 21dbc3c3608d41f4f80381e72508f9e3f7160490 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 25 Sep 2019 13:55:31 +0200 Subject: [PATCH 27/29] Rename MoveitCpp -> MoveItCpp --- .../moveit_cpp/CMakeLists.txt | 2 +- .../include/moveit/moveit_cpp/moveit_cpp.h | 22 ++++---- .../moveit/moveit_cpp/planning_component.h | 4 +- .../src/{moveit.cpp => moveit_cpp.cpp} | 50 +++++++++---------- .../moveit_cpp/src/planning_component.cpp | 4 +- 5 files changed, 41 insertions(+), 41 deletions(-) rename moveit_ros/planning_interface/moveit_cpp/src/{moveit.cpp => moveit_cpp.cpp} (88%) diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt index 45ccc22b0e..098becde4b 100644 --- a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -1,6 +1,6 @@ set(MOVEIT_LIB_NAME moveit_cpp) -add_library(${MOVEIT_LIB_NAME} src/moveit.cpp src/planning_component.cpp) +add_library(${MOVEIT_LIB_NAME} src/moveit_cpp.cpp src/planning_component.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 7d87109ca8..fd81721ec5 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -47,12 +47,12 @@ namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(MoveitCpp); +MOVEIT_CLASS_FORWARD(MoveItCpp); -class MoveitCpp +class MoveItCpp { public: - /// Specification of options to use when constructing the MoveitCpp class + /// Specification of options to use when constructing the MoveItCpp class struct PlanningSceneMonitorOptions { void load(const ros::NodeHandle& nh) @@ -104,7 +104,7 @@ class MoveitCpp double max_acceleration_scaling_factor; }; - /// Parameter container for initializing MoveitCpp + /// Parameter container for initializing MoveItCpp struct Options { Options(const ros::NodeHandle& nh) @@ -120,22 +120,22 @@ class MoveitCpp }; /** \brief Constructor */ - MoveitCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); - MoveitCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); + MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); + MoveItCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); /** * @brief This class owns unique resources (e.g. action clients, threads) and its not very * meaningful to copy. Pass by references, move it, or simply create multiple instances where * required. */ - MoveitCpp(const MoveitCpp&) = delete; - MoveitCpp& operator=(const MoveitCpp&) = delete; + MoveItCpp(const MoveItCpp&) = delete; + MoveItCpp& operator=(const MoveItCpp&) = delete; - MoveitCpp(MoveitCpp&& other); - MoveitCpp& operator=(MoveitCpp&& other); + MoveItCpp(MoveItCpp&& other); + MoveItCpp& operator=(MoveItCpp&& other); /** \brief Destructor */ - ~MoveitCpp(); + ~MoveItCpp(); /** \brief Get the RobotModel object. */ robot_model::RobotModelConstPtr getRobotModel() const; diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index a9c994c538..06d8124edf 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -115,7 +115,7 @@ class PlanningComponent /** \brief Constructor */ PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh); - PlanningComponent(const std::string& group_name, const MoveitCppPtr& moveit_context); + PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_context); /** * @brief This class owns unique resources (e.g. action clients, threads) and its not very @@ -186,7 +186,7 @@ class PlanningComponent private: // Core properties and instances ros::NodeHandle nh_; - MoveitCppPtr moveit_cpp_; + MoveItCppPtr moveit_cpp_; const std::string group_name_; // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources const moveit::core::JointModelGroup* joint_model_group_; diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp similarity index 88% rename from moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp rename to moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp index 8ec4eb5a32..f27efed06b 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -53,12 +53,12 @@ constexpr char LOGNAME[] = "moveit_cpp"; constexpr char PLANNING_SCENE_MONITOR_NAME[] = "moveit_cpp_planning_scene"; constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin"; -MoveitCpp::MoveitCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) - : MoveitCpp(Options(nh), nh, tf_buffer) +MoveItCpp::MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) + : MoveItCpp(Options(nh), nh, tf_buffer) { } -MoveitCpp::MoveitCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) +MoveItCpp::MoveItCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) { if (!tf_buffer_) tf_buffer_.reset(new tf2_ros::Buffer()); @@ -93,21 +93,21 @@ MoveitCpp::MoveitCpp(const Options& opt, const ros::NodeHandle& nh, const std::s trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager( robot_model_, planning_scene_monitor_->getStateMonitor())); - ROS_INFO_NAMED(LOGNAME, "MoveitCpp running"); + ROS_INFO_NAMED(LOGNAME, "MoveItCpp running"); } -MoveitCpp::MoveitCpp(MoveitCpp&& other) +MoveItCpp::MoveItCpp(MoveItCpp&& other) { other.clearContents(); } -MoveitCpp::~MoveitCpp() +MoveItCpp::~MoveItCpp() { - ROS_INFO_NAMED(LOGNAME, "Deleting MoveitCpp"); + ROS_INFO_NAMED(LOGNAME, "Deleting MoveItCpp"); clearContents(); } -MoveitCpp& MoveitCpp::operator=(MoveitCpp&& other) +MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) { if (this != &other) { @@ -122,7 +122,7 @@ MoveitCpp& MoveitCpp::operator=(MoveitCpp&& other) return *this; } -bool MoveitCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt) +bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt) { planning_scene_monitor_.reset( new planning_scene_monitor::PlanningSceneMonitor(opt.robot_description, tf_buffer_, opt.name)); @@ -156,7 +156,7 @@ bool MoveitCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt) return true; } -bool MoveitCpp::loadPlanningPipelines(const PlanningPipelineOptions& opt) +bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& opt) { ros::NodeHandle node_handle(opt.parent_namespace.empty() ? "~" : opt.parent_namespace); for (const auto& planning_pipeline_name : opt.pipeline_names) @@ -206,19 +206,19 @@ bool MoveitCpp::loadPlanningPipelines(const PlanningPipelineOptions& opt) return true; } -robot_model::RobotModelConstPtr MoveitCpp::getRobotModel() const +robot_model::RobotModelConstPtr MoveItCpp::getRobotModel() const { - ROS_DEBUG_NAMED(LOGNAME, "MoveitCpp::getRobotModel()"); + ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp::getRobotModel()"); return robot_model_; } -const ros::NodeHandle& MoveitCpp::getNodeHandle() const +const ros::NodeHandle& MoveItCpp::getNodeHandle() const { - ROS_DEBUG_NAMED(LOGNAME, "MoveitCpp::getNodeHandle()"); + ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp::getNodeHandle()"); return node_handle_; } -bool MoveitCpp::getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds) +bool MoveItCpp::getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds) { if (wait_seconds > 0.0 && !planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), wait_seconds)) @@ -233,19 +233,19 @@ bool MoveitCpp::getCurrentState(robot_state::RobotStatePtr& current_state, doubl return true; } -robot_state::RobotStatePtr MoveitCpp::getCurrentState(double wait) +robot_state::RobotStatePtr MoveItCpp::getCurrentState(double wait) { robot_state::RobotStatePtr current_state; getCurrentState(current_state, wait); return current_state; } -const std::map& MoveitCpp::getPlanningPipelines() const +const std::map& MoveItCpp::getPlanningPipelines() const { return planning_pipelines_; } -std::set MoveitCpp::getPlanningPipelineNames(const std::string& group_name) const +std::set MoveItCpp::getPlanningPipelineNames(const std::string& group_name) const { std::set result_names; if (!group_name.empty() && groups_pipelines_map_.count(group_name) == 0) @@ -268,26 +268,26 @@ std::set MoveitCpp::getPlanningPipelineNames(const std::string& gro return result_names; } -const planning_scene_monitor::PlanningSceneMonitorPtr& MoveitCpp::getPlanningSceneMonitor() const +const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const { return planning_scene_monitor_; } -planning_scene_monitor::PlanningSceneMonitorPtr MoveitCpp::getPlanningSceneMonitorNonConst() +planning_scene_monitor::PlanningSceneMonitorPtr MoveItCpp::getPlanningSceneMonitorNonConst() { return planning_scene_monitor_; } -const trajectory_execution_manager::TrajectoryExecutionManagerPtr& MoveitCpp::getTrajectoryExecutionManager() const +const trajectory_execution_manager::TrajectoryExecutionManagerPtr& MoveItCpp::getTrajectoryExecutionManager() const { return trajectory_execution_manager_; } -trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveitCpp::getTrajectoryExecutionManagerNonConst() +trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveItCpp::getTrajectoryExecutionManagerNonConst() { return trajectory_execution_manager_; } -bool MoveitCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, +bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, bool blocking) { if (!robot_trajectory) @@ -316,12 +316,12 @@ bool MoveitCpp::execute(const std::string& group_name, const robot_trajectory::R return true; } -const std::shared_ptr& MoveitCpp::getTFBuffer() const +const std::shared_ptr& MoveItCpp::getTFBuffer() const { return tf_buffer_; } -void MoveitCpp::clearContents() +void MoveItCpp::clearContents() { robot_description_.clear(); tf_buffer_.reset(); diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp index e81933267d..2ce083b17d 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -70,7 +70,7 @@ namespace planning_interface { constexpr char LOGNAME[] = "planning_component"; -PlanningComponent::PlanningComponent(const std::string& group_name, const MoveitCppPtr& moveit_context) +PlanningComponent::PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_context) : group_name_(group_name), nh_(moveit_context->getNodeHandle()), moveit_cpp_(moveit_context) { joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name); @@ -84,7 +84,7 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const Moveit } PlanningComponent::PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh) - : group_name_(group_name), nh_(nh), moveit_cpp_(new MoveitCpp(nh)) + : group_name_(group_name), nh_(nh), moveit_cpp_(new MoveItCpp(nh)) { } From 7d18c803cb3307558d4904c93c35bb424c418ffc Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 25 Sep 2019 16:11:27 +0200 Subject: [PATCH 28/29] Rename MoveitCpp -> MoveItCpp in unit test --- moveit_ros/planning_interface/test/moveit_cpp_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp index 0b956ac074..13f17bd09a 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp @@ -58,7 +58,7 @@ class MoveItCppTest : public ::testing::Test void SetUp() override { nh_ = ros::NodeHandle("/moveit_cpp_test"); - moveit_cpp_ptr = std::make_shared(nh_); + moveit_cpp_ptr = std::make_shared(nh_); planning_component_ptr = std::make_shared(PLANNING_GROUP, moveit_cpp_ptr); robot_model_ptr = moveit_cpp_ptr->getRobotModel(); jmg_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); @@ -82,7 +82,7 @@ class MoveItCppTest : public ::testing::Test protected: ros::NodeHandle nh_; - MoveitCppPtr moveit_cpp_ptr; + MoveItCppPtr moveit_cpp_ptr; PlanningComponentPtr planning_component_ptr; robot_model::RobotModelConstPtr robot_model_ptr; const moveit::core::JointModelGroup* jmg_ptr; From cd4a455d09779de45bca54f3a927f6e5527a75d0 Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Mon, 14 Oct 2019 18:28:26 -0600 Subject: [PATCH 29/29] fixing jog_arm segfault --- moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 0ff378a533..7b38375301 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -568,6 +568,7 @@ void JogCalcs::enforceJointVelocityLimits(Eigen::VectorXd& calculated_joint_chan bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& new_jt_traj) { bool halting = false; + for (auto joint : joint_model_group_->getJointModels()) { if (!kinematic_state_->satisfiesVelocityBounds(joint)) @@ -575,8 +576,9 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, ros::this_node::getName() << " " << joint->getName() << " " << " close to a " " velocity limit. Enforcing limit."); + kinematic_state_->enforceVelocityBounds(joint); - for (std::size_t c = 0; c < num_joints_; ++c) + for (std::size_t c = 0; c < new_jt_traj.points[0].velocities.size(); ++c) { if (new_jt_traj.joint_names[c] == joint->getName()) { @@ -588,7 +590,7 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n // Halt if we're past a joint margin and joint velocity is moving even farther past double joint_angle = 0; - for (std::size_t c = 0; c < num_joints_; ++c) + for (std::size_t c = 0; c < original_jt_state_.name.size(); ++c) { if (original_jt_state_.name[c] == joint->getName()) { @@ -596,7 +598,6 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n break; } } - if (!kinematic_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin)) { const std::vector limits = joint->getVariableBoundsMsg();