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
|
||||
matrix:
|
||||
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
|
||||
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
|
||||
- IMAGE: master-source
|
||||
- IMAGE: rolling-source
|
||||
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
|
||||
- IMAGE: noetic-source
|
||||
- IMAGE: rolling-source
|
||||
CXX: clang++
|
||||
CLANG_TIDY: true
|
||||
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
|
||||
- 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
|
||||
DOCKER_RUN_OPTS: >-
|
||||
-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"
|
||||
|
||||
env:
|
||||
CATKIN_LINT: true
|
||||
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
|
||||
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
|
||||
BASEDIR: ${{ github.workspace }}/.work
|
||||
CACHE_PREFIX: "${{ matrix.env.IMAGE }}${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && '-ccov' || '' }}"
|
||||
@ -48,7 +52,7 @@ jobs:
|
||||
CC: ${{ 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
|
||||
steps:
|
||||
- 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
|
||||
- name: 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
|
||||
id: precommit
|
||||
- name: Upload pre-commit changes
|
||||
|
||||
@ -42,10 +42,3 @@ repos:
|
||||
language: system
|
||||
files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
|
||||
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)
|
||||
|
||||
add_compile_options(-std=c++14)
|
||||
add_compile_options(-std=c++17)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
actionlib
|
||||
moveit_core
|
||||
moveit_ros_move_group
|
||||
moveit_task_constructor_core
|
||||
moveit_task_constructor_msgs
|
||||
pluginlib
|
||||
std_msgs
|
||||
)
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
catkin_package(
|
||||
LIBRARIES $PROJECT_NAME}
|
||||
CATKIN_DEPENDS
|
||||
actionlib
|
||||
moveit_task_constructor_msgs
|
||||
std_msgs
|
||||
)
|
||||
find_package(Boost REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(moveit_core REQUIRED)
|
||||
find_package(moveit_ros_move_group REQUIRED)
|
||||
find_package(moveit_task_constructor_msgs REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME}
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
src/execute_task_solution_capability.cpp
|
||||
)
|
||||
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||
ament_target_dependencies(${PROJECT_NAME}
|
||||
Boost
|
||||
rclcpp_action
|
||||
moveit_core
|
||||
moveit_ros_move_group
|
||||
moveit_task_constructor_msgs
|
||||
)
|
||||
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION lib
|
||||
)
|
||||
|
||||
install(FILES capabilities_plugin_description.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
pluginlib_export_plugin_description_file(moveit_ros_move_group capabilities_plugin_description.xml)
|
||||
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">
|
||||
<description>
|
||||
Action server to execute solutions generated through the MoveIt Task Constructor.
|
||||
|
||||
@ -10,17 +10,18 @@
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>moveit_core</depend>
|
||||
<depend>moveit_ros_move_group</depend>
|
||||
<depend>actionlib</depend>
|
||||
<depend>moveit_ros_planning</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>moveit_task_constructor_core</depend>
|
||||
<depend>moveit_task_constructor_msgs</depend>
|
||||
|
||||
<export>
|
||||
<moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@ -36,16 +36,16 @@
|
||||
|
||||
#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/trajectory_processing/trajectory_tools.h>
|
||||
#include <moveit/kinematic_constraints/utils.h>
|
||||
#include <moveit/move_group/capability_names.h>
|
||||
#include <moveit/robot_state/conversions.h>
|
||||
#if MOVEIT_HAS_MESSAGE_CHECKS
|
||||
#include <moveit/utils/message_checks.h>
|
||||
#endif
|
||||
#include <moveit/moveit_cpp/moveit_cpp.h>
|
||||
|
||||
#include <boost/algorithm/string/join.hpp>
|
||||
|
||||
namespace {
|
||||
|
||||
@ -80,58 +80,63 @@ const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::Rob
|
||||
}
|
||||
} // namespace
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.execute_task_solution");
|
||||
|
||||
namespace move_group {
|
||||
|
||||
ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability() : MoveGroupCapability("ExecuteTaskSolution") {}
|
||||
|
||||
void ExecuteTaskSolutionCapability::initialize() {
|
||||
// configure the action server
|
||||
as_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>(
|
||||
root_node_handle_, "execute_task_solution",
|
||||
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1), false));
|
||||
as_->registerPreemptCallback(std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this));
|
||||
as_->start();
|
||||
as_ = rclcpp_action::create_server<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
|
||||
context_->moveit_cpp_->getNode(), "execute_task_solution",
|
||||
ActionServerType::GoalCallback(std::bind(&ExecuteTaskSolutionCapability::handleNewGoal, this,
|
||||
std::placeholders::_1, std::placeholders::_2)),
|
||||
ActionServerType::CancelCallback(
|
||||
std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)),
|
||||
ActionServerType::AcceptedCallback(
|
||||
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1)));
|
||||
}
|
||||
|
||||
void ExecuteTaskSolutionCapability::goalCallback(
|
||||
const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal) {
|
||||
moveit_task_constructor_msgs::ExecuteTaskSolutionResult result;
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle) {
|
||||
auto result = std::make_shared<moveit_task_constructor_msgs::action::ExecuteTaskSolution::Result>();
|
||||
|
||||
const auto& goal = goal_handle->get_goal();
|
||||
if (!context_->plan_execution_) {
|
||||
const std::string response = "Cannot execute solution. ~allow_trajectory_execution was set to false";
|
||||
result.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
|
||||
as_->setAborted(result, response);
|
||||
result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED;
|
||||
goal_handle->abort(result);
|
||||
return;
|
||||
}
|
||||
|
||||
plan_execution::ExecutableMotionPlan 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 {
|
||||
ROS_INFO_NAMED("ExecuteTaskSolution", "Executing TaskSolution");
|
||||
result.error_code = context_->plan_execution_->executeAndMonitor(plan);
|
||||
RCLCPP_INFO(LOGGER, "Executing TaskSolution");
|
||||
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::MoveItErrorCodes::SUCCESS)
|
||||
as_->setSucceeded(result, response);
|
||||
else if (result.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
|
||||
as_->setPreempted(result, response);
|
||||
if (result->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
|
||||
goal_handle->succeed(result);
|
||||
else if (result->error_code.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED)
|
||||
goal_handle->canceled(result);
|
||||
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_)
|
||||
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) {
|
||||
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_);
|
||||
state = scene->getCurrentState();
|
||||
@ -139,7 +144,7 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
|
||||
|
||||
plan.plan_components_.reserve(solution.sub_trajectory.size());
|
||||
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_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back();
|
||||
@ -156,12 +161,11 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
|
||||
if (!joint_names.empty()) {
|
||||
group = findJointModelGroup(*model, joint_names);
|
||||
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, ", ") << "}");
|
||||
return false;
|
||||
}
|
||||
ROS_DEBUG_NAMED("ExecuteTaskSolution", "Using JointModelGroup '%s' for execution",
|
||||
group->getName().c_str());
|
||||
RCLCPP_DEBUG(LOGGER, "Using JointModelGroup '%s' for execution", group->getName().c_str());
|
||||
}
|
||||
}
|
||||
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 */
|
||||
exec_traj.effect_on_success_ = [this, sub_traj,
|
||||
description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
|
||||
#if MOVEIT_HAS_MESSAGE_CHECKS
|
||||
if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
|
||||
#else
|
||||
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)) {
|
||||
#endif
|
||||
ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
|
||||
RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);
|
||||
return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
|
||||
}
|
||||
return true;
|
||||
};
|
||||
|
||||
#if MOVEIT_HAS_MESSAGE_CHECKS
|
||||
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)) {
|
||||
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution",
|
||||
"invalid intermediate robot state in scene diff of SubTrajectory " << description);
|
||||
RCLCPP_ERROR_STREAM(LOGGER, "invalid intermediate robot state in scene diff of SubTrajectory " << description);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
@ -42,9 +42,9 @@
|
||||
#pragma once
|
||||
|
||||
#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>
|
||||
|
||||
@ -58,13 +58,23 @@ public:
|
||||
void initialize() override;
|
||||
|
||||
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);
|
||||
|
||||
void goalCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal);
|
||||
void preemptCallback();
|
||||
void goalCallback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle);
|
||||
|
||||
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
|
||||
|
||||
@ -1,34 +1,22 @@
|
||||
cmake_minimum_required(VERSION 3.1.3)
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(moveit_task_constructor_core)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
find_package(Boost REQUIRED)
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
tf2_eigen
|
||||
geometry_msgs
|
||||
moveit_core
|
||||
moveit_ros_planning
|
||||
moveit_ros_planning_interface
|
||||
moveit_task_constructor_msgs
|
||||
roscpp
|
||||
visualization_msgs
|
||||
rviz_marker_tools
|
||||
)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(moveit_core REQUIRED)
|
||||
find_package(moveit_ros_planning REQUIRED)
|
||||
find_package(moveit_ros_planning_interface REQUIRED)
|
||||
find_package(moveit_task_constructor_msgs REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rviz_marker_tools REQUIRED)
|
||||
find_package(tf2_eigen REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
|
||||
catkin_package(
|
||||
LIBRARIES
|
||||
${PROJECT_NAME}
|
||||
${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)
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
endif()
|
||||
|
||||
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(test)
|
||||
|
||||
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
|
||||
install(DIRECTORY include/ DESTINATION include
|
||||
PATTERN "*_p.h" EXCLUDE)
|
||||
install(FILES
|
||||
motion_planning_stages_plugin_description.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
|
||||
pluginlib_export_plugin_description_file(${PROJECT_NAME} motion_planning_stages_plugin_description.xml)
|
||||
|
||||
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
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit_task_constructor_msgs/TaskDescription.h>
|
||||
#include <moveit_task_constructor_msgs/TaskStatistics.h>
|
||||
#include <moveit_task_constructor_msgs/Solution.h>
|
||||
#include <moveit_task_constructor_msgs/GetSolution.h>
|
||||
#include <moveit_task_constructor_msgs/msg/task_description.hpp>
|
||||
#include <moveit_task_constructor_msgs/msg/task_statistics.hpp>
|
||||
#include <moveit_task_constructor_msgs/msg/solution.hpp>
|
||||
#include <moveit_task_constructor_msgs/srv/get_solution.hpp>
|
||||
|
||||
#define DESCRIPTION_TOPIC "description"
|
||||
#define STATISTICS_TOPIC "statistics"
|
||||
@ -72,13 +72,14 @@ public:
|
||||
~Introspection();
|
||||
|
||||
/// fill task description message for publishing the task configuration
|
||||
moveit_task_constructor_msgs::TaskDescription&
|
||||
fillTaskDescription(moveit_task_constructor_msgs::TaskDescription& msg);
|
||||
moveit_task_constructor_msgs::msg::TaskDescription&
|
||||
fillTaskDescription(moveit_task_constructor_msgs::msg::TaskDescription& msg);
|
||||
/// publish detailed task description
|
||||
void publishTaskDescription();
|
||||
|
||||
/// 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
|
||||
void publishTaskState();
|
||||
|
||||
@ -95,8 +96,8 @@ public:
|
||||
void publishAllSolutions(bool wait = true);
|
||||
|
||||
/// get solution
|
||||
bool getSolution(moveit_task_constructor_msgs::GetSolution::Request& req,
|
||||
moveit_task_constructor_msgs::GetSolution::Response& res);
|
||||
bool getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr req,
|
||||
const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr res);
|
||||
|
||||
/// retrieve id of given stage
|
||||
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);
|
||||
|
||||
private:
|
||||
void fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::StageStatistics& s);
|
||||
void fillSolution(moveit_task_constructor_msgs::Solution& msg, const SolutionBase& s);
|
||||
void fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::msg::StageStatistics& s);
|
||||
void fillSolution(moveit_task_constructor_msgs::msg::Solution& msg, const SolutionBase& s);
|
||||
/// retrieve or set id of given stage
|
||||
uint32_t stageId(const moveit::task_constructor::Stage* const s);
|
||||
/// retrieve solution with given id
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#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 <functional>
|
||||
|
||||
@ -19,7 +19,7 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
|
||||
/** 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
|
||||
* 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 >= \
|
||||
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
|
||||
#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 <functional>
|
||||
#include <sstream>
|
||||
#include <ros/serialization.h>
|
||||
#include <rclcpp/serialization.hpp>
|
||||
#include <rosidl_runtime_cpp/traits.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
@ -210,12 +211,12 @@ public:
|
||||
PropertySerializer() { insert(typeid(T), typeName<T>(), &serialize, &deserialize); }
|
||||
|
||||
template <class Q = T>
|
||||
static typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() {
|
||||
return ros::message_traits::DataType<T>::value();
|
||||
static typename std::enable_if<rosidl_generator_traits::is_message<Q>::value, std::string>::type typeName() {
|
||||
return rosidl_generator_traits::data_type<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();
|
||||
}
|
||||
|
||||
|
||||
@ -63,12 +63,12 @@ public:
|
||||
|
||||
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_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,
|
||||
const Eigen::Isometry3d& target, 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;
|
||||
};
|
||||
} // namespace solvers
|
||||
} // namespace task_constructor
|
||||
|
||||
@ -60,12 +60,12 @@ public:
|
||||
|
||||
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
|
||||
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,
|
||||
const Eigen::Isometry3d& target, 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;
|
||||
};
|
||||
} // namespace solvers
|
||||
} // namespace task_constructor
|
||||
|
||||
@ -39,6 +39,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/task_constructor/solvers/planner_interface.h>
|
||||
#include <rclcpp/node.hpp>
|
||||
#include <moveit/macros/class_forward.h>
|
||||
|
||||
namespace planning_pipeline {
|
||||
@ -58,20 +59,25 @@ public:
|
||||
struct Specification
|
||||
{
|
||||
moveit::core::RobotModelConstPtr model;
|
||||
std::string ns{ "move_group" };
|
||||
std::string ns{ "ompl" };
|
||||
std::string pipeline{ "ompl" };
|
||||
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;
|
||||
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);
|
||||
|
||||
@ -81,16 +87,17 @@ public:
|
||||
|
||||
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
|
||||
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,
|
||||
const Eigen::Isometry3d& target, 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;
|
||||
|
||||
protected:
|
||||
std::string pipeline_name_;
|
||||
planning_pipeline::PlanningPipelinePtr planner_;
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
};
|
||||
} // namespace solvers
|
||||
} // namespace task_constructor
|
||||
|
||||
@ -39,7 +39,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit_msgs/Constraints.h>
|
||||
#include <moveit_msgs/msg/constraints.hpp>
|
||||
#include <moveit/task_constructor/properties.h>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
@ -82,13 +82,13 @@ public:
|
||||
virtual 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_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
|
||||
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
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 task_constructor
|
||||
|
||||
@ -43,7 +43,7 @@
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
#include <moveit/task_constructor/cost_queue.h>
|
||||
|
||||
#include <ros/console.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <ostream>
|
||||
#include <chrono>
|
||||
@ -150,7 +150,7 @@ public:
|
||||
void newSolution(const SolutionBasePtr& solution);
|
||||
bool storeFailures() const { return introspection_ != nullptr; }
|
||||
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();
|
||||
try {
|
||||
compute();
|
||||
@ -200,6 +200,7 @@ private:
|
||||
InterfaceWeakPtr next_starts_; // interface to be used for sendForward()
|
||||
|
||||
Introspection* introspection_; // task's introspection instance
|
||||
inline static const rclcpp::Logger LOGGER = rclcpp::get_logger("stage");
|
||||
};
|
||||
PIMPL_FUNCTIONS(Stage)
|
||||
std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
|
||||
|
||||
@ -38,7 +38,7 @@
|
||||
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <moveit/task_constructor/cost_queue.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
namespace moveit {
|
||||
@ -82,7 +82,7 @@ public:
|
||||
void setGroup(const std::string& group) { setProperty("group", group); }
|
||||
|
||||
/// 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);
|
||||
template <typename T>
|
||||
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.
|
||||
* 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 = "");
|
||||
template <typename T>
|
||||
void setTargetPose(const T& p, const std::string& frame = "") {
|
||||
|
||||
@ -41,7 +41,7 @@
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/solvers/planner_interface.h>
|
||||
|
||||
#include <moveit_msgs/Constraints.h>
|
||||
#include <moveit_msgs/msg/constraints.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace core {
|
||||
@ -76,7 +76,7 @@ public:
|
||||
using GroupPlannerVector = std::vector<std::pair<std::string, solvers::PlannerInterfacePtr> >;
|
||||
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));
|
||||
}
|
||||
|
||||
|
||||
@ -40,7 +40,7 @@
|
||||
|
||||
#include <moveit/task_constructor/storage.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>
|
||||
|
||||
namespace moveit {
|
||||
@ -55,7 +55,7 @@ public:
|
||||
void computeForward(const InterfaceState& from) 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); }
|
||||
|
||||
private:
|
||||
|
||||
@ -40,7 +40,7 @@
|
||||
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/cost_queue.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
@ -55,7 +55,7 @@ public:
|
||||
bool canCompute() const override;
|
||||
void compute() override;
|
||||
|
||||
void addPose(const geometry_msgs::PoseStamped& pose);
|
||||
void addPose(const geometry_msgs::msg::PoseStamped& pose);
|
||||
|
||||
protected:
|
||||
void onNewSolution(const SolutionBase& s) override;
|
||||
|
||||
@ -57,9 +57,9 @@ public:
|
||||
void setAngleDelta(double delta) { setProperty("angle_delta", delta); }
|
||||
|
||||
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 moveit_msgs::RobotState& grasp) { properties().set("grasp", grasp); }
|
||||
void setGraspPose(const moveit_msgs::msg::RobotState& grasp) { properties().set("grasp", grasp); }
|
||||
|
||||
protected:
|
||||
void onNewSolution(const SolutionBase& s) override;
|
||||
|
||||
@ -40,7 +40,7 @@
|
||||
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/cost_queue.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
@ -55,7 +55,7 @@ public:
|
||||
bool canCompute() const 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:
|
||||
void onNewSolution(const SolutionBase& s) override;
|
||||
|
||||
@ -41,7 +41,7 @@
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/properties.h>
|
||||
#include <moveit/task_constructor/type_traits.h>
|
||||
#include <moveit_msgs/CollisionObject.h>
|
||||
#include <moveit_msgs/msg/collision_object.hpp>
|
||||
#include <map>
|
||||
|
||||
namespace moveit {
|
||||
@ -78,7 +78,7 @@ public:
|
||||
/// attach or detach a list of objects to the given link
|
||||
void attachObjects(const Names& objects, const std::string& attach_link, bool attach);
|
||||
/// 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
|
||||
void removeObject(const std::string& object_name);
|
||||
|
||||
@ -135,7 +135,7 @@ protected:
|
||||
// list of objects to attach (true) / detach (false) to a given link
|
||||
std::map<std::string, std::pair<Names, bool>> attach_objects_;
|
||||
// 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
|
||||
struct CollisionMatrixPairs
|
||||
@ -150,7 +150,7 @@ protected:
|
||||
protected:
|
||||
// apply stored modifications to scene
|
||||
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,
|
||||
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/solvers/planner_interface.h>
|
||||
#include <moveit_msgs/Constraints.h>
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
#include <geometry_msgs/Vector3Stamped.h>
|
||||
#include <moveit_msgs/msg/constraints.hpp>
|
||||
#include <geometry_msgs/msg/twist_stamped.hpp>
|
||||
#include <geometry_msgs/msg/vector3_stamped.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace core {
|
||||
@ -64,7 +64,7 @@ public:
|
||||
|
||||
void setGroup(const std::string& group) { setProperty("group", group); }
|
||||
/// 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);
|
||||
template <typename T>
|
||||
void setIKFrame(const T& p, const std::string& link) {
|
||||
@ -82,14 +82,14 @@ public:
|
||||
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));
|
||||
}
|
||||
|
||||
/// 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
|
||||
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
|
||||
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/solvers/planner_interface.h>
|
||||
#include <moveit_msgs/Constraints.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/PointStamped.h>
|
||||
#include <moveit_msgs/msg/constraints.hpp>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <geometry_msgs/msg/point_stamped.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace core {
|
||||
@ -63,7 +63,7 @@ public:
|
||||
|
||||
void setGroup(const std::string& group) { setProperty("group", group); }
|
||||
/// 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);
|
||||
template <typename T>
|
||||
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); }
|
||||
|
||||
/// 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
|
||||
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
|
||||
void setGoal(const std::string& named_joint_pose) { setProperty("goal", named_joint_pose); }
|
||||
|
||||
/// 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
|
||||
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));
|
||||
}
|
||||
|
||||
|
||||
@ -37,7 +37,7 @@
|
||||
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <moveit/task_constructor/cost_queue.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
namespace moveit {
|
||||
|
||||
@ -38,7 +38,7 @@
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
#include <geometry_msgs/msg/twist_stamped.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
@ -82,9 +82,9 @@ public:
|
||||
|
||||
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);
|
||||
};
|
||||
|
||||
@ -95,11 +95,11 @@ public:
|
||||
Pick(Stage::pointer&& grasp_stage = Stage::pointer(), const std::string& name = "pick")
|
||||
: 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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
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")
|
||||
: 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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
void setPlaceMotion(const std::map<std::string, double>& joints) { setLiftPlace(joints); }
|
||||
|
||||
@ -38,7 +38,7 @@
|
||||
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
namespace moveit {
|
||||
@ -78,7 +78,7 @@ public:
|
||||
void setObject(const std::string& object) { setProperty("object", object); }
|
||||
|
||||
/// 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);
|
||||
template <typename T>
|
||||
void setIKFrame(const T& t, const std::string& link) {
|
||||
|
||||
@ -42,14 +42,15 @@
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit/task_constructor/properties.h>
|
||||
#include <moveit/task_constructor/cost_queue.h>
|
||||
#include <moveit_task_constructor_msgs/Solution.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#include <moveit_task_constructor_msgs/msg/solution.hpp>
|
||||
#include <visualization_msgs/msg/marker_array.hpp>
|
||||
|
||||
#include <list>
|
||||
#include <vector>
|
||||
#include <deque>
|
||||
#include <cassert>
|
||||
#include <functional>
|
||||
#include <cmath>
|
||||
|
||||
namespace planning_scene {
|
||||
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||
@ -271,9 +272,9 @@ public:
|
||||
const auto& markers() const { return markers_; }
|
||||
|
||||
/// 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;
|
||||
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
|
||||
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
|
||||
std::string comment_;
|
||||
// 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
|
||||
const InterfaceState* start_ = nullptr;
|
||||
@ -313,7 +314,8 @@ public:
|
||||
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
|
||||
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;
|
||||
|
||||
@ -346,7 +348,7 @@ public:
|
||||
void push_back(const SolutionBase& 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;
|
||||
|
||||
@ -377,7 +379,7 @@ public:
|
||||
: SolutionBase(creator, cost), wrapped_(wrapped) {}
|
||||
explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped)
|
||||
: 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;
|
||||
|
||||
double computeCost(const CostTerm& cost, std::string& comment) const override;
|
||||
|
||||
@ -42,11 +42,13 @@
|
||||
#include "container.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_msgs/MoveItErrorCodes.h>
|
||||
#include <moveit_msgs/msg/move_it_error_codes.hpp>
|
||||
|
||||
#include <rclcpp/node.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace core {
|
||||
@ -86,7 +88,7 @@ public:
|
||||
/// setting the robot model also resets the task
|
||||
void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model);
|
||||
/// 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 insert(Stage::pointer&& stage, int before = -1) override;
|
||||
@ -121,7 +123,7 @@ public:
|
||||
/// interrupt current planning (or execution)
|
||||
void preempt();
|
||||
/// 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
|
||||
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"
|
||||
type="moveit::task_constructor::stages::CurrentState"
|
||||
base_class_type="moveit::task_constructor::Stage">
|
||||
|
||||
@ -7,13 +7,11 @@
|
||||
<maintainer email="me@v4hn.de">Michael Goerner</maintainer>
|
||||
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>roscpp</build_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>tf2_eigen</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>moveit_core</depend>
|
||||
<depend>moveit_ros_planning</depend>
|
||||
<depend>moveit_ros_planning_interface</depend>
|
||||
@ -21,11 +19,16 @@
|
||||
<depend>visualization_msgs</depend>
|
||||
<depend>rviz_marker_tools</depend>
|
||||
|
||||
<test_depend>rosunit</test_depend>
|
||||
<test_depend>rostest</test_depend>
|
||||
<test_depend>moveit_resources_fanuc_moveit_config</test_depend>
|
||||
<test_depend>ament_cmake_gmock</test_depend>
|
||||
<test_depend>ament_cmake_gtest</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>
|
||||
<moveit_task_constructor_core plugin="${prefix}/motion_planning_stages_plugin_description.xml"/>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
add_library(${PROJECT_NAME}
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
${PROJECT_INCLUDE}/container.h
|
||||
${PROJECT_INCLUDE}/container_p.h
|
||||
${PROJECT_INCLUDE}/cost_queue.h
|
||||
@ -35,16 +35,24 @@ add_library(${PROJECT_NAME}
|
||||
solvers/pipeline_planner.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}
|
||||
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
|
||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
||||
PUBLIC ${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_subdirectory(stages)
|
||||
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
||||
EXPORT ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib)
|
||||
|
||||
@ -39,7 +39,7 @@
|
||||
#include <moveit/task_constructor/merge.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <ros/console.h>
|
||||
#include <rclcpp/logging.hpp>
|
||||
|
||||
#include <memory>
|
||||
#include <iostream>
|
||||
@ -107,7 +107,7 @@ void ContainerBasePrivate::compute() {
|
||||
}
|
||||
|
||||
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()) {
|
||||
case GENERATE:
|
||||
// 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;
|
||||
|
||||
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);
|
||||
break;
|
||||
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);
|
||||
break;
|
||||
|
||||
@ -127,11 +127,11 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState
|
||||
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
|
||||
auto cimpl = conn->pimpl();
|
||||
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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
@ -446,7 +446,8 @@ inline void updateStatePrios(const SolutionSequence::container_type& partial_sol
|
||||
}
|
||||
|
||||
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() << "'");
|
||||
|
||||
// failures should never trigger this callback
|
||||
@ -907,7 +908,7 @@ void Merger::onNewSolution(const SolutionBase& s) {
|
||||
void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) {
|
||||
const SubTrajectory* trajectory = dynamic_cast<const SubTrajectory*>(&s);
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
@ -113,7 +113,7 @@ double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/)
|
||||
if (traj == nullptr || traj->getWayPointCount() == 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());
|
||||
const auto& first_waypoint = traj->getWayPoint(0);
|
||||
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) {
|
||||
collision_detection::DistanceResult result;
|
||||
if (with_world)
|
||||
#if MOVEIT_HAS_COLLISION_ENV
|
||||
state->scene()->getCollisionEnv()->distanceRobot(request, result, robot);
|
||||
#else
|
||||
state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(),
|
||||
robot);
|
||||
#endif
|
||||
else
|
||||
#if MOVEIT_HAS_COLLISION_ENV
|
||||
state->scene()->getCollisionEnv()->distanceSelf(request, result, robot);
|
||||
#else
|
||||
state->scene()->getCollisionRobot()->distanceSelf(request, result, robot);
|
||||
#endif
|
||||
|
||||
if (result.minimum_distance.distance <= 0) {
|
||||
return result.minimum_distance;
|
||||
|
||||
@ -39,11 +39,11 @@
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
#include <moveit/task_constructor/task.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 <ros/publisher.h>
|
||||
#include <ros/service.h>
|
||||
#include <rclcpp/node.hpp>
|
||||
#include <rclcpp/publisher.hpp>
|
||||
#include <rclcpp/service.hpp>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <sstream>
|
||||
@ -64,34 +64,73 @@ std::string getTaskId(const TaskPrivate* task) {
|
||||
}
|
||||
} // 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
|
||||
{
|
||||
public:
|
||||
IntrospectionPrivate(const TaskPrivate* task, Introspection* self)
|
||||
: nh_(std::string("~/") + task->ns()) // topics + services are advertised in private namespace
|
||||
, task_(task)
|
||||
, task_id_(getTaskId(task)) {
|
||||
task_description_publisher_ =
|
||||
nh_.advertise<moveit_task_constructor_msgs::TaskDescription>(DESCRIPTION_TOPIC, 2, true);
|
||||
IntrospectionPrivate(const TaskPrivate* task, Introspection* self) : task_(task), task_id_(getTaskId(task)) {
|
||||
rclcpp::NodeOptions options;
|
||||
options.arguments({ "--ros-args", "-r", "__node:=introspection_" + task_id_ });
|
||||
node_ = rclcpp::Node::make_shared("_", task_->ns(), options);
|
||||
executor_.add_node(node_);
|
||||
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
|
||||
indicateReset();
|
||||
|
||||
task_statistics_publisher_ =
|
||||
nh_.advertise<moveit_task_constructor_msgs::TaskStatistics>(STATISTICS_TOPIC, 1, true);
|
||||
solution_publisher_ = nh_.advertise<moveit_task_constructor_msgs::Solution>(SOLUTION_TOPIC, 1, true);
|
||||
|
||||
get_solution_service_ =
|
||||
nh_.advertiseService(std::string(GET_SOLUTION_SERVICE "_") + task_id_, &Introspection::getSolution, self);
|
||||
task_statistics_publisher_ = node_->create_publisher<moveit_task_constructor_msgs::msg::TaskStatistics>(
|
||||
STATISTICS_TOPIC, rclcpp::QoS(1).transient_local());
|
||||
solution_publisher_ = node_->create_publisher<moveit_task_constructor_msgs::msg::Solution>(
|
||||
SOLUTION_TOPIC, rclcpp::QoS(1).transient_local());
|
||||
|
||||
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();
|
||||
}
|
||||
~IntrospectionPrivate() { indicateReset(); }
|
||||
~IntrospectionPrivate() {
|
||||
indicateReset();
|
||||
executor_.remove_node(node_);
|
||||
}
|
||||
|
||||
void indicateReset() {
|
||||
// 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_;
|
||||
task_description_publisher_.publish(msg);
|
||||
task_description_publisher_->publish(msg);
|
||||
}
|
||||
|
||||
void resetMaps() {
|
||||
@ -102,21 +141,22 @@ public:
|
||||
id_solution_bimap_.clear();
|
||||
}
|
||||
|
||||
ros::NodeHandle nh_;
|
||||
/// associated task
|
||||
const TaskPrivate* task_;
|
||||
const std::string task_id_;
|
||||
|
||||
/// publish task detailed description and current state
|
||||
ros::Publisher task_description_publisher_;
|
||||
ros::Publisher task_statistics_publisher_;
|
||||
rclcpp::Publisher<moveit_task_constructor_msgs::msg::TaskDescription>::SharedPtr task_description_publisher_;
|
||||
rclcpp::Publisher<moveit_task_constructor_msgs::msg::TaskStatistics>::SharedPtr task_statistics_publisher_;
|
||||
/// publish new solutions
|
||||
ros::Publisher solution_publisher_;
|
||||
rclcpp::Publisher<moveit_task_constructor_msgs::msg::Solution>::SharedPtr solution_publisher_;
|
||||
/// 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
|
||||
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_;
|
||||
};
|
||||
|
||||
@ -127,13 +167,13 @@ Introspection::~Introspection() {
|
||||
}
|
||||
|
||||
void Introspection::publishTaskDescription() {
|
||||
::moveit_task_constructor_msgs::TaskDescription msg;
|
||||
impl->task_description_publisher_.publish(fillTaskDescription(msg));
|
||||
::moveit_task_constructor_msgs::msg::TaskDescription msg;
|
||||
impl->task_description_publisher_->publish(fillTaskDescription(msg));
|
||||
}
|
||||
|
||||
void Introspection::publishTaskState() {
|
||||
::moveit_task_constructor_msgs::TaskStatistics msg;
|
||||
impl->task_statistics_publisher_.publish(fillTaskStatistics(msg));
|
||||
::moveit_task_constructor_msgs::msg::TaskStatistics msg;
|
||||
impl->task_statistics_publisher_->publish(fillTaskStatistics(msg));
|
||||
}
|
||||
|
||||
void Introspection::reset() {
|
||||
@ -145,7 +185,7 @@ void Introspection::registerSolution(const SolutionBase& 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.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) {
|
||||
moveit_task_constructor_msgs::Solution msg;
|
||||
moveit_task_constructor_msgs::msg::Solution msg;
|
||||
fillSolution(msg, s);
|
||||
impl->solution_publisher_.publish(msg);
|
||||
impl->solution_publisher_->publish(msg);
|
||||
}
|
||||
|
||||
void Introspection::publishAllSolutions(bool wait) {
|
||||
@ -178,13 +218,13 @@ const SolutionBase* Introspection::solutionFromId(uint id) const {
|
||||
return it->second;
|
||||
}
|
||||
|
||||
bool Introspection::getSolution(moveit_task_constructor_msgs::GetSolution::Request& req,
|
||||
moveit_task_constructor_msgs::GetSolution::Response& res) {
|
||||
const SolutionBase* solution = solutionFromId(req.solution_id);
|
||||
bool Introspection::getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr req,
|
||||
const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr res) {
|
||||
const SolutionBase* solution = solutionFromId(req->solution_id);
|
||||
if (!solution)
|
||||
return false;
|
||||
|
||||
fillSolution(res.solution, *solution);
|
||||
fillSolution(res->solution, *solution);
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -203,7 +243,7 @@ uint32_t Introspection::solutionId(const SolutionBase& s) {
|
||||
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
|
||||
for (const auto& solution : stage.solutions())
|
||||
s.solved.push_back(solutionId(*solution));
|
||||
@ -216,18 +256,18 @@ void Introspection::fillStageStatistics(const Stage& stage, moveit_task_construc
|
||||
s.num_failed = stage.numFailures();
|
||||
}
|
||||
|
||||
moveit_task_constructor_msgs::TaskDescription&
|
||||
Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription& msg) {
|
||||
moveit_task_constructor_msgs::msg::TaskDescription&
|
||||
Introspection::fillTaskDescription(moveit_task_constructor_msgs::msg::TaskDescription& msg) {
|
||||
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
|
||||
moveit_task_constructor_msgs::StageDescription desc;
|
||||
moveit_task_constructor_msgs::msg::StageDescription desc;
|
||||
desc.id = stageId(&stage);
|
||||
desc.name = stage.name();
|
||||
desc.flags = stage.pimpl()->interfaceFlags();
|
||||
|
||||
// fill 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.description = pair.second.description();
|
||||
p.type = pair.second.typeName();
|
||||
@ -251,11 +291,11 @@ Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription
|
||||
return msg;
|
||||
}
|
||||
|
||||
moveit_task_constructor_msgs::TaskStatistics&
|
||||
Introspection::fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics& msg) {
|
||||
moveit_task_constructor_msgs::msg::TaskStatistics&
|
||||
Introspection::fillTaskStatistics(moveit_task_constructor_msgs::msg::TaskStatistics& msg) {
|
||||
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
|
||||
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);
|
||||
fillStageStatistics(stage, stat);
|
||||
|
||||
|
||||
@ -1,7 +1,6 @@
|
||||
#include <moveit/task_constructor/marker_tools.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_state/robot_state.h>
|
||||
#include <eigen_conversions/eigen_msg.h>
|
||||
|
||||
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()
|
||||
: &link_names;
|
||||
for (const auto &name : *names) {
|
||||
visualization_msgs::MarkerArray markers;
|
||||
visualization_msgs::msg::MarkerArray markers;
|
||||
robot_state.getRobotMarkers(markers, {name}, false);
|
||||
for (auto &marker : markers.markers)
|
||||
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,
|
||||
const urdf::Pose& pose, const urdf::Color& color) {
|
||||
visualization_msgs::msg::Marker& createGeometryMarker(visualization_msgs::msg::Marker& marker,
|
||||
const urdf::Geometry& geom, const urdf::Pose& pose,
|
||||
const urdf::Color& color) {
|
||||
rviz_marker_tools::makeFromGeometry(marker, geom);
|
||||
marker.pose.position.x = pose.position.x;
|
||||
marker.pose.position.y = pose.position.y;
|
||||
@ -110,7 +110,7 @@ void generateMarkers(const moveit::core::RobotState& robot_state, const MarkerCa
|
||||
if (!model)
|
||||
return;
|
||||
|
||||
visualization_msgs::Marker m;
|
||||
visualization_msgs::msg::Marker m;
|
||||
m.header.frame_id = robot_state.getRobotModel()->getModelFrame();
|
||||
|
||||
// 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
|
||||
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)
|
||||
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;
|
||||
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) {
|
||||
bool finished = true;
|
||||
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
|
||||
|
||||
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);
|
||||
merged_state->setJointGroupPositions(sub->getGroup(), values);
|
||||
}
|
||||
@ -162,7 +162,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
|
||||
// add waypoint without timing
|
||||
merged_traj->addSuffixWayPoint(merged_state, 0.0);
|
||||
// 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
|
||||
|
||||
@ -39,12 +39,12 @@
|
||||
#include <moveit/task_constructor/properties.h>
|
||||
#include <boost/format.hpp>
|
||||
#include <functional>
|
||||
#include <ros/console.h>
|
||||
#include <rclcpp/logging.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
|
||||
const std::string LOGNAME = "Properties";
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("Properties");
|
||||
|
||||
class PropertyTypeRegistry
|
||||
{
|
||||
@ -73,7 +73,7 @@ public:
|
||||
const Entry& entry(const std::type_index& type_index) const {
|
||||
auto it = types_.find(type_index);
|
||||
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 it->second;
|
||||
@ -289,7 +289,7 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa
|
||||
} 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));
|
||||
p.setCurrentValue(value);
|
||||
p.initialized_from_ = source;
|
||||
|
||||
@ -41,14 +41,14 @@
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
|
||||
#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
|
||||
#include <moveit/robot_state/cartesian_interpolator.h>
|
||||
#endif
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace solvers {
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("CartesianPath");
|
||||
|
||||
CartesianPath::CartesianPath() {
|
||||
auto& p = properties();
|
||||
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,
|
||||
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
|
||||
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();
|
||||
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;
|
||||
}
|
||||
|
||||
@ -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,
|
||||
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints) {
|
||||
const moveit_msgs::msg::Constraints& path_constraints) {
|
||||
const auto& props = properties();
|
||||
planning_scene::PlanningScenePtr sandbox_scene = from->diff();
|
||||
|
||||
@ -86,21 +86,15 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
|
||||
const double* joint_positions) {
|
||||
state->setJointGroupPositions(jmg, joint_positions);
|
||||
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;
|
||||
};
|
||||
|
||||
std::vector<moveit::core::RobotStatePtr> trajectory;
|
||||
#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
|
||||
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
|
||||
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
|
||||
moveit::core::MaxEEFStep(props.get<double>("step_size")),
|
||||
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
|
||||
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);
|
||||
|
||||
@ -46,6 +46,8 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace solvers {
|
||||
|
||||
static const auto LOGGER = rclcpp::get_logger("JointInterpolationPlanner");
|
||||
|
||||
JointInterpolationPlanner::JointInterpolationPlanner() {
|
||||
auto& p = properties();
|
||||
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 moveit::core::JointModelGroup* jmg, double /*timeout*/,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& /*path_constraints*/) {
|
||||
const moveit_msgs::msg::Constraints& /*path_constraints*/) {
|
||||
const auto& props = properties();
|
||||
|
||||
// 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::JointModelGroup* jmg, double timeout,
|
||||
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();
|
||||
|
||||
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)) {
|
||||
// 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
|
||||
ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target");
|
||||
RCLCPP_WARN(LOGGER, "IK failed for pose target");
|
||||
return false;
|
||||
}
|
||||
to->getCurrentStateNonConst().update();
|
||||
|
||||
@ -40,9 +40,14 @@
|
||||
#include <moveit/task_constructor/task.h>
|
||||
#include <moveit/planning_scene/planning_scene.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>
|
||||
|
||||
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
|
||||
#include <tf2_eigen/tf2_eigen.hpp>
|
||||
#else
|
||||
#include <tf2_eigen/tf2_eigen.h>
|
||||
#endif
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
@ -52,10 +57,10 @@ struct PlannerCache
|
||||
{
|
||||
using PlannerID = std::tuple<std::string, std::string>;
|
||||
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_;
|
||||
|
||||
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
|
||||
ModelList::iterator model_it = cache_.begin();
|
||||
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 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
|
||||
if (!ros::NodeHandle(pipeline_ns).hasParam(PLUGIN_PARAMETER_NAME)) {
|
||||
ROS_WARN("Failed to find '%s/%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME,
|
||||
if (!node->has_parameter(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.");
|
||||
pipeline_ns = spec.ns;
|
||||
pipeline_ns = "move_group";
|
||||
}
|
||||
|
||||
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();
|
||||
if (!planner) {
|
||||
// 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);
|
||||
// store in cache
|
||||
entry = planner;
|
||||
@ -101,12 +111,13 @@ planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const PipelinePla
|
||||
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();
|
||||
p.declare<std::string>("planner", "", "planner id");
|
||||
|
||||
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?");
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline) : PipelinePlanner() {
|
||||
PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline)
|
||||
: PipelinePlanner(rclcpp::Node::SharedPtr()) {
|
||||
planner_ = planning_pipeline;
|
||||
}
|
||||
|
||||
@ -129,7 +141,7 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
|
||||
Specification spec;
|
||||
spec.model = robot_model;
|
||||
spec.pipeline = pipeline_name_;
|
||||
planner_ = create(spec);
|
||||
planner_ = create(node_, spec);
|
||||
} else if (robot_model != planner_->getRobotModel()) {
|
||||
throw std::runtime_error(
|
||||
"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"));
|
||||
}
|
||||
|
||||
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) {
|
||||
req.group_name = jmg->getName();
|
||||
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.max_velocity_scaling_factor = p.get<double>("max_velocity_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,
|
||||
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints) {
|
||||
const moveit_msgs::msg::Constraints& path_constraints) {
|
||||
const auto& props = properties();
|
||||
moveit_msgs::MotionPlanRequest req;
|
||||
moveit_msgs::msg::MotionPlanRequest req;
|
||||
initMotionPlanRequest(req, props, jmg, timeout);
|
||||
|
||||
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,
|
||||
const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints) {
|
||||
const moveit_msgs::msg::Constraints& path_constraints) {
|
||||
const auto& props = properties();
|
||||
moveit_msgs::MotionPlanRequest req;
|
||||
moveit_msgs::msg::MotionPlanRequest req;
|
||||
initMotionPlanRequest(req, props, jmg, timeout);
|
||||
|
||||
geometry_msgs::PoseStamped target;
|
||||
geometry_msgs::msg::PoseStamped target;
|
||||
target.header.frame_id = from->getPlanningFrame();
|
||||
target.pose = tf2::toMsg(target_eigen);
|
||||
|
||||
|
||||
@ -42,7 +42,7 @@
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <ros/console.h>
|
||||
#include <rclcpp/logging.hpp>
|
||||
|
||||
#include <boost/format.hpp>
|
||||
|
||||
@ -87,7 +87,7 @@ void InitStageException::append(InitStageException& other) {
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@ -325,7 +325,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) {
|
||||
impl->properties_.reset();
|
||||
if (impl->parent()) {
|
||||
try {
|
||||
ROS_DEBUG_STREAM_NAMED("Properties", "init '" << name() << "'");
|
||||
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Properties"), "init '" << name() << "'");
|
||||
impl->properties_.performInitFrom(PARENT, impl->parent()->properties());
|
||||
} catch (const Property::error& e) {
|
||||
std::ostringstream oss;
|
||||
@ -795,6 +795,7 @@ std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const {
|
||||
os << reset;
|
||||
return os;
|
||||
}
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("Connecting");
|
||||
|
||||
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();
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@ -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::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_name);
|
||||
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;
|
||||
}
|
||||
|
||||
#if MOVEIT_HAS_OBJECT_POSE
|
||||
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
|
||||
}
|
||||
#endif
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
@ -839,7 +840,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
|
||||
to_it = to_object->shape_poses_.cbegin();
|
||||
from_it != from_end; ++from_it, ++to_it)
|
||||
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
|
||||
}
|
||||
}
|
||||
@ -850,7 +851,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
|
||||
from->getCurrentState().getAttachedBodies(from_attached);
|
||||
to->getCurrentState().getAttachedBodies(to_attached);
|
||||
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;
|
||||
}
|
||||
|
||||
@ -860,18 +861,18 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
|
||||
return object->getName() == from_object->getName();
|
||||
});
|
||||
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;
|
||||
}
|
||||
const moveit::core::AttachedBody* to_object = *it;
|
||||
if (from_object->getAttachedLink() != to_object->getAttachedLink()) {
|
||||
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different attach links: " << from_object->getName()
|
||||
<< " attached to " << from_object->getAttachedLinkName() << " / "
|
||||
RCLCPP_DEBUG_STREAM(LOGGER, name() << ": different attach links: " << from_object->getName() << " attached to "
|
||||
<< from_object->getAttachedLinkName() << " / "
|
||||
<< to_object->getAttachedLinkName());
|
||||
return false; // links not matching
|
||||
}
|
||||
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
|
||||
}
|
||||
|
||||
@ -886,8 +887,8 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
|
||||
#endif
|
||||
for (; from_it != from_end; ++from_it, ++to_it)
|
||||
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
|
||||
ROS_DEBUG_STREAM_NAMED("Connecting",
|
||||
name() << ": different pose of attached object shape: " << from_object->getName());
|
||||
RCLCPP_DEBUG_STREAM(LOGGER, name()
|
||||
<< ": different pose of attached object shape: " << from_object->getName());
|
||||
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/fix_collision_objects.h
|
||||
|
||||
@ -39,13 +39,24 @@ add_library(${PROJECT_NAME}_stages
|
||||
simple_grasp.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
|
||||
)
|
||||
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
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
||||
EXPORT ${PROJECT_NAME}Targets
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib)
|
||||
|
||||
@ -44,16 +44,22 @@
|
||||
#include <moveit/robot_state/robot_state.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>
|
||||
#endif
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <iterator>
|
||||
#include <ros/console.h>
|
||||
#include <rclcpp/logging.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
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)) {
|
||||
auto& p = properties();
|
||||
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");
|
||||
|
||||
// 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::PoseStamped>("target_pose", "goal pose for ik frame");
|
||||
p.declare<geometry_msgs::msg::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
|
||||
p.declare<geometry_msgs::msg::PoseStamped>("target_pose", "goal pose for ik frame");
|
||||
}
|
||||
|
||||
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.pose = tf2::toMsg(pose);
|
||||
setIKFrame(pose_msg);
|
||||
}
|
||||
|
||||
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.pose = tf2::toMsg(pose);
|
||||
setTargetPose(pose_msg);
|
||||
@ -91,11 +97,11 @@ namespace {
|
||||
// ??? 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
|
||||
bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene,
|
||||
robot_state::RobotState& robot_state, Eigen::Isometry3d pose,
|
||||
const robot_model::LinkModel* link,
|
||||
moveit::core::RobotState& robot_state, Eigen::Isometry3d pose,
|
||||
const moveit::core::LinkModel* link,
|
||||
collision_detection::CollisionResult* collision_result = nullptr) {
|
||||
// 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
|
||||
pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent);
|
||||
|
||||
@ -109,10 +115,10 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce
|
||||
while (parent) {
|
||||
pending_links.push_back(&parent->getName());
|
||||
link = parent;
|
||||
const robot_model::JointModel* joint = link->getParentJointModel();
|
||||
const moveit::core::JointModel* joint = link->getParentJointModel();
|
||||
parent = joint->getParentLinkModel();
|
||||
|
||||
if (joint->getType() != robot_model::JointModel::FIXED) {
|
||||
if (joint->getType() != moveit::core::JointModel::FIXED) {
|
||||
for (const std::string* name : pending_links)
|
||||
acm.setDefaultEntry(*name, true);
|
||||
pending_links.clear();
|
||||
@ -240,21 +246,21 @@ void ComputeIK::compute() {
|
||||
std::string msg;
|
||||
|
||||
if (!validateEEF(props, robot_model, eef_jmg, &msg)) {
|
||||
ROS_WARN_STREAM_NAMED("ComputeIK", msg);
|
||||
RCLCPP_WARN_STREAM(LOGGER, msg);
|
||||
return;
|
||||
}
|
||||
if (!validateGroup(props, robot_model, eef_jmg, jmg, &msg)) {
|
||||
ROS_WARN_STREAM_NAMED("ComputeIK", msg);
|
||||
RCLCPP_WARN_STREAM(LOGGER, msg);
|
||||
return;
|
||||
}
|
||||
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;
|
||||
}
|
||||
properties().property("timeout").setDefaultValue(jmg->getDefaultIKTimeout());
|
||||
|
||||
// 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
|
||||
target_pose_msg.header.frame_id = scene->getPlanningFrame();
|
||||
|
||||
@ -262,8 +268,7 @@ void ComputeIK::compute() {
|
||||
tf2::fromMsg(target_pose_msg.pose, target_pose);
|
||||
if (target_pose_msg.header.frame_id != scene->getPlanningFrame()) {
|
||||
if (!scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
|
||||
ROS_WARN_STREAM_NAMED("ComputeIK",
|
||||
"Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
|
||||
RCLCPP_WARN_STREAM(LOGGER, "Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
|
||||
return;
|
||||
}
|
||||
// transform target_pose w.r.t. planning frame
|
||||
@ -271,25 +276,25 @@ void ComputeIK::compute() {
|
||||
}
|
||||
|
||||
// determine IK link from ik_frame
|
||||
const robot_model::LinkModel* link = nullptr;
|
||||
geometry_msgs::PoseStamped ik_pose_msg;
|
||||
const moveit::core::LinkModel* link = nullptr;
|
||||
geometry_msgs::msg::PoseStamped ik_pose_msg;
|
||||
const boost::any& value = props.get("ik_frame");
|
||||
if (value.empty()) { // property undefined
|
||||
// determine IK link from eef/group
|
||||
if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) :
|
||||
jmg->getOnlyOneEndEffectorTip())) {
|
||||
ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link");
|
||||
RCLCPP_WARN_STREAM(LOGGER, "Failed to derive IK target link");
|
||||
return;
|
||||
}
|
||||
ik_pose_msg.header.frame_id = link->getName();
|
||||
ik_pose_msg.pose.orientation.w = 1.0;
|
||||
} 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;
|
||||
tf2::fromMsg(ik_pose_msg.pose, ik_pose);
|
||||
|
||||
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;
|
||||
}
|
||||
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
|
||||
collision_detection::CollisionResult collisions;
|
||||
robot_state::RobotState sandbox_state{ scene->getCurrentState() };
|
||||
moveit::core::RobotState sandbox_state{ scene->getCurrentState() };
|
||||
bool colliding =
|
||||
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions);
|
||||
|
||||
// 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
|
||||
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");
|
||||
// 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.color.a *= 0.5;
|
||||
failure_markers.push_back(marker);
|
||||
@ -338,7 +343,7 @@ void ComputeIK::compute() {
|
||||
std::vector<double> compare_pose;
|
||||
const std::string& compare_pose_name = props.get<std::string>("default_pose");
|
||||
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.copyJointGroupPositions(jmg, compare_pose);
|
||||
} else
|
||||
@ -348,7 +353,7 @@ void ComputeIK::compute() {
|
||||
|
||||
IKSolutions ik_solutions;
|
||||
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) {
|
||||
for (const auto& sol : ik_solutions) {
|
||||
if (jmg->distance(joint_positions, sol.data()) < min_solution_distance)
|
||||
@ -396,7 +401,7 @@ void ComputeIK::compute() {
|
||||
solution.markAsFailure();
|
||||
|
||||
// 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.update();
|
||||
|
||||
|
||||
@ -46,13 +46,15 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace stages {
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("Connect");
|
||||
|
||||
Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) {
|
||||
setTimeout(1.0);
|
||||
setCostTerm(std::make_unique<cost::PathLength>());
|
||||
|
||||
auto& p = properties();
|
||||
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");
|
||||
}
|
||||
|
||||
@ -88,7 +90,7 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) {
|
||||
try {
|
||||
merged_jmg_.reset(task_constructor::merge(groups));
|
||||
} 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_to(to.getJointPositions(jm), num);
|
||||
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() << "]");
|
||||
return false;
|
||||
}
|
||||
@ -131,7 +133,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
const auto& props = properties();
|
||||
double timeout = this->timeout();
|
||||
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();
|
||||
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();
|
||||
const moveit::core::JointModelGroup* jmg = final_goal_state.getJointModelGroup(pair.first);
|
||||
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.update();
|
||||
intermediate_scenes.push_back(end);
|
||||
@ -225,7 +227,7 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
|
||||
|
||||
// check merged trajectory for collisions
|
||||
if (!intermediate_scenes.front()->isPathValid(*trajectory,
|
||||
properties().get<moveit_msgs::Constraints>("path_constraints")))
|
||||
properties().get<moveit_msgs::msg::Constraints>("path_constraints")))
|
||||
return SubTrajectoryPtr();
|
||||
|
||||
return std::make_shared<SubTrajectory>(trajectory);
|
||||
|
||||
@ -37,16 +37,18 @@
|
||||
|
||||
#include <moveit/task_constructor/stages/current_state.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
#include <moveit_msgs/GetPlanningScene.h>
|
||||
#include <moveit_msgs/PlanningSceneComponents.h>
|
||||
#include <moveit_msgs/srv/get_planning_scene.hpp>
|
||||
#include <moveit_msgs/msg/planning_scene_components.hpp>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_model_loader/robot_model_loader.h>
|
||||
#include <ros/ros.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace stages {
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("CurrentState");
|
||||
|
||||
CurrentState::CurrentState(const std::string& name) : Generator(name) {
|
||||
auto& p = properties();
|
||||
Property& timeout = p.property("timeout");
|
||||
@ -67,31 +69,37 @@ bool CurrentState::canCompute() const {
|
||||
void CurrentState::compute() {
|
||||
scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
|
||||
|
||||
ros::NodeHandle h;
|
||||
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
||||
// Add random ID to prevent warnings about multiple publishers within the same node
|
||||
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());
|
||||
if (client.waitForExistence(timeout)) {
|
||||
moveit_msgs::GetPlanningScene::Request req;
|
||||
moveit_msgs::GetPlanningScene::Response res;
|
||||
auto timeout = std::chrono::duration<double>(this->timeout());
|
||||
if (client->wait_for_service(timeout)) {
|
||||
auto req = std::make_shared<moveit_msgs::srv::GetPlanningScene::Request>();
|
||||
|
||||
req.components.components =
|
||||
moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS | moveit_msgs::PlanningSceneComponents::ROBOT_STATE |
|
||||
moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS |
|
||||
moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES |
|
||||
moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY | moveit_msgs::PlanningSceneComponents::OCTOMAP |
|
||||
moveit_msgs::PlanningSceneComponents::TRANSFORMS |
|
||||
moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX |
|
||||
moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING |
|
||||
moveit_msgs::PlanningSceneComponents::OBJECT_COLORS;
|
||||
req->components.components = moveit_msgs::msg::PlanningSceneComponents::SCENE_SETTINGS |
|
||||
moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE |
|
||||
moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS |
|
||||
moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_NAMES |
|
||||
moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY |
|
||||
moveit_msgs::msg::PlanningSceneComponents::OCTOMAP |
|
||||
moveit_msgs::msg::PlanningSceneComponents::TRANSFORMS |
|
||||
moveit_msgs::msg::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX |
|
||||
moveit_msgs::msg::PlanningSceneComponents::LINK_PADDING_AND_SCALING |
|
||||
moveit_msgs::msg::PlanningSceneComponents::OBJECT_COLORS;
|
||||
|
||||
if (client.call(req, res)) {
|
||||
scene_->setPlanningSceneMsg(res.scene);
|
||||
auto res_future = client->async_send_request(req);
|
||||
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);
|
||||
return;
|
||||
}
|
||||
}
|
||||
ROS_WARN("failed to acquire current PlanningScene");
|
||||
RCLCPP_WARN(LOGGER, "failed to acquire current PlanningScene");
|
||||
}
|
||||
} // namespace stages
|
||||
} // namespace task_constructor
|
||||
|
||||
@ -44,9 +44,13 @@
|
||||
#include <moveit/task_constructor/cost_terms.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 <ros/console.h>
|
||||
#endif
|
||||
#include <Eigen/Geometry>
|
||||
#include <rclcpp/logging.hpp>
|
||||
|
||||
namespace vm = visualization_msgs;
|
||||
namespace cd = collision_detection;
|
||||
@ -55,13 +59,15 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace stages {
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("FixCollisionObjects");
|
||||
|
||||
FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) {
|
||||
// TODO: possibly weight solutions based on the required displacement?
|
||||
setCostTerm(std::make_unique<cost::Constant>(0.0));
|
||||
|
||||
auto& p = properties();
|
||||
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) {
|
||||
@ -79,8 +85,7 @@ bool computeCorrection(const std::vector<cd::Contact>& contacts, Eigen::Vector3d
|
||||
correction.setZero();
|
||||
for (const cd::Contact& c : contacts) {
|
||||
if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT && c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) {
|
||||
ROS_WARN_STREAM_NAMED("FixCollisionObjects",
|
||||
"Cannot fix collision between " << c.body_name_1 << " and " << c.body_name_2);
|
||||
RCLCPP_WARN_STREAM(LOGGER, "Cannot fix collision between " << c.body_name_1 << " and " << c.body_name_2);
|
||||
return false;
|
||||
}
|
||||
if (c.body_type_1 == cd::BodyTypes::WORLD_OBJECT)
|
||||
@ -110,20 +115,15 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene&
|
||||
req.verbose = false;
|
||||
req.distance = false;
|
||||
|
||||
vm::Marker m;
|
||||
vm::msg::Marker m;
|
||||
m.header.frame_id = scene.getPlanningFrame();
|
||||
m.ns = "collisions";
|
||||
|
||||
bool failure = false;
|
||||
while (!failure) {
|
||||
res.clear();
|
||||
#if MOVEIT_HAS_COLLISION_ENV
|
||||
scene.getCollisionEnv()->checkRobotCollision(req, res, scene.getCurrentState(),
|
||||
scene.getAllowedCollisionMatrix());
|
||||
#else
|
||||
scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(),
|
||||
scene.getCurrentState(), scene.getAllowedCollisionMatrix());
|
||||
#endif
|
||||
if (!res.collision)
|
||||
return result;
|
||||
|
||||
@ -147,7 +147,7 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene&
|
||||
|
||||
// fix collision by shifting object along 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;
|
||||
scene.getWorldNonConst()->moveObject(name, Eigen::Isometry3d(Eigen::Translation3d(correction)));
|
||||
|
||||
@ -46,7 +46,9 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
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) {
|
||||
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");
|
||||
}
|
||||
|
||||
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");
|
||||
if (!poses.defined())
|
||||
poses.setValue(PosesList({ pose }));
|
||||
@ -82,11 +84,11 @@ void FixedCartesianPoses::compute() {
|
||||
return;
|
||||
|
||||
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())
|
||||
pose.header.frame_id = scene->getPlanningFrame();
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
@ -43,12 +43,18 @@
|
||||
#include <moveit/robot_state/conversions.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>
|
||||
#endif
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace stages {
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("GenerateGraspPose");
|
||||
|
||||
GenerateGraspPose::GenerateGraspPose(const std::string& name) : GeneratePose(name) {
|
||||
auto& p = properties();
|
||||
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 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)
|
||||
throw moveit::Exception{ "RobotState message must be a diff" };
|
||||
const auto& accepted = jmg->getJointModelNames();
|
||||
@ -150,7 +157,7 @@ void GenerateGraspPose::compute() {
|
||||
const std::string& eef = props.get<std::string>("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 {
|
||||
applyPreGrasp(robot_state, jmg, props.property("pregrasp"));
|
||||
} catch (const moveit::Exception& e) {
|
||||
@ -158,7 +165,7 @@ void GenerateGraspPose::compute() {
|
||||
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");
|
||||
|
||||
double current_angle = 0.0;
|
||||
|
||||
@ -45,16 +45,22 @@
|
||||
#include <moveit/robot_state/attached_body.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>
|
||||
#endif
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace stages {
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("GeneratePlacePose");
|
||||
|
||||
GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(name) {
|
||||
auto& p = properties();
|
||||
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) {
|
||||
@ -75,7 +81,7 @@ void GeneratePlacePose::onNewSolution(const SolutionBase& s) {
|
||||
solution.setComment(msg);
|
||||
spawn(std::move(state), std::move(solution));
|
||||
} else
|
||||
ROS_WARN_STREAM_NAMED("GeneratePlacePose", msg);
|
||||
RCLCPP_WARN_STREAM(LOGGER, msg);
|
||||
return;
|
||||
}
|
||||
|
||||
@ -95,13 +101,13 @@ void GeneratePlacePose::compute() {
|
||||
// current object_pose w.r.t. planning frame
|
||||
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;
|
||||
tf2::fromMsg(pose_msg.pose, target_pose);
|
||||
// target pose w.r.t. planning frame
|
||||
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;
|
||||
tf2::fromMsg(ik_frame_msg.pose, ik_frame);
|
||||
ik_frame = robot_state.getGlobalLinkTransform(ik_frame_msg.header.frame_id) * ik_frame;
|
||||
@ -121,7 +127,7 @@ void GeneratePlacePose::compute() {
|
||||
.pretranslate(pos);
|
||||
|
||||
// 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.pose = tf2::toMsg(object * object_to_ik);
|
||||
|
||||
|
||||
@ -45,11 +45,13 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace stages {
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("GeneratePose");
|
||||
|
||||
GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) {
|
||||
setCostTerm(std::make_unique<cost::Constant>(0.0));
|
||||
|
||||
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() {
|
||||
@ -71,11 +73,11 @@ void GeneratePose::compute() {
|
||||
return;
|
||||
|
||||
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())
|
||||
target_pose.header.frame_id = scene->getPlanningFrame();
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
@ -46,6 +46,8 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace stages {
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("ModifyPlanningScene");
|
||||
|
||||
ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) {
|
||||
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());
|
||||
}
|
||||
|
||||
void ModifyPlanningScene::addObject(const moveit_msgs::CollisionObject& collision_object) {
|
||||
if (collision_object.operation != moveit_msgs::CollisionObject::ADD) {
|
||||
ROS_ERROR_STREAM_NAMED("ModifyPlanningScene", name() << ": addObject is called with object's operation not set "
|
||||
void ModifyPlanningScene::addObject(const moveit_msgs::msg::CollisionObject& collision_object) {
|
||||
if (collision_object.operation != moveit_msgs::msg::CollisionObject::ADD) {
|
||||
RCLCPP_ERROR_STREAM(LOGGER, name() << ": addObject is called with object's operation not set "
|
||||
"to ADD -- ignoring the object");
|
||||
return;
|
||||
}
|
||||
@ -66,9 +68,9 @@ void ModifyPlanningScene::addObject(const moveit_msgs::CollisionObject& collisio
|
||||
}
|
||||
|
||||
void ModifyPlanningScene::removeObject(const std::string& object_name) {
|
||||
moveit_msgs::CollisionObject obj;
|
||||
moveit_msgs::msg::CollisionObject obj;
|
||||
obj.id = object_name;
|
||||
obj.operation = moveit_msgs::CollisionObject::REMOVE;
|
||||
obj.operation = moveit_msgs::msg::CollisionObject::REMOVE;
|
||||
collision_objects_.push_back(obj);
|
||||
}
|
||||
|
||||
@ -93,13 +95,13 @@ void ModifyPlanningScene::computeBackward(const InterfaceState& to) {
|
||||
|
||||
void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene,
|
||||
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;
|
||||
bool attach = pair.second.second;
|
||||
if (invert)
|
||||
attach = !attach;
|
||||
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) {
|
||||
obj.object.id = name;
|
||||
scene.processAttachedCollisionObjectMsg(obj);
|
||||
@ -141,7 +143,7 @@ InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool inver
|
||||
}
|
||||
|
||||
void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& scene,
|
||||
const moveit_msgs::CollisionObject& object) {
|
||||
const moveit_msgs::msg::CollisionObject& object) {
|
||||
scene.processCollisionObjectMsg(object);
|
||||
}
|
||||
} // namespace stages
|
||||
|
||||
@ -41,12 +41,18 @@
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.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
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace stages {
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveRelative");
|
||||
|
||||
MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner)
|
||||
: PropagatingEitherWay(name), planner_(planner) {
|
||||
setCostTerm(std::make_unique<cost::PathLength>());
|
||||
@ -54,21 +60,21 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
|
||||
auto& p = properties();
|
||||
p.property("timeout").setDefaultValue(1.0);
|
||||
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");
|
||||
// register actual types
|
||||
PropertySerializer<geometry_msgs::TwistStamped>();
|
||||
PropertySerializer<geometry_msgs::Vector3Stamped>();
|
||||
PropertySerializer<geometry_msgs::msg::TwistStamped>();
|
||||
PropertySerializer<geometry_msgs::msg::Vector3Stamped>();
|
||||
p.declare<double>("min_distance", -1.0, "minimum 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");
|
||||
}
|
||||
|
||||
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.pose = tf2::toMsg(pose);
|
||||
setIKFrame(pose_msg);
|
||||
@ -104,7 +110,7 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c
|
||||
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 Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) {
|
||||
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_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.header.frame_id = frame_id;
|
||||
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,
|
||||
SubTrajectory& solution, Interface::Direction dir) {
|
||||
scene = state.scene()->diff();
|
||||
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
|
||||
const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
|
||||
assert(robot_model);
|
||||
|
||||
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 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;
|
||||
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
|
||||
|
||||
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);
|
||||
tf2::fromMsg(target.twist.linear, linear);
|
||||
tf2::fromMsg(target.twist.angular, angular);
|
||||
@ -252,7 +258,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
}
|
||||
|
||||
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);
|
||||
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);
|
||||
|
||||
robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
||||
moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
||||
reached_state->updateLinkTransforms();
|
||||
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link);
|
||||
|
||||
|
||||
@ -38,7 +38,6 @@
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.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/cost_terms.h>
|
||||
@ -46,11 +45,19 @@
|
||||
#include <moveit/task_constructor/utils.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 task_constructor {
|
||||
namespace stages {
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveTo");
|
||||
|
||||
MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner)
|
||||
: PropagatingEitherWay(name), planner_(planner) {
|
||||
setCostTerm(std::make_unique<cost::PathLength>());
|
||||
@ -58,27 +65,27 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
|
||||
auto& p = properties();
|
||||
p.property("timeout").setDefaultValue(1.0);
|
||||
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");
|
||||
// register actual types
|
||||
PropertySerializer<std::string>();
|
||||
PropertySerializer<moveit_msgs::RobotState>();
|
||||
PropertySerializer<geometry_msgs::PointStamped>();
|
||||
PropertySerializer<geometry_msgs::PoseStamped>();
|
||||
PropertySerializer<moveit_msgs::msg::RobotState>();
|
||||
PropertySerializer<geometry_msgs::msg::PointStamped>();
|
||||
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");
|
||||
}
|
||||
|
||||
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.pose = tf2::toMsg(pose);
|
||||
setIKFrame(pose_msg);
|
||||
}
|
||||
|
||||
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.position.reserve(joints.size());
|
||||
|
||||
@ -109,7 +116,7 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint
|
||||
|
||||
try {
|
||||
// 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)
|
||||
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,
|
||||
Eigen::Isometry3d& target) {
|
||||
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);
|
||||
|
||||
// transform target into global frame
|
||||
target = scene->getFrameTransform(msg.header.frame_id) * target;
|
||||
} 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,
|
||||
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen) {
|
||||
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;
|
||||
tf2::fromMsg(target.point, target_point);
|
||||
// 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,
|
||||
Interface::Direction dir) {
|
||||
scene = state.scene()->diff();
|
||||
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
|
||||
const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
|
||||
assert(robot_model);
|
||||
|
||||
const auto& props = properties();
|
||||
@ -195,7 +203,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
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;
|
||||
bool success = false;
|
||||
|
||||
@ -206,7 +214,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
// Where to go?
|
||||
Eigen::Isometry3d target;
|
||||
// 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;
|
||||
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[]) {
|
||||
geometry_msgs::PoseStamped msg;
|
||||
geometry_msgs::msg::PoseStamped msg;
|
||||
msg.header.frame_id = scene->getPlanningFrame();
|
||||
msg.pose = tf2::toMsg(pose);
|
||||
rviz_marker_tools::appendFrame(solution.markers(), msg, 0.1, name);
|
||||
|
||||
@ -44,11 +44,9 @@
|
||||
#include <moveit/robot_state/robot_state.h>
|
||||
|
||||
#include <Eigen/Geometry>
|
||||
#include <eigen_conversions/eigen_msg.h>
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <iterator>
|
||||
#include <ros/console.h>
|
||||
|
||||
namespace moveit {
|
||||
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
|
||||
|
||||
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");
|
||||
if (frame.empty())
|
||||
return boost::any();
|
||||
@ -79,7 +79,7 @@ void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& 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) {
|
||||
auto& p = approach_stage_->properties();
|
||||
p.set("direction", motion);
|
||||
@ -87,7 +87,8 @@ void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion
|
||||
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();
|
||||
p.set("direction", motion);
|
||||
p.set("min_distance", min_distance);
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#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
|
||||
|
||||
|
||||
@ -44,7 +44,11 @@
|
||||
#include <moveit/planning_scene/planning_scene.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>
|
||||
#endif
|
||||
|
||||
namespace moveit {
|
||||
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) {
|
||||
const std::string& eef = p.get<std::string>("eef");
|
||||
moveit_msgs::AttachedCollisionObject obj;
|
||||
obj.object.operation =
|
||||
forward ? (int8_t)moveit_msgs::CollisionObject::ADD : (int8_t)moveit_msgs::CollisionObject::REMOVE;
|
||||
moveit_msgs::msg::AttachedCollisionObject obj;
|
||||
obj.object.operation = forward ? (int8_t)moveit_msgs::msg::CollisionObject::ADD :
|
||||
(int8_t)moveit_msgs::msg::CollisionObject::REMOVE;
|
||||
obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second;
|
||||
obj.object.id = p.get<std::string>("object");
|
||||
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) {
|
||||
geometry_msgs::PoseStamped pose_msg;
|
||||
geometry_msgs::msg::PoseStamped pose_msg;
|
||||
pose_msg.header.frame_id = link;
|
||||
pose_msg.pose = tf2::toMsg(pose);
|
||||
setIKFrame(pose_msg);
|
||||
|
||||
@ -46,6 +46,8 @@
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("InterfaceState");
|
||||
|
||||
planning_scene::PlanningSceneConstPtr ensureUpdated(const planning_scene::PlanningScenePtr& scene) {
|
||||
// ensure scene's state is updated
|
||||
if (scene->getCurrentState().dirty())
|
||||
@ -58,7 +60,7 @@ InterfaceState::InterfaceState(const planning_scene::PlanningScenePtr& ps) : Int
|
||||
InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr& ps)
|
||||
: scene_(ps), priority_(Priority(0, 0.0)) {
|
||||
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)
|
||||
@ -171,7 +173,7 @@ void SolutionBase::markAsFailure(const std::string& msg) {
|
||||
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.cost = this->cost();
|
||||
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());
|
||||
}
|
||||
|
||||
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();
|
||||
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);
|
||||
|
||||
if (trajectory())
|
||||
@ -202,8 +204,9 @@ void SolutionSequence::push_back(const SolutionBase& solution) {
|
||||
subsolutions_.push_back(&solution);
|
||||
}
|
||||
|
||||
void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const {
|
||||
moveit_task_constructor_msgs::SubSolution sub_msg;
|
||||
void SolutionSequence::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg,
|
||||
Introspection* introspection) const {
|
||||
moveit_task_constructor_msgs::msg::SubSolution sub_msg;
|
||||
SolutionBase::fillInfo(sub_msg.info, introspection);
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution,
|
||||
void WrappedSolution::fillMessage(moveit_task_constructor_msgs::msg::Solution& solution,
|
||||
Introspection* introspection) const {
|
||||
wrapped_->fillMessage(solution, introspection);
|
||||
|
||||
// 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);
|
||||
sub_msg.sub_solution_id.push_back(introspection ? introspection->solutionId(*wrapped_) : 0);
|
||||
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/task_p.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 <actionlib/client/simple_action_client.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
#include <moveit/robot_model_loader/robot_model_loader.h>
|
||||
#include <moveit/planning_pipeline/planning_pipeline.h>
|
||||
|
||||
#include <functional>
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor.task");
|
||||
|
||||
namespace {
|
||||
std::string rosNormalizeName(const std::string& name) {
|
||||
std::string n;
|
||||
@ -111,7 +113,7 @@ Task::Task(const std::string& ns, bool introspection, ContainerBase::pointer&& c
|
||||
// monitor state on commandline
|
||||
// addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout)));
|
||||
// enable introspection by default, but only if ros::init() was called
|
||||
if (ros::isInitialized() && introspection)
|
||||
if (rclcpp::ok() && introspection)
|
||||
enableIntrospection(true);
|
||||
}
|
||||
|
||||
@ -137,7 +139,7 @@ Task::~Task() {
|
||||
|
||||
void Task::setRobotModel(const core::RobotModelConstPtr& robot_model) {
|
||||
if (!robot_model) {
|
||||
ROS_ERROR_STREAM(name() << ": received invalid robot model");
|
||||
RCLCPP_ERROR_STREAM(LOGGER, name() << ": received invalid robot model");
|
||||
return;
|
||||
}
|
||||
auto impl = pimpl();
|
||||
@ -146,9 +148,9 @@ void Task::setRobotModel(const core::RobotModelConstPtr& 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();
|
||||
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());
|
||||
if (!impl->robot_model_)
|
||||
throw Exception("Task failed to construct RobotModel");
|
||||
@ -212,7 +214,7 @@ void Task::reset() {
|
||||
void Task::init() {
|
||||
auto impl = pimpl();
|
||||
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
|
||||
StagePrivate* child = wrapped()->pimpl();
|
||||
@ -269,17 +271,48 @@ void Task::preempt() {
|
||||
pimpl()->preempt_requested_ = true;
|
||||
}
|
||||
|
||||
moveit_msgs::MoveItErrorCodes Task::execute(const SolutionBase& s) {
|
||||
actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> ac("execute_task_solution");
|
||||
ac.waitForServer();
|
||||
moveit_msgs::msg::MoveItErrorCodes Task::execute(const SolutionBase& s) {
|
||||
// Add random ID to prevent warnings about multiple publishers within the same node
|
||||
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.start()->scene()->getPlanningSceneMsg(goal.solution.start_scene);
|
||||
|
||||
ac.sendGoal(goal);
|
||||
ac.waitForResult();
|
||||
return ac.getResult()->error_code;
|
||||
moveit_msgs::msg::MoveItErrorCodes error_code;
|
||||
error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
|
||||
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) {
|
||||
|
||||
@ -35,7 +35,11 @@
|
||||
|
||||
/* 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>
|
||||
#endif
|
||||
|
||||
#include <moveit/robot_state/robot_state.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);
|
||||
} 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 (!(robot_link = get_tip())) {
|
||||
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;
|
||||
} else {
|
||||
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());
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -3,27 +3,36 @@
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
if (CATKIN_ENABLE_TESTING)
|
||||
find_package(rostest REQUIRED)
|
||||
if (BUILD_TESTING)
|
||||
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})
|
||||
|
||||
macro(mtc_add_test TYPE)
|
||||
# Split ARGN into .test file and other sources
|
||||
set(TEST_FILE ${ARGN})
|
||||
set(SOURCES ${ARGN})
|
||||
list(FILTER TEST_FILE INCLUDE REGEX "\.test$")
|
||||
list(FILTER SOURCES EXCLUDE REGEX "\.test$")
|
||||
list(FILTER TEST_FILE INCLUDE REGEX "\.launch.py$")
|
||||
list(FILTER SOURCES EXCLUDE REGEX "\.launch.py$")
|
||||
# Determine TARGET name from first source file
|
||||
list(GET SOURCES 0 TEST_NAME)
|
||||
string(REGEX REPLACE "\.cpp$" "" TEST_NAME ${TEST_NAME})
|
||||
string(REGEX REPLACE "_" "-" TEST_NAME ${TEST_NAME})
|
||||
# Configure build target
|
||||
if(TEST_FILE) # Add rostest if .test file was specified
|
||||
_add_rostest_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${TEST_FILE} ${SOURCES})
|
||||
if(TEST_FILE) # Add launch test if .launch.py file was specified
|
||||
ament_add_gtest_executable(${PROJECT_NAME}-${TEST_NAME} ${SOURCES})
|
||||
add_launch_test(${TEST_FILE}
|
||||
ARGS "test_binary:=$<TARGET_FILE:${PROJECT_NAME}-${TEST_NAME}>")
|
||||
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()
|
||||
target_link_libraries(${PROJECT_NAME}-${TEST_NAME} ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
|
||||
endmacro()
|
||||
@ -45,29 +54,30 @@ if (CATKIN_ENABLE_TESTING)
|
||||
mtc_add_gmock(test_cost_queue.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
|
||||
add_executable(pick_ur5 pick_ur5.cpp)
|
||||
target_link_libraries(pick_ur5 ${PROJECT_NAME}_stages gtest)
|
||||
ament_add_gtest_executable(pick_ur5 pick_ur5.cpp)
|
||||
target_link_libraries(pick_ur5 ${PROJECT_NAME}_stages)
|
||||
|
||||
add_executable(pick_pr2 pick_pr2.cpp)
|
||||
target_link_libraries(pick_pr2 ${PROJECT_NAME}_stages gtest)
|
||||
ament_add_gtest_executable(pick_pr2 pick_pr2.cpp)
|
||||
target_link_libraries(pick_pr2 ${PROJECT_NAME}_stages)
|
||||
|
||||
add_executable(pick_pa10 pick_pa10.cpp)
|
||||
target_link_libraries(pick_pa10 ${PROJECT_NAME}_stages gtest)
|
||||
ament_add_gtest_executable(pick_pa10 pick_pa10.cpp)
|
||||
target_link_libraries(pick_pa10 ${PROJECT_NAME}_stages)
|
||||
|
||||
# running these integrations test naturally requires the moveit configs
|
||||
find_package(tams_ur5_setup_moveit_config QUIET)
|
||||
if(tams_ur5_setup_moveit_config_FOUND)
|
||||
add_rostest(pick_ur5.test)
|
||||
endif()
|
||||
find_package(pr2_moveit_config_FOUND QUIET)
|
||||
if(pr2_moveit_config_FOUND)
|
||||
add_rostest(pick_pr2.test)
|
||||
endif()
|
||||
find_package(pa10_shadow_moveit_config QUIET)
|
||||
if(pa10_shadow_moveit_config_FOUND)
|
||||
add_rostest(pick_pa10.test)
|
||||
endif()
|
||||
# TODO(JafarAbdi): Port integration tests
|
||||
# # running these integrations test naturally requires the moveit configs
|
||||
# find_package(tams_ur5_setup_moveit_config QUIET)
|
||||
# if(tams_ur5_setup_moveit_config_FOUND)
|
||||
# add_rostest(pick_ur5.test)
|
||||
# endif()
|
||||
# find_package(pr2_moveit_config_FOUND QUIET)
|
||||
# if(pr2_moveit_config_FOUND)
|
||||
# add_rostest(pick_pr2.test)
|
||||
# endif()
|
||||
# find_package(pa10_shadow_moveit_config QUIET)
|
||||
# if(pa10_shadow_moveit_config_FOUND)
|
||||
# add_rostest(pick_pa10.test)
|
||||
# endif()
|
||||
endif()
|
||||
|
||||
@ -6,7 +6,8 @@ using namespace moveit::core;
|
||||
|
||||
RobotModelPtr getModel() {
|
||||
// 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
|
||||
moveit::core::RobotModelBuilder builder("robot", "base");
|
||||
@ -17,7 +18,7 @@ RobotModelPtr getModel() {
|
||||
return builder.build();
|
||||
}
|
||||
|
||||
moveit::core::RobotModelPtr loadModel() {
|
||||
static robot_model_loader::RobotModelLoader loader;
|
||||
moveit::core::RobotModelPtr loadModel(const rclcpp::Node::SharedPtr& node) {
|
||||
robot_model_loader::RobotModelLoader loader(node);
|
||||
return loader.getModel();
|
||||
}
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
namespace moveit {
|
||||
namespace core {
|
||||
@ -12,4 +13,4 @@ MOVEIT_CLASS_FORWARD(RobotModel);
|
||||
moveit::core::RobotModelPtr getModel();
|
||||
|
||||
// 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/fix_collision_objects.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
void spawnObject(const planning_scene::PlanningScenePtr& scene) {
|
||||
moveit_msgs::CollisionObject o;
|
||||
moveit_msgs::msg::CollisionObject o;
|
||||
o.id = "object";
|
||||
o.header.frame_id = "world";
|
||||
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].orientation.w = 1.0;
|
||||
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[0] = 0.23;
|
||||
o.primitives[0].dimensions[1] = 0.03;
|
||||
@ -39,13 +39,15 @@ void spawnObject(const planning_scene::PlanningScenePtr& scene) {
|
||||
TEST(PA10, pick) {
|
||||
Task t;
|
||||
t.stages()->setName("pick");
|
||||
t.loadRobotModel();
|
||||
|
||||
auto node = rclcpp::Node::make_shared("pa10");
|
||||
t.loadRobotModel(node);
|
||||
// define global properties used by most stages
|
||||
t.setProperty("group", std::string("left_arm"));
|
||||
t.setProperty("eef", std::string("la_tool_mount"));
|
||||
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");
|
||||
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
||||
|
||||
@ -87,7 +89,7 @@ TEST(PA10, pick) {
|
||||
move->setIKFrame("lh_tool_frame");
|
||||
move->setMinMaxDistance(0.05, 0.1);
|
||||
|
||||
geometry_msgs::Vector3Stamped direction;
|
||||
geometry_msgs::msg::Vector3Stamped direction;
|
||||
direction.header.frame_id = "lh_tool_frame";
|
||||
direction.vector.z = 1;
|
||||
move->setDirection(direction);
|
||||
@ -154,7 +156,7 @@ TEST(PA10, pick) {
|
||||
move->properties().set("marker_ns", std::string("lift"));
|
||||
move->setIKFrame("lh_tool_frame");
|
||||
|
||||
geometry_msgs::Vector3Stamped direction;
|
||||
geometry_msgs::msg::Vector3Stamped direction;
|
||||
direction.header.frame_id = "world";
|
||||
direction.vector.z = 1;
|
||||
move->setDirection(direction);
|
||||
@ -168,7 +170,7 @@ TEST(PA10, pick) {
|
||||
move->properties().set("marker_ns", std::string("lift"));
|
||||
move->setIKFrame("lh_tool_frame");
|
||||
|
||||
geometry_msgs::TwistStamped twist;
|
||||
geometry_msgs::msg::TwistStamped twist;
|
||||
twist.header.frame_id = "object";
|
||||
twist.twist.linear.y = 1;
|
||||
twist.twist.angular.y = 2;
|
||||
@ -189,9 +191,7 @@ TEST(PA10, pick) {
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "pa10");
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
#include <moveit/task_constructor/stages/connect.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 <gtest/gtest.h>
|
||||
|
||||
@ -16,7 +16,7 @@ using namespace moveit::task_constructor;
|
||||
void spawnObject() {
|
||||
moveit::planning_interface::PlanningSceneInterface psi;
|
||||
|
||||
moveit_msgs::CollisionObject o;
|
||||
moveit_msgs::msg::CollisionObject o;
|
||||
o.id = "object";
|
||||
o.header.frame_id = "base_link";
|
||||
o.primitive_poses.resize(1);
|
||||
@ -25,7 +25,7 @@ void spawnObject() {
|
||||
o.primitive_poses[0].position.z = 0.84;
|
||||
o.primitive_poses[0].orientation.w = 1.0;
|
||||
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[0] = 0.23;
|
||||
o.primitives[0].dimensions[1] = 0.03;
|
||||
@ -38,8 +38,9 @@ TEST(PR2, pick) {
|
||||
Stage* initial_stage = new stages::CurrentState("current state");
|
||||
t.add(std::unique_ptr<Stage>(initial_stage));
|
||||
|
||||
auto node = rclcpp::Node::make_shared("pr2");
|
||||
// planner used for connect
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
// connect to pick
|
||||
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));
|
||||
pick->setProperty("eef", std::string("left_gripper"));
|
||||
pick->setProperty("object", std::string("object"));
|
||||
geometry_msgs::TwistStamped approach;
|
||||
geometry_msgs::msg::TwistStamped approach;
|
||||
approach.header.frame_id = "l_gripper_tool_frame";
|
||||
approach.twist.linear.x = 1.0;
|
||||
pick->setApproachMotion(approach, 0.03, 0.1);
|
||||
|
||||
geometry_msgs::TwistStamped lift;
|
||||
geometry_msgs::msg::TwistStamped lift;
|
||||
lift.header.frame_id = "base_link";
|
||||
lift.twist.linear.z = 1.0;
|
||||
pick->setLiftMotion(lift, 0.03, 0.05);
|
||||
@ -87,12 +88,10 @@ TEST(PR2, pick) {
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "pr2");
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
#include <moveit/task_constructor/stages/connect.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 <gtest/gtest.h>
|
||||
|
||||
@ -16,7 +16,7 @@ using namespace moveit::task_constructor;
|
||||
void spawnObject() {
|
||||
moveit::planning_interface::PlanningSceneInterface psi;
|
||||
|
||||
moveit_msgs::CollisionObject o;
|
||||
moveit_msgs::msg::CollisionObject o;
|
||||
o.id = "object";
|
||||
o.header.frame_id = "table_top";
|
||||
o.primitive_poses.resize(1);
|
||||
@ -25,7 +25,7 @@ void spawnObject() {
|
||||
o.primitive_poses[0].position.z = 0.12;
|
||||
o.primitive_poses[0].orientation.w = 1.0;
|
||||
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[0] = 0.23;
|
||||
o.primitives[0].dimensions[1] = 0.03;
|
||||
@ -40,8 +40,9 @@ TEST(UR5, pick) {
|
||||
initial_stage = initial.get();
|
||||
t.add(std::move(initial));
|
||||
|
||||
auto node = rclcpp::Node::make_shared("ur5");
|
||||
// planner used for connect
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
// connect to pick
|
||||
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));
|
||||
pick->setProperty("eef", std::string("gripper"));
|
||||
pick->setProperty("object", std::string("object"));
|
||||
geometry_msgs::TwistStamped approach;
|
||||
geometry_msgs::msg::TwistStamped approach;
|
||||
approach.header.frame_id = "s_model_tool0";
|
||||
approach.twist.linear.x = 1.0;
|
||||
pick->setApproachMotion(approach, 0.03, 0.1);
|
||||
|
||||
geometry_msgs::TwistStamped lift;
|
||||
geometry_msgs::msg::TwistStamped lift;
|
||||
lift.header.frame_id = "world";
|
||||
lift.twist.linear.z = 1.0;
|
||||
pick->setLiftMotion(lift, 0.03, 0.05);
|
||||
@ -89,12 +90,10 @@ TEST(UR5, pick) {
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "ur5");
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
||||
@ -8,7 +8,6 @@
|
||||
#include "models.h"
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <gmock/gmock.h>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
|
||||
@ -9,6 +9,7 @@
|
||||
#include "gtest_value_printers.h"
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <gmock/gmock.h>
|
||||
#include <initializer_list>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
@ -176,8 +177,9 @@ TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) {
|
||||
int main(int argc, char** argv) {
|
||||
for (int i = 1; i < argc; ++i) {
|
||||
if (strcmp(argv[i], "--debug") == 0) {
|
||||
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug))
|
||||
ros::console::notifyLoggerLevelsChanged();
|
||||
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");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@ -8,12 +8,16 @@
|
||||
|
||||
#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>
|
||||
#endif
|
||||
|
||||
#include <moveit_msgs/RobotState.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <moveit_msgs/msg/robot_state.hpp>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
|
||||
#include <ros/console.h>
|
||||
#include <rclcpp/logging.hpp>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
@ -28,9 +32,10 @@ struct PandaMoveTo : public testing::Test
|
||||
Task t;
|
||||
stages::MoveTo* move_to;
|
||||
PlanningScenePtr scene;
|
||||
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_to");
|
||||
|
||||
PandaMoveTo() {
|
||||
t.setRobotModel(loadModel());
|
||||
t.loadRobotModel(node);
|
||||
|
||||
scene = std::make_shared<PlanningScene>(t.getRobotModel());
|
||||
scene->getCurrentStateNonConst().setToDefaultValues();
|
||||
@ -63,7 +68,7 @@ TEST_F(PandaMoveTo, mapTarget) {
|
||||
|
||||
TEST_F(PandaMoveTo, stateTarget) {
|
||||
move_to->setGoal([] {
|
||||
moveit_msgs::RobotState state;
|
||||
moveit_msgs::msg::RobotState state;
|
||||
state.is_diff = true;
|
||||
state.joint_state.name = { "panda_joint1", "panda_joint2" };
|
||||
state.joint_state.position = { TAU / 8, TAU / 8 };
|
||||
@ -72,19 +77,19 @@ TEST_F(PandaMoveTo, stateTarget) {
|
||||
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);
|
||||
auto frame_eigen{ state.getFrameTransform(frame) };
|
||||
geometry_msgs::PoseStamped p;
|
||||
geometry_msgs::msg::PoseStamped p;
|
||||
p.header.frame_id = state.getRobotModel()->getModelFrame();
|
||||
p.pose = tf2::toMsg(frame_eigen);
|
||||
return p;
|
||||
}
|
||||
|
||||
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.point = pose.pose.position;
|
||||
move_to->setGoal(point);
|
||||
@ -104,24 +109,24 @@ TEST_F(PandaMoveTo, poseIKFrameLinkTarget) {
|
||||
EXPECT_ONE_SOLUTION;
|
||||
}
|
||||
|
||||
moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) {
|
||||
moveit_msgs::AttachedCollisionObject aco;
|
||||
moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string& id) {
|
||||
moveit_msgs::msg::AttachedCollisionObject aco;
|
||||
aco.link_name = "panda_hand";
|
||||
aco.object.header.frame_id = aco.link_name;
|
||||
aco.object.operation = aco.object.ADD;
|
||||
aco.object.id = id;
|
||||
aco.object.primitives.resize(1, [] {
|
||||
shape_msgs::SolidPrimitive p;
|
||||
shape_msgs::msg::SolidPrimitive p;
|
||||
p.type = p.SPHERE;
|
||||
p.dimensions.resize(1);
|
||||
p.dimensions[p.SPHERE_RADIUS] = 0.01;
|
||||
return p;
|
||||
}());
|
||||
aco.object.primitive_poses.resize(1);
|
||||
#if MOVEIT_HAS_OBJECT_POSE
|
||||
aco.object.pose.position.z = 0.2;
|
||||
aco.object.pose.orientation.w = 1.0;
|
||||
#else
|
||||
aco.object.primitive_poses.resize(1);
|
||||
aco.object.primitive_poses[0].position.z = 0.2;
|
||||
aco.object.primitive_poses[0].orientation.w = 1.0;
|
||||
#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
|
||||
aco.object.subframe_names.resize(1, "subframe");
|
||||
aco.object.subframe_poses.resize(1, [] {
|
||||
geometry_msgs::Pose p;
|
||||
geometry_msgs::msg::Pose p;
|
||||
p.orientation.w = 1.0;
|
||||
return p;
|
||||
}());
|
||||
@ -162,9 +167,7 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "move_to_test");
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
|
||||
@ -4,10 +4,12 @@
|
||||
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||
#include <moveit/task_constructor/stages/modify_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 <ros/console.h>
|
||||
|
||||
#include <rclcpp/logging.hpp>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
@ -89,7 +91,7 @@ TEST(ModifyPlanningScene, allowCollisions) {
|
||||
|
||||
void spawnObject(PlanningScene& scene, const std::string& name, int type,
|
||||
const std::vector<double>& pos = { 0, 0, 0 }) {
|
||||
moveit_msgs::CollisionObject o;
|
||||
moveit_msgs::msg::CollisionObject o;
|
||||
o.id = name;
|
||||
o.header.frame_id = scene.getPlanningFrame();
|
||||
o.primitive_poses.resize(1);
|
||||
@ -102,13 +104,13 @@ void spawnObject(PlanningScene& scene, const std::string& name, int type,
|
||||
o.primitives[0].type = type;
|
||||
|
||||
switch (type) {
|
||||
case shape_msgs::SolidPrimitive::CYLINDER:
|
||||
case shape_msgs::msg::SolidPrimitive::CYLINDER:
|
||||
o.primitives[0].dimensions = { 0.1, 0.02 };
|
||||
break;
|
||||
case shape_msgs::SolidPrimitive::BOX:
|
||||
case shape_msgs::msg::SolidPrimitive::BOX:
|
||||
o.primitives[0].dimensions = { 0.1, 0.2, 0.3 };
|
||||
break;
|
||||
case shape_msgs::SolidPrimitive::SPHERE:
|
||||
case shape_msgs::msg::SolidPrimitive::SPHERE:
|
||||
o.primitives[0].dimensions = { 0.05 };
|
||||
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) {
|
||||
moveit_msgs::AttachedCollisionObject obj;
|
||||
moveit_msgs::msg::AttachedCollisionObject obj;
|
||||
obj.link_name = link;
|
||||
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;
|
||||
scene.processAttachedCollisionObjectMsg(obj);
|
||||
}
|
||||
@ -129,19 +131,19 @@ TEST(Connect, compatible) {
|
||||
auto scene = std::make_shared<PlanningScene>(getModel());
|
||||
auto& state = scene->getCurrentStateNonConst();
|
||||
state.setToDefaultValues();
|
||||
spawnObject(*scene, "object", shape_msgs::SolidPrimitive::CYLINDER);
|
||||
spawnObject(*scene, "object", shape_msgs::msg::SolidPrimitive::CYLINDER);
|
||||
state.update();
|
||||
|
||||
auto other = scene->diff();
|
||||
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";
|
||||
|
||||
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";
|
||||
|
||||
spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER);
|
||||
spawnObject(*other, "object", shape_msgs::msg::SolidPrimitive::CYLINDER);
|
||||
EXPECT_TRUE(connect.compatible(scene, other)) << "same objects";
|
||||
|
||||
// attached objects
|
||||
@ -152,7 +154,7 @@ TEST(Connect, compatible) {
|
||||
other = scene->diff();
|
||||
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);
|
||||
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)
|
||||
|
||||
if(NOT "${CMAKE_CXX_STANDARD}")
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
endif()
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_CXX_EXTENSIONS OFF)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
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_task_constructor_core
|
||||
moveit_task_constructor_msgs
|
||||
moveit_ros_planning
|
||||
moveit_ros_planning_interface
|
||||
rosparam_shortcuts
|
||||
)
|
||||
tf2_eigen
|
||||
rosparam_shortcuts)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS roscpp
|
||||
)
|
||||
|
||||
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})
|
||||
add_library(${PROJECT_NAME}_pick_place_task SHARED src/pick_place_task.cpp)
|
||||
ament_target_dependencies(${PROJECT_NAME}_pick_place_task ${THIS_PACKAGE_INCLUDE_DEPENDS})
|
||||
install(TARGETS ${PROJECT_NAME}_pick_place_task
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
include_directories(include)
|
||||
|
||||
# declare a demo consisting of a single cpp file
|
||||
function(demo name)
|
||||
add_executable(${PROJECT_NAME}_${name} src/${name}.cpp)
|
||||
add_dependencies(${PROJECT_NAME}_${name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(${PROJECT_NAME}_${name} ${catkin_LIBRARIES})
|
||||
ament_target_dependencies(${PROJECT_NAME}_${name} ${THIS_PACKAGE_INCLUDE_DEPENDS})
|
||||
set_target_properties(${PROJECT_NAME}_${name} PROPERTIES OUTPUT_NAME ${name} PREFIX "")
|
||||
install(TARGETS ${PROJECT_NAME}_${name}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
endfunction()
|
||||
|
||||
@ -55,7 +58,10 @@ demo(pick_place_demo)
|
||||
target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task)
|
||||
|
||||
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
|
||||
|
||||
roslaunch moveit_task_constructor_demo demo.launch
|
||||
ros2 launch moveit_task_constructor_demo demo.launch.py
|
||||
|
||||
@ -1,15 +1,24 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 84
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /Motion Planning Tasks1
|
||||
Splitter Ratio: 0.5393258333206177
|
||||
Tree Height: 533
|
||||
- Class: rviz/Help
|
||||
Name: Help
|
||||
- Class: rviz/Views
|
||||
- /PlanningScene1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 439
|
||||
- Class: rviz_common/Selection
|
||||
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:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
@ -22,32 +31,26 @@ Panels:
|
||||
Name: Motion Planning Tasks
|
||||
Tasks View:
|
||||
property_splitter:
|
||||
- 540
|
||||
- 0
|
||||
- 197
|
||||
- 121
|
||||
solution_sorting:
|
||||
column: 1
|
||||
order: 0
|
||||
column: 0
|
||||
order: 1
|
||||
solutions_splitter:
|
||||
- 306
|
||||
- 98
|
||||
solutions_view_columns:
|
||||
- 38
|
||||
- 52
|
||||
- 0
|
||||
- 397
|
||||
- 198
|
||||
solutions_view_columns: ~
|
||||
tasks_view_columns:
|
||||
- 226
|
||||
- 38
|
||||
- 38
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
- 295
|
||||
- 26
|
||||
- 26
|
||||
- 48
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
@ -63,31 +66,6 @@ Visualization Manager:
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
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
|
||||
Enabled: true
|
||||
Interrupt Display: false
|
||||
@ -104,7 +82,66 @@ Visualization Manager:
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
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 Visual: true
|
||||
Use Fixed Robot Color: false
|
||||
@ -119,62 +156,175 @@ Visualization Manager:
|
||||
Voxel Rendering: Occupied Voxels
|
||||
Show Trail: false
|
||||
State Display Time: 0.05 s
|
||||
Task Solution Topic: /mtc_tutorial/solution
|
||||
Task Solution Topic: /solution
|
||||
Tasks:
|
||||
Cartesian Path: 1
|
||||
Cartesian Path: 0
|
||||
Trail Step Size: 1
|
||||
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
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Default Light: true
|
||||
Fixed Frame: panda_link0
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- 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
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/XYOrbit
|
||||
Distance: 1.3878222703933716
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 3.8008623123168945
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.7853981852531433
|
||||
Focal Point:
|
||||
X: 0.30880630016326904
|
||||
Y: -0.1259305477142334
|
||||
Z: 1.3560062370743253e-6
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.264797568321228
|
||||
Target Frame: panda_link0
|
||||
Yaw: 4.939944744110107
|
||||
Pitch: 0.645397961139679
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.7453980445861816
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 768
|
||||
Help:
|
||||
collapsed: false
|
||||
Height: 1043
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
Motion Planning Tasks:
|
||||
collapsed: false
|
||||
Motion Planning Tasks - Slider:
|
||||
collapsed: false
|
||||
QMainWindow State: 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
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000257000003bdfc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000240000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000158000001b00000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000002a004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b0073010000028100000177000000e300fffffffb0000003c004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b00730020002d00200053006c00690064006500720000000000ffffffff0000003f00ffffff000000010000010f000003bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003bd000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000040e000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1621
|
||||
X: 249
|
||||
Y: 25
|
||||
Width: 1920
|
||||
X: 0
|
||||
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
|
||||
arm_group_name: "panda_arm"
|
||||
eef_name: "hand"
|
||||
hand_group_name: "hand"
|
||||
hand_frame: "panda_link8"
|
||||
# Planning group and link names
|
||||
arm_group_name: "panda_arm"
|
||||
eef_name: "hand"
|
||||
hand_group_name: "hand"
|
||||
hand_frame: "panda_link8"
|
||||
|
||||
# Poses
|
||||
hand_open_pose: "open"
|
||||
hand_close_pose: "close"
|
||||
arm_home_pose: "ready"
|
||||
# Poses
|
||||
hand_open_pose: "open"
|
||||
hand_close_pose: "close"
|
||||
arm_home_pose: "ready"
|
||||
|
||||
# Scene frames
|
||||
world_frame: "world"
|
||||
table_reference_frame: "world"
|
||||
object_reference_frame: "world"
|
||||
surface_link: "table"
|
||||
# Scene frames
|
||||
world_frame: "world"
|
||||
table_reference_frame: "world"
|
||||
object_reference_frame: "world"
|
||||
surface_link: "table"
|
||||
|
||||
# Collision object for picking
|
||||
# CYLINDER object specifications
|
||||
object_name: "object"
|
||||
object_dimensions: [0.25, 0.02] # [height, radius]
|
||||
object_pose: [0.5, -0.25, 0.0, 0, 0, 0]
|
||||
# Collision object for picking
|
||||
# CYLINDER object specifications
|
||||
object_name: "object"
|
||||
object_dimensions: [0.25, 0.02] # [height, radius]
|
||||
object_pose: [0.5, -0.25, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
# Table model
|
||||
spawn_table: true
|
||||
table_name: "table"
|
||||
table_dimensions: [0.4, 0.5, 0.1] # [length, width, height]
|
||||
table_pose: [0.5, -0.25, 0, 0, 0, 0]
|
||||
# Table model
|
||||
spawn_table: true
|
||||
table_name: "table"
|
||||
table_dimensions: [0.4, 0.5, 0.1] # [length, width, height]
|
||||
table_pose: [0.5, -0.25, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
# Gripper grasp frame transform [x,y,z,r,p,y]
|
||||
grasp_frame_transform: [0, 0, 0.1, 1.571, 0.785, 1.571]
|
||||
# Gripper grasp frame transform [x,y,z,r,p,y]
|
||||
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: [0.6, -0.15, 0, 0, 0, 0]
|
||||
place_surface_offset: 0.0001 # place offset from table
|
||||
# Place pose [x,y,z,r,p,y]
|
||||
place_pose: [0.6, -0.15, 0.0, 0.0, 0.0, 0.0]
|
||||
place_surface_offset: 0.0001 # place offset from table
|
||||
|
||||
# Valid distance range when approaching an object for picking
|
||||
approach_object_min_dist: 0.1
|
||||
approach_object_max_dist: 0.15
|
||||
# Valid distance range when approaching an object for picking
|
||||
approach_object_min_dist: 0.1
|
||||
approach_object_max_dist: 0.15
|
||||
|
||||
# Valid height range when lifting an object after pick
|
||||
lift_object_min_dist: 0.01
|
||||
lift_object_max_dist: 0.1
|
||||
# Valid height range when lifting an object after pick
|
||||
lift_object_min_dist: 0.01
|
||||
lift_object_max_dist: 0.1
|
||||
|
||||
@ -35,7 +35,7 @@
|
||||
*/
|
||||
|
||||
// ROS
|
||||
#include <ros/ros.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
// MoveIt
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
@ -56,22 +56,26 @@
|
||||
#include <moveit/task_constructor/stages/predicate_filter.h>
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.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
|
||||
|
||||
namespace moveit_task_constructor_demo {
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
// prepare a demo environment from ROS parameters under pnh
|
||||
void setupDemoScene(ros::NodeHandle& pnh);
|
||||
// prepare a demo environment from ROS parameters under node
|
||||
void setupDemoScene(const rclcpp::Node::SharedPtr& node);
|
||||
|
||||
class PickPlaceTask
|
||||
{
|
||||
public:
|
||||
PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh);
|
||||
PickPlaceTask(const std::string& task_name, const rclcpp::Node::SharedPtr& pnh);
|
||||
~PickPlaceTask() = default;
|
||||
|
||||
bool init();
|
||||
@ -83,9 +87,7 @@ public:
|
||||
private:
|
||||
void loadParameters();
|
||||
|
||||
static constexpr char LOGNAME[]{ "pick_place_task" };
|
||||
|
||||
ros::NodeHandle pnh_;
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
std::string task_name_;
|
||||
moveit::task_constructor::TaskPtr task_;
|
||||
@ -117,7 +119,7 @@ private:
|
||||
double lift_object_max_dist_;
|
||||
|
||||
// Place metrics
|
||||
geometry_msgs::Pose place_pose_;
|
||||
geometry_msgs::msg::Pose place_pose_;
|
||||
double place_surface_offset_;
|
||||
};
|
||||
} // 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>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<depend>roscpp</depend>
|
||||
<depend>rosparam_shortcuts</depend>
|
||||
|
||||
<depend>moveit_core</depend>
|
||||
<depend>moveit_ros_planning_interface</depend>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_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_resources_panda_moveit_config</exec_depend>
|
||||
|
||||
<test_depend>rostest</test_depend>
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#include <ros/ros.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
@ -16,11 +16,15 @@ using namespace moveit::task_constructor;
|
||||
|
||||
/* FixedState - Connect - FixedState */
|
||||
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;
|
||||
t.stages()->setName("alternative path costs");
|
||||
t.loadRobotModel();
|
||||
t.loadRobotModel(node);
|
||||
|
||||
assert(t.getRobotModel()->getName() == "panda");
|
||||
|
||||
@ -33,7 +37,7 @@ int main(int argc, char** argv) {
|
||||
initial->setState(scene);
|
||||
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") };
|
||||
{
|
||||
@ -76,7 +80,8 @@ int main(int argc, char** argv) {
|
||||
std::cout << e << std::endl;
|
||||
}
|
||||
|
||||
ros::spin();
|
||||
// keep alive for interactive inspection in rviz
|
||||
spinning_thread.join();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -41,12 +41,12 @@
|
||||
#include <moveit/task_constructor/stages/move_relative.h>
|
||||
#include <moveit/task_constructor/stages/connect.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
Task createTask() {
|
||||
Task createTask(const rclcpp::Node::SharedPtr& node) {
|
||||
Task t;
|
||||
t.stages()->setName("Cartesian Path");
|
||||
|
||||
@ -59,7 +59,7 @@ Task createTask() {
|
||||
auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>();
|
||||
|
||||
// start from a fixed robot state
|
||||
t.loadRobotModel();
|
||||
t.loadRobotModel(node);
|
||||
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
|
||||
{
|
||||
auto& state = scene->getCurrentStateNonConst();
|
||||
@ -73,7 +73,7 @@ Task createTask() {
|
||||
{
|
||||
auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian_interpolation);
|
||||
stage->setGroup(group);
|
||||
geometry_msgs::Vector3Stamped direction;
|
||||
geometry_msgs::msg::Vector3Stamped direction;
|
||||
direction.header.frame_id = "world";
|
||||
direction.vector.x = 0.2;
|
||||
stage->setDirection(direction);
|
||||
@ -83,7 +83,7 @@ Task createTask() {
|
||||
{
|
||||
auto stage = std::make_unique<stages::MoveRelative>("y -0.3", cartesian_interpolation);
|
||||
stage->setGroup(group);
|
||||
geometry_msgs::Vector3Stamped direction;
|
||||
geometry_msgs::msg::Vector3Stamped direction;
|
||||
direction.header.frame_id = "world";
|
||||
direction.vector.y = -0.3;
|
||||
stage->setDirection(direction);
|
||||
@ -93,7 +93,7 @@ Task createTask() {
|
||||
{ // rotate about TCP
|
||||
auto stage = std::make_unique<stages::MoveRelative>("rz +45°", cartesian_interpolation);
|
||||
stage->setGroup(group);
|
||||
geometry_msgs::TwistStamped twist;
|
||||
geometry_msgs::msg::TwistStamped twist;
|
||||
twist.header.frame_id = "world";
|
||||
twist.twist.angular.z = M_PI / 4.;
|
||||
stage->setDirection(twist);
|
||||
@ -108,16 +108,8 @@ Task createTask() {
|
||||
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
|
||||
// specifying two groups (arm and hand) will try to merge both trajectories
|
||||
stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation }, { eef, joint_interpolation } };
|
||||
stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation } };
|
||||
auto connect = std::make_unique<stages::Connect>("connect", planners);
|
||||
t.add(std::move(connect));
|
||||
}
|
||||
@ -132,12 +124,11 @@ Task createTask() {
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
ros::init(argc, argv, "mtc_tutorial");
|
||||
// run an asynchronous spinner to communicate with the move_group node and rviz
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = rclcpp::Node::make_shared("mtc_tutorial");
|
||||
std::thread spinning_thread([node] { rclcpp::spin(node); });
|
||||
|
||||
auto task = createTask();
|
||||
auto task = createTask(node);
|
||||
try {
|
||||
if (task.plan())
|
||||
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;
|
||||
}
|
||||
|
||||
ros::waitForShutdown(); // keep alive for interactive inspection in rviz
|
||||
// keep alive for interactive inspection in rviz
|
||||
spinning_thread.join();
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#include <ros/ros.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
@ -8,24 +8,23 @@
|
||||
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
#include <rosparam_shortcuts/rosparam_shortcuts.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
/* ComputeIK(FixedState) */
|
||||
int main(int argc, char** argv) {
|
||||
ros::init(argc, argv, "mtc_tutorial");
|
||||
ros::NodeHandle nh{ "~" };
|
||||
|
||||
ros::AsyncSpinner spinner{ 1 };
|
||||
spinner.start();
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = rclcpp::Node::make_shared("moveit_task_constructor_demo");
|
||||
std::thread spinning_thread([node] { rclcpp::spin(node); });
|
||||
|
||||
Task t;
|
||||
t.stages()->setName("clearance IK");
|
||||
|
||||
// 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");
|
||||
|
||||
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.getJointModelGroup("panda_arm"), "extended");
|
||||
|
||||
moveit_msgs::CollisionObject co;
|
||||
moveit_msgs::msg::CollisionObject co;
|
||||
co.id = "obstacle";
|
||||
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[0] = 0.1;
|
||||
co.header.frame_id = "world";
|
||||
@ -56,8 +55,10 @@ int main(int argc, char** argv) {
|
||||
ik->setMaxIKSolutions(100);
|
||||
|
||||
auto cl_cost{ std::make_unique<cost::Clearance>() };
|
||||
cl_cost->cumulative = nh.param("cumulative", false); // sum up pairwise distances?
|
||||
cl_cost->with_world = nh.param("with_world", true); // consider distance to world objects?
|
||||
cl_cost->cumulative = false;
|
||||
rosparam_shortcuts::get(node, "cumulative", cl_cost->cumulative); // sum up pairwise distances?
|
||||
cl_cost->with_world = true;
|
||||
rosparam_shortcuts::get(node, "with_world", cl_cost->cumulative); // consider distance to world objects?
|
||||
ik->setCostTerm(std::move(cl_cost));
|
||||
|
||||
t.add(std::move(ik));
|
||||
@ -68,7 +69,7 @@ int main(int argc, char** argv) {
|
||||
std::cout << e << std::endl;
|
||||
}
|
||||
|
||||
ros::waitForShutdown();
|
||||
|
||||
// Keep introspection alive
|
||||
spinning_thread.join();
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -42,7 +42,7 @@
|
||||
#include <moveit/task_constructor/stages/connect.h>
|
||||
#include <moveit/task_constructor/container.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
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);
|
||||
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
||||
geometry_msgs::Vector3Stamped direction;
|
||||
geometry_msgs::msg::Vector3Stamped direction;
|
||||
direction.header.frame_id = "world";
|
||||
direction.vector.x = 0.2;
|
||||
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);
|
||||
stage->properties().configureInitFrom(Stage::PARENT);
|
||||
geometry_msgs::Vector3Stamped direction;
|
||||
geometry_msgs::msg::Vector3Stamped direction;
|
||||
direction.header.frame_id = "world";
|
||||
direction.vector.y = -0.3;
|
||||
stage->setDirection(direction);
|
||||
@ -79,7 +79,7 @@ std::unique_ptr<SerialContainer> createModule(const std::string& group) {
|
||||
{ // rotate about TCP
|
||||
auto stage = std::make_unique<stages::MoveRelative>("rz +45°", cartesian);
|
||||
stage->properties().configureInitFrom(Stage::PARENT);
|
||||
geometry_msgs::TwistStamped twist;
|
||||
geometry_msgs::msg::TwistStamped twist;
|
||||
twist.header.frame_id = "world";
|
||||
twist.twist.angular.z = M_PI / 4.;
|
||||
stage->setDirection(twist);
|
||||
@ -95,8 +95,9 @@ std::unique_ptr<SerialContainer> createModule(const std::string& group) {
|
||||
return c;
|
||||
}
|
||||
|
||||
Task createTask() {
|
||||
Task createTask(const rclcpp::Node::SharedPtr& node) {
|
||||
Task t;
|
||||
t.loadRobotModel(node);
|
||||
t.stages()->setName("Reusable Containers");
|
||||
t.add(std::make_unique<stages::CurrentState>("current"));
|
||||
|
||||
@ -111,12 +112,11 @@ Task createTask() {
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
ros::init(argc, argv, "mtc_tutorial");
|
||||
// run an asynchronous spinner to communicate with the move_group node and rviz
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = rclcpp::Node::make_shared("mtc_tutorial");
|
||||
std::thread spinning_thread([node] { rclcpp::spin(node); });
|
||||
|
||||
auto task = createTask();
|
||||
auto task = createTask(node);
|
||||
try {
|
||||
if (task.plan())
|
||||
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;
|
||||
}
|
||||
|
||||
ros::waitForShutdown(); // keep alive for interactive inspection in rviz
|
||||
// keep alive for interactive inspection in rviz
|
||||
spinning_thread.join();
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -35,43 +35,44 @@
|
||||
*/
|
||||
|
||||
// ROS
|
||||
#include <ros/ros.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
// MTC pick/place demo implementation
|
||||
#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) {
|
||||
ros::init(argc, argv, "mtc_tutorial");
|
||||
ros::NodeHandle nh, pnh("~");
|
||||
rclcpp::init(argc, argv);
|
||||
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
|
||||
ros::AsyncSpinner spinner(1);
|
||||
spinner.start();
|
||||
|
||||
moveit_task_constructor_demo::setupDemoScene(pnh);
|
||||
moveit_task_constructor_demo::setupDemoScene(node);
|
||||
|
||||
// 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()) {
|
||||
ROS_INFO_NAMED(LOGNAME, "Initialization failed");
|
||||
RCLCPP_INFO(LOGGER, "Initialization failed");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (pick_place_task.plan()) {
|
||||
ROS_INFO_NAMED(LOGNAME, "Planning succeded");
|
||||
if (pnh.param("execute", false)) {
|
||||
RCLCPP_INFO(LOGGER, "Planning succeded");
|
||||
if (bool execute = false; rosparam_shortcuts::get(node, "execute", execute) && execute) {
|
||||
pick_place_task.execute();
|
||||
ROS_INFO_NAMED(LOGNAME, "Execution complete");
|
||||
RCLCPP_INFO(LOGGER, "Execution complete");
|
||||
} else {
|
||||
ROS_INFO_NAMED(LOGNAME, "Execution disabled");
|
||||
RCLCPP_INFO(LOGGER, "Execution disabled");
|
||||
}
|
||||
} else {
|
||||
ROS_INFO_NAMED(LOGNAME, "Planning failed");
|
||||
RCLCPP_INFO(LOGGER, "Planning failed");
|
||||
}
|
||||
|
||||
// Keep introspection alive
|
||||
ros::waitForShutdown();
|
||||
spinning_thread.join();
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -37,71 +37,71 @@
|
||||
#include <moveit_task_constructor_demo/pick_place_task.h>
|
||||
#include <rosparam_shortcuts/rosparam_shortcuts.h>
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo");
|
||||
|
||||
namespace moveit_task_constructor_demo {
|
||||
|
||||
constexpr char LOGNAME[] = "moveit_task_constructor_demo";
|
||||
constexpr char PickPlaceTask::LOGNAME[];
|
||||
|
||||
void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) {
|
||||
void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi,
|
||||
const moveit_msgs::msg::CollisionObject& object) {
|
||||
if (!psi.applyCollisionObject(object))
|
||||
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::vector<double> table_dimensions;
|
||||
geometry_msgs::Pose pose;
|
||||
geometry_msgs::msg::Pose pose;
|
||||
std::size_t errors = 0;
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose);
|
||||
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
|
||||
errors += !rosparam_shortcuts::get(node, "table_name", table_name);
|
||||
errors += !rosparam_shortcuts::get(node, "table_reference_frame", table_reference_frame);
|
||||
errors += !rosparam_shortcuts::get(node, "table_dimensions", table_dimensions);
|
||||
errors += !rosparam_shortcuts::get(node, "table_pose", pose);
|
||||
rosparam_shortcuts::shutdownIfError(errors);
|
||||
|
||||
moveit_msgs::CollisionObject object;
|
||||
moveit_msgs::msg::CollisionObject object;
|
||||
object.id = table_name;
|
||||
object.header.frame_id = table_reference_frame;
|
||||
object.primitives.resize(1);
|
||||
object.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
|
||||
object.primitives[0].dimensions = table_dimensions;
|
||||
object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX;
|
||||
object.primitives[0].dimensions = { table_dimensions.at(0), table_dimensions.at(1), table_dimensions.at(2) };
|
||||
pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world
|
||||
object.primitive_poses.push_back(pose);
|
||||
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::vector<double> object_dimensions;
|
||||
geometry_msgs::Pose pose;
|
||||
geometry_msgs::msg::Pose pose;
|
||||
std::size_t error = 0;
|
||||
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name);
|
||||
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame);
|
||||
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions);
|
||||
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose);
|
||||
rosparam_shortcuts::shutdownIfError(LOGNAME, error);
|
||||
error += !rosparam_shortcuts::get(node, "object_name", object_name);
|
||||
error += !rosparam_shortcuts::get(node, "object_reference_frame", object_reference_frame);
|
||||
error += !rosparam_shortcuts::get(node, "object_dimensions", object_dimensions);
|
||||
error += !rosparam_shortcuts::get(node, "object_pose", pose);
|
||||
rosparam_shortcuts::shutdownIfError(error);
|
||||
|
||||
moveit_msgs::CollisionObject object;
|
||||
moveit_msgs::msg::CollisionObject object;
|
||||
object.id = object_name;
|
||||
object.header.frame_id = object_reference_frame;
|
||||
object.primitives.resize(1);
|
||||
object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
|
||||
object.primitives[0].dimensions = object_dimensions;
|
||||
object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;
|
||||
object.primitives[0].dimensions = { object_dimensions.at(0), object_dimensions.at(1) };
|
||||
pose.position.z += 0.5 * object_dimensions[0];
|
||||
object.primitive_poses.push_back(pose);
|
||||
return object;
|
||||
}
|
||||
|
||||
void setupDemoScene(ros::NodeHandle& pnh) {
|
||||
void setupDemoScene(const rclcpp::Node::SharedPtr& node) {
|
||||
// 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;
|
||||
if (pnh.param("spawn_table", true))
|
||||
spawnObject(psi, createTable(pnh));
|
||||
spawnObject(psi, createObject(pnh));
|
||||
if (bool spawn_table = true; rosparam_shortcuts::get(node, "spawn_table", spawn_table) && spawn_table)
|
||||
spawnObject(psi, createTable(node));
|
||||
spawnObject(psi, createObject(node));
|
||||
}
|
||||
|
||||
PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh)
|
||||
: pnh_(pnh), task_name_(task_name) {
|
||||
PickPlaceTask::PickPlaceTask(const std::string& task_name, const rclcpp::Node::SharedPtr& node)
|
||||
: node_(node), task_name_(task_name) {
|
||||
loadParameters();
|
||||
}
|
||||
|
||||
@ -111,41 +111,41 @@ void PickPlaceTask::loadParameters() {
|
||||
* Load Parameters *
|
||||
* *
|
||||
***************************************************/
|
||||
ROS_INFO_NAMED(LOGNAME, "Loading task parameters");
|
||||
RCLCPP_INFO(LOGGER, "Loading task parameters");
|
||||
|
||||
// Planning group properties
|
||||
size_t errors = 0;
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_group_name", arm_group_name_);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_group_name", hand_group_name_);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "eef_name", eef_name_);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_frame", hand_frame_);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "world_frame", world_frame_);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "grasp_frame_transform", grasp_frame_transform_);
|
||||
errors += !rosparam_shortcuts::get(node_, "arm_group_name", arm_group_name_);
|
||||
errors += !rosparam_shortcuts::get(node_, "hand_group_name", hand_group_name_);
|
||||
errors += !rosparam_shortcuts::get(node_, "eef_name", eef_name_);
|
||||
errors += !rosparam_shortcuts::get(node_, "hand_frame", hand_frame_);
|
||||
errors += !rosparam_shortcuts::get(node_, "world_frame", world_frame_);
|
||||
errors += !rosparam_shortcuts::get(node_, "grasp_frame_transform", grasp_frame_transform_);
|
||||
|
||||
// Predefined pose targets
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_open_pose", hand_open_pose_);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_close_pose", hand_close_pose_);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_home_pose", arm_home_pose_);
|
||||
errors += !rosparam_shortcuts::get(node_, "hand_open_pose", hand_open_pose_);
|
||||
errors += !rosparam_shortcuts::get(node_, "hand_close_pose", hand_close_pose_);
|
||||
errors += !rosparam_shortcuts::get(node_, "arm_home_pose", arm_home_pose_);
|
||||
|
||||
// Target object
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_name", object_name_);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_dimensions", object_dimensions_);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_reference_frame", object_reference_frame_);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "surface_link", surface_link_);
|
||||
errors += !rosparam_shortcuts::get(node_, "object_name", object_name_);
|
||||
errors += !rosparam_shortcuts::get(node_, "object_dimensions", object_dimensions_);
|
||||
errors += !rosparam_shortcuts::get(node_, "object_reference_frame", object_reference_frame_);
|
||||
errors += !rosparam_shortcuts::get(node_, "surface_link", surface_link_);
|
||||
support_surfaces_ = { surface_link_ };
|
||||
|
||||
// Pick/Place metrics
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "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(LOGNAME, pnh_, "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(LOGNAME, pnh_, "place_surface_offset", place_surface_offset_);
|
||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_pose", place_pose_);
|
||||
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
|
||||
errors += !rosparam_shortcuts::get(node_, "approach_object_min_dist", approach_object_min_dist_);
|
||||
errors += !rosparam_shortcuts::get(node_, "approach_object_max_dist", approach_object_max_dist_);
|
||||
errors += !rosparam_shortcuts::get(node_, "lift_object_min_dist", lift_object_min_dist_);
|
||||
errors += !rosparam_shortcuts::get(node_, "lift_object_max_dist", lift_object_max_dist_);
|
||||
errors += !rosparam_shortcuts::get(node_, "place_surface_offset", place_surface_offset_);
|
||||
errors += !rosparam_shortcuts::get(node_, "place_pose", place_pose_);
|
||||
rosparam_shortcuts::shutdownIfError(errors);
|
||||
}
|
||||
|
||||
bool PickPlaceTask::init() {
|
||||
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
|
||||
RCLCPP_INFO(LOGGER, "Initializing task pipeline");
|
||||
const std::string object = object_name_;
|
||||
|
||||
// Reset ROS introspection before constructing the new object
|
||||
@ -155,10 +155,10 @@ bool PickPlaceTask::init() {
|
||||
|
||||
Task& t = *task_;
|
||||
t.stages()->setName(task_name_);
|
||||
t.loadRobotModel();
|
||||
t.loadRobotModel(node_);
|
||||
|
||||
// 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);
|
||||
|
||||
// Cartesian planner
|
||||
@ -245,7 +245,7 @@ bool PickPlaceTask::init() {
|
||||
stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_);
|
||||
|
||||
// Set hand forward direction
|
||||
geometry_msgs::Vector3Stamped vec;
|
||||
geometry_msgs::msg::Vector3Stamped vec;
|
||||
vec.header.frame_id = hand_frame_;
|
||||
vec.vector.z = 1.0;
|
||||
stage->setDirection(vec);
|
||||
@ -326,7 +326,7 @@ bool PickPlaceTask::init() {
|
||||
stage->properties().set("marker_ns", "lift_object");
|
||||
|
||||
// Set upward direction
|
||||
geometry_msgs::Vector3Stamped vec;
|
||||
geometry_msgs::msg::Vector3Stamped vec;
|
||||
vec.header.frame_id = world_frame_;
|
||||
vec.vector.z = 1.0;
|
||||
stage->setDirection(vec);
|
||||
@ -380,7 +380,7 @@ bool PickPlaceTask::init() {
|
||||
stage->setMinMaxDistance(.03, .13);
|
||||
|
||||
// Set downward direction
|
||||
geometry_msgs::Vector3Stamped vec;
|
||||
geometry_msgs::msg::Vector3Stamped vec;
|
||||
vec.header.frame_id = world_frame_;
|
||||
vec.vector.z = -1.0;
|
||||
stage->setDirection(vec);
|
||||
@ -398,7 +398,7 @@ bool PickPlaceTask::init() {
|
||||
stage->setObject(object);
|
||||
|
||||
// Set target pose
|
||||
geometry_msgs::PoseStamped p;
|
||||
geometry_msgs::msg::PoseStamped p;
|
||||
p.header.frame_id = object_reference_frame_;
|
||||
p.pose = place_pose_;
|
||||
p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_;
|
||||
@ -451,7 +451,7 @@ bool PickPlaceTask::init() {
|
||||
stage->setMinMaxDistance(.12, .25);
|
||||
stage->setIKFrame(hand_frame_);
|
||||
stage->properties().set("marker_ns", "retreat");
|
||||
geometry_msgs::Vector3Stamped vec;
|
||||
geometry_msgs::msg::Vector3Stamped vec;
|
||||
vec.header.frame_id = hand_frame_;
|
||||
vec.vector.z = -1.0;
|
||||
stage->setDirection(vec);
|
||||
@ -479,7 +479,7 @@ bool PickPlaceTask::init() {
|
||||
try {
|
||||
t.init();
|
||||
} catch (InitStageException& e) {
|
||||
ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e);
|
||||
RCLCPP_ERROR_STREAM(LOGGER, "Initialization failed: " << e);
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -487,27 +487,28 @@ bool PickPlaceTask::init() {
|
||||
}
|
||||
|
||||
bool PickPlaceTask::plan() {
|
||||
ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
|
||||
int max_solutions = pnh_.param<int>("max_solutions", 10);
|
||||
RCLCPP_INFO(LOGGER, "Start searching for task solutions");
|
||||
int max_solutions = 10;
|
||||
rosparam_shortcuts::get(node_, "max_solutions", max_solutions);
|
||||
|
||||
return task_->plan(max_solutions);
|
||||
}
|
||||
|
||||
bool PickPlaceTask::execute() {
|
||||
ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory");
|
||||
moveit_msgs::MoveItErrorCodes execute_result;
|
||||
RCLCPP_INFO(LOGGER, "Executing solution trajectory");
|
||||
moveit_msgs::msg::MoveItErrorCodes execute_result;
|
||||
|
||||
execute_result = task_->execute(*task_->solutions().front());
|
||||
// // 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();
|
||||
// moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal;
|
||||
// moveit_task_constructor_msgs::msg::ExecuteTaskSolutionGoal execute_goal;
|
||||
// task_->solutions().front()->fillMessage(execute_goal.solution);
|
||||
// execute.sendGoalAndWait(execute_goal);
|
||||
// execute_result = execute.getResult()->error_code;
|
||||
|
||||
if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) {
|
||||
ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_result.val);
|
||||
if (execute_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
|
||||
RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed and returned: " << execute_result.val);
|
||||
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)
|
||||
|
||||
set(MSG_DEPS moveit_msgs visualization_msgs)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
message_generation
|
||||
${MSG_DEPS}
|
||||
)
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(moveit_msgs REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
|
||||
# ROS messages, services and actions
|
||||
add_message_files(DIRECTORY msg FILES
|
||||
Property.msg
|
||||
Solution.msg
|
||||
SolutionInfo.msg
|
||||
StageDescription.msg
|
||||
StageStatistics.msg
|
||||
SubSolution.msg
|
||||
SubTrajectory.msg
|
||||
TaskDescription.msg
|
||||
TaskStatistics.msg
|
||||
set(msg_files
|
||||
"msg/Property.msg"
|
||||
"msg/Solution.msg"
|
||||
"msg/SolutionInfo.msg"
|
||||
"msg/StageDescription.msg"
|
||||
"msg/StageStatistics.msg"
|
||||
"msg/SubSolution.msg"
|
||||
"msg/SubTrajectory.msg"
|
||||
"msg/TaskDescription.msg"
|
||||
"msg/TaskStatistics.msg"
|
||||
)
|
||||
|
||||
add_service_files(DIRECTORY srv FILES
|
||||
GetSolution.srv
|
||||
set(srv_files
|
||||
"srv/GetSolution.srv"
|
||||
)
|
||||
|
||||
add_action_files(DIRECTORY action FILES
|
||||
ExecuteTaskSolution.action
|
||||
set(action_files
|
||||
"action/ExecuteTaskSolution.action"
|
||||
)
|
||||
|
||||
generate_messages(DEPENDENCIES ${MSG_DEPS})
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS
|
||||
message_runtime
|
||||
${MSG_DEPS}
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
${msg_files}
|
||||
${srv_files}
|
||||
${action_files}
|
||||
DEPENDENCIES
|
||||
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