execute_trajectory_action_capability.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2016, Kentaro Wada.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Kentaro Wada */
00036 
00037 #include "execute_trajectory_action_capability.h"
00038 
00039 #include <moveit/plan_execution/plan_execution.h>
00040 #include <moveit/trajectory_processing/trajectory_tools.h>
00041 #include <moveit/kinematic_constraints/utils.h>
00042 #include <moveit/move_group/capability_names.h>
00043 
00044 namespace move_group
00045 {
00046 MoveGroupExecuteTrajectoryAction::MoveGroupExecuteTrajectoryAction() : MoveGroupCapability("ExecuteTrajectoryAction")
00047 {
00048 }
00049 
00050 void MoveGroupExecuteTrajectoryAction::initialize()
00051 {
00052   // start the move action server
00053   execute_action_server_.reset(new actionlib::SimpleActionServer<moveit_msgs::ExecuteTrajectoryAction>(
00054       root_node_handle_, EXECUTE_ACTION_NAME,
00055       boost::bind(&MoveGroupExecuteTrajectoryAction::executePathCallback, this, _1), false));
00056   execute_action_server_->registerPreemptCallback(
00057       boost::bind(&MoveGroupExecuteTrajectoryAction::preemptExecuteTrajectoryCallback, this));
00058   execute_action_server_->start();
00059 }
00060 
00061 void MoveGroupExecuteTrajectoryAction::executePathCallback(const moveit_msgs::ExecuteTrajectoryGoalConstPtr& goal)
00062 {
00063   moveit_msgs::ExecuteTrajectoryResult action_res;
00064   if (!context_->trajectory_execution_manager_)
00065   {
00066     std::string response = "Cannot execute trajectory since ~allow_trajectory_execution was set to false";
00067     action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
00068     execute_action_server_->setAborted(action_res, response);
00069     return;
00070   }
00071 
00072   executePathCallback_Execute(goal, action_res);
00073 
00074   std::string response = getActionResultString(action_res.error_code, false, false);
00075   if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00076   {
00077     execute_action_server_->setSucceeded(action_res, response);
00078   }
00079   else if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
00080   {
00081     execute_action_server_->setPreempted(action_res, response);
00082   }
00083   else
00084   {
00085     execute_action_server_->setAborted(action_res, response);
00086   }
00087 
00088   setExecuteTrajectoryState(IDLE);
00089 }
00090 
00091 void MoveGroupExecuteTrajectoryAction::executePathCallback_Execute(
00092     const moveit_msgs::ExecuteTrajectoryGoalConstPtr& goal, moveit_msgs::ExecuteTrajectoryResult& action_res)
00093 {
00094   ROS_INFO_NAMED("move_group", "Execution request received for ExecuteTrajectory action.");
00095 
00096   context_->trajectory_execution_manager_->clear();
00097   if (context_->trajectory_execution_manager_->push(goal->trajectory))
00098   {
00099     setExecuteTrajectoryState(MONITOR);
00100     context_->trajectory_execution_manager_->execute();
00101     moveit_controller_manager::ExecutionStatus es = context_->trajectory_execution_manager_->waitForExecution();
00102     if (es == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
00103     {
00104       action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00105     }
00106     else if (es == moveit_controller_manager::ExecutionStatus::PREEMPTED)
00107     {
00108       action_res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
00109     }
00110     else if (es == moveit_controller_manager::ExecutionStatus::TIMED_OUT)
00111     {
00112       action_res.error_code.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
00113     }
00114     else
00115     {
00116       action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
00117     }
00118     ROS_INFO_STREAM_NAMED("move_group", "Execution completed: " << es.asString());
00119   }
00120   else
00121   {
00122     action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
00123   }
00124 }
00125 
00126 void MoveGroupExecuteTrajectoryAction::preemptExecuteTrajectoryCallback()
00127 {
00128   context_->trajectory_execution_manager_->stopExecution(true);
00129 }
00130 
00131 void MoveGroupExecuteTrajectoryAction::setExecuteTrajectoryState(MoveGroupState state)
00132 {
00133   moveit_msgs::ExecuteTrajectoryFeedback execute_feedback;
00134   execute_feedback.state = stateToStr(state);
00135   execute_action_server_->publishFeedback(execute_feedback);
00136 }
00137 
00138 }  // namespace move_group
00139 
00140 #include <class_loader/class_loader.h>
00141 CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupExecuteTrajectoryAction, move_group::MoveGroupCapability)


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Apr 23 2018 10:25:35