Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19633] pkg/trunk
|
From: <ei...@us...> - 2009-07-24 22:03:56
|
Revision: 19633
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19633&view=rev
Author: eitanme
Date: 2009-07-24 22:03:46 +0000 (Fri, 24 Jul 2009)
Log Message:
-----------
Merging the new actions from a branch back into trunk.
r28694@att (orig r19424): eitanme | 2009-07-22 13:06:07 -0700
Creating a branch for developing the new action interface
r28695@att (orig r19425): eitanme | 2009-07-22 13:09:21 -0700
Initial commit of some janky code, but we're in a branch so its ok. Don't expect it to compile though.
r28705@att (orig r19435): vijaypradeep | 2009-07-22 14:30:59 -0700
Moving old action client
r28706@att (orig r19436): vijaypradeep | 2009-07-22 14:31:48 -0700
Definitely not going to compile
r28707@att (orig r19437): vijaypradeep | 2009-07-22 14:32:05 -0700
Changed header to action_header
r28711@att (orig r19441): eitanme | 2009-07-22 14:52:18 -0700
Compiling version of a first cut at the server.
r28741@att (orig r19460): vijaypradeep | 2009-07-22 15:57:36 -0700
Lots of Message Changes
r28751@att (orig r19467): eitanme | 2009-07-22 16:49:07 -0700
Now compiles with the new messages... hooray
r28753@att (orig r19469): eitanme | 2009-07-22 16:54:27 -0700
Now publishes a StatusArray over the wire
r28757@att (orig r19473): eitanme | 2009-07-22 17:12:49 -0700
Removing the ActionHeader, GoalStatus contains a GoalID, and ActionGoals will also contain a GoalID
r28762@att (orig r19478): vijaypradeep | 2009-07-22 17:23:22 -0700
Still chugging along
r28765@att (orig r19481): eitanme | 2009-07-22 17:41:56 -0700
Support for sending results over the wire
r28767@att (orig r19483): eitanme | 2009-07-22 18:01:15 -0700
Now reads status from the parameter server
r28771@att (orig r19486): eitanme | 2009-07-22 18:16:06 -0700
Removing some unecessary includes
r28772@att (orig r19487): eitanme | 2009-07-22 18:20:32 -0700
Making all the tracking stuff private
r28811@att (orig r19518): eitanme | 2009-07-23 10:52:56 -0700
Checks for uninitialized GoalHandles
r28824@att (orig r19523): eitanme | 2009-07-23 13:15:12 -0700
Adding checks for illegal state transitions on a GoalHandle with appropriate errors
r28846@att (orig r19544): eitanme | 2009-07-23 17:54:56 -0700
Working version of the SingleGoalActionServer
r28847@att (orig r19545): eitanme | 2009-07-23 17:55:22 -0700
Working version of the new move_base with the SingleGoalActionServer
r28858@att (orig r19556): eitanme | 2009-07-23 19:29:38 -0700
Adding Doxygen so at the next api review we can just pull it up and not have to worry about writing anything
r28859@att (orig r19557): eitanme | 2009-07-23 19:32:00 -0700
Now uses move_base_new
r28861@att (orig r19559): eitanme | 2009-07-23 20:11:41 -0700
Checking in messages I missed
r28868@att (orig r19566): eitanme | 2009-07-23 20:37:07 -0700
Explicit preempt policy
r28869@att (orig r19567): vijaypradeep | 2009-07-23 21:07:47 -0700
First cut of multigoal ActionClient seems to run
r28873@att (orig r19570): vijaypradeep | 2009-07-23 23:31:18 -0700
Timers don't work yet, but the rest of the ActionClient seems to work
r28874@att (orig r19571): vijaypradeep | 2009-07-23 23:45:00 -0700
Minor fixes
r28891@att (orig r19583): eitanme | 2009-07-24 11:06:02 -0700
SingleActionGoalServer now works well with the preempt policy implemented in the ActionServer class
r28893@att (orig r19584): eitanme | 2009-07-24 11:19:58 -0700
Adding a bunch of ROS_DEBUG statements, hopefully this will help later
r28896@att (orig r19585): vijaypradeep | 2009-07-24 11:28:58 -0700
Added feedback to ActionClient. Started doxygenating
r28905@att (orig r19591): vijaypradeep | 2009-07-24 12:11:38 -0700
Started populating the mainpage
r28922@att (orig r19607): vijaypradeep | 2009-07-24 13:38:12 -0700
More dox
r28926@att (orig r19610): vijaypradeep | 2009-07-24 13:54:36 -0700
Moving action_tools to actionlib
r28927@att (orig r19611): vijaypradeep | 2009-07-24 13:57:14 -0700
Fixing move base
r28945@att (orig r19629): eitanme | 2009-07-24 14:58:38 -0700
Encapsulating result_type in its own message which holds the relevant enum so users don't have to re-create it
Modified Paths:
--------------
pkg/trunk/highlevel/move_base_stage/move_base/move_base.xml
pkg/trunk/stacks/nav/move_base/include/move_base/move_base.h
pkg/trunk/stacks/nav/move_base/manifest.xml
pkg/trunk/stacks/nav/move_base/msg/MoveBaseGoal.msg
pkg/trunk/stacks/nav/move_base/msg/MoveBaseResult.msg
pkg/trunk/stacks/nav/move_base/src/move_base.cpp
Added Paths:
-----------
pkg/trunk/sandbox/actionlib/
pkg/trunk/sandbox/actionlib/CMakeLists.txt
pkg/trunk/sandbox/actionlib/Makefile
pkg/trunk/sandbox/actionlib/include/
pkg/trunk/sandbox/actionlib/include/actionlib/
pkg/trunk/sandbox/actionlib/include/actionlib/action_client.h
pkg/trunk/sandbox/actionlib/include/actionlib/action_server.h
pkg/trunk/sandbox/actionlib/include/actionlib/client_goal_status.h
pkg/trunk/sandbox/actionlib/include/actionlib/enclosure_deleter.h
pkg/trunk/sandbox/actionlib/include/actionlib/goal_id_generator.h
pkg/trunk/sandbox/actionlib/include/actionlib/old_action_client.h
pkg/trunk/sandbox/actionlib/include/actionlib/one_shot_timer.h
pkg/trunk/sandbox/actionlib/include/actionlib/single_goal_action_server.h
pkg/trunk/sandbox/actionlib/mainpage.dox
pkg/trunk/sandbox/actionlib/manifest.xml
pkg/trunk/sandbox/actionlib/msg/
pkg/trunk/sandbox/actionlib/msg/GoalID.msg
pkg/trunk/sandbox/actionlib/msg/GoalStatus.msg
pkg/trunk/sandbox/actionlib/msg/GoalStatusArray.msg
pkg/trunk/sandbox/actionlib/msg/MoveBaseAction.msg
pkg/trunk/sandbox/actionlib/msg/MoveBaseActionFeedback.msg
pkg/trunk/sandbox/actionlib/msg/MoveBaseActionGoal.msg
pkg/trunk/sandbox/actionlib/msg/MoveBaseActionResult.msg
pkg/trunk/sandbox/actionlib/msg/MoveBaseFeedback.msg
pkg/trunk/sandbox/actionlib/msg/MoveBaseGoal.msg
pkg/trunk/sandbox/actionlib/msg/MoveBaseResult.msg
pkg/trunk/sandbox/actionlib/msg/RequestType.msg
pkg/trunk/sandbox/actionlib/msg/TestActionGoal.msg
pkg/trunk/sandbox/actionlib/msg/TestActionResult.msg
pkg/trunk/sandbox/actionlib/msg/TestGoal.msg
pkg/trunk/sandbox/actionlib/msg/TestResult.msg
pkg/trunk/sandbox/actionlib/src/
pkg/trunk/sandbox/actionlib/src/move_base_client.cpp
pkg/trunk/sandbox/actionlib/src/move_base_client_old.cpp
pkg/trunk/sandbox/actionlib/src/move_base_server.cpp
pkg/trunk/stacks/nav/move_base/msg/MoveBaseAction.msg
pkg/trunk/stacks/nav/move_base/msg/MoveBaseActionFeedback.msg
pkg/trunk/stacks/nav/move_base/msg/MoveBaseActionGoal.msg
pkg/trunk/stacks/nav/move_base/msg/MoveBaseActionResult.msg
pkg/trunk/stacks/nav/move_base/msg/MoveBaseFeedback.msg
Removed Paths:
-------------
pkg/trunk/sandbox/action_tools/CMakeLists.txt
pkg/trunk/sandbox/action_tools/Makefile
pkg/trunk/sandbox/action_tools/include/action_tools/EnclosureDeleter.h
pkg/trunk/sandbox/action_tools/include/action_tools/action_client.h
pkg/trunk/sandbox/action_tools/include/action_tools/action_server.h
pkg/trunk/sandbox/action_tools/include/action_tools/one_shot_timer.h
pkg/trunk/sandbox/action_tools/mainpage.dox
pkg/trunk/sandbox/action_tools/manifest.xml
pkg/trunk/sandbox/action_tools/msg/GoalID.msg
pkg/trunk/sandbox/action_tools/msg/GoalStatus.msg
pkg/trunk/sandbox/action_tools/msg/MoveBaseGoal.msg
pkg/trunk/sandbox/action_tools/msg/MoveBaseResult.msg
pkg/trunk/sandbox/action_tools/msg/Preempt.msg
pkg/trunk/sandbox/action_tools/msg/TestActionGoal.msg
pkg/trunk/sandbox/action_tools/msg/TestActionResult.msg
pkg/trunk/sandbox/action_tools/msg/TestGoal.msg
pkg/trunk/sandbox/action_tools/msg/TestResult.msg
pkg/trunk/sandbox/action_tools/src/move_base_client.cpp
pkg/trunk/sandbox/action_tools/src/move_base_server.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/milestone2:16602
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/nav_rework:17337
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/tags/milestone2_tags/milestone2_final:16577
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/action_branch:19631
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/milestone2:16602
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/nav_rework:17337
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/tags/milestone2_tags/milestone2_final:16577
Modified: pkg/trunk/highlevel/move_base_stage/move_base/move_base.xml
===================================================================
--- pkg/trunk/highlevel/move_base_stage/move_base/move_base.xml 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/highlevel/move_base_stage/move_base/move_base.xml 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,5 +1,5 @@
<launch>
- <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+ <node pkg="move_base" type="move_base_new" respawn="false" name="move_base" output="screen">
<param name="footprint_padding" value="0.01" />
<rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
Deleted: pkg/trunk/sandbox/action_tools/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/action_tools/CMakeLists.txt 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/CMakeLists.txt 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,25 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rospack(action_tools)
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-genmsg()
-
-rospack_add_executable(move_base_server src/move_base_server.cpp)
-rospack_add_executable(move_base_client src/move_base_client.cpp)
-
-#add_subdirectory(test)
Deleted: pkg/trunk/sandbox/action_tools/Makefile
===================================================================
--- pkg/trunk/sandbox/action_tools/Makefile 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/Makefile 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Deleted: pkg/trunk/sandbox/action_tools/include/action_tools/EnclosureDeleter.h
===================================================================
--- pkg/trunk/sandbox/action_tools/include/action_tools/EnclosureDeleter.h 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/include/action_tools/EnclosureDeleter.h 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,70 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of Willow Garage, Inc. nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-
-#include <boost/shared_ptr.hpp>
-
-#ifndef ACTION_TOOLS_ENCLOSURE_DELETER_H_
-#define ACTION_TOOLS_ENCLOSURE_DELETER_H_
-
-namespace action_tools
-{
-
-/*
- * This allows the creation of a shared pointer to a section
- * of an already reference counted structure. For example,
- * if in the following picture Enclosure is reference counted with
- * a boost::shared_ptr and you want to return a boost::shared_ptr
- * to the Member that is referenced counted along with Enclosure objects
- *
- * Enclosure --------------- <--- Already reference counted
- * -----Member <------- A member of enclosure objects, eg. Enclosure.Member
- */
-template <class Enclosure> class EnclosureDeleter {
- public:
- EnclosureDeleter(const boost::shared_ptr<Enclosure>& enc_ptr) : enc_ptr_(enc_ptr){}
-
- template<class Member> void operator()(Member* member_ptr){
- enc_ptr_.reset();
- }
-
- private:
- boost::shared_ptr<Enclosure> enc_ptr_;
-};
-
-}
-
-#endif
Deleted: pkg/trunk/sandbox/action_tools/include/action_tools/action_client.h
===================================================================
--- pkg/trunk/sandbox/action_tools/include/action_tools/action_client.h 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/include/action_tools/action_client.h 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,601 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#ifndef ACTION_TOOLS_ROBUST_ACTION_CLIENT_H_
-#define ACTION_TOOLS_ROBUST_ACTION_CLIENT_H_
-
-#include <boost/thread.hpp>
-#include "ros/ros.h"
-#include "action_tools/GoalStatus.h"
-#include "action_tools/Preempt.h"
-#include "action_tools/EnclosureDeleter.h"
-
-#include "action_tools/one_shot_timer.h"
-
-#define setState(next_state) \
-{ \
- ROS_DEBUG("Setting ClientState to " #next_state);\
- client_state_ = next_state;\
-}
-
-namespace action_tools
-{
-
-
-namespace TerminalStatuses
-{
- enum TerminalStatus { REJECTED, PREEMPTED, SUCCEEDED, ABORTED, TIMED_OUT, IGNORED, LOST } ;
-}
-
-
-template <class ActionGoal, class Goal, class ActionResult, class Result>
-class ActionClient
-{
-public:
- typedef boost::function<void (const TerminalStatuses::TerminalStatus&, const boost::shared_ptr<const Result>&)> CompletionCallback;
- typedef boost::function<void ()> AckTimeoutCallback;
- typedef boost::function<void ()> PreemptTimeoutCallback;
- typedef ActionClient<ActionGoal, Goal, ActionResult, Result> ActionClientT;
- typedef boost::function<void (void)> FilledCompletionCallback;
- typedef boost::shared_ptr<const ActionResult> ActionResultConstPtr;
- typedef boost::shared_ptr<const Result> ResultConstPtr;
- //typedef boost::function<void (const ros::TimerEvent& e)> AckTimeoutCallback;
-
- ActionClient(std::string name, ros::NodeHandle nh = ros::NodeHandle(),
- bool expecting_result=false,
- const ros::Duration& server_status_timeout = ros::Duration(5,0)) : nh_(nh, name)
- {
- // Initialize all One Shot Timers
- ack_timer_.registerOneShotCb(boost::bind(&ActionClientT::ackTimeoutCallback, this, _1));
- runtime_timer_.registerOneShotCb(boost::bind(&ActionClientT::runtimeTimeoutCallback, this, _1));
- wait_for_preempted_timer_.registerOneShotCb(boost::bind(&ActionClientT::waitForPreemptedTimeoutCallback, this, _1));
- comm_sync_timer_.registerOneShotCb(boost::bind(&ActionClientT::commSyncTimeoutCallback, this, _1));
-
- setState(IDLE);
- expecting_result_ = expecting_result;
-
- goal_pub_ = nh_.advertise<ActionGoal> ("goal", 1);
- preempt_pub_ = nh_.advertise<Preempt> ("preempt", 1);
- status_sub_ = nh_.subscribe("status", 1, &ActionClientT::statusCallback, this);
- result_sub_ = nh_.subscribe("result", 1, &ActionClientT::resultCallback, this);
-
- server_status_timeout_ = server_status_timeout;
- startServerStatusTimer();
- }
-
- void execute(const Goal& goal,
- CompletionCallback completion_callback = CompletionCallback(),
- const ros::Duration& runtime_timeout = ros::Duration(0,0),
- AckTimeoutCallback ack_timeout_callback = AckTimeoutCallback(),
- PreemptTimeoutCallback preempt_timeout_callback = PreemptTimeoutCallback(),
- const ros::Duration& ack_timeout = ros::Duration(5,0),
- const ros::Duration& wait_for_preempted_timeout = ros::Duration(5,0),
- const ros::Duration& comm_sync_timeout = ros::Duration(5,0))
- {
- ROS_DEBUG("Got call the execute()");
- boost::mutex::scoped_lock(client_state_mutex_);
-
- if (client_state_ == SERVER_INACTIVE)
- {
- ROS_ERROR("Trying to send an execute command to an inactive server");
- return;
- }
-
- terminal_status_ = TerminalStatuses::TIMED_OUT;
- cur_goal_.header.stamp = ros::Time::now();
- cur_goal_.goal_id.id = cur_goal_.header.stamp;
- cur_goal_.goal = goal;
- result_.reset();
- goal_pub_.publish(cur_goal_);
- setState(WAITING_FOR_ACK);
- runtime_timeout_ = runtime_timeout;
- wait_for_preempted_timeout_ = wait_for_preempted_timeout;
- comm_sync_timeout_ = comm_sync_timeout;
- completion_callback_ = completion_callback;
- ack_timeout_callback_ = ack_timeout_callback;
- preempt_timeout_callback_ = preempt_timeout_callback;
-
- // don't set an ACK timeout for the special case: duration==0
- if (ack_timeout == ros::Duration(0,0))
- ROS_DEBUG("Not setting a timeout for ACK");
- else
- {
- // Set/reset the timeout for WAITING_FOR_GOAL_ACK
- ROS_DEBUG("Starting the [%.2fs] timer for ACK timeout callback", ack_timeout.toSec());
- ack_timer_ = nh_.createTimer(ack_timeout, ack_timer_.getCb());
- }
- }
-
- TerminalStatuses::TerminalStatus waitUntilDone(ResultConstPtr& result)
- {
- ros::Duration sleep_duration = ros::Duration().fromSec(.1);
- while(true)
- {
- {
- boost::mutex::scoped_lock(client_state_mutex_);
- if (client_state_ == IDLE)
- {
- if (expecting_result_ && result_)
- {
- EnclosureDeleter<const ActionResult> d(result_);
- result = ResultConstPtr(&(result_->result), d);
- }
- else
- result = ResultConstPtr();
- return terminal_status_;
- }
- else if (client_state_ == SERVER_INACTIVE)
- {
- if (expecting_result_ && result_)
- {
- EnclosureDeleter<const ActionResult> d(result_);
- result = ResultConstPtr(&(result_->result), d);
- }
- else
- result = ResultConstPtr();
- return TerminalStatuses::TIMED_OUT;
- }
- }
- sleep_duration.sleep();
- }
- }
-
-private:
-
- boost::recursive_mutex client_state_mutex_;
- // ***** Lockset for client_state_mutex_ *****
-
- enum ClientState {SERVER_INACTIVE,IDLE, WAITING_FOR_ACK, PURSUING_GOAL, WAITING_FOR_PREEMPTED, WAITING_FOR_TERMINAL_STATE, WAITING_FOR_RESULT };
- ClientState client_state_;
- bool server_active_;
- ActionGoal cur_goal_;
- TerminalStatuses::TerminalStatus terminal_status_;
- ros::Duration runtime_timeout_; // Maximum time we're willing to stay in {PURSUING_GOAL, WAITING_FOR_TERMINAL_STATE, WAITING_FOR_RESULT} before Preempting
- ros::Duration wait_for_preempted_timeout_; // Maximum time we're willing to stay in WAITING_FOR_PREEMPTED until we release the goal as TIMED_OUT
- ros::Duration comm_sync_timeout_; // Maximum time we're willing to stay in WAITING_FOR_TERMINAL_STATE or WAITING_FOR_RESULT until we release the goal as TIMED_OUT
- bool expecting_result_;
- boost::shared_ptr<const ActionResult> result_;
- CompletionCallback completion_callback_;
- AckTimeoutCallback ack_timeout_callback_;
- PreemptTimeoutCallback preempt_timeout_callback_;
-
- // *******************************************
-
- //boost::recursive_mutex completion_cb_mutex_;
-
- // Various Timers
- OneShotTimer ack_timer_; //!< Tracks timeout in WAITING_FOR_ACK state
- OneShotTimer runtime_timer_; //!< Tracks timeout in PURSUING_GOAL state
- OneShotTimer wait_for_preempted_timer_; //!< Tracks timeout in WAITING_FOR_PREEMPTED state
- OneShotTimer comm_sync_timer_; //!< Tracks timeout in WAITING_FOR_RESULT or WAITING_FOR_TERMINAL_STATE
- ros::Timer server_status_timer_; //!< Forces client_state_ into SERVER_INACTIVE upon not getting status msgs.
- ros::Duration server_status_timeout_; //!< Duration before we decide that the server has 'died'
-
- ros::NodeHandle nh_;
- ros::Publisher goal_pub_;
- ros::Publisher preempt_pub_;
- ros::Subscriber status_sub_;
- ros::Subscriber result_sub_;
-
- void ackTimeoutCallback(const ros::TimerEvent& e)
- {
- boost::mutex::scoped_lock(client_state_mutex_);
- if ( client_state_ == WAITING_FOR_ACK )
- {
- ROS_WARN("Timed out waiting for ACK");
- terminal_status_ = TerminalStatuses::IGNORED;
- if (ack_timeout_callback_)
- ack_timeout_callback_();
- //completion_callback_(TerminalStatuses::IGNORED, ResultConstPtr());
- //setState(IDLE);
- }
- }
-
- void runtimeTimeoutCallback(const ros::TimerEvent& e)
- {
- boost::mutex::scoped_lock(client_state_mutex_);
- if ( client_state_ == PURSUING_GOAL )
- {
- ROS_WARN("Timed out waiting to finish PURSUING_GOAL");
- preemptGoal();
- }
- }
-
- void waitForPreemptedTimeoutCallback(const ros::TimerEvent& e)
- {
- boost::mutex::scoped_lock(client_state_mutex_);
- if ( client_state_ == WAITING_FOR_PREEMPTED )
- {
- ROS_WARN("Timed out waiting to finish WAITING_FOR_PREEMPTED");
- setState(IDLE);
- terminal_status_ = TerminalStatuses::TIMED_OUT;
- if (preempt_timeout_callback_)
- preempt_timeout_callback_();
- //if (completion_callback_)
- // completion_callback_(TerminalStatuses::TIMED_OUT, ResultConstPtr());
- }
- }
-
- /**
- * Handle the case when we've spent too much time in WAITING_FOR_RESULT or
- * WAITING_FOR_TERMINAL_STATE. We've timed out, but at least call the
- * completion callback with as much information as we've got.
- */
- void commSyncTimeoutCallback(const ros::TimerEvent& e)
- {
- boost::mutex::scoped_lock(client_state_mutex_);
- if (client_state_ == WAITING_FOR_TERMINAL_STATE)
- {
- if (!result_)
- ROS_ERROR("BUG: If we're waiting for a terminal state, then result_ MUST exist");
-
- setState(IDLE);
- terminal_status_ = TerminalStatuses::TIMED_OUT;
- if (completion_callback_)
- {
- EnclosureDeleter<const ActionResult> d(result_);
- completion_callback_(TerminalStatuses::TIMED_OUT, ResultConstPtr(&(result_->result), d));
- }
- }
- else if (client_state_ == WAITING_FOR_RESULT)
- {
- setState(IDLE);
- terminal_status_ = TerminalStatuses::TIMED_OUT;
- if (completion_callback_)
- completion_callback_(TerminalStatuses::TIMED_OUT, ResultConstPtr());
- }
- }
-
-
- void serverStatusTimeoutCallback(const ros::TimerEvent& e)
- {
- boost::mutex::scoped_lock(client_state_mutex_);
- if (client_state_ != SERVER_INACTIVE)
- {
- ROS_WARN("Timed out waiting on status pings from ActionServer for [%.2fs]. Assuming server is inactive", server_status_timeout_.toSec());
- setState(SERVER_INACTIVE);
- }
- }
-
- void gotStatusPing()
- {
- boost::mutex::scoped_lock(client_state_mutex_);
- startServerStatusTimer();
- if (client_state_ == SERVER_INACTIVE)
- {
- ROS_INFO("Started receiving status pings from ActionServer again");
- setState(IDLE);
- }
- }
-
- void startServerStatusTimer()
- {
- server_status_timer_ = nh_.createTimer(server_status_timeout_, boost::bind(&ActionClientT::serverStatusTimeoutCallback, this, _1));
- }
-
- void preemptGoal()
- {
- ROS_DEBUG("About to preemptGoal()");
- boost::mutex::scoped_lock(client_state_mutex_);
- if (client_state_ == PURSUING_GOAL || client_state_ == WAITING_FOR_ACK)
- {
- Preempt preempt;
- preempt.goal_id = cur_goal_.goal_id;
- preempt.header.stamp = cur_goal_.goal_id.id;
- preempt_pub_.publish(preempt);
- if (wait_for_preempted_timeout_ != ros::Duration(0,0))
- {
- ROS_DEBUG("Starting the [%.2fs] timer for the WAIT_FOR_PREEMPTED timeout", wait_for_preempted_timeout_.toSec());
- wait_for_preempted_timer_ = nh_.createTimer(wait_for_preempted_timeout_, wait_for_preempted_timer_.getCb());
- }
- else
- ROS_DEBUG("Infinte timeout for WAIT_FOR_PREEMPTED timeout");
- setState(WAITING_FOR_PREEMPTED);
- }
- else
- ROS_DEBUG("Not in a preemptable state (ClientState=%u)", client_state_);
- }
-
- void resultCallback(const ActionResultConstPtr& msg)
- {
- boost::mutex::scoped_lock(client_state_mutex_);
-
- if (client_state_ == IDLE || client_state_ == SERVER_INACTIVE)
- return;
-
- if (isCurrentGoal(msg->goal_id))
- {
- ROS_DEBUG("Got a result msg for this goal id");
- if (!expecting_result_)
- {
- ROS_WARN("ActionClient was told to not expect result msgs, yet we still got one");
- return;
- }
-
- switch(client_state_)
- {
- case WAITING_FOR_ACK:
- ack_timer_.stop();
- goToWaitingForTerminalState(msg);
- break;
- case PURSUING_GOAL:
- runtime_timer_.stop();
- goToWaitingForTerminalState(msg);
- break;
- case WAITING_FOR_PREEMPTED:
- wait_for_preempted_timer_.stop();
- goToWaitingForTerminalState(msg);
- break;
- case WAITING_FOR_RESULT:
- setState(IDLE);
- if (completion_callback_)
- {
- EnclosureDeleter<const ActionResult> d(msg);
- completion_callback_(terminal_status_, ResultConstPtr(&(msg->result), d));
- }
- break;
- case WAITING_FOR_TERMINAL_STATE:
- ROS_WARN("BUG: Got a 2nd ActionResult for the same goal");
- break;
- default:
- ROS_WARN("BUG: Should not ever get to this code");
- break;
- }
- }
- }
-
- void goToWaitingForTerminalState(const ActionResultConstPtr& result)
- {
- boost::mutex::scoped_lock(client_state_mutex_);
- setState(WAITING_FOR_TERMINAL_STATE);
- result_ = result;
- ROS_DEBUG("Starting the [%.2fs] timer for the WAITING_FOR_TERMINAL_STATE (CommunicationSync) timeout", comm_sync_timeout_.toSec());
- comm_sync_timer_ = nh_.createTimer(comm_sync_timeout_, comm_sync_timer_.getCb());
- }
-
- void statusCallback(const GoalStatusConstPtr& msg)
- {
- boost::mutex::scoped_lock(client_state_mutex_);
-
- // ***** ADD STATUS PING ****
- gotStatusPing();
-
- // Don't do any processing on idle messages
- if (msg->status == GoalStatus::IDLE)
- return;
-
- // Don't care about status if we're not even trying for a goal
- if (client_state_ == IDLE || client_state_ == SERVER_INACTIVE )
- return;
-
- if ( isFutureGoal(msg->goal_id) )
- {
- if (msg->status != GoalStatus::REJECTED)
- {
- ROS_DEBUG("Saw a future goal. Therefore, our goal somehow got lost during execution");
- setState(IDLE);
- terminal_status_ = TerminalStatuses::LOST;
- // Save the callback that we want to call, and call it later (outside the lock).
- if (completion_callback_)
- completion_callback_(TerminalStatuses::LOST, ResultConstPtr());
- }
- else
- {
- ROS_DEBUG("Saw a future goal be Rejected. Ignoring it, since we're ok with rejected future goals");
- }
- }
- else if( isCurrentGoal(msg->goal_id) )
- {
- if (client_state_ == WAITING_FOR_ACK)
- {
- switch (msg->status)
- {
- case GoalStatus::ACTIVE :
- ack_timer_.stop();
- setState(PURSUING_GOAL);
- if (runtime_timeout_ != ros::Duration(0,0))
- {
- ROS_DEBUG("Starting the [(%.2fs] timer for the PURSUING_GOAL timeout", runtime_timeout_.toSec());
- runtime_timer_ = nh_.createTimer(runtime_timeout_, runtime_timer_.getCb());
- }
- else
- ROS_DEBUG("Infinte timeout for PURSUING_GOAL timeout");
- break;
- case GoalStatus::PREEMPTED :
- case GoalStatus::SUCCEEDED :
- case GoalStatus::ABORTED :
- case GoalStatus::REJECTED :
- goToTerminalState(msg->status); break;
- default:
- ROS_DEBUG("Not sure how to handle State: %u", msg->status); break;
- }
- }
- else if (client_state_ == PURSUING_GOAL)
- {
- switch (msg->status)
- {
- case GoalStatus::ACTIVE :
- break;
- case GoalStatus::PREEMPTED :
- case GoalStatus::SUCCEEDED :
- case GoalStatus::ABORTED :
- case GoalStatus::REJECTED :
- runtime_timer_.stop();
- goToTerminalState(msg->status); break;
- default:
- ROS_DEBUG("Not sure how to handle State: %u", msg->status); break;
- }
- }
- else if (client_state_ == WAITING_FOR_PREEMPTED)
- {
- switch (msg->status)
- {
- case GoalStatus::ACTIVE :
- break;
- case GoalStatus::PREEMPTED :
- case GoalStatus::SUCCEEDED :
- case GoalStatus::ABORTED :
- case GoalStatus::REJECTED :
- wait_for_preempted_timer_.stop();
- goToTerminalState(msg->status); break;
- default:
- ROS_DEBUG("Not sure how to handle State: %u", msg->status); break;
- }
- }
- else if (client_state_ == WAITING_FOR_TERMINAL_STATE)
- {
- switch (msg->status)
- {
- case GoalStatus::PREEMPTED :
- case GoalStatus::SUCCEEDED :
- case GoalStatus::ABORTED :
- case GoalStatus::REJECTED :
- goToTerminalState(msg->status); break;
- case GoalStatus::ACTIVE : break;
- default:
- ROS_DEBUG("Not sure how to handle State: %u", msg->status); break;
- }
- }
- else if (client_state_ == WAITING_FOR_RESULT)
- {
- // Do nothing if we're already waiting for the result
- }
- else
- {
- ROS_DEBUG("In ClientState [%u]. Got goal status [%u], Ignoring it", client_state_, msg->status);
- }
- }
- }
-
- bool isFutureGoal(const GoalID& goal_id)
- {
- boost::mutex::scoped_lock(client_state_mutex_);
- return goal_id.id > cur_goal_.goal_id.id;
- }
-
- bool isCurrentGoal(const GoalID& goal_id)
- {
- boost::mutex::scoped_lock(client_state_mutex_);
- return goal_id.id == cur_goal_.goal_id.id;
- }
-
- void goToTerminalState(uint8_t status)
- {
- boost::mutex::scoped_lock(client_state_mutex_);
-
- if(expecting_result_ && !result_)
- {
- switch(status)
- {
- case GoalStatus::PREEMPTED:
- terminal_status_ = TerminalStatuses::PREEMPTED;
- goToWaitingForResult();
- break;
- case GoalStatus::SUCCEEDED:
- terminal_status_ = TerminalStatuses::SUCCEEDED;
- goToWaitingForResult();
- break;
- case GoalStatus::ABORTED:
- terminal_status_ = TerminalStatuses::ABORTED;
- goToWaitingForResult();
- break;
- case GoalStatus::REJECTED: // We don't wait for a result if we rejected a goal
- setState(IDLE);
- terminal_status_ = TerminalStatuses::REJECTED;
- if (completion_callback_)
- completion_callback_(TerminalStatuses::REJECTED, ResultConstPtr());
- break;
- default:
- ROS_WARN("BUG: Tried to go to a terminal status without receiving a terminal status. [GoalStatus.status==%u]", status);
- break;
- }
- }
- else
- {
- ResultConstPtr unwrapped_result;
- if (expecting_result_ && result_)
- {
- EnclosureDeleter<const ActionResult> d(result_);
- unwrapped_result = ResultConstPtr(&(result_->result), d);
- }
- else
- unwrapped_result.reset(); // If they weren't expecting a result or we never got one, then don't give them one
-
- switch(status)
- {
- case GoalStatus::PREEMPTED:
- setState(IDLE);
- terminal_status_ = TerminalStatuses::PREEMPTED;
- if (completion_callback_)
- completion_callback_(TerminalStatuses::PREEMPTED, unwrapped_result);
- break;
- case GoalStatus::SUCCEEDED:
- setState(IDLE);
- terminal_status_ = TerminalStatuses::SUCCEEDED;
- if (completion_callback_)
- completion_callback_(TerminalStatuses::SUCCEEDED, unwrapped_result);
- break;
- case GoalStatus::ABORTED:
- terminal_status_ = TerminalStatuses::ABORTED;
- setState(IDLE);
- if (completion_callback_)
- completion_callback_(TerminalStatuses::ABORTED, unwrapped_result);
- break;
- case GoalStatus::REJECTED:
- setState(IDLE);
- terminal_status_ = TerminalStatuses::REJECTED;
- if (completion_callback_)
- completion_callback_(TerminalStatuses::REJECTED, unwrapped_result);
- break;
- default:
- ROS_WARN("BUG: Tried to go to a terminal status without receiving a terminal status. [GoalStatus.status==%u]", status);
- break;
- }
- }
- return;
- }
-
- void goToWaitingForResult()
- {
- boost::mutex::scoped_lock(client_state_mutex_);
- setState(WAITING_FOR_RESULT);
- ROS_DEBUG("Starting the [%.2fs] timer for the WAITING_FOR_RESULT (CommunicationSync) timeout", comm_sync_timeout_.toSec());
- comm_sync_timer_ = nh_.createTimer(comm_sync_timeout_, comm_sync_timer_.getCb());
- }
-
-};
-
-}
-
-#endif // ACTION_TOOLS_ACTION_CLIENT_H_
Deleted: pkg/trunk/sandbox/action_tools/include/action_tools/action_server.h
===================================================================
--- pkg/trunk/sandbox/action_tools/include/action_tools/action_server.h 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/include/action_tools/action_server.h 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,310 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of Willow Garage, Inc. nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef ACTION_TOOLS_ACTION_SERVER
-#define ACTION_TOOLS_ACTION_SERVER
-
-#include <ros/ros.h>
-#include <boost/thread.hpp>
-#include <boost/shared_ptr.hpp>
-#include <action_tools/Preempt.h>
-#include <action_tools/GoalStatus.h>
-#include <action_tools/EnclosureDeleter.h>
-
-namespace action_tools {
- enum ActionServerState {
- IDLE,
- RUNNING
- };
-
- template <class ActionGoal, class Goal, class ActionResult, class Result>
- class ActionServer {
- public:
- //we need to create a GoalHandle class for this ActionServer
- class GoalHandle{
- public:
- GoalHandle(){}
-
- GoalHandle(const boost::shared_ptr<const ActionGoal>& action_goal, ActionServer<ActionGoal, Goal, ActionResult, Result>* action_server)
- : action_goal_(action_goal), action_server_(action_server){}
-
- boost::shared_ptr<const Goal> getGoal(){
- //if we have a goal that is non-null
- if(action_goal_){
- //create the deleter for our goal subtype
- EnclosureDeleter<const ActionGoal> d(action_goal_);
- return boost::shared_ptr<const Goal>(&(action_goal_->goal), d);
- }
- return boost::shared_ptr<const Goal>();
- }
-
- private:
- boost::shared_ptr<const ActionGoal> action_goal_;
- const ActionServer<ActionGoal, Goal, ActionResult, Result>* action_server_;
- friend class ActionServer<ActionGoal, Goal, ActionResult, Result>;
- };
-
- ActionServer(ros::NodeHandle n, std::string name, double status_frequency)
- : node_(n, name), new_goal_(false), preempt_request_(false) {
- status_pub_ = node_.advertise<action_tools::GoalStatus>("status", 1);
- result_pub_ = node_.advertise<ActionResult>("result", 1);
-
- goal_sub_ = node_.subscribe<ActionGoal>("goal", 1,
- boost::bind(&ActionServer::goalCallback, this, _1));
-
- preempt_sub_ = node_.subscribe<action_tools::Preempt>("preempt", 1,
- boost::bind(&ActionServer::preemptCallback, this, _1));
-
- status_timer_ = node_.createTimer(ros::Duration(1.0 / status_frequency),
- boost::bind(&ActionServer::publishStatus, this, _1));
-
- //initialize our goals to take advantage of the fact that the default constructor
- //for ros::Time gives a value of 0
- current_goal_ = boost::shared_ptr<const ActionGoal>(new ActionGoal());
- next_goal_ = boost::shared_ptr<const ActionGoal>(new ActionGoal());
-
- state_ = IDLE;
- }
-
- bool isNewGoalAvailable(){
- return new_goal_;
- }
-
- void sendResult(const Result& result){
- ActionResult r;
- r.header.stamp = ros::Time::now();
-
- lock_.lock();
- r.goal_id = current_goal_->goal_id;
- lock_.unlock();
-
- r.result = result;
- result_pub_.publish(r);
- }
-
- GoalHandle acceptNextGoal(){
- boost::mutex::scoped_lock(lock_);
-
- //check if we need to send a preempted message for the goal that we're currently pursuing
- if(isActive() && current_goal_->goal_id.id != next_goal_->goal_id.id){
- status_.status = status_.PREEMPTED;
- publishStatus();
- }
-
- //accept the next goal
- current_goal_ = next_goal_;
- new_goal_ = false;
-
- //generate the goal handle to return
- GoalHandle ret(current_goal_, this);
-
- status_.goal_id = current_goal_->goal_id;
- status_.status = status_.ACTIVE;
- state_ = RUNNING;
- publishStatus();
-
- return ret;
- }
-
- bool isPreempted(){
- return preempt_request_;
- }
-
- bool isActive(){
- return state_ == RUNNING;
- }
-
- void succeeded(const Result& result){
- boost::mutex::scoped_lock(lock_);
- sendResult(result);
- succeeded();
- }
-
- void succeeded(){
- boost::mutex::scoped_lock(lock_);
- if(state_ == IDLE){
- ROS_WARN("Trying to succeed on a goal that is already in a terminal state... doing nothing");
- return;
- }
- status_.status = status_.SUCCEEDED;
- state_ = IDLE;
- publishStatus();
- }
-
- void aborted(const Result& result){
- boost::mutex::scoped_lock(lock_);
- sendResult(result);
- aborted();
- }
-
- void aborted(){
- boost::mutex::scoped_lock(lock_);
- if(state_ == IDLE){
- ROS_WARN("Trying to abort on a goal that is already in a terminal state... doing nothing");
- return;
- }
- status_.status = status_.ABORTED;
- state_ = IDLE;
- publishStatus();
- }
-
- void preempted(const Result& result){
- boost::mutex::scoped_lock(lock_);
- sendResult(result);
- preempted();
- }
-
- void preempted(){
- boost::mutex::scoped_lock(lock_);
- if(state_ == IDLE){
- ROS_WARN("Trying to preempt on a goal that is already in a terminal state... doing nothing");
- return;
- }
- status_.status = status_.PREEMPTED;
- state_ = IDLE;
- publishStatus();
- }
-
- void registerGoalCallback(boost::function<void ()> cb){
- boost::mutex::scoped_lock(goal_cb_lock_);
- goal_callback_ = cb;
- }
-
- void registerPreemptCallback(boost::function<void ()> cb){
- boost::mutex::scoped_lock(preempt_cb_lock_);
- preempt_callback_ = cb;
- }
-
- private:
- void goalCallback(const boost::shared_ptr<const ActionGoal>& goal){
- ROS_DEBUG("The action server is in the ROS goal callback");
- boost::mutex::scoped_lock(lock_);
- //check that the timestamp is past that of the current goal, the next goal, and past that of the last preempt
- if(goal->header.stamp > current_goal_->header.stamp
- && goal->header.stamp > next_goal_->header.stamp
- && goal->header.stamp > last_preempt_.header.stamp){
- next_goal_ = goal;
- new_goal_ = true;
-
- //if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
- if(next_goal_->goal_id.id != current_goal_->goal_id.id){
- GoalStatus status;
- status.status = status.PREEMPTED;
- publishGoalStatus(*next_goal_, status);
- }
-
- //if the user has defined a goal callback, we'll call it now
- if(goal_callback_)
- goal_callback_();
- }
- else{
- //the goal requested has already been preempted, so we're not going to execute it
- GoalStatus status;
- status.status = status.PREEMPTED;
- publishGoalStatus(*goal, status);
- }
- }
-
- void preemptCallback(const boost::shared_ptr<const action_tools::Preempt>& preempt){
- boost::mutex::scoped_lock(lock_);
- ROS_DEBUG("In Preempt Callback");
-
- //check that the timestamp is past that of the current goal and the last preempt
- if(preempt->header.stamp >= current_goal_->header.stamp
- && preempt->header.stamp > last_preempt_.header.stamp){
- ROS_DEBUG("Setting preempt_request bit to TRUE");
- preempt_request_ = true;
- last_preempt_ = *preempt;
-
- //if the preempt also applies to the next goal, then we need to preempt it too
- //makes sure that the current and next goals are different
- //also makes sure to set new_goal_ to false if it had been set to true
- if(preempt->header.stamp >= next_goal_->header.stamp
- && next_goal_->goal_id.id != current_goal_->goal_id.id){
- GoalStatus status;
- status.status = status.PREEMPTED;
- publishGoalStatus(*next_goal_, status);
-
- if(new_goal_)
- new_goal_ = false;
- }
-
- //if the user has registered a preempt callback, we'll call it now
- if(preempt_callback_)
- preempt_callback_();
- }
- }
-
- void publishStatus(const ros::TimerEvent& e){
- boost::mutex::scoped_lock(lock_);
- publishStatus();
- }
-
- void publishStatus(){
- status_pub_.publish(status_);
- }
-
- void publishGoalStatus(const ActionGoal& goal, GoalStatus& status){
- //make sure that the status is published with the correct id and stamp
- status.header.stamp = ros::Time::now();
- status.goal_id = goal.goal_id;
- status_pub_.publish(status);
- }
-
- ros::NodeHandle node_;
-
- ros::Subscriber goal_sub_, preempt_sub_;
- ros::Publisher status_pub_, result_pub_;
-
- ActionServerState state_;
- boost::shared_ptr<const ActionGoal> current_goal_;
- boost::shared_ptr<const ActionGoal> next_goal_;
- action_tools::GoalStatus status_;
- action_tools::Preempt last_preempt_;
-
- bool new_goal_, preempt_request_;
-
- boost::recursive_mutex lock_;
-
- ros::Timer status_timer_;
-
- boost::function<void ()> goal_callback_;
- boost::function<void ()> preempt_callback_;
-
-
- };
-};
-#endif
Deleted: pkg/trunk/sandbox/action_tools/include/action_tools/one_shot_timer.h
===================================================================
--- pkg/trunk/sandbox/action_tools/include/action_tools/one_shot_timer.h 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/include/action_tools/one_shot_timer.h 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,89 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#ifndef ACTION_TOOLS_ROBUST_ONE_SHOT_TIMER_H_
-#define ACTION_TOOLS_ROBUST_ONE_SHOT_TIMER_H_
-
-#include "ros/ros.h"
-#include "boost/bind.hpp"
-
-//! Horrible hack until ROS Supports this (ROS Trac #1387)
-class OneShotTimer
-{
-public:
- OneShotTimer() : active_(false) { }
-
- void cb(const ros::TimerEvent& e)
- {
- if (active_)
- {
- active_ = false;
-
- if (callback_)
- callback_(e);
- else
- ROS_ERROR("Got a NULL Timer OneShotTimer Callback");
- }
- }
-
- boost::function<void (const ros::TimerEvent& e)> getCb()
- {
- return boost::bind(&OneShotTimer::cb, this, _1);
- }
-
- void registerOneShotCb(boost::function<void (const ros::TimerEvent& e)> callback)
- {
- callback_ = callback;
- }
-
- void stop()
- {
- //timer_.stop();
- active_ = false;
- }
-
- const ros::Timer& operator=(const ros::Timer& rhs)
- {
- active_ = true;
- timer_ = rhs;
- return timer_;
- }
-private:
- ros::Timer timer_;
- bool active_;
- boost::function<void (const ros::TimerEvent& e)> callback_;
-};
-
-
-#endif
Deleted: pkg/trunk/sandbox/action_tools/mainpage.dox
===================================================================
--- pkg/trunk/sandbox/action_tools/mainpage.dox 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/mainpage.dox 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,119 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b action_tools is ...
-
-<!--
-In addition to providing an overview of your package,
-this is the section where the specification and design/architecture
-should be detailed. While the original specification may be done on the
-wiki, it should be transferred here once your package starts to take shape.
-You can then link to this documentation page from the Wiki.
--->
-
-
-\section codeapi Code API
-
-<!--
-Provide links to specific auto-generated API documentation within your
-package that is of particular interest to a reader. Doxygen will
-document pretty much every part of your code, so do your best here to
-point the reader to the actual API.
-
-If your codebase is fairly large or has different sets of APIs, you
-should use the doxygen 'group' tag to keep these APIs together. For
-example, the roscpp documentation has 'libros' group.
--->
-
-\section rosapi ROS API
-
-<!--
-Names are very important in ROS because they can be remapped on the
-command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
-APPEAR IN THE CODE. You should list names of every topic, service and
-parameter used in your code. There is a template below that you can
-use to document each node separately.
-
-List of nodes:
-- \b node_name1
-- \b node_name2
--->
-
-<!-- START: copy from here to 'END' for each node
-
-<hr>
-
-\subsection node_name node_name
-
-node_name does (provide a basic description of your node)
-
-\subsubsection Usage
-\verbatim
-$ node_type1 [standard ROS args]
-\endverbatim
-
-\par Example
-
-\verbatim
-$ node_type1
-\endverbatim
-
-
-\subsubsection topics ROS topics
-
-Subscribes to:
-- \b "in": [std_msgs/FooType] description of in
-
-Publishes to:
-- \b "out": [std_msgs/FooType] description of out
-
-
-\subsubsection parameters ROS parameters
-
-Reads the following parameters from the parameter server
-
-- \b "~param_name" : \b [type] description of param_name
-- \b "~my_param" : \b [string] description of my_param
-
-Sets the following parameters on the parameter server
-
-- \b "~param_name" : \b [type] description of param_name
-
-
-\subsubsection services ROS services
-- \b "foo_service": [std_srvs/FooType] description of foo_service
-
-
-END: copy for each node -->
-
-
-<!-- START: Uncomment if you have any command-line tools
-
-\section commandline Command-line tools
-
-This section is a catch-all for any additional tools that your package
-provides or uses that may be of use to the reader. For example:
-
-- tools/scripts (e.g. rospack, roscd)
-- roslaunch .launch files
-- xmlparam files
-
-\subsection script_name script_name
-
-Description of what this script/file does.
-
-\subsubsection Usage
-\verbatim
-$ ./script_name [args]
-\endverbatim
-
-\par Example
-
-\verbatim
-$ ./script_name foo bar
-\endverbatim
-
-END: Command-Line Tools Section -->
-
-*/
\ No newline at end of file
Deleted: pkg/trunk/sandbox/action_tools/manifest.xml
===================================================================
--- pkg/trunk/sandbox/action_tools/manifest.xml 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/manifest.xml 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,23 +0,0 @@
-<package>
- <description brief="action_tools">
-
- action_tools
-
- </description>
- <author>Eitan Marder-Eppstein, Vijay Pradeep</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/action_tools</url>
-
- <depend package="robot_msgs" />
- <depend package="roscpp" />
-
- <depend package="gtest" />
-
- <export>
- <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/include" />
- </export>
-
-</package>
-
-
Deleted: pkg/trunk/sandbox/action_tools/msg/GoalID.msg
===================================================================
--- pkg/trunk/sandbox/action_tools/msg/GoalID.msg 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/msg/GoalID.msg 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1 +0,0 @@
-time id
\ No newline at end of file
Deleted: pkg/trunk/sandbox/action_tools/msg/GoalStatus.msg
===================================================================
--- pkg/trunk/sandbox/action_tools/msg/GoalStatus.msg 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/msg/GoalStatus.msg 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,9 +0,0 @@
-Header header
-GoalID goal_id
-uint8 status
-uint8 IDLE = 0
-uint8 ACTIVE = 1
-uint8 PREEMPTED = 2
-uint8 SUCCEEDED = 3
-uint8 ABORTED = 4
-uint8 REJECTED = 5
\ No newline at end of file
Deleted: pkg/trunk/sandbox/action_tools/msg/MoveBaseGoal.msg
===================================================================
--- pkg/trunk/sandbox/action_tools/msg/MoveBaseGoal.msg 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/msg/MoveBaseGoal.msg 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,3 +0,0 @@
-Header header
-GoalID goal_id
-robot_msgs/PoseStamped goal
Deleted: pkg/trunk/sandbox/action_tools/msg/MoveBaseResult.msg
===================================================================
--- pkg/trunk/sandbox/action_tools/msg/MoveBaseResult.msg 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/msg/MoveBaseResult.msg 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,3 +0,0 @@
-Header header
-GoalID goal_id
-robot_msgs/PoseStamped result
Deleted: pkg/trunk/sandbox/action_tools/msg/Preempt.msg
===================================================================
--- pkg/trunk/sandbox/action_tools/msg/Preempt.msg 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/msg/Preempt.msg 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,2 +0,0 @@
-Header header
-GoalID goal_id # Currently not being used(?)
\ No newline at end of file
Deleted: pkg/trunk/sandbox/action_tools/msg/TestActionGoal.msg
===================================================================
--- pkg/trunk/sandbox/action_tools/msg/TestActionGoal.msg 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/msg/TestActionGoal.msg 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,3 +0,0 @@
-Header header
-GoalID goal_id
-TestGoal goal
Deleted: pkg/trunk/sandbox/action_tools/msg/TestActionResult.msg
===================================================================
--- pkg/trunk/sandbox/action_tools/msg/TestActionResult.msg 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/msg/TestActionResult.msg 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,3 +0,0 @@
-Header header
-GoalID goal_id
-TestResult result
\ No newline at end of file
Deleted: pkg/trunk/sandbox/action_tools/msg/TestGoal.msg
===================================================================
--- pkg/trunk/sandbox/action_tools/msg/TestGoal.msg 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/msg/TestGoal.msg 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1,6 +0,0 @@
-uint8 cmd
-uint8 QUICK_SUCCESS = 1
-uint8 SLOW_SUCCESS = 2
-uint8 NEVER_REAL_GOAL = 3
-uint8 FAILING_PREEMPT = 4
-uint8 REJECT = 5
\ No newline at end of file
Deleted: pkg/trunk/sandbox/action_tools/msg/TestResult.msg
===================================================================
--- pkg/trunk/sandbox/action_tools/msg/TestResult.msg 2009-07-24 22:00:51 UTC (rev 19632)
+++ pkg/trunk/sandbox/action_tools/msg/TestResult.msg 2009-07-24 22:03:46 UTC (rev 19633)
@@ -1 +0,0 @@
-uint8 result
\ No newline at end of file
Deleted: pkg/trunk/sandbox/action_tools/src/move_base_client.cpp
===================================================================
--- pkg/trunk/sandbox/act...
[truncated message content] |
