ROS 2 Migration (#170)

This commit is contained in:
Henning Kayser 2021-11-26 12:59:31 +01:00 committed by GitHub
commit 98ced788c8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
144 changed files with 2910 additions and 1791 deletions

View File

@ -14,32 +14,36 @@ jobs:
fail-fast: false fail-fast: false
matrix: matrix:
env: env:
- IMAGE: melodic-source # TODO: We have to use -Wno-redundant-decls since rosidl_generator_c is generating broken headers
- IMAGE: galactic-source
NAME: ccov NAME: ccov
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
- IMAGE: master-source - IMAGE: rolling-source
CXXFLAGS: >- CXXFLAGS: >-
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wno-redundant-decls
-Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter
- IMAGE: noetic-source - IMAGE: rolling-source
CXX: clang++ CXX: clang++
CLANG_TIDY: true CLANG_TIDY: true
CXXFLAGS: >- CXXFLAGS: >-
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wno-redundant-decls
-Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy
- IMAGE: noetic-source # Add fast_unwind_on_malloc=0 to fix stacktraces being too short or do not make sense
# see https://github.com/google/sanitizers/wiki/AddressSanitizer
- IMAGE: rolling-source
NAME: asan NAME: asan
DOCKER_RUN_OPTS: >- DOCKER_RUN_OPTS: >-
-e PRELOAD=libasan.so.5 -e PRELOAD=libasan.so.5
-e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions" -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0"
TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1" TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1"
env: env:
CATKIN_LINT: true
CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }} DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }}
UNDERLAY: /root/ws_moveit/install UNDERLAY: /root/ws_moveit/install
DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master" # TODO: Port to ROS2
# DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
UPSTREAM_WORKSPACE: .repos
CCACHE_DIR: ${{ github.workspace }}/.ccache CCACHE_DIR: ${{ github.workspace }}/.ccache
BASEDIR: ${{ github.workspace }}/.work BASEDIR: ${{ github.workspace }}/.work
CACHE_PREFIX: "${{ matrix.env.IMAGE }}${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && '-ccov' || '' }}" CACHE_PREFIX: "${{ matrix.env.IMAGE }}${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && '-ccov' || '' }}"
@ -48,7 +52,7 @@ jobs:
CC: ${{ matrix.env.CLANG_TIDY && 'clang' }} CC: ${{ matrix.env.CLANG_TIDY && 'clang' }}
CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }} CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }}
name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}" name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}"
runs-on: ubuntu-latest runs-on: ubuntu-latest
steps: steps:
- uses: actions/checkout@v2 - uses: actions/checkout@v2

View File

@ -17,9 +17,6 @@ jobs:
- uses: actions/setup-python@v2 - uses: actions/setup-python@v2
- name: Install clang-format-10 - name: Install clang-format-10
run: sudo apt-get install clang-format-10 run: sudo apt-get install clang-format-10
- uses: rhaschke/install-catkin_lint-action@v1.0
with:
distro: noetic
- uses: pre-commit/action@v2.0.0 - uses: pre-commit/action@v2.0.0
id: precommit id: precommit
- name: Upload pre-commit changes - name: Upload pre-commit changes

View File

@ -42,10 +42,3 @@ repos:
language: system language: system
files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
args: ['-fallback-style=none', '-i'] args: ['-fallback-style=none', '-i']
- id: catkin_lint
name: catkin_lint
description: Check package.xml and cmake files
entry: catkin_lint .
language: system
always_run: true
pass_filenames: false

5
.repos Normal file
View File

@ -0,0 +1,5 @@
repositories:
rosparam_shortcuts:
type: git
url: https://github.com/PickNikRobotics/rosparam_shortcuts
version: ros2

View File

@ -1,40 +1,38 @@
cmake_minimum_required(VERSION 3.1.3) cmake_minimum_required(VERSION 3.5)
project(moveit_task_constructor_capabilities) project(moveit_task_constructor_capabilities)
add_compile_options(-std=c++14) add_compile_options(-std=c++17)
find_package(catkin REQUIRED COMPONENTS find_package(ament_cmake REQUIRED)
actionlib
moveit_core
moveit_ros_move_group
moveit_task_constructor_core
moveit_task_constructor_msgs
pluginlib
std_msgs
)
catkin_package( find_package(Boost REQUIRED)
LIBRARIES $PROJECT_NAME} find_package(rclcpp_action REQUIRED)
CATKIN_DEPENDS find_package(moveit_core REQUIRED)
actionlib find_package(moveit_ros_move_group REQUIRED)
moveit_task_constructor_msgs find_package(moveit_task_constructor_msgs REQUIRED)
std_msgs find_package(pluginlib REQUIRED)
) find_package(std_msgs REQUIRED)
include_directories( add_library(${PROJECT_NAME} SHARED
${catkin_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME}
src/execute_task_solution_capability.cpp src/execute_task_solution_capability.cpp
) )
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) ament_target_dependencies(${PROJECT_NAME}
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) Boost
rclcpp_action
moveit_core
moveit_ros_move_group
moveit_task_constructor_msgs
)
install(TARGETS ${PROJECT_NAME} install(TARGETS ${PROJECT_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION lib
) )
install(FILES capabilities_plugin_description.xml pluginlib_export_plugin_description_file(moveit_ros_move_group capabilities_plugin_description.xml)
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ament_export_dependencies(rclcpp_action)
) ament_export_dependencies(moveit_core)
ament_export_dependencies(moveit_ros_move_group)
ament_export_dependencies(moveit_task_constructor_msgs)
ament_export_dependencies(pluginlib)
ament_export_dependencies(std_msgs)
ament_package()

View File

@ -1,4 +1,4 @@
<library path="libmoveit_task_constructor_capabilities"> <library path="moveit_task_constructor_capabilities">
<class name="move_group/ExecuteTaskSolutionCapability" type="move_group::ExecuteTaskSolutionCapability" base_class_type="move_group::MoveGroupCapability"> <class name="move_group/ExecuteTaskSolutionCapability" type="move_group::ExecuteTaskSolutionCapability" base_class_type="move_group::MoveGroupCapability">
<description> <description>
Action server to execute solutions generated through the MoveIt Task Constructor. Action server to execute solutions generated through the MoveIt Task Constructor.

View File

@ -10,17 +10,18 @@
<license>BSD</license> <license>BSD</license>
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
<depend>moveit_core</depend> <depend>moveit_core</depend>
<depend>moveit_ros_move_group</depend> <depend>moveit_ros_move_group</depend>
<depend>actionlib</depend> <depend>moveit_ros_planning</depend>
<depend>rclcpp_action</depend>
<depend>pluginlib</depend> <depend>pluginlib</depend>
<depend>std_msgs</depend> <depend>std_msgs</depend>
<depend>moveit_task_constructor_core</depend> <depend>moveit_task_constructor_core</depend>
<depend>moveit_task_constructor_msgs</depend> <depend>moveit_task_constructor_msgs</depend>
<export> <export>
<moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/> <build_type>ament_cmake</build_type>
</export> </export>
</package> </package>

View File

@ -36,16 +36,16 @@
#include "execute_task_solution_capability.h" #include "execute_task_solution_capability.h"
#include <moveit/task_constructor/moveit_compat.h> #include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/plan_execution/plan_execution.h> #include <moveit/plan_execution/plan_execution.h>
#include <moveit/trajectory_processing/trajectory_tools.h> #include <moveit/trajectory_processing/trajectory_tools.h>
#include <moveit/kinematic_constraints/utils.h> #include <moveit/kinematic_constraints/utils.h>
#include <moveit/move_group/capability_names.h> #include <moveit/move_group/capability_names.h>
#include <moveit/robot_state/conversions.h> #include <moveit/robot_state/conversions.h>
#if MOVEIT_HAS_MESSAGE_CHECKS
#include <moveit/utils/message_checks.h> #include <moveit/utils/message_checks.h>
#endif #include <moveit/moveit_cpp/moveit_cpp.h>
#include <boost/algorithm/string/join.hpp>
namespace { namespace {
@ -80,58 +80,63 @@ const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::Rob
} }
} // namespace } // namespace
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.execute_task_solution");
namespace move_group { namespace move_group {
ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability() : MoveGroupCapability("ExecuteTaskSolution") {} ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability() : MoveGroupCapability("ExecuteTaskSolution") {}
void ExecuteTaskSolutionCapability::initialize() { void ExecuteTaskSolutionCapability::initialize() {
// configure the action server // configure the action server
as_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>( as_ = rclcpp_action::create_server<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
root_node_handle_, "execute_task_solution", context_->moveit_cpp_->getNode(), "execute_task_solution",
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1), false)); ActionServerType::GoalCallback(std::bind(&ExecuteTaskSolutionCapability::handleNewGoal, this,
as_->registerPreemptCallback(std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this)); std::placeholders::_1, std::placeholders::_2)),
as_->start(); ActionServerType::CancelCallback(
std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)),
ActionServerType::AcceptedCallback(
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1)));
} }
void ExecuteTaskSolutionCapability::goalCallback( void ExecuteTaskSolutionCapability::goalCallback(
const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal) { const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle) {
moveit_task_constructor_msgs::ExecuteTaskSolutionResult result; auto result = std::make_shared<moveit_task_constructor_msgs::action::ExecuteTaskSolution::Result>();
const auto& goal = goal_handle->get_goal();
if (!context_->plan_execution_) { if (!context_->plan_execution_) {
const std::string response = "Cannot execute solution. ~allow_trajectory_execution was set to false"; result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED;
result.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED; goal_handle->abort(result);
as_->setAborted(result, response);
return; return;
} }
plan_execution::ExecutableMotionPlan plan; plan_execution::ExecutableMotionPlan plan;
if (!constructMotionPlan(goal->solution, plan)) if (!constructMotionPlan(goal->solution, plan))
result.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
else { else {
ROS_INFO_NAMED("ExecuteTaskSolution", "Executing TaskSolution"); RCLCPP_INFO(LOGGER, "Executing TaskSolution");
result.error_code = context_->plan_execution_->executeAndMonitor(plan); result->error_code = context_->plan_execution_->executeAndMonitor(plan);
} }
const std::string response = context_->plan_execution_->getErrorCodeString(result.error_code); if (result->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
goal_handle->succeed(result);
if (result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) else if (result->error_code.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED)
as_->setSucceeded(result, response); goal_handle->canceled(result);
else if (result.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
as_->setPreempted(result, response);
else else
as_->setAborted(result, response); goal_handle->abort(result);
} }
void ExecuteTaskSolutionCapability::preemptCallback() { rclcpp_action::CancelResponse ExecuteTaskSolutionCapability::preemptCallback(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle) {
if (context_->plan_execution_) if (context_->plan_execution_)
context_->plan_execution_->stop(); context_->plan_execution_->stop();
return rclcpp_action::CancelResponse::ACCEPT;
} }
bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution,
plan_execution::ExecutableMotionPlan& plan) { plan_execution::ExecutableMotionPlan& plan) {
robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel(); moveit::core::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
robot_state::RobotState state(model); moveit::core::RobotState state(model);
{ {
planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_); planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_);
state = scene->getCurrentState(); state = scene->getCurrentState();
@ -139,7 +144,7 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
plan.plan_components_.reserve(solution.sub_trajectory.size()); plan.plan_components_.reserve(solution.sub_trajectory.size());
for (size_t i = 0; i < solution.sub_trajectory.size(); ++i) { for (size_t i = 0; i < solution.sub_trajectory.size(); ++i) {
const moveit_task_constructor_msgs::SubTrajectory& sub_traj = solution.sub_trajectory[i]; const moveit_task_constructor_msgs::msg::SubTrajectory& sub_traj = solution.sub_trajectory[i];
plan.plan_components_.emplace_back(); plan.plan_components_.emplace_back();
plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back(); plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back();
@ -156,12 +161,11 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
if (!joint_names.empty()) { if (!joint_names.empty()) {
group = findJointModelGroup(*model, joint_names); group = findJointModelGroup(*model, joint_names);
if (!group) { if (!group) {
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {" RCLCPP_ERROR_STREAM(LOGGER, "Could not find JointModelGroup that actuates {"
<< boost::algorithm::join(joint_names, ", ") << "}"); << boost::algorithm::join(joint_names, ", ") << "}");
return false; return false;
} }
ROS_DEBUG_NAMED("ExecuteTaskSolution", "Using JointModelGroup '%s' for execution", RCLCPP_DEBUG(LOGGER, "Using JointModelGroup '%s' for execution", group->getName().c_str());
group->getName().c_str());
} }
} }
exec_traj.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(model, group); exec_traj.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(model, group);
@ -170,25 +174,16 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
/* TODO add action feedback and markers */ /* TODO add action feedback and markers */
exec_traj.effect_on_success_ = [this, sub_traj, exec_traj.effect_on_success_ = [this, sub_traj,
description](const plan_execution::ExecutableMotionPlan* /*plan*/) { description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
#if MOVEIT_HAS_MESSAGE_CHECKS
if (!moveit::core::isEmpty(sub_traj.scene_diff)) { if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
#else RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)) {
#endif
ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff); return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
} }
return true; return true;
}; };
#if MOVEIT_HAS_MESSAGE_CHECKS
if (!moveit::core::isEmpty(sub_traj.scene_diff.robot_state) && if (!moveit::core::isEmpty(sub_traj.scene_diff.robot_state) &&
#else
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&
#endif
!moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)) { !moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)) {
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", RCLCPP_ERROR_STREAM(LOGGER, "invalid intermediate robot state in scene diff of SubTrajectory " << description);
"invalid intermediate robot state in scene diff of SubTrajectory " << description);
return false; return false;
} }
} }

View File

@ -42,9 +42,9 @@
#pragma once #pragma once
#include <moveit/move_group/move_group_capability.h> #include <moveit/move_group/move_group_capability.h>
#include <actionlib/server/simple_action_server.h> #include <rclcpp_action/rclcpp_action.hpp>
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h> #include <moveit_task_constructor_msgs/action/execute_task_solution.hpp>
#include <memory> #include <memory>
@ -58,13 +58,23 @@ public:
void initialize() override; void initialize() override;
private: private:
bool constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, using ExecuteTaskSolutionAction = moveit_task_constructor_msgs::action::ExecuteTaskSolution;
using ActionServerType = rclcpp_action::Server<ExecuteTaskSolutionAction>;
bool constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution,
plan_execution::ExecutableMotionPlan& plan); plan_execution::ExecutableMotionPlan& plan);
void goalCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal); void goalCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle);
void preemptCallback();
std::unique_ptr<actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>> as_; rclcpp_action::CancelResponse
preemptCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle);
/** Always accept the goal */
rclcpp_action::GoalResponse handleNewGoal(const rclcpp_action::GoalUUID& uuid,
const ExecuteTaskSolutionAction::Goal::ConstSharedPtr goal) const {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
ActionServerType::SharedPtr as_;
}; };
} // namespace move_group } // namespace move_group

View File

@ -1,34 +1,22 @@
cmake_minimum_required(VERSION 3.1.3) cmake_minimum_required(VERSION 3.5)
project(moveit_task_constructor_core) project(moveit_task_constructor_core)
find_package(ament_cmake REQUIRED)
find_package(Boost REQUIRED) find_package(Boost REQUIRED)
find_package(catkin REQUIRED COMPONENTS find_package(geometry_msgs REQUIRED)
tf2_eigen find_package(moveit_core REQUIRED)
geometry_msgs find_package(moveit_ros_planning REQUIRED)
moveit_core find_package(moveit_ros_planning_interface REQUIRED)
moveit_ros_planning find_package(moveit_task_constructor_msgs REQUIRED)
moveit_ros_planning_interface find_package(rclcpp REQUIRED)
moveit_task_constructor_msgs find_package(rviz_marker_tools REQUIRED)
roscpp find_package(tf2_eigen REQUIRED)
visualization_msgs find_package(visualization_msgs REQUIRED)
rviz_marker_tools
)
catkin_package( if(NOT CMAKE_CXX_STANDARD)
LIBRARIES set(CMAKE_CXX_STANDARD 17)
${PROJECT_NAME} endif()
${PROJECT_NAME}_stages
${PROJECT_NAME}_stage_plugins
INCLUDE_DIRS
include
CATKIN_DEPENDS
geometry_msgs
moveit_core
moveit_task_constructor_msgs
visualization_msgs
)
set(CMAKE_CXX_STANDARD 14)
add_compile_options(-fvisibility-inlines-hidden) add_compile_options(-fvisibility-inlines-hidden)
@ -37,8 +25,21 @@ set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)
add_subdirectory(src) add_subdirectory(src)
add_subdirectory(test) add_subdirectory(test)
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} install(DIRECTORY include/ DESTINATION include
PATTERN "*_p.h" EXCLUDE) PATTERN "*_p.h" EXCLUDE)
install(FILES
motion_planning_stages_plugin_description.xml pluginlib_export_plugin_description_file(${PROJECT_NAME} motion_planning_stages_plugin_description.xml)
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME} ${PROJECT_NAME}_stages)
ament_export_dependencies(Boost)
ament_export_dependencies(geometry_msgs)
ament_export_dependencies(moveit_core)
ament_export_dependencies(moveit_ros_planning)
ament_export_dependencies(moveit_ros_planning_interface)
ament_export_dependencies(moveit_task_constructor_msgs)
ament_export_dependencies(rclcpp)
ament_export_dependencies(rviz_marker_tools)
ament_export_dependencies(tf2_eigen)
ament_export_dependencies(visualization_msgs)
ament_package()

View File

@ -39,10 +39,10 @@
#pragma once #pragma once
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit_task_constructor_msgs/TaskDescription.h> #include <moveit_task_constructor_msgs/msg/task_description.hpp>
#include <moveit_task_constructor_msgs/TaskStatistics.h> #include <moveit_task_constructor_msgs/msg/task_statistics.hpp>
#include <moveit_task_constructor_msgs/Solution.h> #include <moveit_task_constructor_msgs/msg/solution.hpp>
#include <moveit_task_constructor_msgs/GetSolution.h> #include <moveit_task_constructor_msgs/srv/get_solution.hpp>
#define DESCRIPTION_TOPIC "description" #define DESCRIPTION_TOPIC "description"
#define STATISTICS_TOPIC "statistics" #define STATISTICS_TOPIC "statistics"
@ -72,13 +72,14 @@ public:
~Introspection(); ~Introspection();
/// fill task description message for publishing the task configuration /// fill task description message for publishing the task configuration
moveit_task_constructor_msgs::TaskDescription& moveit_task_constructor_msgs::msg::TaskDescription&
fillTaskDescription(moveit_task_constructor_msgs::TaskDescription& msg); fillTaskDescription(moveit_task_constructor_msgs::msg::TaskDescription& msg);
/// publish detailed task description /// publish detailed task description
void publishTaskDescription(); void publishTaskDescription();
/// fill task state message for publishing the current task state /// fill task state message for publishing the current task state
moveit_task_constructor_msgs::TaskStatistics& fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics& msg); moveit_task_constructor_msgs::msg::TaskStatistics&
fillTaskStatistics(moveit_task_constructor_msgs::msg::TaskStatistics& msg);
/// publish the current state of task /// publish the current state of task
void publishTaskState(); void publishTaskState();
@ -95,8 +96,8 @@ public:
void publishAllSolutions(bool wait = true); void publishAllSolutions(bool wait = true);
/// get solution /// get solution
bool getSolution(moveit_task_constructor_msgs::GetSolution::Request& req, bool getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr req,
moveit_task_constructor_msgs::GetSolution::Response& res); const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr res);
/// retrieve id of given stage /// retrieve id of given stage
uint32_t stageId(const moveit::task_constructor::Stage* const s) const; uint32_t stageId(const moveit::task_constructor::Stage* const s) const;
@ -105,8 +106,8 @@ public:
uint32_t solutionId(const moveit::task_constructor::SolutionBase& s); uint32_t solutionId(const moveit::task_constructor::SolutionBase& s);
private: private:
void fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::StageStatistics& s); void fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::msg::StageStatistics& s);
void fillSolution(moveit_task_constructor_msgs::Solution& msg, const SolutionBase& s); void fillSolution(moveit_task_constructor_msgs::msg::Solution& msg, const SolutionBase& s);
/// retrieve or set id of given stage /// retrieve or set id of given stage
uint32_t stageId(const moveit::task_constructor::Stage* const s); uint32_t stageId(const moveit::task_constructor::Stage* const s);
/// retrieve solution with given id /// retrieve solution with given id

View File

@ -1,7 +1,7 @@
#pragma once #pragma once
#include <rviz_marker_tools/marker_creation.h> #include <rviz_marker_tools/marker_creation.h>
#include <visualization_msgs/Marker.h> #include <visualization_msgs/msg/marker.hpp>
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <functional> #include <functional>
@ -19,7 +19,7 @@ namespace moveit {
namespace task_constructor { namespace task_constructor {
/** signature of callback function, passing the generated marker and the name of the robot link / scene object */ /** signature of callback function, passing the generated marker and the name of the robot link / scene object */
using MarkerCallback = std::function<void(visualization_msgs::Marker&, const std::string&)>; using MarkerCallback = std::function<void(visualization_msgs::msg::Marker&, const std::string&)>;
/** generate marker msgs to visualize the planning scene, calling the given callback for each of them /** generate marker msgs to visualize the planning scene, calling the given callback for each of them
* object_names: set of links to include (or all if empty) */ * object_names: set of links to include (or all if empty) */

View File

@ -44,16 +44,7 @@
(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \ (MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
major * 1'000'000 + minor * 1'000 + patch) major * 1'000'000 + minor * 1'000 + patch)
// use collision env instead of collision robot/world
#define MOVEIT_HAS_COLLISION_ENV MOVEIT_VERSION_GE(1, 1, 0)
// cartesian interpolator got separated from RobotState at some point
#define MOVEIT_HAS_CARTESIAN_INTERPOLATOR MOVEIT_VERSION_GE(1, 1, 0)
// isEmpty got split off into its own header
#define MOVEIT_HAS_MESSAGE_CHECKS MOVEIT_VERSION_GE(1, 1, 0)
// use object shape poses relative to a single object pose // use object shape poses relative to a single object pose
#define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6) #define MOVEIT_HAS_OBJECT_POSE 1
#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6) #define MOVEIT_HAS_STATE_RIGID_PARENT_LINK 0

View File

@ -45,7 +45,8 @@
#include <vector> #include <vector>
#include <functional> #include <functional>
#include <sstream> #include <sstream>
#include <ros/serialization.h> #include <rclcpp/serialization.hpp>
#include <rosidl_runtime_cpp/traits.hpp>
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
@ -210,12 +211,12 @@ public:
PropertySerializer() { insert(typeid(T), typeName<T>(), &serialize, &deserialize); } PropertySerializer() { insert(typeid(T), typeName<T>(), &serialize, &deserialize); }
template <class Q = T> template <class Q = T>
static typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() { static typename std::enable_if<rosidl_generator_traits::is_message<Q>::value, std::string>::type typeName() {
return ros::message_traits::DataType<T>::value(); return rosidl_generator_traits::data_type<T>();
} }
template <class Q = T> template <class Q = T>
static typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() { static typename std::enable_if<!rosidl_generator_traits::is_message<Q>::value, std::string>::type typeName() {
return typeid(T).name(); return typeid(T).name();
} }

View File

@ -63,12 +63,12 @@ public:
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
}; };
} // namespace solvers } // namespace solvers
} // namespace task_constructor } // namespace task_constructor

View File

@ -60,12 +60,12 @@ public:
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
}; };
} // namespace solvers } // namespace solvers
} // namespace task_constructor } // namespace task_constructor

View File

@ -39,6 +39,7 @@
#pragma once #pragma once
#include <moveit/task_constructor/solvers/planner_interface.h> #include <moveit/task_constructor/solvers/planner_interface.h>
#include <rclcpp/node.hpp>
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
namespace planning_pipeline { namespace planning_pipeline {
@ -58,20 +59,25 @@ public:
struct Specification struct Specification
{ {
moveit::core::RobotModelConstPtr model; moveit::core::RobotModelConstPtr model;
std::string ns{ "move_group" }; std::string ns{ "ompl" };
std::string pipeline{ "ompl" }; std::string pipeline{ "ompl" };
std::string adapter_param{ "request_adapters" }; std::string adapter_param{ "request_adapters" };
}; };
static planning_pipeline::PlanningPipelinePtr create(const moveit::core::RobotModelConstPtr& model) { static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node,
const moveit::core::RobotModelConstPtr& model) {
Specification spec; Specification spec;
spec.model = model; spec.model = model;
return create(spec); return create(node, spec);
} }
static planning_pipeline::PlanningPipelinePtr create(const Specification& spec); static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec);
PipelinePlanner(const std::string& pipeline = "ompl"); /**
*
* @param node used to load the parameters for the planning pipeline
*/
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl");
PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline); PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);
@ -81,16 +87,17 @@ public:
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
protected: protected:
std::string pipeline_name_; std::string pipeline_name_;
planning_pipeline::PlanningPipelinePtr planner_; planning_pipeline::PlanningPipelinePtr planner_;
rclcpp::Node::SharedPtr node_;
}; };
} // namespace solvers } // namespace solvers
} // namespace task_constructor } // namespace task_constructor

View File

@ -39,7 +39,7 @@
#pragma once #pragma once
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit_msgs/Constraints.h> #include <moveit_msgs/msg/constraints.hpp>
#include <moveit/task_constructor/properties.h> #include <moveit/task_constructor/properties.h>
#include <Eigen/Geometry> #include <Eigen/Geometry>
@ -82,13 +82,13 @@ public:
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
/// plan trajectory from current robot state to Cartesian target /// plan trajectory from current robot state to Cartesian target
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
}; };
} // namespace solvers } // namespace solvers
} // namespace task_constructor } // namespace task_constructor

View File

@ -43,7 +43,7 @@
#include <moveit/task_constructor/cost_terms.h> #include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/cost_queue.h> #include <moveit/task_constructor/cost_queue.h>
#include <ros/console.h> #include <rclcpp/rclcpp.hpp>
#include <ostream> #include <ostream>
#include <chrono> #include <chrono>
@ -150,7 +150,7 @@ public:
void newSolution(const SolutionBasePtr& solution); void newSolution(const SolutionBasePtr& solution);
bool storeFailures() const { return introspection_ != nullptr; } bool storeFailures() const { return introspection_ != nullptr; }
void runCompute() { void runCompute() {
ROS_DEBUG_STREAM_NAMED("Stage", "Computing stage '" << name() << "'"); RCLCPP_DEBUG_STREAM(LOGGER, "Computing stage '" << name() << "'");
auto compute_start_time = std::chrono::steady_clock::now(); auto compute_start_time = std::chrono::steady_clock::now();
try { try {
compute(); compute();
@ -200,6 +200,7 @@ private:
InterfaceWeakPtr next_starts_; // interface to be used for sendForward() InterfaceWeakPtr next_starts_; // interface to be used for sendForward()
Introspection* introspection_; // task's introspection instance Introspection* introspection_; // task's introspection instance
inline static const rclcpp::Logger LOGGER = rclcpp::get_logger("stage");
}; };
PIMPL_FUNCTIONS(Stage) PIMPL_FUNCTIONS(Stage)
std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);

View File

@ -38,7 +38,7 @@
#include <moveit/task_constructor/container.h> #include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/cost_queue.h> #include <moveit/task_constructor/cost_queue.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/msg/pose_stamped.hpp>
#include <Eigen/Geometry> #include <Eigen/Geometry>
namespace moveit { namespace moveit {
@ -82,7 +82,7 @@ public:
void setGroup(const std::string& group) { setProperty("group", group); } void setGroup(const std::string& group) { setProperty("group", group); }
/// setters for IK frame /// setters for IK frame
void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); } void setIKFrame(const geometry_msgs::msg::PoseStamped& pose) { setProperty("ik_frame", pose); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T> template <typename T>
void setIKFrame(const T& p, const std::string& link) { void setIKFrame(const T& p, const std::string& link) {
@ -97,7 +97,7 @@ public:
* This property should almost always be set in the InterfaceState sent by the child. * This property should almost always be set in the InterfaceState sent by the child.
* If possible, avoid setting it manually. * If possible, avoid setting it manually.
*/ */
void setTargetPose(const geometry_msgs::PoseStamped& pose) { setProperty("target_pose", pose); } void setTargetPose(const geometry_msgs::msg::PoseStamped& pose) { setProperty("target_pose", pose); }
void setTargetPose(const Eigen::Isometry3d& pose, const std::string& frame = ""); void setTargetPose(const Eigen::Isometry3d& pose, const std::string& frame = "");
template <typename T> template <typename T>
void setTargetPose(const T& p, const std::string& frame = "") { void setTargetPose(const T& p, const std::string& frame = "") {

View File

@ -41,7 +41,7 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/solvers/planner_interface.h> #include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit_msgs/Constraints.h> #include <moveit_msgs/msg/constraints.hpp>
namespace moveit { namespace moveit {
namespace core { namespace core {
@ -76,7 +76,7 @@ public:
using GroupPlannerVector = std::vector<std::pair<std::string, solvers::PlannerInterfacePtr> >; using GroupPlannerVector = std::vector<std::pair<std::string, solvers::PlannerInterfacePtr> >;
Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {}); Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {});
void setPathConstraints(moveit_msgs::Constraints path_constraints) { void setPathConstraints(moveit_msgs::msg::Constraints path_constraints) {
setProperty("path_constraints", std::move(path_constraints)); setProperty("path_constraints", std::move(path_constraints));
} }

View File

@ -40,7 +40,7 @@
#include <moveit/task_constructor/storage.h> #include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <geometry_msgs/Vector3.h> #include <geometry_msgs/msg/vector3.hpp>
#include <moveit/collision_detection/collision_common.h> #include <moveit/collision_detection/collision_common.h>
namespace moveit { namespace moveit {
@ -55,7 +55,7 @@ public:
void computeForward(const InterfaceState& from) override; void computeForward(const InterfaceState& from) override;
void computeBackward(const InterfaceState& to) override; void computeBackward(const InterfaceState& to) override;
void setDirection(const geometry_msgs::Vector3& dir) { setProperty("direction", dir); } void setDirection(const geometry_msgs::msg::Vector3& dir) { setProperty("direction", dir); }
void setMaxPenetration(double penetration) { setProperty("max_penetration", penetration); } void setMaxPenetration(double penetration) { setProperty("max_penetration", penetration); }
private: private:

View File

@ -40,7 +40,7 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/cost_queue.h> #include <moveit/task_constructor/cost_queue.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/msg/pose_stamped.hpp>
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
@ -55,7 +55,7 @@ public:
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
void addPose(const geometry_msgs::PoseStamped& pose); void addPose(const geometry_msgs::msg::PoseStamped& pose);
protected: protected:
void onNewSolution(const SolutionBase& s) override; void onNewSolution(const SolutionBase& s) override;

View File

@ -57,9 +57,9 @@ public:
void setAngleDelta(double delta) { setProperty("angle_delta", delta); } void setAngleDelta(double delta) { setProperty("angle_delta", delta); }
void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); } void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); }
void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); } void setPreGraspPose(const moveit_msgs::msg::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); }
void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); } void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); }
void setGraspPose(const moveit_msgs::RobotState& grasp) { properties().set("grasp", grasp); } void setGraspPose(const moveit_msgs::msg::RobotState& grasp) { properties().set("grasp", grasp); }
protected: protected:
void onNewSolution(const SolutionBase& s) override; void onNewSolution(const SolutionBase& s) override;

View File

@ -40,7 +40,7 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/cost_queue.h> #include <moveit/task_constructor/cost_queue.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/msg/pose_stamped.hpp>
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
@ -55,7 +55,7 @@ public:
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
void setPose(const geometry_msgs::PoseStamped& pose) { setProperty("pose", pose); } void setPose(const geometry_msgs::msg::PoseStamped& pose) { setProperty("pose", pose); }
protected: protected:
void onNewSolution(const SolutionBase& s) override; void onNewSolution(const SolutionBase& s) override;

View File

@ -41,7 +41,7 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/properties.h> #include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/type_traits.h> #include <moveit/task_constructor/type_traits.h>
#include <moveit_msgs/CollisionObject.h> #include <moveit_msgs/msg/collision_object.hpp>
#include <map> #include <map>
namespace moveit { namespace moveit {
@ -78,7 +78,7 @@ public:
/// attach or detach a list of objects to the given link /// attach or detach a list of objects to the given link
void attachObjects(const Names& objects, const std::string& attach_link, bool attach); void attachObjects(const Names& objects, const std::string& attach_link, bool attach);
/// Add an object to the planning scene /// Add an object to the planning scene
void addObject(const moveit_msgs::CollisionObject& collision_object); void addObject(const moveit_msgs::msg::CollisionObject& collision_object);
/// Remove an object from the planning scene /// Remove an object from the planning scene
void removeObject(const std::string& object_name); void removeObject(const std::string& object_name);
@ -135,7 +135,7 @@ protected:
// list of objects to attach (true) / detach (false) to a given link // list of objects to attach (true) / detach (false) to a given link
std::map<std::string, std::pair<Names, bool>> attach_objects_; std::map<std::string, std::pair<Names, bool>> attach_objects_;
// list of objects to add / remove to the planning scene // list of objects to add / remove to the planning scene
std::vector<moveit_msgs::CollisionObject> collision_objects_; std::vector<moveit_msgs::msg::CollisionObject> collision_objects_;
// list of objects to mutually // list of objects to mutually
struct CollisionMatrixPairs struct CollisionMatrixPairs
@ -150,7 +150,7 @@ protected:
protected: protected:
// apply stored modifications to scene // apply stored modifications to scene
InterfaceState apply(const InterfaceState& from, bool invert); InterfaceState apply(const InterfaceState& from, bool invert);
void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::CollisionObject& object); void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::msg::CollisionObject& object);
void attachObjects(planning_scene::PlanningScene& scene, const std::pair<std::string, std::pair<Names, bool>>& pair, void attachObjects(planning_scene::PlanningScene& scene, const std::pair<std::string, std::pair<Names, bool>>& pair,
bool invert); bool invert);
void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert); void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert);

View File

@ -40,9 +40,9 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/solvers/planner_interface.h> #include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit_msgs/Constraints.h> #include <moveit_msgs/msg/constraints.hpp>
#include <geometry_msgs/TwistStamped.h> #include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/Vector3Stamped.h> #include <geometry_msgs/msg/vector3_stamped.hpp>
namespace moveit { namespace moveit {
namespace core { namespace core {
@ -64,7 +64,7 @@ public:
void setGroup(const std::string& group) { setProperty("group", group); } void setGroup(const std::string& group) { setProperty("group", group); }
/// setters for IK frame /// setters for IK frame
void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); } void setIKFrame(const geometry_msgs::msg::PoseStamped& pose) { setProperty("ik_frame", pose); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T> template <typename T>
void setIKFrame(const T& p, const std::string& link) { void setIKFrame(const T& p, const std::string& link) {
@ -82,14 +82,14 @@ public:
setProperty("max_distance", max_distance); setProperty("max_distance", max_distance);
} }
void setPathConstraints(moveit_msgs::Constraints path_constraints) { void setPathConstraints(moveit_msgs::msg::Constraints path_constraints) {
setProperty("path_constraints", std::move(path_constraints)); setProperty("path_constraints", std::move(path_constraints));
} }
/// perform twist motion on specified link /// perform twist motion on specified link
void setDirection(const geometry_msgs::TwistStamped& twist) { setProperty("direction", twist); } void setDirection(const geometry_msgs::msg::TwistStamped& twist) { setProperty("direction", twist); }
/// translate link along given direction /// translate link along given direction
void setDirection(const geometry_msgs::Vector3Stamped& direction) { setProperty("direction", direction); } void setDirection(const geometry_msgs::msg::Vector3Stamped& direction) { setProperty("direction", direction); }
/// move specified joint variables by given amount /// move specified joint variables by given amount
void setDirection(const std::map<std::string, double>& joint_deltas) { setProperty("direction", joint_deltas); } void setDirection(const std::map<std::string, double>& joint_deltas) { setProperty("direction", joint_deltas); }

View File

@ -40,9 +40,9 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/solvers/planner_interface.h> #include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit_msgs/Constraints.h> #include <moveit_msgs/msg/constraints.hpp>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/PointStamped.h> #include <geometry_msgs/msg/point_stamped.hpp>
namespace moveit { namespace moveit {
namespace core { namespace core {
@ -63,7 +63,7 @@ public:
void setGroup(const std::string& group) { setProperty("group", group); } void setGroup(const std::string& group) { setProperty("group", group); }
/// setters for IK frame /// setters for IK frame
void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); } void setIKFrame(const geometry_msgs::msg::PoseStamped& pose) { setProperty("ik_frame", pose); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T> template <typename T>
void setIKFrame(const T& p, const std::string& link) { void setIKFrame(const T& p, const std::string& link) {
@ -74,21 +74,21 @@ public:
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
/// move link to given pose /// move link to given pose
void setGoal(const geometry_msgs::PoseStamped& pose) { setProperty("goal", pose); } void setGoal(const geometry_msgs::msg::PoseStamped& pose) { setProperty("goal", pose); }
/// move link to given point, keeping current orientation /// move link to given point, keeping current orientation
void setGoal(const geometry_msgs::PointStamped& point) { setProperty("goal", point); } void setGoal(const geometry_msgs::msg::PointStamped& point) { setProperty("goal", point); }
/// move joint model group to given named pose /// move joint model group to given named pose
void setGoal(const std::string& named_joint_pose) { setProperty("goal", named_joint_pose); } void setGoal(const std::string& named_joint_pose) { setProperty("goal", named_joint_pose); }
/// move joints specified in msg to their target values /// move joints specified in msg to their target values
void setGoal(const moveit_msgs::RobotState& robot_state) { setProperty("goal", robot_state); } void setGoal(const moveit_msgs::msg::RobotState& robot_state) { setProperty("goal", robot_state); }
/// move joints by name to their mapped target value /// move joints by name to their mapped target value
void setGoal(const std::map<std::string, double>& joints); void setGoal(const std::map<std::string, double>& joints);
void setPathConstraints(moveit_msgs::Constraints path_constraints) { void setPathConstraints(moveit_msgs::msg::Constraints path_constraints) {
setProperty("path_constraints", std::move(path_constraints)); setProperty("path_constraints", std::move(path_constraints));
} }

View File

@ -37,7 +37,7 @@
#include <moveit/task_constructor/container.h> #include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/cost_queue.h> #include <moveit/task_constructor/cost_queue.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/msg/pose_stamped.hpp>
#include <Eigen/Geometry> #include <Eigen/Geometry>
namespace moveit { namespace moveit {

View File

@ -38,7 +38,7 @@
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/container.h> #include <moveit/task_constructor/container.h>
#include <geometry_msgs/TwistStamped.h> #include <geometry_msgs/msg/twist_stamped.hpp>
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
@ -82,9 +82,9 @@ public:
solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; } solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; }
void setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); void setApproachRetract(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance);
void setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); void setLiftPlace(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance);
void setLiftPlace(const std::map<std::string, double>& joints); void setLiftPlace(const std::map<std::string, double>& joints);
}; };
@ -95,11 +95,11 @@ public:
Pick(Stage::pointer&& grasp_stage = Stage::pointer(), const std::string& name = "pick") Pick(Stage::pointer&& grasp_stage = Stage::pointer(), const std::string& name = "pick")
: PickPlaceBase(std::move(grasp_stage), name, true) {} : PickPlaceBase(std::move(grasp_stage), name, true) {}
void setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { void setApproachMotion(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance) {
setApproachRetract(motion, min_distance, max_distance); setApproachRetract(motion, min_distance, max_distance);
} }
void setLiftMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { void setLiftMotion(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance) {
setLiftPlace(motion, min_distance, max_distance); setLiftPlace(motion, min_distance, max_distance);
} }
void setLiftMotion(const std::map<std::string, double>& joints) { setLiftPlace(joints); } void setLiftMotion(const std::map<std::string, double>& joints) { setLiftPlace(joints); }
@ -112,11 +112,11 @@ public:
Place(Stage::pointer&& ungrasp_stage = Stage::pointer(), const std::string& name = "place") Place(Stage::pointer&& ungrasp_stage = Stage::pointer(), const std::string& name = "place")
: PickPlaceBase(std::move(ungrasp_stage), name, false) {} : PickPlaceBase(std::move(ungrasp_stage), name, false) {}
void setRetractMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { void setRetractMotion(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance) {
setApproachRetract(motion, min_distance, max_distance); setApproachRetract(motion, min_distance, max_distance);
} }
void setPlaceMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { void setPlaceMotion(const geometry_msgs::msg::TwistStamped& motion, double min_distance, double max_distance) {
setLiftPlace(motion, min_distance, max_distance); setLiftPlace(motion, min_distance, max_distance);
} }
void setPlaceMotion(const std::map<std::string, double>& joints) { setLiftPlace(joints); } void setPlaceMotion(const std::map<std::string, double>& joints) { setLiftPlace(joints); }

View File

@ -38,7 +38,7 @@
#include <moveit/task_constructor/container.h> #include <moveit/task_constructor/container.h>
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/msg/pose_stamped.hpp>
#include <Eigen/Geometry> #include <Eigen/Geometry>
namespace moveit { namespace moveit {
@ -78,7 +78,7 @@ public:
void setObject(const std::string& object) { setProperty("object", object); } void setObject(const std::string& object) { setProperty("object", object); }
/// set properties of IK solver /// set properties of IK solver
void setIKFrame(const geometry_msgs::PoseStamped& transform) { setProperty("ik_frame", transform); } void setIKFrame(const geometry_msgs::msg::PoseStamped& transform) { setProperty("ik_frame", transform); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T> template <typename T>
void setIKFrame(const T& t, const std::string& link) { void setIKFrame(const T& t, const std::string& link) {

View File

@ -42,14 +42,15 @@
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/properties.h> #include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/cost_queue.h> #include <moveit/task_constructor/cost_queue.h>
#include <moveit_task_constructor_msgs/Solution.h> #include <moveit_task_constructor_msgs/msg/solution.hpp>
#include <visualization_msgs/MarkerArray.h> #include <visualization_msgs/msg/marker_array.hpp>
#include <list> #include <list>
#include <vector> #include <vector>
#include <deque> #include <deque>
#include <cassert> #include <cassert>
#include <functional> #include <functional>
#include <cmath>
namespace planning_scene { namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene); MOVEIT_CLASS_FORWARD(PlanningScene);
@ -271,9 +272,9 @@ public:
const auto& markers() const { return markers_; } const auto& markers() const { return markers_; }
/// append this solution to Solution msg /// append this solution to Solution msg
virtual void fillMessage(moveit_task_constructor_msgs::Solution& solution, virtual void fillMessage(moveit_task_constructor_msgs::msg::Solution& solution,
Introspection* introspection = nullptr) const = 0; Introspection* introspection = nullptr) const = 0;
void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const; void fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& info, Introspection* introspection = nullptr) const;
/// required to dispatch to type-specific CostTerm methods via vtable /// required to dispatch to type-specific CostTerm methods via vtable
virtual double computeCost(const CostTerm& cost, std::string& comment) const = 0; virtual double computeCost(const CostTerm& cost, std::string& comment) const = 0;
@ -293,7 +294,7 @@ private:
// comment for this solution, e.g. explanation of failure // comment for this solution, e.g. explanation of failure
std::string comment_; std::string comment_;
// markers for this solution, e.g. target frame or collision indicators // markers for this solution, e.g. target frame or collision indicators
std::deque<visualization_msgs::Marker> markers_; std::deque<visualization_msgs::msg::Marker> markers_;
// begin and end InterfaceState of this solution/trajectory // begin and end InterfaceState of this solution/trajectory
const InterfaceState* start_ = nullptr; const InterfaceState* start_ = nullptr;
@ -313,7 +314,8 @@ public:
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; } void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; }
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override; void fillMessage(moveit_task_constructor_msgs::msg::Solution& msg,
Introspection* introspection = nullptr) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override; double computeCost(const CostTerm& cost, std::string& comment) const override;
@ -346,7 +348,7 @@ public:
void push_back(const SolutionBase& solution); void push_back(const SolutionBase& solution);
/// append all subsolutions to solution /// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; void fillMessage(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override; double computeCost(const CostTerm& cost, std::string& comment) const override;
@ -377,7 +379,7 @@ public:
: SolutionBase(creator, cost), wrapped_(wrapped) {} : SolutionBase(creator, cost), wrapped_(wrapped) {}
explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped) explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped)
: WrappedSolution(creator, wrapped, wrapped->cost()) {} : WrappedSolution(creator, wrapped, wrapped->cost()) {}
void fillMessage(moveit_task_constructor_msgs::Solution& solution, void fillMessage(moveit_task_constructor_msgs::msg::Solution& solution,
Introspection* introspection = nullptr) const override; Introspection* introspection = nullptr) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override; double computeCost(const CostTerm& cost, std::string& comment) const override;

View File

@ -42,11 +42,13 @@
#include "container.h" #include "container.h"
#include <moveit/task_constructor/introspection.h> #include <moveit/task_constructor/introspection.h>
#include <moveit_task_constructor_msgs/Solution.h> #include <moveit_task_constructor_msgs/msg/solution.hpp>
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit_msgs/MoveItErrorCodes.h> #include <moveit_msgs/msg/move_it_error_codes.hpp>
#include <rclcpp/node.hpp>
namespace moveit { namespace moveit {
namespace core { namespace core {
@ -86,7 +88,7 @@ public:
/// setting the robot model also resets the task /// setting the robot model also resets the task
void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model); void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model);
/// load robot model from given parameter /// load robot model from given parameter
void loadRobotModel(const std::string& robot_description = "robot_description"); void loadRobotModel(const rclcpp::Node::SharedPtr& node, const std::string& robot_description = "robot_description");
void add(Stage::pointer&& stage); void add(Stage::pointer&& stage);
void insert(Stage::pointer&& stage, int before = -1) override; void insert(Stage::pointer&& stage, int before = -1) override;
@ -121,7 +123,7 @@ public:
/// interrupt current planning (or execution) /// interrupt current planning (or execution)
void preempt(); void preempt();
/// execute solution, return the result /// execute solution, return the result
moveit_msgs::MoveItErrorCodes execute(const SolutionBase& s); moveit_msgs::msg::MoveItErrorCodes execute(const SolutionBase& s);
/// print current task state (number of found solutions and propagated states) to std::cout /// print current task state (number of found solutions and propagated states) to std::cout
void printState(std::ostream& os = std::cout) const; void printState(std::ostream& os = std::cout) const;

View File

@ -1,4 +1,4 @@
<library path="libmoveit_task_constructor_core_stage_plugins"> <library path="moveit_task_constructor_core_stage_plugins">
<class name="moveit_task_constructor/Current State" <class name="moveit_task_constructor/Current State"
type="moveit::task_constructor::stages::CurrentState" type="moveit::task_constructor::stages::CurrentState"
base_class_type="moveit::task_constructor::Stage"> base_class_type="moveit::task_constructor::Stage">

View File

@ -7,13 +7,11 @@
<maintainer email="me@v4hn.de">Michael Goerner</maintainer> <maintainer email="me@v4hn.de">Michael Goerner</maintainer>
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer> <maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>roscpp</build_depend>
<exec_depend>roscpp</exec_depend>
<depend>tf2_eigen</depend> <depend>tf2_eigen</depend>
<depend>geometry_msgs</depend> <depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>moveit_core</depend> <depend>moveit_core</depend>
<depend>moveit_ros_planning</depend> <depend>moveit_ros_planning</depend>
<depend>moveit_ros_planning_interface</depend> <depend>moveit_ros_planning_interface</depend>
@ -21,11 +19,16 @@
<depend>visualization_msgs</depend> <depend>visualization_msgs</depend>
<depend>rviz_marker_tools</depend> <depend>rviz_marker_tools</depend>
<test_depend>rosunit</test_depend> <test_depend>ament_cmake_gmock</test_depend>
<test_depend>rostest</test_depend> <test_depend>ament_cmake_gtest</test_depend>
<test_depend>moveit_resources_fanuc_moveit_config</test_depend> <!-- TODO(JafarAbdi): Enable after porting integration tests-->
<!-- test_depend>launch</test_depend -->
<!-- test_depend>launch_testing</test_depend -->
<!-- test_depend>launch_testing_ament_cmake</test_depend -->
<!-- test_depend>launch_testing_ros</test_depend -->
<!-- test_depend>moveit_resources_fanuc_moveit_config</test_depend -->
<export> <export>
<moveit_task_constructor_core plugin="${prefix}/motion_planning_stages_plugin_description.xml"/> <build_type>ament_cmake</build_type>
</export> </export>
</package> </package>

View File

@ -1,4 +1,4 @@
add_library(${PROJECT_NAME} add_library(${PROJECT_NAME} SHARED
${PROJECT_INCLUDE}/container.h ${PROJECT_INCLUDE}/container.h
${PROJECT_INCLUDE}/container_p.h ${PROJECT_INCLUDE}/container_p.h
${PROJECT_INCLUDE}/cost_queue.h ${PROJECT_INCLUDE}/cost_queue.h
@ -35,16 +35,24 @@ add_library(${PROJECT_NAME}
solvers/pipeline_planner.cpp solvers/pipeline_planner.cpp
solvers/joint_interpolation.cpp solvers/joint_interpolation.cpp
) )
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) ament_target_dependencies(${PROJECT_NAME}
moveit_core
geometry_msgs
moveit_ros_planning
moveit_ros_planning_interface
moveit_task_constructor_msgs
rclcpp
rviz_marker_tools
visualization_msgs
)
target_include_directories(${PROJECT_NAME} target_include_directories(${PROJECT_NAME}
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include> PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}> PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
PUBLIC ${catkin_INCLUDE_DIRS}
) )
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
add_subdirectory(stages) add_subdirectory(stages)
install(TARGETS ${PROJECT_NAME} install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} EXPORT ${PROJECT_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)

View File

@ -39,7 +39,7 @@
#include <moveit/task_constructor/merge.h> #include <moveit/task_constructor/merge.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <ros/console.h> #include <rclcpp/logging.hpp>
#include <memory> #include <memory>
#include <iostream> #include <iostream>
@ -107,7 +107,7 @@ void ContainerBasePrivate::compute() {
} }
void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
ROS_DEBUG_STREAM_NAMED("Pruning", "'" << child.name() << "' generated a failure"); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "'" << child.name() << "' generated a failure");
switch (child.pimpl()->interfaceFlags()) { switch (child.pimpl()->interfaceFlags()) {
case GENERATE: case GENERATE:
// just ignore: the pair of (new) states isn't known to us anyway // just ignore: the pair of (new) states isn't known to us anyway
@ -115,11 +115,11 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState
break; break;
case PROPAGATE_FORWARDS: // mark from as failed (backwards) case PROPAGATE_FORWARDS: // mark from as failed (backwards)
ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch"); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch");
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED); setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
break; break;
case PROPAGATE_BACKWARDS: // mark to as failed (forwards) case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch"); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch");
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED); setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
break; break;
@ -127,11 +127,11 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) { if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
auto cimpl = conn->pimpl(); auto cimpl = conn->pimpl();
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from)) { if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from)) {
ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch"); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch");
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED); setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
} }
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to)) { if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to)) {
ROS_DEBUG_STREAM_NAMED("Pruning", "prune forward branch"); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune forward branch");
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED); setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
} }
} }
@ -446,8 +446,9 @@ inline void updateStatePrios(const SolutionSequence::container_type& partial_sol
} }
void SerialContainer::onNewSolution(const SolutionBase& current) { void SerialContainer::onNewSolution(const SolutionBase& current) {
ROS_DEBUG_STREAM_NAMED("SerialContainer", "'" << this->name() << "' received solution of child stage '" RCLCPP_DEBUG_STREAM(rclcpp::get_logger("SerialContainer"), "'" << this->name()
<< current.creator()->name() << "'"); << "' received solution of child stage '"
<< current.creator()->name() << "'");
// failures should never trigger this callback // failures should never trigger this callback
assert(!current.isFailure()); assert(!current.isFailure());
@ -907,7 +908,7 @@ void Merger::onNewSolution(const SolutionBase& s) {
void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) { void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) {
const SubTrajectory* trajectory = dynamic_cast<const SubTrajectory*>(&s); const SubTrajectory* trajectory = dynamic_cast<const SubTrajectory*>(&s);
if (!trajectory || !trajectory->trajectory()) { if (!trajectory || !trajectory->trajectory()) {
ROS_ERROR_NAMED("Merger", "Only simple, valid trajectories are supported"); RCLCPP_ERROR(rclcpp::get_logger("Merger"), "Only simple, valid trajectories are supported");
return; return;
} }

View File

@ -113,7 +113,7 @@ double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/)
if (traj == nullptr || traj->getWayPointCount() == 0) if (traj == nullptr || traj->getWayPointCount() == 0)
return 0.0; return 0.0;
std::vector<const robot_model::JointModel*> joint_models; std::vector<const moveit::core::JointModel*> joint_models;
joint_models.reserve(joints.size()); joint_models.reserve(joints.size());
const auto& first_waypoint = traj->getWayPoint(0); const auto& first_waypoint = traj->getWayPoint(0);
for (auto& joint : joints) { for (auto& joint : joints) {
@ -196,18 +196,9 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const
auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) { auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) {
collision_detection::DistanceResult result; collision_detection::DistanceResult result;
if (with_world) if (with_world)
#if MOVEIT_HAS_COLLISION_ENV
state->scene()->getCollisionEnv()->distanceRobot(request, result, robot); state->scene()->getCollisionEnv()->distanceRobot(request, result, robot);
#else
state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(),
robot);
#endif
else else
#if MOVEIT_HAS_COLLISION_ENV
state->scene()->getCollisionEnv()->distanceSelf(request, result, robot); state->scene()->getCollisionEnv()->distanceSelf(request, result, robot);
#else
state->scene()->getCollisionRobot()->distanceSelf(request, result, robot);
#endif
if (result.minimum_distance.distance <= 0) { if (result.minimum_distance.distance <= 0) {
return result.minimum_distance; return result.minimum_distance;

View File

@ -39,11 +39,11 @@
#include <moveit/task_constructor/introspection.h> #include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/task.h> #include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/storage.h> #include <moveit/task_constructor/storage.h>
#include <moveit_task_constructor_msgs/Property.h> #include <moveit_task_constructor_msgs/msg/property.hpp>
#include <ros/node_handle.h> #include <rclcpp/node.hpp>
#include <ros/publisher.h> #include <rclcpp/publisher.hpp>
#include <ros/service.h> #include <rclcpp/service.hpp>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <sstream> #include <sstream>
@ -64,34 +64,73 @@ std::string getTaskId(const TaskPrivate* task) {
} }
} // namespace } // namespace
/**
* A shared executor between all tasks' introspection, this class will keep track of the number of the added nodes and
* will only create a spinning thread when we have nodes in the executor, once all the nodes are removed from the
* executor it will stop the spinning thread, later on if a new node is added a spinning thread will be started again
*/
class IntrospectionExecutor
{
public:
void add_node(const rclcpp::Node::SharedPtr& node) {
std::call_once(once_flag_, [this] { executor_ = rclcpp::executors::SingleThreadedExecutor::make_unique(); });
std::lock_guard<std::mutex> lock(mutex_);
executor_->add_node(node);
if (nodes_count_++ == 0)
executor_thread_ = std::thread([this] { executor_->spin(); });
}
void remove_node(const rclcpp::Node::SharedPtr& node) {
std::lock_guard<std::mutex> lock(mutex_);
executor_->remove_node(node);
if (--nodes_count_ == 0) {
executor_->cancel();
if (executor_thread_.joinable())
executor_thread_.join();
}
}
private:
rclcpp::executors::SingleThreadedExecutor::UniquePtr executor_;
std::thread executor_thread_;
size_t nodes_count_ = 0;
std::mutex mutex_;
std::once_flag once_flag_;
};
class IntrospectionPrivate class IntrospectionPrivate
{ {
public: public:
IntrospectionPrivate(const TaskPrivate* task, Introspection* self) IntrospectionPrivate(const TaskPrivate* task, Introspection* self) : task_(task), task_id_(getTaskId(task)) {
: nh_(std::string("~/") + task->ns()) // topics + services are advertised in private namespace rclcpp::NodeOptions options;
, task_(task) options.arguments({ "--ros-args", "-r", "__node:=introspection_" + task_id_ });
, task_id_(getTaskId(task)) { node_ = rclcpp::Node::make_shared("_", task_->ns(), options);
task_description_publisher_ = executor_.add_node(node_);
nh_.advertise<moveit_task_constructor_msgs::TaskDescription>(DESCRIPTION_TOPIC, 2, true); task_description_publisher_ = node_->create_publisher<moveit_task_constructor_msgs::msg::TaskDescription>(
DESCRIPTION_TOPIC, rclcpp::QoS(2).transient_local());
// send reset message as early as possible to give subscribers time to see it // send reset message as early as possible to give subscribers time to see it
indicateReset(); indicateReset();
task_statistics_publisher_ = task_statistics_publisher_ = node_->create_publisher<moveit_task_constructor_msgs::msg::TaskStatistics>(
nh_.advertise<moveit_task_constructor_msgs::TaskStatistics>(STATISTICS_TOPIC, 1, true); STATISTICS_TOPIC, rclcpp::QoS(1).transient_local());
solution_publisher_ = nh_.advertise<moveit_task_constructor_msgs::Solution>(SOLUTION_TOPIC, 1, true); solution_publisher_ = node_->create_publisher<moveit_task_constructor_msgs::msg::Solution>(
SOLUTION_TOPIC, rclcpp::QoS(1).transient_local());
get_solution_service_ =
nh_.advertiseService(std::string(GET_SOLUTION_SERVICE "_") + task_id_, &Introspection::getSolution, self);
get_solution_service_ = node_->create_service<moveit_task_constructor_msgs::srv::GetSolution>(
std::string(GET_SOLUTION_SERVICE "_") + task_id_,
std::bind(&Introspection::getSolution, self, std::placeholders::_1, std::placeholders::_2));
resetMaps(); resetMaps();
} }
~IntrospectionPrivate() { indicateReset(); } ~IntrospectionPrivate() {
indicateReset();
executor_.remove_node(node_);
}
void indicateReset() { void indicateReset() {
// send empty task description message to indicate reset // send empty task description message to indicate reset
::moveit_task_constructor_msgs::TaskDescription msg; ::moveit_task_constructor_msgs::msg::TaskDescription msg;
msg.task_id = task_id_; msg.task_id = task_id_;
task_description_publisher_.publish(msg); task_description_publisher_->publish(msg);
} }
void resetMaps() { void resetMaps() {
@ -102,21 +141,22 @@ public:
id_solution_bimap_.clear(); id_solution_bimap_.clear();
} }
ros::NodeHandle nh_;
/// associated task /// associated task
const TaskPrivate* task_; const TaskPrivate* task_;
const std::string task_id_; const std::string task_id_;
/// publish task detailed description and current state /// publish task detailed description and current state
ros::Publisher task_description_publisher_; rclcpp::Publisher<moveit_task_constructor_msgs::msg::TaskDescription>::SharedPtr task_description_publisher_;
ros::Publisher task_statistics_publisher_; rclcpp::Publisher<moveit_task_constructor_msgs::msg::TaskStatistics>::SharedPtr task_statistics_publisher_;
/// publish new solutions /// publish new solutions
ros::Publisher solution_publisher_; rclcpp::Publisher<moveit_task_constructor_msgs::msg::Solution>::SharedPtr solution_publisher_;
/// services to provide an individual Solution /// services to provide an individual Solution
ros::ServiceServer get_solution_service_; rclcpp::Service<moveit_task_constructor_msgs::srv::GetSolution>::SharedPtr get_solution_service_;
rclcpp::Node::SharedPtr node_;
inline static IntrospectionExecutor executor_;
/// mapping from stages to their id /// mapping from stages to their id
std::map<const StagePrivate*, moveit_task_constructor_msgs::StageStatistics::_id_type> stage_to_id_map_; std::map<const StagePrivate*, moveit_task_constructor_msgs::msg::StageStatistics::_id_type> stage_to_id_map_;
boost::bimap<uint32_t, const SolutionBase*> id_solution_bimap_; boost::bimap<uint32_t, const SolutionBase*> id_solution_bimap_;
}; };
@ -127,13 +167,13 @@ Introspection::~Introspection() {
} }
void Introspection::publishTaskDescription() { void Introspection::publishTaskDescription() {
::moveit_task_constructor_msgs::TaskDescription msg; ::moveit_task_constructor_msgs::msg::TaskDescription msg;
impl->task_description_publisher_.publish(fillTaskDescription(msg)); impl->task_description_publisher_->publish(fillTaskDescription(msg));
} }
void Introspection::publishTaskState() { void Introspection::publishTaskState() {
::moveit_task_constructor_msgs::TaskStatistics msg; ::moveit_task_constructor_msgs::msg::TaskStatistics msg;
impl->task_statistics_publisher_.publish(fillTaskStatistics(msg)); impl->task_statistics_publisher_->publish(fillTaskStatistics(msg));
} }
void Introspection::reset() { void Introspection::reset() {
@ -145,7 +185,7 @@ void Introspection::registerSolution(const SolutionBase& s) {
solutionId(s); solutionId(s);
} }
void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, const SolutionBase& s) { void Introspection::fillSolution(moveit_task_constructor_msgs::msg::Solution& msg, const SolutionBase& s) {
s.fillMessage(msg, this); s.fillMessage(msg, this);
s.start()->scene()->getPlanningSceneMsg(msg.start_scene); s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
@ -153,9 +193,9 @@ void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, co
} }
void Introspection::publishSolution(const SolutionBase& s) { void Introspection::publishSolution(const SolutionBase& s) {
moveit_task_constructor_msgs::Solution msg; moveit_task_constructor_msgs::msg::Solution msg;
fillSolution(msg, s); fillSolution(msg, s);
impl->solution_publisher_.publish(msg); impl->solution_publisher_->publish(msg);
} }
void Introspection::publishAllSolutions(bool wait) { void Introspection::publishAllSolutions(bool wait) {
@ -178,13 +218,13 @@ const SolutionBase* Introspection::solutionFromId(uint id) const {
return it->second; return it->second;
} }
bool Introspection::getSolution(moveit_task_constructor_msgs::GetSolution::Request& req, bool Introspection::getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr req,
moveit_task_constructor_msgs::GetSolution::Response& res) { const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr res) {
const SolutionBase* solution = solutionFromId(req.solution_id); const SolutionBase* solution = solutionFromId(req->solution_id);
if (!solution) if (!solution)
return false; return false;
fillSolution(res.solution, *solution); fillSolution(res->solution, *solution);
return true; return true;
} }
@ -203,7 +243,7 @@ uint32_t Introspection::solutionId(const SolutionBase& s) {
return result.first->first; return result.first->first;
} }
void Introspection::fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::StageStatistics& s) { void Introspection::fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::msg::StageStatistics& s) {
// successful solutions // successful solutions
for (const auto& solution : stage.solutions()) for (const auto& solution : stage.solutions())
s.solved.push_back(solutionId(*solution)); s.solved.push_back(solutionId(*solution));
@ -216,18 +256,18 @@ void Introspection::fillStageStatistics(const Stage& stage, moveit_task_construc
s.num_failed = stage.numFailures(); s.num_failed = stage.numFailures();
} }
moveit_task_constructor_msgs::TaskDescription& moveit_task_constructor_msgs::msg::TaskDescription&
Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription& msg) { Introspection::fillTaskDescription(moveit_task_constructor_msgs::msg::TaskDescription& msg) {
ContainerBase::StageCallback stage_processor = [this, &msg](const Stage& stage, unsigned int /*depth*/) -> bool { ContainerBase::StageCallback stage_processor = [this, &msg](const Stage& stage, unsigned int /*depth*/) -> bool {
// this method is called for each child stage of a given parent // this method is called for each child stage of a given parent
moveit_task_constructor_msgs::StageDescription desc; moveit_task_constructor_msgs::msg::StageDescription desc;
desc.id = stageId(&stage); desc.id = stageId(&stage);
desc.name = stage.name(); desc.name = stage.name();
desc.flags = stage.pimpl()->interfaceFlags(); desc.flags = stage.pimpl()->interfaceFlags();
// fill stage properties // fill stage properties
for (const auto& pair : stage.properties()) { for (const auto& pair : stage.properties()) {
moveit_task_constructor_msgs::Property p; moveit_task_constructor_msgs::msg::Property p;
p.name = pair.first; p.name = pair.first;
p.description = pair.second.description(); p.description = pair.second.description();
p.type = pair.second.typeName(); p.type = pair.second.typeName();
@ -251,11 +291,11 @@ Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription
return msg; return msg;
} }
moveit_task_constructor_msgs::TaskStatistics& moveit_task_constructor_msgs::msg::TaskStatistics&
Introspection::fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics& msg) { Introspection::fillTaskStatistics(moveit_task_constructor_msgs::msg::TaskStatistics& msg) {
ContainerBase::StageCallback stage_processor = [this, &msg](const Stage& stage, unsigned int /*depth*/) -> bool { ContainerBase::StageCallback stage_processor = [this, &msg](const Stage& stage, unsigned int /*depth*/) -> bool {
// this method is called for each child stage of a given parent // this method is called for each child stage of a given parent
moveit_task_constructor_msgs::StageStatistics stat; // create new Stage msg moveit_task_constructor_msgs::msg::StageStatistics stat; // create new Stage msg
stat.id = stageId(&stage); stat.id = stageId(&stage);
fillStageStatistics(stage, stat); fillStageStatistics(stage, stat);

View File

@ -1,7 +1,6 @@
#include <moveit/task_constructor/marker_tools.h> #include <moveit/task_constructor/marker_tools.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/robot_state.h> #include <moveit/robot_state/robot_state.h>
#include <eigen_conversions/eigen_msg.h>
namespace vm = visualization_msgs; namespace vm = visualization_msgs;
@ -17,7 +16,7 @@ void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr& scen
const std::vector<std::string>* names = object_names.empty() ? &scene->getCollisionObjectMsg() const std::vector<std::string>* names = object_names.empty() ? &scene->getCollisionObjectMsg()
: &link_names; : &link_names;
for (const auto &name : *names) { for (const auto &name : *names) {
visualization_msgs::MarkerArray markers; visualization_msgs::msg::MarkerArray markers;
robot_state.getRobotMarkers(markers, {name}, false); robot_state.getRobotMarkers(markers, {name}, false);
for (auto &marker : markers.markers) for (auto &marker : markers.markers)
callback(marker, name); callback(marker, name);
@ -25,8 +24,9 @@ void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr& scen
*/ */
} }
visualization_msgs::Marker& createGeometryMarker(visualization_msgs::Marker& marker, const urdf::Geometry& geom, visualization_msgs::msg::Marker& createGeometryMarker(visualization_msgs::msg::Marker& marker,
const urdf::Pose& pose, const urdf::Color& color) { const urdf::Geometry& geom, const urdf::Pose& pose,
const urdf::Color& color) {
rviz_marker_tools::makeFromGeometry(marker, geom); rviz_marker_tools::makeFromGeometry(marker, geom);
marker.pose.position.x = pose.position.x; marker.pose.position.x = pose.position.x;
marker.pose.position.y = pose.position.y; marker.pose.position.y = pose.position.y;
@ -110,7 +110,7 @@ void generateMarkers(const moveit::core::RobotState& robot_state, const MarkerCa
if (!model) if (!model)
return; return;
visualization_msgs::Marker m; visualization_msgs::msg::Marker m;
m.header.frame_id = robot_state.getRobotModel()->getModelFrame(); m.header.frame_id = robot_state.getRobotModel()->getModelFrame();
// code adapted from rviz::RobotLink::createVisual() / createCollision() // code adapted from rviz::RobotLink::createVisual() / createCollision()

View File

@ -106,7 +106,7 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
robot_trajectory::RobotTrajectoryPtr robot_trajectory::RobotTrajectoryPtr
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories, merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) { const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) {
if (sub_trajectories.size() <= 1) if (sub_trajectories.size() <= 1)
throw std::runtime_error("Expected multiple sub solutions"); throw std::runtime_error("Expected multiple sub solutions");
@ -141,7 +141,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
std::vector<double> values; std::vector<double> values;
values.reserve(max_num_vars); values.reserve(max_num_vars);
auto merged_state = std::make_shared<robot_state::RobotState>(base_state); auto merged_state = std::make_shared<moveit::core::RobotState>(base_state);
while (true) { while (true) {
bool finished = true; bool finished = true;
size_t index = merged_traj->getWayPointCount(); size_t index = merged_traj->getWayPointCount();
@ -151,7 +151,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
continue; // no more waypoints in this sub solution continue; // no more waypoints in this sub solution
finished = false; // there was a waypoint, continue while loop finished = false; // there was a waypoint, continue while loop
const robot_state::RobotState& sub_state = sub->getWayPoint(index); const moveit::core::RobotState& sub_state = sub->getWayPoint(index);
sub_state.copyJointGroupPositions(sub->getGroup(), values); sub_state.copyJointGroupPositions(sub->getGroup(), values);
merged_state->setJointGroupPositions(sub->getGroup(), values); merged_state->setJointGroupPositions(sub->getGroup(), values);
} }
@ -162,7 +162,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
// add waypoint without timing // add waypoint without timing
merged_traj->addSuffixWayPoint(merged_state, 0.0); merged_traj->addSuffixWayPoint(merged_state, 0.0);
// create new RobotState for next waypoint // create new RobotState for next waypoint
merged_state = std::make_shared<robot_state::RobotState>(*merged_state); merged_state = std::make_shared<moveit::core::RobotState>(*merged_state);
} }
// add timing // add timing

View File

@ -39,12 +39,12 @@
#include <moveit/task_constructor/properties.h> #include <moveit/task_constructor/properties.h>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <functional> #include <functional>
#include <ros/console.h> #include <rclcpp/logging.hpp>
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
const std::string LOGNAME = "Properties"; static const rclcpp::Logger LOGGER = rclcpp::get_logger("Properties");
class PropertyTypeRegistry class PropertyTypeRegistry
{ {
@ -73,7 +73,7 @@ public:
const Entry& entry(const std::type_index& type_index) const { const Entry& entry(const std::type_index& type_index) const {
auto it = types_.find(type_index); auto it = types_.find(type_index);
if (it == types_.end()) { if (it == types_.end()) {
ROS_ERROR_STREAM_NAMED(LOGNAME, "Unregistered type: " << type_index.name()); RCLCPP_ERROR_STREAM(LOGGER, "Unregistered type: " << type_index.name());
return dummy_; return dummy_;
} }
return it->second; return it->second;
@ -289,8 +289,8 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa
} catch (const Property::undefined&) { } catch (const Property::undefined&) {
} }
ROS_DEBUG_STREAM_NAMED(LOGNAME, pair.first << ": " << p.initialized_from_ << " -> " << source << ": " RCLCPP_DEBUG_STREAM(LOGGER, pair.first << ": " << p.initialized_from_ << " -> " << source << ": "
<< Property::serialize(value)); << Property::serialize(value));
p.setCurrentValue(value); p.setCurrentValue(value);
p.initialized_from_ = source; p.initialized_from_ = source;
} }

View File

@ -41,14 +41,14 @@
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h> #include <moveit/trajectory_processing/iterative_time_parameterization.h>
#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
#include <moveit/robot_state/cartesian_interpolator.h> #include <moveit/robot_state/cartesian_interpolator.h>
#endif
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace solvers { namespace solvers {
static const rclcpp::Logger LOGGER = rclcpp::get_logger("CartesianPath");
CartesianPath::CartesianPath() { CartesianPath::CartesianPath() {
auto& p = properties(); auto& p = properties();
p.declare<double>("step_size", 0.01, "step size between consecutive waypoints"); p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
@ -61,10 +61,10 @@ void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) { const moveit_msgs::msg::Constraints& path_constraints) {
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip(); const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
if (!link) { if (!link) {
ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName()); RCLCPP_WARN_STREAM(LOGGER, "no unique tip for joint model group: " << jmg->getName());
return false; return false;
} }
@ -75,7 +75,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) { const moveit_msgs::msg::Constraints& path_constraints) {
const auto& props = properties(); const auto& props = properties();
planning_scene::PlanningScenePtr sandbox_scene = from->diff(); planning_scene::PlanningScenePtr sandbox_scene = from->diff();
@ -86,21 +86,15 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
const double* joint_positions) { const double* joint_positions) {
state->setJointGroupPositions(jmg, joint_positions); state->setJointGroupPositions(jmg, joint_positions);
state->update(); state->update();
return !sandbox_scene->isStateColliding(const_cast<const robot_state::RobotState&>(*state), jmg->getName()) && return !sandbox_scene->isStateColliding(const_cast<const moveit::core::RobotState&>(*state), jmg->getName()) &&
kcs.decide(*state).satisfied; kcs.decide(*state).satisfied;
}; };
std::vector<moveit::core::RobotStatePtr> trajectory; std::vector<moveit::core::RobotStatePtr> trajectory;
#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath( double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true, &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
moveit::core::MaxEEFStep(props.get<double>("step_size")), moveit::core::MaxEEFStep(props.get<double>("step_size")),
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid); moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid);
#else
double achieved_fraction = sandbox_scene->getCurrentStateNonConst().computeCartesianPath(
jmg, trajectory, &link, target, true, props.get<double>("step_size"), props.get<double>("jump_threshold"),
is_valid);
#endif
assert(!trajectory.empty()); // there should be at least the start state assert(!trajectory.empty()); // there should be at least the start state
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg); result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);

View File

@ -46,6 +46,8 @@ namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace solvers { namespace solvers {
static const auto LOGGER = rclcpp::get_logger("JointInterpolationPlanner");
JointInterpolationPlanner::JointInterpolationPlanner() { JointInterpolationPlanner::JointInterpolationPlanner() {
auto& p = properties(); auto& p = properties();
p.declare<double>("max_step", 0.1, "max joint step"); p.declare<double>("max_step", 0.1, "max joint step");
@ -57,7 +59,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
const planning_scene::PlanningSceneConstPtr& to, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double /*timeout*/, const moveit::core::JointModelGroup* jmg, double /*timeout*/,
robot_trajectory::RobotTrajectoryPtr& result, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& /*path_constraints*/) { const moveit_msgs::msg::Constraints& /*path_constraints*/) {
const auto& props = properties(); const auto& props = properties();
// Get maximum joint distance // Get maximum joint distance
@ -101,7 +103,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen, const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup* jmg, double timeout, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) { const moveit_msgs::msg::Constraints& path_constraints) {
const auto start_time = std::chrono::steady_clock::now(); const auto start_time = std::chrono::steady_clock::now();
auto to{ from->diff() }; auto to{ from->diff() };
@ -119,7 +121,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) { if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) {
// TODO(v4hn): planners need a way to add feedback to failing plans // TODO(v4hn): planners need a way to add feedback to failing plans
// in case of an invalid solution feedback should include unwanted collisions or violated constraints // in case of an invalid solution feedback should include unwanted collisions or violated constraints
ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target"); RCLCPP_WARN(LOGGER, "IK failed for pose target");
return false; return false;
} }
to->getCurrentStateNonConst().update(); to->getCurrentStateNonConst().update();

View File

@ -40,9 +40,14 @@
#include <moveit/task_constructor/task.h> #include <moveit/task_constructor/task.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_pipeline/planning_pipeline.h> #include <moveit/planning_pipeline/planning_pipeline.h>
#include <moveit_msgs/MotionPlanRequest.h> #include <moveit_msgs/msg/motion_plan_request.hpp>
#include <moveit/kinematic_constraints/utils.h> #include <moveit/kinematic_constraints/utils.h>
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h> #include <tf2_eigen/tf2_eigen.h>
#endif
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
@ -52,10 +57,10 @@ struct PlannerCache
{ {
using PlannerID = std::tuple<std::string, std::string>; using PlannerID = std::tuple<std::string, std::string>;
using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> >; using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> >;
using ModelList = std::list<std::pair<std::weak_ptr<const robot_model::RobotModel>, PlannerMap> >; using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, PlannerMap> >;
ModelList cache_; ModelList cache_;
PlannerMap::mapped_type& retrieve(const robot_model::RobotModelConstPtr& model, const PlannerID& id) { PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& id) {
// find model in cache_ and remove expired entries while doing so // find model in cache_ and remove expired entries while doing so
ModelList::iterator model_it = cache_.begin(); ModelList::iterator model_it = cache_.begin();
while (model_it != cache_.end()) { while (model_it != cache_.end()) {
@ -74,17 +79,22 @@ struct PlannerCache
} }
}; };
planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const PipelinePlanner::Specification& spec) { planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Node::SharedPtr& node,
const PipelinePlanner::Specification& spec) {
static PlannerCache cache; static PlannerCache cache;
static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin"; static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin";
std::string pipeline_ns = spec.ns + "/planning_pipelines/" + spec.pipeline; std::string pipeline_ns = spec.ns;
const std::string parameter_name = pipeline_ns + "." + PLUGIN_PARAMETER_NAME;
// fallback to old structure for pipeline parameters in MoveIt // fallback to old structure for pipeline parameters in MoveIt
if (!ros::NodeHandle(pipeline_ns).hasParam(PLUGIN_PARAMETER_NAME)) { if (!node->has_parameter(parameter_name)) {
ROS_WARN("Failed to find '%s/%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME, node->declare_parameter(parameter_name, rclcpp::ParameterType::PARAMETER_STRING);
"Attempting to load pipeline from old parameter structure. Please update your MoveIt config."); }
pipeline_ns = spec.ns; if (std::string parameter; !node->get_parameter(parameter_name, parameter)) {
RCLCPP_WARN(node->get_logger(), "Failed to find '%s.%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME,
"Attempting to load pipeline from old parameter structure. Please update your MoveIt config.");
pipeline_ns = "move_group";
} }
PlannerCache::PlannerID id(pipeline_ns, spec.adapter_param); PlannerCache::PlannerID id(pipeline_ns, spec.adapter_param);
@ -93,7 +103,7 @@ planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const PipelinePla
planning_pipeline::PlanningPipelinePtr planner = entry.lock(); planning_pipeline::PlanningPipelinePtr planner = entry.lock();
if (!planner) { if (!planner) {
// create new entry // create new entry
planner = std::make_shared<planning_pipeline::PlanningPipeline>(spec.model, ros::NodeHandle(pipeline_ns), planner = std::make_shared<planning_pipeline::PlanningPipeline>(spec.model, node, pipeline_ns,
PLUGIN_PARAMETER_NAME, spec.adapter_param); PLUGIN_PARAMETER_NAME, spec.adapter_param);
// store in cache // store in cache
entry = planner; entry = planner;
@ -101,13 +111,14 @@ planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const PipelinePla
return planner; return planner;
} }
PipelinePlanner::PipelinePlanner(const std::string& pipeline_name) : pipeline_name_{ pipeline_name } { PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name)
: pipeline_name_{ pipeline_name }, node_(node) {
auto& p = properties(); auto& p = properties();
p.declare<std::string>("planner", "", "planner id"); p.declare<std::string>("planner", "", "planner id");
p.declare<uint>("num_planning_attempts", 1u, "number of planning attempts"); p.declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
p.declare<moveit_msgs::WorkspaceParameters>("workspace_parameters", moveit_msgs::WorkspaceParameters(), p.declare<moveit_msgs::msg::WorkspaceParameters>("workspace_parameters", moveit_msgs::msg::WorkspaceParameters(),
"allowed workspace of mobile base?"); "allowed workspace of mobile base?");
p.declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals"); p.declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");
p.declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals"); p.declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals");
@ -120,7 +131,8 @@ PipelinePlanner::PipelinePlanner(const std::string& pipeline_name) : pipeline_na
planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC); planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
} }
PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline) : PipelinePlanner() { PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline)
: PipelinePlanner(rclcpp::Node::SharedPtr()) {
planner_ = planning_pipeline; planner_ = planning_pipeline;
} }
@ -129,7 +141,7 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
Specification spec; Specification spec;
spec.model = robot_model; spec.model = robot_model;
spec.pipeline = pipeline_name_; spec.pipeline = pipeline_name_;
planner_ = create(spec); planner_ = create(node_, spec);
} else if (robot_model != planner_->getRobotModel()) { } else if (robot_model != planner_->getRobotModel()) {
throw std::runtime_error( throw std::runtime_error(
"The robot model of the planning pipeline isn't the same as the task's robot model -- " "The robot model of the planning pipeline isn't the same as the task's robot model -- "
@ -139,7 +151,7 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
planner_->publishReceivedRequests(properties().get<bool>("publish_planning_requests")); planner_->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
} }
void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMap& p, void initMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& req, const PropertyMap& p,
const moveit::core::JointModelGroup* jmg, double timeout) { const moveit::core::JointModelGroup* jmg, double timeout) {
req.group_name = jmg->getName(); req.group_name = jmg->getName();
req.planner_id = p.get<std::string>("planner"); req.planner_id = p.get<std::string>("planner");
@ -149,15 +161,15 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa
req.num_planning_attempts = p.get<uint>("num_planning_attempts"); req.num_planning_attempts = p.get<uint>("num_planning_attempts");
req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor"); req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor"); req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");
req.workspace_parameters = p.get<moveit_msgs::WorkspaceParameters>("workspace_parameters"); req.workspace_parameters = p.get<moveit_msgs::msg::WorkspaceParameters>("workspace_parameters");
} }
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) { const moveit_msgs::msg::Constraints& path_constraints) {
const auto& props = properties(); const auto& props = properties();
moveit_msgs::MotionPlanRequest req; moveit_msgs::msg::MotionPlanRequest req;
initMotionPlanRequest(req, props, jmg, timeout); initMotionPlanRequest(req, props, jmg, timeout);
req.goal_constraints.resize(1); req.goal_constraints.resize(1);
@ -174,12 +186,12 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg, const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) { const moveit_msgs::msg::Constraints& path_constraints) {
const auto& props = properties(); const auto& props = properties();
moveit_msgs::MotionPlanRequest req; moveit_msgs::msg::MotionPlanRequest req;
initMotionPlanRequest(req, props, jmg, timeout); initMotionPlanRequest(req, props, jmg, timeout);
geometry_msgs::PoseStamped target; geometry_msgs::msg::PoseStamped target;
target.header.frame_id = from->getPlanningFrame(); target.header.frame_id = from->getPlanningFrame();
target.pose = tf2::toMsg(target_eigen); target.pose = tf2::toMsg(target_eigen);

View File

@ -42,7 +42,7 @@
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <ros/console.h> #include <rclcpp/logging.hpp>
#include <boost/format.hpp> #include <boost/format.hpp>
@ -87,7 +87,7 @@ void InitStageException::append(InitStageException& other) {
} }
const char* InitStageException::what() const noexcept { const char* InitStageException::what() const noexcept {
static const char* msg = "Error initializing stage(s). ROS_ERROR_STREAM(e) for details."; static const char* msg = "Error initializing stage(s). RCLCPP_ERROR_STREAM(e) for details.";
return msg; return msg;
} }
@ -325,7 +325,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) {
impl->properties_.reset(); impl->properties_.reset();
if (impl->parent()) { if (impl->parent()) {
try { try {
ROS_DEBUG_STREAM_NAMED("Properties", "init '" << name() << "'"); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Properties"), "init '" << name() << "'");
impl->properties_.performInitFrom(PARENT, impl->parent()->properties()); impl->properties_.performInitFrom(PARENT, impl->parent()->properties());
} catch (const Property::error& e) { } catch (const Property::error& e) {
std::ostringstream oss; std::ostringstream oss;
@ -795,6 +795,7 @@ std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const {
os << reset; os << reset;
return os; return os;
} }
static const rclcpp::Logger LOGGER = rclcpp::get_logger("Connecting");
Connecting::Connecting(const std::string& name) : ComputeBase(new ConnectingPrivate(this, name)) {} Connecting::Connecting(const std::string& name) : ComputeBase(new ConnectingPrivate(this, name)) {}
@ -809,7 +810,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
const planning_scene::PlanningSceneConstPtr& to = to_state.scene(); const planning_scene::PlanningSceneConstPtr& to = to_state.scene();
if (from->getWorld()->size() != to->getWorld()->size()) { if (from->getWorld()->size() != to->getWorld()->size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of collision objects"); RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different number of collision objects");
return false; return false;
} }
@ -819,19 +820,19 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
const collision_detection::World::ObjectPtr& from_object = from_object_pair.second; const collision_detection::World::ObjectPtr& from_object = from_object_pair.second;
const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_name); const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_name);
if (!to_object) { if (!to_object) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_name); RCLCPP_DEBUG_STREAM(LOGGER, name() << ": object missing: " << from_object_name);
return false; return false;
} }
#if MOVEIT_HAS_OBJECT_POSE #if MOVEIT_HAS_OBJECT_POSE
if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) { if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_name); RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object pose: " << from_object_name);
return false; // transforms do not match return false; // transforms do not match
} }
#endif #endif
if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) { if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_name); RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object shapes: " << from_object_name);
return false; // shapes not matching return false; // shapes not matching
} }
@ -839,7 +840,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
to_it = to_object->shape_poses_.cbegin(); to_it = to_object->shape_poses_.cbegin();
from_it != from_end; ++from_it, ++to_it) from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) { if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different shape pose: " << from_object_name); RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different shape pose: " << from_object_name);
return false; // transforms do not match return false; // transforms do not match
} }
} }
@ -850,7 +851,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
from->getCurrentState().getAttachedBodies(from_attached); from->getCurrentState().getAttachedBodies(from_attached);
to->getCurrentState().getAttachedBodies(to_attached); to->getCurrentState().getAttachedBodies(to_attached);
if (from_attached.size() != to_attached.size()) { if (from_attached.size() != to_attached.size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of objects"); RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different number of objects");
return false; return false;
} }
@ -860,18 +861,18 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
return object->getName() == from_object->getName(); return object->getName() == from_object->getName();
}); });
if (it == to_attached.cend()) { if (it == to_attached.cend()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object->getName()); RCLCPP_DEBUG_STREAM(LOGGER, name() << ": object missing: " << from_object->getName());
return false; return false;
} }
const moveit::core::AttachedBody* to_object = *it; const moveit::core::AttachedBody* to_object = *it;
if (from_object->getAttachedLink() != to_object->getAttachedLink()) { if (from_object->getAttachedLink() != to_object->getAttachedLink()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different attach links: " << from_object->getName() RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different attach links: " << from_object->getName() << " attached to "
<< " attached to " << from_object->getAttachedLinkName() << " / " << from_object->getAttachedLinkName() << " / "
<< to_object->getAttachedLinkName()); << to_object->getAttachedLinkName());
return false; // links not matching return false; // links not matching
} }
if (from_object->getShapes().size() != to_object->getShapes().size()) { if (from_object->getShapes().size() != to_object->getShapes().size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object->getName()); RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different object shapes: " << from_object->getName());
return false; // shapes not matching return false; // shapes not matching
} }
@ -886,8 +887,8 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
#endif #endif
for (; from_it != from_end; ++from_it, ++to_it) for (; from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) { if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", RCLCPP_DEBUG_STREAM(LOGGER, name()
name() << ": different pose of attached object shape: " << from_object->getName()); << ": different pose of attached object shape: " << from_object->getName());
return false; // transforms do not match return false; // transforms do not match
} }
} }

View File

@ -1,4 +1,4 @@
add_library(${PROJECT_NAME}_stages add_library(${PROJECT_NAME}_stages SHARED
${PROJECT_INCLUDE}/stages/modify_planning_scene.h ${PROJECT_INCLUDE}/stages/modify_planning_scene.h
${PROJECT_INCLUDE}/stages/fix_collision_objects.h ${PROJECT_INCLUDE}/stages/fix_collision_objects.h
@ -39,13 +39,24 @@ add_library(${PROJECT_NAME}_stages
simple_grasp.cpp simple_grasp.cpp
pick.cpp pick.cpp
) )
target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES}) target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME})
ament_target_dependencies(${PROJECT_NAME}_stages
moveit_core
geometry_msgs
moveit_ros_planning
moveit_ros_planning_interface
moveit_task_constructor_msgs
rclcpp
tf2_eigen
visualization_msgs
)
add_library(${PROJECT_NAME}_stage_plugins add_library(${PROJECT_NAME}_stage_plugins SHARED
plugins.cpp plugins.cpp
) )
target_link_libraries(${PROJECT_NAME}_stage_plugins ${PROJECT_NAME}_stages ${catkin_LIBRARIES}) target_link_libraries(${PROJECT_NAME}_stage_plugins ${PROJECT_NAME}_stages)
install(TARGETS ${PROJECT_NAME}_stages ${PROJECT_NAME}_stage_plugins install(TARGETS ${PROJECT_NAME}_stages ${PROJECT_NAME}_stage_plugins
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} EXPORT ${PROJECT_NAME}Targets
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)

View File

@ -44,16 +44,22 @@
#include <moveit/robot_state/robot_state.h> #include <moveit/robot_state/robot_state.h>
#include <Eigen/Geometry> #include <Eigen/Geometry>
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h> #include <tf2_eigen/tf2_eigen.h>
#endif
#include <chrono> #include <chrono>
#include <functional> #include <functional>
#include <iterator> #include <iterator>
#include <ros/console.h> #include <rclcpp/logging.hpp>
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace stages { namespace stages {
static const rclcpp::Logger LOGGER = rclcpp::get_logger("ComputeIK");
ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) { ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {
auto& p = properties(); auto& p = properties();
p.declare<std::string>("eef", "name of end-effector group"); p.declare<std::string>("eef", "name of end-effector group");
@ -65,19 +71,19 @@ ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperB
"minimum distance between seperate IK solutions for the same target"); "minimum distance between seperate IK solutions for the same target");
// ik_frame and target_pose are read from the interface // ik_frame and target_pose are read from the interface
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose"); p.declare<geometry_msgs::msg::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
p.declare<geometry_msgs::PoseStamped>("target_pose", "goal pose for ik frame"); p.declare<geometry_msgs::msg::PoseStamped>("target_pose", "goal pose for ik frame");
} }
void ComputeIK::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) { void ComputeIK::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg; geometry_msgs::msg::PoseStamped pose_msg;
pose_msg.header.frame_id = link; pose_msg.header.frame_id = link;
pose_msg.pose = tf2::toMsg(pose); pose_msg.pose = tf2::toMsg(pose);
setIKFrame(pose_msg); setIKFrame(pose_msg);
} }
void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string& frame) { void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string& frame) {
geometry_msgs::PoseStamped pose_msg; geometry_msgs::msg::PoseStamped pose_msg;
pose_msg.header.frame_id = frame; pose_msg.header.frame_id = frame;
pose_msg.pose = tf2::toMsg(pose); pose_msg.pose = tf2::toMsg(pose);
setTargetPose(pose_msg); setTargetPose(pose_msg);
@ -91,11 +97,11 @@ namespace {
// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though... // ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though...
// TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface // TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface
bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene, bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene,
robot_state::RobotState& robot_state, Eigen::Isometry3d pose, moveit::core::RobotState& robot_state, Eigen::Isometry3d pose,
const robot_model::LinkModel* link, const moveit::core::LinkModel* link,
collision_detection::CollisionResult* collision_result = nullptr) { collision_detection::CollisionResult* collision_result = nullptr) {
// consider all rigidly connected parent links as well // consider all rigidly connected parent links as well
const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link); const moveit::core::LinkModel* parent = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link);
if (parent != link) // transform pose into pose suitable to place parent if (parent != link) // transform pose into pose suitable to place parent
pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent); pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent);
@ -109,10 +115,10 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce
while (parent) { while (parent) {
pending_links.push_back(&parent->getName()); pending_links.push_back(&parent->getName());
link = parent; link = parent;
const robot_model::JointModel* joint = link->getParentJointModel(); const moveit::core::JointModel* joint = link->getParentJointModel();
parent = joint->getParentLinkModel(); parent = joint->getParentLinkModel();
if (joint->getType() != robot_model::JointModel::FIXED) { if (joint->getType() != moveit::core::JointModel::FIXED) {
for (const std::string* name : pending_links) for (const std::string* name : pending_links)
acm.setDefaultEntry(*name, true); acm.setDefaultEntry(*name, true);
pending_links.clear(); pending_links.clear();
@ -240,21 +246,21 @@ void ComputeIK::compute() {
std::string msg; std::string msg;
if (!validateEEF(props, robot_model, eef_jmg, &msg)) { if (!validateEEF(props, robot_model, eef_jmg, &msg)) {
ROS_WARN_STREAM_NAMED("ComputeIK", msg); RCLCPP_WARN_STREAM(LOGGER, msg);
return; return;
} }
if (!validateGroup(props, robot_model, eef_jmg, jmg, &msg)) { if (!validateGroup(props, robot_model, eef_jmg, jmg, &msg)) {
ROS_WARN_STREAM_NAMED("ComputeIK", msg); RCLCPP_WARN_STREAM(LOGGER, msg);
return; return;
} }
if (!eef_jmg && !jmg) { if (!eef_jmg && !jmg) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Neither eef nor group are well defined"); RCLCPP_WARN_STREAM(LOGGER, "Neither eef nor group are well defined");
return; return;
} }
properties().property("timeout").setDefaultValue(jmg->getDefaultIKTimeout()); properties().property("timeout").setDefaultValue(jmg->getDefaultIKTimeout());
// extract target_pose // extract target_pose
geometry_msgs::PoseStamped target_pose_msg = props.get<geometry_msgs::PoseStamped>("target_pose"); geometry_msgs::msg::PoseStamped target_pose_msg = props.get<geometry_msgs::msg::PoseStamped>("target_pose");
if (target_pose_msg.header.frame_id.empty()) // if not provided, assume planning frame if (target_pose_msg.header.frame_id.empty()) // if not provided, assume planning frame
target_pose_msg.header.frame_id = scene->getPlanningFrame(); target_pose_msg.header.frame_id = scene->getPlanningFrame();
@ -262,8 +268,7 @@ void ComputeIK::compute() {
tf2::fromMsg(target_pose_msg.pose, target_pose); tf2::fromMsg(target_pose_msg.pose, target_pose);
if (target_pose_msg.header.frame_id != scene->getPlanningFrame()) { if (target_pose_msg.header.frame_id != scene->getPlanningFrame()) {
if (!scene->knowsFrameTransform(target_pose_msg.header.frame_id)) { if (!scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
ROS_WARN_STREAM_NAMED("ComputeIK", RCLCPP_WARN_STREAM(LOGGER, "Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
"Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
return; return;
} }
// transform target_pose w.r.t. planning frame // transform target_pose w.r.t. planning frame
@ -271,25 +276,25 @@ void ComputeIK::compute() {
} }
// determine IK link from ik_frame // determine IK link from ik_frame
const robot_model::LinkModel* link = nullptr; const moveit::core::LinkModel* link = nullptr;
geometry_msgs::PoseStamped ik_pose_msg; geometry_msgs::msg::PoseStamped ik_pose_msg;
const boost::any& value = props.get("ik_frame"); const boost::any& value = props.get("ik_frame");
if (value.empty()) { // property undefined if (value.empty()) { // property undefined
// determine IK link from eef/group // determine IK link from eef/group
if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) : if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) :
jmg->getOnlyOneEndEffectorTip())) { jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link"); RCLCPP_WARN_STREAM(LOGGER, "Failed to derive IK target link");
return; return;
} }
ik_pose_msg.header.frame_id = link->getName(); ik_pose_msg.header.frame_id = link->getName();
ik_pose_msg.pose.orientation.w = 1.0; ik_pose_msg.pose.orientation.w = 1.0;
} else { } else {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value); ik_pose_msg = boost::any_cast<geometry_msgs::msg::PoseStamped>(value);
Eigen::Isometry3d ik_pose; Eigen::Isometry3d ik_pose;
tf2::fromMsg(ik_pose_msg.pose, ik_pose); tf2::fromMsg(ik_pose_msg.pose, ik_pose);
if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) { if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) {
ROS_WARN_STREAM_NAMED("ComputeIK", "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'"); RCLCPP_WARN_STREAM(LOGGER, "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'");
return; return;
} }
ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose; ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose;
@ -302,17 +307,17 @@ void ComputeIK::compute() {
// validate placed link for collisions // validate placed link for collisions
collision_detection::CollisionResult collisions; collision_detection::CollisionResult collisions;
robot_state::RobotState sandbox_state{ scene->getCurrentState() }; moveit::core::RobotState sandbox_state{ scene->getCurrentState() };
bool colliding = bool colliding =
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions); !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions);
// markers used for failures // markers used for failures
std::deque<visualization_msgs::Marker> failure_markers; std::deque<visualization_msgs::msg::Marker> failure_markers;
// frames at target pose and ik frame // frames at target pose and ik frame
rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "target frame"); rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "target frame");
rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame"); rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame");
// visualize placed end-effector // visualize placed end-effector
auto appender = [&failure_markers](visualization_msgs::Marker& marker, const std::string& /*name*/) { auto appender = [&failure_markers](visualization_msgs::msg::Marker& marker, const std::string& /*name*/) {
marker.ns = "ik target"; marker.ns = "ik target";
marker.color.a *= 0.5; marker.color.a *= 0.5;
failure_markers.push_back(marker); failure_markers.push_back(marker);
@ -338,7 +343,7 @@ void ComputeIK::compute() {
std::vector<double> compare_pose; std::vector<double> compare_pose;
const std::string& compare_pose_name = props.get<std::string>("default_pose"); const std::string& compare_pose_name = props.get<std::string>("default_pose");
if (!compare_pose_name.empty()) { if (!compare_pose_name.empty()) {
robot_state::RobotState compare_state(robot_model); moveit::core::RobotState compare_state(robot_model);
compare_state.setToDefaultValues(jmg, compare_pose_name); compare_state.setToDefaultValues(jmg, compare_pose_name);
compare_state.copyJointGroupPositions(jmg, compare_pose); compare_state.copyJointGroupPositions(jmg, compare_pose);
} else } else
@ -348,7 +353,7 @@ void ComputeIK::compute() {
IKSolutions ik_solutions; IKSolutions ik_solutions;
auto is_valid = [scene, ignore_collisions, min_solution_distance, auto is_valid = [scene, ignore_collisions, min_solution_distance,
&ik_solutions](robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, &ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg,
const double* joint_positions) { const double* joint_positions) {
for (const auto& sol : ik_solutions) { for (const auto& sol : ik_solutions) {
if (jmg->distance(joint_positions, sol.data()) < min_solution_distance) if (jmg->distance(joint_positions, sol.data()) < min_solution_distance)
@ -396,7 +401,7 @@ void ComputeIK::compute() {
solution.markAsFailure(); solution.markAsFailure();
// set scene's robot state // set scene's robot state
robot_state::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst();
solution_state.setJointGroupPositions(jmg, ik_solutions[i].data()); solution_state.setJointGroupPositions(jmg, ik_solutions[i].data());
solution_state.update(); solution_state.update();

View File

@ -46,14 +46,16 @@ namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace stages { namespace stages {
static const rclcpp::Logger LOGGER = rclcpp::get_logger("Connect");
Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) { Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) {
setTimeout(1.0); setTimeout(1.0);
setCostTerm(std::make_unique<cost::PathLength>()); setCostTerm(std::make_unique<cost::PathLength>());
auto& p = properties(); auto& p = properties();
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode"); p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(), p.declare<moveit_msgs::msg::Constraints>("path_constraints", moveit_msgs::msg::Constraints(),
"constraints to maintain during trajectory"); "constraints to maintain during trajectory");
} }
void Connect::reset() { void Connect::reset() {
@ -88,7 +90,7 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) {
try { try {
merged_jmg_.reset(task_constructor::merge(groups)); merged_jmg_.reset(task_constructor::merge(groups));
} catch (const std::runtime_error& e) { } catch (const std::runtime_error& e) {
ROS_INFO_STREAM_NAMED("Connect", this->name() << ": " << e.what() << ". Disabling merging."); RCLCPP_INFO_STREAM(LOGGER, this->name() << ": " << e.what() << ". Disabling merging.");
} }
} }
@ -119,8 +121,8 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState&
Eigen::Map<const Eigen::VectorXd> positions_from(from.getJointPositions(jm), num); Eigen::Map<const Eigen::VectorXd> positions_from(from.getJointPositions(jm), num);
Eigen::Map<const Eigen::VectorXd> positions_to(to.getJointPositions(jm), num); Eigen::Map<const Eigen::VectorXd> positions_to(to.getJointPositions(jm), num);
if (!(positions_from - positions_to).isZero(1e-4)) { if (!(positions_from - positions_to).isZero(1e-4)) {
ROS_INFO_STREAM_NAMED("Connect", "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose() RCLCPP_INFO_STREAM(LOGGER, "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose()
<< "] != [" << positions_to.transpose() << "]"); << "] != [" << positions_to.transpose() << "]");
return false; return false;
} }
} }
@ -131,7 +133,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
const auto& props = properties(); const auto& props = properties();
double timeout = this->timeout(); double timeout = this->timeout();
MergeMode mode = props.get<MergeMode>("merge_mode"); MergeMode mode = props.get<MergeMode>("merge_mode");
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints"); const auto& path_constraints = props.get<moveit_msgs::msg::Constraints>("path_constraints");
const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState(); const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState();
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories; std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
@ -147,7 +149,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
planning_scene::PlanningScenePtr end = start->diff(); planning_scene::PlanningScenePtr end = start->diff();
const moveit::core::JointModelGroup* jmg = final_goal_state.getJointModelGroup(pair.first); const moveit::core::JointModelGroup* jmg = final_goal_state.getJointModelGroup(pair.first);
final_goal_state.copyJointGroupPositions(jmg, positions); final_goal_state.copyJointGroupPositions(jmg, positions);
robot_state::RobotState& goal_state = end->getCurrentStateNonConst(); moveit::core::RobotState& goal_state = end->getCurrentStateNonConst();
goal_state.setJointGroupPositions(jmg, positions); goal_state.setJointGroupPositions(jmg, positions);
goal_state.update(); goal_state.update();
intermediate_scenes.push_back(end); intermediate_scenes.push_back(end);
@ -225,7 +227,7 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
// check merged trajectory for collisions // check merged trajectory for collisions
if (!intermediate_scenes.front()->isPathValid(*trajectory, if (!intermediate_scenes.front()->isPathValid(*trajectory,
properties().get<moveit_msgs::Constraints>("path_constraints"))) properties().get<moveit_msgs::msg::Constraints>("path_constraints")))
return SubTrajectoryPtr(); return SubTrajectoryPtr();
return std::make_shared<SubTrajectory>(trajectory); return std::make_shared<SubTrajectory>(trajectory);

View File

@ -37,16 +37,18 @@
#include <moveit/task_constructor/stages/current_state.h> #include <moveit/task_constructor/stages/current_state.h>
#include <moveit/task_constructor/storage.h> #include <moveit/task_constructor/storage.h>
#include <moveit_msgs/GetPlanningScene.h> #include <moveit_msgs/srv/get_planning_scene.hpp>
#include <moveit_msgs/PlanningSceneComponents.h> #include <moveit_msgs/msg/planning_scene_components.hpp>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model_loader/robot_model_loader.h> #include <moveit/robot_model_loader/robot_model_loader.h>
#include <ros/ros.h> #include <rclcpp/rclcpp.hpp>
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace stages { namespace stages {
static const rclcpp::Logger LOGGER = rclcpp::get_logger("CurrentState");
CurrentState::CurrentState(const std::string& name) : Generator(name) { CurrentState::CurrentState(const std::string& name) : Generator(name) {
auto& p = properties(); auto& p = properties();
Property& timeout = p.property("timeout"); Property& timeout = p.property("timeout");
@ -67,31 +69,37 @@ bool CurrentState::canCompute() const {
void CurrentState::compute() { void CurrentState::compute() {
scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_); scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
ros::NodeHandle h; // Add random ID to prevent warnings about multiple publishers within the same node
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene"); rclcpp::NodeOptions options;
options.arguments(
{ "--ros-args", "-r", "__node:=current_state_" + std::to_string(reinterpret_cast<std::size_t>(this)) });
auto node = rclcpp::Node::make_shared("_", options);
auto client = node->create_client<moveit_msgs::srv::GetPlanningScene>("get_planning_scene");
ros::Duration timeout(this->timeout()); auto timeout = std::chrono::duration<double>(this->timeout());
if (client.waitForExistence(timeout)) { if (client->wait_for_service(timeout)) {
moveit_msgs::GetPlanningScene::Request req; auto req = std::make_shared<moveit_msgs::srv::GetPlanningScene::Request>();
moveit_msgs::GetPlanningScene::Response res;
req.components.components = req->components.components = moveit_msgs::msg::PlanningSceneComponents::SCENE_SETTINGS |
moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS | moveit_msgs::PlanningSceneComponents::ROBOT_STATE | moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE |
moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS | moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS |
moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES | moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_NAMES |
moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY | moveit_msgs::PlanningSceneComponents::OCTOMAP | moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY |
moveit_msgs::PlanningSceneComponents::TRANSFORMS | moveit_msgs::msg::PlanningSceneComponents::OCTOMAP |
moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX | moveit_msgs::msg::PlanningSceneComponents::TRANSFORMS |
moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING | moveit_msgs::msg::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX |
moveit_msgs::PlanningSceneComponents::OBJECT_COLORS; moveit_msgs::msg::PlanningSceneComponents::LINK_PADDING_AND_SCALING |
moveit_msgs::msg::PlanningSceneComponents::OBJECT_COLORS;
if (client.call(req, res)) { auto res_future = client->async_send_request(req);
scene_->setPlanningSceneMsg(res.scene); if (rclcpp::spin_until_future_complete(node, res_future) == rclcpp::FutureReturnCode::SUCCESS) {
auto res = res_future.get();
scene_->setPlanningSceneMsg(res->scene);
spawn(InterfaceState(scene_), 0.0); spawn(InterfaceState(scene_), 0.0);
return; return;
} }
} }
ROS_WARN("failed to acquire current PlanningScene"); RCLCPP_WARN(LOGGER, "failed to acquire current PlanningScene");
} }
} // namespace stages } // namespace stages
} // namespace task_constructor } // namespace task_constructor

View File

@ -44,9 +44,13 @@
#include <moveit/task_constructor/cost_terms.h> #include <moveit/task_constructor/cost_terms.h>
#include <rviz_marker_tools/marker_creation.h> #include <rviz_marker_tools/marker_creation.h>
#include <Eigen/Geometry> #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h> #include <tf2_eigen/tf2_eigen.h>
#include <ros/console.h> #endif
#include <Eigen/Geometry>
#include <rclcpp/logging.hpp>
namespace vm = visualization_msgs; namespace vm = visualization_msgs;
namespace cd = collision_detection; namespace cd = collision_detection;
@ -55,13 +59,15 @@ namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace stages { namespace stages {
static const rclcpp::Logger LOGGER = rclcpp::get_logger("FixCollisionObjects");
FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) { FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) {
// TODO: possibly weight solutions based on the required displacement? // TODO: possibly weight solutions based on the required displacement?
setCostTerm(std::make_unique<cost::Constant>(0.0)); setCostTerm(std::make_unique<cost::Constant>(0.0));
auto& p = properties(); auto& p = properties();
p.declare<double>("max_penetration", "maximally corrected penetration depth"); p.declare<double>("max_penetration", "maximally corrected penetration depth");
p.declare<geometry_msgs::Vector3>("direction", "direction vector to use for corrections"); p.declare<geometry_msgs::msg::Vector3>("direction", "direction vector to use for corrections");
} }
void FixCollisionObjects::computeForward(const InterfaceState& from) { void FixCollisionObjects::computeForward(const InterfaceState& from) {
@ -79,8 +85,7 @@ bool computeCorrection(const std::vector<cd::Contact>& contacts, Eigen::Vector3d
correction.setZero(); correction.setZero();
for (const cd::Contact& c : contacts) { for (const cd::Contact& c : contacts) {
if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT && c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) { if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT && c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) {
ROS_WARN_STREAM_NAMED("FixCollisionObjects", RCLCPP_WARN_STREAM(LOGGER, "Cannot fix collision between " << c.body_name_1 << " and " << c.body_name_2);
"Cannot fix collision between " << c.body_name_1 << " and " << c.body_name_2);
return false; return false;
} }
if (c.body_type_1 == cd::BodyTypes::WORLD_OBJECT) if (c.body_type_1 == cd::BodyTypes::WORLD_OBJECT)
@ -110,20 +115,15 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene&
req.verbose = false; req.verbose = false;
req.distance = false; req.distance = false;
vm::Marker m; vm::msg::Marker m;
m.header.frame_id = scene.getPlanningFrame(); m.header.frame_id = scene.getPlanningFrame();
m.ns = "collisions"; m.ns = "collisions";
bool failure = false; bool failure = false;
while (!failure) { while (!failure) {
res.clear(); res.clear();
#if MOVEIT_HAS_COLLISION_ENV
scene.getCollisionEnv()->checkRobotCollision(req, res, scene.getCurrentState(), scene.getCollisionEnv()->checkRobotCollision(req, res, scene.getCurrentState(),
scene.getAllowedCollisionMatrix()); scene.getAllowedCollisionMatrix());
#else
scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(),
scene.getCurrentState(), scene.getAllowedCollisionMatrix());
#endif
if (!res.collision) if (!res.collision)
return result; return result;
@ -147,7 +147,7 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene&
// fix collision by shifting object along correction direction // fix collision by shifting object along correction direction
if (!dir.empty()) // if explicitly given, use this correction direction if (!dir.empty()) // if explicitly given, use this correction direction
tf2::fromMsg(boost::any_cast<geometry_msgs::Vector3>(dir), correction); tf2::fromMsg(boost::any_cast<geometry_msgs::msg::Vector3>(dir), correction);
const std::string& name = c.body_type_1 == cd::BodyTypes::WORLD_OBJECT ? c.body_name_1 : c.body_name_2; const std::string& name = c.body_type_1 == cd::BodyTypes::WORLD_OBJECT ? c.body_name_1 : c.body_name_2;
scene.getWorldNonConst()->moveObject(name, Eigen::Isometry3d(Eigen::Translation3d(correction))); scene.getWorldNonConst()->moveObject(name, Eigen::Isometry3d(Eigen::Translation3d(correction)));

View File

@ -46,7 +46,9 @@ namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace stages { namespace stages {
using PosesList = std::vector<geometry_msgs::PoseStamped>; static const rclcpp::Logger LOGGER = rclcpp::get_logger("FixedCartesianPoses");
using PosesList = std::vector<geometry_msgs::msg::PoseStamped>;
FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) { FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) {
setCostTerm(std::make_unique<cost::Constant>(0.0)); setCostTerm(std::make_unique<cost::Constant>(0.0));
@ -55,7 +57,7 @@ FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGe
p.declare<PosesList>("poses", PosesList(), "target poses to spawn"); p.declare<PosesList>("poses", PosesList(), "target poses to spawn");
} }
void FixedCartesianPoses::addPose(const geometry_msgs::PoseStamped& pose) { void FixedCartesianPoses::addPose(const geometry_msgs::msg::PoseStamped& pose) {
moveit::task_constructor::Property& poses = properties().property("poses"); moveit::task_constructor::Property& poses = properties().property("poses");
if (!poses.defined()) if (!poses.defined())
poses.setValue(PosesList({ pose })); poses.setValue(PosesList({ pose }));
@ -82,11 +84,11 @@ void FixedCartesianPoses::compute() {
return; return;
planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff();
for (geometry_msgs::PoseStamped pose : properties().get<PosesList>("poses")) { for (geometry_msgs::msg::PoseStamped pose : properties().get<PosesList>("poses")) {
if (pose.header.frame_id.empty()) if (pose.header.frame_id.empty())
pose.header.frame_id = scene->getPlanningFrame(); pose.header.frame_id = scene->getPlanningFrame();
else if (!scene->knowsFrameTransform(pose.header.frame_id)) { else if (!scene->knowsFrameTransform(pose.header.frame_id)) {
ROS_WARN_NAMED("FixedCartesianPoses", "Unknown frame: '%s'", pose.header.frame_id.c_str()); RCLCPP_WARN(LOGGER, "Unknown frame: '%s'", pose.header.frame_id.c_str());
continue; continue;
} }

View File

@ -43,12 +43,18 @@
#include <moveit/robot_state/conversions.h> #include <moveit/robot_state/conversions.h>
#include <Eigen/Geometry> #include <Eigen/Geometry>
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h> #include <tf2_eigen/tf2_eigen.h>
#endif
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace stages { namespace stages {
static const rclcpp::Logger LOGGER = rclcpp::get_logger("GenerateGraspPose");
GenerateGraspPose::GenerateGraspPose(const std::string& name) : GeneratePose(name) { GenerateGraspPose::GenerateGraspPose(const std::string& name) : GeneratePose(name) {
auto& p = properties(); auto& p = properties();
p.declare<std::string>("eef", "name of end-effector"); p.declare<std::string>("eef", "name of end-effector");
@ -73,7 +79,8 @@ static void applyPreGrasp(moveit::core::RobotState& state, const moveit::core::J
try { try {
// try RobotState // try RobotState
const moveit_msgs::RobotState& robot_state_msg = boost::any_cast<moveit_msgs::RobotState>(diff_property.value()); const moveit_msgs::msg::RobotState& robot_state_msg =
boost::any_cast<moveit_msgs::msg::RobotState>(diff_property.value());
if (!robot_state_msg.is_diff) if (!robot_state_msg.is_diff)
throw moveit::Exception{ "RobotState message must be a diff" }; throw moveit::Exception{ "RobotState message must be a diff" };
const auto& accepted = jmg->getJointModelNames(); const auto& accepted = jmg->getJointModelNames();
@ -150,7 +157,7 @@ void GenerateGraspPose::compute() {
const std::string& eef = props.get<std::string>("eef"); const std::string& eef = props.get<std::string>("eef");
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef); const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef);
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); moveit::core::RobotState& robot_state = scene->getCurrentStateNonConst();
try { try {
applyPreGrasp(robot_state, jmg, props.property("pregrasp")); applyPreGrasp(robot_state, jmg, props.property("pregrasp"));
} catch (const moveit::Exception& e) { } catch (const moveit::Exception& e) {
@ -158,7 +165,7 @@ void GenerateGraspPose::compute() {
return; return;
} }
geometry_msgs::PoseStamped target_pose_msg; geometry_msgs::msg::PoseStamped target_pose_msg;
target_pose_msg.header.frame_id = props.get<std::string>("object"); target_pose_msg.header.frame_id = props.get<std::string>("object");
double current_angle = 0.0; double current_angle = 0.0;

View File

@ -45,16 +45,22 @@
#include <moveit/robot_state/attached_body.h> #include <moveit/robot_state/attached_body.h>
#include <Eigen/Geometry> #include <Eigen/Geometry>
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h> #include <tf2_eigen/tf2_eigen.h>
#endif
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace stages { namespace stages {
static const rclcpp::Logger LOGGER = rclcpp::get_logger("GeneratePlacePose");
GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(name) { GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(name) {
auto& p = properties(); auto& p = properties();
p.declare<std::string>("object"); p.declare<std::string>("object");
p.declare<geometry_msgs::PoseStamped>("ik_frame"); p.declare<geometry_msgs::msg::PoseStamped>("ik_frame");
} }
void GeneratePlacePose::onNewSolution(const SolutionBase& s) { void GeneratePlacePose::onNewSolution(const SolutionBase& s) {
@ -75,7 +81,7 @@ void GeneratePlacePose::onNewSolution(const SolutionBase& s) {
solution.setComment(msg); solution.setComment(msg);
spawn(std::move(state), std::move(solution)); spawn(std::move(state), std::move(solution));
} else } else
ROS_WARN_STREAM_NAMED("GeneratePlacePose", msg); RCLCPP_WARN_STREAM(LOGGER, msg);
return; return;
} }
@ -95,13 +101,13 @@ void GeneratePlacePose::compute() {
// current object_pose w.r.t. planning frame // current object_pose w.r.t. planning frame
const Eigen::Isometry3d& orig_object_pose = object->getGlobalCollisionBodyTransforms()[0]; const Eigen::Isometry3d& orig_object_pose = object->getGlobalCollisionBodyTransforms()[0];
const geometry_msgs::PoseStamped& pose_msg = props.get<geometry_msgs::PoseStamped>("pose"); const geometry_msgs::msg::PoseStamped& pose_msg = props.get<geometry_msgs::msg::PoseStamped>("pose");
Eigen::Isometry3d target_pose; Eigen::Isometry3d target_pose;
tf2::fromMsg(pose_msg.pose, target_pose); tf2::fromMsg(pose_msg.pose, target_pose);
// target pose w.r.t. planning frame // target pose w.r.t. planning frame
scene->getTransforms().transformPose(pose_msg.header.frame_id, target_pose, target_pose); scene->getTransforms().transformPose(pose_msg.header.frame_id, target_pose, target_pose);
const geometry_msgs::PoseStamped& ik_frame_msg = props.get<geometry_msgs::PoseStamped>("ik_frame"); const geometry_msgs::msg::PoseStamped& ik_frame_msg = props.get<geometry_msgs::msg::PoseStamped>("ik_frame");
Eigen::Isometry3d ik_frame; Eigen::Isometry3d ik_frame;
tf2::fromMsg(ik_frame_msg.pose, ik_frame); tf2::fromMsg(ik_frame_msg.pose, ik_frame);
ik_frame = robot_state.getGlobalLinkTransform(ik_frame_msg.header.frame_id) * ik_frame; ik_frame = robot_state.getGlobalLinkTransform(ik_frame_msg.header.frame_id) * ik_frame;
@ -121,7 +127,7 @@ void GeneratePlacePose::compute() {
.pretranslate(pos); .pretranslate(pos);
// target ik_frame's pose w.r.t. planning frame // target ik_frame's pose w.r.t. planning frame
geometry_msgs::PoseStamped target_pose_msg; geometry_msgs::msg::PoseStamped target_pose_msg;
target_pose_msg.header.frame_id = scene->getPlanningFrame(); target_pose_msg.header.frame_id = scene->getPlanningFrame();
target_pose_msg.pose = tf2::toMsg(object * object_to_ik); target_pose_msg.pose = tf2::toMsg(object * object_to_ik);

View File

@ -45,11 +45,13 @@ namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace stages { namespace stages {
static const rclcpp::Logger LOGGER = rclcpp::get_logger("GeneratePose");
GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) { GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) {
setCostTerm(std::make_unique<cost::Constant>(0.0)); setCostTerm(std::make_unique<cost::Constant>(0.0));
auto& p = properties(); auto& p = properties();
p.declare<geometry_msgs::PoseStamped>("pose", "target pose to pass on in spawned states"); p.declare<geometry_msgs::msg::PoseStamped>("pose", "target pose to pass on in spawned states");
} }
void GeneratePose::reset() { void GeneratePose::reset() {
@ -71,11 +73,11 @@ void GeneratePose::compute() {
return; return;
planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff();
geometry_msgs::PoseStamped target_pose = properties().get<geometry_msgs::PoseStamped>("pose"); geometry_msgs::msg::PoseStamped target_pose = properties().get<geometry_msgs::msg::PoseStamped>("pose");
if (target_pose.header.frame_id.empty()) if (target_pose.header.frame_id.empty())
target_pose.header.frame_id = scene->getPlanningFrame(); target_pose.header.frame_id = scene->getPlanningFrame();
else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) { else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) {
ROS_WARN_NAMED("GeneratePose", "Unknown frame: '%s'", target_pose.header.frame_id.c_str()); RCLCPP_WARN(LOGGER, "Unknown frame: '%s'", target_pose.header.frame_id.c_str());
return; return;
} }

View File

@ -46,6 +46,8 @@ namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace stages { namespace stages {
static const rclcpp::Logger LOGGER = rclcpp::get_logger("ModifyPlanningScene");
ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) { ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) {
setCostTerm(std::make_unique<cost::Constant>(0.0)); setCostTerm(std::make_unique<cost::Constant>(0.0));
} }
@ -56,19 +58,19 @@ void ModifyPlanningScene::attachObjects(const Names& objects, const std::string&
o.insert(o.end(), objects.begin(), objects.end()); o.insert(o.end(), objects.begin(), objects.end());
} }
void ModifyPlanningScene::addObject(const moveit_msgs::CollisionObject& collision_object) { void ModifyPlanningScene::addObject(const moveit_msgs::msg::CollisionObject& collision_object) {
if (collision_object.operation != moveit_msgs::CollisionObject::ADD) { if (collision_object.operation != moveit_msgs::msg::CollisionObject::ADD) {
ROS_ERROR_STREAM_NAMED("ModifyPlanningScene", name() << ": addObject is called with object's operation not set " RCLCPP_ERROR_STREAM(LOGGER, name() << ": addObject is called with object's operation not set "
"to ADD -- ignoring the object"); "to ADD -- ignoring the object");
return; return;
} }
collision_objects_.push_back(collision_object); collision_objects_.push_back(collision_object);
} }
void ModifyPlanningScene::removeObject(const std::string& object_name) { void ModifyPlanningScene::removeObject(const std::string& object_name) {
moveit_msgs::CollisionObject obj; moveit_msgs::msg::CollisionObject obj;
obj.id = object_name; obj.id = object_name;
obj.operation = moveit_msgs::CollisionObject::REMOVE; obj.operation = moveit_msgs::msg::CollisionObject::REMOVE;
collision_objects_.push_back(obj); collision_objects_.push_back(obj);
} }
@ -93,13 +95,13 @@ void ModifyPlanningScene::computeBackward(const InterfaceState& to) {
void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene, void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene,
const std::pair<std::string, std::pair<Names, bool> >& pair, bool invert) { const std::pair<std::string, std::pair<Names, bool> >& pair, bool invert) {
moveit_msgs::AttachedCollisionObject obj; moveit_msgs::msg::AttachedCollisionObject obj;
obj.link_name = pair.first; obj.link_name = pair.first;
bool attach = pair.second.second; bool attach = pair.second.second;
if (invert) if (invert)
attach = !attach; attach = !attach;
obj.object.operation = obj.object.operation =
attach ? (int8_t)moveit_msgs::CollisionObject::ADD : (int8_t)moveit_msgs::CollisionObject::REMOVE; attach ? (int8_t)moveit_msgs::msg::CollisionObject::ADD : (int8_t)moveit_msgs::msg::CollisionObject::REMOVE;
for (const std::string& name : pair.second.first) { for (const std::string& name : pair.second.first) {
obj.object.id = name; obj.object.id = name;
scene.processAttachedCollisionObjectMsg(obj); scene.processAttachedCollisionObjectMsg(obj);
@ -141,7 +143,7 @@ InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool inver
} }
void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene, void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene,
const moveit_msgs::CollisionObject& object) { const moveit_msgs::msg::CollisionObject& object) {
scene.processCollisionObjectMsg(object); scene.processCollisionObjectMsg(object);
} }
} // namespace stages } // namespace stages

View File

@ -41,12 +41,18 @@
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h> #include <rviz_marker_tools/marker_creation.h>
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h> #include <tf2_eigen/tf2_eigen.h>
#endif
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace stages { namespace stages {
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveRelative");
MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner) MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner)
: PropagatingEitherWay(name), planner_(planner) { : PropagatingEitherWay(name), planner_(planner) {
setCostTerm(std::make_unique<cost::PathLength>()); setCostTerm(std::make_unique<cost::PathLength>());
@ -54,21 +60,21 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
auto& p = properties(); auto& p = properties();
p.property("timeout").setDefaultValue(1.0); p.property("timeout").setDefaultValue(1.0);
p.declare<std::string>("group", "name of planning group"); p.declare<std::string>("group", "name of planning group");
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved in Cartesian direction"); p.declare<geometry_msgs::msg::PoseStamped>("ik_frame", "frame to be moved in Cartesian direction");
p.declare<boost::any>("direction", "motion specification"); p.declare<boost::any>("direction", "motion specification");
// register actual types // register actual types
PropertySerializer<geometry_msgs::TwistStamped>(); PropertySerializer<geometry_msgs::msg::TwistStamped>();
PropertySerializer<geometry_msgs::Vector3Stamped>(); PropertySerializer<geometry_msgs::msg::Vector3Stamped>();
p.declare<double>("min_distance", -1.0, "minimum distance to move"); p.declare<double>("min_distance", -1.0, "minimum distance to move");
p.declare<double>("max_distance", 0.0, "maximum distance to move"); p.declare<double>("max_distance", 0.0, "maximum distance to move");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(), p.declare<moveit_msgs::msg::Constraints>("path_constraints", moveit_msgs::msg::Constraints(),
"constraints to maintain during trajectory"); "constraints to maintain during trajectory");
} }
void MoveRelative::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) { void MoveRelative::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg; geometry_msgs::msg::PoseStamped pose_msg;
pose_msg.header.frame_id = link; pose_msg.header.frame_id = link;
pose_msg.pose = tf2::toMsg(pose); pose_msg.pose = tf2::toMsg(pose);
setIKFrame(pose_msg); setIKFrame(pose_msg);
@ -104,7 +110,7 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c
return false; return false;
} }
static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Interface::Direction dir, bool success, static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& markers, Interface::Direction dir, bool success,
const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& link_pose, const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& link_pose,
const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) { const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) {
double linear_norm = linear.norm(); double linear_norm = linear.norm();
@ -118,7 +124,7 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
Eigen::Vector3d pos_reached = reached_pose.translation(); Eigen::Vector3d pos_reached = reached_pose.translation();
Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0); Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0);
visualization_msgs::Marker m; visualization_msgs::msg::Marker m;
m.ns = ns; m.ns = ns;
m.header.frame_id = frame_id; m.header.frame_id = frame_id;
if (dir == Interface::FORWARD) { if (dir == Interface::FORWARD) {
@ -164,7 +170,7 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
bool MoveRelative::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, bool MoveRelative::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene,
SubTrajectory& solution, Interface::Direction dir) { SubTrajectory& solution, Interface::Direction dir) {
scene = state.scene()->diff(); scene = state.scene()->diff();
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel(); const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
assert(robot_model); assert(robot_model);
const auto& props = properties(); const auto& props = properties();
@ -183,7 +189,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
double max_distance = props.get<double>("max_distance"); double max_distance = props.get<double>("max_distance");
double min_distance = props.get<double>("min_distance"); double min_distance = props.get<double>("min_distance");
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints"); const auto& path_constraints = props.get<moveit_msgs::msg::Constraints>("path_constraints");
robot_trajectory::RobotTrajectoryPtr robot_trajectory; robot_trajectory::RobotTrajectoryPtr robot_trajectory;
bool success = false; bool success = false;
@ -209,7 +215,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success
try { // try to extract Twist try { // try to extract Twist
const geometry_msgs::TwistStamped& target = boost::any_cast<geometry_msgs::TwistStamped>(direction); const geometry_msgs::msg::TwistStamped& target = boost::any_cast<geometry_msgs::msg::TwistStamped>(direction);
const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id); const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
tf2::fromMsg(target.twist.linear, linear); tf2::fromMsg(target.twist.linear, linear);
tf2::fromMsg(target.twist.angular, angular); tf2::fromMsg(target.twist.angular, angular);
@ -252,7 +258,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
} }
try { // try to extract Vector try { // try to extract Vector
const geometry_msgs::Vector3Stamped& target = boost::any_cast<geometry_msgs::Vector3Stamped>(direction); const geometry_msgs::msg::Vector3Stamped& target =
boost::any_cast<geometry_msgs::msg::Vector3Stamped>(direction);
const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id); const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
tf2::fromMsg(target.vector, linear); tf2::fromMsg(target.vector, linear);
@ -282,7 +289,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
reached_state->updateLinkTransforms(); reached_state->updateLinkTransforms();
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link); const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link);

View File

@ -38,7 +38,6 @@
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h> #include <moveit/robot_state/conversions.h>
#include <tf2_eigen/tf2_eigen.h>
#include <moveit/task_constructor/stages/move_to.h> #include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/cost_terms.h> #include <moveit/task_constructor/cost_terms.h>
@ -46,11 +45,19 @@
#include <moveit/task_constructor/utils.h> #include <moveit/task_constructor/utils.h>
#include <rviz_marker_tools/marker_creation.h> #include <rviz_marker_tools/marker_creation.h>
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif
#include <moveit/robot_state/conversions.h>
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
namespace stages { namespace stages {
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveTo");
MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner) MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner)
: PropagatingEitherWay(name), planner_(planner) { : PropagatingEitherWay(name), planner_(planner) {
setCostTerm(std::make_unique<cost::PathLength>()); setCostTerm(std::make_unique<cost::PathLength>());
@ -58,27 +65,27 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
auto& p = properties(); auto& p = properties();
p.property("timeout").setDefaultValue(1.0); p.property("timeout").setDefaultValue(1.0);
p.declare<std::string>("group", "name of planning group"); p.declare<std::string>("group", "name of planning group");
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose"); p.declare<geometry_msgs::msg::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
p.declare<boost::any>("goal", "goal specification"); p.declare<boost::any>("goal", "goal specification");
// register actual types // register actual types
PropertySerializer<std::string>(); PropertySerializer<std::string>();
PropertySerializer<moveit_msgs::RobotState>(); PropertySerializer<moveit_msgs::msg::RobotState>();
PropertySerializer<geometry_msgs::PointStamped>(); PropertySerializer<geometry_msgs::msg::PointStamped>();
PropertySerializer<geometry_msgs::PoseStamped>(); PropertySerializer<geometry_msgs::msg::PoseStamped>();
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(), p.declare<moveit_msgs::msg::Constraints>("path_constraints", moveit_msgs::msg::Constraints(),
"constraints to maintain during trajectory"); "constraints to maintain during trajectory");
} }
void MoveTo::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) { void MoveTo::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg; geometry_msgs::msg::PoseStamped pose_msg;
pose_msg.header.frame_id = link; pose_msg.header.frame_id = link;
pose_msg.pose = tf2::toMsg(pose); pose_msg.pose = tf2::toMsg(pose);
setIKFrame(pose_msg); setIKFrame(pose_msg);
} }
void MoveTo::setGoal(const std::map<std::string, double>& joints) { void MoveTo::setGoal(const std::map<std::string, double>& joints) {
moveit_msgs::RobotState robot_state; moveit_msgs::msg::RobotState robot_state;
robot_state.joint_state.name.reserve(joints.size()); robot_state.joint_state.name.reserve(joints.size());
robot_state.joint_state.position.reserve(joints.size()); robot_state.joint_state.position.reserve(joints.size());
@ -109,7 +116,7 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint
try { try {
// try RobotState // try RobotState
const moveit_msgs::RobotState& msg = boost::any_cast<moveit_msgs::RobotState>(goal); const moveit_msgs::msg::RobotState& msg = boost::any_cast<moveit_msgs::msg::RobotState>(goal);
if (!msg.is_diff) if (!msg.is_diff)
throw InitStageException(*this, "Expecting a diff state"); throw InitStageException(*this, "Expecting a diff state");
@ -147,8 +154,9 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint
bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene, bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene,
Eigen::Isometry3d& target) { Eigen::Isometry3d& target) {
try { try {
const geometry_msgs::PoseStamped& msg = boost::any_cast<geometry_msgs::PoseStamped>(goal); const geometry_msgs::msg::PoseStamped& msg = boost::any_cast<geometry_msgs::msg::PoseStamped>(goal);
tf2::fromMsg(msg.pose, target); tf2::fromMsg(msg.pose, target);
// transform target into global frame // transform target into global frame
target = scene->getFrameTransform(msg.header.frame_id) * target; target = scene->getFrameTransform(msg.header.frame_id) * target;
} catch (const boost::bad_any_cast&) { } catch (const boost::bad_any_cast&) {
@ -160,7 +168,7 @@ bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningS
bool MoveTo::getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose, bool MoveTo::getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose,
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen) { const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen) {
try { try {
const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal); const geometry_msgs::msg::PointStamped& target = boost::any_cast<geometry_msgs::msg::PointStamped>(goal);
Eigen::Vector3d target_point; Eigen::Vector3d target_point;
tf2::fromMsg(target.point, target_point); tf2::fromMsg(target.point, target_point);
// transform target into global frame // transform target into global frame
@ -178,7 +186,7 @@ bool MoveTo::getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_po
bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& solution, bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& solution,
Interface::Direction dir) { Interface::Direction dir) {
scene = state.scene()->diff(); scene = state.scene()->diff();
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel(); const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
assert(robot_model); assert(robot_model);
const auto& props = properties(); const auto& props = properties();
@ -195,7 +203,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
return false; return false;
} }
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints"); const auto& path_constraints = props.get<moveit_msgs::msg::Constraints>("path_constraints");
robot_trajectory::RobotTrajectoryPtr robot_trajectory; robot_trajectory::RobotTrajectoryPtr robot_trajectory;
bool success = false; bool success = false;
@ -206,7 +214,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
// Where to go? // Where to go?
Eigen::Isometry3d target; Eigen::Isometry3d target;
// What frame+offset in the robot should go there? // What frame+offset in the robot should go there?
geometry_msgs::PoseStamped ik_pose_msg; geometry_msgs::msg::PoseStamped ik_pose_msg;
const moveit::core::LinkModel* link; const moveit::core::LinkModel* link;
Eigen::Isometry3d ik_pose_world; Eigen::Isometry3d ik_pose_world;
@ -219,7 +227,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
} }
auto add_frame{ [&](const Eigen::Isometry3d& pose, const char name[]) { auto add_frame{ [&](const Eigen::Isometry3d& pose, const char name[]) {
geometry_msgs::PoseStamped msg; geometry_msgs::msg::PoseStamped msg;
msg.header.frame_id = scene->getPlanningFrame(); msg.header.frame_id = scene->getPlanningFrame();
msg.pose = tf2::toMsg(pose); msg.pose = tf2::toMsg(pose);
rviz_marker_tools::appendFrame(solution.markers(), msg, 0.1, name); rviz_marker_tools::appendFrame(solution.markers(), msg, 0.1, name);

View File

@ -44,11 +44,9 @@
#include <moveit/robot_state/robot_state.h> #include <moveit/robot_state/robot_state.h>
#include <Eigen/Geometry> #include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <chrono> #include <chrono>
#include <functional> #include <functional>
#include <iterator> #include <iterator>
#include <ros/console.h>
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {

View File

@ -26,7 +26,7 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
auto init_ik_frame = [](const PropertyMap& other) -> boost::any { auto init_ik_frame = [](const PropertyMap& other) -> boost::any {
geometry_msgs::PoseStamped pose; geometry_msgs::msg::PoseStamped pose;
const boost::any& frame = other.get("eef_frame"); const boost::any& frame = other.get("eef_frame");
if (frame.empty()) if (frame.empty())
return boost::any(); return boost::any();
@ -79,7 +79,7 @@ void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
SerialContainer::init(robot_model); SerialContainer::init(robot_model);
} }
void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, void PickPlaceBase::setApproachRetract(const geometry_msgs::msg::TwistStamped& motion, double min_distance,
double max_distance) { double max_distance) {
auto& p = approach_stage_->properties(); auto& p = approach_stage_->properties();
p.set("direction", motion); p.set("direction", motion);
@ -87,7 +87,8 @@ void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion
p.set("max_distance", max_distance); p.set("max_distance", max_distance);
} }
void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) { void PickPlaceBase::setLiftPlace(const geometry_msgs::msg::TwistStamped& motion, double min_distance,
double max_distance) {
auto& p = lift_stage_->properties(); auto& p = lift_stage_->properties();
p.set("direction", motion); p.set("direction", motion);
p.set("min_distance", min_distance); p.set("min_distance", min_distance);

View File

@ -1,6 +1,6 @@
#include <moveit/task_constructor/stages/current_state.h> #include <moveit/task_constructor/stages/current_state.h>
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.hpp>
/// register plugins to use with ClassLoader /// register plugins to use with ClassLoader

View File

@ -44,7 +44,11 @@
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <Eigen/Geometry> #include <Eigen/Geometry>
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h> #include <tf2_eigen/tf2_eigen.h>
#endif
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
@ -126,9 +130,9 @@ void SimpleGraspBase::setup(std::unique_ptr<Stage>&& generator, bool forward) {
attach->setCallback([forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p) { attach->setCallback([forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p) {
const std::string& eef = p.get<std::string>("eef"); const std::string& eef = p.get<std::string>("eef");
moveit_msgs::AttachedCollisionObject obj; moveit_msgs::msg::AttachedCollisionObject obj;
obj.object.operation = obj.object.operation = forward ? (int8_t)moveit_msgs::msg::CollisionObject::ADD :
forward ? (int8_t)moveit_msgs::CollisionObject::ADD : (int8_t)moveit_msgs::CollisionObject::REMOVE; (int8_t)moveit_msgs::msg::CollisionObject::REMOVE;
obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second; obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second;
obj.object.id = p.get<std::string>("object"); obj.object.id = p.get<std::string>("object");
scene->processAttachedCollisionObjectMsg(obj); scene->processAttachedCollisionObjectMsg(obj);
@ -143,7 +147,7 @@ void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model)
} }
void SimpleGraspBase::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) { void SimpleGraspBase::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg; geometry_msgs::msg::PoseStamped pose_msg;
pose_msg.header.frame_id = link; pose_msg.header.frame_id = link;
pose_msg.pose = tf2::toMsg(pose); pose_msg.pose = tf2::toMsg(pose);
setIKFrame(pose_msg); setIKFrame(pose_msg);

View File

@ -46,6 +46,8 @@
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
static const rclcpp::Logger LOGGER = rclcpp::get_logger("InterfaceState");
planning_scene::PlanningSceneConstPtr ensureUpdated(const planning_scene::PlanningScenePtr& scene) { planning_scene::PlanningSceneConstPtr ensureUpdated(const planning_scene::PlanningScenePtr& scene) {
// ensure scene's state is updated // ensure scene's state is updated
if (scene->getCurrentState().dirty()) if (scene->getCurrentState().dirty())
@ -58,7 +60,7 @@ InterfaceState::InterfaceState(const planning_scene::PlanningScenePtr& ps) : Int
InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr& ps) InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr& ps)
: scene_(ps), priority_(Priority(0, 0.0)) { : scene_(ps), priority_(Priority(0, 0.0)) {
if (scene_->getCurrentState().dirty()) if (scene_->getCurrentState().dirty())
ROS_ERROR_NAMED("InterfaceState", "Dirty PlanningScene! Please only forward clean ones into InterfaceState."); RCLCPP_ERROR(LOGGER, "Dirty PlanningScene! Please only forward clean ones into InterfaceState.");
} }
InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr& ps, const Priority& p) InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr& ps, const Priority& p)
@ -171,7 +173,7 @@ void SolutionBase::markAsFailure(const std::string& msg) {
setComment(msg + "\n" + comment()); setComment(msg + "\n" + comment());
} }
void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection) const { void SolutionBase::fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& info, Introspection* introspection) const {
info.id = introspection ? introspection->solutionId(*this) : 0; info.id = introspection ? introspection->solutionId(*this) : 0;
info.cost = this->cost(); info.cost = this->cost();
info.comment = this->comment(); info.comment = this->comment();
@ -183,9 +185,9 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, In
std::copy(markers.begin(), markers.end(), info.markers.begin()); std::copy(markers.begin(), markers.end(), info.markers.begin());
} }
void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { void SubTrajectory::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg, Introspection* introspection) const {
msg.sub_trajectory.emplace_back(); msg.sub_trajectory.emplace_back();
moveit_task_constructor_msgs::SubTrajectory& t = msg.sub_trajectory.back(); moveit_task_constructor_msgs::msg::SubTrajectory& t = msg.sub_trajectory.back();
SolutionBase::fillInfo(t.info, introspection); SolutionBase::fillInfo(t.info, introspection);
if (trajectory()) if (trajectory())
@ -202,8 +204,9 @@ void SolutionSequence::push_back(const SolutionBase& solution) {
subsolutions_.push_back(&solution); subsolutions_.push_back(&solution);
} }
void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { void SolutionSequence::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg,
moveit_task_constructor_msgs::SubSolution sub_msg; Introspection* introspection) const {
moveit_task_constructor_msgs::msg::SubSolution sub_msg;
SolutionBase::fillInfo(sub_msg.info, introspection); SolutionBase::fillInfo(sub_msg.info, introspection);
// Usually subsolutions originate from another stage than this solution. // Usually subsolutions originate from another stage than this solution.
@ -241,12 +244,12 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co
return f(*this, comment); return f(*this, comment);
} }
void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution, void WrappedSolution::fillMessage(moveit_task_constructor_msgs::msg::Solution& solution,
Introspection* introspection) const { Introspection* introspection) const {
wrapped_->fillMessage(solution, introspection); wrapped_->fillMessage(solution, introspection);
// prepend this solutions info as a SubSolution msg // prepend this solutions info as a SubSolution msg
moveit_task_constructor_msgs::SubSolution sub_msg; moveit_task_constructor_msgs::msg::SubSolution sub_msg;
SolutionBase::fillInfo(sub_msg.info, introspection); SolutionBase::fillInfo(sub_msg.info, introspection);
sub_msg.sub_solution_id.push_back(introspection ? introspection->solutionId(*wrapped_) : 0); sub_msg.sub_solution_id.push_back(introspection ? introspection->solutionId(*wrapped_) : 0);
solution.sub_solution.insert(solution.sub_solution.begin(), std::move(sub_msg)); solution.sub_solution.insert(solution.sub_solution.begin(), std::move(sub_msg));

View File

@ -38,16 +38,18 @@
#include <moveit/task_constructor/container_p.h> #include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/task_p.h> #include <moveit/task_constructor/task_p.h>
#include <moveit/task_constructor/introspection.h> #include <moveit/task_constructor/introspection.h>
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h> #include <moveit_task_constructor_msgs/action/execute_task_solution.hpp>
#include <ros/ros.h> #include <rclcpp/rclcpp.hpp>
#include <actionlib/client/simple_action_client.h> #include <rclcpp_action/rclcpp_action.hpp>
#include <moveit/robot_model_loader/robot_model_loader.h> #include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_pipeline/planning_pipeline.h> #include <moveit/planning_pipeline/planning_pipeline.h>
#include <functional> #include <functional>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor.task");
namespace { namespace {
std::string rosNormalizeName(const std::string& name) { std::string rosNormalizeName(const std::string& name) {
std::string n; std::string n;
@ -111,7 +113,7 @@ Task::Task(const std::string& ns, bool introspection, ContainerBase::pointer&& c
// monitor state on commandline // monitor state on commandline
// addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout))); // addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout)));
// enable introspection by default, but only if ros::init() was called // enable introspection by default, but only if ros::init() was called
if (ros::isInitialized() && introspection) if (rclcpp::ok() && introspection)
enableIntrospection(true); enableIntrospection(true);
} }
@ -137,7 +139,7 @@ Task::~Task() {
void Task::setRobotModel(const core::RobotModelConstPtr& robot_model) { void Task::setRobotModel(const core::RobotModelConstPtr& robot_model) {
if (!robot_model) { if (!robot_model) {
ROS_ERROR_STREAM(name() << ": received invalid robot model"); RCLCPP_ERROR_STREAM(LOGGER, name() << ": received invalid robot model");
return; return;
} }
auto impl = pimpl(); auto impl = pimpl();
@ -146,9 +148,9 @@ void Task::setRobotModel(const core::RobotModelConstPtr& robot_model) {
impl->robot_model_ = robot_model; impl->robot_model_ = robot_model;
} }
void Task::loadRobotModel(const std::string& robot_description) { void Task::loadRobotModel(const rclcpp::Node::SharedPtr& node, const std::string& robot_description) {
auto impl = pimpl(); auto impl = pimpl();
impl->robot_model_loader_ = std::make_shared<robot_model_loader::RobotModelLoader>(robot_description); impl->robot_model_loader_ = std::make_shared<robot_model_loader::RobotModelLoader>(node, robot_description);
setRobotModel(impl->robot_model_loader_->getModel()); setRobotModel(impl->robot_model_loader_->getModel());
if (!impl->robot_model_) if (!impl->robot_model_)
throw Exception("Task failed to construct RobotModel"); throw Exception("Task failed to construct RobotModel");
@ -212,7 +214,7 @@ void Task::reset() {
void Task::init() { void Task::init() {
auto impl = pimpl(); auto impl = pimpl();
if (!impl->robot_model_) if (!impl->robot_model_)
loadRobotModel(); throw std::runtime_error("You need to call loadRobotModel or setRobotModel before initializing the task");
// initialize push connections of wrapped child // initialize push connections of wrapped child
StagePrivate* child = wrapped()->pimpl(); StagePrivate* child = wrapped()->pimpl();
@ -269,17 +271,48 @@ void Task::preempt() {
pimpl()->preempt_requested_ = true; pimpl()->preempt_requested_ = true;
} }
moveit_msgs::MoveItErrorCodes Task::execute(const SolutionBase& s) { moveit_msgs::msg::MoveItErrorCodes Task::execute(const SolutionBase& s) {
actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> ac("execute_task_solution"); // Add random ID to prevent warnings about multiple publishers within the same node
ac.waitForServer(); rclcpp::NodeOptions options;
options.arguments(
{ "--ros-args", "-r",
"__node:=moveit_task_constructor_executor_" + std::to_string(reinterpret_cast<std::size_t>(this)) });
auto node = rclcpp::Node::make_shared("_", options);
auto ac = rclcpp_action::create_client<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
node, "execute_task_solution");
ac->wait_for_action_server();
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal; moveit_task_constructor_msgs::action::ExecuteTaskSolution::Goal goal;
s.fillMessage(goal.solution, pimpl()->introspection_.get()); s.fillMessage(goal.solution, pimpl()->introspection_.get());
s.start()->scene()->getPlanningSceneMsg(goal.solution.start_scene); s.start()->scene()->getPlanningSceneMsg(goal.solution.start_scene);
ac.sendGoal(goal); moveit_msgs::msg::MoveItErrorCodes error_code;
ac.waitForResult(); error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
return ac.getResult()->error_code; auto goal_handle_future = ac->async_send_goal(goal);
if (rclcpp::spin_until_future_complete(node, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "Send goal call failed");
return error_code;
}
auto goal_handle = goal_handle_future.get();
if (!goal_handle) {
RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
return error_code;
}
auto result_future = ac->async_get_result(goal_handle);
if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "Get result call failed");
return error_code;
}
auto result = result_future.get();
if (result.code != rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_ERROR(node->get_logger(), "Goal was aborted or canceled");
return error_code;
}
return result.result->error_code;
} }
void Task::publishAllSolutions(bool wait) { void Task::publishAllSolutions(bool wait) {

View File

@ -35,7 +35,11 @@
/* Authors: Michael Goerner, Robert Haschke */ /* Authors: Michael Goerner, Robert Haschke */
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h> #include <tf2_eigen/tf2_eigen.h>
#endif
#include <moveit/robot_state/robot_state.h> #include <moveit/robot_state/robot_state.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
@ -85,7 +89,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
} }
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
} else { } else {
auto ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(property.value()); auto ik_pose_msg = boost::any_cast<geometry_msgs::msg::PoseStamped>(property.value());
if (ik_pose_msg.header.frame_id.empty()) { if (ik_pose_msg.header.frame_id.empty()) {
if (!(robot_link = get_tip())) { if (!(robot_link = get_tip())) {
solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found"); solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found");
@ -99,7 +103,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame; tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame;
} else { } else {
std::stringstream ss; std::stringstream ss;
ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'"; ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'";
solution.markAsFailure(ss.str()); solution.markAsFailure(ss.str());
return false; return false;
} }

View File

@ -3,27 +3,36 @@
############# #############
## Add gtest based cpp test target and link libraries ## Add gtest based cpp test target and link libraries
if (CATKIN_ENABLE_TESTING) if (BUILD_TESTING)
find_package(rostest REQUIRED) find_package(ament_cmake_gtest REQUIRED)
find_package(ament_cmake_gmock REQUIRED)
find_package(launch_testing_ament_cmake REQUIRED)
add_library(gtest_utils SHARED gtest_value_printers.cpp models.cpp stage_mockups.cpp)
add_library(gtest_utils gtest_value_printers.cpp models.cpp stage_mockups.cpp)
target_link_libraries(gtest_utils ${PROJECT_NAME}) target_link_libraries(gtest_utils ${PROJECT_NAME})
macro(mtc_add_test TYPE) macro(mtc_add_test TYPE)
# Split ARGN into .test file and other sources # Split ARGN into .test file and other sources
set(TEST_FILE ${ARGN}) set(TEST_FILE ${ARGN})
set(SOURCES ${ARGN}) set(SOURCES ${ARGN})
list(FILTER TEST_FILE INCLUDE REGEX "\.test$") list(FILTER TEST_FILE INCLUDE REGEX "\.launch.py$")
list(FILTER SOURCES EXCLUDE REGEX "\.test$") list(FILTER SOURCES EXCLUDE REGEX "\.launch.py$")
# Determine TARGET name from first source file # Determine TARGET name from first source file
list(GET SOURCES 0 TEST_NAME) list(GET SOURCES 0 TEST_NAME)
string(REGEX REPLACE "\.cpp$" "" TEST_NAME ${TEST_NAME}) string(REGEX REPLACE "\.cpp$" "" TEST_NAME ${TEST_NAME})
string(REGEX REPLACE "_" "-" TEST_NAME ${TEST_NAME}) string(REGEX REPLACE "_" "-" TEST_NAME ${TEST_NAME})
# Configure build target # Configure build target
if(TEST_FILE) # Add rostest if .test file was specified if(TEST_FILE) # Add launch test if .launch.py file was specified
_add_rostest_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${TEST_FILE} ${SOURCES}) ament_add_gtest_executable(${PROJECT_NAME}-${TEST_NAME} ${SOURCES})
add_launch_test(${TEST_FILE}
ARGS "test_binary:=$<TARGET_FILE:${PROJECT_NAME}-${TEST_NAME}>")
else() else()
_catkin_add_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) if("${TYPE}" STREQUAL "gtest")
ament_add_gtest(${PROJECT_NAME}-${TEST_NAME} ${SOURCES})
elseif("${TYPE}" STREQUAL "gmock")
ament_add_gmock(${PROJECT_NAME}-${TEST_NAME} ${SOURCES})
endif()
endif() endif()
target_link_libraries(${PROJECT_NAME}-${TEST_NAME} ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) target_link_libraries(${PROJECT_NAME}-${TEST_NAME} ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
endmacro() endmacro()
@ -45,29 +54,30 @@ if (CATKIN_ENABLE_TESTING)
mtc_add_gmock(test_cost_queue.cpp) mtc_add_gmock(test_cost_queue.cpp)
mtc_add_gmock(test_interface_state.cpp) mtc_add_gmock(test_interface_state.cpp)
mtc_add_gtest(test_move_to.cpp move_to.test) mtc_add_gtest(test_move_to.cpp move_to.launch.py)
# building these integration tests works without moveit config packages # building these integration tests works without moveit config packages
add_executable(pick_ur5 pick_ur5.cpp) ament_add_gtest_executable(pick_ur5 pick_ur5.cpp)
target_link_libraries(pick_ur5 ${PROJECT_NAME}_stages gtest) target_link_libraries(pick_ur5 ${PROJECT_NAME}_stages)
add_executable(pick_pr2 pick_pr2.cpp) ament_add_gtest_executable(pick_pr2 pick_pr2.cpp)
target_link_libraries(pick_pr2 ${PROJECT_NAME}_stages gtest) target_link_libraries(pick_pr2 ${PROJECT_NAME}_stages)
add_executable(pick_pa10 pick_pa10.cpp) ament_add_gtest_executable(pick_pa10 pick_pa10.cpp)
target_link_libraries(pick_pa10 ${PROJECT_NAME}_stages gtest) target_link_libraries(pick_pa10 ${PROJECT_NAME}_stages)
# running these integrations test naturally requires the moveit configs # TODO(JafarAbdi): Port integration tests
find_package(tams_ur5_setup_moveit_config QUIET) # # running these integrations test naturally requires the moveit configs
if(tams_ur5_setup_moveit_config_FOUND) # find_package(tams_ur5_setup_moveit_config QUIET)
add_rostest(pick_ur5.test) # if(tams_ur5_setup_moveit_config_FOUND)
endif() # add_rostest(pick_ur5.test)
find_package(pr2_moveit_config_FOUND QUIET) # endif()
if(pr2_moveit_config_FOUND) # find_package(pr2_moveit_config_FOUND QUIET)
add_rostest(pick_pr2.test) # if(pr2_moveit_config_FOUND)
endif() # add_rostest(pick_pr2.test)
find_package(pa10_shadow_moveit_config QUIET) # endif()
if(pa10_shadow_moveit_config_FOUND) # find_package(pa10_shadow_moveit_config QUIET)
add_rostest(pick_pa10.test) # if(pa10_shadow_moveit_config_FOUND)
endif() # add_rostest(pick_pa10.test)
# endif()
endif() endif()

View File

@ -6,7 +6,8 @@ using namespace moveit::core;
RobotModelPtr getModel() { RobotModelPtr getModel() {
// suppress RobotModel errors and warnings // suppress RobotModel errors and warnings
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME ".moveit_core.robot_model", ros::console::levels::Fatal); if (rcutils_logging_set_logger_level("moveit_robot_model.robot_model", RCUTILS_LOG_SEVERITY_FATAL) != RCUTILS_RET_OK)
throw std::runtime_error("Failed to set logger level to RCUTILS_LOG_SEVERITY_ERROR");
// create dummy robot model // create dummy robot model
moveit::core::RobotModelBuilder builder("robot", "base"); moveit::core::RobotModelBuilder builder("robot", "base");
@ -17,7 +18,7 @@ RobotModelPtr getModel() {
return builder.build(); return builder.build();
} }
moveit::core::RobotModelPtr loadModel() { moveit::core::RobotModelPtr loadModel(const rclcpp::Node::SharedPtr& node) {
static robot_model_loader::RobotModelLoader loader; robot_model_loader::RobotModelLoader loader(node);
return loader.getModel(); return loader.getModel();
} }

View File

@ -1,6 +1,7 @@
#pragma once #pragma once
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <rclcpp/rclcpp.hpp>
namespace moveit { namespace moveit {
namespace core { namespace core {
@ -12,4 +13,4 @@ MOVEIT_CLASS_FORWARD(RobotModel);
moveit::core::RobotModelPtr getModel(); moveit::core::RobotModelPtr getModel();
// load a model from robot_description // load a model from robot_description
moveit::core::RobotModelPtr loadModel(); moveit::core::RobotModelPtr loadModel(const rclcpp::Node::SharedPtr& node);

110
core/test/move_to.launch.py Normal file
View File

@ -0,0 +1,110 @@
import unittest
import os
import yaml
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
import launch_testing
from launch_testing.asserts import assertExitCodes
from launch_testing.util import KeepAliveProc
from launch_testing.actions import ReadyToTest
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
import pytest
import xacro
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
@pytest.mark.launch_test
def generate_test_description():
# planning_context
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
)
)
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf"
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/kinematics.yaml"
)
robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
robot_description_planning = {
"robot_description_planning": load_yaml(
"moveit_resources_panda_moveit_config", "config/joint_limits.yaml"
)
}
test_exec = Node(
executable=[
LaunchConfiguration("test_binary"),
],
output="screen",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
robot_description_planning,
],
)
return (
LaunchDescription(
[
DeclareLaunchArgument(
name="test_binary",
description="Test executable",
),
test_exec,
KeepAliveProc(),
ReadyToTest(),
]
),
{"test_exec": test_exec},
)
class TestTerminatingProcessStops(unittest.TestCase):
def test_gtest_run_complete(self, proc_info, test_exec):
proc_info.assertWaitForShutdown(process=test_exec, timeout=4000.0)
@launch_testing.post_shutdown_test()
class TaskModelTestAfterShutdown(unittest.TestCase):
def test_exit_code(self, proc_info):
# Check that all processes in the launch exit with code 0
launch_testing.asserts.assertExitCodes(proc_info)

View File

@ -1,7 +0,0 @@
<launch>
<include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<test pkg="moveit_task_constructor_core" type="moveit_task_constructor_core-test-move-to" test-name="move_to"/>
</launch>

View File

@ -13,14 +13,14 @@
#include <moveit/task_constructor/stages/modify_planning_scene.h> #include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/stages/fix_collision_objects.h> #include <moveit/task_constructor/stages/fix_collision_objects.h>
#include <ros/ros.h> #include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
void spawnObject(const planning_scene::PlanningScenePtr& scene) { void spawnObject(const planning_scene::PlanningScenePtr& scene) {
moveit_msgs::CollisionObject o; moveit_msgs::msg::CollisionObject o;
o.id = "object"; o.id = "object";
o.header.frame_id = "world"; o.header.frame_id = "world";
o.primitive_poses.resize(1); o.primitive_poses.resize(1);
@ -29,7 +29,7 @@ void spawnObject(const planning_scene::PlanningScenePtr& scene) {
o.primitive_poses[0].position.z = 0.10; o.primitive_poses[0].position.z = 0.10;
o.primitive_poses[0].orientation.w = 1.0; o.primitive_poses[0].orientation.w = 1.0;
o.primitives.resize(1); o.primitives.resize(1);
o.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; o.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;
o.primitives[0].dimensions.resize(2); o.primitives[0].dimensions.resize(2);
o.primitives[0].dimensions[0] = 0.23; o.primitives[0].dimensions[0] = 0.23;
o.primitives[0].dimensions[1] = 0.03; o.primitives[0].dimensions[1] = 0.03;
@ -39,13 +39,15 @@ void spawnObject(const planning_scene::PlanningScenePtr& scene) {
TEST(PA10, pick) { TEST(PA10, pick) {
Task t; Task t;
t.stages()->setName("pick"); t.stages()->setName("pick");
t.loadRobotModel();
auto node = rclcpp::Node::make_shared("pa10");
t.loadRobotModel(node);
// define global properties used by most stages // define global properties used by most stages
t.setProperty("group", std::string("left_arm")); t.setProperty("group", std::string("left_arm"));
t.setProperty("eef", std::string("la_tool_mount")); t.setProperty("eef", std::string("la_tool_mount"));
t.setProperty("gripper", std::string("left_hand")); t.setProperty("gripper", std::string("left_hand"));
auto pipeline = std::make_shared<solvers::PipelinePlanner>(); auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
pipeline->setPlannerId("RRTConnectkConfigDefault"); pipeline->setPlannerId("RRTConnectkConfigDefault");
auto cartesian = std::make_shared<solvers::CartesianPath>(); auto cartesian = std::make_shared<solvers::CartesianPath>();
@ -87,7 +89,7 @@ TEST(PA10, pick) {
move->setIKFrame("lh_tool_frame"); move->setIKFrame("lh_tool_frame");
move->setMinMaxDistance(0.05, 0.1); move->setMinMaxDistance(0.05, 0.1);
geometry_msgs::Vector3Stamped direction; geometry_msgs::msg::Vector3Stamped direction;
direction.header.frame_id = "lh_tool_frame"; direction.header.frame_id = "lh_tool_frame";
direction.vector.z = 1; direction.vector.z = 1;
move->setDirection(direction); move->setDirection(direction);
@ -154,7 +156,7 @@ TEST(PA10, pick) {
move->properties().set("marker_ns", std::string("lift")); move->properties().set("marker_ns", std::string("lift"));
move->setIKFrame("lh_tool_frame"); move->setIKFrame("lh_tool_frame");
geometry_msgs::Vector3Stamped direction; geometry_msgs::msg::Vector3Stamped direction;
direction.header.frame_id = "world"; direction.header.frame_id = "world";
direction.vector.z = 1; direction.vector.z = 1;
move->setDirection(direction); move->setDirection(direction);
@ -168,7 +170,7 @@ TEST(PA10, pick) {
move->properties().set("marker_ns", std::string("lift")); move->properties().set("marker_ns", std::string("lift"));
move->setIKFrame("lh_tool_frame"); move->setIKFrame("lh_tool_frame");
geometry_msgs::TwistStamped twist; geometry_msgs::msg::TwistStamped twist;
twist.header.frame_id = "object"; twist.header.frame_id = "object";
twist.twist.linear.y = 1; twist.twist.linear.y = 1;
twist.twist.angular.y = 2; twist.twist.angular.y = 2;
@ -189,9 +191,7 @@ TEST(PA10, pick) {
int main(int argc, char** argv) { int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "pa10"); rclcpp::init(argc, argv);
ros::AsyncSpinner spinner(1);
spinner.start();
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }

View File

@ -7,7 +7,7 @@
#include <moveit/task_constructor/stages/connect.h> #include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h> #include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <ros/ros.h> #include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
@ -16,7 +16,7 @@ using namespace moveit::task_constructor;
void spawnObject() { void spawnObject() {
moveit::planning_interface::PlanningSceneInterface psi; moveit::planning_interface::PlanningSceneInterface psi;
moveit_msgs::CollisionObject o; moveit_msgs::msg::CollisionObject o;
o.id = "object"; o.id = "object";
o.header.frame_id = "base_link"; o.header.frame_id = "base_link";
o.primitive_poses.resize(1); o.primitive_poses.resize(1);
@ -25,7 +25,7 @@ void spawnObject() {
o.primitive_poses[0].position.z = 0.84; o.primitive_poses[0].position.z = 0.84;
o.primitive_poses[0].orientation.w = 1.0; o.primitive_poses[0].orientation.w = 1.0;
o.primitives.resize(1); o.primitives.resize(1);
o.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; o.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;
o.primitives[0].dimensions.resize(2); o.primitives[0].dimensions.resize(2);
o.primitives[0].dimensions[0] = 0.23; o.primitives[0].dimensions[0] = 0.23;
o.primitives[0].dimensions[1] = 0.03; o.primitives[0].dimensions[1] = 0.03;
@ -38,8 +38,9 @@ TEST(PR2, pick) {
Stage* initial_stage = new stages::CurrentState("current state"); Stage* initial_stage = new stages::CurrentState("current state");
t.add(std::unique_ptr<Stage>(initial_stage)); t.add(std::unique_ptr<Stage>(initial_stage));
auto node = rclcpp::Node::make_shared("pr2");
// planner used for connect // planner used for connect
auto pipeline = std::make_shared<solvers::PipelinePlanner>(); auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
pipeline->setPlannerId("RRTConnectkConfigDefault"); pipeline->setPlannerId("RRTConnectkConfigDefault");
// connect to pick // connect to pick
stages::Connect::GroupPlannerVector planners = { { "left_arm", pipeline }, { "left_gripper", pipeline } }; stages::Connect::GroupPlannerVector planners = { { "left_arm", pipeline }, { "left_gripper", pipeline } };
@ -61,12 +62,12 @@ TEST(PR2, pick) {
auto pick = std::make_unique<stages::Pick>(std::move(grasp)); auto pick = std::make_unique<stages::Pick>(std::move(grasp));
pick->setProperty("eef", std::string("left_gripper")); pick->setProperty("eef", std::string("left_gripper"));
pick->setProperty("object", std::string("object")); pick->setProperty("object", std::string("object"));
geometry_msgs::TwistStamped approach; geometry_msgs::msg::TwistStamped approach;
approach.header.frame_id = "l_gripper_tool_frame"; approach.header.frame_id = "l_gripper_tool_frame";
approach.twist.linear.x = 1.0; approach.twist.linear.x = 1.0;
pick->setApproachMotion(approach, 0.03, 0.1); pick->setApproachMotion(approach, 0.03, 0.1);
geometry_msgs::TwistStamped lift; geometry_msgs::msg::TwistStamped lift;
lift.header.frame_id = "base_link"; lift.header.frame_id = "base_link";
lift.twist.linear.z = 1.0; lift.twist.linear.z = 1.0;
pick->setLiftMotion(lift, 0.03, 0.05); pick->setLiftMotion(lift, 0.03, 0.05);
@ -87,12 +88,10 @@ TEST(PR2, pick) {
int main(int argc, char** argv) { int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "pr2"); rclcpp::init(argc, argv);
ros::AsyncSpinner spinner(1);
spinner.start();
// wait some time for move_group to come up // wait some time for move_group to come up
ros::WallDuration(5.0).sleep(); rclcpp::sleep_for(std::chrono::seconds(5));
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }

View File

@ -7,7 +7,7 @@
#include <moveit/task_constructor/stages/connect.h> #include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h> #include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <ros/ros.h> #include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
@ -16,7 +16,7 @@ using namespace moveit::task_constructor;
void spawnObject() { void spawnObject() {
moveit::planning_interface::PlanningSceneInterface psi; moveit::planning_interface::PlanningSceneInterface psi;
moveit_msgs::CollisionObject o; moveit_msgs::msg::CollisionObject o;
o.id = "object"; o.id = "object";
o.header.frame_id = "table_top"; o.header.frame_id = "table_top";
o.primitive_poses.resize(1); o.primitive_poses.resize(1);
@ -25,7 +25,7 @@ void spawnObject() {
o.primitive_poses[0].position.z = 0.12; o.primitive_poses[0].position.z = 0.12;
o.primitive_poses[0].orientation.w = 1.0; o.primitive_poses[0].orientation.w = 1.0;
o.primitives.resize(1); o.primitives.resize(1);
o.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; o.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;
o.primitives[0].dimensions.resize(2); o.primitives[0].dimensions.resize(2);
o.primitives[0].dimensions[0] = 0.23; o.primitives[0].dimensions[0] = 0.23;
o.primitives[0].dimensions[1] = 0.03; o.primitives[0].dimensions[1] = 0.03;
@ -40,8 +40,9 @@ TEST(UR5, pick) {
initial_stage = initial.get(); initial_stage = initial.get();
t.add(std::move(initial)); t.add(std::move(initial));
auto node = rclcpp::Node::make_shared("ur5");
// planner used for connect // planner used for connect
auto pipeline = std::make_shared<solvers::PipelinePlanner>(); auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
pipeline->setPlannerId("RRTConnectkConfigDefault"); pipeline->setPlannerId("RRTConnectkConfigDefault");
// connect to pick // connect to pick
stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } }; stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } };
@ -63,12 +64,12 @@ TEST(UR5, pick) {
auto pick = std::make_unique<stages::Pick>(std::move(grasp)); auto pick = std::make_unique<stages::Pick>(std::move(grasp));
pick->setProperty("eef", std::string("gripper")); pick->setProperty("eef", std::string("gripper"));
pick->setProperty("object", std::string("object")); pick->setProperty("object", std::string("object"));
geometry_msgs::TwistStamped approach; geometry_msgs::msg::TwistStamped approach;
approach.header.frame_id = "s_model_tool0"; approach.header.frame_id = "s_model_tool0";
approach.twist.linear.x = 1.0; approach.twist.linear.x = 1.0;
pick->setApproachMotion(approach, 0.03, 0.1); pick->setApproachMotion(approach, 0.03, 0.1);
geometry_msgs::TwistStamped lift; geometry_msgs::msg::TwistStamped lift;
lift.header.frame_id = "world"; lift.header.frame_id = "world";
lift.twist.linear.z = 1.0; lift.twist.linear.z = 1.0;
pick->setLiftMotion(lift, 0.03, 0.05); pick->setLiftMotion(lift, 0.03, 0.05);
@ -89,12 +90,10 @@ TEST(UR5, pick) {
int main(int argc, char** argv) { int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "ur5"); rclcpp::init(argc, argv);
ros::AsyncSpinner spinner(1);
spinner.start();
// wait some time for move_group to come up // wait some time for move_group to come up
ros::WallDuration(5.0).sleep(); rclcpp::sleep_for(std::chrono::seconds(5));
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }

View File

@ -8,7 +8,6 @@
#include "models.h" #include "models.h"
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <gmock/gmock.h>
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {

View File

@ -7,7 +7,7 @@
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/msg/pose_stamped.hpp>
#include <gtest/gtest.h> #include <gtest/gtest.h>

View File

@ -9,6 +9,7 @@
#include "gtest_value_printers.h" #include "gtest_value_printers.h"
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <gmock/gmock.h>
#include <initializer_list> #include <initializer_list>
#include <chrono> #include <chrono>
#include <thread> #include <thread>
@ -176,8 +177,9 @@ TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) {
int main(int argc, char** argv) { int main(int argc, char** argv) {
for (int i = 1; i < argc; ++i) { for (int i = 1; i < argc; ++i) {
if (strcmp(argv[i], "--debug") == 0) { if (strcmp(argv[i], "--debug") == 0) {
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) if (rcutils_logging_set_logger_level("moveit_robot_model.robot_model", RCUTILS_LOG_SEVERITY_FATAL) !=
ros::console::notifyLoggerLevelsChanged(); RCUTILS_RET_OK)
throw std::runtime_error("Failed to set logger level to RCUTILS_LOG_SEVERITY_ERROR");
break; break;
} }
} }

View File

@ -8,12 +8,16 @@
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h> #include <tf2_eigen/tf2_eigen.h>
#endif
#include <moveit_msgs/RobotState.h> #include <moveit_msgs/msg/robot_state.hpp>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/msg/pose_stamped.hpp>
#include <ros/console.h> #include <rclcpp/logging.hpp>
#include <gtest/gtest.h> #include <gtest/gtest.h>
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
@ -28,9 +32,10 @@ struct PandaMoveTo : public testing::Test
Task t; Task t;
stages::MoveTo* move_to; stages::MoveTo* move_to;
PlanningScenePtr scene; PlanningScenePtr scene;
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_to");
PandaMoveTo() { PandaMoveTo() {
t.setRobotModel(loadModel()); t.loadRobotModel(node);
scene = std::make_shared<PlanningScene>(t.getRobotModel()); scene = std::make_shared<PlanningScene>(t.getRobotModel());
scene->getCurrentStateNonConst().setToDefaultValues(); scene->getCurrentStateNonConst().setToDefaultValues();
@ -63,7 +68,7 @@ TEST_F(PandaMoveTo, mapTarget) {
TEST_F(PandaMoveTo, stateTarget) { TEST_F(PandaMoveTo, stateTarget) {
move_to->setGoal([] { move_to->setGoal([] {
moveit_msgs::RobotState state; moveit_msgs::msg::RobotState state;
state.is_diff = true; state.is_diff = true;
state.joint_state.name = { "panda_joint1", "panda_joint2" }; state.joint_state.name = { "panda_joint1", "panda_joint2" };
state.joint_state.position = { TAU / 8, TAU / 8 }; state.joint_state.position = { TAU / 8, TAU / 8 };
@ -72,19 +77,19 @@ TEST_F(PandaMoveTo, stateTarget) {
EXPECT_ONE_SOLUTION; EXPECT_ONE_SOLUTION;
} }
geometry_msgs::PoseStamped getFramePoseOfNamedState(RobotState state, std::string pose, std::string frame) { geometry_msgs::msg::PoseStamped getFramePoseOfNamedState(RobotState state, std::string pose, std::string frame) {
state.setToDefaultValues(state.getRobotModel()->getJointModelGroup("panda_arm"), pose); state.setToDefaultValues(state.getRobotModel()->getJointModelGroup("panda_arm"), pose);
auto frame_eigen{ state.getFrameTransform(frame) }; auto frame_eigen{ state.getFrameTransform(frame) };
geometry_msgs::PoseStamped p; geometry_msgs::msg::PoseStamped p;
p.header.frame_id = state.getRobotModel()->getModelFrame(); p.header.frame_id = state.getRobotModel()->getModelFrame();
p.pose = tf2::toMsg(frame_eigen); p.pose = tf2::toMsg(frame_eigen);
return p; return p;
} }
TEST_F(PandaMoveTo, pointTarget) { TEST_F(PandaMoveTo, pointTarget) {
geometry_msgs::PoseStamped pose{ getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8") }; geometry_msgs::msg::PoseStamped pose{ getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8") };
geometry_msgs::PointStamped point; geometry_msgs::msg::PointStamped point;
point.header = pose.header; point.header = pose.header;
point.point = pose.pose.position; point.point = pose.pose.position;
move_to->setGoal(point); move_to->setGoal(point);
@ -104,24 +109,24 @@ TEST_F(PandaMoveTo, poseIKFrameLinkTarget) {
EXPECT_ONE_SOLUTION; EXPECT_ONE_SOLUTION;
} }
moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) { moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string& id) {
moveit_msgs::AttachedCollisionObject aco; moveit_msgs::msg::AttachedCollisionObject aco;
aco.link_name = "panda_hand"; aco.link_name = "panda_hand";
aco.object.header.frame_id = aco.link_name; aco.object.header.frame_id = aco.link_name;
aco.object.operation = aco.object.ADD; aco.object.operation = aco.object.ADD;
aco.object.id = id; aco.object.id = id;
aco.object.primitives.resize(1, [] { aco.object.primitives.resize(1, [] {
shape_msgs::SolidPrimitive p; shape_msgs::msg::SolidPrimitive p;
p.type = p.SPHERE; p.type = p.SPHERE;
p.dimensions.resize(1); p.dimensions.resize(1);
p.dimensions[p.SPHERE_RADIUS] = 0.01; p.dimensions[p.SPHERE_RADIUS] = 0.01;
return p; return p;
}()); }());
aco.object.primitive_poses.resize(1);
#if MOVEIT_HAS_OBJECT_POSE #if MOVEIT_HAS_OBJECT_POSE
aco.object.pose.position.z = 0.2; aco.object.pose.position.z = 0.2;
aco.object.pose.orientation.w = 1.0; aco.object.pose.orientation.w = 1.0;
#else #else
aco.object.primitive_poses.resize(1);
aco.object.primitive_poses[0].position.z = 0.2; aco.object.primitive_poses[0].position.z = 0.2;
aco.object.primitive_poses[0].orientation.w = 1.0; aco.object.primitive_poses[0].orientation.w = 1.0;
#endif #endif
@ -129,7 +134,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id)
// If we don't have this, we also don't have subframe support // If we don't have this, we also don't have subframe support
aco.object.subframe_names.resize(1, "subframe"); aco.object.subframe_names.resize(1, "subframe");
aco.object.subframe_poses.resize(1, [] { aco.object.subframe_poses.resize(1, [] {
geometry_msgs::Pose p; geometry_msgs::msg::Pose p;
p.orientation.w = 1.0; p.orientation.w = 1.0;
return p; return p;
}()); }());
@ -162,9 +167,7 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
int main(int argc, char** argv) { int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "move_to_test"); rclcpp::init(argc, argv);
ros::AsyncSpinner spinner(1);
spinner.start();
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }

View File

@ -4,10 +4,12 @@
#include <moveit/task_constructor/stages/compute_ik.h> #include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/stages/modify_planning_scene.h> #include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/msg/pose_stamped.hpp>
#include "stage_mockups.h" #include "stage_mockups.h"
#include <ros/console.h>
#include <rclcpp/logging.hpp>
#include <gtest/gtest.h> #include <gtest/gtest.h>
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
@ -89,7 +91,7 @@ TEST(ModifyPlanningScene, allowCollisions) {
void spawnObject(PlanningScene& scene, const std::string& name, int type, void spawnObject(PlanningScene& scene, const std::string& name, int type,
const std::vector<double>& pos = { 0, 0, 0 }) { const std::vector<double>& pos = { 0, 0, 0 }) {
moveit_msgs::CollisionObject o; moveit_msgs::msg::CollisionObject o;
o.id = name; o.id = name;
o.header.frame_id = scene.getPlanningFrame(); o.header.frame_id = scene.getPlanningFrame();
o.primitive_poses.resize(1); o.primitive_poses.resize(1);
@ -102,13 +104,13 @@ void spawnObject(PlanningScene& scene, const std::string& name, int type,
o.primitives[0].type = type; o.primitives[0].type = type;
switch (type) { switch (type) {
case shape_msgs::SolidPrimitive::CYLINDER: case shape_msgs::msg::SolidPrimitive::CYLINDER:
o.primitives[0].dimensions = { 0.1, 0.02 }; o.primitives[0].dimensions = { 0.1, 0.02 };
break; break;
case shape_msgs::SolidPrimitive::BOX: case shape_msgs::msg::SolidPrimitive::BOX:
o.primitives[0].dimensions = { 0.1, 0.2, 0.3 }; o.primitives[0].dimensions = { 0.1, 0.2, 0.3 };
break; break;
case shape_msgs::SolidPrimitive::SPHERE: case shape_msgs::msg::SolidPrimitive::SPHERE:
o.primitives[0].dimensions = { 0.05 }; o.primitives[0].dimensions = { 0.05 };
break; break;
} }
@ -116,10 +118,10 @@ void spawnObject(PlanningScene& scene, const std::string& name, int type,
} }
void attachObject(PlanningScene& scene, const std::string& object, const std::string& link, bool attach) { void attachObject(PlanningScene& scene, const std::string& object, const std::string& link, bool attach) {
moveit_msgs::AttachedCollisionObject obj; moveit_msgs::msg::AttachedCollisionObject obj;
obj.link_name = link; obj.link_name = link;
obj.object.operation = obj.object.operation =
attach ? (int8_t)moveit_msgs::CollisionObject::ADD : (int8_t)moveit_msgs::CollisionObject::REMOVE; attach ? (int8_t)moveit_msgs::msg::CollisionObject::ADD : (int8_t)moveit_msgs::msg::CollisionObject::REMOVE;
obj.object.id = object; obj.object.id = object;
scene.processAttachedCollisionObjectMsg(obj); scene.processAttachedCollisionObjectMsg(obj);
} }
@ -129,19 +131,19 @@ TEST(Connect, compatible) {
auto scene = std::make_shared<PlanningScene>(getModel()); auto scene = std::make_shared<PlanningScene>(getModel());
auto& state = scene->getCurrentStateNonConst(); auto& state = scene->getCurrentStateNonConst();
state.setToDefaultValues(); state.setToDefaultValues();
spawnObject(*scene, "object", shape_msgs::SolidPrimitive::CYLINDER); spawnObject(*scene, "object", shape_msgs::msg::SolidPrimitive::CYLINDER);
state.update(); state.update();
auto other = scene->diff(); auto other = scene->diff();
EXPECT_TRUE(connect.compatible(scene, other)) << "identical scenes"; EXPECT_TRUE(connect.compatible(scene, other)) << "identical scenes";
spawnObject(*other, "object", shape_msgs::SolidPrimitive::BOX); spawnObject(*other, "object", shape_msgs::msg::SolidPrimitive::BOX);
// EXPECT_FALSE(connect.compatible(scene, other)) << "different shapes"; // EXPECT_FALSE(connect.compatible(scene, other)) << "different shapes";
spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, { 0.1, 0, 0 }); spawnObject(*other, "object", shape_msgs::msg::SolidPrimitive::CYLINDER, { 0.1, 0, 0 });
EXPECT_FALSE(connect.compatible(scene, other)) << "different pose"; EXPECT_FALSE(connect.compatible(scene, other)) << "different pose";
spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER); spawnObject(*other, "object", shape_msgs::msg::SolidPrimitive::CYLINDER);
EXPECT_TRUE(connect.compatible(scene, other)) << "same objects"; EXPECT_TRUE(connect.compatible(scene, other)) << "same objects";
// attached objects // attached objects
@ -152,7 +154,7 @@ TEST(Connect, compatible) {
other = scene->diff(); other = scene->diff();
EXPECT_TRUE(connect.compatible(scene, other)) << "identical scenes, attached object"; EXPECT_TRUE(connect.compatible(scene, other)) << "identical scenes, attached object";
spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, { 0.1, 0, 0 }); spawnObject(*other, "object", shape_msgs::msg::SolidPrimitive::CYLINDER, { 0.1, 0, 0 });
attachObject(*other, "object", "tip", true); attachObject(*other, "object", "tip", true);
EXPECT_FALSE(connect.compatible(scene, other)) << "different pose"; EXPECT_FALSE(connect.compatible(scene, other)) << "different pose";
} }

View File

@ -1,48 +1,51 @@
cmake_minimum_required(VERSION 3.1.3) cmake_minimum_required(VERSION 3.5)
project(moveit_task_constructor_demo) project(moveit_task_constructor_demo)
if(NOT "${CMAKE_CXX_STANDARD}") if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD 17)
endif() endif()
set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF) set(CMAKE_CXX_EXTENSIONS OFF)
find_package(catkin REQUIRED COMPONENTS find_package(ament_cmake REQUIRED)
roscpp
moveit_core
moveit_task_constructor_core
moveit_ros_planning_interface
rosparam_shortcuts
)
catkin_package( find_package(rclcpp REQUIRED)
CATKIN_DEPENDS roscpp find_package(moveit_core REQUIRED)
) find_package(moveit_task_constructor_core REQUIRED)
find_package(moveit_task_constructor_msgs REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(rosparam_shortcuts REQUIRED)
include_directories( set(THIS_PACKAGE_INCLUDE_DEPENDS
include rclcpp
${catkin_INCLUDE_DIRS} moveit_core
) moveit_task_constructor_core
moveit_task_constructor_msgs
moveit_ros_planning
moveit_ros_planning_interface
tf2_eigen
rosparam_shortcuts)
add_library(${PROJECT_NAME}_pick_place_task src/pick_place_task.cpp) add_library(${PROJECT_NAME}_pick_place_task SHARED src/pick_place_task.cpp)
target_link_libraries(${PROJECT_NAME}_pick_place_task ${catkin_LIBRARIES}) ament_target_dependencies(${PROJECT_NAME}_pick_place_task ${THIS_PACKAGE_INCLUDE_DEPENDS})
add_dependencies(${PROJECT_NAME}_pick_place_task ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
install(TARGETS ${PROJECT_NAME}_pick_place_task install(TARGETS ${PROJECT_NAME}_pick_place_task
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION lib
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION lib
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} RUNTIME DESTINATION lib/${PROJECT_NAME})
)
include_directories(include)
# declare a demo consisting of a single cpp file # declare a demo consisting of a single cpp file
function(demo name) function(demo name)
add_executable(${PROJECT_NAME}_${name} src/${name}.cpp) add_executable(${PROJECT_NAME}_${name} src/${name}.cpp)
add_dependencies(${PROJECT_NAME}_${name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ament_target_dependencies(${PROJECT_NAME}_${name} ${THIS_PACKAGE_INCLUDE_DEPENDS})
target_link_libraries(${PROJECT_NAME}_${name} ${catkin_LIBRARIES})
set_target_properties(${PROJECT_NAME}_${name} PROPERTIES OUTPUT_NAME ${name} PREFIX "") set_target_properties(${PROJECT_NAME}_${name} PROPERTIES OUTPUT_NAME ${name} PREFIX "")
install(TARGETS ${PROJECT_NAME}_${name} install(TARGETS ${PROJECT_NAME}_${name}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION lib
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION lib
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} RUNTIME DESTINATION lib/${PROJECT_NAME}
) )
endfunction() endfunction()
@ -55,7 +58,10 @@ demo(pick_place_demo)
target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task) target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task)
install(DIRECTORY launch config install(DIRECTORY launch config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} DESTINATION share/${PROJECT_NAME}
) )
add_subdirectory(test) # TODO: Port tests
# add_subdirectory(test)
ament_package()

View File

@ -8,4 +8,4 @@ Developed by Henning Kayser & Simon Goldstein at [PickNik Consulting](http://pic
Run demo Run demo
roslaunch moveit_task_constructor_demo demo.launch ros2 launch moveit_task_constructor_demo demo.launch.py

View File

@ -1,15 +1,24 @@
Panels: Panels:
- Class: rviz/Displays - Class: rviz_common/Displays
Help Height: 84 Help Height: 78
Name: Displays Name: Displays
Property Tree Widget: Property Tree Widget:
Expanded: Expanded:
- /Global Options1
- /Status1
- /Motion Planning Tasks1 - /Motion Planning Tasks1
Splitter Ratio: 0.5393258333206177 - /PlanningScene1
Tree Height: 533 Splitter Ratio: 0.5
- Class: rviz/Help Tree Height: 439
Name: Help - Class: rviz_common/Selection
- Class: rviz/Views Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded: Expanded:
- /Current View1 - /Current View1
Name: Views Name: Views
@ -22,32 +31,26 @@ Panels:
Name: Motion Planning Tasks Name: Motion Planning Tasks
Tasks View: Tasks View:
property_splitter: property_splitter:
- 540 - 197
- 0 - 121
solution_sorting: solution_sorting:
column: 1 column: 0
order: 0 order: 1
solutions_splitter: solutions_splitter:
- 306 - 397
- 98 - 198
solutions_view_columns: solutions_view_columns: ~
- 38
- 52
- 0
tasks_view_columns: tasks_view_columns:
- 226 - 295
- 38 - 26
- 38 - 26
Preferences: - 48
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager: Visualization Manager:
Class: "" Class: ""
Displays: Displays:
- Alpha: 0.5 - Alpha: 0.5
Cell Size: 1 Cell Size: 1
Class: rviz/Grid Class: rviz_default_plugins/Grid
Color: 160; 160; 164 Color: 160; 160; 164
Enabled: true Enabled: true
Line Style: Line Style:
@ -63,31 +66,6 @@ Visualization Manager:
Plane Cell Count: 10 Plane Cell Count: 10
Reference Frame: <Fixed Frame> Reference Frame: <Fixed Frame>
Value: true Value: true
- Class: moveit_rviz_plugin/PlanningScene
Enabled: true
Move Group Namespace: ""
Name: PlanningScene
Planning Scene Topic: move_group/monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.20000000298023224
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Value: true
- Class: moveit_task_constructor/Motion Planning Tasks - Class: moveit_task_constructor/Motion Planning Tasks
Enabled: true Enabled: true
Interrupt Display: false Interrupt Display: false
@ -104,7 +82,66 @@ Visualization Manager:
Expand Link Details: false Expand Link Details: false
Expand Tree: false Expand Tree: false
Link Tree Style: Links in Alphabetic Order Link Tree Style: Links in Alphabetic Order
Robot Alpha: 1 panda_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link8:
Alpha: 1
Show Axes: false
Show Trail: false
panda_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 0.5
Show Robot Collision: false Show Robot Collision: false
Show Robot Visual: true Show Robot Visual: true
Use Fixed Robot Color: false Use Fixed Robot Color: false
@ -119,62 +156,175 @@ Visualization Manager:
Voxel Rendering: Occupied Voxels Voxel Rendering: Occupied Voxels
Show Trail: false Show Trail: false
State Display Time: 0.05 s State Display Time: 0.05 s
Task Solution Topic: /mtc_tutorial/solution Task Solution Topic: /solution
Tasks: Tasks:
Cartesian Path: 1 Cartesian Path: 0
Trail Step Size: 1 Trail Step Size: 1
Value: true Value: true
- Class: moveit_rviz_plugin/PlanningScene
Enabled: true
Move Group Namespace: ""
Name: PlanningScene
Planning Scene Topic: /monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
panda_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link8:
Alpha: 1
Show Axes: false
Show Trail: false
panda_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Value: true
Enabled: true Enabled: true
Global Options: Global Options:
Background Color: 48; 48; 48 Background Color: 48; 48; 48
Default Light: true
Fixed Frame: panda_link0 Fixed Frame: panda_link0
Frame Rate: 30 Frame Rate: 30
Name: root Name: root
Tools: Tools:
- Class: rviz/Interact - Class: rviz_default_plugins/Interact
Hide Inactive Objects: true Hide Inactive Objects: true
- Class: rviz/MoveCamera - Class: rviz_default_plugins/MoveCamera
- Class: rviz/Select - Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true Value: true
Views: Views:
Current: Current:
Class: rviz/XYOrbit Class: rviz_default_plugins/Orbit
Distance: 1.3878222703933716 Distance: 3.8008623123168945
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549 Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1 Stereo Focal Distance: 1
Swap Stereo Eyes: false Swap Stereo Eyes: false
Value: false Value: false
Field of View: 0.7853981852531433
Focal Point: Focal Point:
X: 0.30880630016326904 X: 0
Y: -0.1259305477142334 Y: 0
Z: 1.3560062370743253e-6 Z: 0
Focal Shape Fixed Size: true Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806 Focal Shape Size: 0.05000000074505806
Invert Z Axis: false Invert Z Axis: false
Name: Current View Name: Current View
Near Clip Distance: 0.009999999776482582 Near Clip Distance: 0.009999999776482582
Pitch: 0.264797568321228 Pitch: 0.645397961139679
Target Frame: panda_link0 Target Frame: <Fixed Frame>
Yaw: 4.939944744110107 Value: Orbit (rviz)
Yaw: 0.7453980445861816
Saved: ~ Saved: ~
Window Geometry: Window Geometry:
Displays: Displays:
collapsed: false collapsed: false
Height: 768 Height: 1043
Help:
collapsed: false
Hide Left Dock: false Hide Left Dock: false
Hide Right Dock: false Hide Right Dock: false
Motion Planning Tasks: Motion Planning Tasks:
collapsed: false collapsed: false
Motion Planning Tasks - Slider: Motion Planning Tasks - Slider:
collapsed: false collapsed: false
QMainWindow State: 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 QMainWindow State: 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
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views: Views:
collapsed: false collapsed: false
Width: 1621 Width: 1920
X: 249 X: 0
Y: 25 Y: 0

View File

@ -1,46 +1,48 @@
# Total planning attempts /**:
max_solutions: 10 ros__parameters:
# Total planning attempts
max_solutions: 10
# Planning group and link names # Planning group and link names
arm_group_name: "panda_arm" arm_group_name: "panda_arm"
eef_name: "hand" eef_name: "hand"
hand_group_name: "hand" hand_group_name: "hand"
hand_frame: "panda_link8" hand_frame: "panda_link8"
# Poses # Poses
hand_open_pose: "open" hand_open_pose: "open"
hand_close_pose: "close" hand_close_pose: "close"
arm_home_pose: "ready" arm_home_pose: "ready"
# Scene frames # Scene frames
world_frame: "world" world_frame: "world"
table_reference_frame: "world" table_reference_frame: "world"
object_reference_frame: "world" object_reference_frame: "world"
surface_link: "table" surface_link: "table"
# Collision object for picking # Collision object for picking
# CYLINDER object specifications # CYLINDER object specifications
object_name: "object" object_name: "object"
object_dimensions: [0.25, 0.02] # [height, radius] object_dimensions: [0.25, 0.02] # [height, radius]
object_pose: [0.5, -0.25, 0.0, 0, 0, 0] object_pose: [0.5, -0.25, 0.0, 0.0, 0.0, 0.0]
# Table model # Table model
spawn_table: true spawn_table: true
table_name: "table" table_name: "table"
table_dimensions: [0.4, 0.5, 0.1] # [length, width, height] table_dimensions: [0.4, 0.5, 0.1] # [length, width, height]
table_pose: [0.5, -0.25, 0, 0, 0, 0] table_pose: [0.5, -0.25, 0.0, 0.0, 0.0, 0.0]
# Gripper grasp frame transform [x,y,z,r,p,y] # Gripper grasp frame transform [x,y,z,r,p,y]
grasp_frame_transform: [0, 0, 0.1, 1.571, 0.785, 1.571] grasp_frame_transform: [0.0, 0.0, 0.1, 1.571, 0.785, 1.571]
# Place pose [x,y,z,r,p,y] # Place pose [x,y,z,r,p,y]
place_pose: [0.6, -0.15, 0, 0, 0, 0] place_pose: [0.6, -0.15, 0.0, 0.0, 0.0, 0.0]
place_surface_offset: 0.0001 # place offset from table place_surface_offset: 0.0001 # place offset from table
# Valid distance range when approaching an object for picking # Valid distance range when approaching an object for picking
approach_object_min_dist: 0.1 approach_object_min_dist: 0.1
approach_object_max_dist: 0.15 approach_object_max_dist: 0.15
# Valid height range when lifting an object after pick # Valid height range when lifting an object after pick
lift_object_min_dist: 0.01 lift_object_min_dist: 0.01
lift_object_max_dist: 0.1 lift_object_max_dist: 0.1

View File

@ -31,11 +31,11 @@
*********************************************************************/ *********************************************************************/
/* Author: Henning Kayser, Simon Goldstein /* Author: Henning Kayser, Simon Goldstein
Desc: A demo to show MoveIt Task Constructor in action Desc: A demo to show MoveIt Task Constructor in action
*/ */
// ROS // ROS
#include <ros/ros.h> #include <rclcpp/rclcpp.hpp>
// MoveIt // MoveIt
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
@ -56,22 +56,26 @@
#include <moveit/task_constructor/stages/predicate_filter.h> #include <moveit/task_constructor/stages/predicate_filter.h>
#include <moveit/task_constructor/solvers/cartesian_path.h> #include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h> #include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h> #include <moveit_task_constructor_msgs/action/execute_task_solution.hpp>
#include <eigen_conversions/eigen_msg.h> #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif
#pragma once #pragma once
namespace moveit_task_constructor_demo { namespace moveit_task_constructor_demo {
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
// prepare a demo environment from ROS parameters under pnh // prepare a demo environment from ROS parameters under node
void setupDemoScene(ros::NodeHandle& pnh); void setupDemoScene(const rclcpp::Node::SharedPtr& node);
class PickPlaceTask class PickPlaceTask
{ {
public: public:
PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh); PickPlaceTask(const std::string& task_name, const rclcpp::Node::SharedPtr& pnh);
~PickPlaceTask() = default; ~PickPlaceTask() = default;
bool init(); bool init();
@ -83,9 +87,7 @@ public:
private: private:
void loadParameters(); void loadParameters();
static constexpr char LOGNAME[]{ "pick_place_task" }; rclcpp::Node::SharedPtr node_;
ros::NodeHandle pnh_;
std::string task_name_; std::string task_name_;
moveit::task_constructor::TaskPtr task_; moveit::task_constructor::TaskPtr task_;
@ -117,7 +119,7 @@ private:
double lift_object_max_dist_; double lift_object_max_dist_;
// Place metrics // Place metrics
geometry_msgs::Pose place_pose_; geometry_msgs::msg::Pose place_pose_;
double place_surface_offset_; double place_surface_offset_;
}; };
} // namespace moveit_task_constructor_demo } // namespace moveit_task_constructor_demo

View File

@ -0,0 +1,73 @@
import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description():
# Get URDF and SRDF
robot_description_config = load_file(
"moveit_resources_panda_description", "urdf/panda.urdf"
)
robot_description = {"robot_description": robot_description_config}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf"
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/kinematics.yaml"
)
# Planning Functionality
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/ompl_planning.yaml"
)
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
cartesian_task = Node(
package="moveit_task_constructor_demo",
executable="alternative_path_costs",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
],
)
return LaunchDescription([cartesian_task])

View File

@ -0,0 +1,55 @@
import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description():
# Get URDF and SRDF
robot_description_config = load_file(
"moveit_resources_panda_description", "urdf/panda.urdf"
)
robot_description = {"robot_description": robot_description_config}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf"
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/kinematics.yaml"
)
cartesian_task = Node(
package="moveit_task_constructor_demo",
executable="cartesian",
output="screen",
parameters=[robot_description, robot_description_semantic, kinematics_yaml],
)
return LaunchDescription([cartesian_task])

View File

@ -1,13 +0,0 @@
<?xml version="1.0"?>
<launch>
<!-- Load panda demo with MTC's execution capability -->
<include file="$(find moveit_resources_panda_moveit_config)/launch/demo.launch">
<arg name="use_rviz" value="false"/>
</include>
<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
<!-- Run rviz with prepared config -->
<include file="$(find moveit_resources_panda_moveit_config)/launch/moveit_rviz.launch">
<arg name="rviz_config" value="$(find moveit_task_constructor_demo)/config/mtc.rviz" />
</include>
</launch>

188
demo/launch/demo.launch.py Normal file
View File

@ -0,0 +1,188 @@
import os
import yaml
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import xacro
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description():
# planning_context
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
)
)
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf"
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/kinematics.yaml"
)
# Planning Functionality
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/ompl_planning.yaml"
)
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Functionality
moveit_simple_controllers_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/panda_controllers.yaml"
)
moveit_controllers = {
"moveit_simple_controller_manager": moveit_simple_controllers_yaml,
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
trajectory_execution = {
"moveit_manage_controllers": True,
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}
# Load ExecuteTaskSolutionCapability so we can execute found solutions in simulation
move_group_capabilities = {
"capabilities": "move_group/ExecuteTaskSolutionCapability"
}
# Start the actual move_group node/action server
run_move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
move_group_capabilities,
],
)
# RViz
rviz_config_file = (
get_package_share_directory("moveit_task_constructor_demo") + "/config/mtc.rviz"
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
kinematics_yaml,
],
)
# Static TF
static_tf = Node(
package="tf2_ros",
executable="static_transform_publisher",
name="static_transform_publisher",
output="log",
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
)
# Publish TF
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
parameters=[robot_description],
)
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda_ros_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output={
"stdout": "screen",
"stderr": "screen",
},
)
# Load controllers
load_controllers = []
for controller in [
"panda_arm_controller",
"panda_hand_controller",
"joint_state_broadcaster",
]:
load_controllers += [
ExecuteProcess(
cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
shell=True,
output="screen",
)
]
return LaunchDescription(
[
rviz_node,
static_tf,
robot_state_publisher,
run_move_group_node,
ros2_control_node,
]
+ load_controllers
)

View File

@ -0,0 +1,55 @@
import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description():
# Get URDF and SRDF
robot_description_config = load_file(
"moveit_resources_panda_description", "urdf/panda.urdf"
)
robot_description = {"robot_description": robot_description_config}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf"
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/kinematics.yaml"
)
cartesian_task = Node(
package="moveit_task_constructor_demo",
executable="ik_clearance_cost",
output="screen",
parameters=[robot_description, robot_description_semantic, kinematics_yaml],
)
return LaunchDescription([cartesian_task])

View File

@ -0,0 +1,55 @@
import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description():
# Get URDF and SRDF
robot_description_config = load_file(
"moveit_resources_panda_description", "urdf/panda.urdf"
)
robot_description = {"robot_description": robot_description_config}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf"
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/kinematics.yaml"
)
modular_task = Node(
package="moveit_task_constructor_demo",
executable="modular",
output="screen",
parameters=[robot_description, robot_description_semantic, kinematics_yaml],
)
return LaunchDescription([modular_task])

View File

@ -1,8 +0,0 @@
<?xml version="1.0"?>
<launch>
<!-- Run MTC pick and place -->
<node name="mtc_tutorial" pkg="moveit_task_constructor_demo" type="pick_place_demo" output="screen">
<param name="execute" value="true" />
<rosparam command="load" file="$(find moveit_task_constructor_demo)/config/panda_config.yaml" />
</node>
</launch>

View File

@ -0,0 +1,78 @@
import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description():
# Get URDF and SRDF
robot_description_config = load_file(
"moveit_resources_panda_description", "urdf/panda.urdf"
)
robot_description = {"robot_description": robot_description_config}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf"
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/kinematics.yaml"
)
# Planning Functionality
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/ompl_planning.yaml"
)
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
pick_place_demo = Node(
package="moveit_task_constructor_demo",
executable="pick_place_demo",
output="screen",
parameters=[
os.path.join(
get_package_share_directory("moveit_task_constructor_demo"),
"config",
"panda_config.yaml",
),
robot_description,
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
],
)
return LaunchDescription([pick_place_demo])

View File

@ -10,18 +10,15 @@
<license>BSD</license> <license>BSD</license>
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
<depend>roscpp</depend>
<depend>rosparam_shortcuts</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_task_constructor_core</depend> <depend>moveit_task_constructor_core</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_core</depend>
<depend>rosparam_shortcuts</depend>
<exec_depend>moveit_task_constructor_capabilities</exec_depend> <exec_depend>moveit_task_constructor_capabilities</exec_depend>
<exec_depend>moveit_resources_panda_moveit_config</exec_depend> <exec_depend>moveit_resources_panda_moveit_config</exec_depend>
<test_depend>rostest</test_depend> <export>
<build_type>ament_cmake</build_type>
</export>
</package> </package>

View File

@ -1,4 +1,4 @@
#include <ros/ros.h> #include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
@ -16,11 +16,15 @@ using namespace moveit::task_constructor;
/* FixedState - Connect - FixedState */ /* FixedState - Connect - FixedState */
int main(int argc, char** argv) { int main(int argc, char** argv) {
ros::init(argc, argv, "mtc_tutorial"); rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("mtc_tutorial", node_options);
std::thread spinning_thread([node] { rclcpp::spin(node); });
Task t; Task t;
t.stages()->setName("alternative path costs"); t.stages()->setName("alternative path costs");
t.loadRobotModel(); t.loadRobotModel(node);
assert(t.getRobotModel()->getName() == "panda"); assert(t.getRobotModel()->getName() == "panda");
@ -33,7 +37,7 @@ int main(int argc, char** argv) {
initial->setState(scene); initial->setState(scene);
t.add(std::move(initial)); t.add(std::move(initial));
auto pipeline{ std::make_shared<solvers::PipelinePlanner>() }; auto pipeline{ std::make_shared<solvers::PipelinePlanner>(node) };
auto alternatives{ std::make_unique<Alternatives>("connect") }; auto alternatives{ std::make_unique<Alternatives>("connect") };
{ {
@ -76,7 +80,8 @@ int main(int argc, char** argv) {
std::cout << e << std::endl; std::cout << e << std::endl;
} }
ros::spin(); // keep alive for interactive inspection in rviz
spinning_thread.join();
return 0; return 0;
} }

View File

@ -41,12 +41,12 @@
#include <moveit/task_constructor/stages/move_relative.h> #include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/connect.h> #include <moveit/task_constructor/stages/connect.h>
#include <ros/ros.h> #include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
Task createTask() { Task createTask(const rclcpp::Node::SharedPtr& node) {
Task t; Task t;
t.stages()->setName("Cartesian Path"); t.stages()->setName("Cartesian Path");
@ -59,7 +59,7 @@ Task createTask() {
auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>(); auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>();
// start from a fixed robot state // start from a fixed robot state
t.loadRobotModel(); t.loadRobotModel(node);
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel()); auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
{ {
auto& state = scene->getCurrentStateNonConst(); auto& state = scene->getCurrentStateNonConst();
@ -73,7 +73,7 @@ Task createTask() {
{ {
auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian_interpolation); auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian_interpolation);
stage->setGroup(group); stage->setGroup(group);
geometry_msgs::Vector3Stamped direction; geometry_msgs::msg::Vector3Stamped direction;
direction.header.frame_id = "world"; direction.header.frame_id = "world";
direction.vector.x = 0.2; direction.vector.x = 0.2;
stage->setDirection(direction); stage->setDirection(direction);
@ -83,7 +83,7 @@ Task createTask() {
{ {
auto stage = std::make_unique<stages::MoveRelative>("y -0.3", cartesian_interpolation); auto stage = std::make_unique<stages::MoveRelative>("y -0.3", cartesian_interpolation);
stage->setGroup(group); stage->setGroup(group);
geometry_msgs::Vector3Stamped direction; geometry_msgs::msg::Vector3Stamped direction;
direction.header.frame_id = "world"; direction.header.frame_id = "world";
direction.vector.y = -0.3; direction.vector.y = -0.3;
stage->setDirection(direction); stage->setDirection(direction);
@ -93,7 +93,7 @@ Task createTask() {
{ // rotate about TCP { // rotate about TCP
auto stage = std::make_unique<stages::MoveRelative>("rz +45°", cartesian_interpolation); auto stage = std::make_unique<stages::MoveRelative>("rz +45°", cartesian_interpolation);
stage->setGroup(group); stage->setGroup(group);
geometry_msgs::TwistStamped twist; geometry_msgs::msg::TwistStamped twist;
twist.header.frame_id = "world"; twist.header.frame_id = "world";
twist.twist.angular.z = M_PI / 4.; twist.twist.angular.z = M_PI / 4.;
stage->setDirection(twist); stage->setDirection(twist);
@ -108,16 +108,8 @@ Task createTask() {
t.add(std::move(stage)); t.add(std::move(stage));
} }
{ // move gripper into predefined open state
auto stage = std::make_unique<stages::MoveTo>("open gripper", joint_interpolation);
stage->setGroup(eef);
stage->setGoal("open");
t.add(std::move(stage));
}
{ // move from reached state back to the original state, using joint interpolation { // move from reached state back to the original state, using joint interpolation
// specifying two groups (arm and hand) will try to merge both trajectories stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation } };
stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation }, { eef, joint_interpolation } };
auto connect = std::make_unique<stages::Connect>("connect", planners); auto connect = std::make_unique<stages::Connect>("connect", planners);
t.add(std::move(connect)); t.add(std::move(connect));
} }
@ -132,12 +124,11 @@ Task createTask() {
} }
int main(int argc, char** argv) { int main(int argc, char** argv) {
ros::init(argc, argv, "mtc_tutorial"); rclcpp::init(argc, argv);
// run an asynchronous spinner to communicate with the move_group node and rviz auto node = rclcpp::Node::make_shared("mtc_tutorial");
ros::AsyncSpinner spinner(1); std::thread spinning_thread([node] { rclcpp::spin(node); });
spinner.start();
auto task = createTask(); auto task = createTask(node);
try { try {
if (task.plan()) if (task.plan())
task.introspection().publishSolution(*task.solutions().front()); task.introspection().publishSolution(*task.solutions().front());
@ -145,6 +136,7 @@ int main(int argc, char** argv) {
std::cerr << "planning failed with exception" << std::endl << ex << task; std::cerr << "planning failed with exception" << std::endl << ex << task;
} }
ros::waitForShutdown(); // keep alive for interactive inspection in rviz // keep alive for interactive inspection in rviz
spinning_thread.join();
return 0; return 0;
} }

View File

@ -1,4 +1,4 @@
#include <ros/ros.h> #include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
@ -8,24 +8,23 @@
#include <moveit/task_constructor/stages/compute_ik.h> #include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/cost_terms.h> #include <moveit/task_constructor/cost_terms.h>
#include <rosparam_shortcuts/rosparam_shortcuts.h>
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
/* ComputeIK(FixedState) */ /* ComputeIK(FixedState) */
int main(int argc, char** argv) { int main(int argc, char** argv) {
ros::init(argc, argv, "mtc_tutorial"); rclcpp::init(argc, argv);
ros::NodeHandle nh{ "~" }; auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo");
std::thread spinning_thread([node] { rclcpp::spin(node); });
ros::AsyncSpinner spinner{ 1 };
spinner.start();
Task t; Task t;
t.stages()->setName("clearance IK"); t.stages()->setName("clearance IK");
// announce new task (in case previous run was restarted) // announce new task (in case previous run was restarted)
ros::Duration(0.5).sleep(); rclcpp::sleep_for(std::chrono::milliseconds(500));
t.loadRobotModel(); t.loadRobotModel(node);
assert(t.getRobotModel()->getName() == "panda"); assert(t.getRobotModel()->getName() == "panda");
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel()); auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
@ -33,10 +32,10 @@ int main(int argc, char** argv) {
robot_state.setToDefaultValues(); robot_state.setToDefaultValues();
robot_state.setToDefaultValues(robot_state.getJointModelGroup("panda_arm"), "extended"); robot_state.setToDefaultValues(robot_state.getJointModelGroup("panda_arm"), "extended");
moveit_msgs::CollisionObject co; moveit_msgs::msg::CollisionObject co;
co.id = "obstacle"; co.id = "obstacle";
co.primitives.emplace_back(); co.primitives.emplace_back();
co.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE; co.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
co.primitives[0].dimensions.resize(1); co.primitives[0].dimensions.resize(1);
co.primitives[0].dimensions[0] = 0.1; co.primitives[0].dimensions[0] = 0.1;
co.header.frame_id = "world"; co.header.frame_id = "world";
@ -56,8 +55,10 @@ int main(int argc, char** argv) {
ik->setMaxIKSolutions(100); ik->setMaxIKSolutions(100);
auto cl_cost{ std::make_unique<cost::Clearance>() }; auto cl_cost{ std::make_unique<cost::Clearance>() };
cl_cost->cumulative = nh.param("cumulative", false); // sum up pairwise distances? cl_cost->cumulative = false;
cl_cost->with_world = nh.param("with_world", true); // consider distance to world objects? rosparam_shortcuts::get(node, "cumulative", cl_cost->cumulative); // sum up pairwise distances?
cl_cost->with_world = true;
rosparam_shortcuts::get(node, "with_world", cl_cost->cumulative); // consider distance to world objects?
ik->setCostTerm(std::move(cl_cost)); ik->setCostTerm(std::move(cl_cost));
t.add(std::move(ik)); t.add(std::move(ik));
@ -68,7 +69,7 @@ int main(int argc, char** argv) {
std::cout << e << std::endl; std::cout << e << std::endl;
} }
ros::waitForShutdown(); // Keep introspection alive
spinning_thread.join();
return 0; return 0;
} }

View File

@ -42,7 +42,7 @@
#include <moveit/task_constructor/stages/connect.h> #include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/container.h> #include <moveit/task_constructor/container.h>
#include <ros/ros.h> #include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
@ -59,7 +59,7 @@ std::unique_ptr<SerialContainer> createModule(const std::string& group) {
{ {
auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian); auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian);
stage->properties().configureInitFrom(Stage::PARENT, { "group" }); stage->properties().configureInitFrom(Stage::PARENT, { "group" });
geometry_msgs::Vector3Stamped direction; geometry_msgs::msg::Vector3Stamped direction;
direction.header.frame_id = "world"; direction.header.frame_id = "world";
direction.vector.x = 0.2; direction.vector.x = 0.2;
stage->setDirection(direction); stage->setDirection(direction);
@ -69,7 +69,7 @@ std::unique_ptr<SerialContainer> createModule(const std::string& group) {
{ {
auto stage = std::make_unique<stages::MoveRelative>("y -0.3", cartesian); auto stage = std::make_unique<stages::MoveRelative>("y -0.3", cartesian);
stage->properties().configureInitFrom(Stage::PARENT); stage->properties().configureInitFrom(Stage::PARENT);
geometry_msgs::Vector3Stamped direction; geometry_msgs::msg::Vector3Stamped direction;
direction.header.frame_id = "world"; direction.header.frame_id = "world";
direction.vector.y = -0.3; direction.vector.y = -0.3;
stage->setDirection(direction); stage->setDirection(direction);
@ -79,7 +79,7 @@ std::unique_ptr<SerialContainer> createModule(const std::string& group) {
{ // rotate about TCP { // rotate about TCP
auto stage = std::make_unique<stages::MoveRelative>("rz +45°", cartesian); auto stage = std::make_unique<stages::MoveRelative>("rz +45°", cartesian);
stage->properties().configureInitFrom(Stage::PARENT); stage->properties().configureInitFrom(Stage::PARENT);
geometry_msgs::TwistStamped twist; geometry_msgs::msg::TwistStamped twist;
twist.header.frame_id = "world"; twist.header.frame_id = "world";
twist.twist.angular.z = M_PI / 4.; twist.twist.angular.z = M_PI / 4.;
stage->setDirection(twist); stage->setDirection(twist);
@ -95,8 +95,9 @@ std::unique_ptr<SerialContainer> createModule(const std::string& group) {
return c; return c;
} }
Task createTask() { Task createTask(const rclcpp::Node::SharedPtr& node) {
Task t; Task t;
t.loadRobotModel(node);
t.stages()->setName("Reusable Containers"); t.stages()->setName("Reusable Containers");
t.add(std::make_unique<stages::CurrentState>("current")); t.add(std::make_unique<stages::CurrentState>("current"));
@ -111,12 +112,11 @@ Task createTask() {
} }
int main(int argc, char** argv) { int main(int argc, char** argv) {
ros::init(argc, argv, "mtc_tutorial"); rclcpp::init(argc, argv);
// run an asynchronous spinner to communicate with the move_group node and rviz auto node = rclcpp::Node::make_shared("mtc_tutorial");
ros::AsyncSpinner spinner(1); std::thread spinning_thread([node] { rclcpp::spin(node); });
spinner.start();
auto task = createTask(); auto task = createTask(node);
try { try {
if (task.plan()) if (task.plan())
task.introspection().publishSolution(*task.solutions().front()); task.introspection().publishSolution(*task.solutions().front());
@ -124,6 +124,7 @@ int main(int argc, char** argv) {
std::cerr << "planning failed with exception" << std::endl << ex << task; std::cerr << "planning failed with exception" << std::endl << ex << task;
} }
ros::waitForShutdown(); // keep alive for interactive inspection in rviz // keep alive for interactive inspection in rviz
spinning_thread.join();
return 0; return 0;
} }

View File

@ -31,47 +31,48 @@
*********************************************************************/ *********************************************************************/
/* Author: Henning Kayser, Simon Goldstein /* Author: Henning Kayser, Simon Goldstein
Desc: A demo to show MoveIt Task Constructor in action Desc: A demo to show MoveIt Task Constructor in action
*/ */
// ROS // ROS
#include <ros/ros.h> #include <rclcpp/rclcpp.hpp>
// MTC pick/place demo implementation // MTC pick/place demo implementation
#include <moveit_task_constructor_demo/pick_place_task.h> #include <moveit_task_constructor_demo/pick_place_task.h>
constexpr char LOGNAME[] = "moveit_task_constructor_demo"; #include <rosparam_shortcuts/rosparam_shortcuts.h>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo");
int main(int argc, char** argv) { int main(int argc, char** argv) {
ros::init(argc, argv, "mtc_tutorial"); rclcpp::init(argc, argv);
ros::NodeHandle nh, pnh("~"); rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo", node_options);
std::thread spinning_thread([node] { rclcpp::spin(node); });
// Handle Task introspection requests from RViz & feedback during execution moveit_task_constructor_demo::setupDemoScene(node);
ros::AsyncSpinner spinner(1);
spinner.start();
moveit_task_constructor_demo::setupDemoScene(pnh);
// Construct and run pick/place task // Construct and run pick/place task
moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", pnh); moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", node);
if (!pick_place_task.init()) { if (!pick_place_task.init()) {
ROS_INFO_NAMED(LOGNAME, "Initialization failed"); RCLCPP_INFO(LOGGER, "Initialization failed");
return 1; return 1;
} }
if (pick_place_task.plan()) { if (pick_place_task.plan()) {
ROS_INFO_NAMED(LOGNAME, "Planning succeded"); RCLCPP_INFO(LOGGER, "Planning succeded");
if (pnh.param("execute", false)) { if (bool execute = false; rosparam_shortcuts::get(node, "execute", execute) && execute) {
pick_place_task.execute(); pick_place_task.execute();
ROS_INFO_NAMED(LOGNAME, "Execution complete"); RCLCPP_INFO(LOGGER, "Execution complete");
} else { } else {
ROS_INFO_NAMED(LOGNAME, "Execution disabled"); RCLCPP_INFO(LOGGER, "Execution disabled");
} }
} else { } else {
ROS_INFO_NAMED(LOGNAME, "Planning failed"); RCLCPP_INFO(LOGGER, "Planning failed");
} }
// Keep introspection alive // Keep introspection alive
ros::waitForShutdown(); spinning_thread.join();
return 0; return 0;
} }

View File

@ -31,77 +31,77 @@
*********************************************************************/ *********************************************************************/
/* Author: Henning Kayser, Simon Goldstein /* Author: Henning Kayser, Simon Goldstein
Desc: A demo to show MoveIt Task Constructor in action Desc: A demo to show MoveIt Task Constructor in action
*/ */
#include <moveit_task_constructor_demo/pick_place_task.h> #include <moveit_task_constructor_demo/pick_place_task.h>
#include <rosparam_shortcuts/rosparam_shortcuts.h> #include <rosparam_shortcuts/rosparam_shortcuts.h>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo");
namespace moveit_task_constructor_demo { namespace moveit_task_constructor_demo {
constexpr char LOGNAME[] = "moveit_task_constructor_demo"; void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi,
constexpr char PickPlaceTask::LOGNAME[]; const moveit_msgs::msg::CollisionObject& object) {
void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) {
if (!psi.applyCollisionObject(object)) if (!psi.applyCollisionObject(object))
throw std::runtime_error("Failed to spawn object: " + object.id); throw std::runtime_error("Failed to spawn object: " + object.id);
} }
moveit_msgs::CollisionObject createTable(ros::NodeHandle& pnh) { moveit_msgs::msg::CollisionObject createTable(const rclcpp::Node::SharedPtr& node) {
std::string table_name, table_reference_frame; std::string table_name, table_reference_frame;
std::vector<double> table_dimensions; std::vector<double> table_dimensions;
geometry_msgs::Pose pose; geometry_msgs::msg::Pose pose;
std::size_t errors = 0; std::size_t errors = 0;
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); errors += !rosparam_shortcuts::get(node, "table_name", table_name);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame); errors += !rosparam_shortcuts::get(node, "table_reference_frame", table_reference_frame);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); errors += !rosparam_shortcuts::get(node, "table_dimensions", table_dimensions);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); errors += !rosparam_shortcuts::get(node, "table_pose", pose);
rosparam_shortcuts::shutdownIfError(LOGNAME, errors); rosparam_shortcuts::shutdownIfError(errors);
moveit_msgs::CollisionObject object; moveit_msgs::msg::CollisionObject object;
object.id = table_name; object.id = table_name;
object.header.frame_id = table_reference_frame; object.header.frame_id = table_reference_frame;
object.primitives.resize(1); object.primitives.resize(1);
object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX;
object.primitives[0].dimensions = table_dimensions; object.primitives[0].dimensions = { table_dimensions.at(0), table_dimensions.at(1), table_dimensions.at(2) };
pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world
object.primitive_poses.push_back(pose); object.primitive_poses.push_back(pose);
return object; return object;
} }
moveit_msgs::CollisionObject createObject(ros::NodeHandle& pnh) { moveit_msgs::msg::CollisionObject createObject(const rclcpp::Node::SharedPtr& node) {
std::string object_name, object_reference_frame; std::string object_name, object_reference_frame;
std::vector<double> object_dimensions; std::vector<double> object_dimensions;
geometry_msgs::Pose pose; geometry_msgs::msg::Pose pose;
std::size_t error = 0; std::size_t error = 0;
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name); error += !rosparam_shortcuts::get(node, "object_name", object_name);
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame); error += !rosparam_shortcuts::get(node, "object_reference_frame", object_reference_frame);
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions); error += !rosparam_shortcuts::get(node, "object_dimensions", object_dimensions);
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose); error += !rosparam_shortcuts::get(node, "object_pose", pose);
rosparam_shortcuts::shutdownIfError(LOGNAME, error); rosparam_shortcuts::shutdownIfError(error);
moveit_msgs::CollisionObject object; moveit_msgs::msg::CollisionObject object;
object.id = object_name; object.id = object_name;
object.header.frame_id = object_reference_frame; object.header.frame_id = object_reference_frame;
object.primitives.resize(1); object.primitives.resize(1);
object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;
object.primitives[0].dimensions = object_dimensions; object.primitives[0].dimensions = { object_dimensions.at(0), object_dimensions.at(1) };
pose.position.z += 0.5 * object_dimensions[0]; pose.position.z += 0.5 * object_dimensions[0];
object.primitive_poses.push_back(pose); object.primitive_poses.push_back(pose);
return object; return object;
} }
void setupDemoScene(ros::NodeHandle& pnh) { void setupDemoScene(const rclcpp::Node::SharedPtr& node) {
// Add table and object to planning scene // Add table and object to planning scene
ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service rclcpp::sleep_for(std::chrono::microseconds(100)); // Wait for ApplyPlanningScene service
moveit::planning_interface::PlanningSceneInterface psi; moveit::planning_interface::PlanningSceneInterface psi;
if (pnh.param("spawn_table", true)) if (bool spawn_table = true; rosparam_shortcuts::get(node, "spawn_table", spawn_table) && spawn_table)
spawnObject(psi, createTable(pnh)); spawnObject(psi, createTable(node));
spawnObject(psi, createObject(pnh)); spawnObject(psi, createObject(node));
} }
PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh) PickPlaceTask::PickPlaceTask(const std::string& task_name, const rclcpp::Node::SharedPtr& node)
: pnh_(pnh), task_name_(task_name) { : node_(node), task_name_(task_name) {
loadParameters(); loadParameters();
} }
@ -111,41 +111,41 @@ void PickPlaceTask::loadParameters() {
* Load Parameters * * Load Parameters *
* * * *
***************************************************/ ***************************************************/
ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); RCLCPP_INFO(LOGGER, "Loading task parameters");
// Planning group properties // Planning group properties
size_t errors = 0; size_t errors = 0;
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_group_name", arm_group_name_); errors += !rosparam_shortcuts::get(node_, "arm_group_name", arm_group_name_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_group_name", hand_group_name_); errors += !rosparam_shortcuts::get(node_, "hand_group_name", hand_group_name_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "eef_name", eef_name_); errors += !rosparam_shortcuts::get(node_, "eef_name", eef_name_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_frame", hand_frame_); errors += !rosparam_shortcuts::get(node_, "hand_frame", hand_frame_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "world_frame", world_frame_); errors += !rosparam_shortcuts::get(node_, "world_frame", world_frame_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "grasp_frame_transform", grasp_frame_transform_); errors += !rosparam_shortcuts::get(node_, "grasp_frame_transform", grasp_frame_transform_);
// Predefined pose targets // Predefined pose targets
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_open_pose", hand_open_pose_); errors += !rosparam_shortcuts::get(node_, "hand_open_pose", hand_open_pose_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_close_pose", hand_close_pose_); errors += !rosparam_shortcuts::get(node_, "hand_close_pose", hand_close_pose_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_home_pose", arm_home_pose_); errors += !rosparam_shortcuts::get(node_, "arm_home_pose", arm_home_pose_);
// Target object // Target object
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_name", object_name_); errors += !rosparam_shortcuts::get(node_, "object_name", object_name_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_dimensions", object_dimensions_); errors += !rosparam_shortcuts::get(node_, "object_dimensions", object_dimensions_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_reference_frame", object_reference_frame_); errors += !rosparam_shortcuts::get(node_, "object_reference_frame", object_reference_frame_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "surface_link", surface_link_); errors += !rosparam_shortcuts::get(node_, "surface_link", surface_link_);
support_surfaces_ = { surface_link_ }; support_surfaces_ = { surface_link_ };
// Pick/Place metrics // Pick/Place metrics
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_min_dist", approach_object_min_dist_); errors += !rosparam_shortcuts::get(node_, "approach_object_min_dist", approach_object_min_dist_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_max_dist", approach_object_max_dist_); errors += !rosparam_shortcuts::get(node_, "approach_object_max_dist", approach_object_max_dist_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_min_dist", lift_object_min_dist_); errors += !rosparam_shortcuts::get(node_, "lift_object_min_dist", lift_object_min_dist_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_max_dist", lift_object_max_dist_); errors += !rosparam_shortcuts::get(node_, "lift_object_max_dist", lift_object_max_dist_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_surface_offset", place_surface_offset_); errors += !rosparam_shortcuts::get(node_, "place_surface_offset", place_surface_offset_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_pose", place_pose_); errors += !rosparam_shortcuts::get(node_, "place_pose", place_pose_);
rosparam_shortcuts::shutdownIfError(LOGNAME, errors); rosparam_shortcuts::shutdownIfError(errors);
} }
bool PickPlaceTask::init() { bool PickPlaceTask::init() {
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); RCLCPP_INFO(LOGGER, "Initializing task pipeline");
const std::string object = object_name_; const std::string object = object_name_;
// Reset ROS introspection before constructing the new object // Reset ROS introspection before constructing the new object
@ -155,10 +155,10 @@ bool PickPlaceTask::init() {
Task& t = *task_; Task& t = *task_;
t.stages()->setName(task_name_); t.stages()->setName(task_name_);
t.loadRobotModel(); t.loadRobotModel(node_);
// Sampling planner // Sampling planner
auto sampling_planner = std::make_shared<solvers::PipelinePlanner>(); auto sampling_planner = std::make_shared<solvers::PipelinePlanner>(node_);
sampling_planner->setProperty("goal_joint_tolerance", 1e-5); sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
// Cartesian planner // Cartesian planner
@ -245,7 +245,7 @@ bool PickPlaceTask::init() {
stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_);
// Set hand forward direction // Set hand forward direction
geometry_msgs::Vector3Stamped vec; geometry_msgs::msg::Vector3Stamped vec;
vec.header.frame_id = hand_frame_; vec.header.frame_id = hand_frame_;
vec.vector.z = 1.0; vec.vector.z = 1.0;
stage->setDirection(vec); stage->setDirection(vec);
@ -326,7 +326,7 @@ bool PickPlaceTask::init() {
stage->properties().set("marker_ns", "lift_object"); stage->properties().set("marker_ns", "lift_object");
// Set upward direction // Set upward direction
geometry_msgs::Vector3Stamped vec; geometry_msgs::msg::Vector3Stamped vec;
vec.header.frame_id = world_frame_; vec.header.frame_id = world_frame_;
vec.vector.z = 1.0; vec.vector.z = 1.0;
stage->setDirection(vec); stage->setDirection(vec);
@ -380,7 +380,7 @@ bool PickPlaceTask::init() {
stage->setMinMaxDistance(.03, .13); stage->setMinMaxDistance(.03, .13);
// Set downward direction // Set downward direction
geometry_msgs::Vector3Stamped vec; geometry_msgs::msg::Vector3Stamped vec;
vec.header.frame_id = world_frame_; vec.header.frame_id = world_frame_;
vec.vector.z = -1.0; vec.vector.z = -1.0;
stage->setDirection(vec); stage->setDirection(vec);
@ -398,7 +398,7 @@ bool PickPlaceTask::init() {
stage->setObject(object); stage->setObject(object);
// Set target pose // Set target pose
geometry_msgs::PoseStamped p; geometry_msgs::msg::PoseStamped p;
p.header.frame_id = object_reference_frame_; p.header.frame_id = object_reference_frame_;
p.pose = place_pose_; p.pose = place_pose_;
p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_;
@ -451,7 +451,7 @@ bool PickPlaceTask::init() {
stage->setMinMaxDistance(.12, .25); stage->setMinMaxDistance(.12, .25);
stage->setIKFrame(hand_frame_); stage->setIKFrame(hand_frame_);
stage->properties().set("marker_ns", "retreat"); stage->properties().set("marker_ns", "retreat");
geometry_msgs::Vector3Stamped vec; geometry_msgs::msg::Vector3Stamped vec;
vec.header.frame_id = hand_frame_; vec.header.frame_id = hand_frame_;
vec.vector.z = -1.0; vec.vector.z = -1.0;
stage->setDirection(vec); stage->setDirection(vec);
@ -479,7 +479,7 @@ bool PickPlaceTask::init() {
try { try {
t.init(); t.init();
} catch (InitStageException& e) { } catch (InitStageException& e) {
ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e); RCLCPP_ERROR_STREAM(LOGGER, "Initialization failed: " << e);
return false; return false;
} }
@ -487,27 +487,28 @@ bool PickPlaceTask::init() {
} }
bool PickPlaceTask::plan() { bool PickPlaceTask::plan() {
ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); RCLCPP_INFO(LOGGER, "Start searching for task solutions");
int max_solutions = pnh_.param<int>("max_solutions", 10); int max_solutions = 10;
rosparam_shortcuts::get(node_, "max_solutions", max_solutions);
return task_->plan(max_solutions); return task_->plan(max_solutions);
} }
bool PickPlaceTask::execute() { bool PickPlaceTask::execute() {
ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); RCLCPP_INFO(LOGGER, "Executing solution trajectory");
moveit_msgs::MoveItErrorCodes execute_result; moveit_msgs::msg::MoveItErrorCodes execute_result;
execute_result = task_->execute(*task_->solutions().front()); execute_result = task_->execute(*task_->solutions().front());
// // If you want to inspect the goal message, use this instead: // // If you want to inspect the goal message, use this instead:
// actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> // actionlib::SimpleActionClient<moveit_task_constructor_msgs::msg::ExecuteTaskSolutionAction>
// execute("execute_task_solution", true); execute.waitForServer(); // execute("execute_task_solution", true); execute.waitForServer();
// moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; // moveit_task_constructor_msgs::msg::ExecuteTaskSolutionGoal execute_goal;
// task_->solutions().front()->fillMessage(execute_goal.solution); // task_->solutions().front()->fillMessage(execute_goal.solution);
// execute.sendGoalAndWait(execute_goal); // execute.sendGoalAndWait(execute_goal);
// execute_result = execute.getResult()->error_code; // execute_result = execute.getResult()->error_code;
if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { if (execute_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_result.val); RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed and returned: " << execute_result.val);
return false; return false;
} }

6
doc/MIGRATION_GUIDE.md Normal file
View File

@ -0,0 +1,6 @@
# Migration Guide from ROS1
* Default C++ standard set to 17
* CMake version set to 3.5
* PipelinePlanner's constructor now have a node `rclcpp::Node::SharedPtr` as an argument to load the `PlanningPipeline`'s parameters
* `Task::loadRobotModel` have a node as a parameter and the user have to call loadRobotModel explicitly otherwise init will throw an exception

View File

@ -1,38 +1,43 @@
cmake_minimum_required(VERSION 3.1.3) cmake_minimum_required(VERSION 3.5)
project(moveit_task_constructor_msgs) project(moveit_task_constructor_msgs)
set(MSG_DEPS moveit_msgs visualization_msgs) find_package(ament_cmake REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(catkin REQUIRED COMPONENTS find_package(rosidl_default_generators REQUIRED)
message_generation find_package(moveit_msgs REQUIRED)
${MSG_DEPS} find_package(visualization_msgs REQUIRED)
)
# ROS messages, services and actions # ROS messages, services and actions
add_message_files(DIRECTORY msg FILES set(msg_files
Property.msg "msg/Property.msg"
Solution.msg "msg/Solution.msg"
SolutionInfo.msg "msg/SolutionInfo.msg"
StageDescription.msg "msg/StageDescription.msg"
StageStatistics.msg "msg/StageStatistics.msg"
SubSolution.msg "msg/SubSolution.msg"
SubTrajectory.msg "msg/SubTrajectory.msg"
TaskDescription.msg "msg/TaskDescription.msg"
TaskStatistics.msg "msg/TaskStatistics.msg"
) )
add_service_files(DIRECTORY srv FILES set(srv_files
GetSolution.srv "srv/GetSolution.srv"
) )
add_action_files(DIRECTORY action FILES set(action_files
ExecuteTaskSolution.action "action/ExecuteTaskSolution.action"
) )
generate_messages(DEPENDENCIES ${MSG_DEPS}) rosidl_generate_interfaces(${PROJECT_NAME}
${msg_files}
catkin_package( ${srv_files}
CATKIN_DEPENDS ${action_files}
message_runtime DEPENDENCIES
${MSG_DEPS} builtin_interfaces
moveit_msgs
visualization_msgs
) )
ament_export_dependencies(rosidl_default_runtime)
ament_package()

Some files were not shown because too many files have changed in this diff Show More