From 615e8ef248ec604658e6685851457cd567673cdb Mon Sep 17 00:00:00 2001 From: Jafar Date: Tue, 6 Dec 2022 00:14:50 +0300 Subject: [PATCH] Replace rosparam_shortcuts with generate_parameter_library (#403) --- demo/CMakeLists.txt | 9 +- .../pick_place_task.h | 45 +--- demo/package.xml | 2 +- demo/src/ik_clearance_cost.cpp | 11 +- demo/src/ik_clearance_cost_parameters.yaml | 9 + demo/src/pick_place_demo.cpp | 14 +- demo/src/pick_place_demo_parameters.yaml | 97 +++++++++ demo/src/pick_place_task.cpp | 202 +++++++----------- 8 files changed, 212 insertions(+), 177 deletions(-) create mode 100644 demo/src/ik_clearance_cost_parameters.yaml create mode 100644 demo/src/pick_place_demo_parameters.yaml diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index cec117f7..f70caea1 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -7,13 +7,13 @@ set(CMAKE_CXX_EXTENSIONS OFF) find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(moveit_core REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(moveit_task_constructor_core REQUIRED) find_package(moveit_task_constructor_msgs REQUIRED) find_package(rclcpp REQUIRED) -find_package(rosparam_shortcuts REQUIRED) find_package(tf2_eigen REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS @@ -23,7 +23,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_task_constructor_core moveit_task_constructor_msgs rclcpp - rosparam_shortcuts tf2_eigen ) @@ -39,6 +38,11 @@ include_directories(include) # declare a demo consisting of a single cpp file function(demo name) add_executable(${PROJECT_NAME}_${name} src/${name}.cpp) + set(parameter_filename ${name}_parameters) + if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/src/${parameter_filename}.yaml) + generate_parameter_library(${parameter_filename} src/${parameter_filename}.yaml) + target_link_libraries(${PROJECT_NAME}_${name} ${parameter_filename}) + endif() ament_target_dependencies(${PROJECT_NAME}_${name} ${THIS_PACKAGE_INCLUDE_DEPENDS}) set_target_properties(${PROJECT_NAME}_${name} PROPERTIES OUTPUT_NAME ${name} PREFIX "") install(TARGETS ${PROJECT_NAME}_${name} @@ -55,6 +59,7 @@ demo(ik_clearance_cost) demo(fallbacks_move_to) demo(pick_place_demo) +target_link_libraries(${PROJECT_NAME}_pick_place_task pick_place_demo_parameters) target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task) install(DIRECTORY launch config diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h index cbb45227..e7636d67 100644 --- a/demo/include/moveit_task_constructor_demo/pick_place_task.h +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -35,7 +35,7 @@ */ // ROS -#include +#include // MoveIt #include @@ -57,6 +57,7 @@ #include #include #include +#include "pick_place_demo_parameters.hpp" #if __has_include() #include @@ -70,56 +71,22 @@ namespace moveit_task_constructor_demo { using namespace moveit::task_constructor; // prepare a demo environment from ROS parameters under node -void setupDemoScene(const rclcpp::Node::SharedPtr& node); +void setupDemoScene(const pick_place_task_demo::Params& params); class PickPlaceTask { public: - PickPlaceTask(const std::string& task_name, const rclcpp::Node::SharedPtr& pnh); + PickPlaceTask(const std::string& task_name); ~PickPlaceTask() = default; - bool init(); + bool init(const rclcpp::Node::SharedPtr& node, const pick_place_task_demo::Params& params); - bool plan(); + bool plan(const std::size_t max_solutions); bool execute(); private: - void loadParameters(); - - rclcpp::Node::SharedPtr node_; - std::string task_name_; moveit::task_constructor::TaskPtr task_; - - // planning group properties - std::string arm_group_name_; - std::string eef_name_; - std::string hand_group_name_; - std::string hand_frame_; - - // object + surface - std::vector support_surfaces_; - std::string object_reference_frame_; - std::string surface_link_; - std::string object_name_; - std::string world_frame_; - std::vector object_dimensions_; - - // Predefined pose targets - std::string hand_open_pose_; - std::string hand_close_pose_; - std::string arm_home_pose_; - - // Pick metrics - Eigen::Isometry3d grasp_frame_transform_; - double approach_object_min_dist_; - double approach_object_max_dist_; - double lift_object_min_dist_; - double lift_object_max_dist_; - - // Place metrics - geometry_msgs::msg::Pose place_pose_; - double place_surface_offset_; }; } // namespace moveit_task_constructor_demo diff --git a/demo/package.xml b/demo/package.xml index 6bc46923..f4beb820 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -11,10 +11,10 @@ BSD ament_cmake + generate_parameter_library moveit_core moveit_ros_planning_interface moveit_task_constructor_core - rosparam_shortcuts moveit_configs_utils moveit_resources_panda_moveit_config moveit_task_constructor_capabilities diff --git a/demo/src/ik_clearance_cost.cpp b/demo/src/ik_clearance_cost.cpp index 71c6adb0..3c2b80ce 100644 --- a/demo/src/ik_clearance_cost.cpp +++ b/demo/src/ik_clearance_cost.cpp @@ -8,7 +8,7 @@ #include #include -#include +#include "ik_clearance_cost_parameters.hpp" using namespace moveit::task_constructor; @@ -18,6 +18,9 @@ int main(int argc, char** argv) { auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo"); std::thread spinning_thread([node] { rclcpp::spin(node); }); + const auto param_listener = std::make_shared(node); + const auto params = param_listener->get_params(); + Task t; t.stages()->setName("clearance IK"); @@ -56,10 +59,8 @@ int main(int argc, char** argv) { ik->setMaxIKSolutions(100); auto cl_cost{ std::make_unique() }; - cl_cost->cumulative = false; - rosparam_shortcuts::get(node, "cumulative", cl_cost->cumulative); // sum up pairwise distances? - cl_cost->with_world = true; - rosparam_shortcuts::get(node, "with_world", cl_cost->with_world); // consider distance to world objects? + cl_cost->cumulative = params.cumulative; + cl_cost->with_world = params.with_world; ik->setCostTerm(std::move(cl_cost)); t.add(std::move(ik)); diff --git a/demo/src/ik_clearance_cost_parameters.yaml b/demo/src/ik_clearance_cost_parameters.yaml new file mode 100644 index 00000000..2b51c875 --- /dev/null +++ b/demo/src/ik_clearance_cost_parameters.yaml @@ -0,0 +1,9 @@ +ik_clearance_cost_demo: + cumulative: + type: bool + default_value: false + read_only: true + with_world: + type: bool + default_value: true + read_only: true diff --git a/demo/src/pick_place_demo.cpp b/demo/src/pick_place_demo.cpp index 14a560d8..7c45133a 100644 --- a/demo/src/pick_place_demo.cpp +++ b/demo/src/pick_place_demo.cpp @@ -40,7 +40,7 @@ // MTC pick/place demo implementation #include -#include +#include "pick_place_demo_parameters.hpp" static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo"); @@ -51,18 +51,20 @@ int main(int argc, char** argv) { auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo", node_options); std::thread spinning_thread([node] { rclcpp::spin(node); }); - moveit_task_constructor_demo::setupDemoScene(node); + const auto param_listener = std::make_shared(node); + const auto params = param_listener->get_params(); + moveit_task_constructor_demo::setupDemoScene(params); // Construct and run pick/place task - moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", node); - if (!pick_place_task.init()) { + moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task"); + if (!pick_place_task.init(node, params)) { RCLCPP_INFO(LOGGER, "Initialization failed"); return 1; } - if (pick_place_task.plan()) { + if (pick_place_task.plan(params.max_solutions)) { RCLCPP_INFO(LOGGER, "Planning succeded"); - if (bool execute = false; rosparam_shortcuts::get(node, "execute", execute) && execute) { + if (params.execute) { pick_place_task.execute(); RCLCPP_INFO(LOGGER, "Execution complete"); } else { diff --git a/demo/src/pick_place_demo_parameters.yaml b/demo/src/pick_place_demo_parameters.yaml new file mode 100644 index 00000000..a05c5270 --- /dev/null +++ b/demo/src/pick_place_demo_parameters.yaml @@ -0,0 +1,97 @@ +pick_place_task_demo: + execute: + type: bool + default_value: false + table_name: + type: string + validation: + not_empty<>: [] + table_reference_frame: + type: string + validation: + not_empty<>: [] + table_dimensions: + type: double_array + validation: + fixed_size<>: [3] + table_pose: + type: double_array + validation: + fixed_size<>: [6] + object_name: + type: string + validation: + not_empty<>: [] + object_reference_frame: + type: string + validation: + not_empty<>: [] + object_dimensions: + type: double_array + validation: + fixed_size<>: [2] + object_pose: + type: double_array + validation: + fixed_size<>: [6] + spawn_table: + type: bool + default_value: true + max_solutions: + type: int + default_value: 10 + arm_group_name: + type: string + validation: + not_empty<>: [] + eef_name: + type: string + validation: + not_empty<>: [] + hand_group_name: + type: string + validation: + not_empty<>: [] + hand_frame: + type: string + validation: + not_empty<>: [] + hand_open_pose: + type: string + validation: + not_empty<>: [] + hand_close_pose: + type: string + validation: + not_empty<>: [] + arm_home_pose: + type: string + validation: + not_empty<>: [] + # Scene frames + world_frame: + type: string + validation: + not_empty<>: [] + surface_link: + type: string + validation: + not_empty<>: [] + grasp_frame_transform: + type: double_array + validation: + fixed_size<>: [6] + place_pose: + type: double_array + validation: + fixed_size<>: [6] + place_surface_offset: + type: double + approach_object_min_dist: + type: double + approach_object_max_dist: + type: double + lift_object_min_dist: + type: double + lift_object_max_dist: + type: double diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index cd589ce4..a52f8075 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -34,11 +34,25 @@ Desc: A demo to show MoveIt Task Constructor in action */ +#include #include -#include +#include +#include "pick_place_demo_parameters.hpp" static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo"); +namespace { +Eigen::Isometry3d vectorToEigen(const std::vector& values) { + return Eigen::Translation3d(values[0], values[1], values[2]) * + Eigen::AngleAxisd(values[3], Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(values[4], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(values[5], Eigen::Vector3d::UnitZ()); +} +geometry_msgs::msg::Pose vectorToPose(const std::vector& values) { + return tf2::toMsg(vectorToEigen(values)); +}; +} // namespace + namespace moveit_task_constructor_demo { void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, @@ -47,106 +61,46 @@ void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, throw std::runtime_error("Failed to spawn object: " + object.id); } -moveit_msgs::msg::CollisionObject createTable(const rclcpp::Node::SharedPtr& node) { - std::string table_name, table_reference_frame; - std::vector table_dimensions; - geometry_msgs::msg::Pose pose; - std::size_t errors = 0; - errors += !rosparam_shortcuts::get(node, "table_name", table_name); - errors += !rosparam_shortcuts::get(node, "table_reference_frame", table_reference_frame); - errors += !rosparam_shortcuts::get(node, "table_dimensions", table_dimensions); - errors += !rosparam_shortcuts::get(node, "table_pose", pose); - rosparam_shortcuts::shutdownIfError(errors); - +moveit_msgs::msg::CollisionObject createTable(const pick_place_task_demo::Params& params) { + geometry_msgs::msg::Pose pose = vectorToPose(params.table_pose); moveit_msgs::msg::CollisionObject object; - object.id = table_name; - object.header.frame_id = table_reference_frame; + object.id = params.table_name; + object.header.frame_id = params.table_reference_frame; object.primitives.resize(1); object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; - object.primitives[0].dimensions = { table_dimensions.at(0), table_dimensions.at(1), table_dimensions.at(2) }; - pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world + object.primitives[0].dimensions = { params.table_dimensions.at(0), params.table_dimensions.at(1), + params.table_dimensions.at(2) }; + pose.position.z -= 0.5 * params.table_dimensions[2]; // align surface with world object.primitive_poses.push_back(pose); return object; } -moveit_msgs::msg::CollisionObject createObject(const rclcpp::Node::SharedPtr& node) { - std::string object_name, object_reference_frame; - std::vector object_dimensions; - geometry_msgs::msg::Pose pose; - std::size_t error = 0; - error += !rosparam_shortcuts::get(node, "object_name", object_name); - error += !rosparam_shortcuts::get(node, "object_reference_frame", object_reference_frame); - error += !rosparam_shortcuts::get(node, "object_dimensions", object_dimensions); - error += !rosparam_shortcuts::get(node, "object_pose", pose); - rosparam_shortcuts::shutdownIfError(error); - +moveit_msgs::msg::CollisionObject createObject(const pick_place_task_demo::Params& params) { + geometry_msgs::msg::Pose pose = vectorToPose(params.object_pose); moveit_msgs::msg::CollisionObject object; - object.id = object_name; - object.header.frame_id = object_reference_frame; + object.id = params.object_name; + object.header.frame_id = params.object_reference_frame; object.primitives.resize(1); object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; - object.primitives[0].dimensions = { object_dimensions.at(0), object_dimensions.at(1) }; - pose.position.z += 0.5 * object_dimensions[0]; + object.primitives[0].dimensions = { params.object_dimensions.at(0), params.object_dimensions.at(1) }; + pose.position.z += 0.5 * params.object_dimensions[0]; object.primitive_poses.push_back(pose); return object; } -void setupDemoScene(const rclcpp::Node::SharedPtr& node) { +void setupDemoScene(const pick_place_task_demo::Params& params) { // Add table and object to planning scene rclcpp::sleep_for(std::chrono::microseconds(100)); // Wait for ApplyPlanningScene service moveit::planning_interface::PlanningSceneInterface psi; - if (bool spawn_table = true; rosparam_shortcuts::get(node, "spawn_table", spawn_table) && spawn_table) - spawnObject(psi, createTable(node)); - spawnObject(psi, createObject(node)); + if (params.spawn_table) + spawnObject(psi, createTable(params)); + spawnObject(psi, createObject(params)); } -PickPlaceTask::PickPlaceTask(const std::string& task_name, const rclcpp::Node::SharedPtr& node) - : node_(node), task_name_(task_name) { - loadParameters(); -} +PickPlaceTask::PickPlaceTask(const std::string& task_name) : task_name_(task_name) {} -void PickPlaceTask::loadParameters() { - /**************************************************** - * * - * Load Parameters * - * * - ***************************************************/ - RCLCPP_INFO(LOGGER, "Loading task parameters"); - - // Planning group properties - size_t errors = 0; - errors += !rosparam_shortcuts::get(node_, "arm_group_name", arm_group_name_); - errors += !rosparam_shortcuts::get(node_, "hand_group_name", hand_group_name_); - errors += !rosparam_shortcuts::get(node_, "eef_name", eef_name_); - errors += !rosparam_shortcuts::get(node_, "hand_frame", hand_frame_); - errors += !rosparam_shortcuts::get(node_, "world_frame", world_frame_); - errors += !rosparam_shortcuts::get(node_, "grasp_frame_transform", grasp_frame_transform_); - - // Predefined pose targets - errors += !rosparam_shortcuts::get(node_, "hand_open_pose", hand_open_pose_); - errors += !rosparam_shortcuts::get(node_, "hand_close_pose", hand_close_pose_); - errors += !rosparam_shortcuts::get(node_, "arm_home_pose", arm_home_pose_); - - // Target object - errors += !rosparam_shortcuts::get(node_, "object_name", object_name_); - errors += !rosparam_shortcuts::get(node_, "object_dimensions", object_dimensions_); - errors += !rosparam_shortcuts::get(node_, "object_reference_frame", object_reference_frame_); - errors += !rosparam_shortcuts::get(node_, "surface_link", surface_link_); - support_surfaces_ = { surface_link_ }; - - // Pick/Place metrics - errors += !rosparam_shortcuts::get(node_, "approach_object_min_dist", approach_object_min_dist_); - errors += !rosparam_shortcuts::get(node_, "approach_object_max_dist", approach_object_max_dist_); - errors += !rosparam_shortcuts::get(node_, "lift_object_min_dist", lift_object_min_dist_); - errors += !rosparam_shortcuts::get(node_, "lift_object_max_dist", lift_object_max_dist_); - errors += !rosparam_shortcuts::get(node_, "place_surface_offset", place_surface_offset_); - errors += !rosparam_shortcuts::get(node_, "place_pose", place_pose_); - rosparam_shortcuts::shutdownIfError(errors); -} - -bool PickPlaceTask::init() { +bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_task_demo::Params& params) { RCLCPP_INFO(LOGGER, "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 @@ -155,10 +109,10 @@ bool PickPlaceTask::init() { Task& t = *task_; t.stages()->setName(task_name_); - t.loadRobotModel(node_); + t.loadRobotModel(node); // Sampling planner - auto sampling_planner = std::make_shared(node_); + auto sampling_planner = std::make_shared(node); sampling_planner->setProperty("goal_joint_tolerance", 1e-5); // Cartesian planner @@ -168,11 +122,11 @@ bool PickPlaceTask::init() { 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_); + t.setProperty("group", params.arm_group_name); + t.setProperty("eef", params.eef_name); + t.setProperty("hand", params.hand_group_name); + t.setProperty("hand_grasping_frame", params.hand_frame); + t.setProperty("ik_frame", params.hand_frame); /**************************************************** * * @@ -186,7 +140,7 @@ bool PickPlaceTask::init() { // Verify that object is not attached auto applicability_filter = std::make_unique("applicability test", std::move(current_state)); - applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) { + applicability_filter->setPredicate([object = params.object_name](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; @@ -205,8 +159,8 @@ bool PickPlaceTask::init() { ***************************************************/ { // Open Hand auto stage = std::make_unique("open hand", sampling_planner); - stage->setGroup(hand_group_name_); - stage->setGoal(hand_open_pose_); + stage->setGroup(params.hand_group_name); + stage->setGoal(params.hand_open_pose); t.add(std::move(stage)); } @@ -217,7 +171,7 @@ bool PickPlaceTask::init() { ***************************************************/ { // Move-to pre-grasp auto stage = std::make_unique( - "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); + "move to pick", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } }); stage->setTimeout(5.0); stage->properties().configureInitFrom(Stage::PARENT); t.add(std::move(stage)); @@ -240,13 +194,13 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("approach object", cartesian_planner); stage->properties().set("marker_ns", "approach_object"); - stage->properties().set("link", hand_frame_); + stage->properties().set("link", params.hand_frame); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); + stage->setMinMaxDistance(params.approach_object_min_dist, params.approach_object_max_dist); // Set hand forward direction geometry_msgs::msg::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; + vec.header.frame_id = params.hand_frame; vec.vector.z = 1.0; stage->setDirection(vec); grasp->insert(std::move(stage)); @@ -260,8 +214,8 @@ bool PickPlaceTask::init() { auto stage = std::make_unique("generate grasp pose"); stage->properties().configureInitFrom(Stage::PARENT); stage->properties().set("marker_ns", "grasp_pose"); - stage->setPreGraspPose(hand_open_pose_); - stage->setObject(object); + stage->setPreGraspPose(params.hand_open_pose); + stage->setObject(params.object_name); stage->setAngleDelta(M_PI / 12); stage->setMonitoredStage(current_state_ptr); // Hook into current state @@ -269,7 +223,7 @@ bool PickPlaceTask::init() { auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); wrapper->setMaxIKSolutions(8); wrapper->setMinSolutionDistance(1.0); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); + wrapper->setIKFrame(vectorToEigen(params.grasp_frame_transform), params.hand_frame); wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); grasp->insert(std::move(wrapper)); @@ -281,7 +235,8 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("allow collision (hand,object)"); stage->allowCollisions( - object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), + params.object_name, + t.getRobotModel()->getJointModelGroup(params.hand_group_name)->getLinkModelNamesWithCollisionGeometry(), true); grasp->insert(std::move(stage)); } @@ -291,8 +246,8 @@ bool PickPlaceTask::init() { ***************************************************/ { auto stage = std::make_unique("close hand", sampling_planner); - stage->setGroup(hand_group_name_); - stage->setGoal(hand_close_pose_); + stage->setGroup(params.hand_group_name); + stage->setGoal(params.hand_close_pose); grasp->insert(std::move(stage)); } @@ -301,7 +256,7 @@ bool PickPlaceTask::init() { ***************************************************/ { auto stage = std::make_unique("attach object"); - stage->attachObject(object, hand_frame_); + stage->attachObject(params.object_name, params.hand_frame); attach_object_stage = stage.get(); grasp->insert(std::move(stage)); } @@ -311,7 +266,7 @@ bool PickPlaceTask::init() { ***************************************************/ { auto stage = std::make_unique("allow collision (object,support)"); - stage->allowCollisions({ object }, support_surfaces_, true); + stage->allowCollisions({ params.object_name }, { params.surface_link }, true); grasp->insert(std::move(stage)); } @@ -321,13 +276,13 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("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->setMinMaxDistance(params.lift_object_min_dist, params.lift_object_max_dist); + stage->setIKFrame(params.hand_frame); stage->properties().set("marker_ns", "lift_object"); // Set upward direction geometry_msgs::msg::Vector3Stamped vec; - vec.header.frame_id = world_frame_; + vec.header.frame_id = params.world_frame; vec.vector.z = 1.0; stage->setDirection(vec); grasp->insert(std::move(stage)); @@ -338,7 +293,7 @@ bool PickPlaceTask::init() { ***************************************************/ { auto stage = std::make_unique("forbid collision (object,surface)"); - stage->allowCollisions({ object }, support_surfaces_, false); + stage->allowCollisions({ params.object_name }, { params.surface_link }, false); grasp->insert(std::move(stage)); } @@ -353,7 +308,7 @@ bool PickPlaceTask::init() { *****************************************************/ { auto stage = std::make_unique( - "move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); + "move to place", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } }); stage->setTimeout(5.0); stage->properties().configureInitFrom(Stage::PARENT); t.add(std::move(stage)); @@ -375,13 +330,13 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("lower object", cartesian_planner); stage->properties().set("marker_ns", "lower_object"); - stage->properties().set("link", hand_frame_); + stage->properties().set("link", params.hand_frame); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); stage->setMinMaxDistance(.03, .13); // Set downward direction geometry_msgs::msg::Vector3Stamped vec; - vec.header.frame_id = world_frame_; + vec.header.frame_id = params.world_frame; vec.vector.z = -1.0; stage->setDirection(vec); place->insert(std::move(stage)); @@ -395,20 +350,20 @@ bool PickPlaceTask::init() { auto stage = std::make_unique("generate place pose"); stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); stage->properties().set("marker_ns", "place_pose"); - stage->setObject(object); + stage->setObject(params.object_name); // Set target pose geometry_msgs::msg::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_; + p.header.frame_id = params.object_reference_frame; + p.pose = vectorToPose(params.place_pose); + p.pose.position.z += 0.5 * params.object_dimensions[0] + params.place_surface_offset; stage->setPose(p); stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage // Compute IK auto wrapper = std::make_unique("place pose IK", std::move(stage)); wrapper->setMaxIKSolutions(2); - wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); + wrapper->setIKFrame(vectorToEigen(params.grasp_frame_transform), params.hand_frame); wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); place->insert(std::move(wrapper)); @@ -419,8 +374,8 @@ bool PickPlaceTask::init() { *****************************************************/ { auto stage = std::make_unique("open hand", sampling_planner); - stage->setGroup(hand_group_name_); - stage->setGoal(hand_open_pose_); + stage->setGroup(params.hand_group_name); + stage->setGoal(params.hand_open_pose); place->insert(std::move(stage)); } @@ -429,7 +384,8 @@ bool PickPlaceTask::init() { *****************************************************/ { auto stage = std::make_unique("forbid collision (hand,object)"); - stage->allowCollisions(object_name_, *t.getRobotModel()->getJointModelGroup(hand_group_name_), false); + stage->allowCollisions(params.object_name, *t.getRobotModel()->getJointModelGroup(params.hand_group_name), + false); place->insert(std::move(stage)); } @@ -438,7 +394,7 @@ bool PickPlaceTask::init() { *****************************************************/ { auto stage = std::make_unique("detach object"); - stage->detachObject(object_name_, hand_frame_); + stage->detachObject(params.object_name, params.hand_frame); place->insert(std::move(stage)); } @@ -449,10 +405,10 @@ bool PickPlaceTask::init() { auto stage = std::make_unique("retreat after place", cartesian_planner); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); stage->setMinMaxDistance(.12, .25); - stage->setIKFrame(hand_frame_); + stage->setIKFrame(params.hand_frame); stage->properties().set("marker_ns", "retreat"); geometry_msgs::msg::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; + vec.header.frame_id = params.hand_frame; vec.vector.z = -1.0; stage->setDirection(vec); place->insert(std::move(stage)); @@ -470,7 +426,7 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("move home", sampling_planner); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setGoal(arm_home_pose_); + stage->setGoal(params.arm_home_pose); stage->restrictDirection(stages::MoveTo::FORWARD); t.add(std::move(stage)); } @@ -486,10 +442,8 @@ bool PickPlaceTask::init() { return true; } -bool PickPlaceTask::plan() { +bool PickPlaceTask::plan(const std::size_t max_solutions) { RCLCPP_INFO(LOGGER, "Start searching for task solutions"); - int max_solutions = 10; - rosparam_shortcuts::get(node_, "max_solutions", max_solutions); return static_cast(task_->plan(max_solutions)); }