execute_trajectory_service_capability.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
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: Ioan Sucan */
36 
40 
42  : MoveGroupCapability("ExecuteTrajectoryService")
43  , callback_queue_()
44  , spinner_(1 /* spinner threads */, &callback_queue_)
45 {
46 }
47 
49 {
50  spinner_.stop();
51 }
52 
54 {
55  // We need to serve each service request in a thread independent of the main spinner thread.
56  // Otherwise, a synchronous execution request (i.e. waiting for the execution to finish) would block
57  // execution of the main spinner thread.
58  // Hence, we use our own asynchronous spinner listening to our own callback queue.
60  ops.template init<moveit_msgs::ExecuteKnownTrajectory::Request, moveit_msgs::ExecuteKnownTrajectory::Response>(
64  spinner_.start();
65 }
66 
67 bool move_group::MoveGroupExecuteService::executeTrajectoryService(moveit_msgs::ExecuteKnownTrajectory::Request& req,
68  moveit_msgs::ExecuteKnownTrajectory::Response& res)
69 {
70  ROS_INFO("Received new trajectory execution service request...");
71  if (!context_->trajectory_execution_manager_)
72  {
73  ROS_ERROR("Cannot execute trajectory since ~allow_trajectory_execution was set to false");
74  res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
75  return true;
76  }
77 
78  // \todo unwind trajectory before execution
79  // robot_trajectory::RobotTrajectory to_exec(planning_scene_monitor_->getRobotModel(), ;
80 
81  context_->trajectory_execution_manager_->clear();
82  if (context_->trajectory_execution_manager_->push(req.trajectory))
83  {
84  context_->trajectory_execution_manager_->execute();
85  if (req.wait_for_execution)
86  {
87  moveit_controller_manager::ExecutionStatus es = context_->trajectory_execution_manager_->waitForExecution();
89  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
91  res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
93  res.error_code.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
94  else
95  res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
96  ROS_INFO_STREAM("Execution completed: " << es.asString());
97  }
98  else
99  {
100  ROS_INFO("Trajectory was successfully forwarded to the controller");
101  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
102  }
103  }
104  else
105  {
106  res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
107  }
108  return true;
109 }
110 
CallbackQueueInterface * callback_queue
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool executeTrajectoryService(moveit_msgs::ExecuteKnownTrajectory::Request &req, moveit_msgs::ExecuteKnownTrajectory::Response &res)
#define ROS_INFO(...)
#define ROS_INFO_STREAM(args)
#define ROS_ERROR(...)
CLASS_LOADER_REGISTER_CLASS(Dog, Base)
static const std::string EXECUTE_SERVICE_NAME


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:52