execute_trajectory_action_capability.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2016, Kentaro Wada.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Kentaro Wada */
36 
38 
43 
44 namespace move_group
45 {
47 {
48 }
49 
51 {
52  // start the move action server
55  boost::bind(&MoveGroupExecuteTrajectoryAction::executePathCallback, this, _1), false));
56  execute_action_server_->registerPreemptCallback(
58  execute_action_server_->start();
59 }
60 
61 void MoveGroupExecuteTrajectoryAction::executePathCallback(const moveit_msgs::ExecuteTrajectoryGoalConstPtr& goal)
62 {
63  moveit_msgs::ExecuteTrajectoryResult action_res;
64  if (!context_->trajectory_execution_manager_)
65  {
66  const std::string response = "Cannot execute trajectory since ~allow_trajectory_execution was set to false";
67  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
68  execute_action_server_->setAborted(action_res, response);
69  return;
70  }
71 
72  executePath(goal, action_res);
73 
74  const std::string response = getActionResultString(action_res.error_code, false, false);
75  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
76  {
77  execute_action_server_->setSucceeded(action_res, response);
78  }
79  else if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
80  {
81  execute_action_server_->setPreempted(action_res, response);
82  }
83  else
84  {
85  execute_action_server_->setAborted(action_res, response);
86  }
87 
89 }
90 
91 void MoveGroupExecuteTrajectoryAction::executePath(const moveit_msgs::ExecuteTrajectoryGoalConstPtr& goal,
92  moveit_msgs::ExecuteTrajectoryResult& action_res)
93 {
94  ROS_INFO_NAMED(capability_name_, "Execution request received");
95 
96  context_->trajectory_execution_manager_->clear();
97  if (context_->trajectory_execution_manager_->push(goal->trajectory))
98  {
100  context_->trajectory_execution_manager_->execute();
101  moveit_controller_manager::ExecutionStatus status = context_->trajectory_execution_manager_->waitForExecution();
103  {
104  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
105  }
107  {
108  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
109  }
111  {
112  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
113  }
114  else
115  {
116  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
117  }
118  ROS_INFO_STREAM_NAMED(capability_name_, "Execution completed: " << status.asString());
119  }
120  else
121  {
122  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
123  }
124 }
125 
127 {
128  context_->trajectory_execution_manager_->stopExecution(true);
129 }
130 
132 {
133  moveit_msgs::ExecuteTrajectoryFeedback execute_feedback;
134  execute_feedback.state = stateToStr(state);
135  execute_action_server_->publishFeedback(execute_feedback);
136 }
137 
138 } // namespace move_group
139 
#define ROS_INFO_NAMED(name,...)
std::string stateToStr(MoveGroupState state) const
void executePathCallback(const moveit_msgs::ExecuteTrajectoryGoalConstPtr &goal)
#define ROS_INFO_STREAM_NAMED(name, args)
std::string getActionResultString(const moveit_msgs::MoveItErrorCodes &error_code, bool planned_trajectory_empty, bool plan_only)
std::unique_ptr< actionlib::SimpleActionServer< moveit_msgs::ExecuteTrajectoryAction > > execute_action_server_
static const std::string EXECUTE_ACTION_NAME
void executePath(const moveit_msgs::ExecuteTrajectoryGoalConstPtr &goal, moveit_msgs::ExecuteTrajectoryResult &action_res)
CLASS_LOADER_REGISTER_CLASS(Dog, Base)


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 16 2018 19:28:39