ROS 2 Migration (#170)

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

View File

@ -14,32 +14,36 @@ jobs:
fail-fast: false
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

View File

@ -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

View File

@ -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
View File

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

View File

@ -1,40 +1,38 @@
cmake_minimum_required(VERSION 3.1.3)
cmake_minimum_required(VERSION 3.5)
project(moveit_task_constructor_capabilities)
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()

View File

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

View File

@ -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>

View File

@ -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;
}
}

View File

@ -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

View File

@ -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()

View File

@ -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

View File

@ -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) */

View File

@ -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

View File

@ -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();
}

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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);

View File

@ -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 = "") {

View File

@ -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));
}

View File

@ -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:

View File

@ -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;

View File

@ -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;

View File

@ -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;

View File

@ -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);

View File

@ -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); }

View File

@ -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));
}

View File

@ -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 {

View File

@ -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); }

View File

@ -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) {

View File

@ -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;

View File

@ -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;

View File

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

View File

@ -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>

View File

@ -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)

View File

@ -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;
}

View File

@ -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;

View File

@ -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);

View File

@ -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()

View File

@ -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

View File

@ -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;

View File

@ -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);

View File

@ -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();

View File

@ -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);

View File

@ -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
}
}

View File

@ -1,4 +1,4 @@
add_library(${PROJECT_NAME}_stages
add_library(${PROJECT_NAME}_stages SHARED
${PROJECT_INCLUDE}/stages/modify_planning_scene.h
${PROJECT_INCLUDE}/stages/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)

View File

@ -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();

View File

@ -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);

View File

@ -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

View File

@ -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)));

View File

@ -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;
}

View File

@ -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;

View File

@ -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);

View File

@ -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;
}

View File

@ -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

View File

@ -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);

View File

@ -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);

View File

@ -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 {

View File

@ -26,7 +26,7 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
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);

View File

@ -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

View File

@ -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);

View File

@ -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));

View File

@ -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) {

View File

@ -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;
}

View File

@ -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()

View File

@ -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();
}

View File

@ -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
View File

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

View File

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

View File

@ -13,14 +13,14 @@
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/stages/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();
}

View File

@ -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();
}

View File

@ -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();
}

View File

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

View File

@ -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>

View File

@ -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;
}
}

View File

@ -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();
}

View File

@ -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";
}

View File

@ -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()

View File

@ -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

View File

@ -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: 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
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1621
X: 249
Y: 25
Width: 1920
X: 0
Y: 0

View File

@ -1,3 +1,5 @@
/**:
ros__parameters:
# Total planning attempts
max_solutions: 10
@ -22,19 +24,19 @@ surface_link: "table"
# 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]
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_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]
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_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

View File

@ -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

View File

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

View File

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

View File

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

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

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

View File

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

View File

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

View File

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

View File

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

View File

@ -10,18 +10,15 @@
<license>BSD</license>
<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>

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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
View File

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

View File

@ -1,38 +1,43 @@
cmake_minimum_required(VERSION 3.1.3)
cmake_minimum_required(VERSION 3.5)
project(moveit_task_constructor_msgs)
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