35 #ifndef ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_ 36 #define ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_ 38 #include <boost/thread/condition.hpp> 39 #include <boost/thread/mutex.hpp> 40 #include <boost/scoped_ptr.hpp> 41 #include <boost/concept_check.hpp> 54 #define DEPRECATED __attribute__((deprecated)) 71 template<
class ActionSpec>
133 return ac_->waitForActionServerToStart(timeout);
142 return ac_->isServerConnected();
244 boost::scoped_ptr<ActionClientT>
ac_;
249 void handleFeedback(GoalHandleT gh,
const FeedbackConstPtr & feedback);
256 template<
class ActionSpec>
261 ROS_DEBUG_NAMED(
"actionlib",
"Spinning up a thread for the SimpleActionClient");
272 template<
class ActionSpec>
287 template<
class ActionSpec>
301 template<
class ActionSpec>
307 template<
class ActionSpec>
310 ROS_DEBUG_NAMED(
"actionlib",
"Transitioning SimpleState from [%s] to [%s]",
316 template<
class ActionSpec>
318 SimpleDoneCallback done_cb,
319 SimpleActiveCallback active_cb,
320 SimpleFeedbackCallback feedback_cb)
337 template<
class ActionSpec>
346 switch (comm_state_.
state_) {
376 "Unknown terminal state [%u]. This is a bug in SimpleActionClient",
391 "In WAITING_FOR_RESULT or WAITING_FOR_CANCEL_ACK, yet we are in SimpleGoalState DONE. This is a bug in SimpleActionClient");
395 "Got a SimpleGoalState of [%u]. This is a bug in SimpleActionClient",
406 template<
class ActionSpec>
412 "Trying to getResult() when no goal is running. You are incorrectly using SimpleActionClient");
419 return ResultConstPtr(
new Result);
423 template<
class ActionSpec>
426 ac_->cancelAllGoals();
429 template<
class ActionSpec>
432 ac_->cancelGoalsAtAndBeforeTime(time);
435 template<
class ActionSpec>
440 "Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient");
446 template<
class ActionSpec>
451 "Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient");
456 template<
class ActionSpec>
458 const FeedbackConstPtr & feedback)
462 "Got a callback on a goalHandle that we're not tracking. \ 463 This is an internal SimpleActionClient/ActionClient bug. \ 464 This could also be a GoalID collision");
471 template<
class ActionSpec>
475 switch (comm_state_.
state_) {
478 "BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK");
482 "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
497 "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
511 "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
526 "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
563 template<
class ActionSpec>
568 "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
573 ROS_WARN_NAMED(
"actionlib",
"Timeouts can't be negative. Timeout is [%.2fs]", timeout.
toSec());
599 time_left = loop_period;
603 boost::posix_time::milliseconds(static_cast<int64_t>(time_left.
toSec() * 1000.0f)));
609 template<
class ActionSpec>
618 ROS_DEBUG_NAMED(
"actionlib",
"Goal finished within specified execute_timeout [%.2f]",
619 execute_timeout.
toSec());
623 ROS_DEBUG_NAMED(
"actionlib",
"Goal didn't finish within specified execute_timeout [%.2f]",
624 execute_timeout.
toSec());
631 ROS_DEBUG_NAMED(
"actionlib",
"Preempt finished within specified preempt_timeout [%.2f]",
632 preempt_timeout.
toSec());
634 ROS_DEBUG_NAMED(
"actionlib",
"Preempt didn't finish specified preempt_timeout [%.2f]",
635 preempt_timeout.
toSec());
644 #endif // ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_
boost::scoped_ptr< ActionClientT > ac_
SimpleDoneCallback done_cb_
std::string toString() const
boost::thread * spin_thread_
boost::function< void(const FeedbackConstPtr &feedback)> SimpleFeedbackCallback
ResultConstPtr getResult() const
Get the Result of the current goal.
#define ROS_WARN_NAMED(name,...)
boost::condition done_condition_
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
Blocks until this goal finishes.
bool isServerConnected() const
Checks if the action client is successfully connected to the action server.
ACTION_DEFINITION(ActionSpec)
ros::CallbackQueue callback_queue
SimpleActionClient< ActionSpec > SimpleActionClientT
Full interface to an ActionServer.
#define ROS_ERROR_COND(cond,...)
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
Waits for the ActionServer to connect to this client.
SimpleActionClient(const std::string &name, bool spin_thread=true)
Simple constructor.
#define ROS_DEBUG_NAMED(name,...)
boost::function< void()> SimpleActiveCallback
StateEnum
Defines the various states the SimpleGoalState can be in.
Thin wrapper around an enum in order to help interpret the state of the communication state machine...
ClientGoalHandle< ActionSpec > GoalHandleT
Thin wrapper around an enum in order providing a simplified version of the communication state...
SimpleClientGoalState getState() const
Get the state information for this goal.
void cancelGoal()
Cancel the goal that we are currently pursuing.
CommState getCommState() const
Get the state of this goal's communication state machine from interaction with the server...
std::string toString() const
Duration & fromSec(double t)
bool isExpired() const
Checks if this goal handle is tracking a goal.
void setSimpleState(const SimpleGoalState::StateEnum &next_state)
SimpleClientGoalState sendGoalAndWait(const Goal &goal, const ros::Duration &execute_timeout=ros::Duration(0, 0), const ros::Duration &preempt_timeout=ros::Duration(0, 0))
Sends a goal to the ActionServer, and waits until the goal completes or a timeout is exceeded...
void cancelGoalsAtAndBeforeTime(const ros::Time &time)
Cancel all goals that were stamped at and before the specified time.
A Simple client implementation of the ActionInterface which supports only one goal at a time...
SimpleGoalState cur_simple_state_
SimpleActionClient(ros::NodeHandle &n, const std::string &name, bool spin_thread=true)
Constructor with namespacing options.
void handleTransition(GoalHandleT gh)
boost::function< void(const SimpleClientGoalState &state, const ResultConstPtr &result)> SimpleDoneCallback
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
Sends a goal to the ActionServer, and also registers callbacks.
SimpleActiveCallback active_cb_
ResultConstPtr getResult() const
Get result associated with this goal.
boost::mutex terminate_mutex_
void reset()
Stops goal handle from tracking a goal.
#define ROS_ERROR_NAMED(name,...)
void cancel()
Sends a cancel message for this specific goal to the ActionServer.
void stopTrackingGoal()
Stops tracking the state of the current goal. Unregisters this goal's callbacks.
ActionClient< ActionSpec > ActionClientT
Client side handle to monitor goal progress.
void initSimpleClient(ros::NodeHandle &n, const std::string &name, bool spin_thread)
void cancelAllGoals()
Cancel all goals currently running on the action server.
void handleFeedback(GoalHandleT gh, const FeedbackConstPtr &feedback)
SimpleFeedbackCallback feedback_cb_
TerminalState getTerminalState() const
Get the terminal state information for this goal.