diff --git a/.dockerignore b/.dockerignore index b4bb7fc4e7..d5ac92b441 100644 --- a/.dockerignore +++ b/.dockerignore @@ -31,7 +31,6 @@ !moveit_ros/move_group/package.xml !moveit_ros/robot_interaction/package.xml !moveit_ros/visualization/package.xml -!moveit_ros/manipulation/package.xml !moveit_ros/planning/package.xml !moveit_ros/planning_interface/package.xml !moveit_ros/benchmarks/package.xml diff --git a/.github/CODEOWNERS.disabled b/.github/CODEOWNERS.disabled index 1792eb5173..4582cf07eb 100644 --- a/.github/CODEOWNERS.disabled +++ b/.github/CODEOWNERS.disabled @@ -69,7 +69,6 @@ /moveit_experimental/ @AndyZe /moveit_ros/perception/ @jliukkonen @RoboticsYY -/moveit_ros/manipulation/ @v4hn @felixvd /moveit_ros/benchmarks/ @henningkayser @MohmadAyman /moveit_ros/planning_interface/ @mintar @rhaschke @felixvd /moveit_ros/robot_interaction/ @mikeferguson @rhaschke diff --git a/moveit_ros/README.md b/moveit_ros/README.md index ca7caf0a67..44ce5ca3d7 100644 --- a/moveit_ros/README.md +++ b/moveit_ros/README.md @@ -4,5 +4,4 @@ This repository includes components of MoveIt that use ROS. This is where much o - planning - planning interfaces - benchmarking -- manipulation - visualization diff --git a/moveit_ros/manipulation/CHANGELOG.rst b/moveit_ros/manipulation/CHANGELOG.rst deleted file mode 100644 index c531e54c7a..0000000000 --- a/moveit_ros/manipulation/CHANGELOG.rst +++ /dev/null @@ -1,247 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package moveit_ros_manipulation -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.1.1 (2020-10-13) ------------------- -* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 `_) -* Contributors: Felix von Drigalski - -1.1.0 (2020-09-04) ------------------- - -1.0.6 (2020-08-19) ------------------- -* [maint] Migrate to clang-format-10 -* [maint] Optimize includes (`#2229 `_) -* Contributors: Markus Vieth, Robert Haschke - -1.0.5 (2020-07-15) ------------------- - -1.0.4 (2020-05-30) ------------------- - -1.0.3 (2020-04-26) ------------------- -* [maint] Apply clang-tidy fix to entire code base (`#1394 `_) -* [maint] Fix errors: catkin_lint 1.6.7 (`#1987 `_) -* [maint] Windows build: Fix binary artifact install locations. (`#1575 `_) -* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) -* Contributors: Robert Haschke, Sean Yen, Yu, Yan - -1.0.2 (2019-06-28) ------------------- - -1.0.1 (2019-03-08) ------------------- -* [improve] Apply clang tidy fix to entire code base (Part 1) (`#1366 `_) -* Contributors: Yu, Yan - -1.0.0 (2019-02-24) ------------------- -* [fix] catkin_lint issues (`#1341 `_) -* [improve] Remove (redundant) random seeding and #attempts from RobotState::setFromIK() as the IK solver perform random seeding themselves. `#1288 `_ -* Contributors: Robert Haschke - -0.10.8 (2018-12-24) -------------------- - -0.10.7 (2018-12-13) -------------------- - -0.10.6 (2018-12-09) -------------------- -* [maintenance] Replaced Eigen::Affine3d -> Eigen::Isometry3d (`#1096 `_) -* [maintenance] Code Cleanup (`#1196 `_) -* Contributors: Robert Haschke - -0.10.5 (2018-11-01) -------------------- - -0.10.4 (2018-10-29) -------------------- - -0.10.3 (2018-10-29) -------------------- - -0.10.2 (2018-10-24) -------------------- -* [fix] Eigen alignment issuses due to missing aligned allocation (`#1039 `_) -* [enhancement] Add info messages to pick and place routine (`#1004 `_) -* [maintenance] add minimum required pluginlib version (`#927 `_) -* Contributors: Adrian Zwiener, Felix von Drigalski, Mikael Arguedas, Mohmmad Ayman, Robert Haschke, mike lautman - -0.10.1 (2018-05-25) -------------------- -* [maintenance] migration from tf to tf2 API (`#830 `_) -* Contributors: Ian McMahon - -0.9.11 (2017-12-25) -------------------- - -0.9.10 (2017-12-09) -------------------- -* [capability][kinetic onward][moveit_ros_planning_interface] Adapt pick pipeline to function without object (`#599 `_) -* Contributors: 2scholz - -0.9.9 (2017-08-06) ------------------- - -0.9.8 (2017-06-21) ------------------- - -0.9.7 (2017-06-05) ------------------- - -0.9.6 (2017-04-12) ------------------- -* [fix] Set planning frame correctly in evaluation of reachable and valid pose filter (`#476 `_) -* Contributors: Yannick Jonetzko - -0.9.5 (2017-03-08) ------------------- -* [fix][moveit_ros_warehouse] gcc6 build error `#423 `_ -* [enhancement] Remove "catch (...)" instances, catch std::exception instead of std::runtime_error (`#445 `_) -* Contributors: Bence Magyar, Dave Coleman - -0.9.4 (2017-02-06) ------------------- -* [maintenance] clang-format upgraded to 3.8 (`#367 `_) -* [fix] race conditions when updating PlanningScene (`#350 `_) -* Contributors: Dave Coleman, Robert Haschke - -0.9.3 (2016-11-16) ------------------- -* [maintenance] Updated package.xml maintainers and author emails `#330 `_ -* [enhancement] remove grasp service support from pick_place's fillGrasp (`#328 `_) -* [maintenance] Updated package.xml maintainers and author emails `#330 `_ -* Contributors: Dave Coleman, Ian McMahon, Michael Goerner - -0.9.2 (2016-11-05) ------------------- - -0.6.6 (2016-06-08) ------------------- -* replaced cmake_modules dependency with eigen -* [jade] eigen3 adjustment -* Removed trailing whitespace from entire repository -* Contributors: Dave Coleman, Isaac I.Y. Saito, Robert Haschke - -0.6.5 (2015-01-24) ------------------- -* update maintainers -* Contributors: Michael Ferguson - -0.6.4 (2014-12-20) ------------------- - -0.6.3 (2014-12-03) ------------------- -* trivial fixes for warnings -* use named logging for manipulation components -* Contributors: Michael Ferguson - -0.6.2 (2014-10-31) ------------------- - -0.6.1 (2014-10-31) ------------------- - -0.6.0 (2014-10-27) ------------------- -* Fix bug in place-planning where attached object was not considered in plan. -* Contributors: Dave Hershberger, Sachin Chitta - -0.5.19 (2014-06-23) -------------------- -* fixes `#461 ` and a potential segfault -* Now check if there is a time for the gripper trajectory. - If there is a time, use that one on the gripper trajectory, if not, keep - with the previous strategy of using a default time. -* Contributors: Michael Ferguson, Sammy Pfeiffer - -0.5.18 (2014-03-23) -------------------- - -0.5.17 (2014-03-22) -------------------- -* update build system for ROS indigo -* fix merge -* refactor how we use params for pick&place -* set the pose frame so we don't get a crash in approach&translate -* Contributors: Ioan Sucan - -0.5.16 (2014-02-27) -------------------- - -0.5.14 (2014-02-06) -------------------- - -0.5.13 (2014-02-06) -------------------- -* ApproachAndTranslateStage dynamic reconfigure bug fixed. - The bug shows up in test code, where it becomes apparent that creating a ros::NodeHandle - in a static initializer makes it very difficult to call ros::init() before creating - the first NodeHandle. -* Contributors: Dave Hershberger - -0.5.12 (2014-01-03) -------------------- - -0.5.11 (2014-01-03) -------------------- -* Fixed internal comment. -* Contributors: Dave Hershberger - -0.5.10 (2013-12-08) -------------------- - -0.5.9 (2013-12-03) ------------------- -* corrected maintainers email -* PickPlace: Added comments, renamed variables to be more specific -* use ROS_ERROR instead of logError - -0.5.8 (2013-10-11) ------------------- -* fix `#331 `_. -* try to identify the eef and group based on the attached object name - -0.5.7 (2013-10-01) ------------------- -* use the fact we know an eef must be defined for the place action to simplify code -* abort place if eef cannot be determined, fixes `#325 `_. -* fix segfault in approach translate - -0.5.6 (2013-09-26) ------------------- -* dep on manipulation_msgs needs to be added here - -0.5.5 (2013-09-23) ------------------- -* use new messages for pick & place -* porting to new RobotState API - -0.5.4 (2013-08-14) ------------------- - -* make headers and author definitions aligned the same way; white space fixes -* adding manipulation tab, fixed bugs in planning scene interface - -0.5.2 (2013-07-15) ------------------- - -0.5.1 (2013-07-14) ------------------- - -0.5.0 (2013-07-12) ------------------- -* white space fixes (tabs are now spaces) - -0.4.5 (2013-07-03) ------------------- - -0.4.4 (2013-06-26) ------------------- -* bugfixes diff --git a/moveit_ros/manipulation/CMakeLists.txt b/moveit_ros/manipulation/CMakeLists.txt deleted file mode 100644 index 022bc76e91..0000000000 --- a/moveit_ros/manipulation/CMakeLists.txt +++ /dev/null @@ -1,62 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) -project(moveit_ros_manipulation) - -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - -find_package(catkin REQUIRED COMPONENTS - moveit_core - moveit_msgs - moveit_ros_planning - moveit_ros_move_group - dynamic_reconfigure - roscpp - rosconsole - tf2_eigen - pluginlib - actionlib -) - -find_package(Eigen3 REQUIRED) -find_package(Boost REQUIRED thread system filesystem date_time program_options) - -generate_dynamic_reconfigure_options("pick_place/cfg/PickPlaceDynamicReconfigure.cfg") - -catkin_package( - INCLUDE_DIRS - pick_place/include - move_group_pick_place_capability/include - LIBRARIES - moveit_pick_place_planner - CATKIN_DEPENDS - actionlib - dynamic_reconfigure - moveit_core - moveit_msgs - moveit_ros_planning - roscpp - DEPENDS - EIGEN3 -) - -include_directories(pick_place/include) -include_directories(move_group_pick_place_capability/include) - -include_directories(SYSTEM - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS}) - -add_subdirectory(pick_place) -add_subdirectory(move_group_pick_place_capability) - -install(FILES - pick_place_capability_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/moveit_ros/manipulation/COLCON_IGNORE b/moveit_ros/manipulation/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/CMakeLists.txt b/moveit_ros/manipulation/move_group_pick_place_capability/CMakeLists.txt deleted file mode 100644 index fc1cb27f0e..0000000000 --- a/moveit_ros/manipulation/move_group_pick_place_capability/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -set(MOVEIT_LIB_NAME moveit_move_group_pick_place_capability) - -include_directories(src) - -add_library(${MOVEIT_LIB_NAME} - src/pick_place_action_capability.cpp -) -set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - -target_link_libraries(${MOVEIT_LIB_NAME} moveit_pick_place_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - -install(TARGETS ${MOVEIT_LIB_NAME} - EXPORT ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) - -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h b/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h deleted file mode 100644 index 5feaa87575..0000000000 --- a/moveit_ros/manipulation/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h +++ /dev/null @@ -1,45 +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 */ - -#pragma once - -#include - -namespace move_group -{ -static const std::string PICKUP_ACTION = "pickup"; // name of 'pickup' action -static const std::string PLACE_ACTION = "place"; // name of 'place' action -} // namespace move_group diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp deleted file mode 100644 index b33390b2c5..0000000000 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp +++ /dev/null @@ -1,474 +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 "pick_place_action_capability.h" -#include -#include -#include - -move_group::MoveGroupPickPlaceAction::MoveGroupPickPlaceAction() - : MoveGroupCapability("PickPlaceAction"), pickup_state_(IDLE) -{ -} - -void move_group::MoveGroupPickPlaceAction::initialize() -{ - pick_place_ = std::make_shared(context_->planning_pipeline_); - pick_place_->displayComputedMotionPlans(true); - - if (context_->debug_) - pick_place_->displayProcessedGrasps(true); - - // start the pickup action server - pickup_action_server_ = std::make_unique>( - root_node_handle_, PICKUP_ACTION, [this](const auto& goal) { executePickupCallback(goal); }, false); - pickup_action_server_->registerPreemptCallback([this] { preemptPickupCallback(); }); - pickup_action_server_->start(); - - // start the place action server - place_action_server_ = std::make_unique>( - root_node_handle_, PLACE_ACTION, [this](const auto& goal) { executePlaceCallback(goal); }, false); - place_action_server_->registerPreemptCallback([this] { preemptPlaceCallback(); }); - place_action_server_->start(); -} - -void move_group::MoveGroupPickPlaceAction::startPickupExecutionCallback() -{ - setPickupState(MONITOR); -} - -void move_group::MoveGroupPickPlaceAction::startPickupLookCallback() -{ - setPickupState(LOOK); -} - -void move_group::MoveGroupPickPlaceAction::startPlaceExecutionCallback() -{ - setPlaceState(MONITOR); -} - -void move_group::MoveGroupPickPlaceAction::startPlaceLookCallback() -{ - setPlaceState(LOOK); -} - -void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanOnly( - const moveit_msgs::action::PickupGoalConstPtr& goal, moveit_msgs::action::PickupResult& action_res) -{ - pick_place::PickPlanPtr plan; - try - { - planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_); - plan = pick_place_->planPick(ps, *goal); - } - catch (std::exception& ex) - { - ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what()); - } - - if (plan) - { - const std::vector& success = plan->getSuccessfulManipulationPlans(); - if (success.empty()) - { - action_res.error_code = plan->getErrorCode(); - } - else - { - const pick_place::ManipulationPlanPtr& result = success.back(); - convertToMsg(result->trajectories_, action_res.trajectory_start, action_res.trajectory_stages); - action_res.trajectory_descriptions.resize(result->trajectories_.size()); - for (std::size_t i = 0; i < result->trajectories_.size(); ++i) - action_res.trajectory_descriptions[i] = result->trajectories_[i].description_; - if (result->id_ < goal->possible_grasps.size()) - action_res.grasp = goal->possible_grasps[result->id_]; - action_res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - action_res.planning_time = plan->getLastPlanTime(); - } - } - else - { - action_res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; - } -} - -void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanOnly( - const moveit_msgs::action::PlaceGoalConstPtr& goal, moveit_msgs::action::PlaceResult& action_res) -{ - pick_place::PlacePlanPtr plan; - try - { - planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_); - plan = pick_place_->planPlace(ps, *goal); - } - catch (std::exception& ex) - { - ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what()); - } - - if (plan) - { - const std::vector& success = plan->getSuccessfulManipulationPlans(); - if (success.empty()) - { - action_res.error_code = plan->getErrorCode(); - } - else - { - const pick_place::ManipulationPlanPtr& result = success.back(); - convertToMsg(result->trajectories_, action_res.trajectory_start, action_res.trajectory_stages); - action_res.trajectory_descriptions.resize(result->trajectories_.size()); - for (std::size_t i = 0; i < result->trajectories_.size(); ++i) - action_res.trajectory_descriptions[i] = result->trajectories_[i].description_; - if (result->id_ < goal->place_locations.size()) - action_res.place_location = goal->place_locations[result->id_]; - action_res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - action_res.planning_time = plan->getLastPlanTime(); - } - } - else - { - action_res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; - } -} - -bool move_group::MoveGroupPickPlaceAction::planUsingPickPlacePickup(const moveit_msgs::action::PickupGoal& goal, - moveit_msgs::action::PickupResult* action_res, - plan_execution::ExecutableMotionPlan& plan) -{ - setPickupState(PLANNING); - - planning_scene_monitor::LockedPlanningSceneRO ps(plan.planning_scene_monitor_); - - pick_place::PickPlanPtr pick_plan; - try - { - pick_plan = pick_place_->planPick(plan.planning_scene_, goal); - } - catch (std::exception& ex) - { - ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what()); - } - - if (pick_plan) - { - const std::vector& success = pick_plan->getSuccessfulManipulationPlans(); - if (success.empty()) - { - plan.error_code_ = pick_plan->getErrorCode(); - } - else - { - const pick_place::ManipulationPlanPtr& result = success.back(); - plan.plan_components_ = result->trajectories_; - if (result->id_ < goal.possible_grasps.size()) - action_res->grasp = goal.possible_grasps[result->id_]; - plan.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - action_res->planning_time = pick_plan->getLastPlanTime(); - } - } - else - { - plan.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; - } - - return plan.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; -} - -bool move_group::MoveGroupPickPlaceAction::planUsingPickPlacePlace(const moveit_msgs::action::PlaceGoal& goal, - moveit_msgs::action::PlaceResult* action_res, - plan_execution::ExecutableMotionPlan& plan) -{ - setPlaceState(PLANNING); - - planning_scene_monitor::LockedPlanningSceneRO ps(plan.planning_scene_monitor_); - - pick_place::PlacePlanPtr place_plan; - try - { - place_plan = pick_place_->planPlace(plan.planning_scene_, goal); - } - catch (std::exception& ex) - { - ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what()); - } - - if (place_plan) - { - const std::vector& success = place_plan->getSuccessfulManipulationPlans(); - if (success.empty()) - { - plan.error_code_ = place_plan->getErrorCode(); - } - else - { - const pick_place::ManipulationPlanPtr& result = success.back(); - plan.plan_components_ = result->trajectories_; - if (result->id_ < goal.place_locations.size()) - action_res->place_location = goal.place_locations[result->id_]; - plan.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - action_res->planning_time = place_plan->getLastPlanTime(); - } - } - else - { - plan.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; - } - - return plan.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; -} - -void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanAndExecute( - const moveit_msgs::action::PickupGoalConstPtr& goal, moveit_msgs::action::PickupResult& action_res) -{ - plan_execution::PlanExecution::Options opt; - - opt.replan_ = goal->planning_options.replan; - opt.replan_attempts_ = goal->planning_options.replan_attempts; - opt.replan_delay_ = goal->planning_options.replan_delay; - opt.before_execution_callback_ = [this] { startPickupExecutionCallback(); }; - - opt.plan_callback_ = [this, &g = *goal, result_ptr = &action_res](plan_execution::ExecutableMotionPlan& plan) { - return planUsingPickPlacePickup(g, result_ptr, plan); - }; - if (goal->planning_options.look_around && context_->plan_with_sensing_) - { - opt.plan_callback_ = [plan_with_sensing = context_->plan_with_sensing_.get(), planner = opt.plan_callback_, - attempts = goal->planning_options.look_around_attempts, - safe_execution_cost = goal->planning_options.max_safe_execution_cost]( - plan_execution::ExecutableMotionPlan& plan) { - return plan_with_sensing->computePlan(plan, planner, attempts, safe_execution_cost); - }; - context_->plan_with_sensing_->setBeforeLookCallback([this] { startPickupLookCallback(); }); - } - - plan_execution::ExecutableMotionPlan plan; - context_->plan_execution_->planAndExecute(plan, goal->planning_options.planning_scene_diff, opt); - - convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.trajectory_stages); - action_res.trajectory_descriptions.resize(plan.plan_components_.size()); - for (std::size_t i = 0; i < plan.plan_components_.size(); ++i) - action_res.trajectory_descriptions[i] = plan.plan_components_[i].description_; - action_res.error_code = plan.error_code_; -} - -void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanAndExecute( - const moveit_msgs::action::PlaceGoalConstPtr& goal, moveit_msgs::action::PlaceResult& action_res) -{ - plan_execution::PlanExecution::Options opt; - - opt.replan_ = goal->planning_options.replan; - opt.replan_attempts_ = goal->planning_options.replan_attempts; - opt.replan_delay_ = goal->planning_options.replan_delay; - opt.before_execution_callback_ = [this] { startPlaceExecutionCallback(); }; - opt.plan_callback_ = [this, g = *goal, result_ptr = &action_res](plan_execution::ExecutableMotionPlan& plan) { - return planUsingPickPlacePlace(g, result_ptr, plan); - }; - if (goal->planning_options.look_around && context_->plan_with_sensing_) - { - opt.plan_callback_ = [plan_with_sensing = context_->plan_with_sensing_.get(), planner = opt.plan_callback_, - attempts = goal->planning_options.look_around_attempts, - safe_execution_cost = goal->planning_options.max_safe_execution_cost]( - plan_execution::ExecutableMotionPlan& plan) { - return plan_with_sensing->computePlan(plan, planner, attempts, safe_execution_cost); - }; - context_->plan_with_sensing_->setBeforeLookCallback([this] { startPlaceLookCallback(); }); - } - - plan_execution::ExecutableMotionPlan plan; - context_->plan_execution_->planAndExecute(plan, goal->planning_options.planning_scene_diff, opt); - - convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.trajectory_stages); - action_res.trajectory_descriptions.resize(plan.plan_components_.size()); - for (std::size_t i = 0; i < plan.plan_components_.size(); ++i) - action_res.trajectory_descriptions[i] = plan.plan_components_[i].description_; - action_res.error_code = plan.error_code_; -} - -void move_group::MoveGroupPickPlaceAction::executePickupCallback( - const moveit_msgs::action::PickupGoalConstPtr& input_goal) -{ - setPickupState(PLANNING); - - // before we start planning, ensure that we have the latest robot state received... - auto node = context_->moveit_cpp_->getNode(); - context_->planning_scene_monitor_->waitForCurrentRobotState(node->get_clock()->now()); - context_->planning_scene_monitor_->updateFrameTransforms(); - - moveit_msgs::action::PickupGoalConstPtr goal; - if (input_goal->possible_grasps.empty()) - { - moveit_msgs::action::PickupGoal* copy(new moveit_msgs::action::PickupGoal(*input_goal)); - goal.reset(copy); - fillGrasps(*copy); - } - else - goal = input_goal; - - moveit_msgs::action::PickupResult action_res; - - if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_) - { - if (!goal->planning_options.plan_only) - ROS_WARN_NAMED("manipulation", "This instance of MoveGroup is not allowed to execute trajectories but the pick " - "goal request has plan_only set to false. Only a motion plan will be computed " - "anyway."); - executePickupCallbackPlanOnly(goal, action_res); - } - else - executePickupCallbackPlanAndExecute(goal, action_res); - - bool planned_trajectory_empty = action_res.trajectory_stages.empty(); - std::string response = - getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only); - if (action_res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) - pickup_action_server_->setSucceeded(action_res, response); - else - { - if (action_res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED) - pickup_action_server_->setPreempted(action_res, response); - else - pickup_action_server_->setAborted(action_res, response); - } - - setPickupState(IDLE); -} - -void move_group::MoveGroupPickPlaceAction::executePlaceCallback(const moveit_msgs::action::PlaceGoalConstPtr& goal) -{ - setPlaceState(PLANNING); - - // before we start planning, ensure that we have the latest robot state received... - auto node = context_->moveit_cpp_->getNode(); - context_->planning_scene_monitor_->waitForCurrentRobotState(node->get_clock()->now()); - context_->planning_scene_monitor_->updateFrameTransforms(); - - moveit_msgs::action::PlaceResult action_res; - - if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_) - { - if (!goal->planning_options.plan_only) - ROS_WARN_NAMED("manipulation", "This instance of MoveGroup is not allowed to execute trajectories but the place " - "goal request has plan_only set to false. Only a motion plan will be computed " - "anyway."); - executePlaceCallbackPlanOnly(goal, action_res); - } - else - executePlaceCallbackPlanAndExecute(goal, action_res); - - bool planned_trajectory_empty = action_res.trajectory_stages.empty(); - std::string response = - getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only); - if (action_res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) - place_action_server_->setSucceeded(action_res, response); - else - { - if (action_res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED) - place_action_server_->setPreempted(action_res, response); - else - place_action_server_->setAborted(action_res, response); - } - - setPlaceState(IDLE); -} - -void move_group::MoveGroupPickPlaceAction::preemptPickupCallback() -{ -} - -void move_group::MoveGroupPickPlaceAction::preemptPlaceCallback() -{ -} - -void move_group::MoveGroupPickPlaceAction::setPickupState(MoveGroupState state) -{ - pickup_state_ = state; - pickup_feedback_.state = stateToStr(state); - pickup_action_server_->publishFeedback(pickup_feedback_); -} - -void move_group::MoveGroupPickPlaceAction::setPlaceState(MoveGroupState state) -{ - place_state_ = state; - place_feedback_.state = stateToStr(state); - place_action_server_->publishFeedback(place_feedback_); -} - -void move_group::MoveGroupPickPlaceAction::fillGrasps(moveit_msgs::action::PickupGoal& goal) -{ - planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_); - - ROS_DEBUG_NAMED("manipulation", "Using default grasp poses"); - goal.minimize_object_distance = true; - - // add a number of default grasp points - // \todo add more! - moveit_msgs::msg::Grasp g; - g.grasp_pose.header.frame_id = goal.target_name; - g.grasp_pose.pose.position.x = -0.2; - g.grasp_pose.pose.position.y = 0.0; - g.grasp_pose.pose.position.z = 0.0; - g.grasp_pose.pose.orientation.x = 0.0; - g.grasp_pose.pose.orientation.y = 0.0; - g.grasp_pose.pose.orientation.z = 0.0; - g.grasp_pose.pose.orientation.w = 1.0; - - g.pre_grasp_approach.direction.header.frame_id = lscene->getPlanningFrame(); - g.pre_grasp_approach.direction.vector.x = 1.0; - g.pre_grasp_approach.min_distance = 0.1; - g.pre_grasp_approach.desired_distance = 0.2; - - g.post_grasp_retreat.direction.header.frame_id = lscene->getPlanningFrame(); - g.post_grasp_retreat.direction.vector.z = 1.0; - g.post_grasp_retreat.min_distance = 0.1; - g.post_grasp_retreat.desired_distance = 0.2; - - if (lscene->getRobotModel()->hasEndEffector(goal.end_effector)) - { - g.pre_grasp_posture.joint_names = lscene->getRobotModel()->getEndEffector(goal.end_effector)->getJointModelNames(); - g.pre_grasp_posture.points.resize(1); - g.pre_grasp_posture.points[0].positions.resize(g.pre_grasp_posture.joint_names.size(), - std::numeric_limits::max()); - - g.grasp_posture.joint_names = g.pre_grasp_posture.joint_names; - g.grasp_posture.points.resize(1); - g.grasp_posture.points[0].positions.resize(g.grasp_posture.joint_names.size(), -std::numeric_limits::max()); - } - goal.possible_grasps.push_back(g); -} - -#include -CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupPickPlaceAction, move_group::MoveGroupCapability) diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h deleted file mode 100644 index 923dc26c66..0000000000 --- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.h +++ /dev/null @@ -1,104 +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 */ - -#pragma once - -#include -#include -#include -#include -#include - -#include - -namespace move_group -{ -class MoveGroupPickPlaceAction : public MoveGroupCapability -{ -public: - MoveGroupPickPlaceAction(); - void initialize() override; - -private: - void executePickupCallback(const moveit_msgs::action::PickupGoalConstPtr& goal); - void executePlaceCallback(const moveit_msgs::action::PlaceGoalConstPtr& goal); - - void executePickupCallbackPlanOnly(const moveit_msgs::action::PickupGoalConstPtr& goal, - moveit_msgs::action::PickupResult& action_res); - void executePickupCallbackPlanAndExecute(const moveit_msgs::action::PickupGoalConstPtr& goal, - moveit_msgs::action::PickupResult& action_res); - - void executePlaceCallbackPlanOnly(const moveit_msgs::action::PlaceGoalConstPtr& goal, - moveit_msgs::action::PlaceResult& action_res); - void executePlaceCallbackPlanAndExecute(const moveit_msgs::action::PlaceGoalConstPtr& goal, - moveit_msgs::action::PlaceResult& action_res); - - bool planUsingPickPlacePickup(const moveit_msgs::action::PickupGoal& goal, - moveit_msgs::action::PickupResult* action_res, - plan_execution::ExecutableMotionPlan& plan); - bool planUsingPickPlacePlace(const moveit_msgs::action::PlaceGoal& goal, moveit_msgs::action::PlaceResult* action_res, - plan_execution::ExecutableMotionPlan& plan); - - void preemptPickupCallback(); - void preemptPlaceCallback(); - - void startPickupLookCallback(); - void startPickupExecutionCallback(); - - void startPlaceLookCallback(); - void startPlaceExecutionCallback(); - - void setPickupState(MoveGroupState state); - void setPlaceState(MoveGroupState state); - - void fillGrasps(moveit_msgs::action::PickupGoal& goal); - - pick_place::PickPlacePtr pick_place_; - - std::unique_ptr > pickup_action_server_; - moveit_msgs::action::PickupFeedback pickup_feedback_; - - std::unique_ptr > place_action_server_; - moveit_msgs::action::PlaceFeedback place_feedback_; - - std::unique_ptr diff_attached_object_; - - MoveGroupState pickup_state_; - MoveGroupState place_state_; - - ros::ServiceClient grasp_planning_service_; -}; -} // namespace move_group diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml deleted file mode 100644 index b4b3900194..0000000000 --- a/moveit_ros/manipulation/package.xml +++ /dev/null @@ -1,40 +0,0 @@ - - - - moveit_ros_manipulation - 1.1.7 - Components of MoveIt used for manipulation - Michael Görner - Henning Kayser - Tyler Weaver - MoveIt Release Team - - BSD - - http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 - - Ioan Sucan - Sachin Chitta - - catkin - - actionlib - dynamic_reconfigure - moveit_core - moveit_ros_move_group - moveit_ros_planning - moveit_msgs - roscpp - rosconsole - tf2_eigen - pluginlib - - eigen - - - - - - diff --git a/moveit_ros/manipulation/pick_place/CMakeLists.txt b/moveit_ros/manipulation/pick_place/CMakeLists.txt deleted file mode 100644 index c2e8edc6ee..0000000000 --- a/moveit_ros/manipulation/pick_place/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -set(MOVEIT_LIB_NAME moveit_pick_place_planner) - -add_library(${MOVEIT_LIB_NAME} - src/pick_place_params.cpp - src/manipulation_pipeline.cpp - src/reachable_valid_pose_filter.cpp - src/approach_and_translate_stage.cpp - src/plan_stage.cpp - src/pick_place.cpp - src/pick.cpp - src/place.cpp -) -set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - -target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - -install(TARGETS ${MOVEIT_LIB_NAME} - EXPORT ${MOVEIT_LIB_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/manipulation/pick_place/cfg/PickPlaceDynamicReconfigure.cfg b/moveit_ros/manipulation/pick_place/cfg/PickPlaceDynamicReconfigure.cfg deleted file mode 100755 index 9e4f8260dd..0000000000 --- a/moveit_ros/manipulation/pick_place/cfg/PickPlaceDynamicReconfigure.cfg +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "moveit_ros_manipulation" -from dynamic_reconfigure.parameter_generator_catkin import * -gen = ParameterGenerator() - -gen.add("max_attempted_states_per_pose", int_t, 1, "The maximum number of robot configurations to attempt at a particular eef pose", 5, 1, 100) -gen.add("max_consecutive_fail_attempts", int_t, 2, "The maximum consecutive failures at generating configurations matching a pose before failure", 3, 1, 10) -gen.add("cartesian_motion_step_size", double_t, 3, "The distance (meters, for end-effector) between consecutive waypoints on Cartesian motions", 0.02, 0.005, 0.1) -gen.add("jump_factor", double_t, 4, "The maximum allowed distance in configuration space between consecutive waypoints on Cartesian motions", 2.0, 0.0, 10.0) - -exit(gen.generate(PACKAGE, PACKAGE, "PickPlaceDynamicReconfigure")) diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h deleted file mode 100644 index 8c7014495f..0000000000 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h +++ /dev/null @@ -1,63 +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, Sachin Chitta */ - -#pragma once - -#include -#include -#include - -namespace pick_place -{ -class ApproachAndTranslateStage : public ManipulationStage -{ -public: - ApproachAndTranslateStage(const planning_scene::PlanningSceneConstPtr& scene, - const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix); - - bool evaluate(const ManipulationPlanPtr& plan) const override; - -private: - planning_scene::PlanningSceneConstPtr planning_scene_; - collision_detection::AllowedCollisionMatrixConstPtr collision_matrix_; - trajectory_processing::IterativeParabolicTimeParameterization time_param_; - - unsigned int max_goal_count_; - unsigned int max_fail_; - double max_step_; - double jump_factor_; -}; -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h deleted file mode 100644 index f3471f54ba..0000000000 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h +++ /dev/null @@ -1,118 +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 */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace pick_place -{ -/** \brief Represent the sequence of steps that are executed for a manipulation plan */ -class ManipulationPipeline -{ -public: - ManipulationPipeline(const std::string& name, unsigned int nthreads); - virtual ~ManipulationPipeline(); - - const std::string& getName() const - { - return name_; - } - - void setSolutionCallback(const std::function& callback) - { - solution_callback_ = callback; - } - - void setEmptyQueueCallback(const std::function& callback) - { - empty_queue_callback_ = callback; - } - - ManipulationPipeline& addStage(const ManipulationStagePtr& next); - const ManipulationStagePtr& getFirstStage() const; - const ManipulationStagePtr& getLastStage() const; - void reset(); - - void setVerbose(bool flag); - - void signalStop(); - void start(); - void stop(); - - void push(const ManipulationPlanPtr& grasp); - void clear(); - - const std::vector& getSuccessfulManipulationPlans() const - { - return success_; - } - - const std::vector& getFailedManipulationPlans() const - { - return failed_; - } - - void reprocessLastFailure(); - -protected: - void processingThread(unsigned int index); - - std::string name_; - unsigned int nthreads_; - bool verbose_; - std::vector stages_; - - std::deque queue_; - std::vector success_; - std::vector failed_; - - std::vector processing_threads_; - std::condition_variable queue_access_cond_; - std::mutex queue_access_lock_; - std::mutex result_lock_; - - std::function solution_callback_; - std::function empty_queue_callback_; - unsigned int empty_queue_threads_; - - bool stop_processing_; -}; -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h deleted file mode 100644 index 90f4e95aa4..0000000000 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_plan.h +++ /dev/null @@ -1,144 +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, Sachin Chitta */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace pick_place -{ -MOVEIT_STRUCT_FORWARD(ManipulationPlanSharedData); - -struct ManipulationPlanSharedData -{ - ManipulationPlanSharedData() - : planning_group_(nullptr) - , end_effector_group_(nullptr) - , ik_link_(nullptr) - , max_goal_sampling_attempts_(0) - , minimize_object_distance_(false) - { - } - - const moveit::core::JointModelGroup* planning_group_; - const moveit::core::JointModelGroup* end_effector_group_; - const moveit::core::LinkModel* ik_link_; - - unsigned int max_goal_sampling_attempts_; - - std::string planner_id_; - - bool minimize_object_distance_; - - moveit_msgs::msg::Constraints path_constraints_; - - moveit_msgs::msg::AttachedCollisionObject diff_attached_object_; - - ros::WallTime timeout_; -}; - -MOVEIT_STRUCT_FORWARD(ManipulationPlan); - -struct ManipulationPlan -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - ManipulationPlan(const ManipulationPlanSharedDataConstPtr& shared_data) - : shared_data_(shared_data), processing_stage_(0) - { - } - - /// Restore this plan to a state that makes it look like it never was processed by the manipulation pipeline - void clear() - { - goal_sampler_.reset(); - trajectories_.clear(); - approach_state_.reset(); - possible_goal_states_.clear(); - processing_stage_ = 0; - } - - // Shared data between manipulation plans (set at initialization) - ManipulationPlanSharedDataConstPtr shared_data_; - - // the approach motion towards the goal - moveit_msgs::msg::GripperTranslation approach_; - - // the retreat motion away from the goal - moveit_msgs::msg::GripperTranslation retreat_; - - // the kinematic configuration of the end effector when approaching the goal (an open gripper) - trajectory_msgs::JointTrajectory approach_posture_; - - // the kinematic configuration of the end effector when retreating from the goal (a closed gripper) - trajectory_msgs::JointTrajectory retreat_posture_; - - // -------------- computed data -------------------------- - geometry_msgs::PoseStamped goal_pose_; - Eigen::Isometry3d transformed_goal_pose_; - - moveit_msgs::msg::Constraints goal_constraints_; - - // Allows for the sampling of a kineamtic state for a particular group of a robot - constraint_samplers::ConstraintSamplerPtr goal_sampler_; - - std::vector possible_goal_states_; - - moveit::core::RobotStatePtr approach_state_; - - // The sequence of trajectories produced for execution - std::vector trajectories_; - - // An error code reflecting what went wrong (if anything) - moveit_msgs::msg::MoveItErrorCodes error_code_; - - // The processing stage that was last working on this plan, or was about to work on this plan - std::size_t processing_stage_; - - // An id for this plan; this is usually the index of the Grasp / PlaceLocation in the input request - std::size_t id_; -}; -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h deleted file mode 100644 index 14636fe688..0000000000 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_stage.h +++ /dev/null @@ -1,85 +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, Sachin Chitta */ - -#pragma once - -#include -#include -#include - -namespace pick_place -{ -MOVEIT_CLASS_FORWARD(ManipulationStage); // Defines ManipulationStagePtr, ConstPtr, WeakPtr... etc - -class ManipulationStage -{ -public: - ManipulationStage(const std::string& name) : name_(name), signal_stop_(false), verbose_(false) - { - } - - virtual ~ManipulationStage() - { - } - - const std::string& getName() const - { - return name_; - } - - void setVerbose(bool flag) - { - verbose_ = flag; - } - - virtual void resetStopSignal() - { - signal_stop_ = false; - } - - virtual void signalStop() - { - signal_stop_ = true; - } - - virtual bool evaluate(const ManipulationPlanPtr& plan) const = 0; - -protected: - std::string name_; - bool signal_stop_; - bool verbose_; -}; -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h deleted file mode 100644 index 30fd775408..0000000000 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place.h +++ /dev/null @@ -1,166 +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 */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace pick_place -{ -MOVEIT_CLASS_FORWARD(PickPlace); // Defines PickPlacePtr, ConstPtr, WeakPtr... etc - -class PickPlacePlanBase -{ -public: - PickPlacePlanBase(const PickPlaceConstPtr& pick_place, const std::string& name); - ~PickPlacePlanBase(); - - const std::vector& getSuccessfulManipulationPlans() const - { - return pipeline_.getSuccessfulManipulationPlans(); - } - const std::vector& getFailedManipulationPlans() const - { - return pipeline_.getFailedManipulationPlans(); - } - - const moveit_msgs::msg::MoveItErrorCodes& getErrorCode() const - { - return error_code_; - } - - double getLastPlanTime() const - { - return last_plan_time_; - } - -protected: - void initialize(); - void waitForPipeline(const ros::WallTime& endtime); - void foundSolution(); - void emptyQueue(); - - PickPlaceConstPtr pick_place_; - ManipulationPipeline pipeline_; - - double last_plan_time_; - bool done_; - bool pushed_all_poses_; - std::condition_variable done_condition_; - std::mutex done_mutex_; - moveit_msgs::msg::MoveItErrorCodes error_code_; -}; - -MOVEIT_CLASS_FORWARD(PickPlan); // Defines PickPlanPtr, ConstPtr, WeakPtr... etc - -class PickPlan : public PickPlacePlanBase -{ -public: - PickPlan(const PickPlaceConstPtr& pick_place); - bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::action::PickupGoal& goal); -}; - -MOVEIT_CLASS_FORWARD(PlacePlan); // Defines PlacePlanPtr, ConstPtr, WeakPtr... etc - -class PlacePlan : public PickPlacePlanBase -{ -public: - PlacePlan(const PickPlaceConstPtr& pick_place); - bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::action::PlaceGoal& goal); -}; - -class PickPlace : private boost::noncopyable, public std::enable_shared_from_this -{ -public: - static const std::string DISPLAY_PATH_TOPIC; - static const std::string DISPLAY_GRASP_TOPIC; - - // the amount of time (maximum) to wait for achieving a grasp posture - static const double DEFAULT_GRASP_POSTURE_COMPLETION_DURATION; // seconds - - PickPlace(const planning_pipeline::PlanningPipelinePtr& planning_pipeline); - - const constraint_samplers::ConstraintSamplerManagerPtr& getConstraintsSamplerManager() const - { - return constraint_sampler_manager_loader_->getConstraintSamplerManager(); - } - - const planning_pipeline::PlanningPipelinePtr& getPlanningPipeline() const - { - return planning_pipeline_; - } - - const moveit::core::RobotModelConstPtr& getRobotModel() const - { - return planning_pipeline_->getRobotModel(); - } - - /** \brief Plan the sequence of motions that perform a pickup action */ - PickPlanPtr planPick(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::action::PickupGoal& goal) const; - - /** \brief Plan the sequence of motions that perform a placement action */ - PlacePlanPtr planPlace(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::action::PlaceGoal& goal) const; - - void displayComputedMotionPlans(bool flag); - void displayProcessedGrasps(bool flag); - - void visualizePlan(const ManipulationPlanPtr& plan) const; - - void visualizeGrasp(const ManipulationPlanPtr& plan) const; - - void visualizeGrasps(const std::vector& plans) const; - -private: - ros::NodeHandle nh_; - planning_pipeline::PlanningPipelinePtr planning_pipeline_; - bool display_computed_motion_plans_; - bool display_grasps_; - ros::Publisher display_path_publisher_; - ros::Publisher grasps_publisher_; - - constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_; -}; -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h deleted file mode 100644 index 38be2ba658..0000000000 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/pick_place_params.h +++ /dev/null @@ -1,53 +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 */ - -#pragma once - -namespace pick_place -{ -struct PickPlaceParams -{ - PickPlaceParams(); - - unsigned int max_goal_count_; - unsigned int max_fail_; - double max_step_; - double jump_factor_; -}; - -// Get access to a global variable that contains the pick & place params. -const PickPlaceParams& GetGlobalPickPlaceParams(); -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h deleted file mode 100644 index 7b651e196d..0000000000 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/plan_stage.h +++ /dev/null @@ -1,59 +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, Sachin Chitta */ - -#pragma once - -#include -#include -#include - -namespace pick_place -{ -class PlanStage : public ManipulationStage -{ -public: - PlanStage(const planning_scene::PlanningSceneConstPtr& scene, - const planning_pipeline::PlanningPipelinePtr& planning_pipeline); - - void signalStop() override; - - bool evaluate(const ManipulationPlanPtr& plan) const override; - -private: - planning_scene::PlanningSceneConstPtr planning_scene_; - planning_pipeline::PlanningPipelinePtr planning_pipeline_; -}; -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h deleted file mode 100644 index 0e5ec7b0f3..0000000000 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h +++ /dev/null @@ -1,61 +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, Sachin Chitta */ - -#pragma once - -#include -#include -#include - -namespace pick_place -{ -class ReachableAndValidPoseFilter : public ManipulationStage -{ -public: - ReachableAndValidPoseFilter(const planning_scene::PlanningSceneConstPtr& scene, - const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix, - const constraint_samplers::ConstraintSamplerManagerPtr& constraints_sampler_manager); - - bool evaluate(const ManipulationPlanPtr& plan) const override; - -private: - bool isEndEffectorFree(const ManipulationPlanPtr& plan, moveit::core::RobotState& token_state) const; - - planning_scene::PlanningSceneConstPtr planning_scene_; - collision_detection::AllowedCollisionMatrixConstPtr collision_matrix_; - constraint_samplers::ConstraintSamplerManagerPtr constraints_sampler_manager_; -}; -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp deleted file mode 100644 index c9888ed794..0000000000 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ /dev/null @@ -1,382 +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 -#if __has_include() -#include -#else -#include -#endif -#include - -namespace pick_place -{ -ApproachAndTranslateStage::ApproachAndTranslateStage( - const planning_scene::PlanningSceneConstPtr& scene, - const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix) - : ManipulationStage("approach & translate"), planning_scene_(scene), collision_matrix_(collision_matrix) -{ - max_goal_count_ = GetGlobalPickPlaceParams().max_goal_count_; - max_fail_ = GetGlobalPickPlaceParams().max_fail_; - max_step_ = GetGlobalPickPlaceParams().max_step_; - jump_factor_ = GetGlobalPickPlaceParams().jump_factor_; -} - -namespace -{ -bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene, - const collision_detection::AllowedCollisionMatrix* collision_matrix, bool verbose, - const trajectory_msgs::JointTrajectory* grasp_posture, moveit::core::RobotState* state, - const moveit::core::JointModelGroup* group, const double* joint_group_variable_values) -{ - state->setJointGroupPositions(group, joint_group_variable_values); - - collision_detection::CollisionRequest req; - req.verbose = verbose; - req.group_name = group->getName(); - - if (!grasp_posture->joint_names.empty()) - { - // apply the grasp posture for the end effector (we always apply it here since it could be the case the sampler - // changes this posture) - for (std::size_t i = 0; i < grasp_posture->points.size(); ++i) - { - state->setVariablePositions(grasp_posture->joint_names, grasp_posture->points[i].positions); - collision_detection::CollisionResult res; - planning_scene->checkCollision(req, res, *state, *collision_matrix); - if (res.collision) - return false; - } - } - else - { - collision_detection::CollisionResult res; - planning_scene->checkCollision(req, res, *state, *collision_matrix); - if (res.collision) - return false; - } - return planning_scene->isStateFeasible(*state); -} - -bool samplePossibleGoalStates(const ManipulationPlanPtr& plan, const moveit::core::RobotState& reference_state, - double min_distance, unsigned int attempts) -{ - // initialize with scene state - auto token_state = std::make_shared(reference_state); - for (unsigned int j = 0; j < attempts; ++j) - { - double min_d = std::numeric_limits::infinity(); - - // Samples given the constraints, populating the joint state group - if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_)) - { - // Check if this new sampled state is closest we've found so far - for (std::size_t i = 0; i < plan->possible_goal_states_.size(); ++i) - { - double d = plan->possible_goal_states_[i]->distance(*token_state, plan->shared_data_->planning_group_); - if (d < min_d) - min_d = d; - } - if (min_d >= min_distance) - { - plan->possible_goal_states_.push_back(token_state); - return true; - } - } - } - return false; -} - -// This function is called during trajectory execution, after the gripper is closed, to attach the currently gripped -// object -bool executeAttachObject(const ManipulationPlanSharedDataConstPtr& shared_plan_data, - const trajectory_msgs::JointTrajectory& detach_posture, - const plan_execution::ExecutableMotionPlan& motion_plan) -{ - if (shared_plan_data->diff_attached_object_.object.id.empty()) - { - // the user did not provide an object to attach, so just return successfully - return true; - } - - ROS_DEBUG_NAMED("manipulation", "Applying attached object diff to maintained planning scene (attaching/detaching " - "object to end effector)"); - bool ok = false; - { - planning_scene_monitor::LockedPlanningSceneRW ps(motion_plan.planning_scene_monitor_); - moveit_msgs::msg::AttachedCollisionObject msg = shared_plan_data->diff_attached_object_; - // remember the configuration of the gripper before the grasp; - // this configuration will be set again when releasing the object - msg.detach_posture = detach_posture; - ok = ps->processAttachedCollisionObjectMsg(msg); - } - motion_plan.planning_scene_monitor_->triggerSceneUpdateEvent( - (planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType)( - planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY + - planning_scene_monitor::PlanningSceneMonitor::UPDATE_STATE)); - return ok; -} - -// Add the close end effector trajectory to the overall plan (after the approach trajectory, before the retreat) -void addGripperTrajectory(const ManipulationPlanPtr& plan, - const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix, - const std::string& name) -{ - // Check if a "closed" end effector configuration was specified - if (!plan->retreat_posture_.joint_names.empty()) - { - moveit::core::RobotStatePtr ee_closed_state( - new moveit::core::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint())); - - auto ee_closed_traj = std::make_shared( - ee_closed_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName())); - ee_closed_traj->setRobotTrajectoryMsg(*ee_closed_state, plan->retreat_posture_); - // If user has defined a time for it's gripper movement time, don't add the - // DEFAULT_GRASP_POSTURE_COMPLETION_DURATION - if (!plan->retreat_posture_.points.empty() && - plan->retreat_posture_.points.back().time_from_start > ros::Duration(0.0)) - { - ee_closed_traj->addPrefixWayPoint(ee_closed_state, 0.0); - } - else - { // Do what was done before - ROS_INFO_STREAM("Adding default duration of " << PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION - << " seconds to the grasp closure time. Assign time_from_start to " - << "your trajectory to avoid this."); - ee_closed_traj->addPrefixWayPoint(ee_closed_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION); - } - - plan_execution::ExecutableTrajectory et(ee_closed_traj, name); - - // Add a callback to attach the object to the EE after closing the gripper - et.effect_on_success_ = [plan](const plan_execution::ExecutableMotionPlan* motion_plan) { - return executeAttachObject(plan->shared_data_, plan->approach_posture_, *motion_plan); - }; - et.allowed_collision_matrix_ = collision_matrix; - plan->trajectories_.push_back(et); - } - else - { - ROS_WARN_NAMED("manipulation", "No joint states of grasp postures have been defined in the pick place action."); - } -} - -} // namespace - -bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const -{ - const moveit::core::JointModelGroup* jmg = plan->shared_data_->planning_group_; - // compute what the maximum distance reported between any two states in the planning group could be, and keep 1% of - // that; - // this is the minimum distance between sampled goal states - const double min_distance = 0.01 * jmg->getMaximumExtent(); - - // convert approach direction and retreat direction to Eigen structures - Eigen::Vector3d approach_direction, retreat_direction; - tf2::fromMsg(plan->approach_.direction.vector, approach_direction); - tf2::fromMsg(plan->retreat_.direction.vector, retreat_direction); - - // if translation vectors are specified in the frame of the ik link name, then we assume the frame is local; - // otherwise, the frame is global - bool approach_direction_is_global_frame = !moveit::core::Transforms::sameFrame( - plan->approach_.direction.header.frame_id, plan->shared_data_->ik_link_->getName()); - bool retreat_direction_is_global_frame = !moveit::core::Transforms::sameFrame( - plan->retreat_.direction.header.frame_id, plan->shared_data_->ik_link_->getName()); - - // transform the input vectors in accordance to frame specified in the header; - if (approach_direction_is_global_frame) - // getFrameTransform() returns a valid isometry by contract - approach_direction = - planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).linear() * approach_direction; - if (retreat_direction_is_global_frame) - // getFrameTransform() returns a valid isometry by contract - retreat_direction = - planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).linear() * retreat_direction; - - // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping - moveit::core::GroupStateValidityCallbackFn approach_valid_callback = - [scene = planning_scene_.get(), acm = collision_matrix_.get(), verbose = this->verbose_, - approach_posture = &plan->approach_posture_](moveit::core::RobotState* robot_state, - const moveit::core::JointModelGroup* joint_group, - const double* joint_group_variable_values) { - return isStateCollisionFree(scene, acm, verbose, approach_posture, robot_state, joint_group, - joint_group_variable_values); - }; - plan->goal_sampler_->setVerbose(verbose_); - std::size_t attempted_possible_goal_states = 0; - do // continuously sample possible goal states - { - for (std::size_t i = attempted_possible_goal_states; i < plan->possible_goal_states_.size() && !signal_stop_; - ++i, ++attempted_possible_goal_states) - { - // if we are trying to get as close as possible to the goal (maximum one meter) - if (plan->shared_data_->minimize_object_distance_) - { - static const double MAX_CLOSE_UP_DIST = 1.0; - auto close_up_state = std::make_shared(*plan->possible_goal_states_[i]); - std::vector close_up_states; - double d_close_up = moveit::core::CartesianInterpolator::computeCartesianPath( - close_up_state.get(), plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_, - approach_direction, approach_direction_is_global_frame, MAX_CLOSE_UP_DIST, - moveit::core::MaxEEFStep(max_step_), moveit::core::JumpThreshold(jump_factor_), approach_valid_callback); - // if progress towards the object was made, update the desired goal state - if (d_close_up > 0.0 && close_up_states.size() > 1) - *plan->possible_goal_states_[i] = *close_up_states[close_up_states.size() - 2]; - } - - // try to compute a straight line path that arrives at the goal using the specified approach direction - auto first_approach_state = std::make_shared(*plan->possible_goal_states_[i]); - - std::vector approach_states; - double d_approach = moveit::core::CartesianInterpolator::computeCartesianPath( - first_approach_state.get(), plan->shared_data_->planning_group_, approach_states, - plan->shared_data_->ik_link_, -approach_direction, approach_direction_is_global_frame, - plan->approach_.desired_distance, moveit::core::MaxEEFStep(max_step_), - moveit::core::JumpThreshold(jump_factor_), approach_valid_callback); - - // if we were able to follow the approach direction for sufficient length, try to compute a retreat direction - if (d_approach > plan->approach_.min_distance && !signal_stop_) - { - if (plan->retreat_.desired_distance > 0.0) - { - // construct a planning scene that is just a diff on top of our actual planning scene - planning_scene::PlanningScenePtr planning_scene_after_approach = planning_scene_->diff(); - - // assume the current state of the diff world is the one we plan to reach - planning_scene_after_approach->getCurrentStateNonConst() = *plan->possible_goal_states_[i]; - - // apply the difference message to this world that virtually attaches the object we are manipulating - planning_scene_after_approach->processAttachedCollisionObjectMsg(plan->shared_data_->diff_attached_object_); - - // state validity checking during the retreat after the grasp must ensure the gripper posture is that of the - // actual grasp - moveit::core::GroupStateValidityCallbackFn retreat_valid_callback = - [scene = planning_scene_after_approach.get(), acm = collision_matrix_.get(), verbose = verbose_, - retreat_posture = &plan->retreat_posture_](moveit::core::RobotState* robot_state, - const moveit::core::JointModelGroup* joint_group, - const double* joint_group_variable_values) { - return isStateCollisionFree(scene, acm, verbose, retreat_posture, robot_state, joint_group, - joint_group_variable_values); - }; - - // try to compute a straight line path that moves from the goal in a desired direction - moveit::core::RobotStatePtr last_retreat_state( - new moveit::core::RobotState(planning_scene_after_approach->getCurrentState())); - std::vector retreat_states; - double d_retreat = moveit::core::CartesianInterpolator::computeCartesianPath( - last_retreat_state.get(), plan->shared_data_->planning_group_, retreat_states, - plan->shared_data_->ik_link_, retreat_direction, retreat_direction_is_global_frame, - plan->retreat_.desired_distance, moveit::core::MaxEEFStep(max_step_), - moveit::core::JumpThreshold(jump_factor_), retreat_valid_callback); - - // if sufficient progress was made in the desired direction, we have a goal state that we can consider for - // future stages - if (d_retreat > plan->retreat_.min_distance && !signal_stop_) - { - // Create approach trajectory - std::reverse(approach_states.begin(), approach_states.end()); - auto approach_traj = std::make_shared( - planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName())); - for (const moveit::core::RobotStatePtr& approach_state : approach_states) - approach_traj->addSuffixWayPoint(approach_state, 0.0); - - // Create retreat trajectory - auto retreat_traj = std::make_shared( - planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName())); - for (const moveit::core::RobotStatePtr& retreat_state : retreat_states) - retreat_traj->addSuffixWayPoint(retreat_state, 0.0); - - // Add timestamps to approach|retreat trajectories - time_param_.computeTimeStamps(*approach_traj); - time_param_.computeTimeStamps(*retreat_traj); - - // Convert approach trajectory to an executable trajectory - plan_execution::ExecutableTrajectory et_approach(approach_traj, "approach"); - et_approach.allowed_collision_matrix_ = collision_matrix_; - plan->trajectories_.push_back(et_approach); - - // Add gripper close trajectory - addGripperTrajectory(plan, collision_matrix_, "grasp"); - - // Convert retreat trajectory to an executable trajectory - plan_execution::ExecutableTrajectory et_retreat(retreat_traj, "retreat"); - et_retreat.allowed_collision_matrix_ = collision_matrix_; - plan->trajectories_.push_back(et_retreat); - - plan->approach_state_ = approach_states.front(); - return true; - } - } - else // No retreat was specified, so package up approach and grip trajectories. - { - // Reset the approach_state_ RobotStatePtr so that we can retry computing the cartesian path - plan->approach_state_.swap(first_approach_state); - - // Create approach trajectory - std::reverse(approach_states.begin(), approach_states.end()); - auto approach_traj = std::make_shared( - planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName())); - for (const moveit::core::RobotStatePtr& approach_state : approach_states) - approach_traj->addSuffixWayPoint(approach_state, 0.0); - - // Add timestamps to approach trajectories - time_param_.computeTimeStamps(*approach_traj); - - // Convert approach trajectory to an executable trajectory - plan_execution::ExecutableTrajectory et_approach(approach_traj, "approach"); - et_approach.allowed_collision_matrix_ = collision_matrix_; - plan->trajectories_.push_back(et_approach); - - // Add gripper close trajectory - addGripperTrajectory(plan, collision_matrix_, "grasp"); - - plan->approach_state_ = approach_states.front(); - - return true; - } - } - } - } while (plan->possible_goal_states_.size() < max_goal_count_ && !signal_stop_ && - samplePossibleGoalStates(plan, planning_scene_->getCurrentState(), min_distance, max_fail_)); - - plan->error_code_.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; - - return false; -} -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp deleted file mode 100644 index 39e9c896e8..0000000000 --- a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp +++ /dev/null @@ -1,230 +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 - -namespace pick_place -{ -ManipulationPipeline::ManipulationPipeline(const std::string& name, unsigned int nthreads) - : name_(name), nthreads_(nthreads), verbose_(false), stop_processing_(true) -{ - processing_threads_.resize(nthreads, nullptr); -} - -ManipulationPipeline::~ManipulationPipeline() -{ - reset(); -} - -ManipulationPipeline& ManipulationPipeline::addStage(const ManipulationStagePtr& next) -{ - next->setVerbose(verbose_); - stages_.push_back(next); - return *this; -} - -const ManipulationStagePtr& ManipulationPipeline::getFirstStage() const -{ - if (stages_.empty()) - { - static const ManipulationStagePtr EMPTY; - return EMPTY; - } - else - return stages_.front(); -} - -const ManipulationStagePtr& ManipulationPipeline::getLastStage() const -{ - if (stages_.empty()) - { - static const ManipulationStagePtr EMPTY; - return EMPTY; - } - else - return stages_.back(); -} - -void ManipulationPipeline::reset() -{ - clear(); - stages_.clear(); -} - -void ManipulationPipeline::setVerbose(bool flag) -{ - verbose_ = flag; - for (pick_place::ManipulationStagePtr& stage : stages_) - stage->setVerbose(flag); -} - -void ManipulationPipeline::clear() -{ - stop(); - { - std::mutex::scoped_lock slock(queue_access_lock_); - queue_.clear(); - } - { - std::mutex::scoped_lock slock(result_lock_); - success_.clear(); - failed_.clear(); - } -} - -void ManipulationPipeline::start() -{ - stop_processing_ = false; - empty_queue_threads_ = 0; - for (pick_place::ManipulationStagePtr& stage : stages_) - stage->resetStopSignal(); - for (std::size_t i = 0; i < processing_threads_.size(); ++i) - if (!processing_threads_[i]) - processing_threads_[i] = new std::thread([this, i] { processingThread(i); }); -} - -void ManipulationPipeline::signalStop() -{ - for (pick_place::ManipulationStagePtr& stage : stages_) - stage->signalStop(); - stop_processing_ = true; - queue_access_cond_.notify_all(); -} - -void ManipulationPipeline::stop() -{ - signalStop(); - for (std::thread*& processing_thread : processing_threads_) - if (processing_thread) - { - processing_thread->join(); - delete processing_thread; - processing_thread = nullptr; - } -} - -void ManipulationPipeline::processingThread(unsigned int index) -{ - ROS_DEBUG_STREAM_NAMED("manipulation", "Start thread " << index << " for '" << name_ << "'"); - - while (!stop_processing_) - { - bool inc_queue = false; - std::unique_lock ulock(queue_access_lock_); - // if the queue is empty, we trigger the corresponding event - if (queue_.empty() && !stop_processing_ && empty_queue_callback_) - { - empty_queue_threads_++; - inc_queue = true; - if (empty_queue_threads_ == processing_threads_.size()) - empty_queue_callback_(); - } - while (queue_.empty() && !stop_processing_) - queue_access_cond_.wait(ulock); - while (!stop_processing_ && !queue_.empty()) - { - ManipulationPlanPtr g = queue_.front(); - queue_.pop_front(); - if (inc_queue) - { - empty_queue_threads_--; - inc_queue = false; - } - - queue_access_lock_.unlock(); - try - { - g->error_code_.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; - for (std::size_t i = 0; !stop_processing_ && i < stages_.size(); ++i) - { - bool res = stages_[i]->evaluate(g); - g->processing_stage_ = i + 1; - if (!res) - { - std::scoped_lock slock(result_lock_); - failed_.push_back(g); - ROS_INFO_STREAM_NAMED("manipulation", "Manipulation plan " << g->id_ << " failed at stage '" - << stages_[i]->getName() << "' on thread " - << index); - break; - } - } - if (g->error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) - { - g->processing_stage_++; - { - std::scoped_lock slock(result_lock_); - success_.push_back(g); - } - signalStop(); - ROS_INFO_STREAM_NAMED("manipulation", "Found successful manipulation plan!"); - if (solution_callback_) - solution_callback_(); - } - } - catch (std::exception& ex) - { - ROS_ERROR_NAMED("manipulation", "[%s:%u] %s", name_.c_str(), index, ex.what()); - } - queue_access_lock_.lock(); - } - } -} - -void ManipulationPipeline::push(const ManipulationPlanPtr& plan) -{ - std::scoped_lock slock(queue_access_lock_); - queue_.push_back(plan); - ROS_INFO_STREAM_NAMED("manipulation", - "Added plan for pipeline '" << name_ << "'. Queue is now of size " << queue_.size()); - queue_access_cond_.notify_all(); -} - -void ManipulationPipeline::reprocessLastFailure() -{ - std::scoped_lock slock(queue_access_lock_); - if (failed_.empty()) - return; - ManipulationPlanPtr plan = failed_.back(); - failed_.pop_back(); - plan->clear(); - queue_.push_back(plan); - ROS_INFO_STREAM_NAMED("manipulation", "Re-added last failed plan for pipeline '" - << name_ << "'. Queue is now of size " << queue_.size()); - queue_access_cond_.notify_all(); -} -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/src/pick.cpp b/moveit_ros/manipulation/pick_place/src/pick.cpp deleted file mode 100644 index ec1067dc31..0000000000 --- a/moveit_ros/manipulation/pick_place/src/pick.cpp +++ /dev/null @@ -1,258 +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 - -namespace pick_place -{ -PickPlan::PickPlan(const PickPlaceConstPtr& pick_place) : PickPlacePlanBase(pick_place, "pick") -{ -} - -namespace -{ -struct OrderGraspQuality -{ - OrderGraspQuality(const std::vector& grasps) : grasps_(grasps) - { - } - - bool operator()(const std::size_t a, const std::size_t b) const - { - return grasps_[a].grasp_quality > grasps_[b].grasp_quality; - } - - const std::vector& grasps_; -}; -} // namespace - -bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::action::PickupGoal& goal) -{ - double timeout = goal.allowed_planning_time; - ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout); - - std::string planning_group = goal.group_name; - std::string end_effector = goal.end_effector; - if (end_effector.empty() && !planning_group.empty()) - { - const moveit::core::JointModelGroup* jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group); - if (!jmg) - { - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; - return false; - } - const std::vector& eefs = jmg->getAttachedEndEffectorNames(); - if (!eefs.empty()) - { - end_effector = eefs.front(); - if (eefs.size() > 1) - ROS_WARN_STREAM_NAMED("manipulation", "Choice of end-effector for group '" - << planning_group << "' is ambiguous. Assuming '" << end_effector - << "'"); - } - } - else if (!end_effector.empty() && planning_group.empty()) - { - const moveit::core::JointModelGroup* jmg = planning_scene->getRobotModel()->getEndEffector(end_effector); - if (!jmg) - { - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; - return false; - } - planning_group = jmg->getEndEffectorParentGroup().first; - if (planning_group.empty()) - { - ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" - << end_effector << "'. Please define a parent group in the SRDF."); - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; - return false; - } - else - ROS_INFO_STREAM_NAMED("manipulation", "Assuming the planning group for end effector '" << end_effector << "' is '" - << planning_group << "'"); - } - const moveit::core::JointModelGroup* eef = - end_effector.empty() ? nullptr : planning_scene->getRobotModel()->getEndEffector(end_effector); - if (!eef) - { - ROS_ERROR_NAMED("manipulation", "No end-effector specified for pick action"); - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; - return false; - } - const std::string& ik_link = eef->getEndEffectorParentGroup().second; - - ros::WallTime start_time = ros::WallTime::now(); - - // construct common data for possible manipulation plans - auto plan_data = std::make_shared(); - ManipulationPlanSharedDataConstPtr const_plan_data = plan_data; - plan_data->planning_group_ = planning_scene->getRobotModel()->getJointModelGroup(planning_group); - plan_data->end_effector_group_ = eef; - plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(ik_link); - plan_data->timeout_ = endtime; - plan_data->path_constraints_ = goal.path_constraints; - plan_data->planner_id_ = goal.planner_id; - plan_data->minimize_object_distance_ = goal.minimize_object_distance; - plan_data->max_goal_sampling_attempts_ = 2; - moveit_msgs::msg::AttachedCollisionObject& attach_object_msg = plan_data->diff_attached_object_; - - // construct the attached object message that will change the world to what it would become after a pick - attach_object_msg.link_name = ik_link; - attach_object_msg.object.id = goal.target_name; - attach_object_msg.object.operation = moveit_msgs::msg::CollisionObject::ADD; - attach_object_msg.touch_links = - goal.attached_object_touch_links.empty() ? eef->getLinkModelNames() : goal.attached_object_touch_links; - collision_detection::AllowedCollisionMatrixPtr approach_grasp_acm( - new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix())); - - // we are allowed to touch the target object - approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links, true); - // we are allowed to touch certain other objects with the gripper - approach_grasp_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true); - if (!goal.support_surface_name.empty()) - { - // we are allowed to have contact between the target object and the support surface before the grasp - approach_grasp_acm->setEntry(goal.support_surface_name, goal.target_name, true); - - // optionally, it may be allowed to touch the support surface with the gripper - if (goal.allow_gripper_support_collision) - approach_grasp_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true); - } - - // configure the manipulation pipeline - pipeline_.reset(); - ManipulationStagePtr stage1( - new ReachableAndValidPoseFilter(planning_scene, approach_grasp_acm, pick_place_->getConstraintsSamplerManager())); - ManipulationStagePtr stage2 = std::make_shared(planning_scene, approach_grasp_acm); - ManipulationStagePtr stage3 = std::make_shared(planning_scene, pick_place_->getPlanningPipeline()); - pipeline_.addStage(stage1).addStage(stage2).addStage(stage3); - - initialize(); - pipeline_.start(); - - // order the grasps by quality - std::vector grasp_order(goal.possible_grasps.size()); - for (std::size_t i = 0; i < goal.possible_grasps.size(); ++i) - grasp_order[i] = i; - OrderGraspQuality oq(goal.possible_grasps); - // using stable_sort to preserve order of grasps with equal quality - std::stable_sort(grasp_order.begin(), grasp_order.end(), oq); - - // feed the available grasps to the stages we set up - for (std::size_t i = 0; i < goal.possible_grasps.size(); ++i) - { - auto p = std::make_shared(const_plan_data); - const moveit_msgs::msg::Grasp& g = goal.possible_grasps[grasp_order[i]]; - p->approach_ = g.pre_grasp_approach; - p->retreat_ = g.post_grasp_retreat; - p->goal_pose_ = g.grasp_pose; - p->id_ = grasp_order[i]; - // if no frame of reference was specified, assume the transform to be in the reference frame of the object - if (p->goal_pose_.header.frame_id.empty()) - p->goal_pose_.header.frame_id = goal.target_name; - p->approach_posture_ = g.pre_grasp_posture; - p->retreat_posture_ = g.grasp_posture; - pipeline_.push(p); - } - - // wait till we're done - waitForPipeline(endtime); - pipeline_.stop(); - - last_plan_time_ = (ros::WallTime::now() - start_time).toSec(); - - if (!getSuccessfulManipulationPlans().empty()) - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - else - { - if (last_plan_time_ > timeout) - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT; - else - { - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; - if (!goal.possible_grasps.empty()) - { - ROS_WARN_NAMED("manipulation", "All supplied grasps failed. Retrying last grasp in verbose mode."); - // everything failed. we now start the pipeline again in verbose mode for one grasp - initialize(); - pipeline_.setVerbose(true); - pipeline_.start(); - pipeline_.reprocessLastFailure(); - waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0)); - pipeline_.stop(); - pipeline_.setVerbose(false); - } - } - } - ROS_INFO_NAMED("manipulation", "Pickup planning completed after %lf seconds", last_plan_time_); - - return error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; -} - -PickPlanPtr PickPlace::planPick(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::action::PickupGoal& goal) const -{ - PickPlanPtr p(new PickPlan(shared_from_this())); - - if (moveit::core::isEmpty(goal.planning_options.planning_scene_diff)) - p->plan(planning_scene, goal); - else - p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal); - - if (display_computed_motion_plans_) - { - const std::vector& success = p->getSuccessfulManipulationPlans(); - if (!success.empty()) - visualizePlan(success.back()); - } - - if (display_grasps_) - { - const std::vector& success = p->getSuccessfulManipulationPlans(); - visualizeGrasps(success); - const std::vector& failed = p->getFailedManipulationPlans(); - visualizeGrasps(failed); - } - - return p; -} -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/src/pick_place.cpp b/moveit_ros/manipulation/pick_place/src/pick_place.cpp deleted file mode 100644 index a633749f63..0000000000 --- a/moveit_ros/manipulation/pick_place/src/pick_place.cpp +++ /dev/null @@ -1,201 +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 - -namespace pick_place -{ -const std::string PickPlace::DISPLAY_PATH_TOPIC = planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC; -const std::string PickPlace::DISPLAY_GRASP_TOPIC = "display_grasp_markers"; -const double PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION = 7.0; // seconds - -// functionality specific to pick-only is in pick.cpp; -// functionality specific to place-only is in place.cpp; - -PickPlacePlanBase::PickPlacePlanBase(const PickPlaceConstPtr& pick_place, const std::string& name) - : pick_place_(pick_place), pipeline_(name, 4), last_plan_time_(0.0), done_(false) -{ - pipeline_.setSolutionCallback([this] { foundSolution(); }); - pipeline_.setEmptyQueueCallback([this] { emptyQueue(); }); -} - -PickPlacePlanBase::~PickPlacePlanBase() = default; - -void PickPlacePlanBase::foundSolution() -{ - std::scoped_lock slock(done_mutex_); - done_ = true; - done_condition_.notify_all(); -} - -void PickPlacePlanBase::emptyQueue() -{ - std::scoped_lock slock(done_mutex_); - if (pushed_all_poses_) - { - done_ = true; - done_condition_.notify_all(); - } -} - -void PickPlacePlanBase::initialize() -{ - done_ = false; - pushed_all_poses_ = false; -} - -void PickPlacePlanBase::waitForPipeline(const ros::WallTime& endtime) -{ - // wait till we're done - std::unique_lock lock(done_mutex_); - pushed_all_poses_ = true; - while (!done_ && endtime > ros::WallTime::now()) - done_condition_.timed_wait(lock, (endtime - ros::WallTime::now()).toBoost()); -} - -PickPlace::PickPlace(const planning_pipeline::PlanningPipelinePtr& planning_pipeline) - : nh_("~"), planning_pipeline_(planning_pipeline), display_computed_motion_plans_(false), display_grasps_(false) -{ - constraint_sampler_manager_loader_ = - std::make_shared(); -} - -void PickPlace::displayProcessedGrasps(bool flag) -{ - if (display_grasps_ && !flag) - grasps_publisher_.shutdown(); - else if (!display_grasps_ && flag) - grasps_publisher_ = nh_.advertise(DISPLAY_GRASP_TOPIC, 10, true); - display_grasps_ = flag; -} - -void PickPlace::displayComputedMotionPlans(bool flag) -{ - if (display_computed_motion_plans_ && !flag) - display_path_publisher_.shutdown(); - else if (!display_computed_motion_plans_ && flag) - display_path_publisher_ = nh_.advertise(DISPLAY_PATH_TOPIC, 10, true); - display_computed_motion_plans_ = flag; -} - -void PickPlace::visualizePlan(const ManipulationPlanPtr& plan) const -{ - moveit_msgs::msg::DisplayTrajectory dtraj; - dtraj.model_id = getRobotModel()->getName(); - bool first = true; - for (const plan_execution::ExecutableTrajectory& traj : plan->trajectories_) - { - if (!traj.trajectory_ || traj.trajectory_->empty()) - continue; - if (first) - { - moveit::core::robotStateToRobotStateMsg(traj.trajectory_->getFirstWayPoint(), dtraj.trajectory_start); - first = false; - } - dtraj.trajectory.resize(dtraj.trajectory.size() + 1); - traj.trajectory_->getRobotTrajectoryMsg(dtraj.trajectory.back()); - } - display_path_publisher_.publish(dtraj); -} - -void PickPlace::visualizeGrasp(const ManipulationPlanPtr& plan) const -{ - visualizeGrasps(std::vector(1, plan)); -} - -namespace -{ -std::vector setupDefaultGraspColors() -{ - std::vector result; - result.resize(6); - result[0].r = 0.5f; - result[0].g = 0.5f; - result[0].b = 0.5f; - result[0].a = 1.0f; - result[1].r = 1.0f; - result[1].g = 0.0f; - result[1].b = 0.0f; - result[1].a = 1.0f; - result[2].r = 1.0f; - result[2].g = 0.5f; - result[2].b = 0.0f; - result[2].a = 1.0f; - result[3].r = 0.0f; - result[3].g = 1.0f; - result[3].b = 1.0f; - result[3].a = 1.0f; - result[4].r = 0.0f; - result[4].g = 1.0f; - result[4].b = 0.0f; - result[4].a = 1.0f; - result[5].r = 1.0f; - result[5].g = 0.0f; - result[5].b = 1.0f; - result[5].a = 0.75f; - return result; -} -} // namespace - -void PickPlace::visualizeGrasps(const std::vector& plans) const -{ - if (plans.empty()) - return; - - moveit::core::RobotState state(getRobotModel()); - state.setToDefaultValues(); - - static std::vector colors(setupDefaultGraspColors()); - visualization_msgs::MarkerArray ma; - for (const ManipulationPlanPtr& plan : plans) - { - const moveit::core::JointModelGroup* jmg = plan->shared_data_->end_effector_group_; - if (jmg) - { - unsigned int type = std::min(plan->processing_stage_, colors.size() - 1); - state.updateStateWithLinkAt(plan->shared_data_->ik_link_, plan->transformed_goal_pose_); - state.getRobotMarkers(ma, jmg->getLinkModelNames(), colors[type], - "moveit_grasps:stage_" + boost::lexical_cast(plan->processing_stage_), - ros::Duration(60)); - } - } - - grasps_publisher_.publish(ma); -} -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp deleted file mode 100644 index fcbfa82ff0..0000000000 --- a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp +++ /dev/null @@ -1,85 +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 - -namespace pick_place -{ -namespace -{ -using namespace moveit_ros_manipulation; - -class DynamicReconfigureImpl -{ -public: - DynamicReconfigureImpl() : dynamic_reconfigure_server_(ros::NodeHandle("~/pick_place")) - { - dynamic_reconfigure_server_.setCallback( - [this](auto& config, uint32_t level) { dynamicReconfigureCallback(config, level); }); - } - - const PickPlaceParams& getParams() const - { - return params_; - } - -private: - PickPlaceParams params_; - - void dynamicReconfigureCallback(PickPlaceDynamicReconfigureConfig& config, uint32_t /*level*/) - { - params_.max_goal_count_ = config.max_attempted_states_per_pose; - params_.max_fail_ = config.max_consecutive_fail_attempts; - params_.max_step_ = config.cartesian_motion_step_size; - params_.jump_factor_ = config.jump_factor; - } - - dynamic_reconfigure::Server dynamic_reconfigure_server_; -}; -} // namespace -} // namespace pick_place - -pick_place::PickPlaceParams::PickPlaceParams() : max_goal_count_(5), max_fail_(3), max_step_(0.02), jump_factor_(2.0) -{ -} - -const pick_place::PickPlaceParams& pick_place::GetGlobalPickPlaceParams() -{ - static DynamicReconfigureImpl pick_place_params; - return pick_place_params.getParams(); -} diff --git a/moveit_ros/manipulation/pick_place/src/place.cpp b/moveit_ros/manipulation/pick_place/src/place.cpp deleted file mode 100644 index a383cb1ed5..0000000000 --- a/moveit_ros/manipulation/pick_place/src/place.cpp +++ /dev/null @@ -1,423 +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 -#if __has_include() -#include -#else -#include -#endif -#include - -namespace pick_place -{ -PlacePlan::PlacePlan(const PickPlaceConstPtr& pick_place) : PickPlacePlanBase(pick_place, "place") -{ -} - -namespace -{ -struct OrderPlaceLocationQuality -{ - OrderPlaceLocationQuality(const std::vector& places) : places_(places) - { - } - - bool operator()(const std::size_t a, const std::size_t b) const - { - return places_[a].quality > places_[b].quality; - } - - const std::vector& places_; -}; - -bool transformToEndEffectorGoal(const geometry_msgs::PoseStamped& goal_pose, - const moveit::core::AttachedBody* attached_body, geometry_msgs::PoseStamped& place_pose) -{ - const EigenSTL::vector_Isometry3d& fixed_transforms = attached_body->getShapePosesInLinkFrame(); - if (fixed_transforms.empty()) - return false; - - Eigen::Isometry3d end_effector_transform; - tf2::fromMsg(goal_pose.pose, end_effector_transform); - end_effector_transform = end_effector_transform * fixed_transforms[0].inverse(); - place_pose.header = goal_pose.header; - place_pose.pose = tf2::toMsg(end_effector_transform); - return true; -} -} // namespace - -bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::action::PlaceGoal& goal) -{ - double timeout = goal.allowed_planning_time; - ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout); - std::string attached_object_name = goal.attached_object_name; - const moveit::core::JointModelGroup* jmg = nullptr; - const moveit::core::JointModelGroup* eef = nullptr; - - // if the group specified is actually an end-effector, we use it as such - if (planning_scene->getRobotModel()->hasEndEffector(goal.group_name)) - { - eef = planning_scene->getRobotModel()->getEndEffector(goal.group_name); - if (eef) - { // if we correctly found the eef, then we try to find out what the planning group is - const std::string& eef_parent = eef->getEndEffectorParentGroup().first; - if (eef_parent.empty()) - { - ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" - << goal.group_name - << "'. Please define a parent group in the SRDF."); - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; - return false; - } - else - jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent); - } - } - else - { - // if a group name was specified, try to use it - jmg = goal.group_name.empty() ? nullptr : planning_scene->getRobotModel()->getJointModelGroup(goal.group_name); - if (jmg) - { - // we also try to find the corresponding eef - const std::vector& eef_names = jmg->getAttachedEndEffectorNames(); - if (eef_names.empty()) - { - ROS_ERROR_STREAM_NAMED("manipulation", - "There are no end-effectors specified for group '" << goal.group_name << "'"); - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; - return false; - } - else - // check to see if there is an end effector that has attached objects associaded, so we can complete the place - for (const std::string& eef_name : eef_names) - { - std::vector attached_bodies; - const moveit::core::JointModelGroup* eg = planning_scene->getRobotModel()->getEndEffector(eef_name); - if (eg) - { - // see if there are objects attached to links in the eef - planning_scene->getCurrentState().getAttachedBodies(attached_bodies, eg); - - // is is often possible that the objects are attached to the same link that the eef itself is attached, - // so we check for attached bodies there as well - const moveit::core::LinkModel* attached_link_model = - planning_scene->getRobotModel()->getLinkModel(eg->getEndEffectorParentGroup().second); - if (attached_link_model) - { - std::vector attached_bodies2; - planning_scene->getCurrentState().getAttachedBodies(attached_bodies2, attached_link_model); - attached_bodies.insert(attached_bodies.end(), attached_bodies2.begin(), attached_bodies2.end()); - } - } - - // if this end effector has attached objects, we go on - if (!attached_bodies.empty()) - { - // if the user specified the name of the attached object to place, we check that indeed - // the group contains this attached body - if (!attached_object_name.empty()) - { - bool found = false; - for (const moveit::core::AttachedBody* attached_body : attached_bodies) - if (attached_body->getName() == attached_object_name) - { - found = true; - break; - } - // if the attached body this group has is not the same as the one specified, - // we cannot use this eef - if (!found) - continue; - } - - // if we previoulsy have set the eef it means we have more options we could use, so things are ambiguous - if (eef) - { - ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors for group '" - << goal.group_name - << "' that are currently holding objects. It is ambiguous " - "which end-effector to use. Please specify it explicitly."); - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; - return false; - } - // set the end effector (this was initialized to nullptr above) - eef = planning_scene->getRobotModel()->getEndEffector(eef_name); - } - } - } - } - - // if we know the attached object, but not the eef, we can try to identify that - if (!attached_object_name.empty() && !eef) - { - const moveit::core::AttachedBody* attached_body = - planning_scene->getCurrentState().getAttachedBody(attached_object_name); - if (attached_body) - { - // get the robot model link this attached body is associated to - const moveit::core::LinkModel* link = attached_body->getAttachedLink(); - // check to see if there is a unique end effector containing the link - const std::vector& eefs = - planning_scene->getRobotModel()->getEndEffectors(); - for (const moveit::core::JointModelGroup* end_effector : eefs) - if (end_effector->hasLinkModel(link->getName())) - { - if (eef) - { - ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors that include the link '" - << link->getName() << "' which is where the body '" - << attached_object_name - << "' is attached. It is unclear which end-effector to use."); - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; - return false; - } - eef = end_effector; - } - } - // if the group is also unknown, but we just found out the eef - if (!jmg && eef) - { - const std::string& eef_parent = eef->getEndEffectorParentGroup().first; - if (eef_parent.empty()) - { - ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" - << goal.group_name - << "'. Please define a parent group in the SRDF."); - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; - return false; - } - else - jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent); - } - } - - if (!jmg || !eef) - { - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; - return false; - } - - // try to infer attached body name if possible - int loop_count = 0; - while (attached_object_name.empty() && loop_count < 2) - { - // in the first try, look for objects attached to the eef, if the eef is known; - // otherwise, look for attached bodies in the planning group itself - std::vector attached_bodies; - planning_scene->getCurrentState().getAttachedBodies(attached_bodies, loop_count == 0 ? eef : jmg); - - loop_count++; - if (attached_bodies.size() > 1) - { - ROS_ERROR_NAMED("manipulation", - "Multiple attached bodies for group '%s' but no explicit attached object to place was specified", - goal.group_name.c_str()); - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_OBJECT_NAME; - return false; - } - else - attached_object_name = attached_bodies[0]->getName(); - } - - const moveit::core::AttachedBody* attached_body = - planning_scene->getCurrentState().getAttachedBody(attached_object_name); - if (!attached_body) - { - ROS_ERROR_NAMED("manipulation", "There is no object to detach for place action"); - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_OBJECT_NAME; - return false; - } - - ros::WallTime start_time = ros::WallTime::now(); - - // construct common data for possible manipulation plans - auto plan_data = std::make_shared(); - ManipulationPlanSharedDataConstPtr const_plan_data = plan_data; - plan_data->planning_group_ = jmg; - plan_data->end_effector_group_ = eef; - plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(eef->getEndEffectorParentGroup().second); - - plan_data->timeout_ = endtime; - plan_data->path_constraints_ = goal.path_constraints; - plan_data->planner_id_ = goal.planner_id; - plan_data->minimize_object_distance_ = false; - plan_data->max_goal_sampling_attempts_ = 2; - moveit_msgs::msg::AttachedCollisionObject& detach_object_msg = plan_data->diff_attached_object_; - - // construct the attached object message that will change the world to what it would become after a placement - detach_object_msg.link_name = attached_body->getAttachedLinkName(); - detach_object_msg.object.id = attached_object_name; - detach_object_msg.object.operation = moveit_msgs::msg::CollisionObject::REMOVE; - - collision_detection::AllowedCollisionMatrixPtr approach_place_acm( - new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix())); - - // we are allowed to touch certain other objects with the gripper - approach_place_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true); - - // we are allowed to touch the target object slightly while retreating the end effector - std::vector touch_links(attached_body->getTouchLinks().begin(), attached_body->getTouchLinks().end()); - approach_place_acm->setEntry(attached_object_name, touch_links, true); - - if (!goal.support_surface_name.empty()) - { - // we are allowed to have contact between the target object and the support surface before the place - approach_place_acm->setEntry(goal.support_surface_name, attached_object_name, true); - - // optionally, it may be allowed to touch the support surface with the gripper - if (goal.allow_gripper_support_collision) - approach_place_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true); - } - - // configure the manipulation pipeline - pipeline_.reset(); - - ManipulationStagePtr stage1( - new ReachableAndValidPoseFilter(planning_scene, approach_place_acm, pick_place_->getConstraintsSamplerManager())); - ManipulationStagePtr stage2 = std::make_shared(planning_scene, approach_place_acm); - ManipulationStagePtr stage3 = std::make_shared(planning_scene, pick_place_->getPlanningPipeline()); - pipeline_.addStage(stage1).addStage(stage2).addStage(stage3); - - initialize(); - - pipeline_.start(); - - // order the place locations by quality - std::vector place_locations_order(goal.place_locations.size()); - for (std::size_t i = 0; i < goal.place_locations.size(); ++i) - place_locations_order[i] = i; - OrderPlaceLocationQuality oq(goal.place_locations); - // using stable_sort to preserve order of place locations with equal quality - std::stable_sort(place_locations_order.begin(), place_locations_order.end(), oq); - - // add possible place locations - for (std::size_t i = 0; i < goal.place_locations.size(); ++i) - { - auto p = std::make_shared(const_plan_data); - const moveit_msgs::action::PlaceLocation& pl = goal.place_locations[place_locations_order[i]]; - - if (goal.place_eef) - p->goal_pose_ = pl.place_pose; - else - // The goals are specified for the attached body - // but we want to transform them into goals for the end-effector instead - if (!transformToEndEffectorGoal(pl.place_pose, attached_body, p->goal_pose_)) - { - p->goal_pose_ = pl.place_pose; - ROS_ERROR_NAMED("manipulation", "Unable to transform the desired pose of the object to the pose of the " - "end-effector"); - } - - p->approach_ = pl.pre_place_approach; - p->retreat_ = pl.post_place_retreat; - p->retreat_posture_ = pl.post_place_posture; - p->id_ = place_locations_order[i]; - if (p->retreat_posture_.joint_names.empty()) - p->retreat_posture_ = attached_body->getDetachPosture(); - pipeline_.push(p); - } - ROS_INFO_NAMED("manipulation", "Added %d place locations", static_cast(goal.place_locations.size())); - - // wait till we're done - waitForPipeline(endtime); - - pipeline_.stop(); - - last_plan_time_ = (ros::WallTime::now() - start_time).toSec(); - - if (!getSuccessfulManipulationPlans().empty()) - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - else - { - if (last_plan_time_ > timeout) - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT; - else - { - error_code_.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; - if (!goal.place_locations.empty()) - { - ROS_WARN_NAMED("manipulation", "All supplied place locations failed. Retrying last location in verbose mode."); - // everything failed. we now start the pipeline again in verbose mode for one grasp - initialize(); - pipeline_.setVerbose(true); - pipeline_.start(); - pipeline_.reprocessLastFailure(); - waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0)); - pipeline_.stop(); - pipeline_.setVerbose(false); - } - } - } - ROS_INFO_NAMED("manipulation", "Place planning completed after %lf seconds", last_plan_time_); - - return error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; -} - -PlacePlanPtr PickPlace::planPlace(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::action::PlaceGoal& goal) const -{ - PlacePlanPtr p(new PlacePlan(shared_from_this())); - if (moveit::core::isEmpty(goal.planning_options.planning_scene_diff)) - p->plan(planning_scene, goal); - else - p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal); - - if (display_computed_motion_plans_) - { - const std::vector& success = p->getSuccessfulManipulationPlans(); - if (!success.empty()) - visualizePlan(success.back()); - } - - if (display_grasps_) - { - const std::vector& success = p->getSuccessfulManipulationPlans(); - visualizeGrasps(success); - const std::vector& failed = p->getFailedManipulationPlans(); - visualizeGrasps(failed); - } - - return p; -} -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp b/moveit_ros/manipulation/pick_place/src/plan_stage.cpp deleted file mode 100644 index 27c639c436..0000000000 --- a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp +++ /dev/null @@ -1,124 +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 - -namespace pick_place -{ -PlanStage::PlanStage(const planning_scene::PlanningSceneConstPtr& scene, - const planning_pipeline::PlanningPipelinePtr& planning_pipeline) - : ManipulationStage("plan"), planning_scene_(scene), planning_pipeline_(planning_pipeline) -{ -} - -void PlanStage::signalStop() -{ - ManipulationStage::signalStop(); - planning_pipeline_->terminate(); -} - -// Plan the arm movement to the approach location -bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const -{ - planning_interface::MotionPlanRequest req; - planning_interface::MotionPlanResponse res; - req.group_name = plan->shared_data_->planning_group_->getName(); - req.num_planning_attempts = 1; - req.allowed_planning_time = (plan->shared_data_->timeout_ - ros::WallTime::now()).toSec(); - req.path_constraints = plan->shared_data_->path_constraints_; - req.planner_id = plan->shared_data_->planner_id_; - req.start_state.is_diff = true; - - req.goal_constraints.resize(1, kinematic_constraints::constructGoalConstraints(*plan->approach_state_, - plan->shared_data_->planning_group_)); - unsigned int attempts = 0; - do // give the planner two chances - { - attempts++; - if (!signal_stop_ && planning_pipeline_->generatePlan(planning_scene_, req, res) && - res.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS && res.trajectory_ && - !res.trajectory_->empty()) - { - // We have a valid motion plan, now apply pre-approach end effector posture (open gripper) if applicable - if (!plan->approach_posture_.joint_names.empty()) - { - auto pre_approach_state = std::make_shared(res.trajectory_->getLastWayPoint())); - auto pre_approach_traj = std::make_shared( - pre_approach_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName())); - pre_approach_traj->setRobotTrajectoryMsg(*pre_approach_state, plan->approach_posture_); - - // Apply the open gripper state to the waypoint - // If user has defined a time for it's gripper movement time, don't add the - // DEFAULT_GRASP_POSTURE_COMPLETION_DURATION - if (!plan->approach_posture_.points.empty() && - plan->approach_posture_.points.back().time_from_start > ros::Duration(0.0)) - { - pre_approach_traj->addPrefixWayPoint(pre_approach_state, 0.0); - } - else - { // Do what was done before - ROS_INFO_STREAM("Adding default duration of " << PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION - << " seconds to the grasp closure time. Assign time_from_start " - << "to your trajectory to avoid this."); - pre_approach_traj->addPrefixWayPoint(pre_approach_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION); - } - - // Add the open gripper trajectory to the plan - plan_execution::ExecutableTrajectory et(pre_approach_traj, "pre_grasp"); - plan->trajectories_.insert(plan->trajectories_.begin(), et); - } - - // Add the pre-approach trajectory to the plan - plan_execution::ExecutableTrajectory et(res.trajectory_, name_); - plan->trajectories_.insert(plan->trajectories_.begin(), et); - - plan->error_code_ = res.error_code_; - - return true; - } - else - plan->error_code_ = res.error_code_; - } - // if the planner reported an invalid plan, give it a second chance - while (!signal_stop_ && plan->error_code_.val == moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN && - attempts < 2); - - return false; -} -} // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp deleted file mode 100644 index 54d74c7410..0000000000 --- a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp +++ /dev/null @@ -1,159 +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 -#if __has_include() -#include -#else -#include -#endif -#include -#include - -pick_place::ReachableAndValidPoseFilter::ReachableAndValidPoseFilter( - const planning_scene::PlanningSceneConstPtr& scene, - const collision_detection::AllowedCollisionMatrixConstPtr& collision_matrix, - const constraint_samplers::ConstraintSamplerManagerPtr& constraints_sampler_manager) - : ManipulationStage("reachable & valid pose filter") - , planning_scene_(scene) - , collision_matrix_(collision_matrix) - , constraints_sampler_manager_(constraints_sampler_manager) -{ -} - -namespace -{ -bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene, - const collision_detection::AllowedCollisionMatrix* collision_matrix, bool verbose, - const pick_place::ManipulationPlan* manipulation_plan, moveit::core::RobotState* state, - const moveit::core::JointModelGroup* group, const double* joint_group_variable_values) -{ - collision_detection::CollisionRequest req; - req.verbose = verbose; - req.group_name = manipulation_plan->shared_data_->planning_group_->getName(); - - state->setJointGroupPositions(group, joint_group_variable_values); - - if (manipulation_plan->approach_posture_.points.empty()) - { - state->update(); - collision_detection::CollisionResult res; - planning_scene->checkCollision(req, res, *state, *collision_matrix); - if (res.collision) - return false; - } - else - // apply approach posture for the end effector (we always apply it here since it could be the case the sampler - // changes this posture) - for (std::size_t i = 0; i < manipulation_plan->approach_posture_.points.size(); ++i) - { - state->setVariablePositions(manipulation_plan->approach_posture_.joint_names, - manipulation_plan->approach_posture_.points[i].positions); - state->update(); - collision_detection::CollisionResult res; - planning_scene->checkCollision(req, res, *state, *collision_matrix); - if (res.collision) - return false; - } - return planning_scene->isStateFeasible(*state); -} -} // namespace - -bool pick_place::ReachableAndValidPoseFilter::isEndEffectorFree(const ManipulationPlanPtr& plan, - moveit::core::RobotState& token_state) const -{ - tf2::fromMsg(plan->goal_pose_.pose, plan->transformed_goal_pose_); - plan->transformed_goal_pose_ = - planning_scene_->getFrameTransform(token_state, plan->goal_pose_.header.frame_id) * plan->transformed_goal_pose_; - token_state.updateStateWithLinkAt(plan->shared_data_->ik_link_, plan->transformed_goal_pose_); - collision_detection::CollisionRequest req; - req.verbose = verbose_; - collision_detection::CollisionResult res; - req.group_name = plan->shared_data_->end_effector_group_->getName(); - planning_scene_->checkCollision(req, res, token_state, *collision_matrix_); - return !res.collision; -} - -bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr& plan) const -{ - // initialize with scene state - auto token_state = std::make_shared(planning_scene_->getCurrentState()); - if (isEndEffectorFree(plan, *token_state)) - { - // update the goal pose message if anything has changed; this is because the name of the frame in the input goal - // pose - // can be that of objects in the collision world but most components are unaware of those transforms, - // so we convert to a frame that is certainly known - if (!moveit::core::Transforms::sameFrame(planning_scene_->getPlanningFrame(), plan->goal_pose_.header.frame_id)) - { - plan->goal_pose_.pose = tf2::toMsg(plan->transformed_goal_pose_); - plan->goal_pose_.header.frame_id = planning_scene_->getPlanningFrame(); - } - - // convert the pose we want to reach to a set of constraints - plan->goal_constraints_ = - kinematic_constraints::constructGoalConstraints(plan->shared_data_->ik_link_->getName(), plan->goal_pose_); - - const std::string& planning_group = plan->shared_data_->planning_group_->getName(); - - // construct a sampler for the specified constraints; this can end up calling just IK, but it is more general - // and allows for robot-specific samplers, producing samples that also change the base position if needed, etc - plan->goal_sampler_ = - constraints_sampler_manager_->selectSampler(planning_scene_, planning_group, plan->goal_constraints_); - if (plan->goal_sampler_) - { - plan->goal_sampler_->setGroupStateValidityCallback( - [scene = planning_scene_.get(), acm = collision_matrix_.get(), verbose = verbose_, - p = plan.get()](moveit::core::RobotState* robot_state, const moveit::core::JointModelGroup* joint_group, - const double* joint_group_variable_values) { - return isStateCollisionFree(scene, acm, verbose, p, robot_state, joint_group, joint_group_variable_values); - }); - plan->goal_sampler_->setVerbose(verbose_); - if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_)) - { - plan->possible_goal_states_.push_back(token_state); - return true; - } - else if (verbose_) - ROS_INFO_NAMED("manipulation", "Sampler failed to produce a state"); - } - else - ROS_ERROR_THROTTLE_NAMED(1, "manipulation", "No sampler was constructed"); - } - plan->error_code_.val = moveit_msgs::msg::MoveItErrorCodes::GOAL_IN_COLLISION; - return false; -} diff --git a/moveit_ros/manipulation/pick_place_capability_plugin_description.xml b/moveit_ros/manipulation/pick_place_capability_plugin_description.xml deleted file mode 100644 index 7f1fc8eb60..0000000000 --- a/moveit_ros/manipulation/pick_place_capability_plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - Provide pick and place capability to MoveGroup - - - - diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index c44bd7e47f..79506f4a77 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -59,8 +59,6 @@ static const char* DEFAULT_CAPABILITIES[] = { "move_group/MoveGroupKinematicsService", "move_group/MoveGroupExecuteTrajectoryAction", "move_group/MoveGroupMoveAction", - // TODO (ddengster) : wait for port for moveit_ros_manipulation package - //"move_group/MoveGroupPickPlaceAction", "move_group/MoveGroupPlanService", "move_group/MoveGroupQueryPlannersService", "move_group/MoveGroupStateValidationService", diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index 81b58f829e..89352462b6 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -31,9 +31,6 @@ moveit_ros_robot_interaction moveit_ros_planning_interface moveit_ros_visualization - moveit_ros_move_group ament_lint_auto diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index e2495ff5d8..413e131c03 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -10,7 +10,6 @@ find_package(moveit_msgs REQUIRED) find_package(moveit_core REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(moveit_ros_warehouse REQUIRED) -# find_package(moveit_ros_manipulation REQUIRED) find_package(moveit_ros_move_group REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) @@ -51,7 +50,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_core moveit_ros_planning moveit_ros_warehouse -# moveit_ros_manipulation moveit_ros_move_group tf2 tf2_eigen diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index f8b17dc597..2a93fe0c71 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -62,10 +62,6 @@ #include #include #include -// TODO(JafarAbdi): Enable once moveit_ros_manipulation is ported -// #include -// #include -// #include #include #include @@ -163,14 +159,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl move_action_client_ = rclcpp_action::create_client( pnode_, rclcpp::names::append(opt_.move_group_namespace_, move_group::MOVE_ACTION)); move_action_client_->wait_for_action_server(wait_for_servers.to_chrono>()); - // TODO(JafarAbdi): Enable once moveit_ros_manipulation is ported - // pick_action_client_ = rclcpp_action::create_client( - // node_, move_group::PICKUP_ACTION); - // pick_action_client_->wait_for_action_server(std::chrono::nanoseconds(timeout_for_servers.nanoseconds())); - // - // place_action_client_ = rclcpp_action::create_client( - // node_, move_group::PLACE_ACTION); - // place_action_client_->wait_for_action_server(std::chrono::nanoseconds(timeout_for_servers.nanoseconds())); execute_action_client_ = rclcpp_action::create_client( pnode_, rclcpp::names::append(opt_.move_group_namespace_, move_group::EXECUTE_ACTION_NAME)); execute_action_client_->wait_for_action_server(wait_for_servers.to_chrono>()); diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index 1989497ba9..7220332eaa 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -25,8 +25,6 @@ moveit_ros_planning moveit_ros_warehouse moveit_ros_move_group - - rclcpp rclpy rclcpp_action diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml index 1dcc57195c..3e02eb81a2 100644 --- a/moveit_runtime/package.xml +++ b/moveit_runtime/package.xml @@ -18,9 +18,6 @@ moveit_core moveit_planners moveit_plugins - moveit_ros_move_group