From 12e9210cb834938ff45eb3fccf9f566c2e6d554a Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Thu, 10 Mar 2022 19:32:49 +0100
Subject: [PATCH] ROS distribution compatibility update
---
CHANGELOG.rst | 47 ++
CMakeLists.txt | 14 +-
.../capability_names.h | 7 +-
.../src/pick_place_action_capability.cpp | 57 +-
.../src/pick_place_action_capability.h | 10 +-
package.xml | 6 +-
pick_place/CMakeLists.txt | 2 +
.../pick_place/approach_and_translate_stage.h | 7 +-
.../moveit/pick_place/manipulation_pipeline.h | 71 +-
.../moveit/pick_place/manipulation_plan.h | 23 +-
.../moveit/pick_place/manipulation_stage.h | 9 +-
.../include/moveit/pick_place/pick_place.h | 79 ++-
.../moveit/pick_place/pick_place_params.h | 71 +-
.../include/moveit/pick_place/plan_stage.h | 7 +-
.../pick_place/reachable_valid_pose_filter.h | 9 +-
.../src/approach_and_translate_stage.cpp | 628 +++++++++---------
pick_place/src/manipulation_pipeline.cpp | 92 +--
pick_place/src/pick.cpp | 12 +-
pick_place/src/pick_place.cpp | 29 +-
pick_place/src/pick_place_params.cpp | 6 +-
pick_place/src/place.cpp | 74 ++-
pick_place/src/plan_stage.cpp | 69 +-
.../src/reachable_valid_pose_filter.cpp | 17 +-
23 files changed, 691 insertions(+), 655 deletions(-)
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index d18cd60..0d9f49d 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -2,6 +2,53 @@
Changelog for package moveit_ros_manipulation
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.1.8 (2022-01-30)
+------------------
+
+1.1.7 (2021-12-31)
+------------------
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof
+
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* Introduce a reference frame for collision objects (`#2037 <https://github.com/ros-planning/moveit/issues/2037>`_)
+* Add missing dependencies to generated dynamic_reconfigure headers (`#2772 <https://github.com/ros-planning/moveit/issues/2772>`_)
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Contributors: Felix von Drigalski, Mathias Lüdtke, Robert Haschke, pvanlaar
+
+1.1.5 (2021-05-23)
+------------------
+
+1.1.4 (2021-05-12)
+------------------
+
+1.1.3 (2021-04-29)
+------------------
+
+1.1.2 (2021-04-08)
+------------------
+* Order PlaceLocations by quality during planning (`#2378 <https://github.com/ros-planning/moveit/issues/2378>`_)
+* Contributors: Markus Vieth
+
+1.1.1 (2020-10-13)
+------------------
+* [maint] Add comment to MOVEIT_CLASS_FORWARD (`#2315 <https://github.com/ros-planning/moveit/issues/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 <https://github.com/ros-planning/moveit/issues/2229>`_)
+* Contributors: Markus Vieth, Robert Haschke
+
+1.0.5 (2020-07-15)
+------------------
+
1.0.4 (2020-05-30)
------------------
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1288e6c..4af616d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,14 +1,6 @@
cmake_minimum_required(VERSION 3.1.3)
project(moveit_ros_manipulation)
-set(CMAKE_CXX_STANDARD 14)
-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
@@ -21,6 +13,8 @@ find_package(catkin REQUIRED COMPONENTS
pluginlib
actionlib
)
+moveit_build_options()
+
find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED thread system filesystem date_time program_options)
@@ -46,9 +40,9 @@ catkin_package(
include_directories(pick_place/include)
include_directories(move_group_pick_place_capability/include)
-include_directories(${catkin_INCLUDE_DIRS}
- ${Boost_INCLUDE_DIRS})
include_directories(SYSTEM
+ ${catkin_INCLUDE_DIRS}
+ ${Boost_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS})
add_subdirectory(pick_place)
diff --git a/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h b/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h
index 8058e20..5feaa87 100644
--- a/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h
+++ b/move_group_pick_place_capability/include/moveit/move_group_pick_place_capability/capability_names.h
@@ -34,8 +34,7 @@
/* Author: Ioan Sucan */
-#ifndef MOVEIT_MOVE_GROUP_PICK_PLACE_CAPABILITY_NAMES
-#define MOVEIT_MOVE_GROUP_PICK_PLACE_CAPABILITY_NAMES
+#pragma once
#include <string>
@@ -43,6 +42,4 @@ 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
-}
-
-#endif
+} // namespace move_group
diff --git a/move_group_pick_place_capability/src/pick_place_action_capability.cpp b/move_group_pick_place_capability/src/pick_place_action_capability.cpp
index 32a3d46..91796b7 100644
--- a/move_group_pick_place_capability/src/pick_place_action_capability.cpp
+++ b/move_group_pick_place_capability/src/pick_place_action_capability.cpp
@@ -38,7 +38,6 @@
#include <moveit/plan_execution/plan_execution.h>
#include <moveit/plan_execution/plan_with_sensing.h>
#include <moveit/move_group_pick_place_capability/capability_names.h>
-#include <moveit/trajectory_processing/iterative_time_parameterization.h>
move_group::MoveGroupPickPlaceAction::MoveGroupPickPlaceAction()
: MoveGroupCapability("PickPlaceAction"), pickup_state_(IDLE)
@@ -47,23 +46,24 @@ move_group::MoveGroupPickPlaceAction::MoveGroupPickPlaceAction()
void move_group::MoveGroupPickPlaceAction::initialize()
{
- pick_place_.reset(new pick_place::PickPlace(context_->planning_pipeline_));
+ pick_place_ = std::make_shared<pick_place::PickPlace>(context_->planning_pipeline_);
pick_place_->displayComputedMotionPlans(true);
if (context_->debug_)
pick_place_->displayProcessedGrasps(true);
// start the pickup action server
- pickup_action_server_.reset(new actionlib::SimpleActionServer<moveit_msgs::PickupAction>(
- root_node_handle_, PICKUP_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, _1),
- false));
- pickup_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this));
+ pickup_action_server_ = std::make_unique<actionlib::SimpleActionServer<moveit_msgs::PickupAction>>(
+ root_node_handle_, PICKUP_ACTION,
+ std::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, std::placeholders::_1), false);
+ pickup_action_server_->registerPreemptCallback(std::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this));
pickup_action_server_->start();
// start the place action server
- place_action_server_.reset(new actionlib::SimpleActionServer<moveit_msgs::PlaceAction>(
- root_node_handle_, PLACE_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePlaceCallback, this, _1), false));
- place_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPlaceCallback, this));
+ place_action_server_ = std::make_unique<actionlib::SimpleActionServer<moveit_msgs::PlaceAction>>(
+ root_node_handle_, PLACE_ACTION,
+ std::bind(&MoveGroupPickPlaceAction::executePlaceCallback, this, std::placeholders::_1), false);
+ place_action_server_->registerPreemptCallback(std::bind(&MoveGroupPickPlaceAction::preemptPlaceCallback, this));
place_action_server_->start();
}
@@ -90,7 +90,6 @@ void move_group::MoveGroupPickPlaceAction::startPlaceLookCallback()
void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanOnly(const moveit_msgs::PickupGoalConstPtr& goal,
moveit_msgs::PickupResult& action_res)
{
-
pick_place::PickPlanPtr plan;
try
{
@@ -152,7 +151,6 @@ void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanOnly(const mo
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)
@@ -190,7 +188,6 @@ bool move_group::MoveGroupPickPlaceAction::planUsingPickPlacePickup(const moveit
if (pick_plan)
{
const std::vector<pick_place::ManipulationPlanPtr>& success = pick_plan->getSuccessfulManipulationPlans();
-
if (success.empty())
{
plan.error_code_ = pick_plan->getErrorCode();
@@ -264,21 +261,21 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanAndExecute(
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_ = boost::bind(&MoveGroupPickPlaceAction::startPickupExecutionCallback, this);
+ opt.before_execution_callback_ = std::bind(&MoveGroupPickPlaceAction::startPickupExecutionCallback, this);
- opt.plan_callback_ =
- boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePickup, this, boost::cref(*goal), &action_res, _1);
+ opt.plan_callback_ = std::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePickup, this, boost::cref(*goal),
+ &action_res, std::placeholders::_1);
if (goal->planning_options.look_around && context_->plan_with_sensing_)
{
- opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
- _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
- goal->planning_options.max_safe_execution_cost);
+ opt.plan_callback_ =
+ std::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
+ std::placeholders::_1, opt.plan_callback_, goal->planning_options.look_around_attempts,
+ goal->planning_options.max_safe_execution_cost);
context_->plan_with_sensing_->setBeforeLookCallback(
- boost::bind(&MoveGroupPickPlaceAction::startPickupLookCallback, this));
+ std::bind(&MoveGroupPickPlaceAction::startPickupLookCallback, this));
}
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);
@@ -288,24 +285,25 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanAndExecute(
action_res.error_code = plan.error_code_;
}
-void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanAndExecute(
- const moveit_msgs::PlaceGoalConstPtr& goal, moveit_msgs::PlaceResult& action_res)
+void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanAndExecute(const moveit_msgs::PlaceGoalConstPtr& goal,
+ moveit_msgs::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_ = boost::bind(&MoveGroupPickPlaceAction::startPlaceExecutionCallback, this);
- opt.plan_callback_ =
- boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePlace, this, boost::cref(*goal), &action_res, _1);
+ opt.before_execution_callback_ = std::bind(&MoveGroupPickPlaceAction::startPlaceExecutionCallback, this);
+ opt.plan_callback_ = std::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePlace, this, boost::cref(*goal),
+ &action_res, std::placeholders::_1);
if (goal->planning_options.look_around && context_->plan_with_sensing_)
{
- opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
- _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
- goal->planning_options.max_safe_execution_cost);
+ opt.plan_callback_ =
+ std::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
+ std::placeholders::_1, opt.plan_callback_, goal->planning_options.look_around_attempts,
+ goal->planning_options.max_safe_execution_cost);
context_->plan_with_sensing_->setBeforeLookCallback(
- boost::bind(&MoveGroupPickPlaceAction::startPlaceLookCallback, this));
+ std::bind(&MoveGroupPickPlaceAction::startPlaceLookCallback, this));
}
plan_execution::ExecutableMotionPlan plan;
@@ -327,7 +325,6 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallback(const moveit_ms
context_->planning_scene_monitor_->updateFrameTransforms();
moveit_msgs::PickupGoalConstPtr goal;
-
if (input_goal->possible_grasps.empty())
{
moveit_msgs::PickupGoal* copy(new moveit_msgs::PickupGoal(*input_goal));
diff --git a/move_group_pick_place_capability/src/pick_place_action_capability.h b/move_group_pick_place_capability/src/pick_place_action_capability.h
index d76e624..5945235 100644
--- a/move_group_pick_place_capability/src/pick_place_action_capability.h
+++ b/move_group_pick_place_capability/src/pick_place_action_capability.h
@@ -34,8 +34,7 @@
/* Author: Ioan Sucan */
-#ifndef MOVEIT_MOVE_GROUP_PICK_PLACE_ACTION_CAPABILITY_
-#define MOVEIT_MOVE_GROUP_PICK_PLACE_ACTION_CAPABILITY_
+#pragma once
#include <moveit/move_group/move_group_capability.h>
#include <actionlib/server/simple_action_server.h>
@@ -57,8 +56,7 @@ private:
void executePickupCallback(const moveit_msgs::PickupGoalConstPtr& goal);
void executePlaceCallback(const moveit_msgs::PlaceGoalConstPtr& goal);
- void executePickupCallbackPlanOnly(const moveit_msgs::PickupGoalConstPtr& goal,
- moveit_msgs::PickupResult& action_res);
+ void executePickupCallbackPlanOnly(const moveit_msgs::PickupGoalConstPtr& goal, moveit_msgs::PickupResult& action_res);
void executePickupCallbackPlanAndExecute(const moveit_msgs::PickupGoalConstPtr& goal,
moveit_msgs::PickupResult& action_res);
@@ -100,6 +98,4 @@ private:
ros::ServiceClient grasp_planning_service_;
};
-}
-
-#endif
+} // namespace move_group
diff --git a/package.xml b/package.xml
index 60dc219..d9c6a81 100644
--- a/package.xml
+++ b/package.xml
@@ -1,14 +1,14 @@
<package format="2">
<name>moveit_ros_manipulation</name>
- <version>1.0.5</version>
- <description>Components of MoveIt! used for manipulation</description>
+ <version>1.1.8</version>
+ <description>Components of MoveIt used for manipulation</description>
<author email="isucan@google.com">Ioan Sucan</author>
<author email="robot.moveit@gmail.com">Sachin Chitta</author>
<maintainer email="me@v4hn.de">Michael Görner</maintainer>
<maintainer email="mferguson@fetchrobotics.com">Michael Ferguson</maintainer>
- <maintainer email="moveit_releasers@googlegroups.com">MoveIt! Release Team</maintainer>
+ <maintainer email="moveit_releasers@googlegroups.com">MoveIt Release Team</maintainer>
<license>BSD</license>
diff --git a/pick_place/CMakeLists.txt b/pick_place/CMakeLists.txt
index fb371ae..792c9fb 100644
--- a/pick_place/CMakeLists.txt
+++ b/pick_place/CMakeLists.txt
@@ -14,6 +14,8 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V
target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+add_dependencies(${MOVEIT_LIB_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS}) # don't build until necessary msgs are available
+
install(TARGETS ${MOVEIT_LIB_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
diff --git a/pick_place/include/moveit/pick_place/approach_and_translate_stage.h b/pick_place/include/moveit/pick_place/approach_and_translate_stage.h
index 70f2b2b..79b3eea 100644
--- a/pick_place/include/moveit/pick_place/approach_and_translate_stage.h
+++ b/pick_place/include/moveit/pick_place/approach_and_translate_stage.h
@@ -34,8 +34,7 @@
/* Author: Ioan Sucan, Sachin Chitta */
-#ifndef MOVEIT_PICK_PLACE_APPROACH_AND_TRANSLATE_STAGE_
-#define MOVEIT_PICK_PLACE_APPROACH_AND_TRANSLATE_STAGE_
+#pragma once
#include <moveit/pick_place/manipulation_stage.h>
#include <moveit/planning_pipeline/planning_pipeline.h>
@@ -63,6 +62,4 @@ private:
void configureVelocity(double &max_v, double &max_a) const;
};
-}
-
-#endif
+} // namespace pick_place
diff --git a/pick_place/include/moveit/pick_place/manipulation_pipeline.h b/pick_place/include/moveit/pick_place/manipulation_pipeline.h
index f1bc6a9..942e8ac 100644
--- a/pick_place/include/moveit/pick_place/manipulation_pipeline.h
+++ b/pick_place/include/moveit/pick_place/manipulation_pipeline.h
@@ -1,41 +1,40 @@
/*********************************************************************
-* 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.
-*********************************************************************/
+ * 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 */
-#ifndef MOVEIT_PICK_PLACE_MANIPULATION_PIPELINE_
-#define MOVEIT_PICK_PLACE_MANIPULATION_PIPELINE_
+#pragma once
#include <moveit/pick_place/manipulation_stage.h>
#include <boost/thread.hpp>
@@ -116,6 +115,4 @@ protected:
bool stop_processing_;
};
-}
-
-#endif
+} // namespace pick_place
diff --git a/pick_place/include/moveit/pick_place/manipulation_plan.h b/pick_place/include/moveit/pick_place/manipulation_plan.h
index cc3ff2f..56dd352 100644
--- a/pick_place/include/moveit/pick_place/manipulation_plan.h
+++ b/pick_place/include/moveit/pick_place/manipulation_plan.h
@@ -34,8 +34,7 @@
/* Author: Ioan Sucan, Sachin Chitta */
-#ifndef MOVEIT_PICK_PLACE_MANIPULATION_PLAN_
-#define MOVEIT_PICK_PLACE_MANIPULATION_PLAN_
+#pragma once
#include <moveit/macros/class_forward.h>
#include <moveit/robot_state/robot_state.h>
@@ -56,17 +55,17 @@ MOVEIT_STRUCT_FORWARD(ManipulationPlanSharedData);
struct ManipulationPlanSharedData
{
ManipulationPlanSharedData()
- : planning_group_(NULL)
- , end_effector_group_(NULL)
- , ik_link_(NULL)
+ : planning_group_(nullptr)
+ , end_effector_group_(nullptr)
+ , ik_link_(nullptr)
, max_goal_sampling_attempts_(0)
, minimize_object_distance_(false)
{
}
- const robot_model::JointModelGroup* planning_group_;
- const robot_model::JointModelGroup* end_effector_group_;
- const robot_model::LinkModel* ik_link_;
+ 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_;
@@ -126,9 +125,9 @@ struct ManipulationPlan
// Allows for the sampling of a kineamtic state for a particular group of a robot
constraint_samplers::ConstraintSamplerPtr goal_sampler_;
- std::vector<robot_state::RobotStatePtr> possible_goal_states_;
+ std::vector<moveit::core::RobotStatePtr> possible_goal_states_;
- robot_state::RobotStatePtr approach_state_;
+ moveit::core::RobotStatePtr approach_state_;
// The sequence of trajectories produced for execution
std::vector<plan_execution::ExecutableTrajectory> trajectories_;
@@ -142,6 +141,4 @@ struct ManipulationPlan
// An id for this plan; this is usually the index of the Grasp / PlaceLocation in the input request
std::size_t id_;
};
-}
-
-#endif
+} // namespace pick_place
diff --git a/pick_place/include/moveit/pick_place/manipulation_stage.h b/pick_place/include/moveit/pick_place/manipulation_stage.h
index 5b36f9f..14636fe 100644
--- a/pick_place/include/moveit/pick_place/manipulation_stage.h
+++ b/pick_place/include/moveit/pick_place/manipulation_stage.h
@@ -34,8 +34,7 @@
/* Author: Ioan Sucan, Sachin Chitta */
-#ifndef MOVEIT_PICK_PLACE_MANIPULATION_STAGE_
-#define MOVEIT_PICK_PLACE_MANIPULATION_STAGE_
+#pragma once
#include <moveit/macros/class_forward.h>
#include <moveit/pick_place/manipulation_plan.h>
@@ -43,7 +42,7 @@
namespace pick_place
{
-MOVEIT_CLASS_FORWARD(ManipulationStage);
+MOVEIT_CLASS_FORWARD(ManipulationStage); // Defines ManipulationStagePtr, ConstPtr, WeakPtr... etc
class ManipulationStage
{
@@ -83,6 +82,4 @@ protected:
bool signal_stop_;
bool verbose_;
};
-}
-
-#endif
+} // namespace pick_place
diff --git a/pick_place/include/moveit/pick_place/pick_place.h b/pick_place/include/moveit/pick_place/pick_place.h
index 1507ebe..7781179 100644
--- a/pick_place/include/moveit/pick_place/pick_place.h
+++ b/pick_place/include/moveit/pick_place/pick_place.h
@@ -1,41 +1,40 @@
/*********************************************************************
-* 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.
-*********************************************************************/
+ * 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 */
-#ifndef MOVEIT_PICK_PLACE_PICK_PLACE_
-#define MOVEIT_PICK_PLACE_PICK_PLACE_
+#pragma once
#include <moveit/macros/class_forward.h>
#include <moveit/pick_place/manipulation_pipeline.h>
@@ -49,7 +48,7 @@
namespace pick_place
{
-MOVEIT_CLASS_FORWARD(PickPlace);
+MOVEIT_CLASS_FORWARD(PickPlace); // Defines PickPlacePtr, ConstPtr, WeakPtr... etc
class PickPlacePlanBase
{
@@ -93,7 +92,7 @@ protected:
moveit_msgs::MoveItErrorCodes error_code_;
};
-MOVEIT_CLASS_FORWARD(PickPlan);
+MOVEIT_CLASS_FORWARD(PickPlan); // Defines PickPlanPtr, ConstPtr, WeakPtr... etc
class PickPlan : public PickPlacePlanBase
{
@@ -102,7 +101,7 @@ public:
bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::PickupGoal& goal);
};
-MOVEIT_CLASS_FORWARD(PlacePlan);
+MOVEIT_CLASS_FORWARD(PlacePlan); // Defines PlacePlanPtr, ConstPtr, WeakPtr... etc
class PlacePlan : public PickPlacePlanBase
{
@@ -132,7 +131,7 @@ public:
return planning_pipeline_;
}
- const robot_model::RobotModelConstPtr& getRobotModel() const
+ const moveit::core::RobotModelConstPtr& getRobotModel() const
{
return planning_pipeline_->getRobotModel();
}
@@ -164,6 +163,4 @@ private:
constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_;
};
-}
-
-#endif
+} // namespace pick_place
diff --git a/pick_place/include/moveit/pick_place/pick_place_params.h b/pick_place/include/moveit/pick_place/pick_place_params.h
index baabe49..38be2ba 100644
--- a/pick_place/include/moveit/pick_place/pick_place_params.h
+++ b/pick_place/include/moveit/pick_place/pick_place_params.h
@@ -1,41 +1,40 @@
/*********************************************************************
-* 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.
-*********************************************************************/
+ * 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 */
-#ifndef MOVEIT_PICK_PLACE_PICK_PLACE_PARAMS_
-#define MOVEIT_PICK_PLACE_PICK_PLACE_PARAMS_
+#pragma once
namespace pick_place
{
@@ -51,6 +50,4 @@ struct PickPlaceParams
// Get access to a global variable that contains the pick & place params.
const PickPlaceParams& GetGlobalPickPlaceParams();
-}
-
-#endif
+} // namespace pick_place
diff --git a/pick_place/include/moveit/pick_place/plan_stage.h b/pick_place/include/moveit/pick_place/plan_stage.h
index ffcbc32..8d2de9e 100644
--- a/pick_place/include/moveit/pick_place/plan_stage.h
+++ b/pick_place/include/moveit/pick_place/plan_stage.h
@@ -34,8 +34,7 @@
/* Author: Ioan Sucan, Sachin Chitta */
-#ifndef MOVEIT_PICK_PLACE_PLAN_STAGE_
-#define MOVEIT_PICK_PLACE_PLAN_STAGE_
+#pragma once
#include <moveit/pick_place/manipulation_stage.h>
#include <moveit/planning_pipeline/planning_pipeline.h>
@@ -59,6 +58,4 @@ private:
planning_interface::MotionPlanRequest &configureVelocity(planning_interface::MotionPlanRequest &req) const;
};
-}
-
-#endif
+} // namespace pick_place
diff --git a/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h b/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h
index 7fe44f0..0e5ec7b 100644
--- a/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h
+++ b/pick_place/include/moveit/pick_place/reachable_valid_pose_filter.h
@@ -34,8 +34,7 @@
/* Author: Ioan Sucan, Sachin Chitta */
-#ifndef MOVEIT_PICK_PLACE_REACHABLE_VALID_POSE_FILTER_
-#define MOVEIT_PICK_PLACE_REACHABLE_VALID_POSE_FILTER_
+#pragma once
#include <moveit/pick_place/manipulation_stage.h>
#include <moveit/constraint_samplers/constraint_sampler_manager.h>
@@ -53,12 +52,10 @@ public:
bool evaluate(const ManipulationPlanPtr& plan) const override;
private:
- bool isEndEffectorFree(const ManipulationPlanPtr& plan, robot_state::RobotState& token_state) const;
+ 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_;
};
-}
-
-#endif
+} // namespace pick_place
diff --git a/pick_place/src/approach_and_translate_stage.cpp b/pick_place/src/approach_and_translate_stage.cpp
index 24f0433..0902ac6 100644
--- a/pick_place/src/approach_and_translate_stage.cpp
+++ b/pick_place/src/approach_and_translate_stage.cpp
@@ -37,334 +37,348 @@
#include <moveit/pick_place/pick_place.h>
#include <moveit/pick_place/approach_and_translate_stage.h>
#include <moveit/trajectory_processing/trajectory_tools.h>
+#include <moveit/robot_state/cartesian_interpolator.h>
#include <tf2_eigen/tf2_eigen.h>
#include <ros/console.h>
-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 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;
}
-
- namespace {
- bool isStateCollisionFree(const planning_scene::PlanningScene *planning_scene,
- const collision_detection::AllowedCollisionMatrix *collision_matrix, bool verbose,
- const trajectory_msgs::JointTrajectory *grasp_posture, robot_state::RobotState *state,
- const robot_state::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 robot_state::RobotState &reference_state,
- double min_distance, unsigned int attempts) {
- // initialize with scene state
- robot_state::RobotStatePtr token_state(new robot_state::RobotState(reference_state));
- for (unsigned int j = 0; j < attempts; ++j) {
- double min_d = std::numeric_limits<double>::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;
- }
+ }
+ 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
+ moveit::core::RobotStatePtr token_state(new moveit::core::RobotState(reference_state));
+ for (unsigned int j = 0; j < attempts; ++j)
+ {
+ double min_d = std::numeric_limits<double>::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::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;
- }
+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::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()) {
- robot_state::RobotStatePtr ee_closed_state(
- new robot_state::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint()));
-
- robot_trajectory::RobotTrajectoryPtr ee_closed_traj(new robot_trajectory::RobotTrajectory(
- 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_ = boost::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_,
- _1);
- 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.");
- }
- }
+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()));
+
+ robot_trajectory::RobotTrajectoryPtr ee_closed_traj(new robot_trajectory::RobotTrajectory(
+ 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);
+ }
- } // namespace
-
- bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr &plan) const {
- const robot_model::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 = !robot_state::Transforms::sameFrame(
- plan->approach_.direction.header.frame_id, plan->shared_data_->ik_link_->getName());
- bool retreat_direction_is_global_frame = !robot_state::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)
- approach_direction =
- planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).rotation() *
- approach_direction;
- if (retreat_direction_is_global_frame)
- retreat_direction =
- planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).rotation() *
- retreat_direction;
-
- // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping
- robot_state::GroupStateValidityCallbackFn approach_valid_callback =
- boost::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_,
- &plan->approach_posture_, _1, _2, _3);
- plan->goal_sampler_->setVerbose(verbose_);
- std::size_t attempted_possible_goal_states = 0;
- do // continously sample possible goal states
+ 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_ =
+ std::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_, std::placeholders::_1);
+ 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 =
+ std::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_,
+ &plan->approach_posture_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+ plan->goal_sampler_->setVerbose(verbose_);
+ std::size_t attempted_possible_goal_states = 0;
+ do // continously 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;
+ moveit::core::RobotStatePtr close_up_state(new moveit::core::RobotState(*plan->possible_goal_states_[i]));
+ std::vector<moveit::core::RobotStatePtr> 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
+ moveit::core::RobotStatePtr first_approach_state(new moveit::core::RobotState(*plan->possible_goal_states_[i]));
+
+ std::vector<moveit::core::RobotStatePtr> 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)
{
- 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;
- robot_state::RobotStatePtr close_up_state(
- new robot_state::RobotState(*plan->possible_goal_states_[i]));
- std::vector<robot_state::RobotStatePtr> close_up_states;
- double d_close_up = close_up_state->computeCartesianPath(
- plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_,
- approach_direction,
- approach_direction_is_global_frame, MAX_CLOSE_UP_DIST, max_step_, jump_factor_,
- approach_valid_callback);
-
- // POSSIBLE CHANGE SPOT
-
- // 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
- robot_state::RobotStatePtr first_approach_state(
- new robot_state::RobotState(*plan->possible_goal_states_[i]));
-
- std::vector<robot_state::RobotStatePtr> approach_states;
- double d_approach = first_approach_state->computeCartesianPath(
- plan->shared_data_->planning_group_, approach_states, plan->shared_data_->ik_link_,
- -approach_direction,
- approach_direction_is_global_frame, plan->approach_.desired_distance, max_step_, 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
- robot_state::GroupStateValidityCallbackFn retreat_valid_callback =
- boost::bind(&isStateCollisionFree, planning_scene_after_approach.get(),
- collision_matrix_.get(), verbose_,
- &plan->retreat_posture_, _1, _2, _3);
-
- // try to compute a straight line path that moves from the goal in a desired direction
- robot_state::RobotStatePtr last_retreat_state(
- new robot_state::RobotState(planning_scene_after_approach->getCurrentState()));
- std::vector<robot_state::RobotStatePtr> retreat_states;
- double d_retreat = last_retreat_state->computeCartesianPath(
- plan->shared_data_->planning_group_, retreat_states, plan->shared_data_->ik_link_,
- retreat_direction,
- retreat_direction_is_global_frame, plan->retreat_.desired_distance, max_step_,
- jump_factor_,
- retreat_valid_callback);
-
- if (d_retreat > plan->retreat_.min_distance && !signal_stop_)
- {
- // Create approach trajectory
- std::reverse(approach_states.begin(), approach_states.end());
- robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
- 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
- robot_trajectory::RobotTrajectoryPtr retreat_traj(new robot_trajectory::RobotTrajectory(
- 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);
-
- double max_v = 0.0;
- double max_a = 0.0;
- configureVelocity(max_v, max_a);
-
- // Add timestamps to approach|retreat trajectories
- time_param_.computeTimeStamps(*approach_traj, max_v, max_a);
- time_param_.computeTimeStamps(*retreat_traj, max_v, max_a);
-
- // 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());
- robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
- planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
- for (std::size_t k = 0; k < approach_states.size(); ++k)
- approach_traj->addSuffixWayPoint(approach_states[k], 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::MoveItErrorCodes::PLANNING_FAILED;
+ // 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 =
+ std::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_,
+ &plan->retreat_posture_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+
+ // 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<moveit::core::RobotStatePtr> 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());
+ robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
+ 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
+ robot_trajectory::RobotTrajectoryPtr retreat_traj(new robot_trajectory::RobotTrajectory(
+ 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);
+
+ double max_v = 0.0;
+ double max_a = 0.0;
+ configureVelocity(max_v, max_a);
+
+ // Add timestamps to approach|retreat trajectories
+ time_param_.computeTimeStamps(*approach_traj, max_v, max_a);
+ time_param_.computeTimeStamps(*retreat_traj, max_v, max_a);
+
+ // 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);
- return false;
+ // Create approach trajectory
+ std::reverse(approach_states.begin(), approach_states.end());
+ robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
+ 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::MoveItErrorCodes::PLANNING_FAILED;
+
+ return false;
+}
- void ApproachAndTranslateStage::configureVelocity(double &max_v, double &max_a) const {
+void ApproachAndTranslateStage::configureVelocity(double &max_v, double &max_a) const {
if(!ros::param::has("max_grasp_approach_velocity")){
- ros::param::set("max_grasp_approach_velocity", 0.5);
+ ros::param::set("max_grasp_approach_velocity", 0.1);
max_v = 0.5;
}else{
ros::param::get("max_grasp_approach_velocity", max_v);
}
if(!ros::param::has("max_grasp_approach_acceleration")){
- ros::param::set("max_grasp_approach_acceleration", 0.5);
+ ros::param::set("max_grasp_approach_acceleration", 0.1);
max_a = 0.5;
}else{
ros::param::get("max_grasp_approach_acceleration", max_a);
diff --git a/pick_place/src/manipulation_pipeline.cpp b/pick_place/src/manipulation_pipeline.cpp
index e75a907..b8512d5 100644
--- a/pick_place/src/manipulation_pipeline.cpp
+++ b/pick_place/src/manipulation_pipeline.cpp
@@ -1,36 +1,36 @@
/*********************************************************************
-* 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.
-*********************************************************************/
+ * 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 */
@@ -88,8 +88,8 @@ void ManipulationPipeline::reset()
void ManipulationPipeline::setVerbose(bool flag)
{
verbose_ = flag;
- for (std::size_t i = 0; i < stages_.size(); ++i)
- stages_[i]->setVerbose(flag);
+ for (pick_place::ManipulationStagePtr& stage : stages_)
+ stage->setVerbose(flag);
}
void ManipulationPipeline::clear()
@@ -110,17 +110,17 @@ void ManipulationPipeline::start()
{
stop_processing_ = false;
empty_queue_threads_ = 0;
- for (std::size_t i = 0; i < stages_.size(); ++i)
- stages_[i]->resetStopSignal();
+ 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 boost::thread(boost::bind(&ManipulationPipeline::processingThread, this, i));
+ processing_threads_[i] = new boost::thread(std::bind(&ManipulationPipeline::processingThread, this, i));
}
void ManipulationPipeline::signalStop()
{
- for (std::size_t i = 0; i < stages_.size(); ++i)
- stages_[i]->signalStop();
+ for (pick_place::ManipulationStagePtr& stage : stages_)
+ stage->signalStop();
stop_processing_ = true;
queue_access_cond_.notify_all();
}
@@ -128,12 +128,12 @@ void ManipulationPipeline::signalStop()
void ManipulationPipeline::stop()
{
signalStop();
- for (std::size_t i = 0; i < processing_threads_.size(); ++i)
- if (processing_threads_[i])
+ for (boost::thread*& processing_thread : processing_threads_)
+ if (processing_thread)
{
- processing_threads_[i]->join();
- delete processing_threads_[i];
- processing_threads_[i] = nullptr;
+ processing_thread->join();
+ delete processing_thread;
+ processing_thread = nullptr;
}
}
@@ -209,8 +209,8 @@ void ManipulationPipeline::push(const ManipulationPlanPtr& plan)
{
boost::mutex::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());
+ ROS_INFO_STREAM_NAMED("manipulation",
+ "Added plan for pipeline '" << name_ << "'. Queue is now of size " << queue_.size());
queue_access_cond_.notify_all();
}
diff --git a/pick_place/src/pick.cpp b/pick_place/src/pick.cpp
index 5b922a6..c96ff5a 100644
--- a/pick_place/src/pick.cpp
+++ b/pick_place/src/pick.cpp
@@ -38,6 +38,7 @@
#include <moveit/pick_place/reachable_valid_pose_filter.h>
#include <moveit/pick_place/approach_and_translate_stage.h>
#include <moveit/pick_place/plan_stage.h>
+#include <moveit/utils/message_checks.h>
#include <ros/console.h>
namespace pick_place
@@ -72,7 +73,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
std::string end_effector = goal.end_effector;
if (end_effector.empty() && !planning_group.empty())
{
- const robot_model::JointModelGroup* jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
+ const moveit::core::JointModelGroup* jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
if (!jmg)
{
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
@@ -90,7 +91,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
}
else if (!end_effector.empty() && planning_group.empty())
{
- const robot_model::JointModelGroup* jmg = planning_scene->getRobotModel()->getEndEffector(end_effector);
+ const moveit::core::JointModelGroup* jmg = planning_scene->getRobotModel()->getEndEffector(end_effector);
if (!jmg)
{
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
@@ -108,7 +109,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
ROS_INFO_STREAM_NAMED("manipulation", "Assuming the planning group for end effector '" << end_effector << "' is '"
<< planning_group << "'");
}
- const robot_model::JointModelGroup* eef =
+ const moveit::core::JointModelGroup* eef =
end_effector.empty() ? nullptr : planning_scene->getRobotModel()->getEndEffector(end_effector);
if (!eef)
{
@@ -172,7 +173,8 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
for (std::size_t i = 0; i < goal.possible_grasps.size(); ++i)
grasp_order[i] = i;
OrderGraspQuality oq(goal.possible_grasps);
- std::sort(grasp_order.begin(), grasp_order.end(), oq);
+ // 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)
@@ -230,7 +232,7 @@ PickPlanPtr PickPlace::planPick(const planning_scene::PlanningSceneConstPtr& pla
{
PickPlanPtr p(new PickPlan(shared_from_this()));
- if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff))
+ 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);
diff --git a/pick_place/src/pick_place.cpp b/pick_place/src/pick_place.cpp
index cd85506..21d4bb8 100644
--- a/pick_place/src/pick_place.cpp
+++ b/pick_place/src/pick_place.cpp
@@ -38,7 +38,6 @@
#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <visualization_msgs/MarkerArray.h>
-#include <ros/console.h>
namespace pick_place
{
@@ -52,8 +51,8 @@ const double PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION = 7.0; // sec
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(boost::bind(&PickPlacePlanBase::foundSolution, this));
- pipeline_.setEmptyQueueCallback(boost::bind(&PickPlacePlanBase::emptyQueue, this));
+ pipeline_.setSolutionCallback(std::bind(&PickPlacePlanBase::foundSolution, this));
+ pipeline_.setEmptyQueueCallback(std::bind(&PickPlacePlanBase::emptyQueue, this));
}
PickPlacePlanBase::~PickPlacePlanBase() = default;
@@ -93,7 +92,8 @@ void PickPlacePlanBase::waitForPipeline(const ros::WallTime& endtime)
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_.reset(new constraint_sampler_manager_loader::ConstraintSamplerManagerLoader());
+ constraint_sampler_manager_loader_ =
+ std::make_shared<constraint_sampler_manager_loader::ConstraintSamplerManagerLoader>();
}
void PickPlace::displayProcessedGrasps(bool flag)
@@ -119,18 +119,17 @@ void PickPlace::visualizePlan(const ManipulationPlanPtr& plan) const
moveit_msgs::DisplayTrajectory dtraj;
dtraj.model_id = getRobotModel()->getName();
bool first = true;
- for (std::size_t i = 0; i < plan->trajectories_.size(); ++i)
+ for (const plan_execution::ExecutableTrajectory& traj : plan->trajectories_)
{
- if (!plan->trajectories_[i].trajectory_ || plan->trajectories_[i].trajectory_->empty())
+ if (!traj.trajectory_ || traj.trajectory_->empty())
continue;
if (first)
{
- robot_state::robotStateToRobotStateMsg(plan->trajectories_[i].trajectory_->getFirstWayPoint(),
- dtraj.trajectory_start);
+ moveit::core::robotStateToRobotStateMsg(traj.trajectory_->getFirstWayPoint(), dtraj.trajectory_start);
first = false;
}
dtraj.trajectory.resize(dtraj.trajectory.size() + 1);
- plan->trajectories_[i].trajectory_->getRobotTrajectoryMsg(dtraj.trajectory.back());
+ traj.trajectory_->getRobotTrajectoryMsg(dtraj.trajectory.back());
}
display_path_publisher_.publish(dtraj);
}
@@ -179,20 +178,20 @@ void PickPlace::visualizeGrasps(const std::vector<ManipulationPlanPtr>& plans) c
if (plans.empty())
return;
- robot_state::RobotState state(getRobotModel());
+ moveit::core::RobotState state(getRobotModel());
state.setToDefaultValues();
static std::vector<std_msgs::ColorRGBA> colors(setupDefaultGraspColors());
visualization_msgs::MarkerArray ma;
- for (std::size_t i = 0; i < plans.size(); ++i)
+ for (const ManipulationPlanPtr& plan : plans)
{
- const robot_model::JointModelGroup* jmg = plans[i]->shared_data_->end_effector_group_;
+ const moveit::core::JointModelGroup* jmg = plan->shared_data_->end_effector_group_;
if (jmg)
{
- unsigned int type = std::min(plans[i]->processing_stage_, colors.size() - 1);
- state.updateStateWithLinkAt(plans[i]->shared_data_->ik_link_, plans[i]->transformed_goal_pose_);
+ 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<std::string>(plans[i]->processing_stage_),
+ "moveit_grasps:stage_" + boost::lexical_cast<std::string>(plan->processing_stage_),
ros::Duration(60));
}
}
diff --git a/pick_place/src/pick_place_params.cpp b/pick_place/src/pick_place_params.cpp
index 250c42b..7102d23 100644
--- a/pick_place/src/pick_place_params.cpp
+++ b/pick_place/src/pick_place_params.cpp
@@ -49,8 +49,8 @@ class DynamicReconfigureImpl
public:
DynamicReconfigureImpl() : dynamic_reconfigure_server_(ros::NodeHandle("~/pick_place"))
{
- dynamic_reconfigure_server_.setCallback(
- boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
+ dynamic_reconfigure_server_.setCallback(std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this,
+ std::placeholders::_1, std::placeholders::_2));
}
const PickPlaceParams& getParams() const
@@ -61,7 +61,7 @@ public:
private:
PickPlaceParams params_;
- void dynamicReconfigureCallback(PickPlaceDynamicReconfigureConfig& config, uint32_t level)
+ 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;
diff --git a/pick_place/src/place.cpp b/pick_place/src/place.cpp
index 4b7156a..2a84566 100644
--- a/pick_place/src/place.cpp
+++ b/pick_place/src/place.cpp
@@ -39,6 +39,7 @@
#include <moveit/pick_place/approach_and_translate_stage.h>
#include <moveit/pick_place/plan_stage.h>
#include <moveit/robot_state/conversions.h>
+#include <moveit/utils/message_checks.h>
#include <tf2_eigen/tf2_eigen.h>
#include <ros/console.h>
@@ -50,10 +51,24 @@ PlacePlan::PlacePlan(const PickPlaceConstPtr& pick_place) : PickPlacePlanBase(pi
namespace
{
+struct OrderPlaceLocationQuality
+{
+ OrderPlaceLocationQuality(const std::vector<moveit_msgs::PlaceLocation>& 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<moveit_msgs::PlaceLocation>& places_;
+};
+
bool transformToEndEffectorGoal(const geometry_msgs::PoseStamped& goal_pose,
- const robot_state::AttachedBody* attached_body, geometry_msgs::PoseStamped& place_pose)
+ const moveit::core::AttachedBody* attached_body, geometry_msgs::PoseStamped& place_pose)
{
- const EigenSTL::vector_Isometry3d& fixed_transforms = attached_body->getFixedTransforms();
+ const EigenSTL::vector_Isometry3d& fixed_transforms = attached_body->getShapePosesInLinkFrame();
if (fixed_transforms.empty())
return false;
@@ -71,8 +86,8 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
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 robot_model::JointModelGroup* jmg = nullptr;
- const robot_model::JointModelGroup* eef = nullptr;
+ 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))
@@ -103,17 +118,17 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
const std::vector<std::string>& eef_names = jmg->getAttachedEndEffectorNames();
if (eef_names.empty())
{
- ROS_ERROR_STREAM_NAMED("manipulation", "There are no end-effectors specified for group '" << goal.group_name
- << "'");
+ ROS_ERROR_STREAM_NAMED("manipulation",
+ "There are no end-effectors specified for group '" << goal.group_name << "'");
error_code_.val = moveit_msgs::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 (std::size_t i = 0; i < eef_names.size(); ++i)
+ for (const std::string& eef_name : eef_names)
{
- std::vector<const robot_state::AttachedBody*> attached_bodies;
- const robot_model::JointModelGroup* eg = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
+ std::vector<const moveit::core::AttachedBody*> 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
@@ -121,11 +136,11 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
// 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 robot_model::LinkModel* attached_link_model =
+ const moveit::core::LinkModel* attached_link_model =
planning_scene->getRobotModel()->getLinkModel(eg->getEndEffectorParentGroup().second);
if (attached_link_model)
{
- std::vector<const robot_state::AttachedBody*> attached_bodies2;
+ std::vector<const moveit::core::AttachedBody*> attached_bodies2;
planning_scene->getCurrentState().getAttachedBodies(attached_bodies2, attached_link_model);
attached_bodies.insert(attached_bodies.end(), attached_bodies2.begin(), attached_bodies2.end());
}
@@ -139,8 +154,8 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
if (!attached_object_name.empty())
{
bool found = false;
- for (std::size_t j = 0; j < attached_bodies.size(); ++j)
- if (attached_bodies[j]->getName() == attached_object_name)
+ for (const moveit::core::AttachedBody* attached_body : attached_bodies)
+ if (attached_body->getName() == attached_object_name)
{
found = true;
break;
@@ -162,7 +177,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
return false;
}
// set the end effector (this was initialized to NULL above)
- eef = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
+ eef = planning_scene->getRobotModel()->getEndEffector(eef_name);
}
}
}
@@ -171,16 +186,17 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
// if we know the attached object, but not the eef, we can try to identify that
if (!attached_object_name.empty() && !eef)
{
- const robot_state::AttachedBody* attached_body =
+ 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 robot_model::LinkModel* link = attached_body->getAttachedLink();
+ const moveit::core::LinkModel* link = attached_body->getAttachedLink();
// check to see if there is a unique end effector containing the link
- const std::vector<const robot_model::JointModelGroup*>& eefs = planning_scene->getRobotModel()->getEndEffectors();
- for (std::size_t i = 0; i < eefs.size(); ++i)
- if (eefs[i]->hasLinkModel(link->getName()))
+ const std::vector<const moveit::core::JointModelGroup*>& eefs =
+ planning_scene->getRobotModel()->getEndEffectors();
+ for (const moveit::core::JointModelGroup* end_effector : eefs)
+ if (end_effector->hasLinkModel(link->getName()))
{
if (eef)
{
@@ -191,7 +207,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
- eef = eefs[i];
+ eef = end_effector;
}
}
// if the group is also unknown, but we just found out the eef
@@ -223,7 +239,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
{
// 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<const robot_state::AttachedBody*> attached_bodies;
+ std::vector<const moveit::core::AttachedBody*> attached_bodies;
planning_scene->getCurrentState().getAttachedBodies(attached_bodies, loop_count == 0 ? eef : jmg);
loop_count++;
@@ -239,7 +255,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
attached_object_name = attached_bodies[0]->getName();
}
- const robot_state::AttachedBody* attached_body =
+ const moveit::core::AttachedBody* attached_body =
planning_scene->getCurrentState().getAttachedBody(attached_object_name);
if (!attached_body)
{
@@ -302,11 +318,19 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
pipeline_.start();
+ // order the place locations by quality
+ std::vector<std::size_t> 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)
{
ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
- const moveit_msgs::PlaceLocation& pl = goal.place_locations[i];
+ const moveit_msgs::PlaceLocation& pl = goal.place_locations[place_locations_order[i]];
if (goal.place_eef)
p->goal_pose_ = pl.place_pose;
@@ -323,7 +347,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
p->approach_ = pl.pre_place_approach;
p->retreat_ = pl.post_place_retreat;
p->retreat_posture_ = pl.post_place_posture;
- p->id_ = i;
+ p->id_ = place_locations_order[i];
if (p->retreat_posture_.joint_names.empty())
p->retreat_posture_ = attached_body->getDetachPosture();
pipeline_.push(p);
@@ -369,7 +393,7 @@ PlacePlanPtr PickPlace::planPlace(const planning_scene::PlanningSceneConstPtr& p
const moveit_msgs::PlaceGoal& goal) const
{
PlacePlanPtr p(new PlacePlan(shared_from_this()));
- if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff))
+ 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);
diff --git a/pick_place/src/plan_stage.cpp b/pick_place/src/plan_stage.cpp
index 692bf9c..e757c6e 100644
--- a/pick_place/src/plan_stage.cpp
+++ b/pick_place/src/plan_stage.cpp
@@ -39,10 +39,6 @@
#include <moveit/kinematic_constraints/utils.h>
#include <ros/console.h>
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/trajectory_processing/iterative_time_parameterization.h>
-#include <trajectory_msgs/JointTrajectoryPoint.h>
-
namespace pick_place
{
PlanStage::PlanStage(const planning_scene::PlanningSceneConstPtr& scene,
@@ -69,10 +65,9 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const
req.planner_id = plan->shared_data_->planner_id_;
req.start_state.is_diff = true;
req = configureVelocity(req);
- req.goal_constraints.resize(
- 1, kinematic_constraints::constructGoalConstraints(*plan->approach_state_, plan->shared_data_->planning_group_));
-
- unsigned int attempts = 0;
+ 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++;
@@ -82,7 +77,7 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const
// We have a valid motion plan, now apply pre-approach end effector posture (open gripper) if applicable
if (!plan->approach_posture_.joint_names.empty())
{
- robot_state::RobotStatePtr pre_approach_state(new robot_state::RobotState(res.trajectory_->getLastWayPoint()));
+ moveit::core::RobotStatePtr pre_approach_state(new moveit::core::RobotState(res.trajectory_->getLastWayPoint()));
robot_trajectory::RobotTrajectoryPtr pre_approach_traj(new robot_trajectory::RobotTrajectory(
pre_approach_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName()));
pre_approach_traj->setRobotTrajectoryMsg(*pre_approach_state, plan->approach_posture_);
@@ -100,8 +95,7 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const
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);
+ pre_approach_traj->addPrefixWayPoint(pre_approach_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION);
}
// Add the open gripper trajectory to the plan
@@ -109,21 +103,8 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const
plan->trajectories_.insert(plan->trajectories_.begin(), et);
}
- /* moveit::planning_interface::MoveGroupInterface group("panda_arm");
- robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
-
- moveit_msgs::RobotTrajectory trajectory_msg;
- res.trajectory_->getRobotTrajectoryMsg(trajectory_msg);
- trajectory_processing::IterativeParabolicTimeParameterization iptp;
- iptp.computeTimeStamps(rt, 0.05, 0.05);
- rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
-
- robot_state::RobotStatePtr pre_approach_state(new robot_state::RobotState(res.trajectory_->getLastWayPoint()));
- res.trajectory_->setRobotTrajectoryMsg( *pre_approach_state, trajectory_msg);*/
-
// 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_;
@@ -138,22 +119,28 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const
return false;
}
+planning_interface::MotionPlanRequest &
+PlanStage::configureVelocity(planning_interface::MotionPlanRequest &req) const
+{
+ if (!ros::param::has("max_grasp_transition_velocity"))
+ {
+ ros::param::set("max_grasp_transition_velocity", 0.5);
+ req.max_velocity_scaling_factor = 0.5;
+ }
+ else
+ {
+ ros::param::get("max_grasp_transition_velocity", req.max_velocity_scaling_factor);
+ }
- planning_interface::MotionPlanRequest &
- PlanStage::configureVelocity(planning_interface::MotionPlanRequest &req) const {
- if(!ros::param::has("max_grasp_transition_velocity")){
- ros::param::set("max_grasp_transition_velocity", 0.5);
- req.max_velocity_scaling_factor = 0.5;
- }else{
- ros::param::get("max_grasp_transition_velocity", req.max_velocity_scaling_factor);
- }
-
- if(!ros::param::has("max_grasp_transition_acceleration")){
- ros::param::set("max_grasp_transition_acceleration", 0.5);
- req.max_velocity_scaling_factor = 0.5;
- }else{
- ros::param::get("max_grasp_transition_acceleration", req.max_acceleration_scaling_factor);
- }
- return req;
- }
+ if (!ros::param::has("max_grasp_transition_acceleration"))
+ {
+ ros::param::set("max_grasp_transition_acceleration", 0.5);
+ req.max_velocity_scaling_factor = 0.5;
+ }
+ else
+ {
+ ros::param::get("max_grasp_transition_acceleration", req.max_acceleration_scaling_factor);
+ }
+ return req;
+}
} // namespace pick_place
diff --git a/pick_place/src/reachable_valid_pose_filter.cpp b/pick_place/src/reachable_valid_pose_filter.cpp
index 36221b8..e30e585 100644
--- a/pick_place/src/reachable_valid_pose_filter.cpp
+++ b/pick_place/src/reachable_valid_pose_filter.cpp
@@ -37,7 +37,7 @@
#include <moveit/pick_place/reachable_valid_pose_filter.h>
#include <moveit/kinematic_constraints/utils.h>
#include <tf2_eigen/tf2_eigen.h>
-#include <boost/bind.hpp>
+#include <functional>
#include <ros/console.h>
pick_place::ReachableAndValidPoseFilter::ReachableAndValidPoseFilter(
@@ -55,8 +55,8 @@ namespace
{
bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene,
const collision_detection::AllowedCollisionMatrix* collision_matrix, bool verbose,
- const pick_place::ManipulationPlan* manipulation_plan, robot_state::RobotState* state,
- const robot_model::JointModelGroup* group, const double* joint_group_variable_values)
+ 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;
@@ -90,7 +90,7 @@ bool isStateCollisionFree(const planning_scene::PlanningScene* planning_scene,
} // namespace
bool pick_place::ReachableAndValidPoseFilter::isEndEffectorFree(const ManipulationPlanPtr& plan,
- robot_state::RobotState& token_state) const
+ moveit::core::RobotState& token_state) const
{
tf2::fromMsg(plan->goal_pose_.pose, plan->transformed_goal_pose_);
plan->transformed_goal_pose_ =
@@ -107,14 +107,14 @@ bool pick_place::ReachableAndValidPoseFilter::isEndEffectorFree(const Manipulati
bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr& plan) const
{
// initialize with scene state
- robot_state::RobotStatePtr token_state(new robot_state::RobotState(planning_scene_->getCurrentState()));
+ moveit::core::RobotStatePtr token_state(new moveit::core::RobotState(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 (!robot_state::Transforms::sameFrame(planning_scene_->getPlanningFrame(), plan->goal_pose_.header.frame_id))
+ 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();
@@ -132,8 +132,9 @@ bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr
constraints_sampler_manager_->selectSampler(planning_scene_, planning_group, plan->goal_constraints_);
if (plan->goal_sampler_)
{
- plan->goal_sampler_->setGroupStateValidityCallback(boost::bind(
- &isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, plan.get(), _1, _2, _3));
+ plan->goal_sampler_->setGroupStateValidityCallback(
+ std::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, plan.get(),
+ std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
plan->goal_sampler_->setVerbose(verbose_);
if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_))
{
--
GitLab