execute_trajectory_service_capability.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
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: Ioan Sucan */
00036 
00037 #include "execute_trajectory_service_capability.h"
00038 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
00039 #include <moveit/move_group/capability_names.h>
00040 
00041 move_group::MoveGroupExecuteService::MoveGroupExecuteService()
00042   : MoveGroupCapability("ExecuteTrajectoryService")
00043   , callback_queue_()
00044   , spinner_(1 /* spinner threads */, &callback_queue_)
00045 {
00046 }
00047 
00048 move_group::MoveGroupExecuteService::~MoveGroupExecuteService()
00049 {
00050   spinner_.stop();
00051 }
00052 
00053 void move_group::MoveGroupExecuteService::initialize()
00054 {
00055   // We need to serve each service request in a thread independent of the main spinner thread.
00056   // Otherwise, a synchronous execution request (i.e. waiting for the execution to finish) would block
00057   // execution of the main spinner thread.
00058   // Hence, we use our own asynchronous spinner listening to our own callback queue.
00059   ros::AdvertiseServiceOptions ops;
00060   ops.template init<moveit_msgs::ExecuteKnownTrajectory::Request, moveit_msgs::ExecuteKnownTrajectory::Response>(
00061       EXECUTE_SERVICE_NAME, boost::bind(&MoveGroupExecuteService::executeTrajectoryService, this, _1, _2));
00062   ops.callback_queue = &callback_queue_;
00063   execute_service_ = root_node_handle_.advertiseService(ops);
00064   spinner_.start();
00065 }
00066 
00067 bool move_group::MoveGroupExecuteService::executeTrajectoryService(moveit_msgs::ExecuteKnownTrajectory::Request& req,
00068                                                                    moveit_msgs::ExecuteKnownTrajectory::Response& res)
00069 {
00070   ROS_INFO("Received new trajectory execution service request...");
00071   if (!context_->trajectory_execution_manager_)
00072   {
00073     ROS_ERROR("Cannot execute trajectory since ~allow_trajectory_execution was set to false");
00074     res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
00075     return true;
00076   }
00077 
00078   // \todo unwind trajectory before execution
00079   //    robot_trajectory::RobotTrajectory to_exec(planning_scene_monitor_->getRobotModel(), ;
00080 
00081   context_->trajectory_execution_manager_->clear();
00082   if (context_->trajectory_execution_manager_->push(req.trajectory))
00083   {
00084     context_->trajectory_execution_manager_->execute();
00085     if (req.wait_for_execution)
00086     {
00087       moveit_controller_manager::ExecutionStatus es = context_->trajectory_execution_manager_->waitForExecution();
00088       if (es == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
00089         res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00090       else if (es == moveit_controller_manager::ExecutionStatus::PREEMPTED)
00091         res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
00092       else if (es == moveit_controller_manager::ExecutionStatus::TIMED_OUT)
00093         res.error_code.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
00094       else
00095         res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
00096       ROS_INFO_STREAM("Execution completed: " << es.asString());
00097     }
00098     else
00099     {
00100       ROS_INFO("Trajectory was successfully forwarded to the controller");
00101       res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00102     }
00103   }
00104   else
00105   {
00106     res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
00107   }
00108   return true;
00109 }
00110 
00111 #include <class_loader/class_loader.h>
00112 CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupExecuteService, move_group::MoveGroupCapability)


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