00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, Unbounded Robotics Inc. 00005 * Copyright (c) 2012, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the Willow Garage nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 *********************************************************************/ 00035 00036 /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */ 00037 00038 #ifndef MOVEIT_PLUGINS_ACTION_BASED_CONTROLLER_HANDLE 00039 #define MOVEIT_PLUGINS_ACTION_BASED_CONTROLLER_HANDLE 00040 00041 #include <moveit/controller_manager/controller_manager.h> 00042 #include <actionlib/client/simple_action_client.h> 00043 00044 namespace moveit_simple_controller_manager 00045 { 00046 00047 00048 /* 00049 * This exist solely to inject addJoint/getJoints into base non-templated class. 00050 */ 00051 class ActionBasedControllerHandleBase : public moveit_controller_manager::MoveItControllerHandle 00052 { 00053 public: 00054 ActionBasedControllerHandleBase(const std::string name) : 00055 moveit_controller_manager::MoveItControllerHandle(name) 00056 { 00057 } 00058 00059 virtual void addJoint(std::string name) = 0; 00060 virtual void getJoints(std::vector<std::string> &joints) = 0; 00061 }; 00062 typedef boost::shared_ptr<ActionBasedControllerHandleBase> ActionBasedControllerHandleBasePtr; 00063 00064 00065 /* 00066 * This is a simple base class, which handles all of the action creation/etc 00067 */ 00068 template<typename T> 00069 class ActionBasedControllerHandle : public ActionBasedControllerHandleBase 00070 { 00071 00072 public: 00073 ActionBasedControllerHandle(const std::string &name, const std::string &ns) : 00074 ActionBasedControllerHandleBase(name), 00075 namespace_(ns), 00076 done_(true) 00077 { 00078 controller_action_client_.reset(new actionlib::SimpleActionClient<T>(getActionName(), true)); 00079 unsigned int attempts = 0; 00080 while (ros::ok() && !controller_action_client_->waitForServer(ros::Duration(5.0)) && ++attempts < 3) 00081 ROS_INFO_STREAM("MoveitSimpleControllerManager: Waiting for " << getActionName() << " to come up"); 00082 00083 if (!controller_action_client_->isServerConnected()) 00084 { 00085 ROS_ERROR_STREAM("MoveitSimpleControllerManager: Action client not connected: " << getActionName()); 00086 controller_action_client_.reset(); 00087 } 00088 00089 last_exec_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED; 00090 } 00091 00092 bool isConnected() const 00093 { 00094 return controller_action_client_; 00095 } 00096 00097 virtual bool cancelExecution() 00098 { 00099 if (!controller_action_client_) 00100 return false; 00101 if (!done_) 00102 { 00103 ROS_INFO_STREAM("MoveitSimpleControllerManager: Cancelling execution for " << name_); 00104 controller_action_client_->cancelGoal(); 00105 last_exec_ = moveit_controller_manager::ExecutionStatus::PREEMPTED; 00106 done_ = true; 00107 } 00108 return true; 00109 } 00110 00111 virtual bool waitForExecution(const ros::Duration &timeout = ros::Duration(0)) 00112 { 00113 if (controller_action_client_ && !done_) 00114 return controller_action_client_->waitForResult(timeout); 00115 return true; 00116 } 00117 00118 virtual moveit_controller_manager::ExecutionStatus getLastExecutionStatus() 00119 { 00120 return last_exec_; 00121 } 00122 00123 virtual void addJoint(std::string name) 00124 { 00125 joints_.push_back(name); 00126 } 00127 00128 virtual void getJoints(std::vector<std::string> &joints) 00129 { 00130 joints = joints_; 00131 } 00132 00133 protected: 00134 00135 std::string getActionName() 00136 { 00137 if (namespace_.empty()) 00138 return name_; 00139 else 00140 return name_ +"/" + namespace_; 00141 } 00142 00143 void finishControllerExecution(const actionlib::SimpleClientGoalState& state) 00144 { 00145 ROS_DEBUG_STREAM("MoveitSimpleControllerManager: Controller " << name_ << " is done with state " << state.toString() << ": " << state.getText()); 00146 if (state == actionlib::SimpleClientGoalState::SUCCEEDED) 00147 last_exec_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED; 00148 else 00149 if (state == actionlib::SimpleClientGoalState::ABORTED) 00150 last_exec_ = moveit_controller_manager::ExecutionStatus::ABORTED; 00151 else 00152 if (state == actionlib::SimpleClientGoalState::PREEMPTED) 00153 last_exec_ = moveit_controller_manager::ExecutionStatus::PREEMPTED; 00154 else 00155 last_exec_ = moveit_controller_manager::ExecutionStatus::FAILED; 00156 done_ = true; 00157 } 00158 00159 /* execution status */ 00160 moveit_controller_manager::ExecutionStatus last_exec_; 00161 bool done_; 00162 00163 /* the controller namespace, for instance, topics will map to name/ns/goal, name/ns/result, etc */ 00164 std::string namespace_; 00165 00166 /* the joints controlled by this controller */ 00167 std::vector<std::string> joints_; 00168 00169 /* action client */ 00170 boost::shared_ptr<actionlib::SimpleActionClient<T> > controller_action_client_; 00171 }; 00172 00173 00174 } // end namespace moveit_simple_controller_manager 00175 00176 #endif // MOVEIT_PLUGINS_ACTION_BASED_CONTROLLER_HANDLE