pax_global_header00006660000000000000000000000064135226432420014515gustar00rootroot0000000000000052 comment=e9c6c0dc58dd44a2d4e81c20e9dbd1103f44a62b actionlib-1.12.0/000077500000000000000000000000001352264324200135425ustar00rootroot00000000000000actionlib-1.12.0/.gitignore000066400000000000000000000000061352264324200155260ustar00rootroot00000000000000*.pyc actionlib-1.12.0/CHANGELOG.rst000066400000000000000000000231361352264324200155700ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package actionlib ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.12.0 (2019-08-07) ------------------- * Complete the full set of Ptr typedefs (`#106 `_) * action_server calls initialize in constructor (`#120 `_) * Print the correct error on waiting for result (`#123 `_) * Remove getState error when no goal is running (`#97 `_) * Update maintainer (`#122 `_) * Contributors: Alireza, Bence Magyar, Carl Saldanha, Christopher Wecht, Michael Carroll 1.11.15 (2018-08-17) -------------------- * unique name for axtools title bar (`#107 `_) * [bugfix] add missing ros / ros console includes (`#114 `_) * [bugfix] update posix_time::milliseconds for boost 1.67 (`#111 `_) * [test] Use portable boost::this_thread::sleep() to sleep so it can be built on Windows (`#112 `_) * Revert (`#106 `_) that broke downstream packages (`#113 `_) * Contributors: Felix Messmer, Gianfranco Costamagna, Johnson Shih, Mikael Arguedas 1.11.14 (2018-05-21) -------------------- * Complete the full set of Ptr typedefs (`#106 `_) * Change boost::posix_time::milliseconds init to int64_t (`#105 `_) * Added ROS_ERROR message for Release code when asserts are ignored (`#94 `_) * fix typos. (`#102 `_) * Contributors: Bence Magyar, Patrick Beeson, Tobias Fischer, csukuangfj 1.11.13 (2018-03-14) -------------------- * [bugfix] added missing boost/thread/reverse_lock.hpp include (`#95 `_) * Contributors: Robert Haschke 1.11.12 (2017-12-18) -------------------- * fix uncrustify mixup (`#92 `_) * Contributors: Mikael Arguedas 1.11.11 (2017-10-27) -------------------- * fix typo in server_goal_handle_imp.h (`#89 `_) * Use RAII to handle mutexes (`#87 `_) * Contributors: Cong Liu, Esteve Fernandez, Mikael Arguedas 1.11.10 (2017-07-27) -------------------- * Clang tidy fixes (`#86 `_) * C++ style (`#72 `_) * Proper return value after assert (`#83 `_) * switch to package format 2 (`#82 `_) * remove trailing whitespaces (`#81 `_) * lock listhandle earlier in getCommState in client_goal_handle_imp. active bool critical (`#77 `_) * add missing runtime dependencies (`#79 `_) * Contributors: Esteve Fernandez, Mikael Arguedas, johaq 1.11.9 (2017-03-27) ------------------- * Python3 compatibility + pep8 compliance (`#71 `_) follow-up of (`#43 `_) * - wait for ros::Time::now to become valid before init of connection_monitor (`#62 `_) - bugfix : connection_monitor should wait for result * fixed default value for rosparam. closes `#69 `_ (`#70 `_) * Contributors: 1r0b1n0, Mikael Arguedas, Piyush Khandelwal 1.11.8 (2017-02-17) ------------------- * Fixes a deadlock (`#64 `_) * Removed unused variables warnings (`#63 `_ `#65 `_) * If using sim time, wait for /clock (`#59 `_) * add parameters to configure queue sizes (`#55 `_) * Contributors: Esteve Fernandez, Jonathan Meyer, Mikael Arguedas, Patrick Beeson, Robin Vanhove 1.11.7 (2016-10-24) ------------------- * Merge pull request `#57 `_ from stonier/patch-1 Remove misleading error log * Remove misleading error log This was introduced in https://github.com/ros/actionlib/pull/43. It is not actually correct - you can feasibly get feedback here before a new goal is confirmed. See `send_goal()`.... ``` def send_goal(self, goal, done_cb=None, active_cb=None, feedback_cb=None): # destroys the old goal handle self.stop_tracking_goal() ... self.gh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback) ``` and of course it will take more time on top of this for the server to actually process the incoming goal and confirm it. Meantime, it may have sent us feedback messages. * Improved the const-correctness of some actionlib classes. (`#50 `_) * Issue `#51 `_: Remove annoying debug messages that make useless to enable debug on Python nodes, as they overwhelm less spamming messages (`#54 `_) * reduce change of unncessary exception on shutdown bu checking directly in before publishing for a shutdown (`#53 `_) * Contributors: Blake Anderson, Daniel Stonier, Jorge Santos Simón, Mikael Arguedas, uliklank 1.11.6 (2016-06-22) ------------------- * Python code cleanup (`#43 `_) * Cleaned up semicolons, indentation, spaces. * Removed unused local var after further confirmation of no risk of side effects. * Contributors: Andrew Blakey 1.11.5 (2016-03-14) ------------------- * update maintainer * Merge pull request `#42 `_ from jonbinney/python3-compat Python 3 compatibility changes * More readable iteration in state name lookup * Update syntax for exception handling * Iterate over dictionary in python3 compatible way * Use absolute imports for python3 compatibility * Merge pull request `#39 `_ from clearpathrobotics/action-fixup Minor improvements * Enable UI feedback for preempt-requested goal in axserver.py * Clean up axclient.py initialization to allow starting before actionserver, requires action type passed in * Add hashes to ServerGoalHandle and ClientGoalHandles * Contributors: Esteve Fernandez, Jon Binney, Mikael Arguedas, Paul Bovbel 1.11.4 (2015-04-22) ------------------- * Initialize `execute_thread_` to NULL * Contributors: Esteve Fernandez 1.11.3 (2014-12-23) ------------------- * Increase queue sizes to match Python client publishers. * Adjust size of client publishers in Python * Contributors: Esteve Fernandez, Michael Ferguson 1.11.2 (2014-05-20) ------------------- * Update python publishers to define queue_size. * Use the correct queue for processing MessageEvents * Contributors: Esteve Fernandez, Michael Ferguson, Nican 1.11.1 (2014-05-08) ------------------- * Fix uninitialised `execute_thread_` member pointer * Make rostest in CMakeLists optional * Use catkin_install_python() to install Python scripts * Contributors: Dirk Thomas, Esteve Fernandez, Jordi Pages, Lukas Bulwahn 1.11.0 (2014-02-13) ------------------- * replace usage of __connection_header with MessageEvent (`#20 `_) 1.10.3 (2013-08-27) ------------------- * Merged pull request `#15 `_ Fixes a compile issue for actionlib headers on OS X 1.10.2 (2013-08-21) ------------------- * separating ActionServer implementation into base class and ros-publisher-based class (`#11 `_) * support CATKIN_ENABLE_TESTING * add isValid to ServerGoalHandle (`#14 `_) * make operators const (`#10 `_) * add counting of connections to avoid reconnect problem when callbacks are invoked in different order (`#7 `_) * fix deadlock in simple_action_server.py (`#4 `_) * fix missing runtime destination for library (`#3 `_) 1.10.1 (2013-06-06) ------------------- * fix location of library before installation (`#1 `_) 1.10.0 (2013-04-11) ------------------- * define DEPRECATED only if not defined already * modified dependency type of catkin to buildtool 1.9.11 (2012-12-13) ------------------- * first public release for Groovy 1.8.7 (2012-06-14) ------------------ * add new CommState LOST * added more missing dependencies 1.8.6 (2012-06-05) ------------------ * added missing dependencies 1.8.5 (2012-05-31) ------------------ * make axclient work base on topic name only 1.8.4 (2012-04-05) ------------------ * add missing axserver/axclient install 1.8.3 (2012-03-15) ------------------ * fix issue with locking in action server (`#5391 `_) 1.8.2 (2012-02-29) ------------------ * update to newer catkin API 1.8.1 (2012-02-21) ------------------ * fix Python packaging 1.8.0 (2012-02-07) ------------------ * separated from common stack * converted to use catkin actionlib-1.12.0/CMakeLists.txt000066400000000000000000000025621352264324200163070ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(actionlib) find_package(catkin REQUIRED COMPONENTS actionlib_msgs message_generation roscpp rosunit std_msgs) find_package(Boost REQUIRED COMPONENTS thread) include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) link_directories(${catkin_LIBRARY_DIRS}) catkin_python_setup() add_action_files(DIRECTORY action FILES Test.action TestRequest.action TwoInts.action) generate_messages(DEPENDENCIES actionlib_msgs std_msgs) catkin_package( INCLUDE_DIRS include LIBRARIES actionlib CATKIN_DEPENDS actionlib_msgs message_runtime roscpp std_msgs DEPENDS Boost ) add_library(actionlib src/connection_monitor.cpp src/goal_id_generator.cpp) target_link_libraries(actionlib ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(actionlib actionlib_gencpp) catkin_install_python(PROGRAMS tools/axclient.py tools/axserver.py tools/dynamic_action.py tools/library.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h") if(CATKIN_ENABLE_TESTING) find_package(rostest) add_subdirectory(test) endif() actionlib-1.12.0/action/000077500000000000000000000000001352264324200150175ustar00rootroot00000000000000actionlib-1.12.0/action/Test.action000066400000000000000000000000571352264324200171370ustar00rootroot00000000000000int32 goal --- int32 result --- int32 feedback actionlib-1.12.0/action/TestRequest.action000066400000000000000000000011541352264324200205070ustar00rootroot00000000000000int32 TERMINATE_SUCCESS = 0 int32 TERMINATE_ABORTED = 1 int32 TERMINATE_REJECTED = 2 int32 TERMINATE_LOSE = 3 int32 TERMINATE_DROP = 4 int32 TERMINATE_EXCEPTION = 5 int32 terminate_status bool ignore_cancel # If true, ignores requests to cancel string result_text int32 the_result # Desired value for the_result in the Result bool is_simple_client duration delay_accept # Delays accepting the goal by this amount of time duration delay_terminate # Delays terminating for this amount of time duration pause_status # Pauses the status messages for this amount of time --- int32 the_result bool is_simple_server --- actionlib-1.12.0/action/TwoInts.action000066400000000000000000000000421352264324200176210ustar00rootroot00000000000000int64 a int64 b --- int64 sum --- actionlib-1.12.0/docs/000077500000000000000000000000001352264324200144725ustar00rootroot00000000000000actionlib-1.12.0/docs/action_interface.svg000066400000000000000000000473271352264324200205250ustar00rootroot00000000000000 image/svg+xml Action Interface ActionClient ActionServer goal cancel status result feedback From Client From Server ROS Topics actionlib-1.12.0/docs/cancel_policy.svg000066400000000000000000000304041352264324200200200ustar00rootroot00000000000000 image/svg+xml filled empty empty filled stamp ID cancel all goals cancel all goals before stamp cancel goalGoal ID cancel goalGoal ID+cancel allgoals beforestamp Cancel Request Policy actionlib-1.12.0/docs/server_states_detailed.svg000066400000000000000000007062211352264324200217470ustar00rootroot00000000000000 image/svg+xml Server State Transitions PENDING RECALLING CancelRequest ACTIVE PREEMPTING CancelRequest setAccepted setAccepted setSucceeded REJECTED RECALLED setCanceled setRejected Client Triggered Server Triggered Receive Goal setCancelled setSucceeded setAborted setAborted PREEMPTED SUCCEEDED ABORTED Terminal State setRejected Client State Transitions WAITING FORGOAL ACK PENDING ACTIVE PREEMPTING WAITING FORCANCEL ACK RECALLING WAITING FORRESULT DONE Send Goal [PENDING] [ACTIVE] [PREEMPTING] [RECALLING] [PREEMPTED][ABORTED[[SUCCEEDED] [PREEMPTING] [PREEMPTED][RECALLED][REJECTED] [PREEMPTING] [PREEMPTED][ABORTED][SUCCEEDED] ReceiveResult Msg Cancel Goal Cancel Goal Cancel Goal Client Triggered Server Triggered Terminal State [ACTIVE] WAITING FORGOAL ACK PENDING ACTIVE PREEMPTING WAITING FORCANCEL ACK RECALLING WAITING FORRESULT DONE Send Goal [PENDING] [ACTIVE] [RECALLING] [RECALLED][REJECTED] [PREEMPTING] [ABORTED][SUCCEEDED] [PREEMPTING] [PREEMPTING] [PREEMPTED][ABORTED][SUCCEEDED] ReceiveResult Msg Cancel Goal Cancel Goal Cancel Goal [ACTIVE] [RECALLING] Client Triggered Server Triggered Terminal State [REJECTED] Client State Transitions actionlib-1.12.0/docs/simple_goal_reception.svg000066400000000000000000003216031352264324200215630ustar00rootroot00000000000000 image/svg+xml Simple Action Server - Goal Reception Pending Goal [PENDING] Current Goal [ACTIVE] A C Receive Goal C Push C intoPending Goal Action Server Simple Action Server User Code step 1 B Pending Goal [RECALLED] Current Goal [ACTIVE] A C Action Server Simple Action Server step 2 B [PENDING] User Code Goal Object Goal Object Pointer Simple Action Server - Goal Acceptance Pending Goal [PENDING] Current Goal [ACTIVE] A Action Server Simple Action Server User Code step 1 B Pending Goal [ACTIVE Current Goal [DONE] A Action Server Simple Action Server step 2 B User Code Goal Object Goal Object Pointer Accept Goal B actionlib-1.12.0/include/000077500000000000000000000000001352264324200151655ustar00rootroot00000000000000actionlib-1.12.0/include/actionlib/000077500000000000000000000000001352264324200171315ustar00rootroot00000000000000actionlib-1.12.0/include/actionlib/action_definition.h000066400000000000000000000062231352264324200227720ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__ACTION_DEFINITION_H_ #define ACTIONLIB__ACTION_DEFINITION_H_ // A macro that will generate helpful typedefs for action client, server, and policy implementers namespace actionlib { #define ACTION_DEFINITION(ActionSpec) \ typedef typename ActionSpec::_action_goal_type ActionGoal; \ typedef typename ActionGoal::_goal_type Goal; \ typedef typename ActionSpec::_action_result_type ActionResult; \ typedef typename ActionResult::_result_type Result; \ typedef typename ActionSpec::_action_feedback_type ActionFeedback; \ typedef typename ActionFeedback::_feedback_type Feedback; \ \ typedef boost::shared_ptr ActionGoalConstPtr; \ typedef boost::shared_ptr ActionGoalPtr; \ typedef boost::shared_ptr GoalConstPtr; \ typedef boost::shared_ptr GoalPtr; \ \ typedef boost::shared_ptr ActionResultConstPtr; \ typedef boost::shared_ptr ActionResultPtr; \ typedef boost::shared_ptr ResultConstPtr; \ typedef boost::shared_ptr ResultPtr; \ \ typedef boost::shared_ptr ActionFeedbackConstPtr; \ typedef boost::shared_ptr ActionFeedbackPtr; \ typedef boost::shared_ptr FeedbackConstPtr; \ typedef boost::shared_ptr FeedbackPtr; } // namespace actionlib #endif // ACTIONLIB__ACTION_DEFINITION_H_ actionlib-1.12.0/include/actionlib/client/000077500000000000000000000000001352264324200204075ustar00rootroot00000000000000actionlib-1.12.0/include/actionlib/client/action_client.h000066400000000000000000000267221352264324200234040ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef ACTIONLIB__CLIENT__ACTION_CLIENT_H_ #define ACTIONLIB__CLIENT__ACTION_CLIENT_H_ #include #include #include #include #include #include "ros/ros.h" #include "ros/callback_queue_interface.h" namespace actionlib { /** * \brief Full interface to an ActionServer * * ActionClient provides a complete client side implementation of the ActionInterface protocol. * It provides callbacks for every client side transition, giving the user full observation into * the client side state machine. */ template class ActionClient { public: typedef ClientGoalHandle GoalHandle; private: ACTION_DEFINITION(ActionSpec); typedef ActionClient ActionClientT; typedef boost::function TransitionCallback; typedef boost::function FeedbackCallback; typedef boost::function SendGoalFunc; public: /** * \brief Simple constructor * * Constructs an ActionClient and sets up the necessary ros topics for the ActionInterface * \param name The action name. Defines the namespace in which the action communicates * \param queue CallbackQueue from which this action will process messages. * The default (NULL) is to use the global queue */ ActionClient(const std::string & name, ros::CallbackQueueInterface * queue = NULL) : n_(name), guard_(new DestructionGuard()), manager_(guard_) { initClient(queue); } /** * \brief Constructor with namespacing options * * Constructs an ActionClient and sets up the necessary ros topics for the ActionInterface, * and namespaces them according the a specified NodeHandle * \param n The node handle on top of which we want to namespace our action * \param name The action name. Defines the namespace in which the action communicates * \param queue CallbackQueue from which this action will process messages. * The default (NULL) is to use the global queue */ ActionClient(const ros::NodeHandle & n, const std::string & name, ros::CallbackQueueInterface * queue = NULL) : n_(n, name), guard_(new DestructionGuard()), manager_(guard_) { initClient(queue); } ~ActionClient() { ROS_DEBUG_NAMED("actionlib", "ActionClient: Waiting for destruction guard to clean up"); guard_->destruct(); ROS_DEBUG_NAMED("actionlib", "ActionClient: destruction guard destruct() done"); } /** * \brief Sends a goal to the ActionServer, and also registers callbacks * \param transition_cb Callback that gets called on every client state transition * \param feedback_cb Callback that gets called whenever feedback for this goal is received */ GoalHandle sendGoal(const Goal & goal, TransitionCallback transition_cb = TransitionCallback(), FeedbackCallback feedback_cb = FeedbackCallback()) { ROS_DEBUG_NAMED("actionlib", "about to start initGoal()"); GoalHandle gh = manager_.initGoal(goal, transition_cb, feedback_cb); ROS_DEBUG_NAMED("actionlib", "Done with initGoal()"); return gh; } /** * \brief Cancel all goals currently running on the action server * * This preempts all goals running on the action server at the point that * this message is serviced by the ActionServer. */ void cancelAllGoals() { actionlib_msgs::GoalID cancel_msg; // CancelAll policy encoded by stamp=0, id=0 cancel_msg.stamp = ros::Time(0, 0); cancel_msg.id = ""; cancel_pub_.publish(cancel_msg); } /** * \brief Cancel all goals that were stamped at and before the specified time * \param time All goals stamped at or before `time` will be canceled */ void cancelGoalsAtAndBeforeTime(const ros::Time & time) { actionlib_msgs::GoalID cancel_msg; cancel_msg.stamp = time; cancel_msg.id = ""; cancel_pub_.publish(cancel_msg); } /** * \brief Waits for the ActionServer to connect to this client * Often, it can take a second for the action server & client to negotiate * a connection, thus, risking the first few goals to be dropped. This call lets * the user wait until the network connection to the server is negotiated * NOTE: Using this call in a single threaded ROS application, or any * application where the action client's callback queue is not being * serviced, will not work. Without a separate thread servicing the queue, or * a multi-threaded spinner, there is no way for the client to tell whether * or not the server is up because it can't receive a status message. * \param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout. * \return True if the server connected in the allocated time. False on timeout */ bool waitForActionServerToStart(const ros::Duration & timeout = ros::Duration(0, 0) ) { // if ros::Time::isSimTime(), then wait for it to become valid if (!ros::Time::waitForValid(ros::WallDuration(timeout.sec, timeout.nsec))) { return false; } if (connection_monitor_) { return connection_monitor_->waitForActionServerToStart(timeout, n_); } else { return false; } } /** * @brief Checks if the action client is successfully connected to the action server * @return True if the server is connected, false otherwise */ bool isServerConnected() { return connection_monitor_->isServerConnected(); } private: ros::NodeHandle n_; boost::shared_ptr guard_; GoalManager manager_; ros::Subscriber result_sub_; ros::Subscriber feedback_sub_; boost::shared_ptr connection_monitor_; // Have to destroy subscribers and publishers before the connection_monitor_, since we call callbacks in the connection_monitor_ ros::Publisher goal_pub_; ros::Publisher cancel_pub_; ros::Subscriber status_sub_; void sendGoalFunc(const ActionGoalConstPtr & action_goal) { goal_pub_.publish(action_goal); } void sendCancelFunc(const actionlib_msgs::GoalID & cancel_msg) { cancel_pub_.publish(cancel_msg); } void initClient(ros::CallbackQueueInterface * queue) { ros::Time::waitForValid(); // read parameters indicating publish/subscribe queue sizes int pub_queue_size; int sub_queue_size; n_.param("actionlib_client_pub_queue_size", pub_queue_size, 10); n_.param("actionlib_client_sub_queue_size", sub_queue_size, 1); if (pub_queue_size < 0) {pub_queue_size = 10;} if (sub_queue_size < 0) {sub_queue_size = 1;} status_sub_ = queue_subscribe("status", static_cast(sub_queue_size), &ActionClientT::statusCb, this, queue); feedback_sub_ = queue_subscribe("feedback", static_cast(sub_queue_size), &ActionClientT::feedbackCb, this, queue); result_sub_ = queue_subscribe("result", static_cast(sub_queue_size), &ActionClientT::resultCb, this, queue); connection_monitor_.reset(new ConnectionMonitor(feedback_sub_, result_sub_)); // Start publishers and subscribers goal_pub_ = queue_advertise("goal", static_cast(pub_queue_size), boost::bind(&ConnectionMonitor::goalConnectCallback, connection_monitor_, _1), boost::bind(&ConnectionMonitor::goalDisconnectCallback, connection_monitor_, _1), queue); cancel_pub_ = queue_advertise("cancel", static_cast(pub_queue_size), boost::bind(&ConnectionMonitor::cancelConnectCallback, connection_monitor_, _1), boost::bind(&ConnectionMonitor::cancelDisconnectCallback, connection_monitor_, _1), queue); manager_.registerSendGoalFunc(boost::bind(&ActionClientT::sendGoalFunc, this, _1)); manager_.registerCancelFunc(boost::bind(&ActionClientT::sendCancelFunc, this, _1)); } template ros::Publisher queue_advertise(const std::string & topic, uint32_t queue_size, const ros::SubscriberStatusCallback & connect_cb, const ros::SubscriberStatusCallback & disconnect_cb, ros::CallbackQueueInterface * queue) { ros::AdvertiseOptions ops; ops.init(topic, queue_size, connect_cb, disconnect_cb); ops.tracked_object = ros::VoidPtr(); ops.latch = false; ops.callback_queue = queue; return n_.advertise(ops); } template ros::Subscriber queue_subscribe(const std::string & topic, uint32_t queue_size, void (T::* fp)( const ros::MessageEvent &), T * obj, ros::CallbackQueueInterface * queue) { ros::SubscribeOptions ops; ops.callback_queue = queue; ops.topic = topic; ops.queue_size = queue_size; ops.md5sum = ros::message_traits::md5sum(); ops.datatype = ros::message_traits::datatype(); ops.helper = ros::SubscriptionCallbackHelperPtr( new ros::SubscriptionCallbackHelperT &>( boost::bind(fp, obj, _1) ) ); return n_.subscribe(ops); } void statusCb(const ros::MessageEvent & status_array_event) { ROS_DEBUG_NAMED("actionlib", "Getting status over the wire."); if (connection_monitor_) { connection_monitor_->processStatus( status_array_event.getConstMessage(), status_array_event.getPublisherName()); } manager_.updateStatuses(status_array_event.getConstMessage()); } void feedbackCb(const ros::MessageEvent & action_feedback) { manager_.updateFeedbacks(action_feedback.getConstMessage()); } void resultCb(const ros::MessageEvent & action_result) { manager_.updateResults(action_result.getConstMessage()); } }; } // namespace actionlib #endif // ACTIONLIB__CLIENT__ACTION_CLIENT_H_ actionlib-1.12.0/include/actionlib/client/client_goal_handle_imp.h000066400000000000000000000253731352264324200252320ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* This file has the template implementation for ClientGoalHandle. It should be included with the * class definition. */ #ifndef ACTIONLIB__CLIENT__CLIENT_GOAL_HANDLE_IMP_H_ #define ACTIONLIB__CLIENT__CLIENT_GOAL_HANDLE_IMP_H_ #include namespace actionlib { template ClientGoalHandle::ClientGoalHandle() { gm_ = NULL; active_ = false; } template ClientGoalHandle::~ClientGoalHandle() { reset(); } template ClientGoalHandle::ClientGoalHandle(GoalManagerT * gm, typename ManagedListT::Handle handle, const boost::shared_ptr & guard) { gm_ = gm; active_ = true; list_handle_ = handle; guard_ = guard; } template void ClientGoalHandle::reset() { if (active_) { DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this reset() call"); return; } boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_); list_handle_.reset(); active_ = false; gm_ = NULL; } } template bool ClientGoalHandle::isExpired() const { return !active_; } template CommState ClientGoalHandle::getCommState() const { assert(gm_); if (!gm_) { ROS_ERROR_NAMED("actionlib", "Client should have valid GoalManager"); return CommState(CommState::DONE); } boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_); if (!active_) { ROS_ERROR_NAMED("actionlib", "Trying to getCommState on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle"); return CommState(CommState::DONE); } DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this getCommState() call"); return CommState(CommState::DONE); } return list_handle_.getElem()->getCommState(); } template TerminalState ClientGoalHandle::getTerminalState() const { if (!active_) { ROS_ERROR_NAMED("actionlib", "Trying to getTerminalState on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle"); return TerminalState(TerminalState::LOST); } DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this getTerminalState() call"); return TerminalState(TerminalState::LOST); } assert(gm_); if (!gm_) { ROS_ERROR_NAMED("actionlib", "Client should have valid GoalManager"); return TerminalState(TerminalState::LOST); } boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_); CommState comm_state_ = list_handle_.getElem()->getCommState(); if (comm_state_ != CommState::DONE) { ROS_WARN_NAMED("actionlib", "Asking for the terminal state when we're in [%s]", comm_state_.toString().c_str()); } actionlib_msgs::GoalStatus goal_status = list_handle_.getElem()->getGoalStatus(); switch (goal_status.status) { case actionlib_msgs::GoalStatus::PENDING: case actionlib_msgs::GoalStatus::ACTIVE: case actionlib_msgs::GoalStatus::PREEMPTING: case actionlib_msgs::GoalStatus::RECALLING: ROS_ERROR_NAMED("actionlib", "Asking for terminal state, but latest goal status is %u", goal_status.status); return TerminalState(TerminalState::LOST, goal_status.text); case actionlib_msgs::GoalStatus::PREEMPTED: return TerminalState(TerminalState::PREEMPTED, goal_status.text); case actionlib_msgs::GoalStatus::SUCCEEDED: return TerminalState(TerminalState::SUCCEEDED, goal_status.text); case actionlib_msgs::GoalStatus::ABORTED: return TerminalState(TerminalState::ABORTED, goal_status.text); case actionlib_msgs::GoalStatus::REJECTED: return TerminalState(TerminalState::REJECTED, goal_status.text); case actionlib_msgs::GoalStatus::RECALLED: return TerminalState(TerminalState::RECALLED, goal_status.text); case actionlib_msgs::GoalStatus::LOST: return TerminalState(TerminalState::LOST, goal_status.text); default: ROS_ERROR_NAMED("actionlib", "Unknown goal status: %u", goal_status.status); break; } ROS_ERROR_NAMED("actionlib", "Bug in determining terminal state"); return TerminalState(TerminalState::LOST, goal_status.text); } template typename ClientGoalHandle::ResultConstPtr ClientGoalHandle::getResult() const { if (!active_) { ROS_ERROR_NAMED("actionlib", "Trying to getResult on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle"); } assert(gm_); if (!gm_) { ROS_ERROR_NAMED("actionlib", "Client should have valid GoalManager"); return typename ClientGoalHandle::ResultConstPtr() ; } DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this getResult() call"); return typename ClientGoalHandle::ResultConstPtr(); } boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_); return list_handle_.getElem()->getResult(); } template void ClientGoalHandle::resend() { if (!active_) { ROS_ERROR_NAMED("actionlib", "Trying to resend() on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle"); } DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this resend() call"); return; } assert(gm_); if (!gm_) { ROS_ERROR_NAMED("actionlib", "Client should have valid GoalManager"); return; } boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_); ActionGoalConstPtr action_goal = list_handle_.getElem()->getActionGoal(); if (!action_goal) { ROS_ERROR_NAMED("actionlib", "BUG: Got a NULL action_goal"); } if (gm_->send_goal_func_) { gm_->send_goal_func_(action_goal); } } template void ClientGoalHandle::cancel() { if (!active_) { ROS_ERROR_NAMED("actionlib", "Trying to cancel() on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle"); return; } assert(gm_); if (!gm_) { ROS_ERROR_NAMED("actionlib", "Client should have valid GoalManager"); return; } DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this call"); return; } boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_); switch (list_handle_.getElem()->getCommState().state_) { case CommState::WAITING_FOR_GOAL_ACK: case CommState::PENDING: case CommState::ACTIVE: case CommState::WAITING_FOR_CANCEL_ACK: break; // Continue standard processing case CommState::WAITING_FOR_RESULT: case CommState::RECALLING: case CommState::PREEMPTING: case CommState::DONE: ROS_DEBUG_NAMED("actionlib", "Got a cancel() request while in state [%s], so ignoring it", list_handle_.getElem()->getCommState().toString().c_str()); return; default: ROS_ERROR_NAMED("actionlib", "BUG: Unhandled CommState: %u", list_handle_.getElem()->getCommState().state_); return; } ActionGoalConstPtr action_goal = list_handle_.getElem()->getActionGoal(); actionlib_msgs::GoalID cancel_msg; cancel_msg.stamp = ros::Time(0, 0); cancel_msg.id = list_handle_.getElem()->getActionGoal()->goal_id.id; if (gm_->cancel_func_) { gm_->cancel_func_(cancel_msg); } list_handle_.getElem()->transitionToState(*this, CommState::WAITING_FOR_CANCEL_ACK); } template bool ClientGoalHandle::operator==(const ClientGoalHandle & rhs) const { // Check if both are inactive if (!active_ && !rhs.active_) { return true; } // Check if one or the other is inactive if (!active_ || !rhs.active_) { return false; } DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this operator==() call"); return false; } return list_handle_ == rhs.list_handle_; } template bool ClientGoalHandle::operator!=(const ClientGoalHandle & rhs) const { return !(*this == rhs); } } // namespace actionlib #endif // ACTIONLIB__CLIENT__CLIENT_GOAL_HANDLE_IMP_H_ actionlib-1.12.0/include/actionlib/client/client_helpers.h000066400000000000000000000224771352264324200235740ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef ACTIONLIB__CLIENT__CLIENT_HELPERS_H_ #define ACTIONLIB__CLIENT__CLIENT_HELPERS_H_ #include #include #include #include #include #include "actionlib/action_definition.h" #include "actionlib/managed_list.h" #include "actionlib/enclosure_deleter.h" #include "actionlib/goal_id_generator.h" #include "actionlib/client/comm_state.h" #include "actionlib/client/terminal_state.h" #include "actionlib/destruction_guard.h" // msgs #include "actionlib_msgs/GoalID.h" #include "actionlib_msgs/GoalStatusArray.h" namespace actionlib { template class ClientGoalHandle; template class CommStateMachine; template class GoalManager { public: ACTION_DEFINITION(ActionSpec); typedef GoalManager GoalManagerT; typedef ClientGoalHandle GoalHandleT; typedef boost::function TransitionCallback; typedef boost::function FeedbackCallback; typedef boost::function SendGoalFunc; typedef boost::function CancelFunc; GoalManager(const boost::shared_ptr & guard) : guard_(guard) {} void registerSendGoalFunc(SendGoalFunc send_goal_func); void registerCancelFunc(CancelFunc cancel_func); GoalHandleT initGoal(const Goal & goal, TransitionCallback transition_cb = TransitionCallback(), FeedbackCallback feedback_cb = FeedbackCallback() ); void updateStatuses(const actionlib_msgs::GoalStatusArrayConstPtr & status_array); void updateFeedbacks(const ActionFeedbackConstPtr & action_feedback); void updateResults(const ActionResultConstPtr & action_result); friend class ClientGoalHandle; // should be private typedef ManagedList > > ManagedListT; ManagedListT list_; private: SendGoalFunc send_goal_func_; CancelFunc cancel_func_; boost::shared_ptr guard_; boost::recursive_mutex list_mutex_; GoalIDGenerator id_generator_; void listElemDeleter(typename ManagedListT::iterator it); }; /** * \brief Client side handle to monitor goal progress * * A ClientGoalHandle is a reference counted object that is used to manipulate and monitor the progress * of an already dispatched goal. Once all the goal handles go out of scope (or are reset), an * ActionClient stops maintaining state for that goal. */ template class ClientGoalHandle { private: ACTION_DEFINITION(ActionSpec); public: /** * \brief Create an empty goal handle * * Constructs a goal handle that doesn't track any goal. Calling any method on an empty goal * handle other than operator= will trigger an assertion. */ ClientGoalHandle(); ~ClientGoalHandle(); /** * \brief Stops goal handle from tracking a goal * * Useful if you want to stop tracking the progress of a goal, but it is inconvenient to force * the goal handle to go out of scope. Has pretty much the same semantics as boost::shared_ptr::reset() */ void reset(); /** * \brief Checks if this goal handle is tracking a goal * * Has pretty much the same semantics as boost::shared_ptr::expired() * \return True if this goal handle is not tracking a goal */ inline bool isExpired() const; /** * \brief Get the state of this goal's communication state machine from interaction with the server * * Possible States are: WAITING_FOR_GOAL_ACK, PENDING, ACTIVE, WAITING_FOR_RESULT, * WAITING_FOR_CANCEL_ACK, RECALLING, PREEMPTING, DONE * \return The current goal's communication state with the server */ CommState getCommState() const; /** * \brief Get the terminal state information for this goal * * Possible States Are: RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST * This call only makes sense if CommState==DONE. This will send ROS_WARNs if we're not in DONE * \return The terminal state */ TerminalState getTerminalState() const; /** * \brief Get result associated with this goal * * \return NULL if no result received. Otherwise returns shared_ptr to result. */ ResultConstPtr getResult() const; /** * \brief Resends this goal [with the same GoalID] to the ActionServer * * Useful if the user thinks that the goal may have gotten lost in transit */ void resend(); /** * \brief Sends a cancel message for this specific goal to the ActionServer * * Also transitions the Communication State Machine to WAITING_FOR_CANCEL_ACK */ void cancel(); /** * \brief Check if two goal handles point to the same goal * \return TRUE if both point to the same goal. Also returns TRUE if both handles are inactive. */ bool operator==(const ClientGoalHandle & rhs) const; /** * \brief !(operator==()) */ bool operator!=(const ClientGoalHandle & rhs) const; friend class GoalManager; private: typedef GoalManager GoalManagerT; typedef ManagedList > > ManagedListT; ClientGoalHandle(GoalManagerT * gm, typename ManagedListT::Handle handle, const boost::shared_ptr & guard); GoalManagerT * gm_; bool active_; // typename ManagedListT::iterator it_; boost::shared_ptr guard_; // Guard must still exist when the list_handle_ is destroyed typename ManagedListT::Handle list_handle_; }; template class CommStateMachine { private: // generates typedefs that we'll use to make our lives easier ACTION_DEFINITION(ActionSpec); public: typedef boost::function &)> TransitionCallback; typedef boost::function &, const FeedbackConstPtr &)> FeedbackCallback; typedef ClientGoalHandle GoalHandleT; CommStateMachine(const ActionGoalConstPtr & action_goal, TransitionCallback transition_cb, FeedbackCallback feedback_cb); ActionGoalConstPtr getActionGoal() const; CommState getCommState() const; actionlib_msgs::GoalStatus getGoalStatus() const; ResultConstPtr getResult() const; // Transitions caused by messages void updateStatus(GoalHandleT & gh, const actionlib_msgs::GoalStatusArrayConstPtr & status_array); void updateFeedback(GoalHandleT & gh, const ActionFeedbackConstPtr & action_feedback); void updateResult(GoalHandleT & gh, const ActionResultConstPtr & action_result); // Forced transitions void transitionToState(GoalHandleT & gh, const CommState::StateEnum & next_state); void transitionToState(GoalHandleT & gh, const CommState & next_state); void processLost(GoalHandleT & gh); private: CommStateMachine(); // State CommState state_; ActionGoalConstPtr action_goal_; actionlib_msgs::GoalStatus latest_goal_status_; ActionResultConstPtr latest_result_; // Callbacks TransitionCallback transition_cb_; FeedbackCallback feedback_cb_; // **** Implementation **** //! Change the state, as well as print out ROS_DEBUG info void setCommState(const CommState & state); void setCommState(const CommState::StateEnum & state); const actionlib_msgs::GoalStatus * findGoalStatus( const std::vector & status_vec) const; }; } // namespace actionlib #include "actionlib/client/goal_manager_imp.h" #include "actionlib/client/client_goal_handle_imp.h" #include "actionlib/client/comm_state_machine_imp.h" #endif // ACTIONLIB__CLIENT__CLIENT_HELPERS_H_ actionlib-1.12.0/include/actionlib/client/comm_state.h000066400000000000000000000071101352264324200227120ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef ACTIONLIB__CLIENT__COMM_STATE_H_ #define ACTIONLIB__CLIENT__COMM_STATE_H_ #include #include "ros/console.h" namespace actionlib { /** * \brief Thin wrapper around an enum in order to help interpret the state of the communication state machine **/ class CommState { public: //! \brief Defines the various states the Communication State Machine can be in enum StateEnum { WAITING_FOR_GOAL_ACK = 0, PENDING = 1, ACTIVE = 2, WAITING_FOR_RESULT = 3, WAITING_FOR_CANCEL_ACK = 4, RECALLING = 5, PREEMPTING = 6, DONE = 7 }; CommState(const StateEnum & state) : state_(state) {} inline bool operator==(const CommState & rhs) const { return state_ == rhs.state_; } inline bool operator==(const CommState::StateEnum & rhs) const { return state_ == rhs; } inline bool operator!=(const CommState::StateEnum & rhs) const { return !(*this == rhs); } inline bool operator!=(const CommState & rhs) const { return !(*this == rhs); } std::string toString() const { switch (state_) { case WAITING_FOR_GOAL_ACK: return "WAITING_FOR_GOAL_ACK"; case PENDING: return "PENDING"; case ACTIVE: return "ACTIVE"; case WAITING_FOR_RESULT: return "WAITING_FOR_RESULT"; case WAITING_FOR_CANCEL_ACK: return "WAITING_FOR_CANCEL_ACK"; case RECALLING: return "RECALLING"; case PREEMPTING: return "PREEMPTING"; case DONE: return "DONE"; default: ROS_ERROR_NAMED("actionlib", "BUG: Unhandled CommState: %u", state_); break; } return "BUG-UNKNOWN"; } StateEnum state_; private: CommState(); }; } // namespace actionlib #endif // ACTIONLIB__CLIENT__COMM_STATE_H_ actionlib-1.12.0/include/actionlib/client/comm_state_machine_imp.h000066400000000000000000000453661352264324200252620ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* This file has the template implementation for CommStateMachine. It should be included with the * class definition. */ #ifndef ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_ #define ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_ #include #include "ros/console.h" namespace actionlib { template CommStateMachine::CommStateMachine(const ActionGoalConstPtr & action_goal, TransitionCallback transition_cb, FeedbackCallback feedback_cb) : state_(CommState::WAITING_FOR_GOAL_ACK) { assert(action_goal); action_goal_ = action_goal; transition_cb_ = transition_cb; feedback_cb_ = feedback_cb; // transitionToState( CommState::WAITING_FOR_GOAL_ACK ); } template typename CommStateMachine::ActionGoalConstPtr CommStateMachine:: getActionGoal() const { return action_goal_; } template CommState CommStateMachine::getCommState() const { return state_; } template actionlib_msgs::GoalStatus CommStateMachine::getGoalStatus() const { return latest_goal_status_; } template typename CommStateMachine::ResultConstPtr CommStateMachine::getResult() const { ResultConstPtr result; if (latest_result_) { EnclosureDeleter d(latest_result_); result = ResultConstPtr(&(latest_result_->result), d); } return result; } template void CommStateMachine::setCommState(const CommState::StateEnum & state) { setCommState(CommState(state)); } template void CommStateMachine::setCommState(const CommState & state) { ROS_DEBUG_NAMED("actionlib", "Transitioning CommState from %s to %s", state_.toString().c_str(), state.toString().c_str()); state_ = state; } template const actionlib_msgs::GoalStatus * CommStateMachine::findGoalStatus( const std::vector & status_vec) const { for (unsigned int i = 0; i < status_vec.size(); i++) { if (status_vec[i].goal_id.id == action_goal_->goal_id.id) { return &status_vec[i]; } } return NULL; } template void CommStateMachine::updateFeedback(GoalHandleT & gh, const ActionFeedbackConstPtr & action_feedback) { // Check if this feedback is for us if (action_goal_->goal_id.id != action_feedback->status.goal_id.id) { return; } if (feedback_cb_) { EnclosureDeleter d(action_feedback); FeedbackConstPtr feedback(&(action_feedback->feedback), d); feedback_cb_(gh, feedback); } } template void CommStateMachine::updateResult(GoalHandleT & gh, const ActionResultConstPtr & action_result) { // Check if this feedback is for us if (action_goal_->goal_id.id != action_result->status.goal_id.id) { return; } latest_goal_status_ = action_result->status; latest_result_ = action_result; switch (state_.state_) { case CommState::WAITING_FOR_GOAL_ACK: case CommState::PENDING: case CommState::ACTIVE: case CommState::WAITING_FOR_RESULT: case CommState::WAITING_FOR_CANCEL_ACK: case CommState::RECALLING: case CommState::PREEMPTING: { // A little bit of hackery to call all the right state transitions before processing result actionlib_msgs::GoalStatusArrayPtr status_array(new actionlib_msgs::GoalStatusArray()); status_array->status_list.push_back(action_result->status); updateStatus(gh, status_array); transitionToState(gh, CommState::DONE); break; } case CommState::DONE: ROS_ERROR_NAMED("actionlib", "Got a result when we were already in the DONE state"); break; default: ROS_ERROR_NAMED("actionlib", "In a funny comm state: %u", state_.state_); break; } } template void CommStateMachine::updateStatus(GoalHandleT & gh, const actionlib_msgs::GoalStatusArrayConstPtr & status_array) { const actionlib_msgs::GoalStatus * goal_status = findGoalStatus(status_array->status_list); // It's possible to receive old GoalStatus messages over the wire, even after receiving Result with a terminal state. // Thus, we want to ignore all status that we get after we're done, because it is irrelevant. (See trac #2721) if (state_ == CommState::DONE) { return; } if (goal_status) { latest_goal_status_ = *goal_status; } else { if (state_ != CommState::WAITING_FOR_GOAL_ACK && state_ != CommState::WAITING_FOR_RESULT && state_ != CommState::DONE) { processLost(gh); } return; } switch (state_.state_) { case CommState::WAITING_FOR_GOAL_ACK: { if (goal_status) { switch (goal_status->status) { case actionlib_msgs::GoalStatus::PENDING: transitionToState(gh, CommState::PENDING); break; case actionlib_msgs::GoalStatus::ACTIVE: transitionToState(gh, CommState::ACTIVE); break; case actionlib_msgs::GoalStatus::PREEMPTED: transitionToState(gh, CommState::ACTIVE); transitionToState(gh, CommState::PREEMPTING); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::SUCCEEDED: transitionToState(gh, CommState::ACTIVE); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::ABORTED: transitionToState(gh, CommState::ACTIVE); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::REJECTED: transitionToState(gh, CommState::PENDING); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::RECALLED: transitionToState(gh, CommState::PENDING); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::PREEMPTING: transitionToState(gh, CommState::ACTIVE); transitionToState(gh, CommState::PREEMPTING); break; case actionlib_msgs::GoalStatus::RECALLING: transitionToState(gh, CommState::PENDING); transitionToState(gh, CommState::RECALLING); break; default: ROS_ERROR_NAMED("actionlib", "BUG: Got an unknown status from the ActionServer. status = %u", goal_status->status); break; } } break; } case CommState::PENDING: { switch (goal_status->status) { case actionlib_msgs::GoalStatus::PENDING: break; case actionlib_msgs::GoalStatus::ACTIVE: transitionToState(gh, CommState::ACTIVE); break; case actionlib_msgs::GoalStatus::PREEMPTED: transitionToState(gh, CommState::ACTIVE); transitionToState(gh, CommState::PREEMPTING); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::SUCCEEDED: transitionToState(gh, CommState::ACTIVE); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::ABORTED: transitionToState(gh, CommState::ACTIVE); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::REJECTED: transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::RECALLED: transitionToState(gh, CommState::RECALLING); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::PREEMPTING: transitionToState(gh, CommState::ACTIVE); transitionToState(gh, CommState::PREEMPTING); break; case actionlib_msgs::GoalStatus::RECALLING: transitionToState(gh, CommState::RECALLING); break; default: ROS_ERROR_NAMED("actionlib", "BUG: Got an unknown goal status from the ActionServer. status = %u", goal_status->status); break; } break; } case CommState::ACTIVE: { switch (goal_status->status) { case actionlib_msgs::GoalStatus::PENDING: ROS_ERROR_NAMED("actionlib", "Invalid transition from ACTIVE to PENDING"); break; case actionlib_msgs::GoalStatus::ACTIVE: break; case actionlib_msgs::GoalStatus::REJECTED: ROS_ERROR_NAMED("actionlib", "Invalid transition from ACTIVE to REJECTED"); break; case actionlib_msgs::GoalStatus::RECALLING: ROS_ERROR_NAMED("actionlib", "Invalid transition from ACTIVE to RECALLING"); break; case actionlib_msgs::GoalStatus::RECALLED: ROS_ERROR_NAMED("actionlib", "Invalid transition from ACTIVE to RECALLED"); break; case actionlib_msgs::GoalStatus::PREEMPTED: transitionToState(gh, CommState::PREEMPTING); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::SUCCEEDED: case actionlib_msgs::GoalStatus::ABORTED: transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::PREEMPTING: transitionToState(gh, CommState::PREEMPTING); break; default: ROS_ERROR_NAMED("actionlib", "BUG: Got an unknown goal status from the ActionServer. status = %u", goal_status->status); break; } break; } case CommState::WAITING_FOR_RESULT: { switch (goal_status->status) { case actionlib_msgs::GoalStatus::PENDING: ROS_ERROR_NAMED("actionlib", "Invalid Transition from WAITING_FOR_RESUT to PENDING"); break; case actionlib_msgs::GoalStatus::PREEMPTING: ROS_ERROR_NAMED("actionlib", "Invalid Transition from WAITING_FOR_RESUT to PREEMPTING"); break; case actionlib_msgs::GoalStatus::RECALLING: ROS_ERROR_NAMED("actionlib", "Invalid Transition from WAITING_FOR_RESUT to RECALLING"); break; case actionlib_msgs::GoalStatus::ACTIVE: case actionlib_msgs::GoalStatus::PREEMPTED: case actionlib_msgs::GoalStatus::SUCCEEDED: case actionlib_msgs::GoalStatus::ABORTED: case actionlib_msgs::GoalStatus::REJECTED: case actionlib_msgs::GoalStatus::RECALLED: break; default: ROS_ERROR_NAMED("actionlib", "BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status); break; } break; } case CommState::WAITING_FOR_CANCEL_ACK: { switch (goal_status->status) { case actionlib_msgs::GoalStatus::PENDING: break; case actionlib_msgs::GoalStatus::ACTIVE: break; case actionlib_msgs::GoalStatus::SUCCEEDED: case actionlib_msgs::GoalStatus::ABORTED: case actionlib_msgs::GoalStatus::PREEMPTED: transitionToState(gh, CommState::PREEMPTING); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::RECALLED: transitionToState(gh, CommState::RECALLING); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::REJECTED: transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::PREEMPTING: transitionToState(gh, CommState::PREEMPTING); break; case actionlib_msgs::GoalStatus::RECALLING: transitionToState(gh, CommState::RECALLING); break; default: ROS_ERROR_NAMED("actionlib", "BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status); break; } break; } case CommState::RECALLING: { switch (goal_status->status) { case actionlib_msgs::GoalStatus::PENDING: ROS_ERROR_NAMED("actionlib", "Invalid Transition from RECALLING to PENDING"); break; case actionlib_msgs::GoalStatus::ACTIVE: ROS_ERROR_NAMED("actionlib", "Invalid Transition from RECALLING to ACTIVE"); break; case actionlib_msgs::GoalStatus::SUCCEEDED: case actionlib_msgs::GoalStatus::ABORTED: case actionlib_msgs::GoalStatus::PREEMPTED: transitionToState(gh, CommState::PREEMPTING); transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::RECALLED: transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::REJECTED: transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::PREEMPTING: transitionToState(gh, CommState::PREEMPTING); break; case actionlib_msgs::GoalStatus::RECALLING: break; default: ROS_ERROR_NAMED("actionlib", "BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status); break; } break; } case CommState::PREEMPTING: { switch (goal_status->status) { case actionlib_msgs::GoalStatus::PENDING: ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to PENDING"); break; case actionlib_msgs::GoalStatus::ACTIVE: ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to ACTIVE"); break; case actionlib_msgs::GoalStatus::REJECTED: ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to REJECTED"); break; case actionlib_msgs::GoalStatus::RECALLING: ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to RECALLING"); break; case actionlib_msgs::GoalStatus::RECALLED: ROS_ERROR_NAMED("actionlib", "Invalid Transition from PREEMPTING to RECALLED"); break; break; case actionlib_msgs::GoalStatus::PREEMPTED: case actionlib_msgs::GoalStatus::SUCCEEDED: case actionlib_msgs::GoalStatus::ABORTED: transitionToState(gh, CommState::WAITING_FOR_RESULT); break; case actionlib_msgs::GoalStatus::PREEMPTING: break; default: ROS_ERROR_NAMED("actionlib", "BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status); break; } break; } case CommState::DONE: { switch (goal_status->status) { case actionlib_msgs::GoalStatus::PENDING: ROS_ERROR_NAMED("actionlib", "Invalid Transition from DONE to PENDING"); break; case actionlib_msgs::GoalStatus::ACTIVE: ROS_ERROR_NAMED("actionlib", "Invalid Transition from DONE to ACTIVE"); break; case actionlib_msgs::GoalStatus::RECALLING: ROS_ERROR_NAMED("actionlib", "Invalid Transition from DONE to RECALLING"); break; case actionlib_msgs::GoalStatus::PREEMPTING: ROS_ERROR_NAMED("actionlib", "Invalid Transition from DONE to PREEMPTING"); break; case actionlib_msgs::GoalStatus::PREEMPTED: case actionlib_msgs::GoalStatus::SUCCEEDED: case actionlib_msgs::GoalStatus::ABORTED: case actionlib_msgs::GoalStatus::RECALLED: case actionlib_msgs::GoalStatus::REJECTED: break; default: ROS_ERROR_NAMED("actionlib", "BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status); break; } break; } default: ROS_ERROR_NAMED("actionlib", "In a funny comm state: %u", state_.state_); break; } } template void CommStateMachine::processLost(GoalHandleT & gh) { ROS_WARN_NAMED("actionlib", "Transitioning goal to LOST"); latest_goal_status_.status = actionlib_msgs::GoalStatus::LOST; transitionToState(gh, CommState::DONE); } template void CommStateMachine::transitionToState(GoalHandleT & gh, const CommState::StateEnum & next_state) { transitionToState(gh, CommState(next_state)); } template void CommStateMachine::transitionToState(GoalHandleT & gh, const CommState & next_state) { ROS_DEBUG_NAMED("actionlib", "Trying to transition to %s", next_state.toString().c_str()); setCommState(next_state); if (transition_cb_) { transition_cb_(gh); } } } // namespace actionlib #endif // ACTIONLIB__CLIENT__COMM_STATE_MACHINE_IMP_H_ actionlib-1.12.0/include/actionlib/client/connection_monitor.h000066400000000000000000000064771352264324200245040ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef ACTIONLIB__CLIENT__CONNECTION_MONITOR_H_ #define ACTIONLIB__CLIENT__CONNECTION_MONITOR_H_ #include #include #include #include #include #include #include #include namespace actionlib { class ACTIONLIB_DECL ConnectionMonitor { public: ConnectionMonitor(ros::Subscriber & feedback_sub, ros::Subscriber & result_sub); void goalConnectCallback(const ros::SingleSubscriberPublisher & pub); void goalDisconnectCallback(const ros::SingleSubscriberPublisher & pub); void cancelConnectCallback(const ros::SingleSubscriberPublisher & pub); void cancelDisconnectCallback(const ros::SingleSubscriberPublisher & pub); void processStatus(const actionlib_msgs::GoalStatusArrayConstPtr & status, const std::string & cur_status_caller_id); bool waitForActionServerToStart(const ros::Duration & timeout = ros::Duration(0, 0), const ros::NodeHandle & nh = ros::NodeHandle() ); bool isServerConnected(); private: // status stuff std::string status_caller_id_; bool status_received_; ros::Time latest_status_time_; boost::condition check_connection_condition_; boost::recursive_mutex data_mutex_; std::map goalSubscribers_; std::map cancelSubscribers_; std::string goalSubscribersString(); std::string cancelSubscribersString(); ros::Subscriber & feedback_sub_; ros::Subscriber & result_sub_; }; } // namespace actionlib #endif // ACTIONLIB__CLIENT__CONNECTION_MONITOR_H_ actionlib-1.12.0/include/actionlib/client/goal_manager_imp.h000066400000000000000000000122571352264324200240500ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* This file has the template implementation for GoalHandle. It should be included with the * class definition. */ #ifndef ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_ #define ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_ #include "ros/ros.h" namespace actionlib { template void GoalManager::registerSendGoalFunc(SendGoalFunc send_goal_func) { send_goal_func_ = send_goal_func; } template void GoalManager::registerCancelFunc(CancelFunc cancel_func) { cancel_func_ = cancel_func; } template ClientGoalHandle GoalManager::initGoal(const Goal & goal, TransitionCallback transition_cb, FeedbackCallback feedback_cb) { ActionGoalPtr action_goal(new ActionGoal); action_goal->header.stamp = ros::Time::now(); action_goal->goal_id = id_generator_.generateID(); action_goal->goal = goal; typedef CommStateMachine CommStateMachineT; boost::shared_ptr comm_state_machine(new CommStateMachineT(action_goal, transition_cb, feedback_cb)); boost::recursive_mutex::scoped_lock lock(list_mutex_); typename ManagedListT::Handle list_handle = list_.add(comm_state_machine, boost::bind(&GoalManagerT::listElemDeleter, this, _1), guard_); if (send_goal_func_) { send_goal_func_(action_goal); } else { ROS_WARN_NAMED("actionlib", "Possible coding error: send_goal_func_ set to NULL. Not going to send goal"); } return GoalHandleT(this, list_handle, guard_); } template void GoalManager::listElemDeleter(typename ManagedListT::iterator it) { assert(guard_); if (!guard_) { ROS_ERROR_NAMED("actionlib", "Goal manager deleter should not see invalid guards"); return; } DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "This action client associated with the goal handle has already been destructed. Not going to try delete the CommStateMachine associated with this goal"); return; } ROS_DEBUG_NAMED("actionlib", "About to erase CommStateMachine"); boost::recursive_mutex::scoped_lock lock(list_mutex_); list_.erase(it); ROS_DEBUG_NAMED("actionlib", "Done erasing CommStateMachine"); } template void GoalManager::updateStatuses( const actionlib_msgs::GoalStatusArrayConstPtr & status_array) { boost::recursive_mutex::scoped_lock lock(list_mutex_); typename ManagedListT::iterator it = list_.begin(); while (it != list_.end()) { GoalHandleT gh(this, it.createHandle(), guard_); (*it)->updateStatus(gh, status_array); ++it; } } template void GoalManager::updateFeedbacks(const ActionFeedbackConstPtr & action_feedback) { boost::recursive_mutex::scoped_lock lock(list_mutex_); typename ManagedListT::iterator it = list_.begin(); while (it != list_.end()) { GoalHandleT gh(this, it.createHandle(), guard_); (*it)->updateFeedback(gh, action_feedback); ++it; } } template void GoalManager::updateResults(const ActionResultConstPtr & action_result) { boost::recursive_mutex::scoped_lock lock(list_mutex_); typename ManagedListT::iterator it = list_.begin(); while (it != list_.end()) { GoalHandleT gh(this, it.createHandle(), guard_); (*it)->updateResult(gh, action_result); ++it; } } } // namespace actionlib #endif // ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_ actionlib-1.12.0/include/actionlib/client/service_client.h000066400000000000000000000066471352264324200235730ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__CLIENT__SERVICE_CLIENT_H_ #define ACTIONLIB__CLIENT__SERVICE_CLIENT_H_ #include #include #include namespace actionlib { class ServiceClientImp { public: ServiceClientImp() {} virtual bool call(const void * goal, std::string goal_md5sum, void * result, std::string result_md5sum) = 0; virtual bool waitForServer(const ros::Duration & timeout) = 0; virtual bool isServerConnected() = 0; virtual ~ServiceClientImp() {} }; class ServiceClient { public: ServiceClient(boost::shared_ptr client) : client_(client) {} template bool call(const Goal & goal, Result & result); bool waitForServer(const ros::Duration & timeout = ros::Duration(0, 0)); bool isServerConnected(); private: boost::shared_ptr client_; }; template ServiceClient serviceClient(ros::NodeHandle n, std::string name); template class ServiceClientImpT : public ServiceClientImp { public: ACTION_DEFINITION(ActionSpec); typedef ClientGoalHandle GoalHandleT; typedef SimpleActionClient SimpleActionClientT; ServiceClientImpT(ros::NodeHandle n, std::string name); bool call(const void * goal, std::string goal_md5sum, void * result, std::string result_md5sum); bool waitForServer(const ros::Duration & timeout); bool isServerConnected(); private: boost::scoped_ptr ac_; }; } // namespace actionlib // include the implementation #include #endif // ACTIONLIB__CLIENT__SERVICE_CLIENT_H_ actionlib-1.12.0/include/actionlib/client/service_client_imp.h000066400000000000000000000102401352264324200244200ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__CLIENT__SERVICE_CLIENT_IMP_H_ #define ACTIONLIB__CLIENT__SERVICE_CLIENT_IMP_H_ #include #include "ros/console.h" namespace actionlib { template ServiceClientImpT::ServiceClientImpT(ros::NodeHandle n, std::string name) { ac_.reset(new SimpleActionClientT(n, name, true)); } template bool ServiceClientImpT::waitForServer(const ros::Duration & timeout) { return ac_->waitForServer(timeout); } template bool ServiceClientImpT::isServerConnected() { return ac_->isServerConnected(); } template bool ServiceClientImpT::call(const void * goal, std::string goal_md5sum, void * result, std::string result_md5sum) { // ok... we need to static cast the goal message and result message const Goal * goal_c = static_cast(goal); Result * result_c = static_cast(result); // now we need to check that the md5sums are correct namespace mt = ros::message_traits; if (strcmp(mt::md5sum(*goal_c), goal_md5sum.c_str()) || strcmp(mt::md5sum(*result_c), result_md5sum.c_str())) { ROS_ERROR_NAMED("actionlib", "Incorrect md5Sums for goal and result types"); return false; } if (!ac_->isServerConnected()) { ROS_ERROR_NAMED("actionlib", "Attempting to make a service call when the server isn't actually connected to the client."); return false; } ac_->sendGoalAndWait(*goal_c); if (ac_->getState() == SimpleClientGoalState::SUCCEEDED) { (*result_c) = *(ac_->getResult()); return true; } return false; } //****** ServiceClient ******************* template bool ServiceClient::call(const Goal & goal, Result & result) { namespace mt = ros::message_traits; return client_->call(&goal, mt::md5sum(goal), &result, mt::md5sum(result)); } bool ServiceClient::waitForServer(const ros::Duration & timeout) { return client_->waitForServer(timeout); } bool ServiceClient::isServerConnected() { return client_->isServerConnected(); } //****** actionlib::serviceClient ******************* template ServiceClient serviceClient(ros::NodeHandle n, std::string name) { boost::shared_ptr client_ptr(new ServiceClientImpT(n, name)); return ServiceClient(client_ptr); } } // namespace actionlib #endif // ACTIONLIB__CLIENT__SERVICE_CLIENT_IMP_H_ actionlib-1.12.0/include/actionlib/client/simple_action_client.h000066400000000000000000000544701352264324200247560ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_ #define ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_ #include #include #include #include #include #include "ros/ros.h" #include "ros/callback_queue.h" #include "actionlib/client/action_client.h" #include "actionlib/client/simple_goal_state.h" #include "actionlib/client/simple_client_goal_state.h" #include "actionlib/client/terminal_state.h" #ifndef DEPRECATED #if defined(__GNUC__) #define DEPRECATED __attribute__((deprecated)) #else #define DEPRECATED #endif #endif namespace actionlib { /** * \brief A Simple client implementation of the ActionInterface which supports only one goal at a time * * The SimpleActionClient wraps the existing ActionClient, and exposes a limited set of easy-to-use hooks * for the user. Note that the concept of GoalHandles has been completely hidden from the user, and that * they must query the SimplyActionClient directly in order to monitor a goal. */ template class SimpleActionClient { private: ACTION_DEFINITION(ActionSpec); typedef ClientGoalHandle GoalHandleT; typedef SimpleActionClient SimpleActionClientT; public: typedef boost::function SimpleDoneCallback; typedef boost::function SimpleActiveCallback; typedef boost::function SimpleFeedbackCallback; /** * \brief Simple constructor * * Constructs a SingleGoalActionClient and sets up the necessary ros topics for the ActionInterface * \param name The action name. Defines the namespace in which the action communicates * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false, * then the user has to call ros::spin() themselves. Defaults to True */ SimpleActionClient(const std::string & name, bool spin_thread = true) : cur_simple_state_(SimpleGoalState::PENDING) { initSimpleClient(nh_, name, spin_thread); } /** * \brief Constructor with namespacing options * * Constructs a SingleGoalActionClient and sets up the necessary ros topics for * the ActionInterface, and namespaces them according the a specified NodeHandle * \param n The node handle on top of which we want to namespace our action * \param name The action name. Defines the namespace in which the action communicates * \param spin_thread If true, spins up a thread to service this action's subscriptions. If false, * then the user has to call ros::spin() themselves. Defaults to True */ SimpleActionClient(ros::NodeHandle & n, const std::string & name, bool spin_thread = true) : cur_simple_state_(SimpleGoalState::PENDING) { initSimpleClient(n, name, spin_thread); } ~SimpleActionClient(); /** * \brief Waits for the ActionServer to connect to this client * * Often, it can take a second for the action server & client to negotiate * a connection, thus, risking the first few goals to be dropped. This call lets * the user wait until the network connection to the server is negotiated. * NOTE: Using this call in a single threaded ROS application, or any * application where the action client's callback queue is not being * serviced, will not work. Without a separate thread servicing the queue, or * a multi-threaded spinner, there is no way for the client to tell whether * or not the server is up because it can't receive a status message. * \param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout. * \return True if the server connected in the allocated time. False on timeout */ bool waitForServer(const ros::Duration & timeout = ros::Duration(0, 0) ) const { return ac_->waitForActionServerToStart(timeout); } /** * @brief Checks if the action client is successfully connected to the action server * @return True if the server is connected, false otherwise */ bool isServerConnected() const { return ac_->isServerConnected(); } /** * \brief Sends a goal to the ActionServer, and also registers callbacks * * If a previous goal is already active when this is called. We simply forget * about that goal and start tracking the new goal. No cancel requests are made. * \param done_cb Callback that gets called on transitions to Done * \param active_cb Callback that gets called on transitions to Active * \param feedback_cb Callback that gets called whenever feedback for this goal is received */ void sendGoal(const Goal & goal, SimpleDoneCallback done_cb = SimpleDoneCallback(), SimpleActiveCallback active_cb = SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb = SimpleFeedbackCallback()); /** * \brief Sends a goal to the ActionServer, and waits until the goal completes or a timeout is exceeded * * If the goal doesn't complete by the execute_timeout, then a preempt message is sent. This call * then waits up to the preempt_timeout for the goal to then finish. * * \param goal The goal to be sent to the ActionServer * \param execute_timeout Time to wait until a preempt is sent. 0 implies wait forever * \param preempt_timeout Time to wait after a preempt is sent. 0 implies wait forever * \return The state of the goal when this call is completed */ SimpleClientGoalState sendGoalAndWait(const Goal & goal, const ros::Duration & execute_timeout = ros::Duration(0, 0), const ros::Duration & preempt_timeout = ros::Duration(0, 0)); /** * \brief Blocks until this goal finishes * \param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout. * \return True if the goal finished. False if the goal didn't finish within the allocated timeout */ bool waitForResult(const ros::Duration & timeout = ros::Duration(0, 0)); /** * \brief Get the Result of the current goal * \return shared pointer to the result. Note that this pointer will NEVER be NULL */ ResultConstPtr getResult() const; /** * \brief Get the state information for this goal * * Possible States Are: PENDING, ACTIVE, RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST. * \return The goal's state. Returns LOST if this SimpleActionClient isn't tracking a goal. */ SimpleClientGoalState getState() const; /** * \brief Cancel all goals currently running on the action server * * This preempts all goals running on the action server at the point that * this message is serviced by the ActionServer. */ void cancelAllGoals(); /** * \brief Cancel all goals that were stamped at and before the specified time * \param time All goals stamped at or before `time` will be canceled */ void cancelGoalsAtAndBeforeTime(const ros::Time & time); /** * \brief Cancel the goal that we are currently pursuing */ void cancelGoal(); /** * \brief Stops tracking the state of the current goal. Unregisters this goal's callbacks * * This is useful if we want to make sure we stop calling our callbacks before sending a new goal. * Note that this does not cancel the goal, it simply stops looking for status info about this goal. */ void stopTrackingGoal(); private: typedef ActionClient ActionClientT; ros::NodeHandle nh_; GoalHandleT gh_; SimpleGoalState cur_simple_state_; // Signalling Stuff boost::condition done_condition_; boost::mutex done_mutex_; // User Callbacks SimpleDoneCallback done_cb_; SimpleActiveCallback active_cb_; SimpleFeedbackCallback feedback_cb_; // Spin Thread Stuff boost::mutex terminate_mutex_; bool need_to_terminate_; boost::thread * spin_thread_; ros::CallbackQueue callback_queue; boost::scoped_ptr ac_; // Action client depends on callback_queue, so it must be destroyed before callback_queue // ***** Private Funcs ***** void initSimpleClient(ros::NodeHandle & n, const std::string & name, bool spin_thread); void handleTransition(GoalHandleT gh); void handleFeedback(GoalHandleT gh, const FeedbackConstPtr & feedback); void setSimpleState(const SimpleGoalState::StateEnum & next_state); void setSimpleState(const SimpleGoalState & next_state); void spinThread(); }; template void SimpleActionClient::initSimpleClient(ros::NodeHandle & n, const std::string & name, bool spin_thread) { if (spin_thread) { ROS_DEBUG_NAMED("actionlib", "Spinning up a thread for the SimpleActionClient"); need_to_terminate_ = false; spin_thread_ = new boost::thread(boost::bind(&SimpleActionClient::spinThread, this)); ac_.reset(new ActionClientT(n, name, &callback_queue)); } else { spin_thread_ = NULL; ac_.reset(new ActionClientT(n, name)); } } template SimpleActionClient::~SimpleActionClient() { if (spin_thread_) { { boost::mutex::scoped_lock terminate_lock(terminate_mutex_); need_to_terminate_ = true; } spin_thread_->join(); delete spin_thread_; } gh_.reset(); ac_.reset(); } template void SimpleActionClient::spinThread() { while (nh_.ok()) { { boost::mutex::scoped_lock terminate_lock(terminate_mutex_); if (need_to_terminate_) { break; } } callback_queue.callAvailable(ros::WallDuration(0.1f)); } } template void SimpleActionClient::setSimpleState(const SimpleGoalState::StateEnum & next_state) { setSimpleState(SimpleGoalState(next_state) ); } template void SimpleActionClient::setSimpleState(const SimpleGoalState & next_state) { ROS_DEBUG_NAMED("actionlib", "Transitioning SimpleState from [%s] to [%s]", cur_simple_state_.toString().c_str(), next_state.toString().c_str()); cur_simple_state_ = next_state; } template void SimpleActionClient::sendGoal(const Goal & goal, SimpleDoneCallback done_cb, SimpleActiveCallback active_cb, SimpleFeedbackCallback feedback_cb) { // Reset the old GoalHandle, so that our callbacks won't get called anymore gh_.reset(); // Store all the callbacks done_cb_ = done_cb; active_cb_ = active_cb; feedback_cb_ = feedback_cb; cur_simple_state_ = SimpleGoalState::PENDING; // Send the goal to the ActionServer gh_ = ac_->sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1), boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2)); } template SimpleClientGoalState SimpleActionClient::getState() const { if (gh_.isExpired()) { return SimpleClientGoalState(SimpleClientGoalState::LOST); } CommState comm_state_ = gh_.getCommState(); switch (comm_state_.state_) { case CommState::WAITING_FOR_GOAL_ACK: case CommState::PENDING: case CommState::RECALLING: return SimpleClientGoalState(SimpleClientGoalState::PENDING); case CommState::ACTIVE: case CommState::PREEMPTING: return SimpleClientGoalState(SimpleClientGoalState::ACTIVE); case CommState::DONE: { switch (gh_.getTerminalState().state_) { case TerminalState::RECALLED: return SimpleClientGoalState(SimpleClientGoalState::RECALLED, gh_.getTerminalState().text_); case TerminalState::REJECTED: return SimpleClientGoalState(SimpleClientGoalState::REJECTED, gh_.getTerminalState().text_); case TerminalState::PREEMPTED: return SimpleClientGoalState(SimpleClientGoalState::PREEMPTED, gh_.getTerminalState().text_); case TerminalState::ABORTED: return SimpleClientGoalState(SimpleClientGoalState::ABORTED, gh_.getTerminalState().text_); case TerminalState::SUCCEEDED: return SimpleClientGoalState(SimpleClientGoalState::SUCCEEDED, gh_.getTerminalState().text_); case TerminalState::LOST: return SimpleClientGoalState(SimpleClientGoalState::LOST, gh_.getTerminalState().text_); default: ROS_ERROR_NAMED("actionlib", "Unknown terminal state [%u]. This is a bug in SimpleActionClient", gh_.getTerminalState().state_); return SimpleClientGoalState(SimpleClientGoalState::LOST, gh_.getTerminalState().text_); } } case CommState::WAITING_FOR_RESULT: case CommState::WAITING_FOR_CANCEL_ACK: { switch (cur_simple_state_.state_) { case SimpleGoalState::PENDING: return SimpleClientGoalState(SimpleClientGoalState::PENDING); case SimpleGoalState::ACTIVE: return SimpleClientGoalState(SimpleClientGoalState::ACTIVE); case SimpleGoalState::DONE: ROS_ERROR_NAMED("actionlib", "In WAITING_FOR_RESULT or WAITING_FOR_CANCEL_ACK, yet we are in SimpleGoalState DONE. This is a bug in SimpleActionClient"); return SimpleClientGoalState(SimpleClientGoalState::LOST); default: ROS_ERROR_NAMED("actionlib", "Got a SimpleGoalState of [%u]. This is a bug in SimpleActionClient", cur_simple_state_.state_); } } default: break; } ROS_ERROR_NAMED("actionlib", "Error trying to interpret CommState - %u", comm_state_.state_); return SimpleClientGoalState(SimpleClientGoalState::LOST); } template typename SimpleActionClient::ResultConstPtr SimpleActionClient::getResult() const { if (gh_.isExpired()) { ROS_ERROR_NAMED("actionlib", "Trying to getResult() when no goal is running. You are incorrectly using SimpleActionClient"); } if (gh_.getResult()) { return gh_.getResult(); } return ResultConstPtr(new Result); } template void SimpleActionClient::cancelAllGoals() { ac_->cancelAllGoals(); } template void SimpleActionClient::cancelGoalsAtAndBeforeTime(const ros::Time & time) { ac_->cancelGoalsAtAndBeforeTime(time); } template void SimpleActionClient::cancelGoal() { if (gh_.isExpired()) { ROS_ERROR_NAMED("actionlib", "Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient"); } gh_.cancel(); } template void SimpleActionClient::stopTrackingGoal() { if (gh_.isExpired()) { ROS_ERROR_NAMED("actionlib", "Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient"); } gh_.reset(); } template void SimpleActionClient::handleFeedback(GoalHandleT gh, const FeedbackConstPtr & feedback) { if (gh_ != gh) { ROS_ERROR_NAMED("actionlib", "Got a callback on a goalHandle that we're not tracking. \ This is an internal SimpleActionClient/ActionClient bug. \ This could also be a GoalID collision"); } if (feedback_cb_) { feedback_cb_(feedback); } } template void SimpleActionClient::handleTransition(GoalHandleT gh) { CommState comm_state_ = gh.getCommState(); switch (comm_state_.state_) { case CommState::WAITING_FOR_GOAL_ACK: ROS_ERROR_NAMED("actionlib", "BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK"); break; case CommState::PENDING: ROS_ERROR_COND(cur_simple_state_ != SimpleGoalState::PENDING, "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]", comm_state_.toString().c_str(), cur_simple_state_.toString().c_str()); break; case CommState::ACTIVE: switch (cur_simple_state_.state_) { case SimpleGoalState::PENDING: setSimpleState(SimpleGoalState::ACTIVE); if (active_cb_) { active_cb_(); } break; case SimpleGoalState::ACTIVE: break; case SimpleGoalState::DONE: ROS_ERROR_NAMED("actionlib", "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]", comm_state_.toString().c_str(), cur_simple_state_.toString().c_str()); break; default: ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_); break; } break; case CommState::WAITING_FOR_RESULT: break; case CommState::WAITING_FOR_CANCEL_ACK: break; case CommState::RECALLING: ROS_ERROR_COND(cur_simple_state_ != SimpleGoalState::PENDING, "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]", comm_state_.toString().c_str(), cur_simple_state_.toString().c_str()); break; case CommState::PREEMPTING: switch (cur_simple_state_.state_) { case SimpleGoalState::PENDING: setSimpleState(SimpleGoalState::ACTIVE); if (active_cb_) { active_cb_(); } break; case SimpleGoalState::ACTIVE: break; case SimpleGoalState::DONE: ROS_ERROR_NAMED("actionlib", "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]", comm_state_.toString().c_str(), cur_simple_state_.toString().c_str()); break; default: ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_); break; } break; case CommState::DONE: switch (cur_simple_state_.state_) { case SimpleGoalState::PENDING: case SimpleGoalState::ACTIVE: { boost::mutex::scoped_lock lock(done_mutex_); setSimpleState(SimpleGoalState::DONE); } if (done_cb_) { done_cb_(getState(), gh.getResult()); } done_condition_.notify_all(); break; case SimpleGoalState::DONE: ROS_ERROR_NAMED("actionlib", "BUG: Got a second transition to DONE"); break; default: ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_); break; } break; default: ROS_ERROR_NAMED("actionlib", "Unknown CommState received [%u]", comm_state_.state_); break; } } template bool SimpleActionClient::waitForResult(const ros::Duration & timeout) { if (gh_.isExpired()) { ROS_ERROR_NAMED("actionlib", "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient"); return false; } if (timeout < ros::Duration(0, 0)) { ROS_WARN_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec()); } ros::Time timeout_time = ros::Time::now() + timeout; boost::mutex::scoped_lock lock(done_mutex_); // Hardcode how often we check for node.ok() ros::Duration loop_period = ros::Duration().fromSec(.1); while (nh_.ok()) { // Determine how long we should wait ros::Duration time_left = timeout_time - ros::Time::now(); // Check if we're past the timeout time if (timeout > ros::Duration(0, 0) && time_left <= ros::Duration(0, 0) ) { break; } if (cur_simple_state_ == SimpleGoalState::DONE) { break; } // Truncate the time left if (time_left > loop_period || timeout == ros::Duration()) { time_left = loop_period; } done_condition_.timed_wait(lock, boost::posix_time::milliseconds(static_cast(time_left.toSec() * 1000.0f))); } return cur_simple_state_ == SimpleGoalState::DONE; } template SimpleClientGoalState SimpleActionClient::sendGoalAndWait(const Goal & goal, const ros::Duration & execute_timeout, const ros::Duration & preempt_timeout) { sendGoal(goal); // See if the goal finishes in time if (waitForResult(execute_timeout)) { ROS_DEBUG_NAMED("actionlib", "Goal finished within specified execute_timeout [%.2f]", execute_timeout.toSec()); return getState(); } ROS_DEBUG_NAMED("actionlib", "Goal didn't finish within specified execute_timeout [%.2f]", execute_timeout.toSec()); // It didn't finish in time, so we need to preempt it cancelGoal(); // Now wait again and see if it finishes if (waitForResult(preempt_timeout)) { ROS_DEBUG_NAMED("actionlib", "Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.toSec()); } else { ROS_DEBUG_NAMED("actionlib", "Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.toSec()); } return getState(); } } // namespace actionlib #undef DEPRECATED #endif // ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_ actionlib-1.12.0/include/actionlib/client/simple_client_goal_state.h000066400000000000000000000077101352264324200256160ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef ACTIONLIB__CLIENT__SIMPLE_CLIENT_GOAL_STATE_H_ #define ACTIONLIB__CLIENT__SIMPLE_CLIENT_GOAL_STATE_H_ #include #include "ros/console.h" namespace actionlib { class SimpleClientGoalState { public: //! \brief Defines the various states the goal can be in enum StateEnum { PENDING, ACTIVE, RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST }; StateEnum state_; std::string text_; SimpleClientGoalState(const StateEnum & state, const std::string & text = std::string("")) : state_(state), text_(text) {} inline bool operator==(const SimpleClientGoalState & rhs) const { return state_ == rhs.state_; } inline bool operator==(const SimpleClientGoalState::StateEnum & rhs) const { return state_ == rhs; } inline bool operator!=(const SimpleClientGoalState::StateEnum & rhs) const { return !(*this == rhs); } inline bool operator!=(const SimpleClientGoalState & rhs) const { return !(*this == rhs); } /** * \brief Determine if goal is done executing (ie. reached a terminal state) * \return True if in RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, or LOST. False otherwise */ inline bool isDone() const { switch (state_) { case RECALLED: case REJECTED: case PREEMPTED: case ABORTED: case SUCCEEDED: case LOST: return true; default: return false; } } std::string getText() const { return text_; } //! \brief Convert the state to a string. Useful when printing debugging information std::string toString() const { switch (state_) { case PENDING: return "PENDING"; case ACTIVE: return "ACTIVE"; case RECALLED: return "RECALLED"; case REJECTED: return "REJECTED"; case PREEMPTED: return "PREEMPTED"; case ABORTED: return "ABORTED"; case SUCCEEDED: return "SUCCEEDED"; case LOST: return "LOST"; default: ROS_ERROR_NAMED("actionlib", "BUG: Unhandled SimpleGoalState: %u", state_); break; } return "BUG-UNKNOWN"; } }; } // namespace actionlib #endif // ACTIONLIB__CLIENT__SIMPLE_CLIENT_GOAL_STATE_H_ actionlib-1.12.0/include/actionlib/client/simple_goal_state.h000066400000000000000000000062011352264324200242520ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef ACTIONLIB__CLIENT__SIMPLE_GOAL_STATE_H_ #define ACTIONLIB__CLIENT__SIMPLE_GOAL_STATE_H_ #include #include "ros/console.h" namespace actionlib { /** * \brief Thin wrapper around an enum in order providing a simplified version of the * communication state, but with less states than CommState **/ class SimpleGoalState { public: //! \brief Defines the various states the SimpleGoalState can be in enum StateEnum { PENDING, ACTIVE, DONE }; SimpleGoalState(const StateEnum & state) : state_(state) {} inline bool operator==(const SimpleGoalState & rhs) const { return state_ == rhs.state_; } inline bool operator==(const SimpleGoalState::StateEnum & rhs) const { return state_ == rhs; } inline bool operator!=(const SimpleGoalState::StateEnum & rhs) const { return !(*this == rhs); } inline bool operator!=(const SimpleGoalState & rhs) const { return !(*this == rhs); } std::string toString() const { switch (state_) { case PENDING: return "PENDING"; case ACTIVE: return "ACTIVE"; case DONE: return "DONE"; default: ROS_ERROR_NAMED("actionlib", "BUG: Unhandled SimpleGoalState: %u", state_); break; } return "BUG-UNKNOWN"; } StateEnum state_; private: SimpleGoalState(); }; } // namespace actionlib #endif // ACTIONLIB__CLIENT__SIMPLE_GOAL_STATE_H_ actionlib-1.12.0/include/actionlib/client/terminal_state.h000066400000000000000000000063261352264324200236020ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef ACTIONLIB__CLIENT__TERMINAL_STATE_H_ #define ACTIONLIB__CLIENT__TERMINAL_STATE_H_ #include #include "ros/console.h" namespace actionlib { class TerminalState { public: enum StateEnum { RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST }; TerminalState(const StateEnum & state, const std::string & text = std::string("")) : state_(state), text_(text) {} inline bool operator==(const TerminalState & rhs) const { return state_ == rhs.state_; } inline bool operator==(const TerminalState::StateEnum & rhs) const { return state_ == rhs; } inline bool operator!=(const TerminalState::StateEnum & rhs) const { return !(*this == rhs); } inline bool operator!=(const TerminalState & rhs) const { return !(*this == rhs); } std::string getText() const { return text_; } std::string toString() const { switch (state_) { case RECALLED: return "RECALLED"; case REJECTED: return "REJECTED"; case PREEMPTED: return "PREEMPTED"; case ABORTED: return "ABORTED"; case SUCCEEDED: return "SUCCEEDED"; case LOST: return "LOST"; default: ROS_ERROR_NAMED("actionlib", "BUG: Unhandled TerminalState: %u", state_); break; } return "BUG-UNKNOWN"; } StateEnum state_; std::string text_; private: TerminalState(); }; } // namespace actionlib #endif // ACTIONLIB__CLIENT__TERMINAL_STATE_H_ actionlib-1.12.0/include/actionlib/client_goal_status.h000066400000000000000000000134621352264324200231730ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef ACTIONLIB__CLIENT_GOAL_STATUS_H_ #define ACTIONLIB__CLIENT_GOAL_STATUS_H_ #include #include "actionlib/GoalStatus.h" #include "ros/console.h" namespace actionlib { /** * \brief Thin wrapper around an enum in order to help interpret the client-side status of a goal request * The possible states are defined the ClientGoalStatus::StateEnum. They are also defined in StateEnum. * we can also get there from \link ClientGoalStatus::StateEnum here \endlink **/ class ClientGoalStatus { public: //! \brief Defines the various states the Goal can be in, as perceived by the client enum StateEnum { PENDING, //!< The goal has yet to be processed by the action server ACTIVE, //!< The goal is currently being processed by the action server PREEMPTED, //!< The goal was preempted by either another goal, or a preempt message being sent to the action server SUCCEEDED, //!< The goal was achieved successfully by the action server ABORTED, //!< The goal was aborted by the action server REJECTED, //!< The ActionServer refused to start processing the goal, possibly because a goal is infeasible LOST //!< The goal was sent by the ActionClient, but disappeared due to some communication error }; ClientGoalStatus(StateEnum state) { state_ = state; } /** * \brief Build a ClientGoalStatus from a GoalStatus. * Note that the only GoalStatuses that can be converted into a * ClientGoalStatus are {PREEMPTED, SUCCEEDED, ABORTED, REJECTED} * \param goal_status The GoalStatus msg that we want to convert */ ClientGoalStatus(const GoalStatus & goal_status) { fromGoalStatus(goal_status); } /** * \brief Check if the goal is in a terminal state * \return TRUE if in PREEMPTED, SUCCEDED, ABORTED, REJECTED, or LOST */ inline bool isDone() const { if (state_ == PENDING || state_ == ACTIVE) { return false; } return true; } /** * \brief Copy the raw enum into the object */ inline const StateEnum & operator=(const StateEnum & state) { state_ = state; return state; } /** * \brief Straightforward enum equality check */ inline bool operator==(const ClientGoalStatus & rhs) const { return state_ == rhs.state_; } /** * \brief Straightforward enum inequality check */ inline bool operator!=(const ClientGoalStatus & rhs) const { return !(state_ == rhs.state_); } /** * \brief Store a GoalStatus in a ClientGoalStatus * Note that the only GoalStatuses that can be converted into a * ClientGoalStatus are {PREEMPTED, SUCCEEDED, ABORTED, REJECTED} * \param goal_status The GoalStatus msg that we want to convert */ void fromGoalStatus(const GoalStatus & goal_status) { switch (goal_status.status) { case GoalStatus::PREEMPTED: state_ = ClientGoalStatus::PREEMPTED; break; case GoalStatus::SUCCEEDED: state_ = ClientGoalStatus::SUCCEEDED; break; case GoalStatus::ABORTED: state_ = ClientGoalStatus::ABORTED; break; case GoalStatus::REJECTED: state_ = ClientGoalStatus::REJECTED; break; default: state_ = ClientGoalStatus::LOST; ROS_ERROR_NAMED("actionlib", "Cannot convert GoalStatus %u to ClientGoalState", goal_status.status); break; } } /** * \brief Stringify the enum * \return String that has the name of the enum */ std::string toString() const { switch (state_) { case PENDING: return "PENDING"; case ACTIVE: return "ACTIVE"; case PREEMPTED: return "PREEMPTED"; case SUCCEEDED: return "SUCCEEDED"; case ABORTED: return "ABORTED"; case REJECTED: return "REJECTED"; case LOST: return "LOST"; default: ROS_ERROR_NAMED("actionlib", "BUG: Unhandled ClientGoalStatus"); break; } return "BUG-UNKNOWN"; } private: StateEnum state_; ClientGoalStatus(); //!< Need to always specific an initial state. Thus, no empty constructor }; } // namespace actionlib #endif // ACTIONLIB__CLIENT_GOAL_STATUS_H_ actionlib-1.12.0/include/actionlib/decl.h000066400000000000000000000044441352264324200202170ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * *********************************************************************/ /* * Cross platform macros. * */ #ifndef ACTIONLIB__DECL_H_ #define ACTIONLIB__DECL_H_ #include #ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries #ifdef actionlib_EXPORTS // we are building a shared lib/dll #define ACTIONLIB_DECL ROS_HELPER_EXPORT #else // we are using shared lib/dll #define ACTIONLIB_DECL ROS_HELPER_IMPORT #endif #else // ros is being built around static libraries #define ACTIONLIB_DECL #endif #endif // ACTIONLIB__DECL_H_ actionlib-1.12.0/include/actionlib/destruction_guard.h000066400000000000000000000077131352264324200230370ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__DESTRUCTION_GUARD_H_ #define ACTIONLIB__DESTRUCTION_GUARD_H_ #include #include namespace actionlib { /** * @class DestructionGuard * @brief This class protects an object from being destructed until all users of that object relinquish control of it */ class DestructionGuard { public: /** * @brief Constructor for a DestructionGuard */ DestructionGuard() : use_count_(0), destructing_(false) {} void destruct() { boost::mutex::scoped_lock lock(mutex_); destructing_ = true; while (use_count_ > 0) { count_condition_.timed_wait(lock, boost::posix_time::milliseconds(1000)); } } /** * @brief Attempts to protect the guarded object from being destructed * @return True if protection succeeded, false if protection failed */ bool tryProtect() { boost::mutex::scoped_lock lock(mutex_); if (destructing_) { return false; } use_count_++; return true; } /** * @brief Releases protection on the guarded object */ void unprotect() { boost::mutex::scoped_lock lock(mutex_); use_count_--; } /** * @class ScopedProtector * @brief Protects a DestructionGuard until this object goes out of scope */ class ScopedProtector { public: /** * @brief Constructor for a ScopedProtector * @param guard The DestructionGuard to protect */ ScopedProtector(DestructionGuard & guard) : guard_(guard), protected_(false) { protected_ = guard_.tryProtect(); } /** * @brief Checks if the ScopedProtector successfully protected the DestructionGuard * @return True if protection succeeded, false otherwise */ bool isProtected() { return protected_; } /** * @brief Releases protection of the DestructionGuard if necessary */ ~ScopedProtector() { if (protected_) { guard_.unprotect(); } } private: DestructionGuard & guard_; bool protected_; }; private: boost::mutex mutex_; int use_count_; bool destructing_; boost::condition count_condition_; }; } // namespace actionlib #endif // ACTIONLIB__DESTRUCTION_GUARD_H_ actionlib-1.12.0/include/actionlib/enclosure_deleter.h000066400000000000000000000057111352264324200230110ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #include #ifndef ACTIONLIB__ENCLOSURE_DELETER_H_ #define ACTIONLIB__ENCLOSURE_DELETER_H_ namespace actionlib { /* * This allows the creation of a shared pointer to a section * of an already reference counted structure. For example, * if in the following picture Enclosure is reference counted with * a boost::shared_ptr and you want to return a boost::shared_ptr * to the Member that is referenced counted along with Enclosure objects * * Enclosure --------------- <--- Already reference counted * -----Member <------- A member of enclosure objects, eg. Enclosure.Member */ template class EnclosureDeleter { public: EnclosureDeleter(const boost::shared_ptr & enc_ptr) : enc_ptr_(enc_ptr) {} template void operator()(Member *) { enc_ptr_.reset(); } private: boost::shared_ptr enc_ptr_; }; template boost::shared_ptr share_member(boost::shared_ptr enclosure, Member & member) { EnclosureDeleter d(enclosure); boost::shared_ptr p(&member, d); return p; } } // namespace actionlib #endif // ACTIONLIB__ENCLOSURE_DELETER_H_ actionlib-1.12.0/include/actionlib/goal_id_generator.h000066400000000000000000000053131352264324200227500ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef ACTIONLIB__GOAL_ID_GENERATOR_H_ #define ACTIONLIB__GOAL_ID_GENERATOR_H_ #include #include #include #include "ros/time.h" #include "actionlib_msgs/GoalID.h" namespace actionlib { class ACTIONLIB_DECL GoalIDGenerator { public: /** * Create a generator that prepends the fully qualified node name to the Goal ID */ GoalIDGenerator(); /** * \param name Unique name to prepend to the goal id. This will * generally be a fully qualified node name. */ GoalIDGenerator(const std::string & name); /** * \param name Set the name to prepend to the goal id. This will * generally be a fully qualified node name. */ void setName(const std::string & name); /** * \brief Generates a unique ID * \return A unique GoalID for this action */ actionlib_msgs::GoalID generateID(); private: std::string name_; }; } // namespace actionlib #endif // ACTIONLIB__GOAL_ID_GENERATOR_H_ actionlib-1.12.0/include/actionlib/managed_list.h000066400000000000000000000172251352264324200217400ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef ACTIONLIB__MANAGED_LIST_H_ #define ACTIONLIB__MANAGED_LIST_H_ #include #include #include #include #include #include "ros/console.h" namespace actionlib { /** * \brief wrapper around an STL list to help with reference counting * Provides handles elements in an STL list. When all the handles go out of scope, * the element in the list is destroyed. */ template class ManagedList { private: struct TrackedElem { T elem; boost::weak_ptr handle_tracker_; }; public: class Handle; class iterator { public: iterator() {} T & operator*() {return it_->elem; } T & operator->() {return it_->elem; } const T & operator*() const {return it_->elem; } const T & operator->() const {return it_->elem; } bool operator==(const iterator & rhs) const {return it_ == rhs.it_; } bool operator!=(const iterator & rhs) const {return !(*this == rhs); } void operator++() {it_++; } Handle createHandle(); //!< \brief Creates a refcounted Handle from an iterator friend class ManagedList; private: iterator(typename std::list::iterator it) : it_(it) {} typename std::list::iterator it_; }; typedef typename boost::function CustomDeleter; private: class ElemDeleter { public: ElemDeleter(iterator it, CustomDeleter deleter, const boost::shared_ptr & guard) : it_(it), deleter_(deleter), guard_(guard) {} void operator()(void *) { DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "ManagedList: The DestructionGuard associated with this list has already been destructed. You must delete all list handles before deleting the ManagedList"); return; } ROS_DEBUG_NAMED("actionlib", "IN DELETER"); if (deleter_) { deleter_(it_); } } private: iterator it_; CustomDeleter deleter_; boost::shared_ptr guard_; }; public: class Handle { public: /** * \brief Construct an empty handle */ Handle() : it_(iterator()), handle_tracker_(boost::shared_ptr()), valid_(false) {} Handle & operator=(const Handle & rhs) { if (rhs.valid_) { it_ = rhs.it_; } handle_tracker_ = rhs.handle_tracker_; valid_ = rhs.valid_; return *this; } /** * \brief stop tracking the list element with this handle, even though the * Handle hasn't gone out of scope */ void reset() { valid_ = false; #ifndef _MSC_VER // this prevents a crash on MSVC, but I bet the problem is elsewhere. // it puts the lotion in the basket. it_ = iterator(); #endif handle_tracker_.reset(); } /** * \brief get the list element that this handle points to * fails/asserts if this is an empty handle * \return Reference to the element this handle points to */ T & getElem() { assert(valid_); if (!valid_) { ROS_ERROR_NAMED("actionlib","getElem() should not see invalid handles"); } return *it_; } const T & getElem() const { assert(valid_); if (!valid_) { ROS_ERROR_NAMED("actionlib","getElem() should not see invalid handles"); } return *it_; } /** * \brief Checks if two handles point to the same list elem */ bool operator==(const Handle & rhs) const { assert(valid_); if (!valid_) { ROS_ERROR_NAMED("actionlib", "operator== should not see invalid handles"); } assert(rhs.valid_); if (!rhs.valid_) { ROS_ERROR_NAMED("actionlib", "operator== should not see invalid RHS handles"); } return it_ == rhs.it_; } friend class ManagedList; // Need this friend declaration so that iterator::createHandle() can // call the private Handle::Handle() declared below. friend class iterator; private: Handle(const boost::shared_ptr & handle_tracker, iterator it) : it_(it), handle_tracker_(handle_tracker), valid_(true) {} iterator it_; boost::shared_ptr handle_tracker_; bool valid_; }; ManagedList() {} /** * \brief Add an element to the back of the ManagedList */ Handle add(const T & elem) { return add(elem, boost::bind(&ManagedList::defaultDeleter, this, _1) ); } /** * \brief Add an element to the back of the ManagedList, along with a Custom deleter * \param elem The element we want to add * \param deleter Object on which operator() is called when refcount goes to 0 */ Handle add(const T & elem, CustomDeleter custom_deleter, const boost::shared_ptr & guard) { TrackedElem tracked_t; tracked_t.elem = elem; typename std::list::iterator list_it = list_.insert(list_.end(), tracked_t); iterator managed_it = iterator(list_it); ElemDeleter deleter(managed_it, custom_deleter, guard); boost::shared_ptr tracker( (void *) NULL, deleter); list_it->handle_tracker_ = tracker; return Handle(tracker, managed_it); } /** * \brief Removes an element from the ManagedList */ void erase(iterator it) { list_.erase(it.it_); } iterator end() {return iterator(list_.end()); } iterator begin() {return iterator(list_.begin()); } private: void defaultDeleter(iterator it) { erase(it); } std::list list_; }; template typename ManagedList::Handle ManagedList::iterator::createHandle() { if (it_->handle_tracker_.expired()) { ROS_ERROR_NAMED("actionlib", "Tried to create a handle to a list elem with refcount 0"); } boost::shared_ptr tracker = it_->handle_tracker_.lock(); return Handle(tracker, *this); } } // namespace actionlib #endif // ACTIONLIB__MANAGED_LIST_H_ actionlib-1.12.0/include/actionlib/one_shot_timer.h000066400000000000000000000054351352264324200223270ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #ifndef ACTIONLIB__ONE_SHOT_TIMER_H_ #define ACTIONLIB__ONE_SHOT_TIMER_H_ #include "ros/ros.h" #include "boost/bind.hpp" //! Horrible hack until ROS Supports this (ROS Trac #1387) class OneShotTimer { public: OneShotTimer() : active_(false) {} void cb(const ros::TimerEvent & e) { if (active_) { active_ = false; if (callback_) { callback_(e); } else { ROS_ERROR_NAMED("actionlib", "Got a NULL Timer OneShotTimer Callback"); } } } boost::function getCb() { return boost::bind(&OneShotTimer::cb, this, _1); } void registerOneShotCb(boost::function callback) { callback_ = callback; } void stop() { // timer_.stop(); active_ = false; } const ros::Timer & operator=(const ros::Timer & rhs) { active_ = true; timer_ = rhs; return timer_; } private: ros::Timer timer_; bool active_; boost::function callback_; }; #endif // ACTIONLIB__ONE_SHOT_TIMER_H_ actionlib-1.12.0/include/actionlib/server/000077500000000000000000000000001352264324200204375ustar00rootroot00000000000000actionlib-1.12.0/include/actionlib/server/action_server.h000066400000000000000000000163551352264324200234650ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__ACTION_SERVER_H_ #define ACTIONLIB__SERVER__ACTION_SERVER_H_ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace actionlib { /** * @class ActionServer * @brief The ActionServer is a helpful tool for managing goal requests to a * node. It allows the user to specify callbacks that are invoked when goal * or cancel requests come over the wire, and passes back GoalHandles that * can be used to track the state of a given goal request. The ActionServer * makes no assumptions about the policy used to service these goals, and * sends status for each goal over the wire until the last GoalHandle * associated with a goal request is destroyed. */ template class ActionServer : public ActionServerBase { public: // for convenience when referring to ServerGoalHandles typedef ServerGoalHandle GoalHandle; // generates typedefs that we'll use to make our lives easier ACTION_DEFINITION(ActionSpec); /** * @brief Constructor for an ActionServer * @param n A NodeHandle to create a namespace under * @param name The name of the action * @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire * @param cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire * @param auto_start A boolean value that tells the ActionServer whether or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. */ ActionServer(ros::NodeHandle n, std::string name, boost::function goal_cb, boost::function cancel_cb, bool auto_start); /** * @brief Constructor for an ActionServer * @param n A NodeHandle to create a namespace under * @param name The name of the action * @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire * @param auto_start A boolean value that tells the ActionServer whether or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. */ ActionServer(ros::NodeHandle n, std::string name, boost::function goal_cb, bool auto_start); /** * @brief DEPRECATED Constructor for an ActionServer * @param n A NodeHandle to create a namespace under * @param name The name of the action * @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire * @param cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire */ ROS_DEPRECATED ActionServer(ros::NodeHandle n, std::string name, boost::function goal_cb, boost::function cancel_cb = boost::function()); /** * @brief Constructor for an ActionServer * @param n A NodeHandle to create a namespace under * @param name The name of the action * @param auto_start A boolean value that tells the ActionServer whether or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. */ ActionServer(ros::NodeHandle n, std::string name, bool auto_start); /** * @brief DEPRECATED Constructor for an ActionServer * @param n A NodeHandle to create a namespace under * @param name The name of the action */ ROS_DEPRECATED ActionServer(ros::NodeHandle n, std::string name); /** * @brief Destructor for the ActionServer */ virtual ~ActionServer(); private: /** * @brief Initialize all ROS connections and setup timers */ virtual void initialize(); /** * @brief Publishes a result for a given goal * @param status The status of the goal with which the result is associated * @param result The result to publish */ virtual void publishResult(const actionlib_msgs::GoalStatus & status, const Result & result); /** * @brief Publishes feedback for a given goal * @param status The status of the goal with which the feedback is associated * @param feedback The feedback to publish */ virtual void publishFeedback(const actionlib_msgs::GoalStatus & status, const Feedback & feedback); /** * @brief Explicitly publish status */ virtual void publishStatus(); /** * @brief Publish status for all goals on a timer event */ void publishStatus(const ros::TimerEvent & e); ros::NodeHandle node_; ros::Subscriber goal_sub_, cancel_sub_; ros::Publisher status_pub_, result_pub_, feedback_pub_; ros::Timer status_timer_; }; } // namespace actionlib // include the implementation #include #endif // ACTIONLIB__SERVER__ACTION_SERVER_H_ actionlib-1.12.0/include/actionlib/server/action_server_base.h000066400000000000000000000302631352264324200244510ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_ #define ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace actionlib { /** * @class ActionServerBase * @brief The ActionServerBase implements the logic for an ActionServer. */ template class ActionServerBase { public: // for convenience when referring to ServerGoalHandles typedef ServerGoalHandle GoalHandle; // generates typedefs that we'll use to make our lives easier ACTION_DEFINITION(ActionSpec); /** * @brief Constructor for an ActionServer * @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire * @param cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire * @param auto_start A boolean value that tells the ActionServer whether or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. */ ActionServerBase( boost::function goal_cb, boost::function cancel_cb, bool auto_start = false); /** * @brief Destructor for the ActionServerBase */ virtual ~ActionServerBase(); /** * @brief Register a callback to be invoked when a new goal is received, this will replace any previously registered callback * @param cb The callback to invoke */ void registerGoalCallback(boost::function cb); /** * @brief Register a callback to be invoked when a new cancel is received, this will replace any previously registered callback * @param cb The callback to invoke */ void registerCancelCallback(boost::function cb); /** * @brief Explicitly start the action server, used it auto_start is set to false */ void start(); /** * @brief The ROS callback for goals coming into the ActionServerBase */ void goalCallback(const boost::shared_ptr & goal); /** * @brief The ROS callback for cancel requests coming into the ActionServerBase */ void cancelCallback(const boost::shared_ptr & goal_id); protected: // Allow access to protected fields for helper classes friend class ServerGoalHandle; friend class HandleTrackerDeleter; /** * @brief Initialize all ROS connections and setup timers */ virtual void initialize() = 0; /** * @brief Publishes a result for a given goal * @param status The status of the goal with which the result is associated * @param result The result to publish */ virtual void publishResult(const actionlib_msgs::GoalStatus & status, const Result & result) = 0; /** * @brief Publishes feedback for a given goal * @param status The status of the goal with which the feedback is associated * @param feedback The feedback to publish */ virtual void publishFeedback(const actionlib_msgs::GoalStatus & status, const Feedback & feedback) = 0; /** * @brief Explicitly publish status */ virtual void publishStatus() = 0; boost::recursive_mutex lock_; std::list > status_list_; boost::function goal_callback_; boost::function cancel_callback_; ros::Time last_cancel_; ros::Duration status_list_timeout_; GoalIDGenerator id_generator_; bool started_; boost::shared_ptr guard_; }; template ActionServerBase::ActionServerBase( boost::function goal_cb, boost::function cancel_cb, bool auto_start) : goal_callback_(goal_cb), cancel_callback_(cancel_cb), started_(auto_start), guard_(new DestructionGuard) { } template ActionServerBase::~ActionServerBase() { // Block until we can safely destruct guard_->destruct(); } template void ActionServerBase::registerGoalCallback(boost::function cb) { goal_callback_ = cb; } template void ActionServerBase::registerCancelCallback(boost::function cb) { cancel_callback_ = cb; } template void ActionServerBase::start() { initialize(); started_ = true; publishStatus(); } template void ActionServerBase::goalCallback(const boost::shared_ptr & goal) { boost::recursive_mutex::scoped_lock lock(lock_); // if we're not started... then we're not actually going to do anything if (!started_) { return; } ROS_DEBUG_NAMED("actionlib", "The action server has received a new goal request"); // we need to check if this goal already lives in the status list for (typename std::list >::iterator it = status_list_.begin(); it != status_list_.end(); ++it) { if (goal->goal_id.id == (*it).status_.goal_id.id) { // The goal could already be in a recalling state if a cancel came in before the goal if ( (*it).status_.status == actionlib_msgs::GoalStatus::RECALLING) { (*it).status_.status = actionlib_msgs::GoalStatus::RECALLED; publishResult((*it).status_, Result()); } // if this is a request for a goal that has no active handles left, // we'll bump how long it stays in the list if ((*it).handle_tracker_.expired()) { (*it).handle_destruction_time_ = goal->goal_id.stamp; } // make sure not to call any user callbacks or add duplicate status onto the list return; } } // if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on typename std::list >::iterator it = status_list_.insert( status_list_.end(), StatusTracker(goal)); // we need to create a handle tracker for the incoming goal and update the StatusTracker HandleTrackerDeleter d(this, it, guard_); boost::shared_ptr handle_tracker((void *)NULL, d); (*it).handle_tracker_ = handle_tracker; // check if this goal has already been canceled based on its timestamp if (goal->goal_id.stamp != ros::Time() && goal->goal_id.stamp <= last_cancel_) { // if it has... just create a GoalHandle for it and setCanceled GoalHandle gh(it, this, handle_tracker, guard_); gh.setCanceled( Result(), "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request"); } else { GoalHandle gh = GoalHandle(it, this, handle_tracker, guard_); // make sure that we unlock before calling the users callback boost::reverse_lock unlocker(lock); // now, we need to create a goal handle and call the user's callback goal_callback_(gh); } } template void ActionServerBase::cancelCallback( const boost::shared_ptr & goal_id) { boost::recursive_mutex::scoped_lock lock(lock_); // if we're not started... then we're not actually going to do anything if (!started_) { return; } // we need to handle a cancel for the user ROS_DEBUG_NAMED("actionlib", "The action server has received a new cancel request"); bool goal_id_found = false; for (typename std::list >::iterator it = status_list_.begin(); it != status_list_.end(); ++it) { // check if the goal id is zero or if it is equal to the goal id of // the iterator or if the time of the iterator warrants a cancel if ( (goal_id->id == "" && goal_id->stamp == ros::Time()) || // id and stamp 0 --> cancel everything goal_id->id == (*it).status_.goal_id.id || // ids match... cancel that goal (goal_id->stamp != ros::Time() && (*it).status_.goal_id.stamp <= goal_id->stamp) // stamp != 0 --> cancel everything before stamp ) { // we need to check if we need to store this cancel request for later if (goal_id->id == (*it).status_.goal_id.id) { goal_id_found = true; } // attempt to get the handle_tracker for the list item if it exists boost::shared_ptr handle_tracker = (*it).handle_tracker_.lock(); if ((*it).handle_tracker_.expired()) { // if the handle tracker is expired, then we need to create a new one HandleTrackerDeleter d(this, it, guard_); handle_tracker = boost::shared_ptr((void *)NULL, d); (*it).handle_tracker_ = handle_tracker; // we also need to reset the time that the status is supposed to be removed from the list (*it).handle_destruction_time_ = ros::Time(); } // set the status of the goal to PREEMPTING or RECALLING as approriate // and check if the request should be passed on to the user GoalHandle gh(it, this, handle_tracker, guard_); if (gh.setCancelRequested()) { // make sure that we're unlocked before we call the users callback boost::reverse_lock unlocker(lock); // call the user's cancel callback on the relevant goal cancel_callback_(gh); } } } // if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request if (goal_id->id != "" && !goal_id_found) { typename std::list >::iterator it = status_list_.insert( status_list_.end(), StatusTracker(*goal_id, actionlib_msgs::GoalStatus::RECALLING)); // start the timer for how long the status will live in the list without a goal handle to it (*it).handle_destruction_time_ = goal_id->stamp; } // make sure to set last_cancel_ based on the stamp associated with this cancel request if (goal_id->stamp > last_cancel_) { last_cancel_ = goal_id->stamp; } } } // namespace actionlib #endif // ACTIONLIB__SERVER__ACTION_SERVER_BASE_H_ actionlib-1.12.0/include/actionlib/server/action_server_imp.h000066400000000000000000000230061352264324200243210ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_ #define ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_ #include #include #include namespace actionlib { template ActionServer::ActionServer( ros::NodeHandle n, std::string name, bool auto_start) : ActionServerBase( boost::function(), boost::function(), auto_start), node_(n, name) { // if we're to autostart... then we'll initialize things if (this->started_) { ROS_WARN_NAMED("actionlib", "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.", node_.getNamespace().c_str()); } } template ActionServer::ActionServer(ros::NodeHandle n, std::string name) : ActionServerBase( boost::function(), boost::function(), true), node_(n, name) { // if we're to autostart... then we'll initialize things if (this->started_) { ROS_WARN_NAMED("actionlib", "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.", node_.getNamespace().c_str()); ActionServer::initialize(); publishStatus(); } } template ActionServer::ActionServer(ros::NodeHandle n, std::string name, boost::function goal_cb, boost::function cancel_cb, bool auto_start) : ActionServerBase(goal_cb, cancel_cb, auto_start), node_(n, name) { // if we're to autostart... then we'll initialize things if (this->started_) { ROS_WARN_NAMED("actionlib", "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.", node_.getNamespace().c_str()); ActionServer::initialize(); publishStatus(); } } template ActionServer::ActionServer(ros::NodeHandle n, std::string name, boost::function goal_cb, boost::function cancel_cb) : ActionServerBase(goal_cb, cancel_cb, true), node_(n, name) { // if we're to autostart... then we'll initialize things if (this->started_) { ROS_WARN_NAMED("actionlib", "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.", node_.getNamespace().c_str()); ActionServer::initialize(); publishStatus(); } } template ActionServer::ActionServer(ros::NodeHandle n, std::string name, boost::function goal_cb, bool auto_start) : ActionServerBase(goal_cb, boost::function(), auto_start), node_(n, name) { // if we're to autostart... then we'll initialize things if (this->started_) { ROS_WARN_NAMED("actionlib", "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.", node_.getNamespace().c_str()); ActionServer::initialize(); publishStatus(); } } template ActionServer::~ActionServer() { } template void ActionServer::initialize() { // read the queue size for each of the publish & subscribe components of the action // server int pub_queue_size; int sub_queue_size; node_.param("actionlib_server_pub_queue_size", pub_queue_size, 50); node_.param("actionlib_server_sub_queue_size", sub_queue_size, 50); if (pub_queue_size < 0) {pub_queue_size = 50;} if (sub_queue_size < 0) {sub_queue_size = 50;} result_pub_ = node_.advertise("result", static_cast(pub_queue_size)); feedback_pub_ = node_.advertise("feedback", static_cast(pub_queue_size)); status_pub_ = node_.advertise("status", static_cast(pub_queue_size), true); // read the frequency with which to publish status from the parameter server // if not specified locally explicitly, use search param to find actionlib_status_frequency double status_frequency, status_list_timeout; if (!node_.getParam("status_frequency", status_frequency)) { std::string status_frequency_param_name; if (!node_.searchParam("actionlib_status_frequency", status_frequency_param_name)) { status_frequency = 5.0; } else { node_.param(status_frequency_param_name, status_frequency, 5.0); } } else { ROS_WARN_NAMED("actionlib", "You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency."); } node_.param("status_list_timeout", status_list_timeout, 5.0); this->status_list_timeout_ = ros::Duration(status_list_timeout); if (status_frequency > 0) { status_timer_ = node_.createTimer(ros::Duration(1.0 / status_frequency), boost::bind(&ActionServer::publishStatus, this, _1)); } goal_sub_ = node_.subscribe("goal", static_cast(sub_queue_size), boost::bind(&ActionServerBase::goalCallback, this, _1)); cancel_sub_ = node_.subscribe("cancel", static_cast(sub_queue_size), boost::bind(&ActionServerBase::cancelCallback, this, _1)); } template void ActionServer::publishResult(const actionlib_msgs::GoalStatus & status, const Result & result) { boost::recursive_mutex::scoped_lock lock(this->lock_); // we'll create a shared_ptr to pass to ROS to limit copying boost::shared_ptr ar(new ActionResult); ar->header.stamp = ros::Time::now(); ar->status = status; ar->result = result; ROS_DEBUG_NAMED("actionlib", "Publishing result for goal with id: %s and stamp: %.2f", status.goal_id.id.c_str(), status.goal_id.stamp.toSec()); result_pub_.publish(ar); publishStatus(); } template void ActionServer::publishFeedback(const actionlib_msgs::GoalStatus & status, const Feedback & feedback) { boost::recursive_mutex::scoped_lock lock(this->lock_); // we'll create a shared_ptr to pass to ROS to limit copying boost::shared_ptr af(new ActionFeedback); af->header.stamp = ros::Time::now(); af->status = status; af->feedback = feedback; ROS_DEBUG_NAMED("actionlib", "Publishing feedback for goal with id: %s and stamp: %.2f", status.goal_id.id.c_str(), status.goal_id.stamp.toSec()); feedback_pub_.publish(af); } template void ActionServer::publishStatus(const ros::TimerEvent &) { boost::recursive_mutex::scoped_lock lock(this->lock_); // we won't publish status unless we've been started if (!this->started_) { return; } publishStatus(); } template void ActionServer::publishStatus() { boost::recursive_mutex::scoped_lock lock(this->lock_); // build a status array actionlib_msgs::GoalStatusArray status_array; status_array.header.stamp = ros::Time::now(); status_array.status_list.resize(this->status_list_.size()); unsigned int i = 0; for (typename std::list >::iterator it = this->status_list_.begin(); it != this->status_list_.end(); ) { status_array.status_list[i] = (*it).status_; // check if the item is due for deletion from the status list if ((*it).handle_destruction_time_ != ros::Time() && (*it).handle_destruction_time_ + this->status_list_timeout_ < ros::Time::now()) { it = this->status_list_.erase(it); } else { ++it; } ++i; } status_pub_.publish(status_array); } } // namespace actionlib #endif // ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_ actionlib-1.12.0/include/actionlib/server/handle_tracker_deleter.h000066400000000000000000000057551352264324200252760ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__HANDLE_TRACKER_DELETER_H_ #define ACTIONLIB__SERVER__HANDLE_TRACKER_DELETER_H_ #include #include #include #include #include namespace actionlib { // we need to forward declare the ActionServerBase class template class ActionServerBase; /** * @class HandleTrackerDeleter * @brief A class to help with tracking GoalHandles and removing goals * from the status list when the last GoalHandle associated with a given * goal is deleted. */ // class to help with tracking status objects template class HandleTrackerDeleter { public: HandleTrackerDeleter(ActionServerBase * as, typename std::list >::iterator status_it, boost::shared_ptr guard); void operator()(void * ptr); private: ActionServerBase * as_; typename std::list >::iterator status_it_; boost::shared_ptr guard_; }; } // namespace actionlib #include #endif // ACTIONLIB__SERVER__HANDLE_TRACKER_DELETER_H_ actionlib-1.12.0/include/actionlib/server/handle_tracker_deleter_imp.h000066400000000000000000000054431352264324200261350ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__HANDLE_TRACKER_DELETER_IMP_H_ #define ACTIONLIB__SERVER__HANDLE_TRACKER_DELETER_IMP_H_ #include namespace actionlib { template HandleTrackerDeleter::HandleTrackerDeleter(ActionServerBase * as, typename std::list >::iterator status_it, boost::shared_ptr guard) : as_(as), status_it_(status_it), guard_(guard) {} template void HandleTrackerDeleter::operator()(void *) { if (as_) { // make sure that the action server hasn't been destroyed yet DestructionGuard::ScopedProtector protector(*guard_); if (protector.isProtected()) { // make sure to lock while we erase status for this goal from the list boost::recursive_mutex::scoped_lock lock(as_->lock_); (*status_it_).handle_destruction_time_ = ros::Time::now(); // as_->status_list_.erase(status_it_); } } } } // namespace actionlib #endif // ACTIONLIB__SERVER__HANDLE_TRACKER_DELETER_IMP_H_ actionlib-1.12.0/include/actionlib/server/server_goal_handle.h000066400000000000000000000171651352264324200244450ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__SERVER_GOAL_HANDLE_H_ #define ACTIONLIB__SERVER__SERVER_GOAL_HANDLE_H_ #include #include #include #include #include #include #include #include namespace actionlib { // forward declaration of ActionServerBase template class ActionServerBase; /** * @class ServerGoalHandle * @brief Encapsulates a state machine for a given goal that the user can * trigger transitions on. All ROS interfaces for the goal are managed by * the ActionServer to lessen the burden on the user. */ template class ServerGoalHandle { private: // generates typedefs that we'll use to make our lives easier ACTION_DEFINITION(ActionSpec); public: /** * @brief Default constructor for a ServerGoalHandle */ ServerGoalHandle(); /** * @brief Copy constructor for a ServerGoalHandle * @param gh The goal handle to copy */ ServerGoalHandle(const ServerGoalHandle & gh); /** @brief Accept the goal referenced by the goal handle. This will * transition to the ACTIVE state or the PREEMPTING state depending * on whether a cancel request has been received for the goal * @param text Optionally, any text message about the status change being made that should be passed to the client */ void setAccepted(const std::string & text = std::string("")); /** * @brief Set the status of the goal associated with the ServerGoalHandle to RECALLED or PREEMPTED * depending on what the current status of the goal is * @param result Optionally, the user can pass in a result to be sent to any clients of the goal * @param text Optionally, any text message about the status change being made that should be passed to the client */ void setCanceled(const Result & result = Result(), const std::string & text = std::string("")); /** * @brief Set the status of the goal associated with the ServerGoalHandle to rejected * @param result Optionally, the user can pass in a result to be sent to any clients of the goal * @param text Optionally, any text message about the status change being made that should be passed to the client */ void setRejected(const Result & result = Result(), const std::string & text = std::string("")); /** * @brief Set the status of the goal associated with the ServerGoalHandle to aborted * @param result Optionally, the user can pass in a result to be sent to any clients of the goal * @param text Optionally, any text message about the status change being made that should be passed to the client */ void setAborted(const Result & result = Result(), const std::string & text = std::string("")); /** * @brief Set the status of the goal associated with the ServerGoalHandle to succeeded * @param result Optionally, the user can pass in a result to be sent to any clients of the goal * @param text Optionally, any text message about the status change being made that should be passed to the client */ void setSucceeded(const Result & result = Result(), const std::string & text = std::string("")); /** * @brief Send feedback to any clients of the goal associated with this ServerGoalHandle * @param feedback The feedback to send to the client */ void publishFeedback(const Feedback & feedback); /** * @brief Determine if the goal handle is valid (tracking a valid goal, * and associated with a valid action server). If the handle is valid, it * means that the accessors \ref getGoal, \ref getGoalID, etc, can be * called without generating errors. * * @return True if valid, False if invalid */ bool isValid() const; /** * @brief Accessor for the goal associated with the ServerGoalHandle * @return A shared_ptr to the goal object */ boost::shared_ptr getGoal() const; /** * @brief Accessor for the goal id associated with the ServerGoalHandle * @return The goal id */ actionlib_msgs::GoalID getGoalID() const; /** * @brief Accessor for the status associated with the ServerGoalHandle * @return The goal status */ actionlib_msgs::GoalStatus getGoalStatus() const; /** * @brief Equals operator for a ServerGoalHandle * @param gh The goal handle to copy */ ServerGoalHandle & operator=(const ServerGoalHandle & gh); /** * @brief Equals operator for ServerGoalHandles * @param other The ServerGoalHandle to compare to * @return True if the ServerGoalHandles refer to the same goal, false otherwise */ bool operator==(const ServerGoalHandle & other) const; /** * @brief != operator for ServerGoalHandles * @param other The ServerGoalHandle to compare to * @return True if the ServerGoalHandles refer to different goals, false otherwise */ bool operator!=(const ServerGoalHandle & other) const; private: /** * @brief A private constructor used by the ActionServer to initialize a ServerGoalHandle */ ServerGoalHandle(typename std::list >::iterator status_it, ActionServerBase * as, boost::shared_ptr handle_tracker, boost::shared_ptr guard); /** * @brief A private method to set status to PENDING or RECALLING * @return True if the cancel request should be passed on to the user, false otherwise */ bool setCancelRequested(); typename std::list >::iterator status_it_; boost::shared_ptr goal_; ActionServerBase * as_; boost::shared_ptr handle_tracker_; boost::shared_ptr guard_; friend class ActionServerBase; }; } // namespace actionlib // include the implementation #include #endif // ACTIONLIB__SERVER__SERVER_GOAL_HANDLE_H_ actionlib-1.12.0/include/actionlib/server/server_goal_handle_imp.h000066400000000000000000000360541352264324200253100ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__SERVER_GOAL_HANDLE_IMP_H_ #define ACTIONLIB__SERVER__SERVER_GOAL_HANDLE_IMP_H_ #include #include #include "ros/console.h" namespace actionlib { template ServerGoalHandle::ServerGoalHandle() : as_(NULL) {} template ServerGoalHandle::ServerGoalHandle(const ServerGoalHandle & gh) : status_it_(gh.status_it_), goal_(gh.goal_), as_(gh.as_), handle_tracker_(gh.handle_tracker_), guard_(gh.guard_) {} template void ServerGoalHandle::setAccepted(const std::string & text) { if (as_ == NULL) { ROS_ERROR_NAMED("actionlib", "You are attempting to call methods on an uninitialized goal handle"); return; } // check to see if we can use the action server DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?"); return; } ROS_DEBUG_NAMED("actionlib", "Accepting goal, id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec()); if (goal_) { boost::recursive_mutex::scoped_lock lock(as_->lock_); unsigned int status = (*status_it_).status_.status; // if we were pending before, then we'll go active if (status == actionlib_msgs::GoalStatus::PENDING) { (*status_it_).status_.status = actionlib_msgs::GoalStatus::ACTIVE; (*status_it_).status_.text = text; as_->publishStatus(); } else if (status == actionlib_msgs::GoalStatus::RECALLING) { // if we were recalling before, now we'll go to preempting (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTING; (*status_it_).status_.text = text; as_->publishStatus(); } else { ROS_ERROR_NAMED("actionlib", "To transition to an active state, the goal must be in a pending or recalling state, it is currently in state: %d", (*status_it_).status_.status); } } else { ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle"); } } template void ServerGoalHandle::setCanceled(const Result & result, const std::string & text) { if (as_ == NULL) { ROS_ERROR_NAMED("actionlib", "You are attempting to call methods on an uninitialized goal handle"); return; } // check to see if we can use the action server DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?"); return; } ROS_DEBUG_NAMED("actionlib", "Setting status to canceled on goal, id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec()); if (goal_) { boost::recursive_mutex::scoped_lock lock(as_->lock_); unsigned int status = (*status_it_).status_.status; if (status == actionlib_msgs::GoalStatus::PENDING || status == actionlib_msgs::GoalStatus::RECALLING) { (*status_it_).status_.status = actionlib_msgs::GoalStatus::RECALLED; (*status_it_).status_.text = text; as_->publishResult((*status_it_).status_, result); } else if (status == actionlib_msgs::GoalStatus::ACTIVE || status == actionlib_msgs::GoalStatus::PREEMPTING) { (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTED; (*status_it_).status_.text = text; as_->publishResult((*status_it_).status_, result); } else { ROS_ERROR_NAMED("actionlib", "To transition to a cancelled state, the goal must be in a pending, recalling, active, or preempting state, it is currently in state: %d", (*status_it_).status_.status); } } else { ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle"); } } template void ServerGoalHandle::setRejected(const Result & result, const std::string & text) { if (as_ == NULL) { ROS_ERROR_NAMED("actionlib", "You are attempting to call methods on an uninitialized goal handle"); return; } // check to see if we can use the action server DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?"); return; } ROS_DEBUG_NAMED("actionlib", "Setting status to rejected on goal, id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec()); if (goal_) { boost::recursive_mutex::scoped_lock lock(as_->lock_); unsigned int status = (*status_it_).status_.status; if (status == actionlib_msgs::GoalStatus::PENDING || status == actionlib_msgs::GoalStatus::RECALLING) { (*status_it_).status_.status = actionlib_msgs::GoalStatus::REJECTED; (*status_it_).status_.text = text; as_->publishResult((*status_it_).status_, result); } else { ROS_ERROR_NAMED("actionlib", "To transition to a rejected state, the goal must be in a pending or recalling state, it is currently in state: %d", (*status_it_).status_.status); } } else { ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle"); } } template void ServerGoalHandle::setAborted(const Result & result, const std::string & text) { if (as_ == NULL) { ROS_ERROR_NAMED("actionlib", "You are attempting to call methods on an uninitialized goal handle"); return; } // check to see if we can use the action server DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?"); return; } ROS_DEBUG_NAMED("actionlib", "Setting status to aborted on goal, id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec()); if (goal_) { boost::recursive_mutex::scoped_lock lock(as_->lock_); unsigned int status = (*status_it_).status_.status; if (status == actionlib_msgs::GoalStatus::PREEMPTING || status == actionlib_msgs::GoalStatus::ACTIVE) { (*status_it_).status_.status = actionlib_msgs::GoalStatus::ABORTED; (*status_it_).status_.text = text; as_->publishResult((*status_it_).status_, result); } else { ROS_ERROR_NAMED("actionlib", "To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: %d", status); } } else { ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle"); } } template void ServerGoalHandle::setSucceeded(const Result & result, const std::string & text) { if (as_ == NULL) { ROS_ERROR_NAMED("actionlib", "You are attempting to call methods on an uninitialized goal handle"); return; } // check to see if we can use the action server DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?"); return; } ROS_DEBUG_NAMED("actionlib", "Setting status to succeeded on goal, id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec()); if (goal_) { boost::recursive_mutex::scoped_lock lock(as_->lock_); unsigned int status = (*status_it_).status_.status; if (status == actionlib_msgs::GoalStatus::PREEMPTING || status == actionlib_msgs::GoalStatus::ACTIVE) { (*status_it_).status_.status = actionlib_msgs::GoalStatus::SUCCEEDED; (*status_it_).status_.text = text; as_->publishResult((*status_it_).status_, result); } else { ROS_ERROR_NAMED("actionlib", "To transition to a succeeded state, the goal must be in a preempting or active state, it is currently in state: %d", status); } } else { ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle"); } } template void ServerGoalHandle::publishFeedback(const Feedback & feedback) { if (as_ == NULL) { ROS_ERROR_NAMED("actionlib", "You are attempting to call methods on an uninitialized goal handle"); return; } // check to see if we can use the action server DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?"); return; } ROS_DEBUG_NAMED("actionlib", "Publishing feedback for goal, id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec()); if (goal_) { boost::recursive_mutex::scoped_lock lock(as_->lock_); as_->publishFeedback((*status_it_).status_, feedback); } else { ROS_ERROR_NAMED("actionlib", "Attempt to publish feedback on an uninitialized ServerGoalHandle"); } } template bool ServerGoalHandle::isValid() const { return goal_ && as_ != NULL; } template boost::shared_ptr::Goal> ServerGoalHandle:: getGoal() const { // if we have a goal that is non-null if (goal_) { // create the deleter for our goal subtype EnclosureDeleter d(goal_); return boost::shared_ptr(&(goal_->goal), d); } return boost::shared_ptr(); } template actionlib_msgs::GoalID ServerGoalHandle::getGoalID() const { if (goal_ && as_ != NULL) { DestructionGuard::ScopedProtector protector(*guard_); if (protector.isProtected()) { boost::recursive_mutex::scoped_lock lock(as_->lock_); return (*status_it_).status_.goal_id; } else { return actionlib_msgs::GoalID(); } } else { ROS_ERROR_NAMED("actionlib", "Attempt to get a goal id on an uninitialized ServerGoalHandle or one that has no ActionServer associated with it."); return actionlib_msgs::GoalID(); } } template actionlib_msgs::GoalStatus ServerGoalHandle::getGoalStatus() const { if (goal_ && as_ != NULL) { DestructionGuard::ScopedProtector protector(*guard_); if (protector.isProtected()) { boost::recursive_mutex::scoped_lock lock(as_->lock_); return (*status_it_).status_; } else { return actionlib_msgs::GoalStatus(); } } else { ROS_ERROR_NAMED("actionlib", "Attempt to get goal status on an uninitialized ServerGoalHandle or one that has no ActionServer associated with it."); return actionlib_msgs::GoalStatus(); } } template ServerGoalHandle & ServerGoalHandle::operator=(const ServerGoalHandle & gh) { status_it_ = gh.status_it_; goal_ = gh.goal_; as_ = gh.as_; handle_tracker_ = gh.handle_tracker_; guard_ = gh.guard_; return *this; } template bool ServerGoalHandle::operator==(const ServerGoalHandle & other) const { if (!goal_ && !other.goal_) { return true; } if (!goal_ || !other.goal_) { return false; } actionlib_msgs::GoalID my_id = getGoalID(); actionlib_msgs::GoalID their_id = other.getGoalID(); return my_id.id == their_id.id; } template bool ServerGoalHandle::operator!=(const ServerGoalHandle & other) const { return !(*this == other); } template ServerGoalHandle::ServerGoalHandle( typename std::list >::iterator status_it, ActionServerBase * as, boost::shared_ptr handle_tracker, boost::shared_ptr guard) : status_it_(status_it), goal_((*status_it).goal_), as_(as), handle_tracker_(handle_tracker), guard_(guard) {} template bool ServerGoalHandle::setCancelRequested() { if (as_ == NULL) { ROS_ERROR_NAMED("actionlib", "You are attempting to call methods on an uninitialized goal handle"); return false; } // check to see if we can use the action server DestructionGuard::ScopedProtector protector(*guard_); if (!protector.isProtected()) { ROS_ERROR_NAMED("actionlib", "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?"); return false; } ROS_DEBUG_NAMED("actionlib", "Transitioning to a cancel requested state on goal id: %s, stamp: %.2f", getGoalID().id.c_str(), getGoalID().stamp.toSec()); if (goal_) { boost::recursive_mutex::scoped_lock lock(as_->lock_); unsigned int status = (*status_it_).status_.status; if (status == actionlib_msgs::GoalStatus::PENDING) { (*status_it_).status_.status = actionlib_msgs::GoalStatus::RECALLING; as_->publishStatus(); return true; } if (status == actionlib_msgs::GoalStatus::ACTIVE) { (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTING; as_->publishStatus(); return true; } } return false; } } // namespace actionlib #endif // ACTIONLIB__SERVER__SERVER_GOAL_HANDLE_IMP_H_ actionlib-1.12.0/include/actionlib/server/service_server.h000066400000000000000000000062651352264324200236470ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__SERVICE_SERVER_H_ #define ACTIONLIB__SERVER__SERVICE_SERVER_H_ #include #include #include namespace actionlib { class ServiceServerImp { public: ServiceServerImp() {} virtual ~ServiceServerImp() {} }; class ServiceServer { public: ServiceServer(boost::shared_ptr server) : server_(server) {} private: boost::shared_ptr server_; }; template ServiceServer advertiseService(ros::NodeHandle n, std::string name, boost::function service_cb); template class ServiceServerImpT : public ServiceServerImp { public: // generates typedefs that we'll use to make our lives easier ACTION_DEFINITION(ActionSpec); typedef typename ActionServer::GoalHandle GoalHandle; ServiceServerImpT(ros::NodeHandle n, std::string name, boost::function service_cb); void goalCB(GoalHandle g); private: boost::shared_ptr > as_; boost::function service_cb_; }; } // namespace actionlib // include the implementation #include #endif // ACTIONLIB__SERVER__SERVICE_SERVER_H_ actionlib-1.12.0/include/actionlib/server/service_server_imp.h000066400000000000000000000063071352264324200245110ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__SERVICE_SERVER_IMP_H_ #define ACTIONLIB__SERVER__SERVICE_SERVER_IMP_H_ #include namespace actionlib { template ServiceServer advertiseService(ros::NodeHandle n, std::string name, boost::function service_cb) { boost::shared_ptr server_ptr(new ServiceServerImpT(n, name, service_cb)); return ServiceServer(server_ptr); } template ServiceServerImpT::ServiceServerImpT(ros::NodeHandle n, std::string name, boost::function service_cb) : service_cb_(service_cb) { as_ = boost::shared_ptr >(new ActionServer(n, name, boost::bind(&ServiceServerImpT::goalCB, this, _1), false)); as_->start(); } template void ServiceServerImpT::goalCB(GoalHandle goal) { goal.setAccepted("This goal has been accepted by the service server"); // we need to pass the result into the users callback Result r; if (service_cb_(*(goal.getGoal()), r)) { goal.setSucceeded(r, "The service server successfully processed the request"); } else { goal.setAborted(r, "The service server failed to process the request"); } } } // namespace actionlib #endif // ACTIONLIB__SERVER__SERVICE_SERVER_IMP_H_ actionlib-1.12.0/include/actionlib/server/simple_action_server.h000066400000000000000000000250631352264324200250320ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_H_ #define ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_H_ #include #include #include #include #include namespace actionlib { /** @class SimpleActionServer @brief The SimpleActionServer * implements a single goal policy on top of the ActionServer class. The * specification of the policy is as follows: only one goal can have an * active status at a time, new goals preempt previous goals based on the * stamp in their GoalID field (later goals preempt earlier ones), an * explicit preempt goal preempts all goals with timestamps that are less * than or equal to the stamp associated with the preempt, accepting a new * goal implies successful preemption of any old goal and the status of the * old goal will be changed automatically to reflect this. */ template class SimpleActionServer { public: // generates typedefs that we'll use to make our lives easier ACTION_DEFINITION(ActionSpec); typedef typename ActionServer::GoalHandle GoalHandle; typedef boost::function ExecuteCallback; /** * @brief Constructor for a SimpleActionServer * @param name A name for the action server * @param execute_callback Optional callback that gets called in a separate thread whenever * a new goal is received, allowing users to have blocking callbacks. * Adding an execute callback also deactivates the goalCallback. * @param auto_start A boolean value that tells the ActionServer whether or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. */ SimpleActionServer(std::string name, ExecuteCallback execute_callback, bool auto_start); /** * @brief Constructor for a SimpleActionServer * @param name A name for the action server * @param auto_start A boolean value that tells the ActionServer whether or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. */ SimpleActionServer(std::string name, bool auto_start); /** * @brief DEPRECATED: Constructor for a SimpleActionServer * @param name A name for the action server * @param execute_callback Optional callback that gets called in a separate thread whenever * a new goal is received, allowing users to have blocking callbacks. * Adding an execute callback also deactivates the goalCallback. */ ROS_DEPRECATED SimpleActionServer(std::string name, ExecuteCallback execute_callback = NULL); /** * @brief Constructor for a SimpleActionServer * @param n A NodeHandle to create a namespace under * @param name A name for the action server * @param execute_callback Optional callback that gets called in a separate thread whenever * a new goal is received, allowing users to have blocking callbacks. * Adding an execute callback also deactivates the goalCallback. * @param auto_start A boolean value that tells the ActionServer whether or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. */ SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback, bool auto_start); /** * @brief Constructor for a SimpleActionServer * @param n A NodeHandle to create a namespace under * @param name A name for the action server * @param auto_start A boolean value that tells the ActionServer whether or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. */ SimpleActionServer(ros::NodeHandle n, std::string name, bool auto_start); /** * @brief Constructor for a SimpleActionServer * @param n A NodeHandle to create a namespace under * @param name A name for the action server * @param execute_callback Optional callback that gets called in a separate thread whenever * a new goal is received, allowing users to have blocking callbacks. * Adding an execute callback also deactivates the goalCallback. */ ROS_DEPRECATED SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback = NULL); ~SimpleActionServer(); /** * @brief Accepts a new goal when one is available. The status of this * goal is set to active upon acceptance, and the status of any * previously active goal is set to preempted. Preempts received for the * new goal between checking if isNewGoalAvailable or invocation of a * goal callback and the acceptNewGoal call will not trigger a preempt * callback. This means, isPreemptRequested should be called after * accepting the goal even for callback-based implementations to make * sure the new goal does not have a pending preempt request. * @return A shared_ptr to the new goal. */ boost::shared_ptr acceptNewGoal(); /** * @brief Allows polling implementations to query about the availability of a new goal * @return True if a new goal is available, false otherwise */ bool isNewGoalAvailable(); /** * @brief Allows polling implementations to query about preempt requests * @return True if a preempt is requested, false otherwise */ bool isPreemptRequested(); /** * @brief Allows polling implementations to query about the status of the current goal * @return True if a goal is active, false otherwise */ bool isActive(); /** * @brief Sets the status of the active goal to succeeded * @param result An optional result to send back to any clients of the goal * @param result An optional text message to send back to any clients of the goal */ void setSucceeded(const Result & result = Result(), const std::string & text = std::string("")); /** * @brief Sets the status of the active goal to aborted * @param result An optional result to send back to any clients of the goal * @param result An optional text message to send back to any clients of the goal */ void setAborted(const Result & result = Result(), const std::string & text = std::string("")); /** * @brief Publishes feedback for a given goal * @param feedback Shared pointer to the feedback to publish */ void publishFeedback(const FeedbackConstPtr & feedback); /** * @brief Publishes feedback for a given goal * @param feedback The feedback to publish */ void publishFeedback(const Feedback & feedback); /** * @brief Sets the status of the active goal to preempted * @param result An optional result to send back to any clients of the goal * @param result An optional text message to send back to any clients of the goal */ void setPreempted(const Result & result = Result(), const std::string & text = std::string("")); /** * @brief Allows users to register a callback to be invoked when a new goal is available * @param cb The callback to be invoked */ void registerGoalCallback(boost::function cb); /** * @brief Allows users to register a callback to be invoked when a new preempt request is available * @param cb The callback to be invoked */ void registerPreemptCallback(boost::function cb); /** * @brief Explicitly start the action server, used it auto_start is set to false */ void start(); /** * @brief Explicitly shutdown the action server */ void shutdown(); private: /** * @brief Callback for when the ActionServer receives a new goal and passes it on */ void goalCallback(GoalHandle goal); /** * @brief Callback for when the ActionServer receives a new preempt and passes it on */ void preemptCallback(GoalHandle preempt); /** * @brief Called from a separate thread to call blocking execute calls */ void executeLoop(); ros::NodeHandle n_; boost::shared_ptr > as_; GoalHandle current_goal_, next_goal_; bool new_goal_, preempt_request_, new_goal_preempt_request_; boost::recursive_mutex lock_; boost::function goal_callback_; boost::function preempt_callback_; ExecuteCallback execute_callback_; boost::condition execute_condition_; boost::thread * execute_thread_; boost::mutex terminate_mutex_; bool need_to_terminate_; }; } // namespace actionlib // include the implementation here #include #endif // ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_H_ actionlib-1.12.0/include/actionlib/server/simple_action_server_imp.h000066400000000000000000000344751352264324200257060ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_IMP_H_ #define ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_IMP_H_ #include #include namespace actionlib { template SimpleActionServer::SimpleActionServer(std::string name, ExecuteCallback execute_callback, bool auto_start) : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_( execute_callback), execute_thread_(NULL), need_to_terminate_(false) { if (execute_callback_ != NULL) { execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this)); } // create the action server as_ = boost::shared_ptr >(new ActionServer(n_, name, boost::bind(&SimpleActionServer::goalCallback, this, _1), boost::bind(&SimpleActionServer::preemptCallback, this, _1), auto_start)); } template SimpleActionServer::SimpleActionServer(std::string name, bool auto_start) : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_( NULL), execute_thread_(NULL), need_to_terminate_(false) { // create the action server as_ = boost::shared_ptr >(new ActionServer(n_, name, boost::bind(&SimpleActionServer::goalCallback, this, _1), boost::bind(&SimpleActionServer::preemptCallback, this, _1), auto_start)); if (execute_callback_ != NULL) { execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this)); } } template SimpleActionServer::SimpleActionServer(std::string name, ExecuteCallback execute_callback) : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_( execute_callback), execute_thread_(NULL), need_to_terminate_(false) { // create the action server as_ = boost::shared_ptr >(new ActionServer(n_, name, boost::bind(&SimpleActionServer::goalCallback, this, _1), boost::bind(&SimpleActionServer::preemptCallback, this, _1), true)); if (execute_callback_ != NULL) { execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this)); } } template SimpleActionServer::SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback, bool auto_start) : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(execute_callback), execute_thread_(NULL), need_to_terminate_(false) { // create the action server as_ = boost::shared_ptr >(new ActionServer(n, name, boost::bind(&SimpleActionServer::goalCallback, this, _1), boost::bind(&SimpleActionServer::preemptCallback, this, _1), auto_start)); if (execute_callback_ != NULL) { execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this)); } } template SimpleActionServer::SimpleActionServer(ros::NodeHandle n, std::string name, bool auto_start) : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(NULL), execute_thread_(NULL), need_to_terminate_(false) { // create the action server as_ = boost::shared_ptr >(new ActionServer(n, name, boost::bind(&SimpleActionServer::goalCallback, this, _1), boost::bind(&SimpleActionServer::preemptCallback, this, _1), auto_start)); if (execute_callback_ != NULL) { execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this)); } } template SimpleActionServer::SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback) : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(execute_callback), execute_thread_(NULL), need_to_terminate_(false) { // create the action server as_ = boost::shared_ptr >(new ActionServer(n, name, boost::bind(&SimpleActionServer::goalCallback, this, _1), boost::bind(&SimpleActionServer::preemptCallback, this, _1), true)); if (execute_callback_ != NULL) { execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this)); } } template SimpleActionServer::~SimpleActionServer() { if (execute_thread_) { shutdown(); } } template void SimpleActionServer::shutdown() { if (execute_callback_) { { boost::mutex::scoped_lock terminate_lock(terminate_mutex_); need_to_terminate_ = true; } assert(execute_thread_); if (execute_thread_) { execute_thread_->join(); delete execute_thread_; execute_thread_ = NULL; } } } template boost::shared_ptr::Goal> SimpleActionServer ::acceptNewGoal() { boost::recursive_mutex::scoped_lock lock(lock_); if (!new_goal_ || !next_goal_.getGoal()) { ROS_ERROR_NAMED("actionlib", "Attempting to accept the next goal when a new goal is not available"); return boost::shared_ptr(); } // check if we need to send a preempted message for the goal that we're currently pursuing if (isActive() && current_goal_.getGoal() && current_goal_ != next_goal_) { current_goal_.setCanceled( Result(), "This goal was canceled because another goal was recieved by the simple action server"); } ROS_DEBUG_NAMED("actionlib", "Accepting a new goal"); // accept the next goal current_goal_ = next_goal_; new_goal_ = false; // set preempt to request to equal the preempt state of the new goal preempt_request_ = new_goal_preempt_request_; new_goal_preempt_request_ = false; // set the status of the current goal to be active current_goal_.setAccepted("This goal has been accepted by the simple action server"); return current_goal_.getGoal(); } template bool SimpleActionServer::isNewGoalAvailable() { return new_goal_; } template bool SimpleActionServer::isPreemptRequested() { return preempt_request_; } template bool SimpleActionServer::isActive() { if (!current_goal_.getGoal()) { return false; } unsigned int status = current_goal_.getGoalStatus().status; return status == actionlib_msgs::GoalStatus::ACTIVE || status == actionlib_msgs::GoalStatus::PREEMPTING; } template void SimpleActionServer::setSucceeded(const Result & result, const std::string & text) { boost::recursive_mutex::scoped_lock lock(lock_); ROS_DEBUG_NAMED("actionlib", "Setting the current goal as succeeded"); current_goal_.setSucceeded(result, text); } template void SimpleActionServer::setAborted(const Result & result, const std::string & text) { boost::recursive_mutex::scoped_lock lock(lock_); ROS_DEBUG_NAMED("actionlib", "Setting the current goal as aborted"); current_goal_.setAborted(result, text); } template void SimpleActionServer::setPreempted(const Result & result, const std::string & text) { boost::recursive_mutex::scoped_lock lock(lock_); ROS_DEBUG_NAMED("actionlib", "Setting the current goal as canceled"); current_goal_.setCanceled(result, text); } template void SimpleActionServer::registerGoalCallback(boost::function cb) { // Cannot register a goal callback if an execute callback exists if (execute_callback_) { ROS_WARN_NAMED("actionlib", "Cannot call SimpleActionServer::registerGoalCallback() because an executeCallback exists. Not going to register it."); } else { goal_callback_ = cb; } } template void SimpleActionServer::registerPreemptCallback(boost::function cb) { preempt_callback_ = cb; } template void SimpleActionServer::publishFeedback(const FeedbackConstPtr & feedback) { current_goal_.publishFeedback(*feedback); } template void SimpleActionServer::publishFeedback(const Feedback & feedback) { current_goal_.publishFeedback(feedback); } template void SimpleActionServer::goalCallback(GoalHandle goal) { boost::recursive_mutex::scoped_lock lock(lock_); ROS_DEBUG_NAMED("actionlib", "A new goal has been recieved by the single goal action server"); // check that the timestamp is past or equal to that of the current goal and the next goal if ((!current_goal_.getGoal() || goal.getGoalID().stamp >= current_goal_.getGoalID().stamp) && (!next_goal_.getGoal() || goal.getGoalID().stamp >= next_goal_.getGoalID().stamp)) { // if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting if (next_goal_.getGoal() && (!current_goal_.getGoal() || next_goal_ != current_goal_)) { next_goal_.setCanceled( Result(), "This goal was canceled because another goal was recieved by the simple action server"); } next_goal_ = goal; new_goal_ = true; new_goal_preempt_request_ = false; // if the server is active, we'll want to call the preempt callback for the current goal if (isActive()) { preempt_request_ = true; // if the user has registered a preempt callback, we'll call it now if (preempt_callback_) { preempt_callback_(); } } // if the user has defined a goal callback, we'll call it now if (goal_callback_) { goal_callback_(); } // Trigger runLoop to call execute() execute_condition_.notify_all(); } else { // the goal requested has already been preempted by a different goal, so we're not going to execute it goal.setCanceled( Result(), "This goal was canceled because another goal was recieved by the simple action server"); } } template void SimpleActionServer::preemptCallback(GoalHandle preempt) { boost::recursive_mutex::scoped_lock lock(lock_); ROS_DEBUG_NAMED("actionlib", "A preempt has been received by the SimpleActionServer"); // if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback if (preempt == current_goal_) { ROS_DEBUG_NAMED("actionlib", "Setting preempt_request bit for the current goal to TRUE and invoking callback"); preempt_request_ = true; // if the user has registered a preempt callback, we'll call it now if (preempt_callback_) { preempt_callback_(); } } else if (preempt == next_goal_) { // if the preempt applies to the next goal, we'll set the preempt bit for that ROS_DEBUG_NAMED("actionlib", "Setting preempt request bit for the next goal to TRUE"); new_goal_preempt_request_ = true; } } template void SimpleActionServer::executeLoop() { ros::Duration loop_duration = ros::Duration().fromSec(.1); while (n_.ok()) { { boost::mutex::scoped_lock terminate_lock(terminate_mutex_); if (need_to_terminate_) { break; } } boost::recursive_mutex::scoped_lock lock(lock_); if (isActive()) { ROS_ERROR_NAMED("actionlib", "Should never reach this code with an active goal"); } else if (isNewGoalAvailable()) { GoalConstPtr goal = acceptNewGoal(); ROS_FATAL_COND(!execute_callback_, "execute_callback_ must exist. This is a bug in SimpleActionServer"); { // Make sure we're not locked when we call execute boost::reverse_lock unlocker(lock); execute_callback_(goal); } if (isActive()) { ROS_WARN_NAMED("actionlib", "Your executeCallback did not set the goal to a terminal status.\n" "This is a bug in your ActionServer implementation. Fix your code!\n" "For now, the ActionServer will set this goal to aborted"); setAborted( Result(), "This goal was aborted by the simple action server. The user should have set a terminal status on this goal and did not"); } } else { execute_condition_.timed_wait(lock, boost::posix_time::milliseconds(static_cast(loop_duration.toSec() * 1000.0f))); } } } template void SimpleActionServer::start() { as_->start(); } } // namespace actionlib #endif // ACTIONLIB__SERVER__SIMPLE_ACTION_SERVER_IMP_H_ actionlib-1.12.0/include/actionlib/server/status_tracker.h000066400000000000000000000055471352264324200236610ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__STATUS_TRACKER_H_ #define ACTIONLIB__SERVER__STATUS_TRACKER_H_ #include #include #include #include namespace actionlib { /** * @class StatusTracker * @brief A class for storing the status of each goal the action server * is currently working on */ template class StatusTracker { private: // generates typedefs that we'll use to make our lives easier ACTION_DEFINITION(ActionSpec); public: StatusTracker(const actionlib_msgs::GoalID & goal_id, unsigned int status); StatusTracker(const boost::shared_ptr & goal); boost::shared_ptr goal_; boost::weak_ptr handle_tracker_; actionlib_msgs::GoalStatus status_; ros::Time handle_destruction_time_; private: GoalIDGenerator id_generator_; }; } // namespace actionlib // include the implementation #include #endif // ACTIONLIB__SERVER__STATUS_TRACKER_H_ actionlib-1.12.0/include/actionlib/server/status_tracker_imp.h000066400000000000000000000055111352264324200245150ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef ACTIONLIB__SERVER__STATUS_TRACKER_IMP_H_ #define ACTIONLIB__SERVER__STATUS_TRACKER_IMP_H_ namespace actionlib { template StatusTracker::StatusTracker(const actionlib_msgs::GoalID & goal_id, unsigned int status) { // set the goal id and status appropriately status_.goal_id = goal_id; status_.status = status; } template StatusTracker::StatusTracker(const boost::shared_ptr & goal) : goal_(goal) { // set the goal_id from the message status_.goal_id = goal_->goal_id; // initialize the status of the goal to pending status_.status = actionlib_msgs::GoalStatus::PENDING; // if the goal id is zero, then we need to make up an id for the goal if (status_.goal_id.id == "") { status_.goal_id = id_generator_.generateID(); } // if the timestamp of the goal is zero, then we'll set it to now() if (status_.goal_id.stamp == ros::Time()) { status_.goal_id.stamp = ros::Time::now(); } } } // namespace actionlib #endif // ACTIONLIB__SERVER__STATUS_TRACKER_IMP_H_ actionlib-1.12.0/mainpage.dox000066400000000000000000000030651352264324200160430ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \section ActionClientAPI ActionClient Code API - \link actionlib::SimpleActionClient SimpleActionClient (C++) \endlink - \link actionlib::simple_action_client::SimpleActionClient SimpleActionClient (Python) \endlink \section ActionServerAPI ActionServer Code API - actionlib::SimpleActionServer - \link actionlib::simple_action_server::SimpleActionServer SimpleActionServer (Python) \endlink \section protocol Communication Protocol The values for the status of a goal are as follows: - \b PENDING - The goal has yet to be processed by the action server - \b ACTIVE - The goal is currently being processed by the action server - \b REJECTED - The goal was rejected by the action server without being processed and without a request from the action client to cancel - \b SUCCEEDED - The goal was achieved successfully by the action server - \b ABORTED - The goal was aborted by the action server - \b PREEMPTING - Processing of the goal was canceled by either another goal, or a cancel request sent to the action server - \b PREEMPTED - The goal was preempted by either another goal, or a preempt message being sent to the action server - \b RECALLING - The goal has not been processed and a cancel request has been received from the action client, but the action server has not confirmed the goal is canceled - \b RECALLED - The goal was canceled by either another goal, or a cancel request before the action server began processing the goal - \b LOST - The goal was sent by the ActionClient, but disappeared due to some communication error */ actionlib-1.12.0/package.xml000066400000000000000000000026261352264324200156650ustar00rootroot00000000000000 actionlib 1.12.0 The actionlib stack provides a standardized interface for interfacing with preemptable tasks. Examples of this include moving the base to a target location, performing a laser scan and returning the resulting point cloud, detecting the handle of a door, etc. Michael Carroll BSD http://www.ros.org/wiki/actionlib https://github.com/ros/actionlib/issues https://github.com/ros/actionlib Eitan Marder-Eppstein Vijay Pradeep Mikael Arguedas catkin message_generation actionlib_msgs boost roscpp rospy rostest std_msgs message_runtime python-wxtools roslib rostopic rosnode actionlib-1.12.0/setup.py000066400000000000000000000003321352264324200152520ustar00rootroot00000000000000#!/usr/bin/env python from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['actionlib'], package_dir={'': 'src'} ) setup(**d) actionlib-1.12.0/src/000077500000000000000000000000001352264324200143315ustar00rootroot00000000000000actionlib-1.12.0/src/actionlib/000077500000000000000000000000001352264324200162755ustar00rootroot00000000000000actionlib-1.12.0/src/actionlib/__init__.py000066400000000000000000000033211352264324200204050ustar00rootroot00000000000000# Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. from actionlib.action_client import * from actionlib.simple_action_client import * from actionlib.action_server import * from actionlib.simple_action_server import * actionlib-1.12.0/src/actionlib/action_client.py000077500000000000000000000636101352264324200214730ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Stuart Glaser ''' Example: from move_base.msg import * rospy.init_node('foo') from move_base.msg import * from geometry_msgs.msg import * g1 = MoveBaseGoal(PoseStamped(Header(frame_id = 'base_link'), Pose(Point(2, 0, 0), Quaternion(0, 0, 0, 1)))) g2 = MoveBaseGoal(PoseStamped(Header(frame_id = 'base_link'), Pose(Point(5, 0, 0), Quaternion(0, 0, 0, 1)))) client = ActionClient('move_base', MoveBaseAction) h1 = client.send_goal(g1) h2 = client.send_goal(g2) client.cancel_all_goals() ''' import threading import weakref import time import rospy from rospy import Header from actionlib_msgs.msg import GoalID, GoalStatus, GoalStatusArray from actionlib.exceptions import ActionException g_goal_id = 1 def get_name_of_constant(C, n): for k, v in C.__dict__.items(): if isinstance(v, int) and v == n: return k return "NO_SUCH_STATE_%d" % n class CommState(object): WAITING_FOR_GOAL_ACK = 0 PENDING = 1 ACTIVE = 2 WAITING_FOR_RESULT = 3 WAITING_FOR_CANCEL_ACK = 4 RECALLING = 5 PREEMPTING = 6 DONE = 7 LOST = 8 class TerminalState(object): RECALLED = GoalStatus.RECALLED REJECTED = GoalStatus.REJECTED PREEMPTED = GoalStatus.PREEMPTED ABORTED = GoalStatus.ABORTED SUCCEEDED = GoalStatus.SUCCEEDED LOST = GoalStatus.LOST GoalStatus.to_string = classmethod(get_name_of_constant) CommState.to_string = classmethod(get_name_of_constant) TerminalState.to_string = classmethod(get_name_of_constant) def _find_status_by_goal_id(status_array, id): for s in status_array.status_list: if s.goal_id.id == id: return s return None ## @brief Client side handle to monitor goal progress. ## ## A ClientGoalHandle is a reference counted object that is used to ## manipulate and monitor the progress of an already dispatched ## goal. Once all the goal handles go out of scope (or are reset), an ## ActionClient stops maintaining state for that goal. class ClientGoalHandle: ## @brief Internal use only ## ## ClientGoalHandle objects should be created by the action ## client. You should never need to construct one yourself. def __init__(self, comm_state_machine): self.comm_state_machine = comm_state_machine # print "GH created. id = %.3f" % self.comm_state_machine.action_goal.goal_id.stamp.to_sec() ## @brief True iff the two ClientGoalHandle's are tracking the same goal def __eq__(self, o): if not o: return False return self.comm_state_machine == o.comm_state_machine ## @brief True iff the two ClientGoalHandle's are tracking different goals def __ne__(self, o): if not o: return True return not (self.comm_state_machine == o.comm_state_machine) ## @brieft Hash function for ClientGoalHandle def __hash__(self): return hash(self.comm_state_machine) ## @brief Sends a cancel message for this specific goal to the ActionServer. ## ## Also transitions the client state to WAITING_FOR_CANCEL_ACK def cancel(self): with self.comm_state_machine.mutex: cancel_msg = GoalID(stamp=rospy.Time(0), id=self.comm_state_machine.action_goal.goal_id.id) self.comm_state_machine.send_cancel_fn(cancel_msg) self.comm_state_machine.transition_to(CommState.WAITING_FOR_CANCEL_ACK) ## @brief Get the state of this goal's communication state machine from interaction with the server ## ## Possible States are: WAITING_FOR_GOAL_ACK, PENDING, ACTIVE, WAITING_FOR_RESULT, ## WAITING_FOR_CANCEL_ACK, RECALLING, PREEMPTING, DONE ## ## @return The current goal's communication state with the server def get_comm_state(self): if not self.comm_state_machine: rospy.logerr("Trying to get_comm_state on an inactive ClientGoalHandle.") return CommState.LOST return self.comm_state_machine.state ## @brief Returns the current status of the goal. ## ## Possible states are listed in the enumeration in the ## actionlib_msgs/GoalStatus message. ## ## @return The current status of the goal. def get_goal_status(self): if not self.comm_state_machine: rospy.logerr("Trying to get_goal_status on an inactive ClientGoalHandle.") return GoalStatus.PENDING return self.comm_state_machine.latest_goal_status.status ## @brief Returns the current status text of the goal. ## ## The text is sent by the action server. ## ## @return The current status text of the goal. def get_goal_status_text(self): if not self.comm_state_machine: rospy.logerr("Trying to get_goal_status_text on an inactive ClientGoalHandle.") return "ERROR: Trying to get_goal_status_text on an inactive ClientGoalHandle." return self.comm_state_machine.latest_goal_status.text ## @brief Gets the result produced by the action server for this goal. ## ## @return None if no result was receieved. Otherwise the goal's result as a *Result message. def get_result(self): if not self.comm_state_machine: rospy.logerr("Trying to get_result on an inactive ClientGoalHandle.") return None if not self.comm_state_machine.latest_result: # rospy.logerr("Trying to get_result on a ClientGoalHandle when no result has been received.") return None return self.comm_state_machine.latest_result.result ## @brief Gets the terminal state information for this goal. ## ## Possible States Are: RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST ## This call only makes sense if CommState==DONE. This will send ROS_WARNs if we're not in DONE ## ## @return The terminal state as an integer from the GoalStatus message. def get_terminal_state(self): if not self.comm_state_machine: rospy.logerr("Trying to get_terminal_state on an inactive ClientGoalHandle.") return GoalStatus.LOST with self.comm_state_machine.mutex: if self.comm_state_machine.state != CommState.DONE: rospy.logwarn("Asking for the terminal state when we're in [%s]", CommState.to_string(self.comm_state_machine.state)) goal_status = self.comm_state_machine.latest_goal_status.status if goal_status in [GoalStatus.PREEMPTED, GoalStatus.SUCCEEDED, GoalStatus.ABORTED, GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.LOST]: return goal_status rospy.logerr("Asking for a terminal state, but the goal status is %d", goal_status) return GoalStatus.LOST NO_TRANSITION = -1 INVALID_TRANSITION = -2 _transitions = { CommState.WAITING_FOR_GOAL_ACK: { GoalStatus.PENDING: CommState.PENDING, GoalStatus.ACTIVE: CommState.ACTIVE, GoalStatus.REJECTED: (CommState.PENDING, CommState.WAITING_FOR_RESULT), GoalStatus.RECALLING: (CommState.PENDING, CommState.RECALLING), GoalStatus.RECALLED: (CommState.PENDING, CommState.WAITING_FOR_RESULT), GoalStatus.PREEMPTED: (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT), GoalStatus.SUCCEEDED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT), GoalStatus.ABORTED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT), GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)}, CommState.PENDING: { GoalStatus.PENDING: NO_TRANSITION, GoalStatus.ACTIVE: CommState.ACTIVE, GoalStatus.REJECTED: CommState.WAITING_FOR_RESULT, GoalStatus.RECALLING: CommState.RECALLING, GoalStatus.RECALLED: (CommState.RECALLING, CommState.WAITING_FOR_RESULT), GoalStatus.PREEMPTED: (CommState.ACTIVE, CommState.PREEMPTING, CommState.WAITING_FOR_RESULT), GoalStatus.SUCCEEDED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT), GoalStatus.ABORTED: (CommState.ACTIVE, CommState.WAITING_FOR_RESULT), GoalStatus.PREEMPTING: (CommState.ACTIVE, CommState.PREEMPTING)}, CommState.ACTIVE: { GoalStatus.PENDING: INVALID_TRANSITION, GoalStatus.ACTIVE: NO_TRANSITION, GoalStatus.REJECTED: INVALID_TRANSITION, GoalStatus.RECALLING: INVALID_TRANSITION, GoalStatus.RECALLED: INVALID_TRANSITION, GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT), GoalStatus.SUCCEEDED: CommState.WAITING_FOR_RESULT, GoalStatus.ABORTED: CommState.WAITING_FOR_RESULT, GoalStatus.PREEMPTING: CommState.PREEMPTING}, CommState.WAITING_FOR_RESULT: { GoalStatus.PENDING: INVALID_TRANSITION, GoalStatus.ACTIVE: NO_TRANSITION, GoalStatus.REJECTED: NO_TRANSITION, GoalStatus.RECALLING: INVALID_TRANSITION, GoalStatus.RECALLED: NO_TRANSITION, GoalStatus.PREEMPTED: NO_TRANSITION, GoalStatus.SUCCEEDED: NO_TRANSITION, GoalStatus.ABORTED: NO_TRANSITION, GoalStatus.PREEMPTING: INVALID_TRANSITION}, CommState.WAITING_FOR_CANCEL_ACK: { GoalStatus.PENDING: NO_TRANSITION, GoalStatus.ACTIVE: NO_TRANSITION, GoalStatus.REJECTED: CommState.WAITING_FOR_RESULT, GoalStatus.RECALLING: CommState.RECALLING, GoalStatus.RECALLED: (CommState.RECALLING, CommState.WAITING_FOR_RESULT), GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT), GoalStatus.SUCCEEDED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT), GoalStatus.ABORTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT), GoalStatus.PREEMPTING: CommState.PREEMPTING}, CommState.RECALLING: { GoalStatus.PENDING: INVALID_TRANSITION, GoalStatus.ACTIVE: INVALID_TRANSITION, GoalStatus.REJECTED: CommState.WAITING_FOR_RESULT, GoalStatus.RECALLING: NO_TRANSITION, GoalStatus.RECALLED: CommState.WAITING_FOR_RESULT, GoalStatus.PREEMPTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT), GoalStatus.SUCCEEDED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT), GoalStatus.ABORTED: (CommState.PREEMPTING, CommState.WAITING_FOR_RESULT), GoalStatus.PREEMPTING: CommState.PREEMPTING}, CommState.PREEMPTING: { GoalStatus.PENDING: INVALID_TRANSITION, GoalStatus.ACTIVE: INVALID_TRANSITION, GoalStatus.REJECTED: INVALID_TRANSITION, GoalStatus.RECALLING: INVALID_TRANSITION, GoalStatus.RECALLED: INVALID_TRANSITION, GoalStatus.PREEMPTED: CommState.WAITING_FOR_RESULT, GoalStatus.SUCCEEDED: CommState.WAITING_FOR_RESULT, GoalStatus.ABORTED: CommState.WAITING_FOR_RESULT, GoalStatus.PREEMPTING: NO_TRANSITION}, CommState.DONE: { GoalStatus.PENDING: INVALID_TRANSITION, GoalStatus.ACTIVE: INVALID_TRANSITION, GoalStatus.REJECTED: NO_TRANSITION, GoalStatus.RECALLING: INVALID_TRANSITION, GoalStatus.RECALLED: NO_TRANSITION, GoalStatus.PREEMPTED: NO_TRANSITION, GoalStatus.SUCCEEDED: NO_TRANSITION, GoalStatus.ABORTED: NO_TRANSITION, GoalStatus.PREEMPTING: INVALID_TRANSITION}} class CommStateMachine: def __init__(self, action_goal, transition_cb, feedback_cb, send_goal_fn, send_cancel_fn): self.action_goal = action_goal self.transition_cb = transition_cb self.feedback_cb = feedback_cb self.send_goal_fn = send_goal_fn self.send_cancel_fn = send_cancel_fn self.state = CommState.WAITING_FOR_GOAL_ACK self.mutex = threading.RLock() self.latest_goal_status = GoalStatus(status=GoalStatus.PENDING) self.latest_result = None def __eq__(self, o): return self.action_goal.goal_id.id == o.action_goal.goal_id.id ## @brieft Hash function for CommStateMachine def __hash__(self): return hash(self.action_goal.goal_id.id) def set_state(self, state): rospy.logdebug("Transitioning CommState from %s to %s", CommState.to_string(self.state), CommState.to_string(state)) self.state = state ## ## @param gh ClientGoalHandle ## @param status_array actionlib_msgs/GoalStatusArray def update_status(self, status_array): with self.mutex: if self.state == CommState.DONE: return status = _find_status_by_goal_id(status_array, self.action_goal.goal_id.id) # You mean you haven't heard of me? if not status: if self.state not in [CommState.WAITING_FOR_GOAL_ACK, CommState.WAITING_FOR_RESULT, CommState.DONE]: self._mark_as_lost() return self.latest_goal_status = status # Determines the next state from the lookup table if self.state not in _transitions: rospy.logerr("CommStateMachine is in a funny state: %i" % self.state) return if status.status not in _transitions[self.state]: rospy.logerr("Got an unknown status from the ActionServer: %i" % status.status) return next_state = _transitions[self.state][status.status] # Knowing the next state, what should we do? if next_state == NO_TRANSITION: pass elif next_state == INVALID_TRANSITION: rospy.logerr("Invalid goal status transition from %s to %s" % (CommState.to_string(self.state), GoalStatus.to_string(status.status))) else: if hasattr(next_state, '__getitem__'): for s in next_state: self.transition_to(s) else: self.transition_to(next_state) def transition_to(self, state): rospy.logdebug("Transitioning to %s (from %s, goal: %s)", CommState.to_string(state), CommState.to_string(self.state), self.action_goal.goal_id.id) self.state = state if self.transition_cb: self.transition_cb(ClientGoalHandle(self)) def _mark_as_lost(self): self.latest_goal_status.status = GoalStatus.LOST self.transition_to(CommState.DONE) def update_result(self, action_result): # Might not be for us if self.action_goal.goal_id.id != action_result.status.goal_id.id: return with self.mutex: self.latest_goal_status = action_result.status self.latest_result = action_result if self.state in [CommState.WAITING_FOR_GOAL_ACK, CommState.WAITING_FOR_CANCEL_ACK, CommState.PENDING, CommState.ACTIVE, CommState.WAITING_FOR_RESULT, CommState.RECALLING, CommState.PREEMPTING]: # Stuffs the goal status in the result into a GoalStatusArray status_array = GoalStatusArray() status_array.status_list.append(action_result.status) self.update_status(status_array) self.transition_to(CommState.DONE) elif self.state == CommState.DONE: rospy.logerr("Got a result when we were already in the DONE state") else: rospy.logerr("In a funny state: %i" % self.state) def update_feedback(self, action_feedback): # Might not be for us if self.action_goal.goal_id.id != action_feedback.status.goal_id.id: return # with self.mutex: if self.feedback_cb and self.state != CommState.DONE: self.feedback_cb(ClientGoalHandle(self), action_feedback.feedback) class GoalManager: # statuses - a list of weak references to CommStateMachine objects def __init__(self, ActionSpec): self.list_mutex = threading.RLock() self.statuses = [] self.send_goal_fn = None try: a = ActionSpec() self.ActionSpec = ActionSpec self.ActionGoal = type(a.action_goal) self.ActionResult = type(a.action_result) self.ActionFeedback = type(a.action_feedback) except AttributeError: raise ActionException("Type is not an action spec: %s" % str(ActionSpec)) def _generate_id(self): global g_goal_id id, g_goal_id = g_goal_id, g_goal_id + 1 now = rospy.Time.now() return GoalID(id="%s-%i-%.3f" % (rospy.get_caller_id(), id, now.to_sec()), stamp=now) def register_send_goal_fn(self, fn): self.send_goal_fn = fn def register_cancel_fn(self, fn): self.cancel_fn = fn ## Sends off a goal and starts tracking its status. ## ## @return ClientGoalHandle for the sent goal. def init_goal(self, goal, transition_cb=None, feedback_cb=None): action_goal = self.ActionGoal(header=Header(), goal_id=self._generate_id(), goal=goal) action_goal.header.stamp = rospy.get_rostime() csm = CommStateMachine(action_goal, transition_cb, feedback_cb, self.send_goal_fn, self.cancel_fn) with self.list_mutex: self.statuses.append(weakref.ref(csm)) self.send_goal_fn(action_goal) return ClientGoalHandle(csm) # Pulls out the statuses that are still live (creating strong # references to them) def _get_live_statuses(self): with self.list_mutex: live_statuses = [r() for r in self.statuses] live_statuses = filter(lambda x: x, live_statuses) return live_statuses ## Updates the statuses of all goals from the information in status_array. ## ## @param status_array (\c actionlib_msgs/GoalStatusArray) def update_statuses(self, status_array): with self.list_mutex: # Garbage collects dead status objects self.statuses = [r for r in self.statuses if r()] for status in self._get_live_statuses(): status.update_status(status_array) def update_results(self, action_result): for status in self._get_live_statuses(): status.update_result(action_result) def update_feedbacks(self, action_feedback): for status in self._get_live_statuses(): status.update_feedback(action_feedback) class ActionClient: ## @brief Constructs an ActionClient and opens connections to an ActionServer. ## ## @param ns The namespace in which to access the action. For ## example, the "goal" topic should occur under ns/goal ## ## @param ActionSpec The *Action message type. The ActionClient ## will grab the other message types from this type. def __init__(self, ns, ActionSpec): self.ns = ns self.last_status_msg = None try: a = ActionSpec() self.ActionSpec = ActionSpec self.ActionGoal = type(a.action_goal) self.ActionResult = type(a.action_result) self.ActionFeedback = type(a.action_feedback) except AttributeError: raise ActionException("Type is not an action spec: %s" % str(ActionSpec)) self.pub_queue_size = rospy.get_param('actionlib_client_pub_queue_size', 10) if self.pub_queue_size < 0: self.pub_queue_size = 10 self.pub_goal = rospy.Publisher(rospy.remap_name(ns) + '/goal', self.ActionGoal, queue_size=self.pub_queue_size) self.pub_cancel = rospy.Publisher(rospy.remap_name(ns) + '/cancel', GoalID, queue_size=self.pub_queue_size) self.manager = GoalManager(ActionSpec) self.manager.register_send_goal_fn(self.pub_goal.publish) self.manager.register_cancel_fn(self.pub_cancel.publish) self.sub_queue_size = rospy.get_param('actionlib_client_sub_queue_size', -1) if self.sub_queue_size < 0: self.sub_queue_size = None self.status_sub = rospy.Subscriber(rospy.remap_name(ns) + '/status', GoalStatusArray, callback=self._status_cb, queue_size=self.sub_queue_size) self.result_sub = rospy.Subscriber(rospy.remap_name(ns) + '/result', self.ActionResult, callback=self._result_cb, queue_size=self.sub_queue_size) self.feedback_sub = rospy.Subscriber(rospy.remap_name(ns) + '/feedback', self.ActionFeedback, callback=self._feedback_cb, queue_size=self.sub_queue_size) ## @brief Sends a goal to the action server ## ## @param goal An instance of the *Goal message. ## ## @param transition_cb Callback that gets called on every client ## state transition for the sent goal. It should take in a ## ClientGoalHandle as an argument. ## ## @param feedback_cb Callback that gets called every time ## feedback is received for the sent goal. It takes two ## parameters: a ClientGoalHandle and an instance of the *Feedback ## message. ## ## @return ClientGoalHandle for the sent goal. def send_goal(self, goal, transition_cb=None, feedback_cb=None): return self.manager.init_goal(goal, transition_cb, feedback_cb) ## @brief Cancels all goals currently running on the action server. ## ## Preempts all goals running on the action server at the point ## that the cancel message is serviced by the action server. def cancel_all_goals(self): cancel_msg = GoalID(stamp=rospy.Time.from_sec(0.0), id="") self.pub_cancel.publish(cancel_msg) ## @brief Cancels all goals prior to a given timestamp ## ## This preempts all goals running on the action server for which the ## time stamp is earlier than the specified time stamp ## this message is serviced by the ActionServer. def cancel_goals_at_and_before_time(self, time): cancel_msg = GoalID(stamp=time, id="") self.pub_cancel.publish(cancel_msg) ## @brief [Deprecated] Use wait_for_server def wait_for_action_server_to_start(self, timeout=rospy.Duration(0.0)): return self.wait_for_server(timeout) ## @brief Waits for the ActionServer to connect to this client ## ## Often, it can take a second for the action server & client to negotiate ## a connection, thus, risking the first few goals to be dropped. This call lets ## the user wait until the network connection to the server is negotiated def wait_for_server(self, timeout=rospy.Duration(0.0)): started = False timeout_time = rospy.get_rostime() + timeout while not rospy.is_shutdown(): if self.last_status_msg: server_id = self.last_status_msg._connection_header['callerid'] if self.pub_goal.impl.has_connection(server_id) and \ self.pub_cancel.impl.has_connection(server_id): # We'll also check that all of the subscribers have at least # one publisher, this isn't a perfect check, but without # publisher callbacks... it'll have to do status_num_pubs = 0 for stat in self.status_sub.impl.get_stats()[1]: if stat[4]: status_num_pubs += 1 result_num_pubs = 0 for stat in self.result_sub.impl.get_stats()[1]: if stat[4]: result_num_pubs += 1 feedback_num_pubs = 0 for stat in self.feedback_sub.impl.get_stats()[1]: if stat[4]: feedback_num_pubs += 1 if status_num_pubs > 0 and result_num_pubs > 0 and feedback_num_pubs > 0: started = True break if timeout != rospy.Duration(0.0) and rospy.get_rostime() >= timeout_time: break time.sleep(0.01) return started def _status_cb(self, msg): self.last_status_msg = msg self.manager.update_statuses(msg) def _result_cb(self, msg): self.manager.update_results(msg) def _feedback_cb(self, msg): self.manager.update_feedbacks(msg) actionlib-1.12.0/src/actionlib/action_server.py000066400000000000000000000367411352264324200215250ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: Alexander Sorokin. # Based on C++ action_server.h by Eitan Marder-Eppstein import rospy import threading from actionlib_msgs.msg import GoalID, GoalStatus, GoalStatusArray from actionlib.goal_id_generator import GoalIDGenerator from actionlib.status_tracker import StatusTracker from actionlib.handle_tracker_deleter import HandleTrackerDeleter from actionlib.server_goal_handle import ServerGoalHandle from actionlib.exceptions import ActionException def nop_cb(goal_handle): pass def ros_timer(callable, frequency): rate = rospy.Rate(frequency) rospy.logdebug("Starting timer") while not rospy.is_shutdown(): try: rate.sleep() callable() except rospy.exceptions.ROSInterruptException: rospy.logdebug("Sleep interrupted") ## @class ActionServer ## @brief The ActionServer is a helpful tool for managing goal requests to a ## node. It allows the user to specify callbacks that are invoked when goal ## or cancel requests come over the wire, and passes back GoalHandles that ## can be used to track the state of a given goal request. The ActionServer ## makes no assumptions about the policy used to service these goals, and ## sends status for each goal over the wire until the last GoalHandle ## associated with a goal request is destroyed. class ActionServer: ## @brief Constructor for an ActionServer ## @param ns/name A namespace for the action server ## @param actionspec An explicit specification of the action ## @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire ## @param cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire ## @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. def __init__(self, ns, ActionSpec, goal_cb, cancel_cb=nop_cb, auto_start=True): self.ns = ns try: a = ActionSpec() self.ActionSpec = ActionSpec self.ActionGoal = type(a.action_goal) self.ActionResult = type(a.action_result) self.ActionResultType = type(a.action_result.result) self.ActionFeedback = type(a.action_feedback) except AttributeError: raise ActionException("Type is not an action spec: %s" % str(ActionSpec)) self.goal_sub = None self.cancel_sub = None self.status_pub = None self.result_pub = None self.feedback_pub = None self.lock = threading.RLock() self.status_timer = None self.status_list = [] self.last_cancel = rospy.Time() self.status_list_timeout = rospy.Duration() self.id_generator = GoalIDGenerator() self.goal_callback = goal_cb assert(self.goal_callback) self.cancel_callback = cancel_cb self.auto_start = auto_start self.started = False if self.auto_start: rospy.logwarn("You've passed in true for auto_start to the python action server, you should always pass " "in false to avoid race conditions.") self.start() ## @brief Register a callback to be invoked when a new goal is received, this will replace any previously registered callback ## @param cb The callback to invoke def register_goal_callback(self, cb): self.goal_callback = cb ## @brief Register a callback to be invoked when a new cancel is received, this will replace any previously registered callback ## @param cb The callback to invoke def register_cancel_callback(self, cancel_cb): self.cancel_callback = cancel_cb ## @brief Start the action server def start(self): with self.lock: self.initialize() self.started = True self.publish_status() ## @brief Initialize all ROS connections and setup timers def initialize(self): self.pub_queue_size = rospy.get_param('actionlib_server_pub_queue_size', 50) if self.pub_queue_size < 0: self.pub_queue_size = 50 self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True, queue_size=self.pub_queue_size) self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult, queue_size=self.pub_queue_size) self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback, queue_size=self.pub_queue_size) self.sub_queue_size = rospy.get_param('actionlib_server_sub_queue_size', -1) if self.sub_queue_size < 0: self.sub_queue_size = None self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal, callback=self.internal_goal_callback, queue_size=self.sub_queue_size) self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID, callback=self.internal_cancel_callback, queue_size=self.sub_queue_size) # read the frequency with which to publish status from the parameter server # if not specified locally explicitly, use search param to find actionlib_status_frequency resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency" if rospy.has_param(resolved_status_frequency_name): self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0) rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.") else: search_status_frequency_name = rospy.search_param("actionlib_status_frequency") if search_status_frequency_name is None: self.status_frequency = 5.0 else: self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0) status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0) self.status_list_timeout = rospy.Duration(status_list_timeout) self.status_timer = threading.Thread(None, ros_timer, None, (self.publish_status_async, self.status_frequency)) self.status_timer.start() ## @brief Publishes a result for a given goal ## @param status The status of the goal with which the result is associated ## @param result The result to publish def publish_result(self, status, result): with self.lock: ar = self.ActionResult() ar.header.stamp = rospy.Time.now() ar.status = status ar.result = result if not rospy.is_shutdown(): self.result_pub.publish(ar) self.publish_status() ## @brief Publishes feedback for a given goal ## @param status The status of the goal with which the feedback is associated ## @param feedback The feedback to publish def publish_feedback(self, status, feedback): with self.lock: af = self.ActionFeedback() af.header.stamp = rospy.Time.now() af.status = status af.feedback = feedback if not rospy.is_shutdown(): self.feedback_pub.publish(af) ## @brief The ROS callback for cancel requests coming into the ActionServer def internal_cancel_callback(self, goal_id): with self.lock: # if we're not started... then we're not actually going to do anything if not self.started: return # we need to handle a cancel for the user rospy.logdebug("The action server has received a new cancel request") goal_id_found = False for st in self.status_list[:]: # check if the goal id is zero or if it is equal to the goal id of # the iterator or if the time of the iterator warrants a cancel cancel_everything = (goal_id.id == "" and goal_id.stamp == rospy.Time()) # rospy::Time()) #id and stamp 0 --> cancel everything cancel_this_one = (goal_id.id == st.status.goal_id.id) # ids match... cancel that goal cancel_before_stamp = (goal_id.stamp != rospy.Time() and st.status.goal_id.stamp <= goal_id.stamp) # //stamp != 0 --> cancel everything before stamp if cancel_everything or cancel_this_one or cancel_before_stamp: # we need to check if we need to store this cancel request for later if goal_id.id == st.status.goal_id.id: goal_id_found = True # attempt to get the handle_tracker for the list item if it exists handle_tracker = st.handle_tracker if handle_tracker is None: # if the handle tracker is expired, then we need to create a new one handle_tracker = HandleTrackerDeleter(self, st) st.handle_tracker = handle_tracker # we also need to reset the time that the status is supposed to be removed from the list st.handle_destruction_time = rospy.Time.now() # set the status of the goal to PREEMPTING or RECALLING as approriate # and check if the request should be passed on to the user gh = ServerGoalHandle(st, self, handle_tracker) if gh.set_cancel_requested(): # call the user's cancel callback on the relevant goal self.cancel_callback(gh) # if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request if goal_id.id != "" and not goal_id_found: tracker = StatusTracker(goal_id, GoalStatus.RECALLING) self.status_list.append(tracker) # start the timer for how long the status will live in the list without a goal handle to it tracker.handle_destruction_time = rospy.Time.now() # make sure to set last_cancel_ based on the stamp associated with this cancel request if goal_id.stamp > self.last_cancel: self.last_cancel = goal_id.stamp ## @brief The ROS callback for goals coming into the ActionServer def internal_goal_callback(self, goal): with self.lock: # if we're not started... then we're not actually going to do anything if not self.started: return rospy.logdebug("The action server has received a new goal request") # we need to check if this goal already lives in the status list for st in self.status_list[:]: if goal.goal_id.id == st.status.goal_id.id: rospy.logdebug("Goal %s was already in the status list with status %i" % (goal.goal_id.id, st.status.status)) # Goal could already be in recalling state if a cancel came in before the goal if st.status.status == GoalStatus.RECALLING: st.status.status = GoalStatus.RECALLED self.publish_result(st.status, self.ActionResultType()) # if this is a request for a goal that has no active handles left, # we'll bump how long it stays in the list if st.handle_tracker is None: st.handle_destruction_time = rospy.Time.now() # make sure not to call any user callbacks or add duplicate status onto the list return # if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on st = StatusTracker(None, None, goal) self.status_list.append(st) # we need to create a handle tracker for the incoming goal and update the StatusTracker handle_tracker = HandleTrackerDeleter(self, st) st.handle_tracker = handle_tracker # check if this goal has already been canceled based on its timestamp gh = ServerGoalHandle(st, self, handle_tracker) if goal.goal_id.stamp != rospy.Time() and goal.goal_id.stamp <= self.last_cancel: # if it has... just create a GoalHandle for it and setCanceled gh.set_canceled(None, "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request") else: # now, we need to create a goal handle and call the user's callback self.goal_callback(gh) ## @brief Publish status for all goals on a timer event def publish_status_async(self): with self.lock: # we won't publish status unless we've been started if not self.started: return self.publish_status() ## @brief Explicitly publish status def publish_status(self): with self.lock: # build a status array status_array = GoalStatusArray() # status_array.set_status_list_size(len(self.status_list)); i = 0 while i < len(self.status_list): st = self.status_list[i] # check if the item is due for deletion from the status list if st.handle_destruction_time != rospy.Time() and st.handle_destruction_time + self.status_list_timeout < rospy.Time.now(): rospy.logdebug("Item %s with destruction time of %.3f being removed from list. Now = %.3f" % (st.status.goal_id, st.handle_destruction_time.to_sec(), rospy.Time.now().to_sec())) del self.status_list[i] else: status_array.status_list.append(st.status) i += 1 status_array.header.stamp = rospy.Time.now() if not rospy.is_shutdown(): self.status_pub.publish(status_array) actionlib-1.12.0/src/actionlib/exceptions.py000077500000000000000000000032111352264324200210300ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2010, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Jonathan Bohren class ActionException(Exception): pass actionlib-1.12.0/src/actionlib/goal_id_generator.py000066400000000000000000000056611352264324200223230ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Alexander Sorokin. # Based on C++ goal_id_generator.h/cpp import rospy from actionlib_msgs.msg import GoalID import threading global s_goalcount_lock global s_goalcount s_goalcount_lock = threading.Lock() s_goalcount = 0 class GoalIDGenerator: def __init__(self, name=None): """ * Create a generator that prepends the fully qualified node name to the Goal ID * \param name Unique name to prepend to the goal id. This will * generally be a fully qualified node name. """ if name is not None: self.set_name(name) else: self.set_name(rospy.get_name()) def set_name(self, name): """ * \param name Set the name to prepend to the goal id. This will * generally be a fully qualified node name. """ self.name = name def generate_ID(self): """ * \brief Generates a unique ID * \return A unique GoalID for this action """ id = GoalID() cur_time = rospy.Time.now() ss = self.name + "-" global s_goalcount_lock global s_goalcount with s_goalcount_lock: s_goalcount += 1 ss += str(s_goalcount) + "-" ss += str(cur_time.secs) + "." + str(cur_time.nsecs) id.id = ss id.stamp = cur_time return id actionlib-1.12.0/src/actionlib/handle_tracker_deleter.py000066400000000000000000000043651352264324200233310ustar00rootroot00000000000000# Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Alexander Sorokin. # Based on C++ goal_id_generator.h/cpp import rospy class HandleTrackerDeleter: """ * @class HandleTrackerDeleter * @brief A class to help with tracking GoalHandles and removing goals * from the status list when the last GoalHandle associated with a given * goal is deleted. """ def __init__(self, action_server, status_tracker): """ @brief create deleter """ self.action_server = action_server self.status_tracker = status_tracker def __call__(self, ptr): if self.action_server: with self.action_server.lock: self.status_tracker.handle_destruction_time = rospy.Time.now() actionlib-1.12.0/src/actionlib/server_goal_handle.py000066400000000000000000000346401352264324200225010ustar00rootroot00000000000000# Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Alexander Sorokin. # Based on C++ goal_id_generator.h/cpp import rospy import actionlib_msgs.msg class ServerGoalHandle: """ * @class ServerGoalHandle * @brief Encapsulates a state machine for a given goal that the user can * trigger transisions on. All ROS interfaces for the goal are managed by * the ActionServer to lessen the burden on the user. """ def __init__(self, status_tracker=None, action_server=None, handle_tracker=None): """ A private constructor used by the ActionServer to initialize a ServerGoalHandle. @node The default constructor was not ported. """ self.status_tracker = status_tracker self.action_server = action_server self.handle_tracker = handle_tracker if status_tracker: self.goal = status_tracker.goal else: self.goal = None def get_default_result(self): return self.action_server.ActionResultType() def set_accepted(self, text=""): """ Accept the goal referenced by the goal handle. This will transition to the ACTIVE state or the PREEMPTING state depending on whether a cancel request has been received for the goal """ rospy.logdebug("Accepting goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec()) if self.goal: with self.action_server.lock: status = self.status_tracker.status.status # if we were pending before, then we'll go active if status == actionlib_msgs.msg.GoalStatus.PENDING: self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.ACTIVE self.status_tracker.status.text = text self.action_server.publish_status() # if we were recalling before, now we'll go to preempting elif status == actionlib_msgs.msg.GoalStatus.RECALLING: self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTING self.status_tracker.status.text = text self.action_server.publish_status() else: rospy.logerr("To transition to an active state, the goal must be in a pending or recalling state, it is currently in state: %d", self.status_tracker.status.status) else: rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle") def set_canceled(self, result=None, text=""): """ Set the status of the goal associated with the ServerGoalHandle to RECALLED or PREEMPTED depending on what the current status of the goal is @param result Optionally, the user can pass in a result to be sent to any clients of the goal """ if not result: result = self.get_default_result() rospy.logdebug("Setting status to canceled on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec()) if self.goal: with self.action_server.lock: status = self.status_tracker.status.status if status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.RECALLING: self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.RECALLED self.status_tracker.status.text = text # on transition to a terminal state, we'll also set the handle destruction time self.status_tracker.handle_destruction_time = rospy.Time.now() self.action_server.publish_result(self.status_tracker.status, result) elif status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING: self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTED self.status_tracker.status.text = text # on transition to a terminal state, we'll also set the handle destruction time self.status_tracker.handle_destruction_time = rospy.Time.now() self.action_server.publish_result(self.status_tracker.status, result) else: rospy.logerr("To transition to a cancelled state, the goal must be in a pending, recalling, active, or preempting state, it is currently in state: %d", self.status_tracker.status.status) else: rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle") def set_rejected(self, result=None, text=""): """ * @brief Set the status of the goal associated with the ServerGoalHandle to rejected * @param result Optionally, the user can pass in a result to be sent to any clients of the goal """ if not result: result = self.get_default_result() rospy.logdebug("Setting status to rejected on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec()) if self.goal: with self.action_server.lock: status = self.status_tracker.status.status if status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.RECALLING: self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.REJECTED self.status_tracker.status.text = text # on transition to a terminal state, we'll also set the handle destruction time self.status_tracker.handle_destruction_time = rospy.Time.now() self.action_server.publish_result(self.status_tracker.status, result) else: rospy.logerr("To transition to a rejected state, the goal must be in a pending or recalling state, it is currently in state: %d", self.status_tracker.status.status) else: rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle") def set_aborted(self, result=None, text=""): """ Set the status of the goal associated with the ServerGoalHandle to aborted @param result Optionally, the user can pass in a result to be sent to any clients of the goal """ if not result: result = self.get_default_result() rospy.logdebug("Setting status to aborted on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec()) if self.goal: with self.action_server.lock: status = self.status_tracker.status.status if status == actionlib_msgs.msg.GoalStatus.PREEMPTING or status == actionlib_msgs.msg.GoalStatus.ACTIVE: self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.ABORTED self.status_tracker.status.text = text # on transition to a terminal state, we'll also set the handle destruction time self.status_tracker.handle_destruction_time = rospy.Time.now() self.action_server.publish_result(self.status_tracker.status, result) else: rospy.logerr("To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: %d", status) else: rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle") def set_succeeded(self, result=None, text=""): """ Set the status of the goal associated with the ServerGoalHandle to succeeded @param result Optionally, the user can pass in a result to be sent to any clients of the goal """ if not result: result = self.get_default_result() rospy.logdebug("Setting status to succeeded on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec()) if self.goal: with self.action_server.lock: status = self.status_tracker.status.status if status == actionlib_msgs.msg.GoalStatus.PREEMPTING or status == actionlib_msgs.msg.GoalStatus.ACTIVE: self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.SUCCEEDED self.status_tracker.status.text = text # on transition to a terminal state, we'll also set the handle destruction time self.status_tracker.handle_destruction_time = rospy.Time.now() self.action_server.publish_result(self.status_tracker.status, result) else: rospy.logerr("To transition to a succeeded state, the goal must be in a preempting or active state, it is currently in state: %d", status) else: rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle") def publish_feedback(self, feedback): """ Send feedback to any clients of the goal associated with this ServerGoalHandle @param feedback The feedback to send to the client """ rospy.logdebug("Publishing feedback for goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec()) if self.goal: with self.action_server.lock: self.action_server.publish_feedback(self.status_tracker.status, feedback) else: rospy.logerr("Attempt to publish feedback on an uninitialized ServerGoalHandle") def get_goal(self): """ Accessor for the goal associated with the ServerGoalHandle @return A shared_ptr to the goal object """ # if we have a goal that is non-null if self.goal: # @todo Test that python reference counting automatically handles this. # create the deleter for our goal subtype # d = EnclosureDeleter(self.goal) # weakref.ref(boost::shared_ptr(&(goal_->goal), d) return self.goal.goal return None def get_goal_id(self): """ Accessor for the goal id associated with the ServerGoalHandle @return The goal id """ if self.goal: with self.action_server.lock: return self.status_tracker.status.goal_id else: rospy.logerr("Attempt to get a goal id on an uninitialized ServerGoalHandle") return actionlib_msgs.msg.GoalID() def get_goal_status(self): """ Accessor for the status associated with the ServerGoalHandle @return The goal status """ if self.goal: with self.action_server.lock: return self.status_tracker.status else: rospy.logerr("Attempt to get goal status on an uninitialized ServerGoalHandle") return actionlib_msgs.msg.GoalStatus() def __eq__(self, other): """ Equals operator for ServerGoalHandles @param other The ServerGoalHandle to compare to @return True if the ServerGoalHandles refer to the same goal, false otherwise """ if not self.goal or not other.goal: return False my_id = self.get_goal_id() their_id = other.get_goal_id() return my_id.id == their_id.id def __ne__(self, other): """ != operator for ServerGoalHandles @param other The ServerGoalHandle to compare to @return True if the ServerGoalHandles refer to different goals, false otherwise """ if not self.goal or not other.goal: return True my_id = self.get_goal_id() their_id = other.get_goal_id() return my_id.id != their_id.id def __hash__(self): """ hash function for ServerGoalHandles @return hash of the goal ID """ return hash(self.get_goal_id().id) def set_cancel_requested(self): """ A private method to set status to PENDING or RECALLING @return True if the cancel request should be passed on to the user, false otherwise """ rospy.logdebug("Transisitoning to a cancel requested state on goal id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec()) if self.goal: with self.action_server.lock: status = self.status_tracker.status.status if status == actionlib_msgs.msg.GoalStatus.PENDING: self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.RECALLING self.action_server.publish_status() return True if status == actionlib_msgs.msg.GoalStatus.ACTIVE: self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTING self.action_server.publish_status() return True return False actionlib-1.12.0/src/actionlib/simple_action_client.py000066400000000000000000000262431352264324200230420ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Stuart Glaser import rospy import threading from actionlib_msgs.msg import GoalStatus from actionlib.action_client import ActionClient, CommState, get_name_of_constant class SimpleGoalState: PENDING = 0 ACTIVE = 1 DONE = 2 SimpleGoalState.to_string = classmethod(get_name_of_constant) class SimpleActionClient: ## @brief Constructs a SimpleActionClient and opens connections to an ActionServer. ## ## @param ns The namespace in which to access the action. For ## example, the "goal" topic should occur under ns/goal ## ## @param ActionSpec The *Action message type. The SimpleActionClient ## will grab the other message types from this type. def __init__(self, ns, ActionSpec): self.action_client = ActionClient(ns, ActionSpec) self.simple_state = SimpleGoalState.DONE self.gh = None self.done_condition = threading.Condition() ## @brief Blocks until the action server connects to this client ## ## @param timeout Max time to block before returning. A zero ## timeout is interpreted as an infinite timeout. ## ## @return True if the server connected in the allocated time. False on timeout def wait_for_server(self, timeout=rospy.Duration()): return self.action_client.wait_for_server(timeout) ## @brief Sends a goal to the ActionServer, and also registers callbacks ## ## If a previous goal is already active when this is called. We simply forget ## about that goal and start tracking the new goal. No cancel requests are made. ## ## @param done_cb Callback that gets called on transitions to ## Done. The callback should take two parameters: the terminal ## state (as an integer from actionlib_msgs/GoalStatus) and the ## result. ## ## @param active_cb No-parameter callback that gets called on transitions to Active. ## ## @param feedback_cb Callback that gets called whenever feedback ## for this goal is received. Takes one parameter: the feedback. def send_goal(self, goal, done_cb=None, active_cb=None, feedback_cb=None): # destroys the old goal handle self.stop_tracking_goal() self.done_cb = done_cb self.active_cb = active_cb self.feedback_cb = feedback_cb self.simple_state = SimpleGoalState.PENDING self.gh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback) ## @brief Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary ## ## If a previous goal is already active when this is called. We simply forget ## about that goal and start tracking the new goal. No cancel requests are made. ## ## If the goal does not complete within the execute_timeout, the goal gets preempted ## ## If preemption of the goal does not complete withing the preempt_timeout, this ## method simply returns ## ## @param execute_timeout The time to wait for the goal to complete ## ## @param preempt_timeout The time to wait for preemption to complete ## ## @return The goal's state. def send_goal_and_wait(self, goal, execute_timeout=rospy.Duration(), preempt_timeout=rospy.Duration()): self.send_goal(goal) if not self.wait_for_result(execute_timeout): # preempt action rospy.logdebug("Canceling goal") self.cancel_goal() if self.wait_for_result(preempt_timeout): rospy.logdebug("Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.to_sec()) else: rospy.logdebug("Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.to_sec()) return self.get_state() ## @brief Blocks until this goal transitions to done ## @param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout. ## @return True if the goal finished. False if the goal didn't finish within the allocated timeout def wait_for_result(self, timeout=rospy.Duration()): if not self.gh: rospy.logerr("Called wait_for_result when no goal exists") return False timeout_time = rospy.get_rostime() + timeout loop_period = rospy.Duration(0.1) with self.done_condition: while not rospy.is_shutdown(): time_left = timeout_time - rospy.get_rostime() if timeout > rospy.Duration(0.0) and time_left <= rospy.Duration(0.0): break if self.simple_state == SimpleGoalState.DONE: break if time_left > loop_period or timeout == rospy.Duration(): time_left = loop_period self.done_condition.wait(time_left.to_sec()) return self.simple_state == SimpleGoalState.DONE ## @brief Gets the Result of the current goal def get_result(self): if not self.gh: rospy.logerr("Called get_result when no goal is running") return None return self.gh.get_result() ## @brief Get the state information for this goal ## ## Possible States Are: PENDING, ACTIVE, RECALLED, REJECTED, ## PREEMPTED, ABORTED, SUCCEEDED, LOST. ## ## @return The goal's state. Returns LOST if this ## SimpleActionClient isn't tracking a goal. def get_state(self): if not self.gh: return GoalStatus.LOST status = self.gh.get_goal_status() if status == GoalStatus.RECALLING: status = GoalStatus.PENDING elif status == GoalStatus.PREEMPTING: status = GoalStatus.ACTIVE return status ## @brief Returns the current status text of the goal. ## ## The text is sent by the action server. It is designed to ## help debugging issues on the server side. ## ## @return The current status text of the goal. def get_goal_status_text(self): if not self.gh: rospy.logerr("Called get_goal_status_text when no goal is running") return "ERROR: Called get_goal_status_text when no goal is running" return self.gh.get_goal_status_text() ## @brief Cancels all goals currently running on the action server ## ## This preempts all goals running on the action server at the point that ## this message is serviced by the ActionServer. def cancel_all_goals(self): self.action_client.cancel_all_goals() ## @brief Cancels all goals prior to a given timestamp ## ## This preempts all goals running on the action server for which the ## time stamp is earlier than the specified time stamp ## this message is serviced by the ActionServer. def cancel_goals_at_and_before_time(self, time): self.action_client.cancel_goals_at_and_before_time(time) ## @brief Cancels the goal that we are currently pursuing def cancel_goal(self): if self.gh: self.gh.cancel() ## @brief Stops tracking the state of the current goal. Unregisters this goal's callbacks ## ## This is useful if we want to make sure we stop calling our callbacks before sending a new goal. ## Note that this does not cancel the goal, it simply stops looking for status info about this goal. def stop_tracking_goal(self): self.gh = None def _handle_transition(self, gh): comm_state = gh.get_comm_state() error_msg = "Received comm state %s when in simple state %s with SimpleActionClient in NS %s" % \ (CommState.to_string(comm_state), SimpleGoalState.to_string(self.simple_state), rospy.resolve_name(self.action_client.ns)) if comm_state == CommState.ACTIVE: if self.simple_state == SimpleGoalState.PENDING: self._set_simple_state(SimpleGoalState.ACTIVE) if self.active_cb: self.active_cb() elif self.simple_state == SimpleGoalState.DONE: rospy.logerr(error_msg) elif comm_state == CommState.RECALLING: if self.simple_state != SimpleGoalState.PENDING: rospy.logerr(error_msg) elif comm_state == CommState.PREEMPTING: if self.simple_state == SimpleGoalState.PENDING: self._set_simple_state(SimpleGoalState.ACTIVE) if self.active_cb: self.active_cb() elif self.simple_state == SimpleGoalState.DONE: rospy.logerr(error_msg) elif comm_state == CommState.DONE: if self.simple_state in [SimpleGoalState.PENDING, SimpleGoalState.ACTIVE]: self._set_simple_state(SimpleGoalState.DONE) if self.done_cb: self.done_cb(gh.get_goal_status(), gh.get_result()) with self.done_condition: self.done_condition.notifyAll() elif self.simple_state == SimpleGoalState.DONE: rospy.logerr("SimpleActionClient received DONE twice") def _handle_feedback(self, gh, feedback): if not self.gh: # this is not actually an error - there can be a small window in which old feedback # can be received between the time this variable is reset and a new goal is # sent and confirmed return if gh != self.gh: rospy.logerr("Got a feedback callback on a goal handle that we're not tracking. %s vs %s" % (self.gh.comm_state_machine.action_goal.goal_id.id, gh.comm_state_machine.action_goal.goal_id.id)) return if self.feedback_cb: self.feedback_cb(feedback) def _set_simple_state(self, state): self.simple_state = state actionlib-1.12.0/src/actionlib/simple_action_server.py000066400000000000000000000350071352264324200230700ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: Alexander Sorokin. # Based on C++ simple_action_server.h by Eitan Marder-Eppstein import rospy import threading import traceback from actionlib_msgs.msg import GoalStatus from actionlib import ActionServer from actionlib.server_goal_handle import ServerGoalHandle def nop_cb(goal_handle): pass ## @class SimpleActionServer ## @brief The SimpleActionServer ## implements a singe goal policy on top of the ActionServer class. The ## specification of the policy is as follows: only one goal can have an ## active status at a time, new goals preempt previous goals based on the ## stamp in their GoalID field (later goals preempt earlier ones), an ## explicit preempt goal preempts all goals with timestamps that are less ## than or equal to the stamp associated with the preempt, accepting a new ## goal implies successful preemption of any old goal and the status of the ## old goal will be change automatically to reflect this. class SimpleActionServer: ## @brief Constructor for a SimpleActionServer ## @param name A name for the action server ## @param execute_cb Optional callback that gets called in a separate thread whenever ## a new goal is received, allowing users to have blocking callbacks. ## Adding an execute callback also deactivates the goalCallback. ## @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True): self.new_goal = False self.preempt_request = False self.new_goal_preempt_request = False self.execute_callback = execute_cb self.goal_callback = None self.preempt_callback = None self.need_to_terminate = False self.terminate_mutex = threading.RLock() # since the internal_goal/preempt_callbacks are invoked from the # ActionServer while holding the self.action_server.lock # self.lock must always be locked after the action server lock # to avoid an inconsistent lock acquisition order self.lock = threading.RLock() self.execute_condition = threading.Condition(self.lock) self.current_goal = ServerGoalHandle() self.next_goal = ServerGoalHandle() if self.execute_callback: self.execute_thread = threading.Thread(None, self.executeLoop) self.execute_thread.start() else: self.execute_thread = None # create the action server self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start) def __del__(self): if hasattr(self, 'execute_callback') and self.execute_callback: with self.terminate_mutex: self.need_to_terminate = True assert(self.execute_thread) self.execute_thread.join() ## @brief Accepts a new goal when one is available The status of this ## goal is set to active upon acceptance, and the status of any ## previously active goal is set to preempted. Preempts received for the ## new goal between checking if isNewGoalAvailable or invokation of a ## goal callback and the acceptNewGoal call will not trigger a preempt ## callback. This means, isPreemptReqauested should be called after ## accepting the goal even for callback-based implementations to make ## sure the new goal does not have a pending preempt request. ## @return A shared_ptr to the new goal. def accept_new_goal(self): with self.action_server.lock, self.lock: if not self.new_goal or not self.next_goal.get_goal(): rospy.logerr("Attempting to accept the next goal when a new goal is not available") return None # check if we need to send a preempted message for the goal that we're currently pursuing if self.is_active() and self.current_goal.get_goal() and self.current_goal != self.next_goal: self.current_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server") rospy.logdebug("Accepting a new goal") # accept the next goal self.current_goal = self.next_goal self.new_goal = False # set preempt to request to equal the preempt state of the new goal self.preempt_request = self.new_goal_preempt_request self.new_goal_preempt_request = False # set the status of the current goal to be active self.current_goal.set_accepted("This goal has been accepted by the simple action server") return self.current_goal.get_goal() ## @brief Allows polling implementations to query about the availability of a new goal ## @return True if a new goal is available, false otherwise def is_new_goal_available(self): return self.new_goal ## @brief Allows polling implementations to query about preempt requests ## @return True if a preempt is requested, false otherwise def is_preempt_requested(self): return self.preempt_request ## @brief Allows polling implementations to query about the status of the current goal ## @return True if a goal is active, false otherwise def is_active(self): if not self.current_goal.get_goal(): return False status = self.current_goal.get_goal_status().status return status == GoalStatus.ACTIVE or status == GoalStatus.PREEMPTING ## @brief Sets the status of the active goal to succeeded ## @param result An optional result to send back to any clients of the goal def set_succeeded(self, result=None, text=""): with self.action_server.lock, self.lock: if not result: result = self.get_default_result() self.current_goal.set_succeeded(result, text) ## @brief Sets the status of the active goal to aborted ## @param result An optional result to send back to any clients of the goal def set_aborted(self, result=None, text=""): with self.action_server.lock, self.lock: if not result: result = self.get_default_result() self.current_goal.set_aborted(result, text) ## @brief Publishes feedback for a given goal ## @param feedback Shared pointer to the feedback to publish def publish_feedback(self, feedback): self.current_goal.publish_feedback(feedback) def get_default_result(self): return self.action_server.ActionResultType() ## @brief Sets the status of the active goal to preempted ## @param result An optional result to send back to any clients of the goal def set_preempted(self, result=None, text=""): if not result: result = self.get_default_result() with self.action_server.lock, self.lock: rospy.logdebug("Setting the current goal as canceled") self.current_goal.set_canceled(result, text) ## @brief Allows users to register a callback to be invoked when a new goal is available ## @param cb The callback to be invoked def register_goal_callback(self, cb): if self.execute_callback: rospy.logwarn("Cannot call SimpleActionServer.register_goal_callback() because an executeCallback exists. Not going to register it.") else: self.goal_callback = cb ## @brief Allows users to register a callback to be invoked when a new preempt request is available ## @param cb The callback to be invoked def register_preempt_callback(self, cb): self.preempt_callback = cb ## @brief Explicitly start the action server, used it auto_start is set to false def start(self): self.action_server.start() ## @brief Callback for when the ActionServer receives a new goal and passes it on def internal_goal_callback(self, goal): self.execute_condition.acquire() try: rospy.logdebug("A new goal %shas been recieved by the single goal action server", goal.get_goal_id().id) # check that the timestamp is past that of the current goal and the next goal if((not self.current_goal.get_goal() or goal.get_goal_id().stamp >= self.current_goal.get_goal_id().stamp) and (not self.next_goal.get_goal() or goal.get_goal_id().stamp >= self.next_goal.get_goal_id().stamp)): # if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting if(self.next_goal.get_goal() and (not self.current_goal.get_goal() or self.next_goal != self.current_goal)): self.next_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server") self.next_goal = goal self.new_goal = True self.new_goal_preempt_request = False # if the server is active, we'll want to call the preempt callback for the current goal if(self.is_active()): self.preempt_request = True # if the user has registered a preempt callback, we'll call it now if(self.preempt_callback): self.preempt_callback() # if the user has defined a goal callback, we'll call it now if self.goal_callback: self.goal_callback() # Trigger runLoop to call execute() self.execute_condition.notify() self.execute_condition.release() else: # the goal requested has already been preempted by a different goal, so we're not going to execute it goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server") self.execute_condition.release() except Exception as e: rospy.logerr("SimpleActionServer.internal_goal_callback - exception %s", str(e)) self.execute_condition.release() ## @brief Callback for when the ActionServer receives a new preempt and passes it on def internal_preempt_callback(self, preempt): with self.lock: rospy.logdebug("A preempt has been received by the SimpleActionServer") # if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback if(preempt == self.current_goal): rospy.logdebug("Setting preempt_request bit for the current goal to TRUE and invoking callback") self.preempt_request = True # if the user has registered a preempt callback, we'll call it now if(self.preempt_callback): self.preempt_callback() # if the preempt applies to the next goal, we'll set the preempt bit for that elif(preempt == self.next_goal): rospy.logdebug("Setting preempt request bit for the next goal to TRUE") self.new_goal_preempt_request = True ## @brief Called from a separate thread to call blocking execute calls def executeLoop(self): loop_duration = rospy.Duration.from_sec(.1) while (not rospy.is_shutdown()): with self.terminate_mutex: if (self.need_to_terminate): break # the following checks (is_active, is_new_goal_available) # are performed without locking # the worst thing that might happen in case of a race # condition is a warning/error message on the console if (self.is_active()): rospy.logerr("Should never reach this code with an active goal") return if (self.is_new_goal_available()): # accept_new_goal() is performing its own locking goal = self.accept_new_goal() if not self.execute_callback: rospy.logerr("execute_callback_ must exist. This is a bug in SimpleActionServer") return try: self.execute_callback(goal) if self.is_active(): rospy.logwarn("Your executeCallback did not set the goal to a terminal status. " + "This is a bug in your ActionServer implementation. Fix your code! " + "For now, the ActionServer will set this goal to aborted") self.set_aborted(None, "No terminal state was set.") except Exception as ex: rospy.logerr("Exception in your execute callback: %s\n%s", str(ex), traceback.format_exc()) self.set_aborted(None, "Exception in execute callback: %s" % str(ex)) with self.execute_condition: self.execute_condition.wait(loop_duration.to_sec()) actionlib-1.12.0/src/actionlib/status_tracker.py000066400000000000000000000057751352264324200217230ustar00rootroot00000000000000# Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Alexander Sorokin. # Based on C++ goal_id_generator.h/cpp import rospy import actionlib_msgs.msg from actionlib import goal_id_generator class StatusTracker: """ * @class StatusTracker * @brief A class for storing the status of each goal the action server * is currently working on """ def __init__(self, goal_id=None, status=None, goal=None): """ @brief create status tracker. Either pass goal_id and status OR goal """ self.goal = None self.handle_tracker = None self.status = actionlib_msgs.msg.GoalStatus() self.handle_destruction_time = rospy.Time() self.id_generator = goal_id_generator.GoalIDGenerator() if goal_id: # set the goal id and status appropriately self.status.goal_id = goal_id self.status.status = status else: self.goal = goal self.status.goal_id = goal.goal_id # initialize the status of the goal to pending self.status.status = actionlib_msgs.msg.GoalStatus.PENDING # if the goal id is zero, then we need to make up an id for the goal if self.status.goal_id.id == "": self.status.goal_id = self.id_generator.generate_ID() # if the timestamp of the goal is zero, then we'll set it to now() if self.status.goal_id.stamp == rospy.Time(): self.status.goal_id.stamp = rospy.Time.now() actionlib-1.12.0/src/connection_monitor.cpp000066400000000000000000000236251352264324200207530ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #include #include #include #include #include // using namespace actionlib; #define CONNECTION_DEBUG(fmt, ...) \ ROS_DEBUG_NAMED("ConnectionMonitor", fmt, ## __VA_ARGS__) #define CONNECTION_WARN(fmt, ...) \ ROS_WARN_NAMED("ConnectionMonitor", fmt, ## __VA_ARGS__) actionlib::ConnectionMonitor::ConnectionMonitor(ros::Subscriber & feedback_sub, ros::Subscriber & result_sub) : feedback_sub_(feedback_sub), result_sub_(result_sub) { status_received_ = false; } // ********* Goal Connections ********* void actionlib::ConnectionMonitor::goalConnectCallback(const ros::SingleSubscriberPublisher & pub) { boost::recursive_mutex::scoped_lock lock(data_mutex_); // Check if it's not in the list if (goalSubscribers_.find(pub.getSubscriberName()) == goalSubscribers_.end()) { CONNECTION_DEBUG("goalConnectCallback: Adding [%s] to goalSubscribers", pub.getSubscriberName().c_str()); goalSubscribers_[pub.getSubscriberName()] = 1; } else { CONNECTION_WARN( "goalConnectCallback: Trying to add [%s] to goalSubscribers, but it is already in the goalSubscribers list", pub.getSubscriberName().c_str()); goalSubscribers_[pub.getSubscriberName()]++; } CONNECTION_DEBUG("%s", goalSubscribersString().c_str()); check_connection_condition_.notify_all(); } void actionlib::ConnectionMonitor::goalDisconnectCallback(const ros::SingleSubscriberPublisher & pub) { boost::recursive_mutex::scoped_lock lock(data_mutex_); std::map::iterator it; it = goalSubscribers_.find(pub.getSubscriberName()); if (it == goalSubscribers_.end()) { CONNECTION_WARN( "goalDisconnectCallback: Trying to remove [%s] to goalSubscribers, but it is not in the goalSubscribers list", pub.getSubscriberName().c_str()); } else { CONNECTION_DEBUG("goalDisconnectCallback: Removing [%s] from goalSubscribers", pub.getSubscriberName().c_str()); goalSubscribers_[pub.getSubscriberName()]--; if (goalSubscribers_[pub.getSubscriberName()] == 0) { goalSubscribers_.erase(it); } } CONNECTION_DEBUG("%s", goalSubscribersString().c_str()); } std::string actionlib::ConnectionMonitor::goalSubscribersString() { boost::recursive_mutex::scoped_lock lock(data_mutex_); std::ostringstream ss; ss << "Goal Subscribers (" << goalSubscribers_.size() << " total)"; for (std::map::iterator it = goalSubscribers_.begin(); it != goalSubscribers_.end(); it++) { ss << "\n - " << it->first; } return ss.str(); } // ********* Cancel Connections ********* void actionlib::ConnectionMonitor::cancelConnectCallback(const ros::SingleSubscriberPublisher & pub) { boost::recursive_mutex::scoped_lock lock(data_mutex_); // Check if it's not in the list if (cancelSubscribers_.find(pub.getSubscriberName()) == cancelSubscribers_.end()) { CONNECTION_DEBUG("cancelConnectCallback: Adding [%s] to cancelSubscribers", pub.getSubscriberName().c_str()); cancelSubscribers_[pub.getSubscriberName()] = 1; } else { CONNECTION_WARN( "cancelConnectCallback: Trying to add [%s] to cancelSubscribers, but it is already in the cancelSubscribers list", pub.getSubscriberName().c_str()); cancelSubscribers_[pub.getSubscriberName()]++; } CONNECTION_DEBUG("%s", cancelSubscribersString().c_str()); check_connection_condition_.notify_all(); } void actionlib::ConnectionMonitor::cancelDisconnectCallback( const ros::SingleSubscriberPublisher & pub) { boost::recursive_mutex::scoped_lock lock(data_mutex_); std::map::iterator it; it = cancelSubscribers_.find(pub.getSubscriberName()); if (it == cancelSubscribers_.end()) { CONNECTION_WARN( "cancelDisconnectCallback: Trying to remove [%s] to cancelSubscribers, but it is not in the cancelSubscribers list", pub.getSubscriberName().c_str()); } else { CONNECTION_DEBUG("cancelDisconnectCallback: Removing [%s] from cancelSubscribers", pub.getSubscriberName().c_str()); cancelSubscribers_[pub.getSubscriberName()]--; if (cancelSubscribers_[pub.getSubscriberName()] == 0) { cancelSubscribers_.erase(it); } } CONNECTION_DEBUG("%s", cancelSubscribersString().c_str()); } std::string actionlib::ConnectionMonitor::cancelSubscribersString() { boost::recursive_mutex::scoped_lock lock(data_mutex_); std::ostringstream ss; ss << "cancel Subscribers (" << cancelSubscribers_.size() << " total)"; for (std::map::iterator it = cancelSubscribers_.begin(); it != cancelSubscribers_.end(); it++) { ss << "\n - " << it->first; } return ss.str(); } // ********* GoalStatus Connections ********* void actionlib::ConnectionMonitor::processStatus( const actionlib_msgs::GoalStatusArrayConstPtr & status, const std::string & cur_status_caller_id) { boost::recursive_mutex::scoped_lock lock(data_mutex_); if (status_received_) { if (status_caller_id_ != cur_status_caller_id) { CONNECTION_WARN( "processStatus: Previously received status from [%s], but we now received status from [%s]. Did the ActionServer change?", status_caller_id_.c_str(), cur_status_caller_id.c_str()); status_caller_id_ = cur_status_caller_id; } latest_status_time_ = status->header.stamp; } else { CONNECTION_DEBUG( "processStatus: Just got our first status message from the ActionServer at node [%s]", cur_status_caller_id.c_str()); status_received_ = true; status_caller_id_ = cur_status_caller_id; latest_status_time_ = status->header.stamp; } check_connection_condition_.notify_all(); } // ********* Connection logic ********* bool actionlib::ConnectionMonitor::isServerConnected() { boost::recursive_mutex::scoped_lock lock(data_mutex_); if (!status_received_) { CONNECTION_DEBUG("isServerConnected: Didn't receive status yet, so not connected yet"); return false; } if (goalSubscribers_.find(status_caller_id_) == goalSubscribers_.end()) { CONNECTION_DEBUG( "isServerConnected: Server [%s] has not yet subscribed to the goal topic, so not connected yet", status_caller_id_.c_str()); CONNECTION_DEBUG("%s", goalSubscribersString().c_str()); return false; } if (cancelSubscribers_.find(status_caller_id_) == cancelSubscribers_.end()) { CONNECTION_DEBUG( "isServerConnected: Server [%s] has not yet subscribed to the cancel topic, so not connected yet", status_caller_id_.c_str()); CONNECTION_DEBUG("%s", cancelSubscribersString().c_str()); return false; } if (feedback_sub_.getNumPublishers() == 0) { CONNECTION_DEBUG( "isServerConnected: Client has not yet connected to feedback topic of server [%s]", status_caller_id_.c_str()); return false; } if (result_sub_.getNumPublishers() == 0) { CONNECTION_DEBUG( "isServerConnected: Client has not yet connected to result topic of server [%s]", status_caller_id_.c_str()); return false; } CONNECTION_DEBUG("isServerConnected: Server [%s] is fully connected", status_caller_id_.c_str()); return true; } bool actionlib::ConnectionMonitor::waitForActionServerToStart(const ros::Duration & timeout, const ros::NodeHandle & nh) { if (timeout < ros::Duration(0, 0)) { ROS_ERROR_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec()); } ros::Time timeout_time = ros::Time::now() + timeout; boost::recursive_mutex::scoped_lock lock(data_mutex_); if (isServerConnected()) { return true; } // Hardcode how often we check for node.ok() ros::Duration loop_period = ros::Duration().fromSec(.5); while (nh.ok() && !isServerConnected()) { // Determine how long we should wait ros::Duration time_left = timeout_time - ros::Time::now(); // Check if we're past the timeout time if (timeout != ros::Duration(0, 0) && time_left <= ros::Duration(0, 0) ) { break; } // Truncate the time left if (time_left > loop_period || timeout == ros::Duration()) { time_left = loop_period; } check_connection_condition_.timed_wait(lock, boost::posix_time::milliseconds(static_cast(time_left.toSec() * 1000.0f))); } return isServerConnected(); } actionlib-1.12.0/src/goal_id_generator.cpp000066400000000000000000000051551352264324200205070ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ #include #include #include #include // using namespace actionlib; static boost::mutex s_goalcount_mutex_; static unsigned int s_goalcount_ = 0; actionlib::GoalIDGenerator::GoalIDGenerator() { setName(ros::this_node::getName()); } actionlib::GoalIDGenerator::GoalIDGenerator(const std::string & name) { setName(name); } void actionlib::GoalIDGenerator::setName(const std::string & name) { name_ = name; } actionlib_msgs::GoalID actionlib::GoalIDGenerator::generateID() { actionlib_msgs::GoalID id; ros::Time cur_time = ros::Time::now(); std::stringstream ss; ss << name_ << "-"; { boost::mutex::scoped_lock lock(s_goalcount_mutex_); s_goalcount_++; ss << s_goalcount_ << "-"; } ss << cur_time.sec << "." << cur_time.nsec; id.id = ss.str(); id.stamp = cur_time; return id; } actionlib-1.12.0/test/000077500000000000000000000000001352264324200145215ustar00rootroot00000000000000actionlib-1.12.0/test/CMakeLists.txt000066400000000000000000000065261352264324200172720ustar00rootroot00000000000000if(GTEST_FOUND) include_directories(${GTEST_INCLUDE_DIRS}) link_directories(${GTEST_LIBRARY_DIRS}) add_executable(actionlib-add_two_ints_server EXCLUDE_FROM_ALL add_two_ints_server.cpp) target_link_libraries(actionlib-add_two_ints_server ${PROJECT_NAME} ${GTEST_LIBRARIES}) add_executable(actionlib-ref_server EXCLUDE_FROM_ALL ref_server.cpp) target_link_libraries(actionlib-ref_server ${PROJECT_NAME}) add_executable(actionlib-simple_client_test EXCLUDE_FROM_ALL simple_client_test.cpp) target_link_libraries(actionlib-simple_client_test ${PROJECT_NAME} ${GTEST_LIBRARIES}) add_executable(actionlib-simple_execute_ref_server EXCLUDE_FROM_ALL simple_execute_ref_server.cpp) target_link_libraries(actionlib-simple_execute_ref_server ${PROJECT_NAME}) add_executable(actionlib-server_goal_handle_destruction EXCLUDE_FROM_ALL server_goal_handle_destruction.cpp) target_link_libraries(actionlib-server_goal_handle_destruction ${PROJECT_NAME} ${GTEST_LIBRARIES}) add_executable(actionlib-simple_client_wait_test EXCLUDE_FROM_ALL simple_client_wait_test.cpp) target_link_libraries(actionlib-simple_client_wait_test ${PROJECT_NAME} ${GTEST_LIBRARIES}) add_executable(actionlib-simple_client_allocator_test EXCLUDE_FROM_ALL simple_client_allocator_test.cpp) target_link_libraries(actionlib-simple_client_allocator_test ${PROJECT_NAME} ${GTEST_LIBRARIES}) add_executable(actionlib-action_client_destruction_test EXCLUDE_FROM_ALL action_client_destruction_test.cpp) target_link_libraries(actionlib-action_client_destruction_test ${PROJECT_NAME} ${GTEST_LIBRARIES}) add_executable(actionlib-test_cpp_simple_client_cancel_crash EXCLUDE_FROM_ALL test_cpp_simple_client_cancel_crash.cpp) target_link_libraries(actionlib-test_cpp_simple_client_cancel_crash ${PROJECT_NAME} ${GTEST_LIBRARIES}) add_executable(actionlib-exercise_simple_client EXCLUDE_FROM_ALL exercise_simple_client.cpp) target_link_libraries(actionlib-exercise_simple_client ${PROJECT_NAME} ${GTEST_LIBRARIES}) if(TARGET tests) add_dependencies(tests actionlib-add_two_ints_server actionlib-ref_server actionlib-simple_client_test actionlib-simple_execute_ref_server actionlib-server_goal_handle_destruction actionlib-simple_client_wait_test actionlib-simple_client_allocator_test actionlib-action_client_destruction_test actionlib-test_cpp_simple_client_cancel_crash actionlib-exercise_simple_client ) endif() endif() add_rostest(ref_server_test.launch) add_rostest(simple_execute_ref_server_test.launch) add_rostest(test_python_simple_client.launch) add_rostest(test_cpp_simple_client_allocator.launch) add_rostest(test_cpp_action_client_destruction.launch) add_rostest(test_server_goal_handle_destruction.launch) add_rostest(test_cpp_simple_client_cancel_crash.launch) add_rostest(test_imports.launch) add_rostest(test_python_server_components.launch) add_rostest(test_python_server.launch) add_rostest(test_python_server2.launch) add_rostest(test_python_server3.launch) add_rostest(test_python_simple_server.launch) add_rostest(test_exercise_simple_clients.launch) add_rostest(test_simple_action_server_deadlock_python.launch) catkin_add_gtest(actionlib-destruction_guard_test destruction_guard_test.cpp) if(TARGET actionlib-destruction_guard_test) target_link_libraries(actionlib-destruction_guard_test ${PROJECT_NAME}) endif() actionlib-1.12.0/test/action_client_destruction_test.cpp000066400000000000000000000046501352264324200235270ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ //! \author Vijay Pradeep #include #include #include #include using namespace actionlib; TEST(ActionClientDestruction, persistent_goal_handles_1) { ActionClient * test_client = new ActionClient("test_action"); ClientGoalHandle gh = test_client->sendGoal(TestGoal()); ros::Duration(.1).sleep(); printf("Destroying ActionClient\n"); delete test_client; printf("Done Destroying ActionClient\n"); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "simple_client_allocator"); return RUN_ALL_TESTS(); } actionlib-1.12.0/test/add_two_ints_client.cpp000066400000000000000000000047101352264324200212430ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #include #include #include int main(int argc, char ** argv) { ros::init(argc, argv, "add_two_ints_client"); if (argc != 3) { ROS_INFO_NAMED("actionlib", "Usage: add_two_ints_client X Y"); return 1; } ros::NodeHandle n; actionlib::ServiceClient client = actionlib::serviceClient(n, "add_two_ints"); client.waitForServer(); actionlib::TwoIntsGoal req; actionlib::TwoIntsResult resp; req.a = atoi(argv[1]); req.b = atoi(argv[2]); if (client.call(req, resp)) { ROS_INFO_NAMED("actionlib", "Sum: %ld", (int64_t)resp.sum); return 1; } return 0; } actionlib-1.12.0/test/add_two_ints_server.cpp000066400000000000000000000046751352264324200213050ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #include #include #include bool add(const actionlib::TwoIntsGoal & req, actionlib::TwoIntsResult & res) { res.sum = req.a + req.b; ROS_INFO_NAMED("actionlib", "request: x=%ld, y=%ld", (int64_t)req.a, (int64_t)req.b); ROS_INFO_NAMED("actionlib", " sending back response: [%ld]", (int64_t)res.sum); return true; } int main(int argc, char ** argv) { ros::init(argc, argv, "add_two_ints_server"); ros::NodeHandle n; actionlib::ServiceServer service = actionlib::advertiseService(n, "add_two_ints", boost::bind(add, _1, _2)); ros::spin(); return 0; } actionlib-1.12.0/test/destruction_guard_test.cpp000066400000000000000000000076371352264324200220260ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ //! \author Vijay Pradeep #include #include #include #include using namespace actionlib; class TestRunner : public testing::Test { public: TestRunner() : done_protecting_(false) { } void protectingThread() { DestructionGuard::ScopedProtector protector_1(guard_); EXPECT_TRUE(protector_1.isProtected()); DestructionGuard::ScopedProtector protector_2(guard_); EXPECT_TRUE(protector_2.isProtected()); // Let the main thread know that we've successfully created to protectors { boost::mutex::scoped_lock lock(mutex_); done_protecting_ = true; cond_.notify_all(); } // Don't destruct the protectors immeadiately. Sleep for a little bit, and then destruct. // This will force the main thread to have to wait in it's destruct() call printf("protecting thread is sleeping\n"); boost::this_thread::sleep(boost::posix_time::microseconds(5000000)); printf("protecting thread is exiting\n"); } protected: DestructionGuard guard_; bool done_protecting_; boost::mutex mutex_; boost::condition cond_; }; TEST_F(TestRunner, threaded_test) { boost::thread spin_thread(boost::bind(&TestRunner::protectingThread, this)); { boost::mutex::scoped_lock lock(mutex_); while (!done_protecting_) { cond_.timed_wait(lock, boost::posix_time::milliseconds(100)); } } printf("About to destruct\n"); guard_.destruct(); printf("Done destructing\n"); // Already 'destructed', so protector should fail DestructionGuard::ScopedProtector protector(guard_); EXPECT_FALSE(protector.isProtected()); spin_thread.join(); } TEST(DestructionGuard, easy_test) { DestructionGuard guard; { DestructionGuard::ScopedProtector protector_1(guard); EXPECT_TRUE(protector_1.isProtected()); DestructionGuard::ScopedProtector protector_2(guard); EXPECT_TRUE(protector_2.isProtected()); } guard.destruct(); { DestructionGuard::ScopedProtector protector_3(guard); EXPECT_FALSE(protector_3.isProtected()); } } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } actionlib-1.12.0/test/exercise_simple_client.cpp000066400000000000000000000126661352264324200217560ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ // Derived from excercise_simple_server.py #include #include #include #include #include #include #define EXPECT_STATE(expected_state) EXPECT_TRUE( \ ac_.getState() == SimpleClientGoalState::expected_state) \ << "Expected [" << #expected_state << "], but goal state is [" << ac_.getState().toString() << \ "]"; using namespace actionlib; using namespace actionlib_msgs; class SimpleClientFixture : public testing::Test { public: SimpleClientFixture() : ac_("test_request_action"), default_wait_(60.0) {} protected: virtual void SetUp() { // Make sure that the server comes up ASSERT_TRUE(ac_.waitForServer(ros::Duration(10.0)) ); } SimpleActionClient ac_; ros::Duration default_wait_; }; TEST_F(SimpleClientFixture, just_succeed) { TestRequestGoal goal; goal.terminate_status = TestRequestGoal::TERMINATE_SUCCESS; goal.the_result = 42; ac_.sendGoal(goal); ac_.waitForResult(default_wait_); EXPECT_STATE(SUCCEEDED); EXPECT_EQ(42, ac_.getResult()->the_result); } TEST_F(SimpleClientFixture, just_abort) { TestRequestGoal goal; goal.terminate_status = TestRequestGoal::TERMINATE_ABORTED; goal.the_result = 42; ac_.sendGoal(goal); ac_.waitForResult(default_wait_); EXPECT_STATE(ABORTED); EXPECT_EQ(42, ac_.getResult()->the_result); } TEST_F(SimpleClientFixture, just_preempt) { TestRequestGoal goal; goal.terminate_status = TestRequestGoal::TERMINATE_SUCCESS; goal.delay_terminate = ros::Duration(1000); goal.the_result = 42; ac_.sendGoal(goal); // Sleep for 10 seconds or until we hear back from the action server for (unsigned int i = 0; i < 100; i++) { ROS_DEBUG_NAMED("actionlib", "Waiting for Server Response"); if (ac_.getState() != SimpleClientGoalState::PENDING) { break; } ros::Duration(0.1).sleep(); } ac_.cancelGoal(); ac_.waitForResult(default_wait_); EXPECT_STATE(PREEMPTED); EXPECT_EQ(42, ac_.getResult()->the_result); } TEST_F(SimpleClientFixture, drop) { TestRequestGoal goal; goal.terminate_status = TestRequestGoal::TERMINATE_DROP; goal.the_result = 42; ac_.sendGoal(goal); ac_.waitForResult(default_wait_); EXPECT_STATE(ABORTED); EXPECT_EQ(0, ac_.getResult()->the_result); } TEST_F(SimpleClientFixture, exception) { TestRequestGoal goal; goal.terminate_status = TestRequestGoal::TERMINATE_EXCEPTION; goal.the_result = 42; ac_.sendGoal(goal); ac_.waitForResult(default_wait_); EXPECT_STATE(ABORTED); EXPECT_EQ(0, ac_.getResult()->the_result); } TEST_F(SimpleClientFixture, ignore_cancel_and_succeed) { TestRequestGoal goal; goal.terminate_status = TestRequestGoal::TERMINATE_SUCCESS; goal.delay_terminate = ros::Duration(2.0); goal.ignore_cancel = true; goal.the_result = 42; ac_.sendGoal(goal); // Sleep for 10 seconds or until we hear back from the action server for (unsigned int i = 0; i < 100; i++) { ROS_DEBUG_NAMED("actionlib", "Waiting for Server Response"); if (ac_.getState() != SimpleClientGoalState::PENDING) { break; } ros::Duration(0.1).sleep(); } ac_.cancelGoal(); ac_.waitForResult(default_wait_ + default_wait_); EXPECT_STATE(SUCCEEDED); EXPECT_EQ(42, ac_.getResult()->the_result); } TEST_F(SimpleClientFixture, lose) { TestRequestGoal goal; goal.terminate_status = TestRequestGoal::TERMINATE_LOSE; goal.the_result = 42; ac_.sendGoal(goal); ac_.waitForResult(default_wait_); EXPECT_STATE(LOST); } void spinThread() { ros::NodeHandle nh; ros::spin(); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "simple_client_test"); boost::thread spin_thread(&spinThread); int result = RUN_ALL_TESTS(); ros::shutdown(); spin_thread.join(); return result; } actionlib-1.12.0/test/exercise_simple_client.py000077500000000000000000000144001352264324200216130ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PKG = 'actionlib' import sys import unittest import rospy import rostest from actionlib import SimpleActionClient from actionlib_msgs.msg import GoalStatus from actionlib.msg import TestRequestAction, TestRequestGoal class SimpleExerciser(unittest.TestCase): def setUp(self): self.default_wait = rospy.Duration(60.0) self.client = SimpleActionClient('test_request_action', TestRequestAction) self.assert_(self.client.wait_for_server(self.default_wait)) def test_just_succeed(self): goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_SUCCESS, the_result=42) self.client.send_goal(goal) self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state()) self.assertEqual(42, self.client.get_result().the_result) def test_just_abort(self): goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_ABORTED, the_result=42) self.client.send_goal(goal) self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.ABORTED, self.client.get_state()) self.assertEqual(42, self.client.get_result().the_result) def test_just_preempt(self): goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_SUCCESS, delay_terminate=rospy.Duration(100000), the_result=42) self.client.send_goal(goal) # Ensure that the action server got the goal, before continuing timeout_time = rospy.Time.now() + self.default_wait while rospy.Time.now() < timeout_time: if (self.client.get_state() != GoalStatus.PENDING): break self.client.cancel_goal() self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state()) self.assertEqual(42, self.client.get_result().the_result) # Should print out errors about not setting a terminal status in the action server. def test_drop(self): goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_DROP, the_result=42) self.client.send_goal(goal) self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.ABORTED, self.client.get_state()) self.assertEqual(0, self.client.get_result().the_result) # Should print out errors about throwing an exception def test_exception(self): goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_EXCEPTION, the_result=42) self.client.send_goal(goal) self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.ABORTED, self.client.get_state()) self.assertEqual(0, self.client.get_result().the_result) def test_ignore_cancel_and_succeed(self): goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_SUCCESS, delay_terminate=rospy.Duration(2.0), ignore_cancel=True, the_result=42) self.client.send_goal(goal) # Ensure that the action server got the goal, before continuing timeout_time = rospy.Time.now() + self.default_wait while rospy.Time.now() < timeout_time: if (self.client.get_state() != GoalStatus.PENDING): break self.client.cancel_goal() self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state()) self.assertEqual(42, self.client.get_result().the_result) def test_lose(self): goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_LOSE, the_result=42) self.client.send_goal(goal) self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.LOST, self.client.get_state()) # test_freeze_server has been removed, as it is undecided what should happen # when the action server disappears. # # # def test_freeze_server(self): # # goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS, # # the_result = 42, # # pause_status = rospy.Duration(10.0)) # # self.client.send_goal(goal) # # self.client.wait_for_result(rospy.Duration(13.0)) # # # # self.assertEqual(GoalStatus.LOST, self.client.get_state()) # if __name__ == '__main__': rospy.init_node("exercise_simple_server") rostest.run(PKG, 'exercise_simple_server', SimpleExerciser, sys.argv) actionlib-1.12.0/test/mock_simple_server.py000077500000000000000000000113511352264324200207670ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. from __future__ import print_function import time import rospy from actionlib.simple_action_server import SimpleActionServer from actionlib.server_goal_handle import ServerGoalHandle from actionlib.msg import TestRequestAction, TestRequestGoal, TestRequestResult class RefSimpleServer(SimpleActionServer): def __init__(self, name): SimpleActionServer.__init__(self, name, TestRequestAction, self.execute_cb, False) self.start() def execute_cb(self, goal): rospy.logdebug("Goal:\n" + str(goal)) result = TestRequestResult(goal.the_result, True) if goal.pause_status > rospy.Duration(0.0): rospy.loginfo("Locking the action server for %.3f seconds" % goal.pause_status.to_sec()) status_continue_time = rospy.get_rostime() + goal.pause_status # Takes the action server lock to prevent status from # being published (simulates a freeze). with self.action_server.lock: while rospy.get_rostime() < status_continue_time: time.sleep(0.02) rospy.loginfo("Unlocking the action server") terminate_time = rospy.get_rostime() + goal.delay_terminate while rospy.get_rostime() < terminate_time: time.sleep(0.02) if not goal.ignore_cancel: if self.is_preempt_requested(): self.set_preempted(result, goal.result_text) return rospy.logdebug("Terminating goal as: %i" % goal.terminate_status) if goal.terminate_status == TestRequestGoal.TERMINATE_SUCCESS: self.set_succeeded(result, goal.result_text) elif goal.terminate_status == TestRequestGoal.TERMINATE_ABORTED: self.set_aborted(result, goal.result_text) elif goal.terminate_status == TestRequestGoal.TERMINATE_REJECTED: rospy.logerr("Simple action server cannot reject goals") self.set_aborted(None, "Simple action server cannot reject goals") elif goal.terminate_status == TestRequestGoal.TERMINATE_DROP: rospy.loginfo("About to drop the goal. This should produce an error message.") return elif goal.terminate_status == TestRequestGoal.TERMINATE_EXCEPTION: rospy.loginfo("About to throw an exception. This should produce an error message.") raise Exception("Terminating by throwing an exception") elif goal.terminate_status == TestRequestGoal.TERMINATE_LOSE: # Losing the goal requires messing about in the action server's innards for i, s in enumerate(self.action_server.status_list): if s.status.goal_id.id == self.current_goal.goal.goal_id.id: del self.action_server.status_list[i] break self.current_goal = ServerGoalHandle() else: rospy.logerr("Don't know how to terminate as %d" % goal.terminate_status) self.set_aborted(None, "Don't know how to terminate as %d" % goal.terminate_status) if __name__ == '__main__': rospy.init_node("ref_simple_server") ref_server = RefSimpleServer("test_request_action") print("Spinning") rospy.spin() actionlib-1.12.0/test/ref_server.cpp000066400000000000000000000062501352264324200173720ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ //! \author Vijay Pradeep #include #include #include #include #include namespace actionlib { class RefServer : public ActionServer { public: typedef ServerGoalHandle GoalHandle; RefServer(ros::NodeHandle & n, const std::string & name); private: void goalCallback(GoalHandle gh); void cancelCallback(GoalHandle gh); }; } // namespace actionlib using namespace actionlib; RefServer::RefServer(ros::NodeHandle & n, const std::string & name) : ActionServer(n, name, boost::bind(&RefServer::goalCallback, this, _1), boost::bind(&RefServer::cancelCallback, this, _1), false) { start(); printf("Creating ActionServer [%s]\n", name.c_str()); } void RefServer::goalCallback(GoalHandle gh) { TestGoal goal = *gh.getGoal(); switch (goal.goal) { case 1: gh.setAccepted(); gh.setSucceeded(TestResult(), "The ref server has succeeded"); break; case 2: gh.setAccepted(); gh.setAborted(TestResult(), "The ref server has aborted"); break; case 3: gh.setRejected(TestResult(), "The ref server has rejected"); break; default: break; } } void RefServer::cancelCallback(GoalHandle) { } int main(int argc, char ** argv) { ros::init(argc, argv, "ref_server"); ros::NodeHandle nh; RefServer ref_server(nh, "reference_action"); ros::spin(); } actionlib-1.12.0/test/ref_server.py000077500000000000000000000076131352264324200172470ustar00rootroot00000000000000#!/usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Alexander Sorokin. # Based on code from ref_server.cpp by Vijay Pradeep PKG = 'actionlib' import rospy from actionlib.action_server import ActionServer from actionlib.msg import TestAction, TestFeedback, TestResult class RefServer (ActionServer): def __init__(self, name): action_spec = TestAction ActionServer.__init__( self, name, action_spec, self.goalCallback, self.cancelCallback, False) self.start() rospy.loginfo("Creating ActionServer [%s]\n", name) self.saved_goals = [] def goalCallback(self, gh): goal = gh.get_goal() rospy.loginfo("Got goal %d", int(goal.goal)) if goal.goal == 1: gh.set_accepted() gh.set_succeeded(None, "The ref server has succeeded") elif goal.goal == 2: gh.set_accepted() gh.set_aborted(None, "The ref server has aborted") elif goal.goal == 3: gh.set_rejected(None, "The ref server has rejected") elif goal.goal == 4: self.saved_goals.append(gh) gh.set_accepted() elif goal.goal == 5: gh.set_accepted() for g in self.saved_goals: g.set_succeeded() self.saved_goals = [] gh.set_succeeded() elif goal.goal == 6: gh.set_accepted() for g in self.saved_goals: g.set_aborted() self.saved_goals = [] gh.set_succeeded() elif goal.goal == 7: gh.set_accepted() n = len(self.saved_goals) for i, g in enumerate(self.saved_goals): g.publish_feedback(TestFeedback(n-i)) gh.set_succeeded() elif goal.goal == 8: gh.set_accepted() n = len(self.saved_goals) for i, g in enumerate(self.saved_goals): if i % 2 == 0: g.set_succeeded(TestResult(n - i), "The ref server has succeeded") else: g.set_aborted(TestResult(n - i), "The ref server has aborted") self.saved_goals = [] gh.set_succeeded() else: pass def cancelCallback(self, gh): pass if __name__ == "__main__": rospy.init_node("ref_server") ref_server = RefServer("reference_action") rospy.spin() actionlib-1.12.0/test/ref_server_test.launch000066400000000000000000000003101352264324200211100ustar00rootroot00000000000000 actionlib-1.12.0/test/ref_simple_server.py000077500000000000000000000065741352264324200206250ustar00rootroot00000000000000#!/usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Alexander Sorokin. # Based on code from ref_server.cpp by Vijay Pradeep PKG = 'actionlib' import rospy from actionlib.simple_action_server import SimpleActionServer from actionlib.msg import TestAction, TestFeedback class RefSimpleServer (SimpleActionServer): def __init__(self, name): action_spec = TestAction SimpleActionServer.__init__( self, name, action_spec, self.goal_callback, False) self.start() rospy.loginfo("Creating SimpleActionServer [%s]\n", name) def goal_callback(self, goal): rospy.loginfo("Got goal %d", int(goal.goal)) if goal.goal == 1: self.set_succeeded(None, "The ref server has succeeded") elif goal.goal == 2: self.set_aborted(None, "The ref server has aborted") elif goal.goal == 3: self.set_aborted(None, "The simple action server can't reject goals") elif goal.goal == 4: self.set_aborted(None, "Simple server can't save goals") elif goal.goal == 5: self.set_aborted(None, "Simple server can't save goals") elif goal.goal == 6: self.set_aborted(None, "Simple server can't save goals") elif goal.goal == 7: self.set_aborted(None, "Simple server can't save goals") elif goal.goal == 8: self.set_aborted(None, "Simple server can't save goals") elif goal.goal == 9: rospy.sleep(1) rospy.loginfo("Sending feedback") self.publish_feedback(TestFeedback(9)) # by the goal ID rospy.sleep(1) self.set_succeeded(None, "The ref server has succeeded") else: pass if __name__ == "__main__": rospy.init_node("ref_simple_server") ref_server = RefSimpleServer("reference_simple_action") rospy.spin() actionlib-1.12.0/test/server_goal_handle_destruction.cpp000066400000000000000000000077141352264324200235040ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ //! \author Eitan Marder-Eppstein #include #include #include #include #include namespace actionlib { class ServerGoalHandleDestructionTester { public: typedef ServerGoalHandle GoalHandle; ServerGoalHandleDestructionTester(); ros::NodeHandle nh_; ActionServer * as_; GoalHandle * gh_; ~ServerGoalHandleDestructionTester(); void goalCallback(GoalHandle gh); }; } // namespace actionlib using namespace actionlib; ServerGoalHandleDestructionTester::ServerGoalHandleDestructionTester() { as_ = new ActionServer(nh_, "reference_action", false); as_->start(); as_->registerGoalCallback(boost::bind(&ServerGoalHandleDestructionTester::goalCallback, this, _1)); gh_ = new GoalHandle(); } ServerGoalHandleDestructionTester::~ServerGoalHandleDestructionTester() { delete as_; gh_->setAccepted(); delete gh_; } void ServerGoalHandleDestructionTester::goalCallback(GoalHandle gh) { ROS_ERROR_NAMED("actionlib", "In callback"); // assign to our stored goal handle *gh_ = gh; TestGoal goal = *gh.getGoal(); switch (goal.goal) { case 1: gh.setAccepted(); gh.setSucceeded(TestResult(), "The ref server has succeeded"); break; case 2: gh.setAccepted(); gh.setAborted(TestResult(), "The ref server has aborted"); break; case 3: gh.setRejected(TestResult(), "The ref server has rejected"); break; default: break; } ros::shutdown(); } void spinner() { ros::spin(); } TEST(ServerGoalHandleDestruction, destruction_test) { boost::thread spin_thread(&spinner); ServerGoalHandleDestructionTester server; SimpleActionClient client("reference_action", true); ROS_ERROR_NAMED("actionlib", "Waiting for server"); client.waitForServer(); ROS_ERROR_NAMED("actionlib", "Done waiting for server"); TestGoal goal; goal.goal = 1; client.sendGoal(goal); ROS_ERROR_NAMED("actionlib", "Sending goal"); spin_thread.join(); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "ref_server"); return RUN_ALL_TESTS(); } actionlib-1.12.0/test/simple_action_server_deadlock_companion.py000077500000000000000000000056341352264324200252130ustar00rootroot00000000000000#! /usr/bin/env python # # Copyright (c) 2013, Miguel Sarabia # Imperial College London # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of Imperial College London nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # class Constants: node = "simple_action_server_deadlock_companion" topic = "deadlock" max_action_duration = 3 import random import actionlib from actionlib.msg import TestAction, TestGoal from actionlib_msgs.msg import GoalStatus import rospy class DeadlockCompanion: def __init__(self): # Seed random with fully resolved name of node and current time random.seed(rospy.get_name() + str(rospy.Time.now().to_sec())) # Create actionlib client self.action_client = actionlib.SimpleActionClient( Constants.topic, TestAction) def run(self): while not rospy.is_shutdown(): # Send dummy goal self.action_client.send_goal(TestGoal()) # Wait for a random amount of time action_duration = random.uniform(0, Constants.max_action_duration) self.action_client.wait_for_result(rospy.Duration(action_duration)) state = self.action_client.get_state() if state == GoalStatus.ACTIVE or state == GoalStatus.PENDING: self.action_client.cancel_goal() if __name__ == '__main__': rospy.init_node(Constants.node) try: companion = DeadlockCompanion() companion.run() except (KeyboardInterrupt, SystemExit): raise except: pass actionlib-1.12.0/test/simple_client_allocator_test.cpp000066400000000000000000000044521352264324200231600ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ //! \author Vijay Pradeep #include #include #include #include using namespace actionlib; TEST(SimpleClientAllocator, easy_test) { typedef actionlib::SimpleActionClient TrajClient; TrajClient * traj_client_ = new TrajClient("test_action", true); ros::Duration(1, 0).sleep(); delete traj_client_; } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "simple_client_allocator"); return RUN_ALL_TESTS(); } actionlib-1.12.0/test/simple_client_test.cpp000066400000000000000000000112201352264324200211070ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ //! \author Vijay Pradeep #include #include #include using namespace actionlib; TEST(SimpleClient, easy_tests) { ros::NodeHandle n; SimpleActionClient client(n, "reference_action"); bool started = client.waitForServer(ros::Duration(10.0)); ASSERT_TRUE(started); TestGoal goal; bool finished; goal.goal = 1; // sleep a bit to make sure that all topics are properly connected to the server. ros::Duration(0.01).sleep(); client.sendGoal(goal); finished = client.waitForResult(ros::Duration(10.0)); ASSERT_TRUE(finished); EXPECT_TRUE( client.getState() == SimpleClientGoalState::SUCCEEDED) << "Expected [SUCCEEDED], but goal state is [" << client.getState().toString() << "]"; // test that setting the text field for the status works EXPECT_TRUE(client.getState().getText() == "The ref server has succeeded"); goal.goal = 2; client.sendGoal(goal); finished = client.waitForResult(ros::Duration(10.0)); ASSERT_TRUE(finished); EXPECT_TRUE( client.getState() == SimpleClientGoalState::ABORTED) << "Expected [ABORTED], but goal state is [" << client.getState().toString() << "]"; // test that setting the text field for the status works EXPECT_TRUE(client.getState().getText() == "The ref server has aborted"); client.cancelAllGoals(); // Don't need this line, but keep it as a compilation check client.cancelGoalsAtAndBeforeTime(ros::Time(1.0)); } void easyDoneCallback(bool * called, const SimpleClientGoalState & state, const TestResultConstPtr &) { *called = true; EXPECT_TRUE(state == SimpleClientGoalState::SUCCEEDED) << "Expected [SUCCEEDED], but goal state is [" << state.toString() << "]"; } void easyOldDoneCallback(bool * called, const TerminalState & terminal_state, const TestResultConstPtr &) { *called = true; EXPECT_TRUE(terminal_state == TerminalState::SUCCEEDED) << "Expected [SUCCEEDED], but terminal state is [" << terminal_state.toString() << "]"; } /* Intermittent failures #5087 TEST(SimpleClient, easy_callback) { ros::NodeHandle n; SimpleActionClient client(n, "reference_action"); bool started = client.waitForServer(ros::Duration(10.0)); ASSERT_TRUE(started); TestGoal goal; bool finished; bool called = false; goal.goal = 1; SimpleActionClient::SimpleDoneCallback func = boost::bind(&easyDoneCallback, &called, _1, _2); client.sendGoal(goal, func); finished = client.waitForResult(ros::Duration(10.0)); ASSERT_TRUE(finished); EXPECT_TRUE(called) << "easyDoneCallback() was never called" ; } */ void spinThread() { ros::NodeHandle nh; ros::spin(); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "simple_client_test"); boost::thread spin_thread(&spinThread); int result = RUN_ALL_TESTS(); ros::shutdown(); spin_thread.join(); return result; } actionlib-1.12.0/test/simple_client_wait_test.cpp000066400000000000000000000057451352264324200221520ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ //! \author Vijay Pradeep #include #include #include using namespace actionlib; TEST(SimpleClient, easy_wait_test) { ros::NodeHandle n; SimpleActionClient client(n, "reference_action"); bool started = client.waitForServer(ros::Duration(10.0)); ASSERT_TRUE(started); TestGoal goal; SimpleClientGoalState state(SimpleClientGoalState::LOST); goal.goal = 1; state = client.sendGoalAndWait(goal, ros::Duration(10, 0), ros::Duration(10, 0)); EXPECT_TRUE(state == SimpleClientGoalState::SUCCEEDED) << "Expected [SUCCEEDED], but goal state is [" << client.getState().toString() << "]"; goal.goal = 4; state = client.sendGoalAndWait(goal, ros::Duration(2, 0), ros::Duration(1, 0)); EXPECT_TRUE(state == SimpleClientGoalState::PREEMPTED) << "Expected [PREEMPTED], but goal state is [" << client.getState().toString() << "]"; } void spinThread() { ros::NodeHandle nh; ros::spin(); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "simple_client_test"); boost::thread spin_thread(&spinThread); int result = RUN_ALL_TESTS(); ros::shutdown(); spin_thread.join(); return result; } actionlib-1.12.0/test/simple_execute_ref_server.cpp000066400000000000000000000065031352264324200224660ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ //! \author Vijay Pradeep #include #include #include namespace actionlib { class SimpleExecuteRefServer { public: typedef ServerGoalHandle GoalHandle; SimpleExecuteRefServer(); private: ros::NodeHandle nh_; SimpleActionServer as_; void executeCallback(const TestGoalConstPtr & goal); }; } // namespace actionlib using namespace actionlib; SimpleExecuteRefServer::SimpleExecuteRefServer() : as_(nh_, "reference_action", boost::bind(&SimpleExecuteRefServer::executeCallback, this, _1), false) { as_.start(); } void SimpleExecuteRefServer::executeCallback(const TestGoalConstPtr & goal) { ROS_DEBUG_NAMED("actionlib", "Got a goal of type [%u]", goal->goal); switch (goal->goal) { case 1: ROS_DEBUG_NAMED("actionlib", "Got goal #1"); as_.setSucceeded(TestResult(), "The ref server has succeeded"); break; case 2: ROS_DEBUG_NAMED("actionlib", "Got goal #2"); as_.setAborted(TestResult(), "The ref server has aborted"); break; case 4: { ROS_DEBUG_NAMED("actionlib", "Got goal #4"); ros::Duration sleep_dur(.1); for (unsigned int i = 0; i < 100; i++) { sleep_dur.sleep(); if (as_.isPreemptRequested()) { as_.setPreempted(); return; } } as_.setAborted(); break; } default: break; } } int main(int argc, char ** argv) { ros::init(argc, argv, "ref_server"); SimpleExecuteRefServer server; ros::spin(); return 0; } actionlib-1.12.0/test/simple_execute_ref_server_test.launch000066400000000000000000000005021352264324200242060ustar00rootroot00000000000000 actionlib-1.12.0/test/simple_python_client_test.py000077500000000000000000000054661352264324200224000ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import unittest import rospy from actionlib_msgs.msg import GoalStatus from actionlib import SimpleActionClient from actionlib.msg import TestAction, TestGoal class TestSimpleActionClient(unittest.TestCase): def testsimple(self): client = SimpleActionClient('reference_action', TestAction) self.assert_(client.wait_for_server(rospy.Duration(10.0)), 'Could not connect to the action server') goal = TestGoal(1) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) self.assertEqual("The ref server has succeeded", client.get_goal_status_text()) goal = TestGoal(2) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.ABORTED, client.get_state()) self.assertEqual("The ref server has aborted", client.get_goal_status_text()) if __name__ == '__main__': import rostest rospy.init_node('simple_python_client_test') rostest.rosrun('actionlib', 'test_simple_action_client_python', TestSimpleActionClient) actionlib-1.12.0/test/test_cpp_action_client_destruction.launch000066400000000000000000000002141352264324200250510ustar00rootroot00000000000000 actionlib-1.12.0/test/test_cpp_simple_client_allocator.launch000066400000000000000000000002101352264324200244760ustar00rootroot00000000000000 actionlib-1.12.0/test/test_cpp_simple_client_cancel_crash.cpp000066400000000000000000000047231352264324200244500ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ //! \author Vijay Pradeep #include #include #include #include typedef actionlib::SimpleActionClient Client; TEST(SimpleClientCancelCrash, uninitialized_crash) { ros::NodeHandle nh; Client client("test_client", true); ROS_INFO_NAMED("actionlib", "calling cancelGoal()"); client.cancelGoal(); ROS_INFO_NAMED("actionlib", "Done calling cancelGoal()"); EXPECT_TRUE(true); ROS_INFO_NAMED("actionlib", "Successfully done with test. Exiting"); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_cpp_simple_client_cancel_crash"); return RUN_ALL_TESTS(); } actionlib-1.12.0/test/test_cpp_simple_client_cancel_crash.launch000066400000000000000000000002221352264324200251260ustar00rootroot00000000000000 actionlib-1.12.0/test/test_exercise_simple_clients.launch000066400000000000000000000005251352264324200236570ustar00rootroot00000000000000 actionlib-1.12.0/test/test_imports.launch000066400000000000000000000001361352264324200204510ustar00rootroot00000000000000 actionlib-1.12.0/test/test_imports.py000077500000000000000000000046061352264324200176400ustar00rootroot00000000000000#!/usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Alexander Sorokin. from __future__ import print_function PKG = 'actionlib' import sys import unittest # A simple unit test to make sure python module structure and files aren't broken class TestImports(unittest.TestCase): # import everything def test_imports(self): from actionlib import simple_action_client from actionlib import action_client from actionlib import goal_id_generator from actionlib import handle_tracker_deleter from actionlib import server_goal_handle from actionlib import status_tracker from actionlib import action_server from actionlib import simple_action_server self.assertEquals(1, 1, "1!=1") if __name__ == '__main__': import rostest print(sys.path) rostest.rosrun(PKG, 'test_imports', TestImports) actionlib-1.12.0/test/test_python_server.launch000066400000000000000000000003031352264324200216570ustar00rootroot00000000000000 actionlib-1.12.0/test/test_python_server2.launch000066400000000000000000000003031352264324200217410ustar00rootroot00000000000000 actionlib-1.12.0/test/test_python_server3.launch000066400000000000000000000003011352264324200217400ustar00rootroot00000000000000 actionlib-1.12.0/test/test_python_server_components.launch000066400000000000000000000001621352264324200241270ustar00rootroot00000000000000 actionlib-1.12.0/test/test_python_simple_client.launch000066400000000000000000000003411352264324200232020ustar00rootroot00000000000000 actionlib-1.12.0/test/test_python_simple_server.launch000066400000000000000000000003341352264324200232340ustar00rootroot00000000000000 actionlib-1.12.0/test/test_ref_action_server.py000077500000000000000000000174131352264324200216420ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PKG = 'actionlib' import unittest import rospy from actionlib_msgs.msg import GoalStatus # TODO(mikaelarguedas) use SimpleActionClient here if it makes sense # from actionlib import SimpleActionClient from actionlib import ActionClient from actionlib.msg import TestAction, TestGoal class TestRefSimpleActionServer(unittest.TestCase): def testsimple(self): return client = ActionClient('reference_action', TestAction) self.assert_(client.wait_for_action_server_to_start(rospy.Duration(2.0)), 'Could not connect to the action server') goal = TestGoal(1) client.send_goal(goal) self.assert_(client.wait_for_goal_to_finish(rospy.Duration(2.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_terminal_state()) self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) goal = TestGoal(2) client.send_goal(goal) self.assert_(client.wait_for_goal_to_finish(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.ABORTED, client.get_terminal_state()) self.assertEqual(GoalStatus.ABORTED, client.get_state()) def test_abort(self): client = ActionClient('reference_action', TestAction) self.assert_(client.wait_for_server(rospy.Duration(2.0)), 'Could not connect to the action server') goal_work = TestGoal(4) goal_abort = TestGoal(6) goal_feedback = TestGoal(8) g1 = client.send_goal(goal_work) g2 = client.send_goal(goal_work) g3 = client.send_goal(goal_work) g4 = client.send_goal(goal_work) rospy.sleep(0.5) self.assertEqual(g1.get_goal_status(), GoalStatus.ACTIVE) # ,"Should be active") self.assertEqual(g2.get_goal_status(), GoalStatus.ACTIVE, "Should be active") self.assertEqual(g3.get_goal_status(), GoalStatus.ACTIVE, "Shoule be active") self.assertEqual(g4.get_goal_status(), GoalStatus.ACTIVE, "Should be active") g5 = client.send_goal(goal_abort) rospy.sleep(0.5) self.assertEqual(g5.get_goal_status(), GoalStatus.SUCCEEDED, "Should be done") self.assertEqual(g1.get_goal_status(), GoalStatus.ABORTED, "Should be aborted") self.assertEqual(g2.get_goal_status(), GoalStatus.ABORTED, "Should be aborted") self.assertEqual(g3.get_goal_status(), GoalStatus.ABORTED, "Shoule be aborted") self.assertEqual(g4.get_goal_status(), GoalStatus.ABORTED, "Should be aborted") def test_feedback(self): client = ActionClient('reference_action', TestAction) self.assert_(client.wait_for_server(rospy.Duration(2.0)), 'Could not connect to the action server') goal_work = TestGoal(4) goal_abort = TestGoal(6) goal_feedback = TestGoal(7) rospy.logwarn("This is a hacky way to associate goals with feedback") feedback = {} def update_feedback(id, g, f): feedback[id] = f g1 = client.send_goal(goal_work, feedback_cb=lambda g, f: update_feedback(0, g, f)) g2 = client.send_goal(goal_work, feedback_cb=lambda g, f: update_feedback(1, g, f)) g3 = client.send_goal(goal_work, feedback_cb=lambda g, f: update_feedback(2, g, f)) g4 = client.send_goal(goal_work, feedback_cb=lambda g, f: update_feedback(3, g, f)) rospy.sleep(0.5) self.assertEqual(g1.get_goal_status(), GoalStatus.ACTIVE, "Should be active") self.assertEqual(g2.get_goal_status(), GoalStatus.ACTIVE, "Should be active") self.assertEqual(g3.get_goal_status(), GoalStatus.ACTIVE, "Shoule be active") self.assertEqual(g4.get_goal_status(), GoalStatus.ACTIVE, "Should be active") g5 = client.send_goal(goal_feedback) rospy.sleep(0.5) self.assertEqual(g5.get_goal_status(), GoalStatus.SUCCEEDED, "Should be done") self.assertEqual(g1.get_goal_status(), GoalStatus.ACTIVE) self.assertEqual(feedback[0].feedback, 4) self.assertEqual(g2.get_goal_status(), GoalStatus.ACTIVE) self.assertEqual(feedback[1].feedback, 3) self.assertEqual(g3.get_goal_status(), GoalStatus.ACTIVE) self.assertEqual(feedback[2].feedback, 2) self.assertEqual(g4.get_goal_status(), GoalStatus.ACTIVE) self.assertEqual(feedback[3].feedback, 1) g6 = client.send_goal(goal_abort) rospy.sleep(0.5) def test_result(self): client = ActionClient('reference_action', TestAction) self.assert_(client.wait_for_server(rospy.Duration(2.0)), 'Could not connect to the action server') goal_work = TestGoal(4) goal_abort = TestGoal(6) goal_result = TestGoal(8) rospy.logwarn("This is a hacky way to associate goals with feedback") g1 = client.send_goal(goal_work) g2 = client.send_goal(goal_work) g3 = client.send_goal(goal_work) g4 = client.send_goal(goal_work) rospy.sleep(0.5) self.assertEqual(g1.get_goal_status(), GoalStatus.ACTIVE, "Should be active") self.assertEqual(g2.get_goal_status(), GoalStatus.ACTIVE, "Should be active") self.assertEqual(g3.get_goal_status(), GoalStatus.ACTIVE, "Shoule be active") self.assertEqual(g4.get_goal_status(), GoalStatus.ACTIVE, "Should be active") g5 = client.send_goal(goal_result) rospy.sleep(0.5) self.assertEqual(g5.get_goal_status(), GoalStatus.SUCCEEDED, "Should be done") self.assertEqual(g1.get_goal_status(), GoalStatus.SUCCEEDED) self.assertEqual(g1.get_result().result, 4) self.assertEqual(g2.get_goal_status(), GoalStatus.ABORTED) self.assertEqual(g2.get_result().result, 3) self.assertEqual(g3.get_goal_status(), GoalStatus.SUCCEEDED) self.assertEqual(g3.get_result().result, 2) self.assertEqual(g4.get_goal_status(), GoalStatus.ABORTED) self.assertEqual(g4.get_result().result, 1) g6 = client.send_goal(goal_abort) rospy.sleep(0.5) if __name__ == '__main__': import rostest rospy.init_node('test_ref_simple_action_server') rostest.rosrun('actionlib', 'test_simple_action_client_python', TestRefSimpleActionServer) actionlib-1.12.0/test/test_ref_simple_action_server.py000077500000000000000000000066521352264324200232160ustar00rootroot00000000000000#! /usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. PKG = 'actionlib' import unittest import rospy from actionlib_msgs.msg import GoalStatus from actionlib import SimpleActionClient from actionlib import ActionClient from actionlib.msg import TestAction, TestGoal class TestRefSimpleActionServer(unittest.TestCase): def test_one(self): client = SimpleActionClient('reference_simple_action', TestAction) self.assert_(client.wait_for_server(rospy.Duration(2.0)), 'Could not connect to the action server') goal = TestGoal(1) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(2.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) goal = TestGoal(2) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.ABORTED, client.get_state()) goal = TestGoal(3) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") # The simple server can't reject goals self.assertEqual(GoalStatus.ABORTED, client.get_state()) goal = TestGoal(9) saved_feedback = {} def on_feedback(fb): rospy.loginfo("Got feedback") saved_feedback[0] = fb client.send_goal(goal, feedback_cb=on_feedback) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) self.assertEqual(saved_feedback[0].feedback, 9) if __name__ == '__main__': import rostest rospy.init_node('test_ref_simple_action_server') rostest.rosrun('actionlib', 'test_simple_action_client_python', TestRefSimpleActionServer) actionlib-1.12.0/test/test_server_components.py000077500000000000000000000101331352264324200217060ustar00rootroot00000000000000#!/usr/bin/env python # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Author: Alexander Sorokin. PKG = 'actionlib' import rospy import unittest import threading from actionlib import goal_id_generator, status_tracker import actionlib_msgs.msg # A sample python unit test class TestGoalIDGenerator(unittest.TestCase): def test_generator(self): gen1 = goal_id_generator.GoalIDGenerator() id1 = gen1.generate_ID() id2 = gen1.generate_ID() id3 = gen1.generate_ID() nn1, s1, ts1 = id1.id.split('-') nn2, s2, ts2 = id2.id.split('-') nn3, s3, ts3 = id3.id.split('-') self.assertEquals(nn1, "/test_goal_id_generator", "node names are different") self.assertEquals(nn1, nn2, "node names are different") self.assertEquals(nn1, nn3, "node names are different") self.assertEquals(s1, "1", "Sequence numbers mismatch") self.assertEquals(s2, "2", "Sequence numbers mismatch") self.assertEquals(s3, "3", "Sequence numbers mismatch") def test_threaded_generation(self): """ This test checks that all the ids are generated unique. This test should fail if the synchronization is set incorrectly. @note this test is can succeed when the errors are present. """ global ids_lists ids_lists = {} def gen_ids(tID=1, num_ids=1000): gen = goal_id_generator.GoalIDGenerator() for i in range(0, num_ids): id = gen.generate_ID() ids_lists[tID].append(id) num_ids = 1000 num_threads = 200 threads = [] for tID in range(0, num_threads): ids_lists[tID] = [] t = threading.Thread(None, gen_ids, None, (), {'tID': tID, 'num_ids': num_ids}) threads.append(t) t.start() for t in threads: t.join() all_ids = {} for tID in range(0, num_threads): self.assertEquals(len(ids_lists[tID]), num_ids) for id in ids_lists[tID]: nn, s, ts = id.id.split('-') self.assertFalse(s in all_ids, "Duplicate ID found") all_ids[s] = 1 def test_status_tracker(self): tracker = status_tracker.StatusTracker("test-id", actionlib_msgs.msg.GoalStatus.PENDING) self.assertEquals(tracker.status.goal_id, "test-id") self.assertEquals(tracker.status.status, actionlib_msgs.msg.GoalStatus.PENDING) if __name__ == '__main__': import rostest rospy.init_node("test_goal_id_generator") rostest.rosrun(PKG, 'test_goal_id_generator', TestGoalIDGenerator) actionlib-1.12.0/test/test_server_goal_handle_destruction.launch000066400000000000000000000002161352264324200252210ustar00rootroot00000000000000 actionlib-1.12.0/test/test_simple_action_server_deadlock.py000077500000000000000000000106461352264324200242060ustar00rootroot00000000000000#! /usr/bin/env python # # Copyright (c) 2013, Miguel Sarabia # Imperial College London # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of Imperial College London nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import random import threading import unittest import actionlib from actionlib.msg import TestAction import rosnode import rospy class Constants: pkg = "actionlib" node = "test_simple_action_server_deadlock" topic = "deadlock" deadlock_timeout = 45 # in seconds shutdown_timeout = 2 # in seconds max_action_duration = 3 class DeadlockTest(unittest.TestCase): def test_deadlock(self): # Prepare condition (for safe preemption) self.condition = threading.Condition() self.last_execution_time = None # Prepare Simple Action Server self.action_server = actionlib.SimpleActionServer( Constants.topic, TestAction, execute_cb=self.execute_callback, auto_start=False) self.action_server.register_preempt_callback(self.preempt_callback) self.action_server.start() # Sleep for the amount specified rospy.sleep(Constants.deadlock_timeout) # Start actual tests running_nodes = set(rosnode.get_node_names()) required_nodes = { "/deadlock_companion_1", "/deadlock_companion_2", "/deadlock_companion_3", "/deadlock_companion_4", "/deadlock_companion_5"} self.assertTrue( required_nodes.issubset(running_nodes), "Required companion nodes are not currently running") # Shutdown companions so that we can exit nicely termination_time = rospy.Time.now() rosnode.kill_nodes(required_nodes) rospy.sleep(Constants.shutdown_timeout) # Check last execution wasn't too long ago... self.assertIsNotNone( self.last_execution_time is None, "Execute Callback was never executed") time_since_last_execution = ( termination_time - self.last_execution_time).to_sec() self.assertTrue( time_since_last_execution < 2 * Constants.max_action_duration, "Too long since last goal was executed; likely due to a deadlock") def execute_callback(self, goal): # Note down last_execution time self.last_execution_time = rospy.Time.now() # Determine duration of this action action_duration = random.uniform(0, Constants.max_action_duration) with self.condition: if not self.action_server.is_preempt_requested(): self.condition.wait(action_duration) if self.action_server.is_preempt_requested(): self.action_server.set_preempted() else: self.action_server.set_succeeded() def preempt_callback(self): with self.condition: self.condition.notify() if __name__ == '__main__': import rostest rospy.init_node(Constants.node) rostest.rosrun(Constants.pkg, Constants.node, DeadlockTest) actionlib-1.12.0/test/test_simple_action_server_deadlock_python.launch000066400000000000000000000014621352264324200264220ustar00rootroot00000000000000 actionlib-1.12.0/tools/000077500000000000000000000000001352264324200147025ustar00rootroot00000000000000actionlib-1.12.0/tools/axclient.py000077500000000000000000000236241352264324200170750ustar00rootroot00000000000000#! /usr/bin/python # ********************************************************** # Software License Agreement (BSD License) # # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: Eitan Marder-Eppstein # ********************************************************** """ usage: %prog /action_name action_type """ PKG = 'actionlib' import roslib.message from optparse import OptionParser import wx import rospy import actionlib import threading import rostopic from cStringIO import StringIO from library import to_yaml, yaml_msg_str from dynamic_action import DynamicAction from actionlib_msgs.msg import GoalStatus class AXClientApp(wx.App): def __init__(self, action_type, action_name): self.action_type = action_type self.action_name = action_name wx.App.__init__(self) self.client = actionlib.SimpleActionClient( self.action_name, self.action_type.action) self.condition = threading.Condition() self.goal_msg = None self.execute_type = None def set_status(self, label, color): self.status_bg.SetBackgroundColour(color) self.status.SetLabel(label) def set_cancel_button(self, enabled): if enabled: self.cancel_goal.Enable() else: self.cancel_goal.Disable() def set_server_status(self, label, color, enabled): self.server_status_bg.SetBackgroundColour(color) self.server_status.SetLabel(label) if enabled: self.send_goal.Enable() else: self.send_goal.Disable() def server_check(self, event): TIMEOUT = 0.01 if self.client.wait_for_server(rospy.Duration.from_sec(TIMEOUT)): wx.CallAfter(self.set_server_status, "Connected to server", wx.Colour(192, 252, 253), True) else: wx.CallAfter(self.set_server_status, "Disconnected from server", wx.Colour(200, 0, 0), False) def on_cancel(self, event): # we'll cancel the current goal self.client.cancel_goal() self.set_status("Canceling goal", wx.Colour(211, 34, 243)) def on_goal(self, event): try: self.goal_msg = yaml_msg_str(self.action_type.goal, self.goal.GetValue()) buff = StringIO() self.goal_msg.serialize(buff) # send the goal to the action server and register the relevant # callbacks self.client.send_goal(self.goal_msg, self.done_cb, self.active_cb, self.feedback_cb) self.set_status("Goal is pending", wx.Colour(255, 174, 59)) self.set_cancel_button(True) except roslib.message.SerializationError as e: self.goal_msg = None wx.MessageBox(str(e), "Error serializing goal", wx.OK) def set_result(self, result): try: self.result.SetValue(to_yaml(result)) except UnicodeDecodeError: self.result.SetValue("Cannot display result due to unprintable characters") def status_gui(self, status): return {GoalStatus.PENDING: ['PENDING', wx.Colour(255, 174, 59)], GoalStatus.ACTIVE: ['ACTIVE', wx.Colour(0, 255, 0)], GoalStatus.PREEMPTED: ['PREEMPTED', wx.Colour(255, 252, 16)], GoalStatus.SUCCEEDED: ['SUCCEEDED', wx.Colour(38, 250, 253)], GoalStatus.ABORTED: ['ABORTED', wx.Colour(200, 0, 0)], GoalStatus.REJECTED: ['REJECTED', wx.Colour(253, 38, 159)], GoalStatus.PREEMPTING: ['PREEMPTING', wx.Colour(253, 38, 159)], GoalStatus.RECALLING: ['RECALLING', wx.Colour(230, 38, 253)], GoalStatus.RECALLED: ['RECALLED', wx.Colour(230, 38, 253)], GoalStatus.LOST: ['LOST', wx.Colour(255, 0, 0)]}[status] def done_cb(self, state, result): status_string, status_color = self.status_gui(state) wx.CallAfter(self.set_status, ''.join(["Goal finished with status: ", status_string]), status_color) wx.CallAfter(self.set_result, result) wx.CallAfter(self.set_cancel_button, False) def active_cb(self): wx.CallAfter(self.set_status, "Goal is active", wx.Colour(0, 200, 0)) def set_feedback(self, feedback): try: self.feedback.SetValue(to_yaml(feedback)) except UnicodeDecodeError: self.feedback.SetValue("Cannot display feedback due to unprintable characters") def feedback_cb(self, feedback): wx.CallAfter(self.set_feedback, feedback) def OnQuit(self): self.server_check_timer.Stop() def OnInit(self): self.frame = wx.Frame( None, -1, self.action_name + ' - ' + self.action_type.name + ' - GUI Client' ) self.sz = wx.BoxSizer(wx.VERTICAL) tmp_goal = self.action_type.goal() self.goal = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE) self.goal.SetValue(to_yaml(tmp_goal)) self.goal_st_bx = wx.StaticBox(self.frame, -1, "Goal") self.goal_st = wx.StaticBoxSizer(self.goal_st_bx, wx.VERTICAL) self.goal_st.Add(self.goal, 1, wx.EXPAND) self.feedback = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE | wx.TE_READONLY)) self.feedback_st_bx = wx.StaticBox(self.frame, -1, "Feedback") self.feedback_st = wx.StaticBoxSizer(self.feedback_st_bx, wx.VERTICAL) self.feedback_st.Add(self.feedback, 1, wx.EXPAND) self.result = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE | wx.TE_READONLY)) self.result_st_bx = wx.StaticBox(self.frame, -1, "Result") self.result_st = wx.StaticBoxSizer(self.result_st_bx, wx.VERTICAL) self.result_st.Add(self.result, 1, wx.EXPAND) self.send_goal = wx.Button(self.frame, -1, label="SEND GOAL") self.send_goal.Bind(wx.EVT_BUTTON, self.on_goal) self.send_goal.Disable() self.cancel_goal = wx.Button(self.frame, -1, label="CANCEL GOAL") self.cancel_goal.Bind(wx.EVT_BUTTON, self.on_cancel) self.cancel_goal.Disable() self.status_bg = wx.Panel(self.frame, -1) self.status_bg.SetBackgroundColour(wx.Colour(200, 0, 0)) self.status = wx.StaticText(self.status_bg, -1, label="No Goal") self.server_status_bg = wx.Panel(self.frame, -1) self.server_status_bg.SetBackgroundColour(wx.Colour(200, 0, 0)) self.server_status = wx.StaticText(self.server_status_bg, -1, label="Disconnected from server.") self.sz.Add(self.goal_st, 1, wx.EXPAND) self.sz.Add(self.feedback_st, 1, wx.EXPAND) self.sz.Add(self.result_st, 1, wx.EXPAND) self.sz.Add(self.send_goal, 0, wx.EXPAND) self.sz.Add(self.cancel_goal, 0, wx.EXPAND) self.sz.Add(self.status_bg, 0, wx.EXPAND) self.sz.Add(self.server_status_bg, 0, wx.EXPAND) self.frame.SetSizer(self.sz) self.server_check_timer = wx.Timer(self.frame) self.frame.Bind(wx.EVT_TIMER, self.server_check, self.server_check_timer) self.server_check_timer.Start(1000) self.sz.Layout() self.frame.Show() return True def main(): rospy.init_node('axclient', anonymous=True) parser = OptionParser(__doc__.strip()) # parser.add_option("-t","--test",action="store_true", dest="test",default=False, # help="A testing flag") # parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah") (options, args) = parser.parse_args(rospy.myargv()) if (len(args) == 2): # get action type via rostopic topic_type = rostopic._get_topic_type("%s/goal" % args[1])[0] # remove "Goal" string from action type assert("Goal" in topic_type) topic_type = topic_type[0:len(topic_type)-4] elif (len(args) == 3): topic_type = args[2] print(topic_type) assert("Action" in topic_type) else: parser.error("You must specify the action topic name (and optionally type) Eg: ./axclient.py action_topic actionlib/TwoIntsAction ") action = DynamicAction(topic_type) app = AXClientApp(action, args[1]) app.MainLoop() app.OnQuit() rospy.signal_shutdown('GUI shutdown') if __name__ == '__main__': main() actionlib-1.12.0/tools/axserver.py000077500000000000000000000217331352264324200171240ustar00rootroot00000000000000#!/usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. """ usage: %prog /action_name action_type """ PKG = 'actionlib' from optparse import OptionParser import roslib.message import wx import rospy import actionlib import threading from cStringIO import StringIO from library import to_yaml, yaml_msg_str from dynamic_action import DynamicAction SEND_FEEDBACK = 0 SUCCEED = 1 ABORT = 2 PREEMPT = 3 class AXServerApp(wx.App): def __init__(self, action_type, action_name): self.action_type = action_type self.action_name = action_name wx.App.__init__(self) self.server = actionlib.SimpleActionServer( self.action_name, self.action_type.action, self.execute ) self.condition = threading.Condition() self.feedback_msg = None self.result_msg = None self.execute_type = None def set_goal(self, goal): if goal is None: self.status_bg.SetBackgroundColour(wx.Colour(200, 0, 0)) self.status.SetLabel("Waiting For Goal...") self.send_feedback.Disable() self.succeed.Disable() self.abort.Disable() self.preempt.Disable() self.goal.SetValue("") else: self.status_bg.SetBackgroundColour(wx.Colour(0, 200, 0)) self.status.SetLabel("Received Goal. Send feedback, succeed, or abort.") self.send_feedback.Enable() self.succeed.Enable() self.abort.Enable() self.preempt.Enable() try: self.goal.SetValue(to_yaml(goal)) except UnicodeDecodeError: self.goal.SetValue("Cannot display goal due to unprintable characters") def set_preempt_requested(self): self.status_bg.SetBackgroundColour(wx.Colour(0, 200, 200)) self.status.SetLabel("Preempt requested...") def execute(self, goal): wx.CallAfter(self.set_goal, goal) self.condition.acquire() self.result_msg = None self.feedback_msg = None self.execute_type = None while self.execute_type is None or self.execute_type == SEND_FEEDBACK: self.result_msg = None self.feedback_msg = None self.execute_type = None while self.execute_type is None: if self.server.is_preempt_requested(): wx.CallAfter(self.set_preempt_requested) self.condition.wait(1.0) if self.execute_type == SEND_FEEDBACK: if self.feedback_msg is not None: self.server.publish_feedback(self.feedback_msg) if self.execute_type == SUCCEED: self.server.set_succeeded(self.result_msg) if self.execute_type == ABORT: self.server.set_aborted() if self.execute_type == PREEMPT: self.server.set_preempted() wx.CallAfter(self.set_goal, None) self.condition.release() def on_feedback(self, event): self.condition.acquire() try: self.feedback_msg = yaml_msg_str(self.action_type.feedback, self.feedback.GetValue()) buff = StringIO() self.feedback_msg.serialize(buff) self.execute_type = SEND_FEEDBACK self.condition.notify() except roslib.message.SerializationError as e: self.feedback_msg = None wx.MessageBox(str(e), "Error serializing feedback", wx.OK) self.condition.release() def on_succeed(self, event): self.condition.acquire() try: self.result_msg = yaml_msg_str(self.action_type.result, self.result.GetValue()) buff = StringIO() self.result_msg.serialize(buff) self.execute_type = SUCCEED self.condition.notify() except roslib.message.SerializationError as e: self.result_msg = None wx.MessageBox(str(e), "Error serializing result", wx.OK) self.condition.release() def on_abort(self, event): self.condition.acquire() self.execute_type = ABORT self.condition.notify() self.condition.release() def on_preempt(self, event): self.condition.acquire() self.execute_type = PREEMPT self.condition.notify() self.condition.release() def OnInit(self): self.frame = wx.Frame( None, -1, self.action_name + ' - ' + self.action_type.name + ' - Standin' ) self.sz = wx.BoxSizer(wx.VERTICAL) tmp_feedback = self.action_type.feedback() tmp_result = self.action_type.result() self.goal = wx.TextCtrl(self.frame, -1, style=(wx.TE_MULTILINE | wx.TE_READONLY)) self.goal_st_bx = wx.StaticBox(self.frame, -1, "Goal") self.goal_st = wx.StaticBoxSizer(self.goal_st_bx, wx.VERTICAL) self.goal_st.Add(self.goal, 1, wx.EXPAND) self.feedback = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE) self.feedback.SetValue(to_yaml(tmp_feedback)) self.feedback_st_bx = wx.StaticBox(self.frame, -1, "Feedback") self.feedback_st = wx.StaticBoxSizer(self.feedback_st_bx, wx.VERTICAL) self.feedback_st.Add(self.feedback, 1, wx.EXPAND) self.result = wx.TextCtrl(self.frame, -1, style=wx.TE_MULTILINE) self.result.SetValue(to_yaml(tmp_result)) self.result_st_bx = wx.StaticBox(self.frame, -1, "Result") self.result_st = wx.StaticBoxSizer(self.result_st_bx, wx.VERTICAL) self.result_st.Add(self.result, 1, wx.EXPAND) self.send_feedback = wx.Button(self.frame, -1, label="SEND FEEDBACK") self.send_feedback.Bind(wx.EVT_BUTTON, self.on_feedback) self.succeed = wx.Button(self.frame, -1, label="SUCCEED") self.succeed.Bind(wx.EVT_BUTTON, self.on_succeed) self.abort = wx.Button(self.frame, -1, label="ABORT") self.abort.Bind(wx.EVT_BUTTON, self.on_abort) self.preempt = wx.Button(self.frame, -1, label="PREEMPT") self.preempt.Bind(wx.EVT_BUTTON, self.on_preempt) self.status_bg = wx.Panel(self.frame, -1) self.status_bg.SetBackgroundColour(wx.Colour(200, 0, 0)) self.status = wx.StaticText(self.status_bg, -1, label="Waiting For Goal...") self.sz.Add(self.goal_st, 1, wx.EXPAND) self.sz.Add(self.feedback_st, 1, wx.EXPAND) self.sz.Add(self.result_st, 1, wx.EXPAND) self.sz.Add(self.send_feedback, 0, wx.EXPAND) self.sz.Add(self.succeed, 0, wx.EXPAND) self.sz.Add(self.abort, 0, wx.EXPAND) self.sz.Add(self.preempt, 0, wx.EXPAND) self.sz.Add(self.status_bg, 0, wx.EXPAND) self.frame.SetSizer(self.sz) self.set_goal(None) self.sz.Layout() self.frame.Show() return True if __name__ == '__main__': rospy.init_node('axserver', anonymous=True) parser = OptionParser(__doc__.strip()) # parser.add_option("-t","--test",action="store_true", dest="test",default=False, # help="A testing flag") # parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah") (options, args) = parser.parse_args(rospy.myargv()) if (len(args) != 3): parser.error("You must specify the action name and type. Eg: ./axserver.py my_action actionlib/Test") action = DynamicAction(args[2]) app = AXServerApp(action, args[1]) app.MainLoop() rospy.signal_shutdown('GUI shutdown') actionlib-1.12.0/tools/dynamic_action.py000066400000000000000000000047241352264324200202440ustar00rootroot00000000000000#! /usr/bin/python # ********************************************************** # Software License Agreement (BSD License) # # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: Eitan Marder-Eppstein # ********************************************************** import roslib import rospy import sys class DynamicAction(object): def __init__(self, name): # remove "Action" string from name assert("Action" in name) self.name = name[0:len(name)-6] self.action = self.load_submsg('Action') self.goal = self.load_submsg('Goal') self.feedback = self.load_submsg('Feedback') self.result = self.load_submsg('Result') def load_submsg(self, subname): msgclass = roslib.message.get_message_class(self.name + subname) if msgclass is None: rospy.logfatal('Could not load message for: %s' % (self.name + subname)) sys.exit(1) return msgclass actionlib-1.12.0/tools/library.py000066400000000000000000000150111352264324200167160ustar00rootroot00000000000000# Software License Agreement (BSD License) # # Copyright (c) 2010, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Revision $Id: library.py 9993 2010-06-09 02:35:02Z kwc $ """ Top-level library routines we expose to the end-user """ from __future__ import with_statement import yaml import roslib.message import roslib.packages import rospy def findros(pkg, resource): """ Find ROS resource inside of a package. @param pkg: ROS package name @type pkg: str @param resource: resource filename @type resource: str """ val = roslib.packages.find_resource(pkg, resource) if val: return val[0] else: raise rospy.ROSException("cannot find resource") def YAMLBag(object): def __init__(self, filename): self.filename = filename self._fp = open(filename, 'w') def append(self, msg): self._fp.write(to_yaml(msg)) def close(self): if self._fp is not None: self._fp.close() self._fp = None def to_yaml(obj): if isinstance(obj, roslib.message.Message): return _message_to_yaml(obj) pass else: return yaml.dump(obj) def yaml_msg_str(type_, yaml_str, filename=None): """ Load single message from YAML dictionary representation. @param type_: Message class @type type_: class (Message subclass) @param filename: Name of YAML file @type filename: str """ import yaml if yaml_str.strip() == '': msg_dict = {} else: msg_dict = yaml.load(yaml_str) if not isinstance(msg_dict, dict): if filename: raise ValueError("yaml file [%s] does not contain a dictionary" % filename) else: raise ValueError("yaml string does not contain a dictionary") m = type_() roslib.message.fill_message_args(m, [msg_dict]) return m def yaml_msg(type_, filename): """ Load single message from YAML dictionary representation. @param type_: Message class @type type_: class (Message subclass) @param filename: Name of YAML file @type filename: str """ with open(filename, 'r') as f: return yaml_msg_str(type_, f.read(), filename=filename) def yaml_msgs_str(type_, yaml_str, filename=None): """ Load messages from YAML list-of-dictionaries representation. @param type_: Message class @type type_: class (Message subclass) @param filename: Name of YAML file @type filename: str """ import yaml yaml_doc = yaml.load(yaml_str) msgs = [] for msg_dict in yaml_doc: if not isinstance(msg_dict, dict): if filename: raise ValueError("yaml file [%s] does not contain a list of dictionaries" % filename) else: raise ValueError("yaml string does not contain a list of dictionaries") m = type_() roslib.message.fill_message_args(m, msg_dict) msgs.append(m) return msgs def yaml_msgs(type_, filename): """ Load messages from YAML list-of-dictionaries representation. @param type_: Message class @type type_: class (Message subclass) @param filename: Name of YAML file @type filename: str """ with open(filename, 'r') as f: return yaml_msgs_str(type_, f.read(), filename=filename) def _message_to_yaml(msg, indent='', time_offset=None): """ convert value to YAML representation @param val: to convert to string representation. Most likely a Message. @type val: Value @param indent: indentation @type indent: str @param time_offset: if not None, time fields will be displayed as deltas from time_offset @type time_offset: Time """ if type(msg) in [int, long, float, str, bool]: # TODO: need to actually escape return msg elif isinstance(msg, rospy.Time) or isinstance(msg, rospy.Duration): if time_offset is not None and isinstance(msg, rospy.Time): msg = msg-time_offset return '\n%ssecs: %s\n%snsecs: %s' % (indent, msg.secs, indent, msg.nsecs) elif type(msg) in [list, tuple]: # have to convert tuple->list to be yaml-safe if len(msg) == 0: return str(list(msg)) msg0 = msg[0] if type(msg0) in [int, float, str, bool] or \ isinstance(msg0, rospy.Time) or isinstance(msg0, rospy.Duration) or \ isinstance(msg0, list) or isinstance(msg0, tuple): # no array-of-arrays support yet return str(list(msg)) else: indent = indent + ' ' return "["+','.join([roslib.message.strify_message(v, indent, time_offset) for v in msg])+"]" elif isinstance(msg, rospy.Message): if indent: return '\n' + \ '\n'.join(['%s%s: %s' % ( indent, f, roslib.message.strify_message(getattr(msg, f), ' ' + indent, time_offset)) for f in msg.__slots__]) return '\n'.join(['%s%s: %s' % (indent, f, roslib.message.strify_message(getattr(msg, f), ' ' + indent, time_offset)) for f in msg.__slots__]) else: return str(msg) # punt