Skip to content

Commit

Permalink
MGI: Expose constructPickGoal and constructPlaceGoal (moveit#1498)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mvieth @v4hn
mvieth authored and v4hn committed Mar 31, 2020
1 parent 220ef34 commit a911665
Show file tree
Hide file tree
Showing 4 changed files with 113 additions and 84 deletions.
75 changes: 59 additions & 16 deletions ...interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,8 @@
#include <moveit_msgs/Constraints.h>
#include <moveit_msgs/Grasp.h>
#include <moveit_msgs/PlaceLocation.h>
#include <moveit_msgs/PickupGoal.h>
#include <moveit_msgs/PlaceGoal.h>
#include <moveit_msgs/MotionPlanRequest.h>
#include <moveit_msgs/MoveGroupAction.h>
#include <geometry_msgs/PoseStamped.h>
Expand Down Expand Up @@ -245,14 +247,14 @@ class MoveGroupInterface
/** \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
(importantly, 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
(importantly, 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);

Expand Down Expand Up @@ -755,6 +757,17 @@ class MoveGroupInterface
in \e request */
void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request);

/** \brief Build a PickupGoal for an object named \e object and store it in \e goal_out */
moveit_msgs::PickupGoal constructPickupGoal(const std::string& object, std::vector<moveit_msgs::Grasp> grasps,
bool plan_only);

/** \brief Build a PlaceGoal for an object named \e object and store it in \e goal_out */
moveit_msgs::PlaceGoal constructPlaceGoal(const std::string& object,
std::vector<moveit_msgs::PlaceLocation> locations, bool plan_only);

/** \brief Convert a vector of PoseStamped to a vector of PlaceLocation */
std::vector<moveit_msgs::PlaceLocation> posesToPlaceLocations(const std::vector<geometry_msgs::PoseStamped>& poses);

/**@}*/

/**
Expand All @@ -765,16 +778,28 @@ class MoveGroupInterface
/** \brief Pick up an object
This applies a number of hard-coded default grasps */
MoveItErrorCode pick(const std::string& object, bool plan_only = false);
MoveItErrorCode pick(const std::string& object, bool plan_only = false)
{
return pick(constructPickupGoal(object, std::vector<moveit_msgs::Grasp>(), plan_only));
}

/** \brief Pick up an object given a grasp pose */
MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only = false);
MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only = false)
{
return pick(constructPickupGoal(object, { grasp }, plan_only));
}

/** \brief Pick up an object given possible grasp poses */
MoveItErrorCode pick(const std::string& object, std::vector<moveit_msgs::Grasp> grasps, bool plan_only = false)
{
return pick(constructPickupGoal(object, std::move(grasps), plan_only));
}

/** \brief Pick up an object given possible grasp poses
/** \brief Pick up an object given a PickupGoal
if the vector is left empty this behaves like pick(const std::string &object) */
MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps,
bool plan_only = false);
Use as follows: first create the goal with constructPickupGoal(), then set \e possible_grasps and any other
desired variable in the goal, and finally pass it on to this function */
MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal);

/** \brief Pick up an object
Expand All @@ -787,18 +812,36 @@ class MoveGroupInterface
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);
MoveItErrorCode place(const std::string& object, bool plan_only = false)
{
return place(constructPlaceGoal(object, std::vector<moveit_msgs::PlaceLocation>(), plan_only));
}

/** \brief Place an object at one of the specified possible locations */
MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations,
bool plan_only = false);
MoveItErrorCode place(const std::string& object, std::vector<moveit_msgs::PlaceLocation> locations,
bool plan_only = false)
{
return place(constructPlaceGoal(object, std::move(locations), plan_only));
}

/** \brief Place an object at one of the specified possible locations */
MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses,
bool plan_only = false);
bool plan_only = false)
{
return place(constructPlaceGoal(object, posesToPlaceLocations(poses), plan_only));
}

/** \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);
MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only = false)
{
return place(constructPlaceGoal(object, posesToPlaceLocations({ pose }), plan_only));
}

/** \brief Place an object given a PlaceGoal
Use as follows: first create the goal with constructPlaceGoal(), then set \e place_locations and any other
desired variable in the goal, and finally pass it on to this function */
MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal);

/** \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
Expand Down Expand Up @@ -888,7 +931,7 @@ class MoveGroupInterface
return remembered_joint_values_;
}

/** \brief Forget the joint values remebered under \e name */
/** \brief Forget the joint values remembered under \e name */
void forgetJointValues(const std::string& name);

/**@}*/
Expand Down Expand Up @@ -934,7 +977,7 @@ class MoveGroupInterface
class MoveGroupInterfaceImpl;
MoveGroupInterfaceImpl* impl_;
};
}
}
} // namespace planning_interface
} // namespace moveit

#endif
4 changes: 2 additions & 2 deletions moveit_ros/planning_interface/move_group_interface/src/demo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ void demoPick(moveit::planning_interface::MoveGroupInterface& group)

grasps.push_back(g);
}
group.pick("bubu", grasps);
group.pick("bubu", std::move(grasps));
}

void demoPlace(moveit::planning_interface::MoveGroupInterface& group)
Expand Down Expand Up @@ -98,7 +98,7 @@ void demoPlace(moveit::planning_interface::MoveGroupInterface& group)

loc.push_back(g);
}
group.place("bubu", loc);
group.place("bubu", std::move(loc));
}

void attachObject()
Expand Down
116 changes: 51 additions & 65 deletions moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -576,9 +576,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
return true;
}

/** \brief Place an object at one of the specified possible locations */
MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses,
bool plan_only = false)
/** \brief Convert a vector of PoseStamped to a vector of PlaceLocation */
std::vector<moveit_msgs::PlaceLocation> posesToPlaceLocations(const std::vector<geometry_msgs::PoseStamped>& poses)
{
std::vector<moveit_msgs::PlaceLocation> locations;
for (std::size_t i = 0; i < poses.size(); ++i)
Expand All @@ -600,11 +599,10 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
}
ROS_DEBUG_NAMED("move_group_interface", "Move group interface has %u place locations",
(unsigned int)locations.size());
return place(object, locations, plan_only);
return locations;
}

MoveItErrorCode place(const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& locations,
bool plan_only = false)
MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal)
{
if (!place_action_client_)
{
Expand All @@ -616,15 +614,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
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());
Expand All @@ -644,7 +633,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
}
}

MoveItErrorCode pick(const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps, bool plan_only = false)
MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal)
{
if (!pick_action_client_)
{
Expand All @@ -656,15 +645,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
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())
Expand All @@ -689,8 +669,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
{
return planGraspsAndPick(moveit_msgs::CollisionObject());
}
moveit::planning_interface::PlanningSceneInterface psi;

moveit::planning_interface::PlanningSceneInterface psi;
std::map<std::string, moveit_msgs::CollisionObject> objects = psi.getObjects(std::vector<std::string>(1, object));

if (objects.empty())
Expand Down Expand Up @@ -729,7 +709,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
}

return pick(object.id, response.grasps, plan_only);
return pick(constructPickupGoal(object.id, std::move(response.grasps), plan_only));
}

MoveItErrorCode plan(Plan& plan)
Expand Down Expand Up @@ -1075,39 +1055,57 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
constructMotionPlanRequest(goal.request);
}

void constructGoal(moveit_msgs::PickupGoal& goal_out, const std::string& object)
moveit_msgs::PickupGoal constructPickupGoal(const std::string& object, std::vector<moveit_msgs::Grasp>&& grasps,
bool plan_only = false)
{
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_;
goal.possible_grasps = std::move(grasps);
if (!support_surface_.empty())
goal.allow_gripper_support_collision = true;

if (path_constraints_)
goal.path_constraints = *path_constraints_;

goal_out = goal;
goal.planner_id = planner_id_;
goal.allowed_planning_time = allowed_planning_time_;

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;
return goal;
}

void constructGoal(moveit_msgs::PlaceGoal& goal_out, const std::string& object)
moveit_msgs::PlaceGoal constructPlaceGoal(const std::string& object,
std::vector<moveit_msgs::PlaceLocation>&& locations, bool plan_only = false)
{
moveit_msgs::PlaceGoal goal;
goal.attached_object_name = object;
goal.group_name = opt_.group_name_;
goal.allowed_planning_time = allowed_planning_time_;
goal.attached_object_name = object;
goal.support_surface_name = support_surface_;
goal.planner_id = planner_id_;
goal.place_locations = std::move(locations);
if (!support_surface_.empty())
goal.allow_gripper_support_collision = true;

if (path_constraints_)
goal.path_constraints = *path_constraints_;

goal_out = goal;
goal.planner_id = planner_id_;
goal.allowed_planning_time = allowed_planning_time_;

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;
return goal;
}

void setPathConstraints(const moveit_msgs::Constraints& constraint)
Expand Down Expand Up @@ -1443,22 +1441,28 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGrou
return impl_->plan(plan);
}

moveit::planning_interface::MoveItErrorCode
moveit::planning_interface::MoveGroupInterface::pick(const std::string& object, bool plan_only)
moveit_msgs::PickupGoal moveit::planning_interface::MoveGroupInterface::constructPickupGoal(
const std::string& object, std::vector<moveit_msgs::Grasp> grasps, bool plan_only = false)
{
return impl_->pick(object, std::vector<moveit_msgs::Grasp>(), plan_only);
return impl_->constructPickupGoal(object, std::move(grasps), plan_only);
}

moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::pick(
const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only)
moveit_msgs::PlaceGoal moveit::planning_interface::MoveGroupInterface::constructPlaceGoal(
const std::string& object, std::vector<moveit_msgs::PlaceLocation> locations, bool plan_only = false)
{
return impl_->pick(object, std::vector<moveit_msgs::Grasp>(1, grasp), plan_only);
return impl_->constructPlaceGoal(object, std::move(locations), plan_only);
}

moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::pick(
const std::string& object, const std::vector<moveit_msgs::Grasp>& grasps, bool plan_only)
std::vector<moveit_msgs::PlaceLocation> moveit::planning_interface::MoveGroupInterface::posesToPlaceLocations(
const std::vector<geometry_msgs::PoseStamped>& poses)
{
return impl_->pick(object, grasps, plan_only);
return impl_->posesToPlaceLocations(poses);
}

moveit::planning_interface::MoveItErrorCode
moveit::planning_interface::MoveGroupInterface::pick(const moveit_msgs::PickupGoal& goal)
{
return impl_->pick(goal);
}

moveit::planning_interface::MoveItErrorCode
Expand All @@ -1474,27 +1478,9 @@ moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGrou
}

moveit::planning_interface::MoveItErrorCode
moveit::planning_interface::MoveGroupInterface::place(const std::string& object, bool plan_only)
{
return impl_->place(object, std::vector<moveit_msgs::PlaceLocation>(), plan_only);
}

moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::place(
const std::string& object, const std::vector<moveit_msgs::PlaceLocation>& 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<geometry_msgs::PoseStamped>& 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)
moveit::planning_interface::MoveGroupInterface::place(const moveit_msgs::PlaceGoal& goal)
{
return impl_->place(object, std::vector<geometry_msgs::PoseStamped>(1, pose), plan_only);
return impl_->place(goal);
}

double moveit::planning_interface::MoveGroupInterface::computeCartesianPath(
Expand Down
2 changes: 1 addition & 1 deletion moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -275,7 +275,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
{
std::vector<moveit_msgs::PlaceLocation> locations(1);
py_bindings_tools::deserializeMsg(location_str, locations[0]);
return place(object_name, locations, plan_only) == MoveItErrorCode::SUCCESS;
return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS;
}

bool placeAnywhere(const std::string& object_name, bool plan_only = false)
Expand Down

0 comments on commit a911665

Please sign in to comment.