mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
ROS 2 Migration (#170)
This commit is contained in:
commit
98ced788c8
26
.github/workflows/ci.yaml
vendored
26
.github/workflows/ci.yaml
vendored
@ -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
|
||||||
|
|||||||
3
.github/workflows/format.yaml
vendored
3
.github/workflows/format.yaml
vendored
@ -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
|
||||||
|
|||||||
@ -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
5
.repos
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
repositories:
|
||||||
|
rosparam_shortcuts:
|
||||||
|
type: git
|
||||||
|
url: https://github.com/PickNikRobotics/rosparam_shortcuts
|
||||||
|
version: ros2
|
||||||
@ -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()
|
||||||
|
|||||||
@ -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.
|
||||||
|
|||||||
@ -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>
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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()
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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) */
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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 = "") {
|
||||||
|
|||||||
@ -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));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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:
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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); }
|
||||||
|
|
||||||
|
|||||||
@ -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));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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 {
|
||||||
|
|||||||
@ -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); }
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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">
|
||||||
|
|||||||
@ -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>
|
||||||
|
|||||||
@ -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)
|
||||||
|
|||||||
@ -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,7 +446,8 @@ 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()
|
||||||
|
<< "' received solution of child stage '"
|
||||||
<< current.creator()->name() << "'");
|
<< current.creator()->name() << "'");
|
||||||
|
|
||||||
// failures should never trigger this callback
|
// failures should never trigger this callback
|
||||||
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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);
|
||||||
|
|
||||||
|
|||||||
@ -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()
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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,7 +289,7 @@ 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;
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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();
|
||||||
|
|||||||
@ -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);
|
||||||
|
}
|
||||||
|
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.");
|
"Attempting to load pipeline from old parameter structure. Please update your MoveIt config.");
|
||||||
pipeline_ns = spec.ns;
|
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,12 +111,13 @@ 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");
|
||||||
@ -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);
|
||||||
|
|
||||||
|
|||||||
@ -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
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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)
|
||||||
|
|||||||
@ -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();
|
||||||
|
|
||||||
|
|||||||
@ -46,13 +46,15 @@ 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");
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -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,7 +121,7 @@ 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);
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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)));
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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);
|
||||||
|
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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,9 +58,9 @@ 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;
|
||||||
}
|
}
|
||||||
@ -66,9 +68,9 @@ void ModifyPlanningScene::addObject(const moveit_msgs::CollisionObject& collisio
|
|||||||
}
|
}
|
||||||
|
|
||||||
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
|
||||||
|
|||||||
@ -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);
|
||||||
|
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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 {
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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
|
||||||
|
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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));
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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()
|
||||||
|
|||||||
@ -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();
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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
110
core/test/move_to.launch.py
Normal 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)
|
||||||
@ -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>
|
|
||||||
@ -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();
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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();
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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();
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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 {
|
||||||
|
|||||||
@ -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>
|
||||||
|
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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();
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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";
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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
|
|
||||||
|
find_package(rclcpp REQUIRED)
|
||||||
|
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)
|
||||||
|
|
||||||
|
set(THIS_PACKAGE_INCLUDE_DEPENDS
|
||||||
|
rclcpp
|
||||||
moveit_core
|
moveit_core
|
||||||
moveit_task_constructor_core
|
moveit_task_constructor_core
|
||||||
|
moveit_task_constructor_msgs
|
||||||
|
moveit_ros_planning
|
||||||
moveit_ros_planning_interface
|
moveit_ros_planning_interface
|
||||||
rosparam_shortcuts
|
tf2_eigen
|
||||||
)
|
rosparam_shortcuts)
|
||||||
|
|
||||||
catkin_package(
|
add_library(${PROJECT_NAME}_pick_place_task SHARED src/pick_place_task.cpp)
|
||||||
CATKIN_DEPENDS roscpp
|
ament_target_dependencies(${PROJECT_NAME}_pick_place_task ${THIS_PACKAGE_INCLUDE_DEPENDS})
|
||||||
)
|
|
||||||
|
|
||||||
include_directories(
|
|
||||||
include
|
|
||||||
${catkin_INCLUDE_DIRS}
|
|
||||||
)
|
|
||||||
|
|
||||||
add_library(${PROJECT_NAME}_pick_place_task src/pick_place_task.cpp)
|
|
||||||
target_link_libraries(${PROJECT_NAME}_pick_place_task ${catkin_LIBRARIES})
|
|
||||||
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()
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -35,7 +35,7 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
// 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
|
||||||
|
|||||||
73
demo/launch/alternative_path_costs.launch.py
Normal file
73
demo/launch/alternative_path_costs.launch.py
Normal 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])
|
||||||
55
demo/launch/cartesian.launch.py
Normal file
55
demo/launch/cartesian.launch.py
Normal 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])
|
||||||
@ -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
188
demo/launch/demo.launch.py
Normal 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
|
||||||
|
)
|
||||||
55
demo/launch/ik_clearance_cost.launch.py
Normal file
55
demo/launch/ik_clearance_cost.launch.py
Normal 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])
|
||||||
55
demo/launch/modular.launch.py
Normal file
55
demo/launch/modular.launch.py
Normal 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])
|
||||||
@ -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>
|
|
||||||
78
demo/launch/pickplace.launch.py
Normal file
78
demo/launch/pickplace.launch.py
Normal 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])
|
||||||
@ -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>
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -35,43 +35,44 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
// 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;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -37,71 +37,71 @@
|
|||||||
#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
6
doc/MIGRATION_GUIDE.md
Normal 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
|
||||||
@ -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
Loading…
Reference in New Issue
Block a user