pax_global_header00006660000000000000000000000064125173565070014525gustar00rootroot0000000000000052 comment=ce2c24890af6fee4e71a2f0a20a76443e8e409b0 ros-actionlib-1.11.4/000077500000000000000000000000001251735650700143565ustar00rootroot00000000000000ros-actionlib-1.11.4/.gitignore000066400000000000000000000000061251735650700163420ustar00rootroot00000000000000*.pyc ros-actionlib-1.11.4/CHANGELOG.rst000066400000000000000000000056151251735650700164060ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package actionlib ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 ros-actionlib-1.11.4/CMakeLists.txt000066400000000000000000000025621251735650700171230ustar00rootroot00000000000000cmake_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() ros-actionlib-1.11.4/action/000077500000000000000000000000001251735650700156335ustar00rootroot00000000000000ros-actionlib-1.11.4/action/Test.action000066400000000000000000000000571251735650700177530ustar00rootroot00000000000000int32 goal --- int32 result --- int32 feedback ros-actionlib-1.11.4/action/TestRequest.action000066400000000000000000000011541251735650700213230ustar00rootroot00000000000000int32 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 --- ros-actionlib-1.11.4/action/TwoInts.action000066400000000000000000000000421251735650700204350ustar00rootroot00000000000000int64 a int64 b --- int64 sum --- ros-actionlib-1.11.4/docs/000077500000000000000000000000001251735650700153065ustar00rootroot00000000000000ros-actionlib-1.11.4/docs/action_interface.svg000066400000000000000000000473271251735650700213410ustar00rootroot00000000000000 image/svg+xml Action Interface ActionClient ActionServer goal cancel status result feedback From Client From Server ROS Topics ros-actionlib-1.11.4/docs/cancel_policy.svg000066400000000000000000000304041251735650700206340ustar00rootroot00000000000000 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 ros-actionlib-1.11.4/docs/server_states_detailed.svg000066400000000000000000007062211251735650700225630ustar00rootroot00000000000000 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 ros-actionlib-1.11.4/docs/simple_goal_reception.svg000066400000000000000000003216031251735650700223770ustar00rootroot00000000000000 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 ros-actionlib-1.11.4/include/000077500000000000000000000000001251735650700160015ustar00rootroot00000000000000ros-actionlib-1.11.4/include/actionlib/000077500000000000000000000000001251735650700177455ustar00rootroot00000000000000ros-actionlib-1.11.4/include/actionlib/action_definition.h000066400000000000000000000054621251735650700236120ustar00rootroot00000000000000/********************************************************************* * * 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 ACTION_DEFINITION_H_ #define 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 ActionResultConstPtr; \ typedef boost::shared_ptr ResultConstPtr;\ \ typedef boost::shared_ptr ActionFeedbackConstPtr; \ typedef boost::shared_ptr FeedbackConstPtr; }; #endif ros-actionlib-1.11.4/include/actionlib/client/000077500000000000000000000000001251735650700212235ustar00rootroot00000000000000ros-actionlib-1.11.4/include/actionlib/client/action_client.h000066400000000000000000000256331251735650700242200ustar00rootroot00000000000000/********************************************************************* * 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_ACTION_CLIENT_H_ #define ACTIONLIB_ACTION_CLIENT_H_ #include #include "ros/ros.h" #include "ros/callback_queue_interface.h" #include #include #include 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 (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) { status_sub_ = queue_subscribe("status", 1, &ActionClientT::statusCb, this, queue); feedback_sub_ = queue_subscribe("feedback", 1, &ActionClientT::feedbackCb, this, queue); result_sub_ = queue_subscribe("result", 1, &ActionClientT::resultCb, this, queue); connection_monitor_.reset(new ConnectionMonitor(feedback_sub_, status_sub_)); // Start publishers and subscribers goal_pub_ = queue_advertise("goal", 10, boost::bind(&ConnectionMonitor::goalConnectCallback, connection_monitor_, _1), boost::bind(&ConnectionMonitor::goalDisconnectCallback, connection_monitor_, _1), queue); cancel_pub_ = queue_advertise("cancel", 10, 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()); } }; } #endif ros-actionlib-1.11.4/include/actionlib/client/client_goal_handle_imp.h000066400000000000000000000233241251735650700260400ustar00rootroot00000000000000/********************************************************************* * 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. */ 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() { 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); } assert(gm_); boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_); return list_handle_.getElem()->getCommState(); } template TerminalState ClientGoalHandle::getTerminalState() { 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_); 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() { if (!active_) ROS_ERROR_NAMED("actionlib", "Trying to getResult on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle"); assert(gm_); 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_); 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_); 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); } } ros-actionlib-1.11.4/include/actionlib/client/client_helpers.h000066400000000000000000000225171251735650700244030ustar00rootroot00000000000000/********************************************************************* * 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_MANAGER_H_ #define ACTIONLIB_GOAL_MANAGER_H_ #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< boost::shared_ptr > > 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(); /** * \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(); /** * \brief Get result associated with this goal * * \return NULL if no reseult received. Otherwise returns shared_ptr to result. */ ResultConstPtr getResult(); /** * \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< boost::shared_ptr > > 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_callback, FeedbackCallback feedback_callback); 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& feedback); void updateResult(GoalHandleT& gh, const ActionResultConstPtr& 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; }; } #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_GOAL_MANAGER_H_ ros-actionlib-1.11.4/include/actionlib/client/comm_state.h000066400000000000000000000070141251735650700235310ustar00rootroot00000000000000/********************************************************************* * 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(); } ; } #endif ros-actionlib-1.11.4/include/actionlib/client/comm_state_machine_imp.h000066400000000000000000000436001251735650700260630ustar00rootroot00000000000000/********************************************************************* * 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. */ 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; igoal_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); } } ros-actionlib-1.11.4/include/actionlib/client/connection_monitor.h000066400000000000000000000063031251735650700253040ustar00rootroot00000000000000/********************************************************************* * 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_ACTION_CONNECTION_MONITOR_H_ #define ACTIONLIB_ACTION_CONNECTION_MONITOR_H_ #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& 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_; }; } #endif ros-actionlib-1.11.4/include/actionlib/client/goal_manager_imp.h000066400000000000000000000117161251735650700246630ustar00rootroot00000000000000/********************************************************************* * 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. */ 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; 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"); 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_); return GoalHandleT(this, list_handle, guard_); } template void GoalManager::listElemDeleter(typename ManagedListT::iterator it) { assert(guard_); 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; } } } ros-actionlib-1.11.4/include/actionlib/client/service_client.h000066400000000000000000000066701251735650700244030ustar00rootroot00000000000000/********************************************************************* * * 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 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_; }; }; //include the implementation #include #endif ros-actionlib-1.11.4/include/actionlib/client/service_client_imp.h000066400000000000000000000103541251735650700252420ustar00rootroot00000000000000/********************************************************************* * * 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_ 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); } }; #endif ros-actionlib-1.11.4/include/actionlib/client/simple_action_client.h000066400000000000000000000545331251735650700255720ustar00rootroot00000000000000/********************************************************************* * 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_SIMPLE_ACTION_CLIENT_H_ #define ACTIONLIB_SIMPLE_ACTION_CLIENT_H_ #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 exisitng 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) ) { 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() { 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(); /** * \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(); /** * \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() { if (gh_.isExpired()) { ROS_ERROR_NAMED("actionlib", "Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient"); 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() { 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: done_mutex_.lock(); setSimpleState(SimpleGoalState::DONE); done_mutex_.unlock(); 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(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(); } } #undef DEPRECATED #endif // ACTIONLIB_SINGLE_GOAL_ACTION_CLIENT_H_ ros-actionlib-1.11.4/include/actionlib/client/simple_client_goal_state.h000066400000000000000000000075761251735650700264440ustar00rootroot00000000000000/********************************************************************* * 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_ 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"; } }; } #endif // ACTIONLIB_CLIENT_SIMPLE_CLIENT_GOAL_STATE_H_ ros-actionlib-1.11.4/include/actionlib/client/simple_goal_state.h000066400000000000000000000060761251735650700251000ustar00rootroot00000000000000/********************************************************************* * 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(); } ; } #endif ros-actionlib-1.11.4/include/actionlib/client/terminal_state.h000066400000000000000000000061501251735650700244110ustar00rootroot00000000000000/********************************************************************* * 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_ 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(); } ; } #endif ros-actionlib-1.11.4/include/actionlib/client_goal_status.h000066400000000000000000000133561251735650700240110ustar00rootroot00000000000000/********************************************************************* * 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" 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 }; } #endif // ACTION_TOOLS_CLIENT_GOAL_STATE_H_ ros-actionlib-1.11.4/include/actionlib/decl.h000066400000000000000000000044611251735650700210320ustar00rootroot00000000000000/********************************************************************* * * 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_INCLUDED #define ACTIONLIB_DECL_H_INCLUDED #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 #define ACTIONLIB_DECL #endif #endif ros-actionlib-1.11.4/include/actionlib/destruction_guard.h000066400000000000000000000102451251735650700236450ustar00rootroot00000000000000/********************************************************************* * * 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_ #define ACTIONLIB_DESTRUCTION_GUARD_ #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.0f)); } } /** * @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_; }; }; #endif ros-actionlib-1.11.4/include/actionlib/enclosure_deleter.h000066400000000000000000000056331251735650700236300ustar00rootroot00000000000000/********************************************************************* * * 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* member_ptr){ 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; } } #endif ros-actionlib-1.11.4/include/actionlib/goal_id_generator.h000066400000000000000000000052161251735650700235660ustar00rootroot00000000000000/********************************************************************* * 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 ACTION_LIB_GOAL_ID_GENERATOR_H_ #define ACTION_LIB_GOAL_ID_GENERATOR_H_ #include #include #include "ros/time.h" #include "actionlib_msgs/GoalID.h" #include 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_ ; }; } #endif ros-actionlib-1.11.4/include/actionlib/managed_list.h000066400000000000000000000162161251735650700225530ustar00rootroot00000000000000/********************************************************************* * 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 "ros/console.h" #include #include #include #include #include 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; } 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< void(iterator)> CustomDeleter; private: class ElemDeleter { public: ElemDeleter(iterator it, CustomDeleter deleter, const boost::shared_ptr& guard) : it_(it), deleter_(deleter), guard_(guard) { } void operator() (void* ptr) { 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) { } const Handle& operator=(const Handle& rhs) { if ( rhs.valid_ ) { it_ = rhs.it_; } handle_tracker_ = rhs.handle_tracker_; valid_ = rhs.valid_; return rhs; } /** * \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_); return *it_; } /** * \brief Checks if two handles point to the same list elem */ bool operator==(const Handle& rhs) const { assert(valid_); assert(rhs.valid_); 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); } } #endif ros-actionlib-1.11.4/include/actionlib/one_shot_timer.h000066400000000000000000000053771251735650700231500ustar00rootroot00000000000000/********************************************************************* * 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 ACTION_TOOLS_ROBUST_ONE_SHOT_TIMER_H_ #define ACTION_TOOLS_ROBUST_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 ros-actionlib-1.11.4/include/actionlib/server/000077500000000000000000000000001251735650700212535ustar00rootroot00000000000000ros-actionlib-1.11.4/include/actionlib/server/action_server.h000066400000000000000000000167271251735650700243040ustar00rootroot00000000000000/********************************************************************* * * 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 ACTION_LIB_ACTION_SERVER #define ACTION_LIB_ACTION_SERVER #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 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. */ 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 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. */ 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 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. */ 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_; }; }; //include the implementation #include #endif ros-actionlib-1.11.4/include/actionlib/server/action_server_base.h000066400000000000000000000310061251735650700252610ustar00rootroot00000000000000/********************************************************************* * * 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 ACTION_LIB_ACTION_SERVER_BASE #define ACTION_LIB_ACTION_SERVER_BASE #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 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. */ 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 lock_.unlock(); //now, we need to create a goal handle and call the user's callback goal_callback_(gh); lock_.lock(); } } 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 lock_.unlock(); //call the user's cancel callback on the relevant goal cancel_callback_(gh); //lock for further modification of the status list lock_.lock(); } } } //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; } } #endif ros-actionlib-1.11.4/include/actionlib/server/action_server_imp.h000066400000000000000000000216461251735650700251450ustar00rootroot00000000000000/********************************************************************* * * 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_SERVER_IMP_H_ #define ACTIONLIB_ACTION_SERVER_IMP_H_ 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()); 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()); 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()); 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()); initialize(); publishStatus(); } } template ActionServer::~ActionServer() { } template void ActionServer::initialize() { result_pub_ = node_.advertise("result", 50); feedback_pub_ = node_.advertise("feedback", 50); status_pub_ = node_.advertise("status", 50, 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", 50, boost::bind(&ActionServerBase::goalCallback, this, _1)); cancel_sub_ = node_.subscribe("cancel", 50, 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& e) { 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); } }; #endif ros-actionlib-1.11.4/include/actionlib/server/handle_tracker_deleter.h000066400000000000000000000057071251735650700261070ustar00rootroot00000000000000/********************************************************************* * * 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 ACTONLIB_HANDLE_TRACKER_DELETER_H_ #define ACTONLIB_HANDLE_TRACKER_DELETER_H_ #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_; }; }; #include #endif ros-actionlib-1.11.4/include/actionlib/server/handle_tracker_deleter_imp.h000066400000000000000000000053151251735650700267470ustar00rootroot00000000000000/********************************************************************* * * 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_HANDLE_TRACKER_DELETER_IMP_H_ #define ACTIONLIB_HANDLE_TRACKER_DELETER_IMP_H_ 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* ptr){ 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 as_->lock_.lock(); (*status_it_).handle_destruction_time_ = ros::Time::now(); //as_->status_list_.erase(status_it_); as_->lock_.unlock(); } } } }; #endif ros-actionlib-1.11.4/include/actionlib/server/server_goal_handle.h000066400000000000000000000176561251735650700252660ustar00rootroot00000000000000/********************************************************************* * * 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_GOAL_HANDLE_H_ #define ACTIONLIB_SERVER_GOAL_HANDLE_H_ #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 transisions 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; }; }; //include the implementation #include #endif ros-actionlib-1.11.4/include/actionlib/server/server_goal_handle_imp.h000066400000000000000000000361261251735650700261240ustar00rootroot00000000000000/********************************************************************* * * 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_GOAL_HANDLE_IMP_H_ #define ACTIONLIB_SERVER_GOAL_HANDLE_IMP_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(); } //if we were recalling before, now we'll go to preempting else if(status == actionlib_msgs::GoalStatus::RECALLING){ (*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", "Transisitoning 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; } }; #endif ros-actionlib-1.11.4/include/actionlib/server/service_server.h000066400000000000000000000063261251735650700244610ustar00rootroot00000000000000/********************************************************************* * * 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 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_; }; }; //include the implementation #include #endif ros-actionlib-1.11.4/include/actionlib/server/service_server_imp.h000066400000000000000000000062731251735650700253270ustar00rootroot00000000000000/********************************************************************* * * 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_ 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"); } }; #endif ros-actionlib-1.11.4/include/actionlib/server/simple_action_server.h000066400000000000000000000257341251735650700256530ustar00rootroot00000000000000/********************************************************************* * * 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_SIMPLE_ACTION_SERVER_H_ #define ACTIONLIB_SIMPLE_ACTION_SERVER_H_ #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 change 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_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. */ SimpleActionServer(std::string name, ExecuteCallback execute_cb, 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 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. */ SimpleActionServer(std::string name, bool auto_start); /** * @brief DEPRECATED: 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. */ ROS_DEPRECATED SimpleActionServer(std::string name, ExecuteCallback execute_cb = 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_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. */ SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb, 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 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. */ 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_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. */ ROS_DEPRECATED SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb = 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 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. */ 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_; }; }; //include the implementation here #include #endif ros-actionlib-1.11.4/include/actionlib/server/simple_action_server_imp.h000066400000000000000000000345371251735650700265210ustar00rootroot00000000000000/********************************************************************* * * 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 *********************************************************************/ 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_); 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_(); } //if the preempt applies to the next goal, we'll set the preempt bit for that else if(preempt == next_goal_){ 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 lock.unlock(); execute_callback_(goal); lock.lock(); 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(loop_duration.toSec() * 1000.0f)); } } template void SimpleActionServer::start(){ as_->start(); } }; ros-actionlib-1.11.4/include/actionlib/server/status_tracker.h000066400000000000000000000055561251735650700244750ustar00rootroot00000000000000/********************************************************************* * * 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_STATUS_TRACKER_H_ #define ACTIONLIB_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_; }; }; //include the implementation #include #endif ros-actionlib-1.11.4/include/actionlib/server/status_tracker_imp.h000066400000000000000000000054571251735650700253420ustar00rootroot00000000000000/********************************************************************* * * 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_STATUS_TRACKER_IMP_H_ #define ACTIONLIB_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(); } } }; #endif ros-actionlib-1.11.4/mainpage.dox000066400000000000000000000030651251735650700166570ustar00rootroot00000000000000/** \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 */ ros-actionlib-1.11.4/package.xml000066400000000000000000000023701251735650700164750ustar00rootroot00000000000000 actionlib 1.11.4 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. Esteve Fernandez BSD http://www.ros.org/wiki/actionlib Eitan Marder-Eppstein Vijay Pradeep catkin actionlib_msgs boost message_generation roscpp rospy rostest std_msgs actionlib_msgs boost message_runtime roscpp rospy rostest std_msgs rosnode ros-actionlib-1.11.4/setup.py000066400000000000000000000003321251735650700160660ustar00rootroot00000000000000#!/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) ros-actionlib-1.11.4/src/000077500000000000000000000000001251735650700151455ustar00rootroot00000000000000ros-actionlib-1.11.4/src/actionlib/000077500000000000000000000000001251735650700171115ustar00rootroot00000000000000ros-actionlib-1.11.4/src/actionlib/__init__.py000066400000000000000000000032571251735650700212310ustar00rootroot00000000000000# 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 action_client import * from simple_action_client import * from action_server import * from simple_action_server import * ros-actionlib-1.11.4/src/actionlib/action_client.py000077500000000000000000000624161251735650700223120ustar00rootroot00000000000000#! /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 * from actionlib.exceptions import * g_goal_id = 1 def get_name_of_constant(C, n): for k,v in C.__dict__.iteritems(): if type(v) is 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) ## @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 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): live_statuses = [] 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_goal = rospy.Publisher(rospy.remap_name(ns) + '/goal', self.ActionGoal, queue_size=10) self.pub_cancel = rospy.Publisher(rospy.remap_name(ns) + '/cancel', GoalID, queue_size=10) self.manager = GoalManager(ActionSpec) self.manager.register_send_goal_fn(self.pub_goal.publish) self.manager.register_cancel_fn(self.pub_cancel.publish) self.status_sub = rospy.Subscriber(rospy.remap_name(ns) + '/status', GoalStatusArray, self._status_cb) self.result_sub = rospy.Subscriber(rospy.remap_name(ns) + '/result', self.ActionResult, self._result_cb) self.feedback_sub = rospy.Subscriber(rospy.remap_name(ns) + '/feedback', self.ActionFeedback, self._feedback_cb) ## @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) ros-actionlib-1.11.4/src/actionlib/action_server.py000066400000000000000000000362571251735650700223430ustar00rootroot00000000000000#! /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 * from goal_id_generator import GoalIDGenerator from status_tracker import StatusTracker from handle_tracker_deleter import HandleTrackerDeleter from server_goal_handle import ServerGoalHandle from actionlib.exceptions import * 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.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True, queue_size=50); self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult, queue_size=50); self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback, queue_size=50); self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal,self.internal_goal_callback); self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID,self.internal_cancel_callback); #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; 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; 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, actionlib_msgs.msg.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 == actionlib_msgs.msg.GoalStatus.RECALLING: st.status.status = actionlib_msgs.msg.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): rospy.logdebug("Status async"); 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 = actionlib_msgs.msg.GoalStatusArray() #status_array.set_status_list_size(len(self.status_list)); i=0; while i(&(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 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; ros-actionlib-1.11.4/src/actionlib/simple_action_client.py000066400000000000000000000264241251735650700236570ustar00rootroot00000000000000#! /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 threading import time import rospy from rospy import Header from actionlib_msgs.msg import * from 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_goal_to_finish 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: rospy.logerr("Called get_state when no goal is running") 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: rospy.logerr("Got a feedback callback when we're not tracking a goal. (id: %s)" % \ gh.comm_state_machine.action_goal.goal_id.id) 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 ros-actionlib-1.11.4/src/actionlib/simple_action_server.py000066400000000000000000000354221251735650700237050ustar00rootroot00000000000000#! /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 * 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 == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.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, 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()): rospy.logdebug("SAS: execute"); 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, 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()); ros-actionlib-1.11.4/src/actionlib/status_tracker.py000066400000000000000000000060351251735650700225250ustar00rootroot00000000000000# 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 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(); ros-actionlib-1.11.4/src/connection_monitor.cpp000066400000000000000000000227741251735650700215730ustar00rootroot00000000000000/********************************************************************* * 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 using namespace std; 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__) ConnectionMonitor::ConnectionMonitor(ros::Subscriber&feedback_sub, ros::Subscriber& result_sub) : feedback_sub_(feedback_sub), result_sub_(result_sub) { status_received_ = false; } // ********* Goal Connections ********* void 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 ConnectionMonitor::goalDisconnectCallback(const ros::SingleSubscriberPublisher& pub) { boost::recursive_mutex::scoped_lock lock(data_mutex_); 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()); } string ConnectionMonitor::goalSubscribersString() { boost::recursive_mutex::scoped_lock lock(data_mutex_); ostringstream ss; ss << "Goal Subscribers (" << goalSubscribers_.size() << " total)"; for (map::iterator it=goalSubscribers_.begin(); it != goalSubscribers_.end(); it++) ss << "\n - " << it->first; return ss.str(); } // ********* Cancel Connections ********* void 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 ConnectionMonitor::cancelDisconnectCallback(const ros::SingleSubscriberPublisher& pub) { boost::recursive_mutex::scoped_lock lock(data_mutex_); 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()); } string ConnectionMonitor::cancelSubscribersString() { boost::recursive_mutex::scoped_lock lock(data_mutex_); ostringstream ss; ss << "cancel Subscribers (" << cancelSubscribers_.size() << " total)"; for (map::iterator it=cancelSubscribers_.begin(); it != cancelSubscribers_.end(); it++) ss << "\n - " << it->first; return ss.str(); } // ********* GoalStatus Connections ********* void 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 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 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(time_left.toSec() * 1000.0f)); } return isServerConnected(); } ros-actionlib-1.11.4/src/goal_id_generator.cpp000066400000000000000000000050521251735650700213170ustar00rootroot00000000000000/********************************************************************* * 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 using namespace actionlib; static boost::mutex s_goalcount_mutex_; static unsigned int s_goalcount_ = 0; GoalIDGenerator::GoalIDGenerator() { setName(ros::this_node::getName()); } GoalIDGenerator::GoalIDGenerator(const std::string& name) { setName(name); } void GoalIDGenerator::setName(const std::string& name) { name_ = name; } actionlib_msgs::GoalID 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; } ros-actionlib-1.11.4/test/000077500000000000000000000000001251735650700153355ustar00rootroot00000000000000ros-actionlib-1.11.4/test/CMakeLists.txt000066400000000000000000000065261251735650700201060ustar00rootroot00000000000000if(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() ros-actionlib-1.11.4/test/action_client_destruction_test.cpp000066400000000000000000000046471251735650700243510ustar00rootroot00000000000000/********************************************************************* * 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(); } ros-actionlib-1.11.4/test/add_two_ints_client.cpp000066400000000000000000000047051251735650700220630ustar00rootroot00000000000000/********************************************************************* * * 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", (long int)resp.sum); return 1; } return 0; } ros-actionlib-1.11.4/test/add_two_ints_server.cpp000066400000000000000000000046611251735650700221140ustar00rootroot00000000000000/********************************************************************* * * 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", (long int)req.a, (long int)req.b); ROS_INFO_NAMED("actionlib", " sending back response: [%ld]", (long int)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; } ros-actionlib-1.11.4/test/destruction_guard_test.cpp000066400000000000000000000075571251735650700226430ustar00rootroot00000000000000/********************************************************************* * 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"); usleep(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.0f)); } } 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(); } ros-actionlib-1.11.4/test/exercise_simple_client.cpp000066400000000000000000000126301251735650700225610ustar00rootroot00000000000000/* * 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; } ros-actionlib-1.11.4/test/exercise_simple_client.py000077500000000000000000000143231251735650700224330ustar00rootroot00000000000000#! /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 * from actionlib.msg import * 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) ros-actionlib-1.11.4/test/mock_simple_server.py000077500000000000000000000112531251735650700216040ustar00rootroot00000000000000#! /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 sys import time import rospy import rostest from actionlib.simple_action_server import SimpleActionServer from actionlib.server_goal_handle import ServerGoalHandle; from actionlib.msg import * 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() ros-actionlib-1.11.4/test/ref_server.cpp000066400000000000000000000063211251735650700202050ustar00rootroot00000000000000/********************************************************************* * 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 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); }; } 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 gh) { } int main(int argc, char** argv) { ros::init(argc, argv, "ref_server"); ros::NodeHandle nh; RefServer ref_server(nh, "reference_action"); ros::spin(); } ros-actionlib-1.11.4/test/ref_server.py000077500000000000000000000076411251735650700200640ustar00rootroot00000000000000#!/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 import sys 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(); ros-actionlib-1.11.4/test/ref_server_test.launch000066400000000000000000000003121251735650700217260ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/ref_simple_server.py000077500000000000000000000066151251735650700214350ustar00rootroot00000000000000#!/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 import sys 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(); ros-actionlib-1.11.4/test/server_goal_handle_destruction.cpp000066400000000000000000000076571251735650700243260ustar00rootroot00000000000000/********************************************************************* * 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); }; } 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(); } ros-actionlib-1.11.4/test/simple_action_server_deadlock_companion.py000077500000000000000000000056331251735650700260260ustar00rootroot00000000000000#! /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 ros-actionlib-1.11.4/test/simple_client_allocator_test.cpp000066400000000000000000000044471251735650700240000ustar00rootroot00000000000000/********************************************************************* * 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(); } ros-actionlib-1.11.4/test/simple_client_test.cpp000066400000000000000000000110371251735650700217310ustar00rootroot00000000000000/********************************************************************* * 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; 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& result) { *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& result) { *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; } ros-actionlib-1.11.4/test/simple_client_wait_test.cpp000066400000000000000000000057411251735650700227620ustar00rootroot00000000000000/********************************************************************* * 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; } ros-actionlib-1.11.4/test/simple_execute_ref_server.cpp000066400000000000000000000064301251735650700233010ustar00rootroot00000000000000/********************************************************************* * 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); }; } 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; } ros-actionlib-1.11.4/test/simple_execute_ref_server_test.launch000066400000000000000000000005021251735650700250220ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/simple_python_client_test.py000077500000000000000000000054701251735650700232070ustar00rootroot00000000000000#! /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 sys import unittest import rospy from actionlib_msgs.msg import * 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) ros-actionlib-1.11.4/test/test_cpp_action_client_destruction.launch000066400000000000000000000002141251735650700256650ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/test_cpp_simple_client_allocator.launch000066400000000000000000000002101251735650700253120ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/test_cpp_simple_client_cancel_crash.cpp000066400000000000000000000046741251735650700252710ustar00rootroot00000000000000/********************************************************************* * 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 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(); } ros-actionlib-1.11.4/test/test_cpp_simple_client_cancel_crash.launch000066400000000000000000000002221251735650700257420ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/test_exercise_simple_clients.launch000066400000000000000000000005251251735650700244730ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/test_imports.launch000066400000000000000000000001361251735650700212650ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/test_imports.py000077500000000000000000000045441251735650700204550ustar00rootroot00000000000000#!/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 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) ros-actionlib-1.11.4/test/test_python_server.launch000066400000000000000000000003051251735650700224750ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/test_python_server2.launch000066400000000000000000000003061251735650700225600ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/test_python_server3.launch000066400000000000000000000003051251735650700225600ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/test_python_server_components.launch000066400000000000000000000001621251735650700247430ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/test_python_simple_client.launch000066400000000000000000000003431251735650700240200ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/test_python_simple_server.launch000066400000000000000000000003361251735650700240520ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/test_ref_action_server.py000077500000000000000000000171461251735650700224610ustar00rootroot00000000000000#! /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 from actionlib_msgs.msg import * 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) ros-actionlib-1.11.4/test/test_ref_simple_action_server.py000077500000000000000000000066451251735650700240340ustar00rootroot00000000000000#! /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 from actionlib_msgs.msg import * 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) ros-actionlib-1.11.4/test/test_server_components.py000077500000000000000000000101551251735650700225260ustar00rootroot00000000000000#!/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 sys 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) ros-actionlib-1.11.4/test/test_server_goal_handle_destruction.launch000066400000000000000000000002161251735650700260350ustar00rootroot00000000000000 ros-actionlib-1.11.4/test/test_simple_action_server_deadlock.py000077500000000000000000000106311251735650700250140ustar00rootroot00000000000000#! /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: pkg = "actionlib" node = "test_simple_action_server_deadlock" topic = "deadlock" deadlock_timeout = 45 # in seconds shutdown_timeout = 2 # in seconds max_action_duration = 3 import random import sys import threading import unittest import actionlib from actionlib.msg import TestAction import rosnode import rospy 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) ros-actionlib-1.11.4/test/test_simple_action_server_deadlock_python.launch000066400000000000000000000014661251735650700272420ustar00rootroot00000000000000 ros-actionlib-1.11.4/tools/000077500000000000000000000000001251735650700155165ustar00rootroot00000000000000ros-actionlib-1.11.4/tools/axclient.py000077500000000000000000000231341251735650700177050ustar00rootroot00000000000000#! /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 sys import rospy import actionlib import time import threading import socket import rostopic from cStringIO import StringIO from library import * 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 wx.App.__init__(self) self.client = actionlib.SimpleActionClient(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, 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_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): parser.error("You must specify the action topic name Eg: ./axclient.py my_action_topic") # get action type from topic 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] action = DynamicAction(topic_type) app = AXClientApp(action, args[1]) app.MainLoop() app.OnQuit() rospy.signal_shutdown('GUI shutdown') if __name__ == '__main__': main() ros-actionlib-1.11.4/tools/axserver.py000077500000000000000000000212321251735650700177320ustar00rootroot00000000000000#!/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 sys import rospy import actionlib import time import threading from cStringIO import StringIO from library import * 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 wx.App.__init__(self) self.server = actionlib.SimpleActionServer(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 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: self.condition.wait() 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, 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, 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_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') ros-actionlib-1.11.4/tools/dynamic_action.py000066400000000000000000000047411251735650700210570ustar00rootroot00000000000000#! /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 ros-actionlib-1.11.4/tools/library.py000066400000000000000000000147571251735650700175520ustar00rootroot00000000000000# 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 ROSHException("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 type(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 type(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, Time) or isinstance(msg0, Duration) or \ type(msg0) in [list, 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, 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