mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
521 lines
22 KiB
C++
521 lines
22 KiB
C++
/*********************************************************************
|
|
* BSD 3-Clause 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 the copyright holder 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 HOLDER 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, Simon Goldstein
|
|
Desc: A demo to show MoveIt Task Constructor in action
|
|
*/
|
|
|
|
#include <moveit_task_constructor_demo/pick_place_task.h>
|
|
#include <rosparam_shortcuts/rosparam_shortcuts.h>
|
|
|
|
namespace moveit_task_constructor_demo {
|
|
|
|
constexpr char LOGNAME[] = "moveit_task_constructor_demo";
|
|
constexpr char PickPlaceTask::LOGNAME[];
|
|
|
|
void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) {
|
|
if (!psi.applyCollisionObject(object))
|
|
throw std::runtime_error("Failed to spawn object: " + object.id);
|
|
}
|
|
|
|
moveit_msgs::CollisionObject createTable(const ros::NodeHandle& pnh) {
|
|
std::string table_name, table_reference_frame;
|
|
std::vector<double> table_dimensions;
|
|
geometry_msgs::Pose pose;
|
|
std::size_t errors = 0;
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose);
|
|
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
|
|
|
|
moveit_msgs::CollisionObject object;
|
|
object.id = table_name;
|
|
object.header.frame_id = table_reference_frame;
|
|
object.primitives.resize(1);
|
|
object.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
|
|
object.primitives[0].dimensions = table_dimensions;
|
|
pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world
|
|
object.primitive_poses.push_back(pose);
|
|
return object;
|
|
}
|
|
|
|
moveit_msgs::CollisionObject createObject(const ros::NodeHandle& pnh) {
|
|
std::string object_name, object_reference_frame;
|
|
std::vector<double> object_dimensions;
|
|
geometry_msgs::Pose pose;
|
|
std::size_t error = 0;
|
|
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name);
|
|
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame);
|
|
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions);
|
|
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose);
|
|
rosparam_shortcuts::shutdownIfError(LOGNAME, error);
|
|
|
|
moveit_msgs::CollisionObject object;
|
|
object.id = object_name;
|
|
object.header.frame_id = object_reference_frame;
|
|
object.primitives.resize(1);
|
|
object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
|
|
object.primitives[0].dimensions = object_dimensions;
|
|
pose.position.z += 0.5 * object_dimensions[0];
|
|
object.primitive_poses.push_back(pose);
|
|
return object;
|
|
}
|
|
|
|
void setupDemoScene(ros::NodeHandle& pnh) {
|
|
// Add table and object to planning scene
|
|
ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service
|
|
moveit::planning_interface::PlanningSceneInterface psi;
|
|
if (pnh.param("spawn_table", true))
|
|
spawnObject(psi, createTable(pnh));
|
|
spawnObject(psi, createObject(pnh));
|
|
}
|
|
|
|
PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh)
|
|
: pnh_(pnh), task_name_(task_name) {
|
|
loadParameters();
|
|
}
|
|
|
|
void PickPlaceTask::loadParameters() {
|
|
/****************************************************
|
|
* *
|
|
* Load Parameters *
|
|
* *
|
|
***************************************************/
|
|
ROS_INFO_NAMED(LOGNAME, "Loading task parameters");
|
|
|
|
// Planning group properties
|
|
size_t errors = 0;
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_group_name", arm_group_name_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_group_name", hand_group_name_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "eef_name", eef_name_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_frame", hand_frame_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "world_frame", world_frame_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "grasp_frame_transform", grasp_frame_transform_);
|
|
|
|
// Predefined pose targets
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_open_pose", hand_open_pose_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_close_pose", hand_close_pose_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_home_pose", arm_home_pose_);
|
|
|
|
// Target object
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_name", object_name_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_dimensions", object_dimensions_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_reference_frame", object_reference_frame_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "surface_link", surface_link_);
|
|
support_surfaces_ = { surface_link_ };
|
|
|
|
// Pick/Place metrics
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_min_dist", approach_object_min_dist_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_max_dist", approach_object_max_dist_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_min_dist", lift_object_min_dist_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_max_dist", lift_object_max_dist_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_surface_offset", place_surface_offset_);
|
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_pose", place_pose_);
|
|
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
|
|
}
|
|
|
|
// Initialize the task pipeline, defining individual movement stages
|
|
bool PickPlaceTask::init() {
|
|
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
|
|
const std::string object = object_name_;
|
|
|
|
// Reset ROS introspection before constructing the new object
|
|
// TODO(v4hn): global storage for Introspection services to enable one-liner
|
|
task_.reset();
|
|
task_.reset(new moveit::task_constructor::Task());
|
|
|
|
// Individual movement stages are collected within the Task object
|
|
Task& t = *task_;
|
|
t.stages()->setName(task_name_);
|
|
t.loadRobotModel();
|
|
|
|
/* Create planners used in various stages. Various options are available,
|
|
namely Cartesian, MoveIt pipeline, and joint interpolation. */
|
|
// Sampling planner
|
|
auto sampling_planner = std::make_shared<solvers::PipelinePlanner>();
|
|
sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
|
|
|
|
// Cartesian planner
|
|
auto cartesian_planner = std::make_shared<solvers::CartesianPath>();
|
|
cartesian_planner->setMaxVelocityScalingFactor(1.0);
|
|
cartesian_planner->setMaxAccelerationScalingFactor(1.0);
|
|
cartesian_planner->setStepSize(.01);
|
|
|
|
// Set task properties
|
|
t.setProperty("group", arm_group_name_);
|
|
t.setProperty("eef", eef_name_);
|
|
t.setProperty("hand", hand_group_name_);
|
|
t.setProperty("hand_grasping_frame", hand_frame_);
|
|
t.setProperty("ik_frame", hand_frame_);
|
|
|
|
/****************************************************
|
|
* *
|
|
* Current State *
|
|
* *
|
|
***************************************************/
|
|
{
|
|
auto current_state = std::make_unique<stages::CurrentState>("current state");
|
|
|
|
// Verify that object is not attached
|
|
auto applicability_filter =
|
|
std::make_unique<stages::PredicateFilter>("applicability test", std::move(current_state));
|
|
applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) {
|
|
if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
|
|
comment = "object with id '" + object + "' is already attached and cannot be picked";
|
|
return false;
|
|
}
|
|
return true;
|
|
});
|
|
t.add(std::move(applicability_filter));
|
|
}
|
|
|
|
/****************************************************
|
|
* *
|
|
* Open Hand *
|
|
* *
|
|
***************************************************/
|
|
Stage* initial_state_ptr = nullptr;
|
|
{
|
|
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
|
|
stage->setGroup(hand_group_name_);
|
|
stage->setGoal(hand_open_pose_);
|
|
initial_state_ptr = stage.get(); // remember start state for monitoring grasp pose generator
|
|
t.add(std::move(stage));
|
|
}
|
|
|
|
/****************************************************
|
|
* *
|
|
* Move to Pick *
|
|
* *
|
|
***************************************************/
|
|
// Connect initial open-hand state with pre-grasp pose defined in the following
|
|
{
|
|
auto stage = std::make_unique<stages::Connect>(
|
|
"move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
|
|
stage->setTimeout(5.0);
|
|
stage->properties().configureInitFrom(Stage::PARENT);
|
|
t.add(std::move(stage));
|
|
}
|
|
|
|
/****************************************************
|
|
* *
|
|
* Pick Object *
|
|
* *
|
|
***************************************************/
|
|
Stage* pick_stage_ptr = nullptr;
|
|
{
|
|
// A SerialContainer combines several sub-stages, here for picking the object
|
|
auto grasp = std::make_unique<SerialContainer>("pick object");
|
|
t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
|
|
grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
|
|
|
|
/****************************************************
|
|
---- * Approach Object *
|
|
***************************************************/
|
|
{
|
|
// Move the eef link forward along its z-axis by an amount within the given min-max range
|
|
auto stage = std::make_unique<stages::MoveRelative>("approach object", cartesian_planner);
|
|
stage->properties().set("marker_ns", "approach_object");
|
|
stage->properties().set("link", hand_frame_); // link to perform IK for
|
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" }); // inherit group from parent stage
|
|
stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_);
|
|
|
|
// Set hand forward direction
|
|
geometry_msgs::Vector3Stamped vec;
|
|
vec.header.frame_id = hand_frame_;
|
|
vec.vector.z = 1.0;
|
|
stage->setDirection(vec);
|
|
grasp->insert(std::move(stage));
|
|
}
|
|
|
|
/****************************************************
|
|
---- * Generate Grasp Pose *
|
|
***************************************************/
|
|
{
|
|
// Sample grasp pose candidates in angle increments around the z-axis of the object
|
|
auto stage = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
|
stage->properties().configureInitFrom(Stage::PARENT);
|
|
stage->properties().set("marker_ns", "grasp_pose");
|
|
stage->setPreGraspPose(hand_open_pose_);
|
|
stage->setObject(object); // object to sample grasps for
|
|
stage->setAngleDelta(M_PI / 12);
|
|
stage->setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
|
|
|
|
// Compute IK for sampled grasp poses
|
|
auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage));
|
|
wrapper->setMaxIKSolutions(8); // limit number of solutions
|
|
wrapper->setMinSolutionDistance(1.0);
|
|
wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); // define virtual frame to reach the target_pose
|
|
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); // inherit properties from parent
|
|
wrapper->properties().configureInitFrom(Stage::INTERFACE,
|
|
{ "target_pose" }); // inherit property from child solution
|
|
grasp->insert(std::move(wrapper));
|
|
}
|
|
|
|
/****************************************************
|
|
---- * Allow Collision (hand object) *
|
|
***************************************************/
|
|
{
|
|
// Modify planning scene (w/o altering the robot's pose) to allow touching the object for picking
|
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (hand,object)");
|
|
stage->allowCollisions(
|
|
object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
|
|
true);
|
|
grasp->insert(std::move(stage));
|
|
}
|
|
|
|
/****************************************************
|
|
---- * Close Hand *
|
|
***************************************************/
|
|
{
|
|
auto stage = std::make_unique<stages::MoveTo>("close hand", sampling_planner);
|
|
stage->setGroup(hand_group_name_);
|
|
stage->setGoal(hand_close_pose_);
|
|
grasp->insert(std::move(stage));
|
|
}
|
|
|
|
/****************************************************
|
|
.... * Attach Object *
|
|
***************************************************/
|
|
{
|
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
|
|
stage->attachObject(object, hand_frame_); // attach object to hand_frame_
|
|
grasp->insert(std::move(stage));
|
|
}
|
|
|
|
/****************************************************
|
|
.... * Allow collision (object support) *
|
|
***************************************************/
|
|
{
|
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (object,support)");
|
|
stage->allowCollisions({ object }, support_surfaces_, true);
|
|
grasp->insert(std::move(stage));
|
|
}
|
|
|
|
/****************************************************
|
|
.... * Lift object *
|
|
***************************************************/
|
|
{
|
|
auto stage = std::make_unique<stages::MoveRelative>("lift object", cartesian_planner);
|
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
|
stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_);
|
|
stage->setIKFrame(hand_frame_);
|
|
stage->properties().set("marker_ns", "lift_object");
|
|
|
|
// Set upward direction
|
|
geometry_msgs::Vector3Stamped vec;
|
|
vec.header.frame_id = world_frame_;
|
|
vec.vector.z = 1.0;
|
|
stage->setDirection(vec);
|
|
grasp->insert(std::move(stage));
|
|
}
|
|
|
|
/****************************************************
|
|
.... * Forbid collision (object support) *
|
|
***************************************************/
|
|
{
|
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (object,support)");
|
|
stage->allowCollisions({ object }, support_surfaces_, false);
|
|
grasp->insert(std::move(stage));
|
|
}
|
|
|
|
pick_stage_ptr = grasp.get(); // remember for monitoring place pose generator
|
|
|
|
// Add grasp container to task
|
|
t.add(std::move(grasp));
|
|
}
|
|
|
|
/******************************************************
|
|
* *
|
|
* Move to Place *
|
|
* *
|
|
*****************************************************/
|
|
{
|
|
// Connect the grasped state to the pre-place state, i.e. realize the object transport
|
|
auto stage = std::make_unique<stages::Connect>(
|
|
"move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
|
|
stage->setTimeout(5.0);
|
|
stage->properties().configureInitFrom(Stage::PARENT);
|
|
t.add(std::move(stage));
|
|
}
|
|
|
|
/******************************************************
|
|
* *
|
|
* Place Object *
|
|
* *
|
|
*****************************************************/
|
|
// All placing sub-stages are collected within a serial container again
|
|
{
|
|
auto place = std::make_unique<SerialContainer>("place object");
|
|
t.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
|
|
place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" });
|
|
|
|
/******************************************************
|
|
---- * Lower Object *
|
|
*****************************************************/
|
|
{
|
|
auto stage = std::make_unique<stages::MoveRelative>("lower object", cartesian_planner);
|
|
stage->properties().set("marker_ns", "lower_object");
|
|
stage->properties().set("link", hand_frame_);
|
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
|
stage->setMinMaxDistance(.03, .13);
|
|
|
|
// Set downward direction
|
|
geometry_msgs::Vector3Stamped vec;
|
|
vec.header.frame_id = world_frame_;
|
|
vec.vector.z = -1.0;
|
|
stage->setDirection(vec);
|
|
place->insert(std::move(stage));
|
|
}
|
|
|
|
/******************************************************
|
|
---- * Generate Place Pose *
|
|
*****************************************************/
|
|
{
|
|
// Generate Place Pose
|
|
auto stage = std::make_unique<stages::GeneratePlacePose>("generate place pose");
|
|
stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" });
|
|
stage->properties().set("marker_ns", "place_pose");
|
|
stage->setObject(object);
|
|
|
|
// Set target pose
|
|
geometry_msgs::PoseStamped p;
|
|
p.header.frame_id = object_reference_frame_;
|
|
p.pose = place_pose_;
|
|
p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_;
|
|
stage->setPose(p);
|
|
stage->setMonitoredStage(pick_stage_ptr); // hook into successful pick solutions
|
|
|
|
// Compute IK
|
|
auto wrapper = std::make_unique<stages::ComputeIK>("place pose IK", std::move(stage));
|
|
wrapper->setMaxIKSolutions(2);
|
|
wrapper->setIKFrame(grasp_frame_transform_, hand_frame_);
|
|
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
|
|
wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
|
|
place->insert(std::move(wrapper));
|
|
}
|
|
|
|
/******************************************************
|
|
---- * Open Hand *
|
|
*****************************************************/
|
|
{
|
|
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
|
|
stage->setGroup(hand_group_name_);
|
|
stage->setGoal(hand_open_pose_);
|
|
place->insert(std::move(stage));
|
|
}
|
|
|
|
/******************************************************
|
|
---- * Forbid collision (hand, object) *
|
|
*****************************************************/
|
|
{
|
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (hand,object)");
|
|
stage->allowCollisions(object_name_, *t.getRobotModel()->getJointModelGroup(hand_group_name_), false);
|
|
place->insert(std::move(stage));
|
|
}
|
|
|
|
/******************************************************
|
|
---- * Detach Object *
|
|
*****************************************************/
|
|
{
|
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("detach object");
|
|
stage->detachObject(object_name_, hand_frame_);
|
|
place->insert(std::move(stage));
|
|
}
|
|
|
|
/******************************************************
|
|
---- * Retreat Motion *
|
|
*****************************************************/
|
|
{
|
|
auto stage = std::make_unique<stages::MoveRelative>("retreat after place", cartesian_planner);
|
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
|
stage->setMinMaxDistance(.12, .25);
|
|
stage->setIKFrame(hand_frame_);
|
|
stage->properties().set("marker_ns", "retreat");
|
|
geometry_msgs::Vector3Stamped vec;
|
|
vec.header.frame_id = hand_frame_;
|
|
vec.vector.z = -1.0;
|
|
stage->setDirection(vec);
|
|
place->insert(std::move(stage));
|
|
}
|
|
|
|
// Add place container to task
|
|
t.add(std::move(place));
|
|
}
|
|
|
|
/******************************************************
|
|
* *
|
|
* Move to Home *
|
|
* *
|
|
*****************************************************/
|
|
{
|
|
auto stage = std::make_unique<stages::MoveTo>("move home", sampling_planner);
|
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
|
stage->setGoal(arm_home_pose_);
|
|
stage->restrictDirection(stages::MoveTo::FORWARD);
|
|
t.add(std::move(stage));
|
|
}
|
|
|
|
// prepare Task structure for planning
|
|
try {
|
|
t.init();
|
|
} catch (InitStageException& e) {
|
|
ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e);
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool PickPlaceTask::plan() {
|
|
ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
|
|
int max_solutions = pnh_.param<int>("max_solutions", 10);
|
|
|
|
return static_cast<bool>(task_->plan(max_solutions));
|
|
}
|
|
|
|
bool PickPlaceTask::execute() {
|
|
ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory");
|
|
moveit_msgs::MoveItErrorCodes execute_result;
|
|
|
|
execute_result = task_->execute(*task_->solutions().front());
|
|
|
|
if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) {
|
|
ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_result.val);
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
} // namespace moveit_task_constructor_demo
|