Port capabilities to ROS2

This commit is contained in:
JafarAbdi 2021-11-23 16:59:36 +03:00
parent f21c8ccd35
commit d7ceaa01dd
5 changed files with 87 additions and 83 deletions

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 {"
<< boost::algorithm::join(joint_names, ", ") << "}");
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