├── .gitignore ├── actionlib_tools ├── src │ └── actionlib_tools │ │ ├── __init__.py │ │ ├── dynamic_action.py │ │ └── library.py ├── setup.py ├── CMakeLists.txt ├── package.xml └── CHANGELOG.rst ├── actionlib ├── action │ ├── TwoInts.action │ ├── Test.action │ └── TestRequest.action ├── test │ ├── test_imports.launch │ ├── test_python_server_components.launch │ ├── test_cpp_simple_client_allocator.launch │ ├── test_cpp_action_client_destruction.launch │ ├── test_server_goal_handle_destruction.launch │ ├── test_cpp_simple_client_cancel_crash.launch │ ├── ref_server_test.launch │ ├── test_python_server.launch │ ├── test_python_server2.launch │ ├── test_python_server3.launch │ ├── test_python_simple_server.launch │ ├── test_python_simple_client.launch │ ├── test_python_exercise_simple_client.launch │ ├── test_cpp_exercise_simple_client.launch │ ├── simple_execute_ref_server_test.launch │ ├── test_simple_action_server_deadlock_python.launch │ ├── simple_client_allocator_test.cpp │ ├── test_imports.py │ ├── action_client_destruction_test.cpp │ ├── add_two_ints_client.cpp │ ├── test_cpp_simple_client_cancel_crash.cpp │ ├── add_two_ints_server.cpp │ ├── simple_python_client_test.py │ ├── simple_action_server_deadlock_companion.py │ ├── simple_client_wait_test.cpp │ ├── ref_simple_server.py │ ├── ref_server.cpp │ ├── test_ref_simple_action_server.py │ ├── simple_execute_ref_server.cpp │ ├── simple_action_server_construction_test.cpp │ ├── CMakeLists.txt │ ├── ref_server.py │ ├── destruction_guard_test.cpp │ ├── test_server_components.py │ ├── server_goal_handle_destruction.cpp │ ├── mock_simple_server.py │ ├── test_simple_action_server_deadlock.py │ ├── simple_client_test.cpp │ ├── exercise_simple_client.cpp │ └── exercise_simple_client.py ├── setup.py ├── CMakeLists.txt ├── package.xml ├── mainpage.dox ├── src │ ├── actionlib │ │ ├── exceptions.py │ │ ├── __init__.py │ │ ├── handle_tracker_deleter.py │ │ ├── goal_id_generator.py │ │ └── status_tracker.py │ └── goal_id_generator.cpp └── include │ └── actionlib │ ├── decl.h │ ├── goal_id_generator.h │ ├── server │ ├── handle_tracker_deleter_imp.h │ ├── status_tracker_imp.h │ ├── status_tracker.h │ ├── handle_tracker_deleter.h │ ├── service_server_imp.h │ ├── service_server.h │ └── action_server.h │ ├── one_shot_timer.h │ ├── enclosure_deleter.h │ ├── action_definition.h │ ├── client │ ├── simple_goal_state.h │ ├── connection_monitor.h │ ├── terminal_state.h │ ├── service_client.h │ ├── comm_state.h │ ├── simple_client_goal_state.h │ ├── service_client_imp.h │ └── goal_manager_imp.h │ └── destruction_guard.h └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | -------------------------------------------------------------------------------- /actionlib_tools/src/actionlib_tools/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /actionlib/action/TwoInts.action: -------------------------------------------------------------------------------- 1 | int64 a 2 | int64 b 3 | --- 4 | int64 sum 5 | --- 6 | -------------------------------------------------------------------------------- /actionlib/action/Test.action: -------------------------------------------------------------------------------- 1 | int32 goal 2 | --- 3 | int32 result 4 | --- 5 | int32 feedback 6 | -------------------------------------------------------------------------------- /actionlib/test/test_imports.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /actionlib/test/test_python_server_components.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /actionlib/test/test_cpp_simple_client_allocator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /actionlib/test/test_cpp_action_client_destruction.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /actionlib/test/test_server_goal_handle_destruction.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /actionlib/test/test_cpp_simple_client_cancel_crash.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Archived - ROS 1 End-of-life 2 | 3 | This repository contains ROS 1 packages. 4 | The last supported ROS 1 release, ROS Noetic, [officially reached end of life on May 31st, 2025](https://bit.ly/NoeticEOL). 5 | 6 | -------------------------------------------------------------------------------- /actionlib/test/ref_server_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /actionlib/test/test_python_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /actionlib/test/test_python_server2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /actionlib/test/test_python_server3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /actionlib/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['actionlib'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /actionlib/test/test_python_simple_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /actionlib/test/test_python_simple_client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /actionlib_tools/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['actionlib_tools'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /actionlib/test/test_python_exercise_simple_client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /actionlib/test/test_cpp_exercise_simple_client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /actionlib_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(actionlib_tools) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_python_setup() 7 | 8 | catkin_package() 9 | 10 | catkin_install_python(PROGRAMS 11 | scripts/axclient.py 12 | scripts/axserver.py 13 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 14 | -------------------------------------------------------------------------------- /actionlib/test/simple_execute_ref_server_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /actionlib/action/TestRequest.action: -------------------------------------------------------------------------------- 1 | int32 TERMINATE_SUCCESS = 0 2 | int32 TERMINATE_ABORTED = 1 3 | int32 TERMINATE_REJECTED = 2 4 | int32 TERMINATE_LOSE = 3 5 | int32 TERMINATE_DROP = 4 6 | int32 TERMINATE_EXCEPTION = 5 7 | int32 terminate_status 8 | bool ignore_cancel # If true, ignores requests to cancel 9 | string result_text 10 | int32 the_result # Desired value for the_result in the Result 11 | bool is_simple_client 12 | duration delay_accept # Delays accepting the goal by this amount of time 13 | duration delay_terminate # Delays terminating for this amount of time 14 | duration pause_status # Pauses the status messages for this amount of time 15 | --- 16 | int32 the_result 17 | bool is_simple_server 18 | --- 19 | -------------------------------------------------------------------------------- /actionlib/test/test_simple_action_server_deadlock_python.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /actionlib_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | actionlib_tools 4 | 1.14.3 5 | The actionlib_tools package 6 | 7 | Michael Carroll 8 | Jacob Perron 9 | BSD 10 | 11 | http://www.ros.org/wiki/actionlib 12 | https://github.com/ros/actionlib/issues 13 | https://github.com/ros/actionlib 14 | Eitan Marder-Eppstein 15 | Vijay Pradeep 16 | Mikael Arguedas 17 | 18 | catkin 19 | 20 | rospy 21 | rostopic 22 | roslib 23 | actionlib 24 | actionlib_msgs 25 | 26 | python3-wxgtk4.0 27 | 28 | 29 | -------------------------------------------------------------------------------- /actionlib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(actionlib) 3 | 4 | find_package(catkin REQUIRED COMPONENTS actionlib_msgs message_generation roscpp std_msgs) 5 | find_package(Boost REQUIRED COMPONENTS thread) 6 | 7 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 8 | link_directories(${catkin_LIBRARY_DIRS}) 9 | 10 | catkin_python_setup() 11 | 12 | add_action_files(DIRECTORY action FILES Test.action TestRequest.action TwoInts.action) 13 | generate_messages(DEPENDENCIES actionlib_msgs std_msgs) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS include 17 | LIBRARIES actionlib 18 | CATKIN_DEPENDS actionlib_msgs message_runtime roscpp std_msgs 19 | DEPENDS Boost 20 | ) 21 | 22 | add_library(actionlib src/connection_monitor.cpp src/goal_id_generator.cpp) 23 | target_link_libraries(actionlib ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 24 | add_dependencies(actionlib actionlib_gencpp) 25 | 26 | install(TARGETS ${PROJECT_NAME} 27 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 28 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 29 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 30 | install(DIRECTORY include/${PROJECT_NAME}/ 31 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 32 | FILES_MATCHING PATTERN "*.h") 33 | 34 | if(CATKIN_ENABLE_TESTING) 35 | find_package(rostest) 36 | add_subdirectory(test) 37 | endif() 38 | -------------------------------------------------------------------------------- /actionlib/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | actionlib 5 | 1.14.3 6 | 7 | The actionlib stack provides a standardized interface for 8 | interfacing with preemptable tasks. Examples of this include moving 9 | the base to a target location, performing a laser scan and returning 10 | the resulting point cloud, detecting the handle of a door, etc. 11 | 12 | Michael Carroll 13 | Jacob Perron 14 | BSD 15 | 16 | http://www.ros.org/wiki/actionlib 17 | https://github.com/ros/actionlib/issues 18 | https://github.com/ros/actionlib 19 | Eitan Marder-Eppstein 20 | Vijay Pradeep 21 | Mikael Arguedas 22 | 23 | catkin 24 | 25 | message_generation 26 | 27 | actionlib_msgs 28 | libboost-dev 29 | libboost-thread-dev 30 | roscpp 31 | rospy 32 | rostest 33 | std_msgs 34 | 35 | message_runtime 36 | 37 | rosnode 38 | rosunit 39 | 40 | -------------------------------------------------------------------------------- /actionlib/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \section ActionClientAPI ActionClient Code API 6 | - \link actionlib::SimpleActionClient SimpleActionClient (C++) \endlink 7 | - \link actionlib::simple_action_client::SimpleActionClient SimpleActionClient (Python) \endlink 8 | 9 | \section ActionServerAPI ActionServer Code API 10 | - actionlib::SimpleActionServer 11 | - \link actionlib::simple_action_server::SimpleActionServer SimpleActionServer (Python) \endlink 12 | 13 | \section protocol Communication Protocol 14 | The values for the status of a goal are as follows: 15 | - \b PENDING - The goal has yet to be processed by the action server 16 | - \b ACTIVE - The goal is currently being processed by the action server 17 | - \b REJECTED - The goal was rejected by the action server without being processed and without a request from the action client to cancel 18 | - \b SUCCEEDED - The goal was achieved successfully by the action server 19 | - \b ABORTED - The goal was aborted by the action server 20 | - \b PREEMPTING - Processing of the goal was canceled by either another goal, or a cancel request sent to the action server 21 | - \b PREEMPTED - The goal was preempted by either another goal, or a preempt message being sent to the action server 22 | - \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 23 | - \b RECALLED - The goal was canceled by either another goal, or a cancel request before the action server began processing the goal 24 | - \b LOST - The goal was sent by the ActionClient, but disappeared due to some communication error 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /actionlib/src/actionlib/exceptions.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2010, Willow Garage, Inc. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Willow Garage, Inc. nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | 28 | # Author: Jonathan Bohren 29 | 30 | 31 | class ActionException(Exception): 32 | pass 33 | -------------------------------------------------------------------------------- /actionlib/src/actionlib/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2009, Willow Garage, Inc. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Willow Garage, Inc. nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | 28 | from actionlib.action_client import * 29 | from actionlib.simple_action_client import * 30 | from actionlib.action_server import * 31 | from actionlib.simple_action_server import * 32 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/decl.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Willow Garage nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | /* 37 | * Cross platform macros. 38 | * 39 | */ 40 | #ifndef ACTIONLIB__DECL_H_ 41 | #define ACTIONLIB__DECL_H_ 42 | 43 | #include 44 | 45 | #ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries 46 | #ifdef actionlib_EXPORTS // we are building a shared lib/dll 47 | #define ACTIONLIB_DECL ROS_HELPER_EXPORT 48 | #else // we are using shared lib/dll 49 | #define ACTIONLIB_DECL ROS_HELPER_IMPORT 50 | #endif 51 | #else // ros is being built around static libraries 52 | #define ACTIONLIB_DECL 53 | #endif 54 | 55 | #endif // ACTIONLIB__DECL_H_ 56 | -------------------------------------------------------------------------------- /actionlib/src/actionlib/handle_tracker_deleter.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2009, Willow Garage, Inc. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Willow Garage, Inc. nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | 28 | # Author: Alexander Sorokin. 29 | # Based on C++ goal_id_generator.h/cpp 30 | import rospy 31 | 32 | 33 | class HandleTrackerDeleter: 34 | """ 35 | * @class HandleTrackerDeleter 36 | * @brief A class to help with tracking GoalHandles and removing goals 37 | * from the status list when the last GoalHandle associated with a given 38 | * goal is deleted. 39 | """ 40 | 41 | def __init__(self, action_server, status_tracker): 42 | """ 43 | @brief create deleter 44 | """ 45 | self.action_server = action_server 46 | self.status_tracker = status_tracker 47 | 48 | def __call__(self, ptr): 49 | if self.action_server: 50 | with self.action_server.lock: 51 | self.status_tracker.handle_destruction_time = rospy.Time.now() 52 | -------------------------------------------------------------------------------- /actionlib/test/simple_client_allocator_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | //! \author Vijay Pradeep 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | using namespace actionlib; 43 | 44 | TEST(SimpleClientAllocator, easy_test) { 45 | typedef actionlib::SimpleActionClient TrajClient; 46 | 47 | TrajClient * traj_client_ = new TrajClient("test_action", true); 48 | 49 | ros::Duration(1, 0).sleep(); 50 | 51 | delete traj_client_; 52 | } 53 | 54 | int main(int argc, char ** argv) 55 | { 56 | testing::InitGoogleTest(&argc, argv); 57 | 58 | ros::init(argc, argv, "simple_client_allocator"); 59 | 60 | return RUN_ALL_TESTS(); 61 | } 62 | -------------------------------------------------------------------------------- /actionlib/test/test_imports.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2009, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | # Author: Alexander Sorokin. 30 | 31 | 32 | PKG = 'actionlib' 33 | 34 | import sys 35 | import unittest 36 | 37 | 38 | # A simple unit test to make sure python module structure and files aren't broken 39 | class TestImports(unittest.TestCase): 40 | # import everything 41 | def test_imports(self): 42 | from actionlib import simple_action_client 43 | from actionlib import action_client 44 | 45 | from actionlib import goal_id_generator 46 | from actionlib import handle_tracker_deleter 47 | from actionlib import server_goal_handle 48 | from actionlib import status_tracker 49 | 50 | from actionlib import action_server 51 | from actionlib import simple_action_server 52 | 53 | self.assertEqual(1, 1, "1!=1") 54 | 55 | 56 | if __name__ == '__main__': 57 | import rostest 58 | print(sys.path) 59 | rostest.rosrun(PKG, 'test_imports', TestImports) 60 | -------------------------------------------------------------------------------- /actionlib_tools/src/actionlib_tools/dynamic_action.py: -------------------------------------------------------------------------------- 1 | # ********************************************************** 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2009, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of Willow Garage, Inc. nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Author: Eitan Marder-Eppstein 35 | # ********************************************************** 36 | import roslib 37 | import rospy 38 | import sys 39 | 40 | 41 | class DynamicAction(object): 42 | def __init__(self, name): 43 | # remove "Action" string from name 44 | assert("Action" in name) 45 | self.name = name[0:len(name)-6] 46 | self.action = self.load_submsg('Action') 47 | self.goal = self.load_submsg('Goal') 48 | self.feedback = self.load_submsg('Feedback') 49 | self.result = self.load_submsg('Result') 50 | 51 | def load_submsg(self, subname): 52 | msgclass = roslib.message.get_message_class(self.name + subname) 53 | if msgclass is None: 54 | rospy.logfatal('Could not load message for: %s' % (self.name + subname)) 55 | sys.exit(1) 56 | return msgclass 57 | -------------------------------------------------------------------------------- /actionlib/test/action_client_destruction_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | //! \author Vijay Pradeep 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | using namespace actionlib; 43 | 44 | TEST(ActionClientDestruction, persistent_goal_handles_1) { 45 | ActionClient * test_client = new ActionClient("test_action"); 46 | 47 | ClientGoalHandle gh = test_client->sendGoal(TestGoal()); 48 | 49 | ros::Duration(.1).sleep(); 50 | 51 | printf("Destroying ActionClient\n"); 52 | delete test_client; 53 | printf("Done Destroying ActionClient\n"); 54 | } 55 | 56 | int main(int argc, char ** argv) 57 | { 58 | testing::InitGoogleTest(&argc, argv); 59 | 60 | ros::init(argc, argv, "simple_client_allocator"); 61 | 62 | return RUN_ALL_TESTS(); 63 | } 64 | -------------------------------------------------------------------------------- /actionlib/test/add_two_ints_client.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #include 38 | #include 39 | #include 40 | 41 | int main(int argc, char ** argv) 42 | { 43 | ros::init(argc, argv, "add_two_ints_client"); 44 | if (argc != 3) { 45 | ROS_INFO_NAMED("actionlib", "Usage: add_two_ints_client X Y"); 46 | return 1; 47 | } 48 | 49 | ros::NodeHandle n; 50 | actionlib::ServiceClient client = actionlib::serviceClient(n, 51 | "add_two_ints"); 52 | client.waitForServer(); 53 | actionlib::TwoIntsGoal req; 54 | actionlib::TwoIntsResult resp; 55 | 56 | req.a = atoi(argv[1]); 57 | req.b = atoi(argv[2]); 58 | 59 | if (client.call(req, resp)) { 60 | ROS_INFO_NAMED("actionlib", "Sum: %ld", (int64_t)resp.sum); 61 | return 1; 62 | } 63 | 64 | return 0; 65 | } 66 | -------------------------------------------------------------------------------- /actionlib/test/test_cpp_simple_client_cancel_crash.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | //! \author Vijay Pradeep 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | typedef actionlib::SimpleActionClient Client; 43 | 44 | TEST(SimpleClientCancelCrash, uninitialized_crash) { 45 | ros::NodeHandle nh; 46 | Client client("test_client", true); 47 | ROS_INFO_NAMED("actionlib", "calling cancelGoal()"); 48 | client.cancelGoal(); 49 | ROS_INFO_NAMED("actionlib", "Done calling cancelGoal()"); 50 | EXPECT_TRUE(true); 51 | ROS_INFO_NAMED("actionlib", "Successfully done with test. Exiting"); 52 | } 53 | 54 | int main(int argc, char ** argv) 55 | { 56 | testing::InitGoogleTest(&argc, argv); 57 | 58 | ros::init(argc, argv, "test_cpp_simple_client_cancel_crash"); 59 | 60 | return RUN_ALL_TESTS(); 61 | } 62 | -------------------------------------------------------------------------------- /actionlib/test/add_two_ints_server.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #include 38 | #include 39 | #include 40 | 41 | bool add(const actionlib::TwoIntsGoal & req, actionlib::TwoIntsResult & res) 42 | { 43 | res.sum = req.a + req.b; 44 | ROS_INFO_NAMED("actionlib", "request: x=%ld, y=%ld", (int64_t)req.a, (int64_t)req.b); 45 | ROS_INFO_NAMED("actionlib", " sending back response: [%ld]", (int64_t)res.sum); 46 | return true; 47 | } 48 | 49 | int main(int argc, char ** argv) 50 | { 51 | ros::init(argc, argv, "add_two_ints_server"); 52 | ros::NodeHandle n; 53 | 54 | actionlib::ServiceServer service = actionlib::advertiseService(n, 55 | "add_two_ints", 56 | boost::bind(add, boost::placeholders::_1, boost::placeholders::_2)); 57 | 58 | ros::spin(); 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /actionlib/src/goal_id_generator.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | // using namespace actionlib; 41 | 42 | static boost::mutex s_goalcount_mutex_; 43 | static unsigned int s_goalcount_ = 0; 44 | 45 | actionlib::GoalIDGenerator::GoalIDGenerator() 46 | { 47 | setName(ros::this_node::getName()); 48 | } 49 | 50 | actionlib::GoalIDGenerator::GoalIDGenerator(const std::string & name) 51 | { 52 | setName(name); 53 | } 54 | 55 | void actionlib::GoalIDGenerator::setName(const std::string & name) 56 | { 57 | name_ = name; 58 | } 59 | 60 | actionlib_msgs::GoalID actionlib::GoalIDGenerator::generateID() 61 | { 62 | actionlib_msgs::GoalID id; 63 | ros::Time cur_time = ros::Time::now(); 64 | std::stringstream ss; 65 | 66 | ss << name_ << "-"; 67 | 68 | { 69 | boost::mutex::scoped_lock lock(s_goalcount_mutex_); 70 | s_goalcount_++; 71 | ss << s_goalcount_ << "-"; 72 | } 73 | 74 | ss << cur_time.sec << "." << cur_time.nsec; 75 | id.id = ss.str(); 76 | id.stamp = cur_time; 77 | return id; 78 | } 79 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/goal_id_generator.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ACTIONLIB__GOAL_ID_GENERATOR_H_ 36 | #define ACTIONLIB__GOAL_ID_GENERATOR_H_ 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | #include "ros/time.h" 44 | #include "actionlib_msgs/GoalID.h" 45 | 46 | namespace actionlib 47 | { 48 | 49 | class ACTIONLIB_DECL GoalIDGenerator 50 | { 51 | public: 52 | /** 53 | * Create a generator that prepends the fully qualified node name to the Goal ID 54 | */ 55 | GoalIDGenerator(); 56 | 57 | /** 58 | * \param name Unique name to prepend to the goal id. This will 59 | * generally be a fully qualified node name. 60 | */ 61 | GoalIDGenerator(const std::string & name); 62 | 63 | /** 64 | * \param name Set the name to prepend to the goal id. This will 65 | * generally be a fully qualified node name. 66 | */ 67 | void setName(const std::string & name); 68 | 69 | /** 70 | * \brief Generates a unique ID 71 | * \return A unique GoalID for this action 72 | */ 73 | actionlib_msgs::GoalID generateID(); 74 | 75 | private: 76 | std::string name_; 77 | }; 78 | 79 | } // namespace actionlib 80 | 81 | #endif // ACTIONLIB__GOAL_ID_GENERATOR_H_ 82 | -------------------------------------------------------------------------------- /actionlib/test/simple_python_client_test.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2009, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | import unittest 30 | import rospy 31 | from actionlib_msgs.msg import GoalStatus 32 | from actionlib import SimpleActionClient 33 | from actionlib.msg import TestAction, TestGoal 34 | 35 | 36 | class TestSimpleActionClient(unittest.TestCase): 37 | 38 | def testsimple(self): 39 | client = SimpleActionClient('reference_action', TestAction) 40 | self.assertTrue(client.wait_for_server(rospy.Duration(10.0)), 41 | 'Could not connect to the action server') 42 | 43 | goal = TestGoal(1) 44 | client.send_goal(goal) 45 | self.assertTrue(client.wait_for_result(rospy.Duration(10.0)), 46 | "Goal didn't finish") 47 | self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) 48 | self.assertEqual("The ref server has succeeded", client.get_goal_status_text()) 49 | 50 | goal = TestGoal(2) 51 | client.send_goal(goal) 52 | self.assertTrue(client.wait_for_result(rospy.Duration(10.0)), 53 | "Goal didn't finish") 54 | self.assertEqual(GoalStatus.ABORTED, client.get_state()) 55 | self.assertEqual("The ref server has aborted", client.get_goal_status_text()) 56 | 57 | 58 | if __name__ == '__main__': 59 | import rostest 60 | rospy.init_node('simple_python_client_test') 61 | rostest.rosrun('actionlib', 'test_simple_action_client_python', TestSimpleActionClient) 62 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/server/handle_tracker_deleter_imp.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef ACTIONLIB__SERVER__HANDLE_TRACKER_DELETER_IMP_H_ 38 | #define ACTIONLIB__SERVER__HANDLE_TRACKER_DELETER_IMP_H_ 39 | 40 | #include 41 | 42 | namespace actionlib 43 | { 44 | template 45 | HandleTrackerDeleter::HandleTrackerDeleter(ActionServerBase * as, 46 | typename std::list >::iterator status_it, 47 | boost::shared_ptr guard) 48 | : as_(as), status_it_(status_it), guard_(guard) {} 49 | 50 | template 51 | void HandleTrackerDeleter::operator()(void *) 52 | { 53 | if (as_) { 54 | // make sure that the action server hasn't been destroyed yet 55 | DestructionGuard::ScopedProtector protector(*guard_); 56 | if (protector.isProtected()) { 57 | // make sure to lock while we erase status for this goal from the list 58 | boost::recursive_mutex::scoped_lock lock(as_->lock_); 59 | (*status_it_).handle_destruction_time_ = ros::Time::now(); 60 | // as_->status_list_.erase(status_it_); 61 | } 62 | } 63 | } 64 | } // namespace actionlib 65 | #endif // ACTIONLIB__SERVER__HANDLE_TRACKER_DELETER_IMP_H_ 66 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/server/status_tracker_imp.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef ACTIONLIB__SERVER__STATUS_TRACKER_IMP_H_ 38 | #define ACTIONLIB__SERVER__STATUS_TRACKER_IMP_H_ 39 | namespace actionlib 40 | { 41 | template 42 | StatusTracker::StatusTracker(const actionlib_msgs::GoalID & goal_id, 43 | unsigned int status) 44 | { 45 | // set the goal id and status appropriately 46 | status_.goal_id = goal_id; 47 | status_.status = status; 48 | } 49 | 50 | template 51 | StatusTracker::StatusTracker(const boost::shared_ptr & goal) 52 | : goal_(goal) 53 | { 54 | // set the goal_id from the message 55 | status_.goal_id = goal_->goal_id; 56 | 57 | // initialize the status of the goal to pending 58 | status_.status = actionlib_msgs::GoalStatus::PENDING; 59 | 60 | // if the goal id is zero, then we need to make up an id for the goal 61 | if (status_.goal_id.id == "") { 62 | status_.goal_id = id_generator_.generateID(); 63 | } 64 | 65 | // if the timestamp of the goal is zero, then we'll set it to now() 66 | if (status_.goal_id.stamp == ros::Time()) { 67 | status_.goal_id.stamp = ros::Time::now(); 68 | } 69 | } 70 | } // namespace actionlib 71 | #endif // ACTIONLIB__SERVER__STATUS_TRACKER_IMP_H_ 72 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/server/status_tracker.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef ACTIONLIB__SERVER__STATUS_TRACKER_H_ 38 | #define ACTIONLIB__SERVER__STATUS_TRACKER_H_ 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | 46 | namespace actionlib 47 | { 48 | 49 | /** 50 | * @class StatusTracker 51 | * @brief A class for storing the status of each goal the action server 52 | * is currently working on 53 | */ 54 | template 55 | class StatusTracker 56 | { 57 | private: 58 | // generates typedefs that we'll use to make our lives easier 59 | ACTION_DEFINITION(ActionSpec) 60 | 61 | public: 62 | StatusTracker(const actionlib_msgs::GoalID & goal_id, unsigned int status); 63 | 64 | StatusTracker(const boost::shared_ptr & goal); 65 | 66 | boost::shared_ptr goal_; 67 | boost::weak_ptr handle_tracker_; 68 | actionlib_msgs::GoalStatus status_; 69 | ros::Time handle_destruction_time_; 70 | 71 | private: 72 | GoalIDGenerator id_generator_; 73 | }; 74 | } // namespace actionlib 75 | 76 | // include the implementation 77 | #include 78 | #endif // ACTIONLIB__SERVER__STATUS_TRACKER_H_ 79 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/one_shot_timer.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ACTIONLIB__ONE_SHOT_TIMER_H_ 36 | #define ACTIONLIB__ONE_SHOT_TIMER_H_ 37 | 38 | #include "ros/ros.h" 39 | #include "boost/bind/bind.hpp" 40 | 41 | //! Horrible hack until ROS Supports this (ROS Trac #1387) 42 | class OneShotTimer 43 | { 44 | public: 45 | OneShotTimer() 46 | : active_(false) {} 47 | 48 | void cb(const ros::TimerEvent & e) 49 | { 50 | if (active_) { 51 | active_ = false; 52 | 53 | if (callback_) { 54 | callback_(e); 55 | } else { 56 | ROS_ERROR_NAMED("actionlib", "Got a nullptr Timer OneShotTimer Callback"); 57 | } 58 | } 59 | } 60 | 61 | boost::function getCb() 62 | { 63 | return boost::bind(&OneShotTimer::cb, this, boost::placeholders::_1); 64 | } 65 | 66 | void registerOneShotCb(boost::function callback) 67 | { 68 | callback_ = callback; 69 | } 70 | 71 | void stop() 72 | { 73 | // timer_.stop(); 74 | active_ = false; 75 | } 76 | 77 | const ros::Timer & operator=(const ros::Timer & rhs) 78 | { 79 | active_ = true; 80 | timer_ = rhs; 81 | return timer_; 82 | } 83 | 84 | private: 85 | ros::Timer timer_; 86 | bool active_; 87 | boost::function callback_; 88 | }; 89 | 90 | 91 | #endif // ACTIONLIB__ONE_SHOT_TIMER_H_ 92 | -------------------------------------------------------------------------------- /actionlib/src/actionlib/goal_id_generator.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2009, Willow Garage, Inc. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Willow Garage, Inc. nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | 28 | # Author: Alexander Sorokin. 29 | # Based on C++ goal_id_generator.h/cpp 30 | import rospy 31 | 32 | from actionlib_msgs.msg import GoalID 33 | import threading 34 | 35 | global s_goalcount_lock 36 | global s_goalcount 37 | s_goalcount_lock = threading.Lock() 38 | s_goalcount = 0 39 | 40 | 41 | class GoalIDGenerator: 42 | 43 | def __init__(self, name=None): 44 | """ 45 | * Create a generator that prepends the fully qualified node name to the Goal ID 46 | * \param name Unique name to prepend to the goal id. This will 47 | * generally be a fully qualified node name. 48 | """ 49 | if name is not None: 50 | self.set_name(name) 51 | else: 52 | self.set_name(rospy.get_name()) 53 | 54 | def set_name(self, name): 55 | """ 56 | * \param name Set the name to prepend to the goal id. This will 57 | * generally be a fully qualified node name. 58 | """ 59 | self.name = name 60 | 61 | def generate_ID(self): 62 | """ 63 | * \brief Generates a unique ID 64 | * \return A unique GoalID for this action 65 | """ 66 | id = GoalID() 67 | cur_time = rospy.Time.now() 68 | ss = self.name + "-" 69 | global s_goalcount_lock 70 | global s_goalcount 71 | with s_goalcount_lock: 72 | s_goalcount += 1 73 | ss += str(s_goalcount) + "-" 74 | ss += str(cur_time.secs) + "." + str(cur_time.nsecs) 75 | 76 | id.id = ss 77 | id.stamp = cur_time 78 | return id 79 | -------------------------------------------------------------------------------- /actionlib/test/simple_action_server_deadlock_companion.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # 3 | # Copyright (c) 2013, Miguel Sarabia 4 | # Imperial College London 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above copyright 13 | # notice, this list of conditions and the following disclaimer in the 14 | # documentation and/or other materials provided with the distribution. 15 | # * Neither the name of Imperial College London nor the names of its 16 | # contributors may be used to endorse or promote products derived from 17 | # this software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | # 31 | 32 | 33 | class Constants: 34 | node = "simple_action_server_deadlock_companion" 35 | topic = "deadlock" 36 | max_action_duration = 3 37 | 38 | 39 | import random 40 | 41 | import actionlib 42 | from actionlib.msg import TestAction, TestGoal 43 | from actionlib_msgs.msg import GoalStatus 44 | import rospy 45 | 46 | 47 | class DeadlockCompanion: 48 | 49 | def __init__(self): 50 | # Seed random with fully resolved name of node and current time 51 | random.seed(rospy.get_name() + str(rospy.Time.now().to_sec())) 52 | 53 | # Create actionlib client 54 | self.action_client = actionlib.SimpleActionClient( 55 | Constants.topic, 56 | TestAction) 57 | 58 | def run(self): 59 | while not rospy.is_shutdown(): 60 | # Send dummy goal 61 | self.action_client.send_goal(TestGoal()) 62 | 63 | # Wait for a random amount of time 64 | action_duration = random.uniform(0, Constants.max_action_duration) 65 | self.action_client.wait_for_result(rospy.Duration(action_duration)) 66 | 67 | state = self.action_client.get_state() 68 | if state == GoalStatus.ACTIVE or state == GoalStatus.PENDING: 69 | self.action_client.cancel_goal() 70 | 71 | 72 | if __name__ == '__main__': 73 | rospy.init_node(Constants.node) 74 | try: 75 | companion = DeadlockCompanion() 76 | companion.run() 77 | except (KeyboardInterrupt, SystemExit): 78 | raise 79 | except: 80 | pass 81 | -------------------------------------------------------------------------------- /actionlib/src/actionlib/status_tracker.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2009, Willow Garage, Inc. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Willow Garage, Inc. nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | 28 | # Author: Alexander Sorokin. 29 | # Based on C++ goal_id_generator.h/cpp 30 | import rospy 31 | 32 | import actionlib_msgs.msg 33 | from actionlib import goal_id_generator 34 | 35 | 36 | class StatusTracker: 37 | """ 38 | * @class StatusTracker 39 | * @brief A class for storing the status of each goal the action server 40 | * is currently working on 41 | """ 42 | 43 | def __init__(self, goal_id=None, status=None, goal=None): 44 | """ 45 | @brief create status tracker. Either pass goal_id and status OR goal 46 | """ 47 | self.goal = None 48 | self.handle_tracker = None 49 | self.status = actionlib_msgs.msg.GoalStatus() 50 | 51 | self.handle_destruction_time = rospy.Time() 52 | 53 | self.id_generator = goal_id_generator.GoalIDGenerator() 54 | 55 | if goal_id: 56 | # set the goal id and status appropriately 57 | self.status.goal_id = goal_id 58 | self.status.status = status 59 | else: 60 | self.goal = goal 61 | self.status.goal_id = goal.goal_id 62 | 63 | # initialize the status of the goal to pending 64 | self.status.status = actionlib_msgs.msg.GoalStatus.PENDING 65 | 66 | # if the goal id is zero, then we need to make up an id for the goal 67 | if self.status.goal_id.id == "": 68 | self.status.goal_id = self.id_generator.generate_ID() 69 | 70 | # if the timestamp of the goal is zero, then we'll set it to now() 71 | if self.status.goal_id.stamp == rospy.Time(): 72 | self.status.goal_id.stamp = rospy.Time.now() 73 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/enclosure_deleter.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | 38 | #include 39 | 40 | #ifndef ACTIONLIB__ENCLOSURE_DELETER_H_ 41 | #define ACTIONLIB__ENCLOSURE_DELETER_H_ 42 | 43 | namespace actionlib 44 | { 45 | 46 | /* 47 | * This allows the creation of a shared pointer to a section 48 | * of an already reference counted structure. For example, 49 | * if in the following picture Enclosure is reference counted with 50 | * a boost::shared_ptr and you want to return a boost::shared_ptr 51 | * to the Member that is referenced counted along with Enclosure objects 52 | * 53 | * Enclosure --------------- <--- Already reference counted 54 | * -----Member <------- A member of enclosure objects, eg. Enclosure.Member 55 | */ 56 | template 57 | class EnclosureDeleter 58 | { 59 | public: 60 | EnclosureDeleter(const boost::shared_ptr & enc_ptr) 61 | : enc_ptr_(enc_ptr) {} 62 | 63 | template 64 | void operator()(Member *) 65 | { 66 | enc_ptr_.reset(); 67 | } 68 | 69 | private: 70 | boost::shared_ptr enc_ptr_; 71 | }; 72 | 73 | template 74 | boost::shared_ptr share_member(boost::shared_ptr enclosure, Member & member) 75 | { 76 | EnclosureDeleter d(enclosure); 77 | boost::shared_ptr p(&member, d); 78 | return p; 79 | } 80 | 81 | } // namespace actionlib 82 | 83 | #endif // ACTIONLIB__ENCLOSURE_DELETER_H_ 84 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/server/handle_tracker_deleter.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef ACTIONLIB__SERVER__HANDLE_TRACKER_DELETER_H_ 38 | #define ACTIONLIB__SERVER__HANDLE_TRACKER_DELETER_H_ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace actionlib 47 | { 48 | // we need to forward declare the ActionServerBase class 49 | template 50 | class ActionServerBase; 51 | 52 | /** 53 | * @class HandleTrackerDeleter 54 | * @brief A class to help with tracking GoalHandles and removing goals 55 | * from the status list when the last GoalHandle associated with a given 56 | * goal is deleted. 57 | */ 58 | // class to help with tracking status objects 59 | template 60 | class HandleTrackerDeleter 61 | { 62 | public: 63 | HandleTrackerDeleter(ActionServerBase * as, 64 | typename std::list >::iterator status_it, 65 | boost::shared_ptr guard); 66 | 67 | void operator()(void * ptr); 68 | 69 | private: 70 | ActionServerBase * as_; 71 | typename std::list >::iterator status_it_; 72 | boost::shared_ptr guard_; 73 | }; 74 | 75 | } // namespace actionlib 76 | #include 77 | #endif // ACTIONLIB__SERVER__HANDLE_TRACKER_DELETER_H_ 78 | -------------------------------------------------------------------------------- /actionlib/test/simple_client_wait_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | //! \author Vijay Pradeep 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | using namespace actionlib; 42 | 43 | TEST(SimpleClient, easy_wait_test) { 44 | ros::NodeHandle n; 45 | SimpleActionClient client(n, "reference_action"); 46 | 47 | bool started = client.waitForServer(ros::Duration(10.0)); 48 | ASSERT_TRUE(started); 49 | 50 | TestGoal goal; 51 | SimpleClientGoalState state(SimpleClientGoalState::LOST); 52 | 53 | goal.goal = 1; 54 | state = client.sendGoalAndWait(goal, ros::Duration(10, 0), ros::Duration(10, 0)); 55 | EXPECT_TRUE(state == SimpleClientGoalState::SUCCEEDED) << 56 | "Expected [SUCCEEDED], but goal state is [" << client.getState().toString() << "]"; 57 | 58 | goal.goal = 4; 59 | state = client.sendGoalAndWait(goal, ros::Duration(2, 0), ros::Duration(1, 0)); 60 | EXPECT_TRUE(state == SimpleClientGoalState::PREEMPTED) << 61 | "Expected [PREEMPTED], but goal state is [" << client.getState().toString() << "]"; 62 | } 63 | 64 | void spinThread() 65 | { 66 | ros::NodeHandle nh; 67 | ros::spin(); 68 | } 69 | 70 | int main(int argc, char ** argv) 71 | { 72 | testing::InitGoogleTest(&argc, argv); 73 | 74 | ros::init(argc, argv, "simple_client_test"); 75 | 76 | boost::thread spin_thread(&spinThread); 77 | 78 | int result = RUN_ALL_TESTS(); 79 | 80 | ros::shutdown(); 81 | 82 | spin_thread.join(); 83 | 84 | return result; 85 | } 86 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/action_definition.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef ACTIONLIB__ACTION_DEFINITION_H_ 38 | #define ACTIONLIB__ACTION_DEFINITION_H_ 39 | // A macro that will generate helpful typedefs for action client, server, and policy implementers 40 | namespace actionlib 41 | { 42 | #define ACTION_DEFINITION(ActionSpec) \ 43 | typedef typename ActionSpec::_action_goal_type ActionGoal; \ 44 | typedef typename ActionGoal::_goal_type Goal; \ 45 | typedef typename ActionSpec::_action_result_type ActionResult; \ 46 | typedef typename ActionResult::_result_type Result; \ 47 | typedef typename ActionSpec::_action_feedback_type ActionFeedback; \ 48 | typedef typename ActionFeedback::_feedback_type Feedback; \ 49 | \ 50 | typedef boost::shared_ptr ActionGoalConstPtr; \ 51 | typedef boost::shared_ptr ActionGoalPtr; \ 52 | typedef boost::shared_ptr GoalConstPtr; \ 53 | typedef boost::shared_ptr GoalPtr; \ 54 | \ 55 | typedef boost::shared_ptr ActionResultConstPtr; \ 56 | typedef boost::shared_ptr ActionResultPtr; \ 57 | typedef boost::shared_ptr ResultConstPtr; \ 58 | typedef boost::shared_ptr ResultPtr; \ 59 | \ 60 | typedef boost::shared_ptr ActionFeedbackConstPtr; \ 61 | typedef boost::shared_ptr ActionFeedbackPtr; \ 62 | typedef boost::shared_ptr FeedbackConstPtr; \ 63 | typedef boost::shared_ptr FeedbackPtr; 64 | } // namespace actionlib 65 | #endif // ACTIONLIB__ACTION_DEFINITION_H_ 66 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/server/service_server_imp.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef ACTIONLIB__SERVER__SERVICE_SERVER_IMP_H_ 38 | #define ACTIONLIB__SERVER__SERVICE_SERVER_IMP_H_ 39 | 40 | #include 41 | 42 | namespace actionlib 43 | { 44 | 45 | template 46 | ServiceServer advertiseService(ros::NodeHandle n, std::string name, 47 | boost::function service_cb) 49 | { 50 | boost::shared_ptr server_ptr(new ServiceServerImpT(n, name, 51 | service_cb)); 52 | return ServiceServer(server_ptr); 53 | } 54 | 55 | template 56 | ServiceServerImpT::ServiceServerImpT(ros::NodeHandle n, std::string name, 57 | boost::function service_cb) 58 | : service_cb_(service_cb) 59 | { 60 | as_ = boost::shared_ptr >(new ActionServer(n, name, 61 | boost::bind(&ServiceServerImpT::goalCB, this, boost::placeholders::_1), false)); 62 | as_->start(); 63 | } 64 | 65 | template 66 | void ServiceServerImpT::goalCB(GoalHandle goal) 67 | { 68 | goal.setAccepted("This goal has been accepted by the service server"); 69 | 70 | // we need to pass the result into the users callback 71 | Result r; 72 | if (service_cb_(*(goal.getGoal()), r)) { 73 | goal.setSucceeded(r, "The service server successfully processed the request"); 74 | } else { 75 | goal.setAborted(r, "The service server failed to process the request"); 76 | } 77 | } 78 | 79 | } // namespace actionlib 80 | #endif // ACTIONLIB__SERVER__SERVICE_SERVER_IMP_H_ 81 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/server/service_server.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef ACTIONLIB__SERVER__SERVICE_SERVER_H_ 38 | #define ACTIONLIB__SERVER__SERVICE_SERVER_H_ 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | namespace actionlib 46 | { 47 | 48 | class ServiceServerImp 49 | { 50 | public: 51 | ServiceServerImp() {} 52 | virtual ~ServiceServerImp() {} 53 | }; 54 | 55 | class ServiceServer 56 | { 57 | public: 58 | ServiceServer(boost::shared_ptr server) 59 | : server_(server) {} 60 | 61 | private: 62 | boost::shared_ptr server_; 63 | }; 64 | 65 | template 66 | ServiceServer advertiseService(ros::NodeHandle n, std::string name, 67 | boost::function service_cb); 69 | 70 | template 71 | class ServiceServerImpT : public ServiceServerImp 72 | { 73 | public: 74 | // generates typedefs that we'll use to make our lives easier 75 | ACTION_DEFINITION(ActionSpec) 76 | 77 | typedef typename ActionServer::GoalHandle GoalHandle; 78 | 79 | ServiceServerImpT(ros::NodeHandle n, std::string name, 80 | boost::function service_cb); 81 | void goalCB(GoalHandle g); 82 | 83 | private: 84 | boost::shared_ptr > as_; 85 | boost::function service_cb_; 86 | }; 87 | 88 | } // namespace actionlib 89 | 90 | // include the implementation 91 | #include 92 | #endif // ACTIONLIB__SERVER__SERVICE_SERVER_H_ 93 | -------------------------------------------------------------------------------- /actionlib/test/ref_simple_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2009, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | # Author: Alexander Sorokin. 30 | # Based on code from ref_server.cpp by Vijay Pradeep 31 | PKG = 'actionlib' 32 | import rospy 33 | 34 | from actionlib.simple_action_server import SimpleActionServer 35 | from actionlib.msg import TestAction, TestFeedback 36 | 37 | 38 | class RefSimpleServer (SimpleActionServer): 39 | 40 | def __init__(self, name): 41 | action_spec = TestAction 42 | SimpleActionServer.__init__( 43 | self, name, action_spec, self.goal_callback, False) 44 | self.start() 45 | rospy.loginfo("Creating SimpleActionServer [%s]\n", name) 46 | 47 | def goal_callback(self, goal): 48 | rospy.loginfo("Got goal %d", int(goal.goal)) 49 | if goal.goal == 1: 50 | self.set_succeeded(None, "The ref server has succeeded") 51 | elif goal.goal == 2: 52 | self.set_aborted(None, "The ref server has aborted") 53 | elif goal.goal == 3: 54 | self.set_aborted(None, "The simple action server can't reject goals") 55 | elif goal.goal == 4: 56 | self.set_aborted(None, "Simple server can't save goals") 57 | elif goal.goal == 5: 58 | self.set_aborted(None, "Simple server can't save goals") 59 | elif goal.goal == 6: 60 | self.set_aborted(None, "Simple server can't save goals") 61 | elif goal.goal == 7: 62 | self.set_aborted(None, "Simple server can't save goals") 63 | elif goal.goal == 8: 64 | self.set_aborted(None, "Simple server can't save goals") 65 | elif goal.goal == 9: 66 | rospy.sleep(1) 67 | rospy.loginfo("Sending feedback") 68 | self.publish_feedback(TestFeedback(9)) # by the goal ID 69 | rospy.sleep(1) 70 | self.set_succeeded(None, "The ref server has succeeded") 71 | else: 72 | pass 73 | 74 | 75 | if __name__ == "__main__": 76 | rospy.init_node("ref_simple_server") 77 | ref_server = RefSimpleServer("reference_simple_action") 78 | 79 | rospy.spin() 80 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/client/simple_goal_state.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ACTIONLIB__CLIENT__SIMPLE_GOAL_STATE_H_ 36 | #define ACTIONLIB__CLIENT__SIMPLE_GOAL_STATE_H_ 37 | 38 | #include 39 | #include "ros/console.h" 40 | 41 | namespace actionlib 42 | { 43 | 44 | /** 45 | * \brief Thin wrapper around an enum in order providing a simplified version of the 46 | * communication state, but with less states than CommState 47 | **/ 48 | class SimpleGoalState 49 | { 50 | public: 51 | //! \brief Defines the various states the SimpleGoalState can be in 52 | enum StateEnum 53 | { 54 | PENDING, 55 | ACTIVE, 56 | DONE 57 | }; 58 | 59 | SimpleGoalState(const StateEnum & state) 60 | : state_(state) {} 61 | 62 | inline bool operator==(const SimpleGoalState & rhs) const 63 | { 64 | return state_ == rhs.state_; 65 | } 66 | 67 | inline bool operator==(const SimpleGoalState::StateEnum & rhs) const 68 | { 69 | return state_ == rhs; 70 | } 71 | 72 | inline bool operator!=(const SimpleGoalState::StateEnum & rhs) const 73 | { 74 | return !(*this == rhs); 75 | } 76 | 77 | inline bool operator!=(const SimpleGoalState & rhs) const 78 | { 79 | return !(*this == rhs); 80 | } 81 | 82 | std::string toString() const 83 | { 84 | switch (state_) { 85 | case PENDING: 86 | return "PENDING"; 87 | case ACTIVE: 88 | return "ACTIVE"; 89 | case DONE: 90 | return "DONE"; 91 | default: 92 | ROS_ERROR_NAMED("actionlib", "BUG: Unhandled SimpleGoalState: %u", state_); 93 | break; 94 | } 95 | return "BUG-UNKNOWN"; 96 | } 97 | 98 | StateEnum state_; 99 | 100 | private: 101 | SimpleGoalState(); 102 | }; 103 | 104 | } // namespace actionlib 105 | 106 | #endif // ACTIONLIB__CLIENT__SIMPLE_GOAL_STATE_H_ 107 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/client/connection_monitor.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ACTIONLIB__CLIENT__CONNECTION_MONITOR_H_ 36 | #define ACTIONLIB__CLIENT__CONNECTION_MONITOR_H_ 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | 50 | namespace actionlib 51 | { 52 | 53 | class ACTIONLIB_DECL ConnectionMonitor 54 | { 55 | public: 56 | ConnectionMonitor(ros::Subscriber & feedback_sub, ros::Subscriber & result_sub); 57 | 58 | void goalConnectCallback(const ros::SingleSubscriberPublisher & pub); 59 | 60 | void goalDisconnectCallback(const ros::SingleSubscriberPublisher & pub); 61 | 62 | void cancelConnectCallback(const ros::SingleSubscriberPublisher & pub); 63 | 64 | void cancelDisconnectCallback(const ros::SingleSubscriberPublisher & pub); 65 | 66 | void processStatus(const actionlib_msgs::GoalStatusArrayConstPtr & status, 67 | const std::string & cur_status_caller_id); 68 | 69 | bool waitForActionServerToStart(const ros::Duration & timeout = ros::Duration(0, 0), 70 | const ros::NodeHandle & nh = ros::NodeHandle() ); 71 | bool isServerConnected(); 72 | 73 | private: 74 | // status stuff 75 | std::string status_caller_id_; 76 | bool status_received_; 77 | ros::Time latest_status_time_; 78 | 79 | boost::condition check_connection_condition_; 80 | 81 | boost::recursive_mutex data_mutex_; 82 | std::map goalSubscribers_; 83 | std::map cancelSubscribers_; 84 | 85 | std::string goalSubscribersString(); 86 | std::string cancelSubscribersString(); 87 | 88 | ros::Subscriber & feedback_sub_; 89 | ros::Subscriber & result_sub_; 90 | }; 91 | 92 | } // namespace actionlib 93 | 94 | #endif // ACTIONLIB__CLIENT__CONNECTION_MONITOR_H_ 95 | -------------------------------------------------------------------------------- /actionlib/test/ref_server.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | //! \author Vijay Pradeep 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace actionlib 44 | { 45 | 46 | class RefServer : public ActionServer 47 | { 48 | public: 49 | typedef ServerGoalHandle GoalHandle; 50 | 51 | RefServer(ros::NodeHandle & n, const std::string & name); 52 | 53 | private: 54 | void goalCallback(GoalHandle gh); 55 | void cancelCallback(GoalHandle gh); 56 | }; 57 | 58 | } // namespace actionlib 59 | 60 | using namespace actionlib; 61 | 62 | RefServer::RefServer(ros::NodeHandle & n, const std::string & name) 63 | : ActionServer(n, name, 64 | boost::bind(&RefServer::goalCallback, this, boost::placeholders::_1), 65 | boost::bind(&RefServer::cancelCallback, this, boost::placeholders::_1), 66 | false) 67 | { 68 | start(); 69 | printf("Creating ActionServer [%s]\n", name.c_str()); 70 | } 71 | 72 | void RefServer::goalCallback(GoalHandle gh) 73 | { 74 | TestGoal goal = *gh.getGoal(); 75 | 76 | switch (goal.goal) { 77 | case 1: 78 | gh.setAccepted(); 79 | gh.setSucceeded(TestResult(), "The ref server has succeeded"); 80 | break; 81 | case 2: 82 | gh.setAccepted(); 83 | gh.setAborted(TestResult(), "The ref server has aborted"); 84 | break; 85 | case 3: 86 | gh.setRejected(TestResult(), "The ref server has rejected"); 87 | break; 88 | default: 89 | break; 90 | } 91 | } 92 | 93 | void RefServer::cancelCallback(GoalHandle) 94 | { 95 | } 96 | 97 | 98 | int main(int argc, char ** argv) 99 | { 100 | ros::init(argc, argv, "ref_server"); 101 | 102 | ros::NodeHandle nh; 103 | 104 | RefServer ref_server(nh, "reference_action"); 105 | 106 | ros::spin(); 107 | } 108 | -------------------------------------------------------------------------------- /actionlib/test/test_ref_simple_action_server.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2009, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | PKG = 'actionlib' 29 | 30 | import unittest 31 | import rospy 32 | from actionlib_msgs.msg import GoalStatus 33 | from actionlib import SimpleActionClient 34 | from actionlib import ActionClient 35 | from actionlib.msg import TestAction, TestGoal 36 | 37 | 38 | class TestRefSimpleActionServer(unittest.TestCase): 39 | 40 | def test_one(self): 41 | client = SimpleActionClient('reference_simple_action', TestAction) 42 | self.assertTrue(client.wait_for_server(rospy.Duration(2.0)), 43 | 'Could not connect to the action server') 44 | 45 | goal = TestGoal(1) 46 | client.send_goal(goal) 47 | self.assertTrue(client.wait_for_result(rospy.Duration(2.0)), 48 | "Goal didn't finish") 49 | self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) 50 | 51 | goal = TestGoal(2) 52 | client.send_goal(goal) 53 | self.assertTrue(client.wait_for_result(rospy.Duration(10.0)), 54 | "Goal didn't finish") 55 | self.assertEqual(GoalStatus.ABORTED, client.get_state()) 56 | 57 | goal = TestGoal(3) 58 | client.send_goal(goal) 59 | self.assertTrue(client.wait_for_result(rospy.Duration(10.0)), 60 | "Goal didn't finish") 61 | 62 | # The simple server can't reject goals 63 | self.assertEqual(GoalStatus.ABORTED, client.get_state()) 64 | 65 | goal = TestGoal(9) 66 | saved_feedback = {} 67 | 68 | def on_feedback(fb): 69 | rospy.loginfo("Got feedback") 70 | saved_feedback[0] = fb 71 | 72 | client.send_goal(goal, feedback_cb=on_feedback) 73 | self.assertTrue(client.wait_for_result(rospy.Duration(10.0)), 74 | "Goal didn't finish") 75 | self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) 76 | 77 | self.assertEqual(saved_feedback[0].feedback, 9) 78 | 79 | 80 | if __name__ == '__main__': 81 | import rostest 82 | rospy.init_node('test_ref_simple_action_server') 83 | rostest.rosrun('actionlib', 'test_simple_action_client_python', TestRefSimpleActionServer) 84 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/client/terminal_state.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ACTIONLIB__CLIENT__TERMINAL_STATE_H_ 36 | #define ACTIONLIB__CLIENT__TERMINAL_STATE_H_ 37 | 38 | #include 39 | #include "ros/console.h" 40 | 41 | namespace actionlib 42 | { 43 | 44 | class TerminalState 45 | { 46 | public: 47 | enum StateEnum 48 | { 49 | RECALLED, 50 | REJECTED, 51 | PREEMPTED, 52 | ABORTED, 53 | SUCCEEDED, 54 | LOST 55 | }; 56 | 57 | TerminalState(const StateEnum & state, const std::string & text = std::string("")) 58 | : state_(state), text_(text) {} 59 | 60 | inline bool operator==(const TerminalState & rhs) const 61 | { 62 | return state_ == rhs.state_; 63 | } 64 | 65 | inline bool operator==(const TerminalState::StateEnum & rhs) const 66 | { 67 | return state_ == rhs; 68 | } 69 | 70 | inline bool operator!=(const TerminalState::StateEnum & rhs) const 71 | { 72 | return !(*this == rhs); 73 | } 74 | 75 | inline bool operator!=(const TerminalState & rhs) const 76 | { 77 | return !(*this == rhs); 78 | } 79 | 80 | std::string getText() const 81 | { 82 | return text_; 83 | } 84 | 85 | std::string toString() const 86 | { 87 | switch (state_) { 88 | case RECALLED: 89 | return "RECALLED"; 90 | case REJECTED: 91 | return "REJECTED"; 92 | case PREEMPTED: 93 | return "PREEMPTED"; 94 | case ABORTED: 95 | return "ABORTED"; 96 | case SUCCEEDED: 97 | return "SUCCEEDED"; 98 | case LOST: 99 | return "LOST"; 100 | 101 | default: 102 | ROS_ERROR_NAMED("actionlib", "BUG: Unhandled TerminalState: %u", state_); 103 | break; 104 | } 105 | return "BUG-UNKNOWN"; 106 | } 107 | 108 | StateEnum state_; 109 | std::string text_; 110 | 111 | private: 112 | TerminalState(); 113 | }; 114 | 115 | } // namespace actionlib 116 | 117 | #endif // ACTIONLIB__CLIENT__TERMINAL_STATE_H_ 118 | -------------------------------------------------------------------------------- /actionlib/test/simple_execute_ref_server.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | //! \author Vijay Pradeep 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | namespace actionlib 42 | { 43 | 44 | class SimpleExecuteRefServer 45 | { 46 | public: 47 | typedef ServerGoalHandle GoalHandle; 48 | 49 | SimpleExecuteRefServer(); 50 | 51 | private: 52 | ros::NodeHandle nh_; 53 | SimpleActionServer as_; 54 | 55 | void executeCallback(const TestGoalConstPtr & goal); 56 | }; 57 | 58 | } // namespace actionlib 59 | 60 | using namespace actionlib; 61 | 62 | SimpleExecuteRefServer::SimpleExecuteRefServer() 63 | : as_(nh_, "reference_action", boost::bind(&SimpleExecuteRefServer::executeCallback, this, 64 | boost::placeholders::_1), false) 65 | { 66 | as_.start(); 67 | } 68 | 69 | void SimpleExecuteRefServer::executeCallback(const TestGoalConstPtr & goal) 70 | { 71 | ROS_DEBUG_NAMED("actionlib", "Got a goal of type [%u]", goal->goal); 72 | switch (goal->goal) { 73 | case 1: 74 | ROS_DEBUG_NAMED("actionlib", "Got goal #1"); 75 | as_.setSucceeded(TestResult(), "The ref server has succeeded"); 76 | break; 77 | case 2: 78 | ROS_DEBUG_NAMED("actionlib", "Got goal #2"); 79 | as_.setAborted(TestResult(), "The ref server has aborted"); 80 | break; 81 | case 4: 82 | { 83 | ROS_DEBUG_NAMED("actionlib", "Got goal #4"); 84 | ros::Duration sleep_dur(.1); 85 | for (unsigned int i = 0; i < 100; i++) { 86 | sleep_dur.sleep(); 87 | if (as_.isPreemptRequested()) { 88 | as_.setPreempted(); 89 | return; 90 | } 91 | } 92 | as_.setAborted(); 93 | break; 94 | } 95 | default: 96 | break; 97 | } 98 | } 99 | 100 | int main(int argc, char ** argv) 101 | { 102 | ros::init(argc, argv, "ref_server"); 103 | 104 | SimpleExecuteRefServer server; 105 | 106 | ros::spin(); 107 | 108 | return 0; 109 | } 110 | -------------------------------------------------------------------------------- /actionlib/test/simple_action_server_construction_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2021, Smart Robotics BV. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | //! \author Ramon Wijnands 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #pragma GCC diagnostic ignored "-Wdeprecated-declarations" 43 | 44 | using namespace actionlib; 45 | 46 | TEST(SimpleActionServerConstruction, test_name_cb_autostart) { 47 | SimpleActionServer::ExecuteCallback callback = [](const TestGoalConstPtr&){}; 48 | SimpleActionServer as("name", callback, false); 49 | } 50 | 51 | TEST(SimpleActionServerConstruction, test_name_autostart) { 52 | SimpleActionServer as("name", false); 53 | } 54 | 55 | TEST(SimpleActionServerConstruction, test_name) { 56 | SimpleActionServer as("name"); 57 | } 58 | 59 | TEST(SimpleActionServerConstruction, test_name_cb) { 60 | SimpleActionServer::ExecuteCallback callback = [](const TestGoalConstPtr&){}; 61 | SimpleActionServer as("name", callback); 62 | } 63 | 64 | TEST(SimpleActionServerConstruction, test_nh_name_cb_autostart) { 65 | ros::NodeHandle nh; 66 | SimpleActionServer::ExecuteCallback callback = [](const TestGoalConstPtr&){}; 67 | SimpleActionServer as(nh, "name", callback, false); 68 | } 69 | 70 | TEST(SimpleActionServerConstruction, test_nh_name_autostart) { 71 | ros::NodeHandle nh; 72 | SimpleActionServer as(nh, "name", false); 73 | } 74 | 75 | TEST(SimpleActionServerConstruction, test_nh_name) { 76 | ros::NodeHandle nh; 77 | SimpleActionServer as(nh, "name"); 78 | } 79 | 80 | TEST(SimpleActionServerConstruction, test_nh_name_cb) { 81 | ros::NodeHandle nh; 82 | SimpleActionServer::ExecuteCallback callback = [](const TestGoalConstPtr&){}; 83 | SimpleActionServer as(nh, "name", callback); 84 | } 85 | 86 | int main(int argc, char **argv) { 87 | testing::InitGoogleTest(&argc, argv); 88 | 89 | ros::init(argc, argv, "simple_action_server_construction"); 90 | 91 | return RUN_ALL_TESTS(); 92 | } 93 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/client/service_client.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | 38 | #ifndef ACTIONLIB__CLIENT__SERVICE_CLIENT_H_ 39 | #define ACTIONLIB__CLIENT__SERVICE_CLIENT_H_ 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | 46 | namespace actionlib 47 | { 48 | 49 | class ServiceClientImp 50 | { 51 | public: 52 | ServiceClientImp() {} 53 | virtual bool call(const void * goal, std::string goal_md5sum, void * result, 54 | std::string result_md5sum) = 0; 55 | virtual bool waitForServer(const ros::Duration & timeout) = 0; 56 | virtual bool isServerConnected() = 0; 57 | virtual ~ServiceClientImp() {} 58 | }; 59 | 60 | class ServiceClient 61 | { 62 | public: 63 | ServiceClient(boost::shared_ptr client) 64 | : client_(client) {} 65 | 66 | template 67 | bool call(const Goal & goal, Result & result); 68 | 69 | bool waitForServer(const ros::Duration & timeout = ros::Duration(0, 0)); 70 | bool isServerConnected(); 71 | 72 | private: 73 | boost::shared_ptr client_; 74 | }; 75 | 76 | template 77 | ServiceClient serviceClient(ros::NodeHandle n, std::string name); 78 | 79 | template 80 | class ServiceClientImpT : public ServiceClientImp 81 | { 82 | public: 83 | ACTION_DEFINITION(ActionSpec) 84 | typedef ClientGoalHandle GoalHandleT; 85 | typedef SimpleActionClient SimpleActionClientT; 86 | 87 | ServiceClientImpT(ros::NodeHandle n, std::string name); 88 | 89 | bool call(const void * goal, std::string goal_md5sum, void * result, std::string result_md5sum); 90 | bool waitForServer(const ros::Duration & timeout); 91 | bool isServerConnected(); 92 | 93 | private: 94 | boost::scoped_ptr ac_; 95 | }; 96 | } // namespace actionlib 97 | 98 | // include the implementation 99 | #include 100 | #endif // ACTIONLIB__CLIENT__SERVICE_CLIENT_H_ 101 | -------------------------------------------------------------------------------- /actionlib/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(GTEST_FOUND) 2 | include_directories(${GTEST_INCLUDE_DIRS}) 3 | link_directories(${GTEST_LIBRARY_DIRS}) 4 | 5 | add_executable(actionlib-add_two_ints_server EXCLUDE_FROM_ALL add_two_ints_server.cpp) 6 | target_link_libraries(actionlib-add_two_ints_server ${PROJECT_NAME} ${GTEST_LIBRARIES}) 7 | 8 | add_executable(actionlib-ref_server EXCLUDE_FROM_ALL ref_server.cpp) 9 | target_link_libraries(actionlib-ref_server ${PROJECT_NAME}) 10 | 11 | add_executable(actionlib-simple_client_test EXCLUDE_FROM_ALL simple_client_test.cpp) 12 | target_link_libraries(actionlib-simple_client_test ${PROJECT_NAME} ${GTEST_LIBRARIES}) 13 | 14 | add_executable(actionlib-simple_execute_ref_server EXCLUDE_FROM_ALL simple_execute_ref_server.cpp) 15 | target_link_libraries(actionlib-simple_execute_ref_server ${PROJECT_NAME}) 16 | 17 | add_executable(actionlib-server_goal_handle_destruction EXCLUDE_FROM_ALL server_goal_handle_destruction.cpp) 18 | target_link_libraries(actionlib-server_goal_handle_destruction ${PROJECT_NAME} ${GTEST_LIBRARIES}) 19 | 20 | add_executable(actionlib-simple_client_wait_test EXCLUDE_FROM_ALL simple_client_wait_test.cpp) 21 | target_link_libraries(actionlib-simple_client_wait_test ${PROJECT_NAME} ${GTEST_LIBRARIES}) 22 | 23 | add_executable(actionlib-simple_client_allocator_test EXCLUDE_FROM_ALL simple_client_allocator_test.cpp) 24 | target_link_libraries(actionlib-simple_client_allocator_test ${PROJECT_NAME} ${GTEST_LIBRARIES}) 25 | 26 | add_executable(actionlib-simple_action_server_construction_test EXCLUDE_FROM_ALL simple_action_server_construction_test.cpp) 27 | target_link_libraries(actionlib-simple_action_server_construction_test ${PROJECT_NAME} ${GTEST_LIBRARIES}) 28 | 29 | add_executable(actionlib-action_client_destruction_test EXCLUDE_FROM_ALL action_client_destruction_test.cpp) 30 | target_link_libraries(actionlib-action_client_destruction_test ${PROJECT_NAME} ${GTEST_LIBRARIES}) 31 | 32 | add_executable(actionlib-test_cpp_simple_client_cancel_crash EXCLUDE_FROM_ALL test_cpp_simple_client_cancel_crash.cpp) 33 | target_link_libraries(actionlib-test_cpp_simple_client_cancel_crash ${PROJECT_NAME} ${GTEST_LIBRARIES}) 34 | 35 | add_executable(actionlib-exercise_simple_client EXCLUDE_FROM_ALL exercise_simple_client.cpp) 36 | target_link_libraries(actionlib-exercise_simple_client ${PROJECT_NAME} ${GTEST_LIBRARIES}) 37 | 38 | if(TARGET tests) 39 | add_dependencies(tests 40 | actionlib-add_two_ints_server 41 | actionlib-ref_server 42 | actionlib-simple_client_test 43 | actionlib-simple_execute_ref_server 44 | actionlib-server_goal_handle_destruction 45 | actionlib-simple_client_wait_test 46 | actionlib-simple_client_allocator_test 47 | actionlib-simple_action_server_construction_test 48 | actionlib-action_client_destruction_test 49 | actionlib-test_cpp_simple_client_cancel_crash 50 | actionlib-exercise_simple_client 51 | ) 52 | endif() 53 | endif() 54 | 55 | add_rostest(ref_server_test.launch) 56 | add_rostest(simple_execute_ref_server_test.launch) 57 | add_rostest(test_python_simple_client.launch) 58 | add_rostest(test_cpp_simple_client_allocator.launch) 59 | add_rostest(test_cpp_action_client_destruction.launch) 60 | add_rostest(test_server_goal_handle_destruction.launch) 61 | add_rostest(test_cpp_simple_client_cancel_crash.launch) 62 | add_rostest(test_imports.launch) 63 | add_rostest(test_python_server_components.launch) 64 | add_rostest(test_python_server.launch) 65 | add_rostest(test_python_server2.launch) 66 | add_rostest(test_python_server3.launch) 67 | add_rostest(test_python_simple_server.launch) 68 | add_rostest(test_cpp_exercise_simple_client.launch) 69 | add_rostest(test_python_exercise_simple_client.launch) 70 | add_rostest(test_simple_action_server_deadlock_python.launch) 71 | 72 | catkin_add_gtest(actionlib-destruction_guard_test destruction_guard_test.cpp) 73 | if(TARGET actionlib-destruction_guard_test) 74 | target_link_libraries(actionlib-destruction_guard_test ${PROJECT_NAME}) 75 | endif() 76 | -------------------------------------------------------------------------------- /actionlib_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package actionlib_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.14.3 (2025-05-09) 6 | ------------------- 7 | 8 | 1.14.2 (2025-04-25) 9 | ------------------- 10 | 11 | 1.14.1 (2025-04-10) 12 | ------------------- 13 | 14 | 1.14.0 (2022-12-09) 15 | ------------------- 16 | * Improve axclient argument parsing (`#189 `_) 17 | * Add maintainer (`#174 `_) 18 | * Contributors: Jacob Perron, Mikael Arguedas 19 | 20 | 1.13.2 (2020-08-21) 21 | ------------------- 22 | * Port actionlib_tools to Python 3 (`#169 `_) 23 | * Address RVD`#2401 `_ (`#171 `_) 24 | * Contributors: Mikael Arguedas, Víctor Mayoral Vilches 25 | 26 | 27 | 1.13.1 (2020-05-19) 28 | ------------------- 29 | * Remove scripts using wx (`#166 `_) 30 | * Contributors: Shane Loretz 31 | 32 | 1.13.0 (2020-03-03) 33 | ------------------- 34 | * Fix syntax error; missing : (`#159 `_) 35 | * bump CMake minimum version to use new behavior of CMP0048 (`#158 `_) 36 | * Split actionlib and tools into separate packages (`#152 `_) 37 | * Contributors: Michael Carroll, Shane Loretz, jschleicher 38 | 39 | 1.12.0 (2019-08-07) 40 | ------------------- 41 | 42 | 1.11.15 (2018-08-17) 43 | -------------------- 44 | 45 | 1.11.14 (2018-05-21) 46 | -------------------- 47 | 48 | 1.11.13 (2018-03-14) 49 | -------------------- 50 | 51 | 1.11.12 (2017-12-18) 52 | -------------------- 53 | 54 | 1.11.11 (2017-10-27) 55 | -------------------- 56 | 57 | 1.11.10 (2017-07-27) 58 | -------------------- 59 | 60 | 1.11.9 (2017-03-27) 61 | ------------------- 62 | 63 | 1.11.8 (2017-02-17) 64 | ------------------- 65 | 66 | 1.11.7 (2016-10-24) 67 | ------------------- 68 | 69 | 1.11.6 (2016-06-22) 70 | ------------------- 71 | 72 | 1.11.5 (2016-03-14) 73 | ------------------- 74 | 75 | 1.11.4 (2015-04-22) 76 | ------------------- 77 | 78 | 1.11.3 (2014-12-23) 79 | ------------------- 80 | 81 | 1.11.2 (2014-05-20) 82 | ------------------- 83 | 84 | 1.11.1 (2014-05-08) 85 | ------------------- 86 | 87 | 1.11.0 (2014-02-13) 88 | ------------------- 89 | 90 | 1.10.3 (2013-08-27) 91 | ------------------- 92 | 93 | 1.10.2 (2013-08-21) 94 | ------------------- 95 | 96 | 1.10.1 (2013-06-06) 97 | ------------------- 98 | 99 | 1.10.0 (2013-04-11) 100 | ------------------- 101 | 102 | 1.9.11 (2012-12-13) 103 | ------------------- 104 | 105 | 1.9.10 (2012-12-06) 106 | ------------------- 107 | 108 | 1.9.9 (2012-11-04) 109 | ------------------ 110 | 111 | 1.9.8 (2012-10-30) 112 | ------------------ 113 | 114 | 1.9.7 (2012-10-26) 115 | ------------------ 116 | 117 | 1.9.6 (2012-10-25) 118 | ------------------ 119 | 120 | 1.9.5 (2012-10-03 02:55) 121 | ------------------------ 122 | 123 | 1.9.4 (2012-10-03 02:06) 124 | ------------------------ 125 | 126 | 1.9.3 (2012-09-06) 127 | ------------------ 128 | 129 | 1.9.2 (2012-09-05) 130 | ------------------ 131 | 132 | 1.9.1 (2012-09-03) 133 | ------------------ 134 | 135 | 1.9.0 (2012-08-29) 136 | ------------------ 137 | 138 | 1.8.7 (2012-06-14) 139 | ------------------ 140 | 141 | 1.8.6 (2012-06-05) 142 | ------------------ 143 | 144 | 1.8.5 (2012-05-31) 145 | ------------------ 146 | 147 | 1.8.4 (2012-04-05) 148 | ------------------ 149 | 150 | 1.8.3 (2012-03-15) 151 | ------------------ 152 | 153 | 1.8.2 (2012-02-29) 154 | ------------------ 155 | 156 | 1.8.1 (2012-02-21) 157 | ------------------ 158 | 159 | 1.8.0 (2012-02-07) 160 | ------------------ 161 | 162 | 1.7.5 (2012-01-25) 163 | ------------------ 164 | 165 | 1.7.4 (2012-01-24 18:39) 166 | ------------------------ 167 | 168 | 1.7.3 (2012-01-24 16:07) 169 | ------------------------ 170 | 171 | 1.7.2 (2012-01-24 15:46) 172 | ------------------------ 173 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/client/comm_state.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ACTIONLIB__CLIENT__COMM_STATE_H_ 36 | #define ACTIONLIB__CLIENT__COMM_STATE_H_ 37 | 38 | #include 39 | #include "ros/console.h" 40 | 41 | namespace actionlib 42 | { 43 | 44 | /** 45 | * \brief Thin wrapper around an enum in order to help interpret the state of the communication state machine 46 | **/ 47 | class CommState 48 | { 49 | public: 50 | //! \brief Defines the various states the Communication State Machine can be in 51 | enum StateEnum 52 | { 53 | WAITING_FOR_GOAL_ACK = 0, 54 | PENDING = 1, 55 | ACTIVE = 2, 56 | WAITING_FOR_RESULT = 3, 57 | WAITING_FOR_CANCEL_ACK = 4, 58 | RECALLING = 5, 59 | PREEMPTING = 6, 60 | DONE = 7 61 | }; 62 | 63 | CommState(const StateEnum & state) 64 | : state_(state) {} 65 | 66 | inline bool operator==(const CommState & rhs) const 67 | { 68 | return state_ == rhs.state_; 69 | } 70 | 71 | inline bool operator==(const CommState::StateEnum & rhs) const 72 | { 73 | return state_ == rhs; 74 | } 75 | 76 | inline bool operator!=(const CommState::StateEnum & rhs) const 77 | { 78 | return !(*this == rhs); 79 | } 80 | 81 | inline bool operator!=(const CommState & rhs) const 82 | { 83 | return !(*this == rhs); 84 | } 85 | 86 | std::string toString() const 87 | { 88 | switch (state_) { 89 | case WAITING_FOR_GOAL_ACK: 90 | return "WAITING_FOR_GOAL_ACK"; 91 | case PENDING: 92 | return "PENDING"; 93 | case ACTIVE: 94 | return "ACTIVE"; 95 | case WAITING_FOR_RESULT: 96 | return "WAITING_FOR_RESULT"; 97 | case WAITING_FOR_CANCEL_ACK: 98 | return "WAITING_FOR_CANCEL_ACK"; 99 | case RECALLING: 100 | return "RECALLING"; 101 | case PREEMPTING: 102 | return "PREEMPTING"; 103 | case DONE: 104 | return "DONE"; 105 | default: 106 | ROS_ERROR_NAMED("actionlib", "BUG: Unhandled CommState: %u", state_); 107 | break; 108 | } 109 | return "BUG-UNKNOWN"; 110 | } 111 | 112 | StateEnum state_; 113 | 114 | private: 115 | CommState(); 116 | }; 117 | 118 | } // namespace actionlib 119 | 120 | #endif // ACTIONLIB__CLIENT__COMM_STATE_H_ 121 | -------------------------------------------------------------------------------- /actionlib/test/ref_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2009, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | # Author: Alexander Sorokin. 30 | # Based on code from ref_server.cpp by Vijay Pradeep 31 | PKG = 'actionlib' 32 | import rospy 33 | 34 | from actionlib.action_server import ActionServer 35 | from actionlib.msg import TestAction, TestFeedback, TestResult 36 | 37 | 38 | class RefServer (ActionServer): 39 | 40 | def __init__(self, name): 41 | action_spec = TestAction 42 | ActionServer.__init__( 43 | self, name, action_spec, self.goalCallback, self.cancelCallback, False) 44 | self.start() 45 | rospy.loginfo("Creating ActionServer [%s]\n", name) 46 | 47 | self.saved_goals = [] 48 | 49 | def goalCallback(self, gh): 50 | goal = gh.get_goal() 51 | 52 | rospy.loginfo("Got goal %d", int(goal.goal)) 53 | if goal.goal == 1: 54 | gh.set_accepted() 55 | gh.set_succeeded(None, "The ref server has succeeded") 56 | elif goal.goal == 2: 57 | gh.set_accepted() 58 | gh.set_aborted(None, "The ref server has aborted") 59 | elif goal.goal == 3: 60 | gh.set_rejected(None, "The ref server has rejected") 61 | elif goal.goal == 4: 62 | self.saved_goals.append(gh) 63 | gh.set_accepted() 64 | elif goal.goal == 5: 65 | gh.set_accepted() 66 | for g in self.saved_goals: 67 | g.set_succeeded() 68 | self.saved_goals = [] 69 | gh.set_succeeded() 70 | elif goal.goal == 6: 71 | gh.set_accepted() 72 | for g in self.saved_goals: 73 | g.set_aborted() 74 | self.saved_goals = [] 75 | gh.set_succeeded() 76 | elif goal.goal == 7: 77 | gh.set_accepted() 78 | n = len(self.saved_goals) 79 | for i, g in enumerate(self.saved_goals): 80 | g.publish_feedback(TestFeedback(n-i)) 81 | gh.set_succeeded() 82 | elif goal.goal == 8: 83 | gh.set_accepted() 84 | n = len(self.saved_goals) 85 | for i, g in enumerate(self.saved_goals): 86 | if i % 2 == 0: 87 | g.set_succeeded(TestResult(n - i), "The ref server has succeeded") 88 | else: 89 | g.set_aborted(TestResult(n - i), "The ref server has aborted") 90 | self.saved_goals = [] 91 | gh.set_succeeded() 92 | else: 93 | pass 94 | 95 | def cancelCallback(self, gh): 96 | pass 97 | 98 | 99 | if __name__ == "__main__": 100 | rospy.init_node("ref_server") 101 | ref_server = RefServer("reference_action") 102 | 103 | rospy.spin() 104 | -------------------------------------------------------------------------------- /actionlib/test/destruction_guard_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | //! \author Vijay Pradeep 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | using namespace actionlib; 43 | 44 | class TestRunner : public testing::Test 45 | { 46 | public: 47 | TestRunner() 48 | : done_protecting_(false) 49 | { 50 | } 51 | 52 | void protectingThread() 53 | { 54 | DestructionGuard::ScopedProtector protector_1(guard_); 55 | EXPECT_TRUE(protector_1.isProtected()); 56 | 57 | DestructionGuard::ScopedProtector protector_2(guard_); 58 | EXPECT_TRUE(protector_2.isProtected()); 59 | 60 | // Let the main thread know that we've successfully created to protectors 61 | { 62 | boost::mutex::scoped_lock lock(mutex_); 63 | done_protecting_ = true; 64 | cond_.notify_all(); 65 | } 66 | 67 | // Don't destruct the protectors immediately. Sleep for a little bit, and then destruct. 68 | // This will force the main thread to have to wait in it's destruct() call 69 | printf("protecting thread is sleeping\n"); 70 | boost::this_thread::sleep(boost::posix_time::microseconds(5000000)); 71 | printf("protecting thread is exiting\n"); 72 | } 73 | 74 | protected: 75 | DestructionGuard guard_; 76 | 77 | bool done_protecting_; 78 | boost::mutex mutex_; 79 | boost::condition cond_; 80 | }; 81 | 82 | 83 | TEST_F(TestRunner, threaded_test) { 84 | boost::thread spin_thread(boost::bind(&TestRunner::protectingThread, this)); 85 | 86 | { 87 | boost::mutex::scoped_lock lock(mutex_); 88 | while (!done_protecting_) { 89 | cond_.timed_wait(lock, boost::posix_time::milliseconds(100)); 90 | } 91 | } 92 | 93 | printf("About to destruct\n"); 94 | guard_.destruct(); 95 | printf("Done destructing\n"); 96 | 97 | // Already 'destructed', so protector should fail 98 | DestructionGuard::ScopedProtector protector(guard_); 99 | EXPECT_FALSE(protector.isProtected()); 100 | 101 | spin_thread.join(); 102 | } 103 | 104 | 105 | TEST(DestructionGuard, easy_test) { 106 | DestructionGuard guard; 107 | 108 | { 109 | DestructionGuard::ScopedProtector protector_1(guard); 110 | EXPECT_TRUE(protector_1.isProtected()); 111 | 112 | DestructionGuard::ScopedProtector protector_2(guard); 113 | EXPECT_TRUE(protector_2.isProtected()); 114 | } 115 | 116 | guard.destruct(); 117 | 118 | { 119 | DestructionGuard::ScopedProtector protector_3(guard); 120 | EXPECT_FALSE(protector_3.isProtected()); 121 | } 122 | } 123 | 124 | 125 | int main(int argc, char ** argv) 126 | { 127 | testing::InitGoogleTest(&argc, argv); 128 | 129 | 130 | return RUN_ALL_TESTS(); 131 | } 132 | -------------------------------------------------------------------------------- /actionlib/test/test_server_components.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2009, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | # Author: Alexander Sorokin. 30 | 31 | PKG = 'actionlib' 32 | import rospy 33 | 34 | import unittest 35 | import threading 36 | 37 | from actionlib import goal_id_generator, status_tracker 38 | import actionlib_msgs.msg 39 | 40 | 41 | # A sample python unit test 42 | class TestGoalIDGenerator(unittest.TestCase): 43 | 44 | def test_generator(self): 45 | gen1 = goal_id_generator.GoalIDGenerator() 46 | 47 | id1 = gen1.generate_ID() 48 | id2 = gen1.generate_ID() 49 | id3 = gen1.generate_ID() 50 | nn1, s1, ts1 = id1.id.split('-') 51 | nn2, s2, ts2 = id2.id.split('-') 52 | nn3, s3, ts3 = id3.id.split('-') 53 | 54 | self.assertEqual(nn1, "/test_goal_id_generator", "node names are different") 55 | self.assertEqual(nn1, nn2, "node names are different") 56 | self.assertEqual(nn1, nn3, "node names are different") 57 | 58 | self.assertEqual(s1, "1", "Sequence numbers mismatch") 59 | self.assertEqual(s2, "2", "Sequence numbers mismatch") 60 | self.assertEqual(s3, "3", "Sequence numbers mismatch") 61 | 62 | def test_threaded_generation(self): 63 | """ 64 | This test checks that all the ids are generated unique. This test should fail if the synchronization is set incorrectly. 65 | @note this test is can succeed when the errors are present. 66 | """ 67 | global ids_lists 68 | ids_lists = {} 69 | 70 | def gen_ids(tID=1, num_ids=1000): 71 | gen = goal_id_generator.GoalIDGenerator() 72 | for i in range(0, num_ids): 73 | id = gen.generate_ID() 74 | ids_lists[tID].append(id) 75 | 76 | num_ids = 1000 77 | num_threads = 200 78 | threads = [] 79 | for tID in range(0, num_threads): 80 | ids_lists[tID] = [] 81 | t = threading.Thread(None, gen_ids, None, (), {'tID': tID, 'num_ids': num_ids}) 82 | threads.append(t) 83 | t.start() 84 | 85 | for t in threads: 86 | t.join() 87 | 88 | all_ids = {} 89 | for tID in range(0, num_threads): 90 | self.assertEqual(len(ids_lists[tID]), num_ids) 91 | for id in ids_lists[tID]: 92 | nn, s, ts = id.id.split('-') 93 | self.assertFalse(s in all_ids, "Duplicate ID found") 94 | all_ids[s] = 1 95 | 96 | def test_status_tracker(self): 97 | tracker = status_tracker.StatusTracker("test-id", actionlib_msgs.msg.GoalStatus.PENDING) 98 | self.assertEqual(tracker.status.goal_id, "test-id") 99 | self.assertEqual(tracker.status.status, actionlib_msgs.msg.GoalStatus.PENDING) 100 | 101 | 102 | if __name__ == '__main__': 103 | import rostest 104 | rospy.init_node("test_goal_id_generator") 105 | rostest.rosrun(PKG, 'test_goal_id_generator', TestGoalIDGenerator) 106 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/destruction_guard.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef ACTIONLIB__DESTRUCTION_GUARD_H_ 38 | #define ACTIONLIB__DESTRUCTION_GUARD_H_ 39 | 40 | #include 41 | #include 42 | 43 | namespace actionlib 44 | { 45 | /** 46 | * @class DestructionGuard 47 | * @brief This class protects an object from being destructed until all users of that object relinquish control of it 48 | */ 49 | class DestructionGuard 50 | { 51 | public: 52 | /** 53 | * @brief Constructor for a DestructionGuard 54 | */ 55 | DestructionGuard() 56 | : use_count_(0), destructing_(false) {} 57 | void destruct() 58 | { 59 | boost::mutex::scoped_lock lock(mutex_); 60 | destructing_ = true; 61 | while (use_count_ > 0) { 62 | count_condition_.timed_wait(lock, boost::posix_time::milliseconds(1000)); 63 | } 64 | } 65 | 66 | /** 67 | * @brief Attempts to protect the guarded object from being destructed 68 | * @return True if protection succeeded, false if protection failed 69 | */ 70 | bool tryProtect() 71 | { 72 | boost::mutex::scoped_lock lock(mutex_); 73 | if (destructing_) { 74 | return false; 75 | } 76 | use_count_++; 77 | return true; 78 | } 79 | 80 | /** 81 | * @brief Releases protection on the guarded object 82 | */ 83 | void unprotect() 84 | { 85 | boost::mutex::scoped_lock lock(mutex_); 86 | use_count_--; 87 | } 88 | 89 | /** 90 | * @class ScopedProtector 91 | * @brief Protects a DestructionGuard until this object goes out of scope 92 | */ 93 | class ScopedProtector 94 | { 95 | public: 96 | /** 97 | * @brief Constructor for a ScopedProtector 98 | * @param guard The DestructionGuard to protect 99 | */ 100 | ScopedProtector(DestructionGuard & guard) 101 | : guard_(guard), protected_(false) 102 | { 103 | protected_ = guard_.tryProtect(); 104 | } 105 | 106 | /** 107 | * @brief Checks if the ScopedProtector successfully protected the DestructionGuard 108 | * @return True if protection succeeded, false otherwise 109 | */ 110 | bool isProtected() 111 | { 112 | return protected_; 113 | } 114 | 115 | /** 116 | * @brief Releases protection of the DestructionGuard if necessary 117 | */ 118 | ~ScopedProtector() 119 | { 120 | if (protected_) { 121 | guard_.unprotect(); 122 | } 123 | } 124 | 125 | private: 126 | DestructionGuard & guard_; 127 | bool protected_; 128 | }; 129 | 130 | private: 131 | boost::mutex mutex_; 132 | int use_count_; 133 | bool destructing_; 134 | boost::condition count_condition_; 135 | }; 136 | 137 | } // namespace actionlib 138 | #endif // ACTIONLIB__DESTRUCTION_GUARD_H_ 139 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/client/simple_client_goal_state.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ACTIONLIB__CLIENT__SIMPLE_CLIENT_GOAL_STATE_H_ 36 | #define ACTIONLIB__CLIENT__SIMPLE_CLIENT_GOAL_STATE_H_ 37 | 38 | #include 39 | 40 | #include "ros/console.h" 41 | 42 | namespace actionlib 43 | { 44 | 45 | class SimpleClientGoalState 46 | { 47 | public: 48 | //! \brief Defines the various states the goal can be in 49 | enum StateEnum 50 | { 51 | PENDING, 52 | ACTIVE, 53 | RECALLED, 54 | REJECTED, 55 | PREEMPTED, 56 | ABORTED, 57 | SUCCEEDED, 58 | LOST 59 | }; 60 | 61 | StateEnum state_; 62 | std::string text_; 63 | 64 | SimpleClientGoalState(const StateEnum & state, 65 | const std::string & text = std::string("")) 66 | : state_(state), text_(text) {} 67 | 68 | inline bool operator==(const SimpleClientGoalState & rhs) const 69 | { 70 | return state_ == rhs.state_; 71 | } 72 | 73 | inline bool operator==(const SimpleClientGoalState::StateEnum & rhs) const 74 | { 75 | return state_ == rhs; 76 | } 77 | 78 | inline bool operator!=(const SimpleClientGoalState::StateEnum & rhs) const 79 | { 80 | return !(*this == rhs); 81 | } 82 | 83 | inline bool operator!=(const SimpleClientGoalState & rhs) const 84 | { 85 | return !(*this == rhs); 86 | } 87 | 88 | /** 89 | * \brief Determine if goal is done executing (ie. reached a terminal state) 90 | * \return True if in RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, or LOST. False otherwise 91 | */ 92 | inline bool isDone() const 93 | { 94 | switch (state_) { 95 | case RECALLED: 96 | case REJECTED: 97 | case PREEMPTED: 98 | case ABORTED: 99 | case SUCCEEDED: 100 | case LOST: 101 | return true; 102 | default: 103 | return false; 104 | } 105 | } 106 | 107 | std::string getText() const 108 | { 109 | return text_; 110 | } 111 | 112 | //! \brief Convert the state to a string. Useful when printing debugging information 113 | std::string toString() const 114 | { 115 | switch (state_) { 116 | case PENDING: 117 | return "PENDING"; 118 | case ACTIVE: 119 | return "ACTIVE"; 120 | case RECALLED: 121 | return "RECALLED"; 122 | case REJECTED: 123 | return "REJECTED"; 124 | case PREEMPTED: 125 | return "PREEMPTED"; 126 | case ABORTED: 127 | return "ABORTED"; 128 | case SUCCEEDED: 129 | return "SUCCEEDED"; 130 | case LOST: 131 | return "LOST"; 132 | default: 133 | ROS_ERROR_NAMED("actionlib", "BUG: Unhandled SimpleGoalState: %u", state_); 134 | break; 135 | } 136 | return "BUG-UNKNOWN"; 137 | } 138 | }; 139 | 140 | } // namespace actionlib 141 | 142 | #endif // ACTIONLIB__CLIENT__SIMPLE_CLIENT_GOAL_STATE_H_ 143 | -------------------------------------------------------------------------------- /actionlib/test/server_goal_handle_destruction.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | //! \author Eitan Marder-Eppstein 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace actionlib 44 | { 45 | 46 | class ServerGoalHandleDestructionTester 47 | { 48 | public: 49 | typedef ServerGoalHandle GoalHandle; 50 | 51 | ServerGoalHandleDestructionTester(); 52 | 53 | ros::NodeHandle nh_; 54 | ActionServer * as_; 55 | GoalHandle * gh_; 56 | 57 | ~ServerGoalHandleDestructionTester(); 58 | void goalCallback(GoalHandle gh); 59 | }; 60 | 61 | } // namespace actionlib 62 | 63 | using namespace actionlib; 64 | 65 | ServerGoalHandleDestructionTester::ServerGoalHandleDestructionTester() 66 | { 67 | as_ = new ActionServer(nh_, "reference_action", false); 68 | as_->start(); 69 | as_->registerGoalCallback(boost::bind(&ServerGoalHandleDestructionTester::goalCallback, this, 70 | boost::placeholders::_1)); 71 | gh_ = new GoalHandle(); 72 | } 73 | 74 | ServerGoalHandleDestructionTester::~ServerGoalHandleDestructionTester() 75 | { 76 | delete as_; 77 | gh_->setAccepted(); 78 | delete gh_; 79 | } 80 | 81 | void ServerGoalHandleDestructionTester::goalCallback(GoalHandle gh) 82 | { 83 | ROS_ERROR_NAMED("actionlib", "In callback"); 84 | // assign to our stored goal handle 85 | *gh_ = gh; 86 | 87 | TestGoal goal = *gh.getGoal(); 88 | 89 | switch (goal.goal) { 90 | case 1: 91 | gh.setAccepted(); 92 | gh.setSucceeded(TestResult(), "The ref server has succeeded"); 93 | break; 94 | case 2: 95 | gh.setAccepted(); 96 | gh.setAborted(TestResult(), "The ref server has aborted"); 97 | break; 98 | case 3: 99 | gh.setRejected(TestResult(), "The ref server has rejected"); 100 | break; 101 | default: 102 | break; 103 | } 104 | 105 | ros::shutdown(); 106 | } 107 | 108 | void spinner() 109 | { 110 | ros::spin(); 111 | } 112 | 113 | TEST(ServerGoalHandleDestruction, destruction_test) { 114 | boost::thread spin_thread(&spinner); 115 | 116 | ServerGoalHandleDestructionTester server; 117 | 118 | SimpleActionClient client("reference_action", true); 119 | 120 | ROS_ERROR_NAMED("actionlib", "Waiting for server"); 121 | client.waitForServer(); 122 | ROS_ERROR_NAMED("actionlib", "Done waiting for server"); 123 | 124 | TestGoal goal; 125 | 126 | goal.goal = 1; 127 | client.sendGoal(goal); 128 | ROS_ERROR_NAMED("actionlib", "Sending goal"); 129 | 130 | spin_thread.join(); 131 | } 132 | 133 | int main(int argc, char ** argv) 134 | { 135 | testing::InitGoogleTest(&argc, argv); 136 | 137 | ros::init(argc, argv, "ref_server"); 138 | 139 | return RUN_ALL_TESTS(); 140 | } 141 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/client/service_client_imp.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef ACTIONLIB__CLIENT__SERVICE_CLIENT_IMP_H_ 38 | #define ACTIONLIB__CLIENT__SERVICE_CLIENT_IMP_H_ 39 | 40 | #include 41 | 42 | #include "ros/console.h" 43 | 44 | namespace actionlib 45 | { 46 | template 47 | ServiceClientImpT::ServiceClientImpT(ros::NodeHandle n, std::string name) 48 | { 49 | ac_.reset(new SimpleActionClientT(n, name, true)); 50 | } 51 | 52 | template 53 | bool ServiceClientImpT::waitForServer(const ros::Duration & timeout) 54 | { 55 | return ac_->waitForServer(timeout); 56 | } 57 | 58 | template 59 | bool ServiceClientImpT::isServerConnected() 60 | { 61 | return ac_->isServerConnected(); 62 | } 63 | 64 | template 65 | bool ServiceClientImpT::call(const void * goal, std::string goal_md5sum, 66 | void * result, std::string result_md5sum) 67 | { 68 | // ok... we need to static cast the goal message and result message 69 | const Goal * goal_c = static_cast(goal); 70 | Result * result_c = static_cast(result); 71 | 72 | // now we need to check that the md5sums are correct 73 | namespace mt = ros::message_traits; 74 | 75 | if (strcmp(mt::md5sum(*goal_c), 76 | goal_md5sum.c_str()) || strcmp(mt::md5sum(*result_c), result_md5sum.c_str())) 77 | { 78 | ROS_ERROR_NAMED("actionlib", "Incorrect md5Sums for goal and result types"); 79 | return false; 80 | } 81 | 82 | if (!ac_->isServerConnected()) { 83 | ROS_ERROR_NAMED("actionlib", 84 | "Attempting to make a service call when the server isn't actually connected to the client."); 85 | return false; 86 | } 87 | 88 | ac_->sendGoalAndWait(*goal_c); 89 | if (ac_->getState() == SimpleClientGoalState::SUCCEEDED) { 90 | (*result_c) = *(ac_->getResult()); 91 | return true; 92 | } 93 | 94 | return false; 95 | } 96 | 97 | //****** ServiceClient ******************* 98 | template 99 | bool ServiceClient::call(const Goal & goal, Result & result) 100 | { 101 | namespace mt = ros::message_traits; 102 | return client_->call(&goal, mt::md5sum(goal), &result, mt::md5sum(result)); 103 | } 104 | 105 | bool ServiceClient::waitForServer(const ros::Duration & timeout) 106 | { 107 | return client_->waitForServer(timeout); 108 | } 109 | 110 | bool ServiceClient::isServerConnected() 111 | { 112 | return client_->isServerConnected(); 113 | } 114 | 115 | //****** actionlib::serviceClient ******************* 116 | template 117 | ServiceClient serviceClient(ros::NodeHandle n, std::string name) 118 | { 119 | boost::shared_ptr client_ptr(new ServiceClientImpT(n, name)); 120 | return ServiceClient(client_ptr); 121 | } 122 | 123 | } // namespace actionlib 124 | #endif // ACTIONLIB__CLIENT__SERVICE_CLIENT_IMP_H_ 125 | -------------------------------------------------------------------------------- /actionlib/test/mock_simple_server.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2009, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | import time 31 | import rospy 32 | 33 | from actionlib.simple_action_server import SimpleActionServer 34 | from actionlib.server_goal_handle import ServerGoalHandle 35 | from actionlib.msg import TestRequestAction, TestRequestGoal, TestRequestResult 36 | 37 | 38 | class RefSimpleServer(SimpleActionServer): 39 | def __init__(self, name): 40 | SimpleActionServer.__init__(self, name, TestRequestAction, self.execute_cb, False) 41 | self.start() 42 | 43 | def execute_cb(self, goal): 44 | rospy.logdebug("Goal:\n" + str(goal)) 45 | result = TestRequestResult(goal.the_result, True) 46 | 47 | if goal.pause_status > rospy.Duration(0.0): 48 | rospy.loginfo("Locking the action server for %.3f seconds" % goal.pause_status.to_sec()) 49 | status_continue_time = rospy.get_rostime() + goal.pause_status 50 | # Takes the action server lock to prevent status from 51 | # being published (simulates a freeze). 52 | with self.action_server.lock: 53 | while rospy.get_rostime() < status_continue_time: 54 | time.sleep(0.02) 55 | rospy.loginfo("Unlocking the action server") 56 | 57 | terminate_time = rospy.get_rostime() + goal.delay_terminate 58 | while rospy.get_rostime() < terminate_time: 59 | time.sleep(0.02) 60 | if not goal.ignore_cancel: 61 | if self.is_preempt_requested(): 62 | self.set_preempted(result, goal.result_text) 63 | return 64 | 65 | rospy.logdebug("Terminating goal as: %i" % goal.terminate_status) 66 | 67 | if goal.terminate_status == TestRequestGoal.TERMINATE_SUCCESS: 68 | self.set_succeeded(result, goal.result_text) 69 | elif goal.terminate_status == TestRequestGoal.TERMINATE_ABORTED: 70 | self.set_aborted(result, goal.result_text) 71 | elif goal.terminate_status == TestRequestGoal.TERMINATE_REJECTED: 72 | rospy.logerr("Simple action server cannot reject goals") 73 | self.set_aborted(None, "Simple action server cannot reject goals") 74 | elif goal.terminate_status == TestRequestGoal.TERMINATE_DROP: 75 | rospy.loginfo("About to drop the goal. This should produce an error message.") 76 | return 77 | elif goal.terminate_status == TestRequestGoal.TERMINATE_EXCEPTION: 78 | rospy.loginfo("About to throw an exception. This should produce an error message.") 79 | raise Exception("Terminating by throwing an exception") 80 | elif goal.terminate_status == TestRequestGoal.TERMINATE_LOSE: 81 | # Losing the goal requires messing about in the action server's innards 82 | for i, s in enumerate(self.action_server.status_list): 83 | if s.status.goal_id.id == self.current_goal.goal.goal_id.id: 84 | del self.action_server.status_list[i] 85 | break 86 | self.current_goal = ServerGoalHandle() 87 | else: 88 | rospy.logerr("Don't know how to terminate as %d" % goal.terminate_status) 89 | self.set_aborted(None, "Don't know how to terminate as %d" % goal.terminate_status) 90 | 91 | 92 | if __name__ == '__main__': 93 | rospy.init_node("ref_simple_server") 94 | ref_server = RefSimpleServer("test_request_action") 95 | print("Spinning") 96 | rospy.spin() 97 | -------------------------------------------------------------------------------- /actionlib/test/test_simple_action_server_deadlock.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # 3 | # Copyright (c) 2013, Miguel Sarabia 4 | # Imperial College London 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above copyright 13 | # notice, this list of conditions and the following disclaimer in the 14 | # documentation and/or other materials provided with the distribution. 15 | # * Neither the name of Imperial College London nor the names of its 16 | # contributors may be used to endorse or promote products derived from 17 | # this software without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import random 32 | import threading 33 | import unittest 34 | 35 | import actionlib 36 | from actionlib.msg import TestAction 37 | import rosnode 38 | import rospy 39 | 40 | 41 | class Constants: 42 | pkg = "actionlib" 43 | node = "test_simple_action_server_deadlock" 44 | topic = "deadlock" 45 | deadlock_timeout = 45 # in seconds 46 | shutdown_timeout = 2 # in seconds 47 | max_action_duration = 3 48 | 49 | 50 | class DeadlockTest(unittest.TestCase): 51 | 52 | def test_deadlock(self): 53 | # Prepare condition (for safe preemption) 54 | self.condition = threading.Condition() 55 | self.last_execution_time = None 56 | 57 | # Prepare Simple Action Server 58 | self.action_server = actionlib.SimpleActionServer( 59 | Constants.topic, 60 | TestAction, 61 | execute_cb=self.execute_callback, 62 | auto_start=False) 63 | 64 | self.action_server.register_preempt_callback(self.preempt_callback) 65 | self.action_server.start() 66 | 67 | # Sleep for the amount specified 68 | rospy.sleep(Constants.deadlock_timeout) 69 | 70 | # Start actual tests 71 | running_nodes = set(rosnode.get_node_names()) 72 | required_nodes = { 73 | "/deadlock_companion_1", 74 | "/deadlock_companion_2", 75 | "/deadlock_companion_3", 76 | "/deadlock_companion_4", 77 | "/deadlock_companion_5"} 78 | 79 | self.assertTrue( 80 | required_nodes.issubset(running_nodes), 81 | "Required companion nodes are not currently running") 82 | 83 | # Shutdown companions so that we can exit nicely 84 | termination_time = rospy.Time.now() 85 | rosnode.kill_nodes(required_nodes) 86 | 87 | rospy.sleep(Constants.shutdown_timeout) 88 | 89 | # Check last execution wasn't too long ago... 90 | self.assertIsNotNone( 91 | self.last_execution_time is None, 92 | "Execute Callback was never executed") 93 | 94 | time_since_last_execution = ( 95 | termination_time - self.last_execution_time).to_sec() 96 | 97 | self.assertTrue( 98 | time_since_last_execution < 2 * Constants.max_action_duration, 99 | "Too long since last goal was executed; likely due to a deadlock") 100 | 101 | def execute_callback(self, goal): 102 | # Note down last_execution time 103 | self.last_execution_time = rospy.Time.now() 104 | 105 | # Determine duration of this action 106 | action_duration = random.uniform(0, Constants.max_action_duration) 107 | 108 | with self.condition: 109 | if not self.action_server.is_preempt_requested(): 110 | self.condition.wait(action_duration) 111 | 112 | if self.action_server.is_preempt_requested(): 113 | self.action_server.set_preempted() 114 | else: 115 | self.action_server.set_succeeded() 116 | 117 | def preempt_callback(self): 118 | with self.condition: 119 | self.condition.notify() 120 | 121 | 122 | if __name__ == '__main__': 123 | import rostest 124 | rospy.init_node(Constants.node) 125 | rostest.rosrun(Constants.pkg, Constants.node, DeadlockTest) 126 | -------------------------------------------------------------------------------- /actionlib/test/simple_client_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | //! \author Vijay Pradeep 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | using namespace actionlib; 42 | 43 | TEST(SimpleClient, easy_tests) { 44 | ros::NodeHandle n; 45 | SimpleActionClient client(n, "reference_action"); 46 | 47 | bool started = client.waitForServer(ros::Duration(10.0)); 48 | ASSERT_TRUE(started); 49 | 50 | TestGoal goal; 51 | bool finished; 52 | 53 | goal.goal = 1; 54 | // sleep a bit to make sure that all topics are properly connected to the server. 55 | ros::Duration(0.01).sleep(); 56 | client.sendGoal(goal); 57 | finished = client.waitForResult(ros::Duration(10.0)); 58 | ASSERT_TRUE(finished); 59 | EXPECT_TRUE( client.getState() == SimpleClientGoalState::SUCCEEDED) 60 | << "Expected [SUCCEEDED], but goal state is [" << client.getState().toString() << "]"; 61 | 62 | // test that setting the text field for the status works 63 | EXPECT_TRUE(client.getState().getText() == "The ref server has succeeded"); 64 | 65 | goal.goal = 2; 66 | client.sendGoal(goal); 67 | finished = client.waitForResult(ros::Duration(10.0)); 68 | ASSERT_TRUE(finished); 69 | EXPECT_TRUE( client.getState() == SimpleClientGoalState::ABORTED) 70 | << "Expected [ABORTED], but goal state is [" << client.getState().toString() << "]"; 71 | 72 | // test that setting the text field for the status works 73 | EXPECT_TRUE(client.getState().getText() == "The ref server has aborted"); 74 | 75 | client.cancelAllGoals(); 76 | 77 | // Don't need this line, but keep it as a compilation check 78 | client.cancelGoalsAtAndBeforeTime(ros::Time(1.0)); 79 | } 80 | 81 | 82 | void easyDoneCallback(bool * called, SimpleActionClient * ac, const SimpleClientGoalState & state, 83 | const TestResultConstPtr &) 84 | { 85 | EXPECT_TRUE(ac->getState() == SimpleClientGoalState::SUCCEEDED) 86 | << "Expected [SUCCEEDED], but getState() returned [" << ac->getState().toString() << "]"; 87 | EXPECT_TRUE(state == SimpleClientGoalState::SUCCEEDED) 88 | << "Expected [SUCCEEDED], but goal state is [" << state.toString() << "]"; 89 | ros::Duration(0.1).sleep(); 90 | *called = true; 91 | } 92 | 93 | TEST(SimpleClient, easy_callback) 94 | { 95 | ros::NodeHandle n; 96 | SimpleActionClient client(n, "reference_action"); 97 | 98 | bool started = client.waitForServer(ros::Duration(10.0)); 99 | ASSERT_TRUE(started); 100 | 101 | // sleep a bit to make sure that all topics are properly connected to the server. 102 | ros::Duration(0.01).sleep(); 103 | 104 | TestGoal goal; 105 | bool finished; 106 | 107 | bool called = false; 108 | goal.goal = 1; 109 | SimpleActionClient::SimpleDoneCallback func = boost::bind(&easyDoneCallback, &called, &client, boost::placeholders::_1, boost::placeholders::_2); 110 | client.sendGoal(goal, func); 111 | finished = client.waitForResult(ros::Duration(10.0)); 112 | ASSERT_TRUE(finished); 113 | EXPECT_TRUE(called) << "easyDoneCallback() was never called" ; 114 | } 115 | 116 | void spinThread() 117 | { 118 | ros::NodeHandle nh; 119 | ros::spin(); 120 | } 121 | 122 | int main(int argc, char ** argv) 123 | { 124 | testing::InitGoogleTest(&argc, argv); 125 | 126 | ros::init(argc, argv, "simple_client_test"); 127 | 128 | boost::thread spin_thread(&spinThread); 129 | 130 | int result = RUN_ALL_TESTS(); 131 | 132 | ros::shutdown(); 133 | 134 | spin_thread.join(); 135 | 136 | return result; 137 | } 138 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/client/goal_manager_imp.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* This file has the template implementation for GoalHandle. It should be included with the 36 | * class definition. 37 | */ 38 | 39 | #ifndef ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_ 40 | #define ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_ 41 | 42 | #include "ros/ros.h" 43 | 44 | namespace actionlib 45 | { 46 | 47 | template 48 | void GoalManager::registerSendGoalFunc(SendGoalFunc send_goal_func) 49 | { 50 | send_goal_func_ = send_goal_func; 51 | } 52 | 53 | template 54 | void GoalManager::registerCancelFunc(CancelFunc cancel_func) 55 | { 56 | cancel_func_ = cancel_func; 57 | } 58 | 59 | 60 | template 61 | ClientGoalHandle GoalManager::initGoal(const Goal & goal, 62 | TransitionCallback transition_cb, 63 | FeedbackCallback feedback_cb) 64 | { 65 | ActionGoalPtr action_goal(new ActionGoal); 66 | action_goal->header.stamp = ros::Time::now(); 67 | action_goal->goal_id = id_generator_.generateID(); 68 | action_goal->goal = goal; 69 | 70 | typedef CommStateMachine CommStateMachineT; 71 | boost::shared_ptr comm_state_machine(new CommStateMachineT(action_goal, 72 | transition_cb, 73 | feedback_cb)); 74 | 75 | boost::recursive_mutex::scoped_lock lock(list_mutex_); 76 | typename ManagedListT::Handle list_handle = 77 | list_.add(comm_state_machine, boost::bind(&GoalManagerT::listElemDeleter, this, boost::placeholders::_1), guard_); 78 | 79 | if (send_goal_func_) { 80 | send_goal_func_(action_goal); 81 | } else { 82 | ROS_WARN_NAMED("actionlib", 83 | "Possible coding error: send_goal_func_ set to nullptr. Not going to send goal"); 84 | } 85 | 86 | return GoalHandleT(this, list_handle, guard_); 87 | } 88 | 89 | template 90 | void GoalManager::listElemDeleter(typename ManagedListT::iterator it) 91 | { 92 | assert(guard_); 93 | if (!guard_) 94 | { 95 | ROS_ERROR_NAMED("actionlib", "Goal manager deleter should not see invalid guards"); 96 | return; 97 | } 98 | DestructionGuard::ScopedProtector protector(*guard_); 99 | if (!protector.isProtected()) { 100 | ROS_ERROR_NAMED("actionlib", 101 | "This action client associated with the goal handle has already been destructed. Not going to try delete the CommStateMachine associated with this goal"); 102 | return; 103 | } 104 | 105 | ROS_DEBUG_NAMED("actionlib", "About to erase CommStateMachine"); 106 | boost::recursive_mutex::scoped_lock lock(list_mutex_); 107 | list_.erase(it); 108 | ROS_DEBUG_NAMED("actionlib", "Done erasing CommStateMachine"); 109 | } 110 | 111 | template 112 | void GoalManager::updateStatuses( 113 | const actionlib_msgs::GoalStatusArrayConstPtr & status_array) 114 | { 115 | boost::recursive_mutex::scoped_lock lock(list_mutex_); 116 | typename ManagedListT::iterator it = list_.begin(); 117 | 118 | while (it != list_.end()) { 119 | GoalHandleT gh(this, it.createHandle(), guard_); 120 | (*it)->updateStatus(gh, status_array); 121 | ++it; 122 | } 123 | } 124 | 125 | template 126 | void GoalManager::updateFeedbacks(const ActionFeedbackConstPtr & action_feedback) 127 | { 128 | boost::recursive_mutex::scoped_lock lock(list_mutex_); 129 | typename ManagedListT::iterator it = list_.begin(); 130 | 131 | while (it != list_.end()) { 132 | GoalHandleT gh(this, it.createHandle(), guard_); 133 | (*it)->updateFeedback(gh, action_feedback); 134 | ++it; 135 | } 136 | } 137 | 138 | template 139 | void GoalManager::updateResults(const ActionResultConstPtr & action_result) 140 | { 141 | boost::recursive_mutex::scoped_lock lock(list_mutex_); 142 | typename ManagedListT::iterator it = list_.begin(); 143 | 144 | while (it != list_.end()) { 145 | GoalHandleT gh(this, it.createHandle(), guard_); 146 | (*it)->updateResult(gh, action_result); 147 | ++it; 148 | } 149 | } 150 | 151 | 152 | } // namespace actionlib 153 | 154 | #endif // ACTIONLIB__CLIENT__GOAL_MANAGER_IMP_H_ 155 | -------------------------------------------------------------------------------- /actionlib/test/exercise_simple_client.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | // Derived from exercise_simple_server.py 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | 40 | #define EXPECT_STATE(expected_state) EXPECT_TRUE( \ 41 | ac_.getState() == SimpleClientGoalState::expected_state) \ 42 | << "Expected [" << #expected_state << "], but goal state is [" << ac_.getState().toString() << \ 43 | "]"; 44 | 45 | 46 | using namespace actionlib; 47 | using namespace actionlib_msgs; 48 | 49 | class SimpleClientFixture : public testing::Test 50 | { 51 | public: 52 | SimpleClientFixture() 53 | : ac_("test_request_action"), default_wait_(60.0) {} 54 | 55 | protected: 56 | virtual void SetUp() 57 | { 58 | // Make sure that the server comes up 59 | ASSERT_TRUE(ac_.waitForServer(ros::Duration(10.0)) ); 60 | } 61 | 62 | SimpleActionClient ac_; 63 | ros::Duration default_wait_; 64 | }; 65 | 66 | TEST_F(SimpleClientFixture, just_succeed) { 67 | TestRequestGoal goal; 68 | goal.terminate_status = TestRequestGoal::TERMINATE_SUCCESS; 69 | goal.the_result = 42; 70 | ac_.sendGoal(goal); 71 | ac_.waitForResult(default_wait_); 72 | EXPECT_STATE(SUCCEEDED); 73 | EXPECT_EQ(42, ac_.getResult()->the_result); 74 | } 75 | 76 | TEST_F(SimpleClientFixture, just_abort) { 77 | TestRequestGoal goal; 78 | goal.terminate_status = TestRequestGoal::TERMINATE_ABORTED; 79 | goal.the_result = 42; 80 | ac_.sendGoal(goal); 81 | ac_.waitForResult(default_wait_); 82 | EXPECT_STATE(ABORTED); 83 | EXPECT_EQ(42, ac_.getResult()->the_result); 84 | } 85 | 86 | TEST_F(SimpleClientFixture, just_preempt) { 87 | TestRequestGoal goal; 88 | goal.terminate_status = TestRequestGoal::TERMINATE_SUCCESS; 89 | goal.delay_terminate = ros::Duration(1000); 90 | goal.the_result = 42; 91 | ac_.sendGoal(goal); 92 | 93 | // Sleep for 10 seconds or until we hear back from the action server 94 | for (unsigned int i = 0; i < 100; i++) { 95 | ROS_DEBUG_NAMED("actionlib", "Waiting for Server Response"); 96 | if (ac_.getState() != SimpleClientGoalState::PENDING) { 97 | break; 98 | } 99 | ros::Duration(0.1).sleep(); 100 | } 101 | 102 | ac_.cancelGoal(); 103 | ac_.waitForResult(default_wait_); 104 | EXPECT_STATE(PREEMPTED); 105 | EXPECT_EQ(42, ac_.getResult()->the_result); 106 | } 107 | 108 | TEST_F(SimpleClientFixture, drop) { 109 | TestRequestGoal goal; 110 | goal.terminate_status = TestRequestGoal::TERMINATE_DROP; 111 | goal.the_result = 42; 112 | ac_.sendGoal(goal); 113 | ac_.waitForResult(default_wait_); 114 | EXPECT_STATE(ABORTED); 115 | EXPECT_EQ(0, ac_.getResult()->the_result); 116 | } 117 | 118 | TEST_F(SimpleClientFixture, exception) { 119 | TestRequestGoal goal; 120 | goal.terminate_status = TestRequestGoal::TERMINATE_EXCEPTION; 121 | goal.the_result = 42; 122 | ac_.sendGoal(goal); 123 | ac_.waitForResult(default_wait_); 124 | EXPECT_STATE(ABORTED); 125 | EXPECT_EQ(0, ac_.getResult()->the_result); 126 | } 127 | 128 | TEST_F(SimpleClientFixture, ignore_cancel_and_succeed) { 129 | TestRequestGoal goal; 130 | goal.terminate_status = TestRequestGoal::TERMINATE_SUCCESS; 131 | goal.delay_terminate = ros::Duration(2.0); 132 | goal.ignore_cancel = true; 133 | goal.the_result = 42; 134 | ac_.sendGoal(goal); 135 | 136 | // Sleep for 10 seconds or until we hear back from the action server 137 | for (unsigned int i = 0; i < 100; i++) { 138 | ROS_DEBUG_NAMED("actionlib", "Waiting for Server Response"); 139 | if (ac_.getState() != SimpleClientGoalState::PENDING) { 140 | break; 141 | } 142 | ros::Duration(0.1).sleep(); 143 | } 144 | 145 | ac_.cancelGoal(); 146 | ac_.waitForResult(default_wait_ + default_wait_); 147 | EXPECT_STATE(SUCCEEDED); 148 | EXPECT_EQ(42, ac_.getResult()->the_result); 149 | } 150 | 151 | TEST_F(SimpleClientFixture, lose) { 152 | TestRequestGoal goal; 153 | goal.terminate_status = TestRequestGoal::TERMINATE_LOSE; 154 | goal.the_result = 42; 155 | ac_.sendGoal(goal); 156 | ac_.waitForResult(default_wait_); 157 | EXPECT_STATE(LOST); 158 | } 159 | 160 | void spinThread() 161 | { 162 | ros::NodeHandle nh; 163 | ros::spin(); 164 | } 165 | 166 | int main(int argc, char ** argv) 167 | { 168 | testing::InitGoogleTest(&argc, argv); 169 | 170 | ros::init(argc, argv, "simple_client_test"); 171 | 172 | boost::thread spin_thread(&spinThread); 173 | 174 | int result = RUN_ALL_TESTS(); 175 | 176 | ros::shutdown(); 177 | 178 | spin_thread.join(); 179 | 180 | return result; 181 | } 182 | -------------------------------------------------------------------------------- /actionlib/test/exercise_simple_client.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Copyright (c) 2009, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | PKG = 'actionlib' 30 | 31 | import sys 32 | import unittest 33 | import rospy 34 | import rostest 35 | from actionlib import SimpleActionClient 36 | from actionlib_msgs.msg import GoalStatus 37 | from actionlib.msg import TestRequestAction, TestRequestGoal 38 | 39 | 40 | class SimpleExerciser(unittest.TestCase): 41 | 42 | def setUp(self): 43 | self.default_wait = rospy.Duration(60.0) 44 | self.client = SimpleActionClient('test_request_action', TestRequestAction) 45 | self.assertTrue(self.client.wait_for_server(self.default_wait)) 46 | 47 | def test_just_succeed(self): 48 | goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_SUCCESS, 49 | the_result=42) 50 | self.client.send_goal(goal) 51 | self.client.wait_for_result(self.default_wait) 52 | 53 | self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state()) 54 | self.assertEqual(42, self.client.get_result().the_result) 55 | 56 | def test_just_abort(self): 57 | goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_ABORTED, 58 | the_result=42) 59 | self.client.send_goal(goal) 60 | self.client.wait_for_result(self.default_wait) 61 | 62 | self.assertEqual(GoalStatus.ABORTED, self.client.get_state()) 63 | self.assertEqual(42, self.client.get_result().the_result) 64 | 65 | def test_just_preempt(self): 66 | goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_SUCCESS, 67 | delay_terminate=rospy.Duration(100000), 68 | the_result=42) 69 | self.client.send_goal(goal) 70 | 71 | # Ensure that the action server got the goal, before continuing 72 | timeout_time = rospy.Time.now() + self.default_wait 73 | while rospy.Time.now() < timeout_time: 74 | if (self.client.get_state() != GoalStatus.PENDING): 75 | break 76 | self.client.cancel_goal() 77 | 78 | self.client.wait_for_result(self.default_wait) 79 | self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state()) 80 | self.assertEqual(42, self.client.get_result().the_result) 81 | 82 | # Should print out errors about not setting a terminal status in the action server. 83 | def test_drop(self): 84 | goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_DROP, 85 | the_result=42) 86 | self.client.send_goal(goal) 87 | self.client.wait_for_result(self.default_wait) 88 | 89 | self.assertEqual(GoalStatus.ABORTED, self.client.get_state()) 90 | self.assertEqual(0, self.client.get_result().the_result) 91 | 92 | # Should print out errors about throwing an exception 93 | def test_exception(self): 94 | goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_EXCEPTION, 95 | the_result=42) 96 | self.client.send_goal(goal) 97 | self.client.wait_for_result(self.default_wait) 98 | 99 | self.assertEqual(GoalStatus.ABORTED, self.client.get_state()) 100 | self.assertEqual(0, self.client.get_result().the_result) 101 | 102 | def test_ignore_cancel_and_succeed(self): 103 | goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_SUCCESS, 104 | delay_terminate=rospy.Duration(2.0), 105 | ignore_cancel=True, 106 | the_result=42) 107 | self.client.send_goal(goal) 108 | 109 | # Ensure that the action server got the goal, before continuing 110 | timeout_time = rospy.Time.now() + self.default_wait 111 | while rospy.Time.now() < timeout_time: 112 | if (self.client.get_state() != GoalStatus.PENDING): 113 | break 114 | self.client.cancel_goal() 115 | 116 | self.client.wait_for_result(self.default_wait) 117 | 118 | self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state()) 119 | self.assertEqual(42, self.client.get_result().the_result) 120 | 121 | def test_lose(self): 122 | goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_LOSE, 123 | the_result=42) 124 | self.client.send_goal(goal) 125 | self.client.wait_for_result(self.default_wait) 126 | 127 | self.assertEqual(GoalStatus.LOST, self.client.get_state()) 128 | 129 | # test_freeze_server has been removed, as it is undecided what should happen 130 | # when the action server disappears. 131 | # 132 | # # def test_freeze_server(self): 133 | # # goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS, 134 | # # the_result = 42, 135 | # # pause_status = rospy.Duration(10.0)) 136 | # # self.client.send_goal(goal) 137 | # # self.client.wait_for_result(rospy.Duration(13.0)) 138 | # # 139 | # # self.assertEqual(GoalStatus.LOST, self.client.get_state()) 140 | # 141 | 142 | 143 | if __name__ == '__main__': 144 | rospy.init_node("exercise_simple_server") 145 | rostest.run(PKG, 'exercise_simple_server', SimpleExerciser, sys.argv) 146 | -------------------------------------------------------------------------------- /actionlib_tools/src/actionlib_tools/library.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2010, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of Willow Garage, Inc. nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | # Revision $Id: library.py 9993 2010-06-09 02:35:02Z kwc $ 34 | """ 35 | Top-level library routines we expose to the end-user 36 | """ 37 | 38 | 39 | import yaml 40 | import sys 41 | 42 | import roslib.message 43 | import roslib.packages 44 | 45 | import rospy 46 | 47 | _NATIVE_YAML_TYPES = {int, float, str, bool} 48 | if sys.version_info < (3, 0): 49 | _NATIVE_YAML_TYPES.add(long) 50 | 51 | 52 | def findros(pkg, resource): 53 | """ 54 | Find ROS resource inside of a package. 55 | 56 | @param pkg: ROS package name 57 | @type pkg: str 58 | @param resource: resource filename 59 | @type resource: str 60 | """ 61 | val = roslib.packages.find_resource(pkg, resource) 62 | if val: 63 | return val[0] 64 | else: 65 | raise rospy.ROSException("cannot find resource") 66 | 67 | 68 | def YAMLBag(object): 69 | def __init__(self, filename): 70 | self.filename = filename 71 | self._fp = open(filename, 'w') 72 | 73 | def append(self, msg): 74 | self._fp.write(to_yaml(msg)) 75 | 76 | def close(self): 77 | if self._fp is not None: 78 | self._fp.close() 79 | self._fp = None 80 | 81 | 82 | def to_yaml(obj): 83 | if isinstance(obj, roslib.message.Message): 84 | return _message_to_yaml(obj) 85 | pass 86 | else: 87 | return yaml.dump(obj) 88 | 89 | 90 | def yaml_msg_str(type_, yaml_str, filename=None): 91 | """ 92 | Load single message from YAML dictionary representation. 93 | 94 | @param type_: Message class 95 | @type type_: class (Message subclass) 96 | @param filename: Name of YAML file 97 | @type filename: str 98 | """ 99 | import yaml 100 | if yaml_str.strip() == '': 101 | msg_dict = {} 102 | else: 103 | msg_dict = yaml.safe_load(yaml_str) 104 | if not isinstance(msg_dict, dict): 105 | if filename: 106 | raise ValueError("yaml file [%s] does not contain a dictionary" % filename) 107 | else: 108 | raise ValueError("yaml string does not contain a dictionary") 109 | m = type_() 110 | roslib.message.fill_message_args(m, [msg_dict]) 111 | return m 112 | 113 | 114 | def yaml_msg(type_, filename): 115 | """ 116 | Load single message from YAML dictionary representation. 117 | 118 | @param type_: Message class 119 | @type type_: class (Message subclass) 120 | @param filename: Name of YAML file 121 | @type filename: str 122 | """ 123 | with open(filename, 'r') as f: 124 | return yaml_msg_str(type_, f.read(), filename=filename) 125 | 126 | 127 | def yaml_msgs_str(type_, yaml_str, filename=None): 128 | """ 129 | Load messages from YAML list-of-dictionaries representation. 130 | 131 | @param type_: Message class 132 | @type type_: class (Message subclass) 133 | @param filename: Name of YAML file 134 | @type filename: str 135 | """ 136 | import yaml 137 | yaml_doc = yaml.safe_load(yaml_str) 138 | msgs = [] 139 | for msg_dict in yaml_doc: 140 | if not isinstance(msg_dict, dict): 141 | if filename: 142 | raise ValueError("yaml file [%s] does not contain a list of dictionaries" % filename) 143 | else: 144 | raise ValueError("yaml string does not contain a list of dictionaries") 145 | m = type_() 146 | roslib.message.fill_message_args(m, msg_dict) 147 | msgs.append(m) 148 | return msgs 149 | 150 | 151 | def yaml_msgs(type_, filename): 152 | """ 153 | Load messages from YAML list-of-dictionaries representation. 154 | 155 | @param type_: Message class 156 | @type type_: class (Message subclass) 157 | @param filename: Name of YAML file 158 | @type filename: str 159 | """ 160 | with open(filename, 'r') as f: 161 | return yaml_msgs_str(type_, f.read(), filename=filename) 162 | 163 | 164 | def _message_to_yaml(msg, indent='', time_offset=None): 165 | """ 166 | convert value to YAML representation 167 | @param val: to convert to string representation. Most likely a Message. 168 | @type val: Value 169 | @param indent: indentation 170 | @type indent: str 171 | @param time_offset: if not None, time fields will be displayed 172 | as deltas from time_offset 173 | @type time_offset: Time 174 | """ 175 | if type(msg) in _NATIVE_YAML_TYPES: 176 | # TODO: need to actually escape 177 | return msg 178 | elif isinstance(msg, rospy.Time) or isinstance(msg, rospy.Duration): 179 | if time_offset is not None and isinstance(msg, rospy.Time): 180 | msg = msg-time_offset 181 | 182 | return '\n%ssecs: %s\n%snsecs: %s' % (indent, msg.secs, indent, msg.nsecs) 183 | 184 | elif type(msg) in [list, tuple]: 185 | # have to convert tuple->list to be yaml-safe 186 | if len(msg) == 0: 187 | return str(list(msg)) 188 | msg0 = msg[0] 189 | if type(msg0) in [int, float, str, bool] or \ 190 | isinstance(msg0, rospy.Time) or isinstance(msg0, rospy.Duration) or \ 191 | isinstance(msg0, list) or isinstance(msg0, tuple): 192 | # no array-of-arrays support yet 193 | return str(list(msg)) 194 | else: 195 | indent = indent + ' ' 196 | return "["+','.join([roslib.message.strify_message(v, indent, time_offset) for v in msg])+"]" 197 | elif isinstance(msg, rospy.Message): 198 | if indent: 199 | return '\n' + \ 200 | '\n'.join(['%s%s: %s' % ( 201 | indent, f, roslib.message.strify_message(getattr(msg, f), ' ' + indent, time_offset)) for f in msg.__slots__]) 202 | return '\n'.join(['%s%s: %s' % (indent, f, roslib.message.strify_message(getattr(msg, f), ' ' + indent, time_offset)) for f in msg.__slots__]) 203 | else: 204 | return str(msg) # punt 205 | -------------------------------------------------------------------------------- /actionlib/include/actionlib/server/action_server.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef ACTIONLIB__SERVER__ACTION_SERVER_H_ 38 | #define ACTIONLIB__SERVER__ACTION_SERVER_H_ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | 56 | #include 57 | #include 58 | 59 | namespace actionlib 60 | { 61 | /** 62 | * @class ActionServer 63 | * @brief The ActionServer is a helpful tool for managing goal requests to a 64 | * node. It allows the user to specify callbacks that are invoked when goal 65 | * or cancel requests come over the wire, and passes back GoalHandles that 66 | * can be used to track the state of a given goal request. The ActionServer 67 | * makes no assumptions about the policy used to service these goals, and 68 | * sends status for each goal over the wire until the last GoalHandle 69 | * associated with a goal request is destroyed. 70 | */ 71 | template 72 | class ActionServer : public ActionServerBase 73 | { 74 | public: 75 | // for convenience when referring to ServerGoalHandles 76 | typedef ServerGoalHandle GoalHandle; 77 | 78 | // generates typedefs that we'll use to make our lives easier 79 | ACTION_DEFINITION(ActionSpec) 80 | 81 | /** 82 | * @brief Constructor for an ActionServer 83 | * @param n A NodeHandle to create a namespace under 84 | * @param name The name of the action 85 | * @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire 86 | * @param cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire 87 | * @param auto_start A boolean value that tells the ActionServer whether or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. 88 | */ 89 | ActionServer(ros::NodeHandle n, std::string name, 90 | boost::function goal_cb, 91 | boost::function cancel_cb, 92 | bool auto_start); 93 | 94 | /** 95 | * @brief Constructor for an ActionServer 96 | * @param n A NodeHandle to create a namespace under 97 | * @param name The name of the action 98 | * @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire 99 | * @param auto_start A boolean value that tells the ActionServer whether or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. 100 | */ 101 | ActionServer(ros::NodeHandle n, std::string name, 102 | boost::function goal_cb, 103 | bool auto_start); 104 | 105 | /** 106 | * @brief DEPRECATED Constructor for an ActionServer 107 | * @param n A NodeHandle to create a namespace under 108 | * @param name The name of the action 109 | * @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire 110 | * @param cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire 111 | */ 112 | ROS_DEPRECATED ActionServer(ros::NodeHandle n, std::string name, 113 | boost::function goal_cb, 114 | boost::function cancel_cb = boost::function()); 115 | 116 | /** 117 | * @brief Constructor for an ActionServer 118 | * @param n A NodeHandle to create a namespace under 119 | * @param name The name of the action 120 | * @param auto_start A boolean value that tells the ActionServer whether or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server. 121 | */ 122 | ActionServer(ros::NodeHandle n, std::string name, 123 | bool auto_start); 124 | 125 | /** 126 | * @brief DEPRECATED Constructor for an ActionServer 127 | * @param n A NodeHandle to create a namespace under 128 | * @param name The name of the action 129 | */ 130 | ROS_DEPRECATED ActionServer(ros::NodeHandle n, std::string name); 131 | 132 | /** 133 | * @brief Destructor for the ActionServer 134 | */ 135 | virtual ~ActionServer(); 136 | 137 | private: 138 | /** 139 | * @brief Initialize all ROS connections and setup timers 140 | */ 141 | virtual void initialize(); 142 | 143 | /** 144 | * @brief Publishes a result for a given goal 145 | * @param status The status of the goal with which the result is associated 146 | * @param result The result to publish 147 | */ 148 | virtual void publishResult(const actionlib_msgs::GoalStatus & status, const Result & result); 149 | 150 | /** 151 | * @brief Publishes feedback for a given goal 152 | * @param status The status of the goal with which the feedback is associated 153 | * @param feedback The feedback to publish 154 | */ 155 | virtual void publishFeedback(const actionlib_msgs::GoalStatus & status, 156 | const Feedback & feedback); 157 | 158 | /** 159 | * @brief Explicitly publish status 160 | */ 161 | virtual void publishStatus(); 162 | 163 | /** 164 | * @brief Publish status for all goals on a timer event 165 | */ 166 | void publishStatus(const ros::TimerEvent & e); 167 | 168 | ros::NodeHandle node_; 169 | 170 | ros::Subscriber goal_sub_, cancel_sub_; 171 | ros::Publisher status_pub_, result_pub_, feedback_pub_; 172 | 173 | ros::Timer status_timer_; 174 | }; 175 | } // namespace actionlib 176 | 177 | // include the implementation 178 | #include 179 | #endif // ACTIONLIB__SERVER__ACTION_SERVER_H_ 180 | --------------------------------------------------------------------------------