mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
a move_group capability to execute Task Solutions
This does not work perfectly yet in practice, but this is (probably) due to problems in upstream MoveIt.
This commit is contained in:
parent
7bd9bcf972
commit
73b7475cdb
37
capabilities/CMakeLists.txt
Normal file
37
capabilities/CMakeLists.txt
Normal file
@ -0,0 +1,37 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(moveit_task_constructor_capabilities)
|
||||
|
||||
add_compile_options(-std=c++14)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
actionlib
|
||||
moveit_core
|
||||
moveit_ros_move_group
|
||||
moveit_task_constructor_msgs
|
||||
pluginlib
|
||||
std_msgs
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
LIBRARIES $PROJECT_NAME}
|
||||
CATKIN_DEPENDS
|
||||
std_msgs
|
||||
)
|
||||
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME}
|
||||
src/execute_task_solution_capability.cpp
|
||||
)
|
||||
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
)
|
||||
|
||||
install(FILES capabilities_plugin_description.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
7
capabilities/capabilities_plugin_description.xml
Normal file
7
capabilities/capabilities_plugin_description.xml
Normal file
@ -0,0 +1,7 @@
|
||||
<library path="libmoveit_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.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
29
capabilities/package.xml
Normal file
29
capabilities/package.xml
Normal file
@ -0,0 +1,29 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>moveit_task_constructor_capabilities</name>
|
||||
<version>0.0.0</version>
|
||||
<description>
|
||||
MoveGroupCapabilites to interact with MoveIt
|
||||
</description>
|
||||
|
||||
<maintainer email="me@v4hn.de">Michael v4hn Goerner</maintainer>
|
||||
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>moveit_core</build_depend>
|
||||
<build_depend>moveit_ros_move_group</build_depend>
|
||||
<build_depend>pluginlib</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
|
||||
<run_depend>moveit_core</run_depend>
|
||||
<run_depend>moveit_ros_move_group</run_depend>
|
||||
<run_depend>pluginlib</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
|
||||
<export>
|
||||
<moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/>
|
||||
</export>
|
||||
</package>
|
186
capabilities/src/execute_task_solution_capability.cpp
Normal file
186
capabilities/src/execute_task_solution_capability.cpp
Normal file
@ -0,0 +1,186 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016, Kentaro Wada.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Author: Michael 'v4hn' Goerner */
|
||||
|
||||
#include "execute_task_solution_capability.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>
|
||||
|
||||
namespace {
|
||||
|
||||
// TODO: move to moveit::core::RobotModel
|
||||
const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::RobotModel& model, const std::vector<std::string>& joints) {
|
||||
std::set<std::string> joint_set(joints.begin(), joints.end());
|
||||
|
||||
const std::vector<const moveit::core::JointModelGroup*>& jmgs = model.getJointModelGroups();
|
||||
|
||||
for(const moveit::core::JointModelGroup* jmg : jmgs){
|
||||
const std::vector<std::string>& jmg_joints = jmg->getJointModelNames();
|
||||
std::set<std::string> jmg_joint_set(jmg_joints.begin(), jmg_joints.end());
|
||||
|
||||
// return group if sets agree on all active joints
|
||||
if(std::includes(jmg_joint_set.begin(), jmg_joint_set.end(), joint_set.begin(), joint_set.end())) {
|
||||
std::set<std::string> difference;
|
||||
std::set_difference(jmg_joint_set.begin(), jmg_joint_set.end(),
|
||||
joint_set.begin(), joint_set.end(),
|
||||
std::inserter(difference, difference.begin()));
|
||||
for(const std::string& diff_joint : difference){
|
||||
const moveit::core::JointModel* diff_jm = model.getJointModel(diff_joint);
|
||||
if(!diff_jm->isPassive() && !diff_jm->getMimic())
|
||||
continue;
|
||||
}
|
||||
return jmg;
|
||||
}
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
void ExecuteTaskSolutionCapability::goalCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal) {
|
||||
moveit_task_constructor_msgs::ExecuteTaskSolutionResult result;
|
||||
|
||||
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);
|
||||
return;
|
||||
}
|
||||
|
||||
plan_execution::ExecutableMotionPlan plan;
|
||||
if(!constructMotionPlan(goal->task_solution, plan))
|
||||
result.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
||||
else {
|
||||
ROS_INFO_NAMED("ExecuteTaskSolution", "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);
|
||||
else
|
||||
as_->setAborted(result, response);
|
||||
}
|
||||
|
||||
void ExecuteTaskSolutionCapability::preemptCallback() {
|
||||
if(context_->plan_execution_)
|
||||
context_->plan_execution_->stop();
|
||||
}
|
||||
|
||||
bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, plan_execution::ExecutableMotionPlan& plan) {
|
||||
robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
|
||||
|
||||
robot_state::RobotState state(model);
|
||||
{
|
||||
planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_);
|
||||
state= scene->getCurrentState();
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
plan.plan_components_.emplace_back();
|
||||
plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back();
|
||||
|
||||
exec_traj.description_ = std::to_string(i+1) + "/"
|
||||
+ std::to_string(solution.sub_trajectory.size())
|
||||
+ " - subsolution " + std::to_string(sub_traj.id)
|
||||
+ " of stage " + std::to_string(sub_traj.stage_id);
|
||||
|
||||
const moveit::core::JointModelGroup* group = nullptr;
|
||||
{
|
||||
std::vector<std::string> joint_names(sub_traj.trajectory.joint_trajectory.joint_names);
|
||||
joint_names.insert(joint_names.end(),
|
||||
sub_traj.trajectory.multi_dof_joint_trajectory.joint_names.begin(),
|
||||
sub_traj.trajectory.multi_dof_joint_trajectory.joint_names.end());
|
||||
group = findJointModelGroup(*model, joint_names);
|
||||
if(!group){
|
||||
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {" << boost::algorithm::join(joint_names, ", ") << "}" );
|
||||
return false;
|
||||
}
|
||||
}
|
||||
exec_traj.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(model, group);
|
||||
exec_traj.trajectory_->setRobotTrajectoryMsg(state, sub_traj.trajectory);
|
||||
|
||||
/* TODO add action feedback and markers */
|
||||
exec_traj.effect_on_success_ = [this,sub_traj](const plan_execution::ExecutableMotionPlan*){
|
||||
if(!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)){
|
||||
planning_scene_monitor::LockedPlanningSceneRW scene(context_->planning_scene_monitor_);
|
||||
return scene->usePlanningSceneMsg(sub_traj.scene_diff);
|
||||
}
|
||||
return true;
|
||||
};
|
||||
|
||||
if(!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&
|
||||
!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 " << exec_traj.description_);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace move_group
|
||||
|
||||
#include <class_loader/class_loader.hpp>
|
||||
CLASS_LOADER_REGISTER_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability)
|
70
capabilities/src/execute_task_solution_capability.h
Normal file
70
capabilities/src/execute_task_solution_capability.h
Normal file
@ -0,0 +1,70 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Hamburg University.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Hamburg University nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/*
|
||||
* Capability to execute trajectories through a ROS action
|
||||
* that were generated by the MoveIt Task Constructor.
|
||||
*
|
||||
* Author: Michael 'v4hn' Goerner
|
||||
* */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <moveit/move_group/move_group_capability.h>
|
||||
#include <actionlib/server/simple_action_server.h>
|
||||
|
||||
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
|
||||
|
||||
#include <memory>
|
||||
|
||||
namespace move_group
|
||||
{
|
||||
|
||||
class ExecuteTaskSolutionCapability : public MoveGroupCapability
|
||||
{
|
||||
public:
|
||||
ExecuteTaskSolutionCapability();
|
||||
|
||||
virtual void initialize();
|
||||
|
||||
private:
|
||||
bool constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, plan_execution::ExecutableMotionPlan& plan);
|
||||
|
||||
void goalCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal);
|
||||
void preemptCallback();
|
||||
|
||||
std::unique_ptr<actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>> as_;
|
||||
};
|
||||
|
||||
} // namespace move_group
|
@ -24,6 +24,10 @@ add_service_files(DIRECTORY srv FILES
|
||||
GetSolution.srv
|
||||
)
|
||||
|
||||
add_action_files(DIRECTORY action FILES
|
||||
ExecuteTaskSolution.action
|
||||
)
|
||||
|
||||
generate_messages(DEPENDENCIES ${MSG_DEPS})
|
||||
|
||||
catkin_package(
|
||||
|
12
msgs/action/ExecuteTaskSolution.action
Normal file
12
msgs/action/ExecuteTaskSolution.action
Normal file
@ -0,0 +1,12 @@
|
||||
# Task solution to execute
|
||||
Solution task_solution
|
||||
|
||||
---
|
||||
|
||||
# result of execution
|
||||
moveit_msgs/MoveItErrorCodes error_code
|
||||
|
||||
---
|
||||
|
||||
# current stage/subtrajectory during execution
|
||||
string state
|
Loading…
Reference in New Issue
Block a user