mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Port capabilities to ROS2
This commit is contained in:
parent
f21c8ccd35
commit
d7ceaa01dd
@ -1,40 +1,38 @@
|
||||
cmake_minimum_required(VERSION 3.1.3)
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(moveit_task_constructor_capabilities)
|
||||
|
||||
add_compile_options(-std=c++14)
|
||||
add_compile_options(-std=c++17)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
actionlib
|
||||
moveit_core
|
||||
moveit_ros_move_group
|
||||
moveit_task_constructor_core
|
||||
moveit_task_constructor_msgs
|
||||
pluginlib
|
||||
std_msgs
|
||||
)
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
catkin_package(
|
||||
LIBRARIES $PROJECT_NAME}
|
||||
CATKIN_DEPENDS
|
||||
actionlib
|
||||
moveit_task_constructor_msgs
|
||||
std_msgs
|
||||
)
|
||||
find_package(Boost REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(moveit_core REQUIRED)
|
||||
find_package(moveit_ros_move_group REQUIRED)
|
||||
find_package(moveit_task_constructor_msgs REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME}
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
src/execute_task_solution_capability.cpp
|
||||
)
|
||||
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||
ament_target_dependencies(${PROJECT_NAME}
|
||||
Boost
|
||||
rclcpp_action
|
||||
moveit_core
|
||||
moveit_ros_move_group
|
||||
moveit_task_constructor_msgs
|
||||
)
|
||||
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION lib
|
||||
)
|
||||
|
||||
install(FILES capabilities_plugin_description.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
pluginlib_export_plugin_description_file(moveit_ros_move_group capabilities_plugin_description.xml)
|
||||
ament_export_dependencies(rclcpp_action)
|
||||
ament_export_dependencies(moveit_core)
|
||||
ament_export_dependencies(moveit_ros_move_group)
|
||||
ament_export_dependencies(moveit_task_constructor_msgs)
|
||||
ament_export_dependencies(pluginlib)
|
||||
ament_export_dependencies(std_msgs)
|
||||
ament_package()
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
<library path="libmoveit_task_constructor_capabilities">
|
||||
<library path="moveit_task_constructor_capabilities">
|
||||
<class name="move_group/ExecuteTaskSolutionCapability" type="move_group::ExecuteTaskSolutionCapability" base_class_type="move_group::MoveGroupCapability">
|
||||
<description>
|
||||
Action server to execute solutions generated through the MoveIt Task Constructor.
|
||||
|
||||
@ -10,17 +10,18 @@
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>moveit_core</depend>
|
||||
<depend>moveit_ros_move_group</depend>
|
||||
<depend>actionlib</depend>
|
||||
<depend>moveit_ros_planning</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>moveit_task_constructor_core</depend>
|
||||
<depend>moveit_task_constructor_msgs</depend>
|
||||
|
||||
<export>
|
||||
<moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@ -36,16 +36,16 @@
|
||||
|
||||
#include "execute_task_solution_capability.h"
|
||||
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/moveit_cpp/moveit_cpp.h>
|
||||
#include <moveit/plan_execution/plan_execution.h>
|
||||
#include <moveit/trajectory_processing/trajectory_tools.h>
|
||||
#include <moveit/kinematic_constraints/utils.h>
|
||||
#include <moveit/move_group/capability_names.h>
|
||||
#include <moveit/robot_state/conversions.h>
|
||||
#if MOVEIT_HAS_MESSAGE_CHECKS
|
||||
#include <moveit/utils/message_checks.h>
|
||||
#endif
|
||||
#include <moveit/moveit_cpp/moveit_cpp.h>
|
||||
|
||||
#include <boost/algorithm/string/join.hpp>
|
||||
|
||||
namespace {
|
||||
|
||||
@ -80,58 +80,63 @@ const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::Rob
|
||||
}
|
||||
} // namespace
|
||||
|
||||
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.execute_task_solution");
|
||||
|
||||
namespace move_group {
|
||||
|
||||
ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability() : MoveGroupCapability("ExecuteTaskSolution") {}
|
||||
|
||||
void ExecuteTaskSolutionCapability::initialize() {
|
||||
// configure the action server
|
||||
as_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>(
|
||||
root_node_handle_, "execute_task_solution",
|
||||
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1), false));
|
||||
as_->registerPreemptCallback(std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this));
|
||||
as_->start();
|
||||
as_ = rclcpp_action::create_server<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
|
||||
context_->moveit_cpp_->getNode(), "execute_task_solution",
|
||||
ActionServerType::GoalCallback(std::bind(&ExecuteTaskSolutionCapability::handleNewGoal, this,
|
||||
std::placeholders::_1, std::placeholders::_2)),
|
||||
ActionServerType::CancelCallback(
|
||||
std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)),
|
||||
ActionServerType::AcceptedCallback(
|
||||
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1)));
|
||||
}
|
||||
|
||||
void ExecuteTaskSolutionCapability::goalCallback(
|
||||
const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal) {
|
||||
moveit_task_constructor_msgs::ExecuteTaskSolutionResult result;
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle) {
|
||||
auto result = std::make_shared<moveit_task_constructor_msgs::action::ExecuteTaskSolution::Result>();
|
||||
|
||||
const auto& goal = goal_handle->get_goal();
|
||||
if (!context_->plan_execution_) {
|
||||
const std::string response = "Cannot execute solution. ~allow_trajectory_execution was set to false";
|
||||
result.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
|
||||
as_->setAborted(result, response);
|
||||
result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED;
|
||||
goal_handle->abort(result);
|
||||
return;
|
||||
}
|
||||
|
||||
plan_execution::ExecutableMotionPlan plan;
|
||||
if (!constructMotionPlan(goal->solution, plan))
|
||||
result.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
||||
result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
||||
else {
|
||||
ROS_INFO_NAMED("ExecuteTaskSolution", "Executing TaskSolution");
|
||||
result.error_code = context_->plan_execution_->executeAndMonitor(plan);
|
||||
RCLCPP_INFO(LOGGER, "Executing TaskSolution");
|
||||
result->error_code = context_->plan_execution_->executeAndMonitor(plan);
|
||||
}
|
||||
|
||||
const std::string response = context_->plan_execution_->getErrorCodeString(result.error_code);
|
||||
|
||||
if (result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
|
||||
as_->setSucceeded(result, response);
|
||||
else if (result.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
|
||||
as_->setPreempted(result, response);
|
||||
if (result->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
|
||||
goal_handle->succeed(result);
|
||||
else if (result->error_code.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED)
|
||||
goal_handle->canceled(result);
|
||||
else
|
||||
as_->setAborted(result, response);
|
||||
goal_handle->abort(result);
|
||||
}
|
||||
|
||||
void ExecuteTaskSolutionCapability::preemptCallback() {
|
||||
rclcpp_action::CancelResponse ExecuteTaskSolutionCapability::preemptCallback(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle) {
|
||||
if (context_->plan_execution_)
|
||||
context_->plan_execution_->stop();
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution,
|
||||
bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::msg::Solution& solution,
|
||||
plan_execution::ExecutableMotionPlan& plan) {
|
||||
robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
|
||||
moveit::core::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
|
||||
|
||||
robot_state::RobotState state(model);
|
||||
moveit::core::RobotState state(model);
|
||||
{
|
||||
planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_);
|
||||
state = scene->getCurrentState();
|
||||
@ -139,7 +144,7 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
|
||||
|
||||
plan.plan_components_.reserve(solution.sub_trajectory.size());
|
||||
for (size_t i = 0; i < solution.sub_trajectory.size(); ++i) {
|
||||
const moveit_task_constructor_msgs::SubTrajectory& sub_traj = solution.sub_trajectory[i];
|
||||
const moveit_task_constructor_msgs::msg::SubTrajectory& sub_traj = solution.sub_trajectory[i];
|
||||
|
||||
plan.plan_components_.emplace_back();
|
||||
plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back();
|
||||
@ -156,12 +161,11 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
|
||||
if (!joint_names.empty()) {
|
||||
group = findJointModelGroup(*model, joint_names);
|
||||
if (!group) {
|
||||
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {"
|
||||
<< 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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user