From 0bc7a69954a21bec9bd00aefd4f185c6c96c5810 Mon Sep 17 00:00:00 2001 From: Hugo Laloge Date: Thu, 14 Oct 2021 17:25:14 +0200 Subject: [PATCH 1/2] rtt_actionlib: implement action client --- .../include/rtt_actionlib/rtt_action_client.h | 212 +++++++++++++ .../rtt_actionlib/rtt_simple_action_client.h | 295 ++++++++++++++++++ 2 files changed, 507 insertions(+) create mode 100644 rtt_actionlib/include/rtt_actionlib/rtt_action_client.h create mode 100644 rtt_actionlib/include/rtt_actionlib/rtt_simple_action_client.h diff --git a/rtt_actionlib/include/rtt_actionlib/rtt_action_client.h b/rtt_actionlib/include/rtt_actionlib/rtt_action_client.h new file mode 100644 index 00000000..fed66ea6 --- /dev/null +++ b/rtt_actionlib/include/rtt_actionlib/rtt_action_client.h @@ -0,0 +1,212 @@ +#ifndef __RTT_ACTION_CLIENT_H +#define __RTT_ACTION_CLIENT_H + +#include +#include +#include +#include + +namespace rtt_actionlib +{ + +//! Orocos RTT-Based Action Client +template +class RTTActionClient +{ +public: + // Generates typedefs that make our lives easier + ACTION_DEFINITION(ActionSpec); + + typedef actionlib::ClientGoalHandle GoalHandle; + typedef boost::function TransitionCallback; + typedef boost::function FeedbackCallback; + typedef boost::function SendGoalFunc; + +public: + RTTActionClient(); + ~RTTActionClient() = default; + + //! Add actionlib ports to a given rtt service + bool addPorts(RTT::Service::shared_ptr service, + const bool create_topics = false, + const std::string &topic_namespace = ""); + + //! Check if the client is ready to be started + bool ready() const; + + /** + * \brief Sends a goal to the ActionServer, and also registers callbacks + * \param transition_cb Callback that gets called on every client state transition + * \param feedback_cb Callback that gets called whenever feedback for this goal is received + */ + GoalHandle sendGoal(const Goal &goal, + TransitionCallback transition_cb = TransitionCallback(), + FeedbackCallback feedback_cb = FeedbackCallback()); + + /** + * \brief Cancel all goals currently running on the action server + * + * This preempts all goals running on the action server at the point that + * this message is serviced by the ActionServer. + */ + void cancelAllGoals(); + + /** + * \brief Cancel all goals that were stamped at and before the specified time + * \param time All goals stamped at or before `time` will be canceled + */ + void cancelGoalsAtAndBeforeTime(const ros::Time &time); + +private: + void sendGoalFunc(const ActionGoalConstPtr &action_goal); + void sendCancelFunc(const actionlib_msgs::GoalID &cancel_msg); + + void resultCallback(RTT::base::PortInterface *); + void statusCallback(RTT::base::PortInterface *); + void feedbackCallback(RTT::base::PortInterface *); + +private: + boost::shared_ptr guard_; + ///! ROS Actionlib GoalManager + actionlib::GoalManager manager_; + + //! Action bridge container for RTT ports corresponding to the action interface + rtt_actionlib::ActionBridge action_bridge_; +}; + +template +RTTActionClient::RTTActionClient() : guard_ {new actionlib::DestructionGuard {}} + , manager_ {guard_} +{ + manager_.registerSendGoalFunc(boost::bind(&RTTActionClient::sendGoalFunc, this, _1)); + manager_.registerCancelFunc(boost::bind(&RTTActionClient::sendCancelFunc, this, _1)); +} + +template +bool RTTActionClient::addPorts(RTT::Service::shared_ptr service, + const bool create_topics, + const string &topic_namespace) +{ + // Try to get existing ports from service + if (!action_bridge_.setPortsFromService(service)) + { + // Create the ports + if (!action_bridge_.createClientPorts()) + { + return false; + } + } + + // Add the ports to the service + service->addPort(action_bridge_.goalOutput()) + .doc("Actionlib goal port. Do not write to this port directly."); + + service->addPort(action_bridge_.cancelOutput()).doc("Actionlib cancel port. Do not write to this port directly."); + + service->addEventPort(action_bridge_.resultInput(), + boost::bind(&RTTActionClient::resultCallback, this, _1)) + .doc("Actionlib result port. Do not read from this port directly."); + + service->addEventPort(action_bridge_.statusInput(), + boost::bind(&RTTActionClient::statusCallback, this, _1)) + .doc("Actionlib status port. Do not read from this port directly."); + + service->addEventPort(action_bridge_.feedbackInput(), + boost::bind(&RTTActionClient::feedbackCallback, this, _1)) + .doc("Actionlib feedback port. Do not read from this port directly."); + + // Create ROS topics + if (create_topics) + { + action_bridge_.goalOutput().createStream(rtt_roscomm::topic(topic_namespace + "goal")); + action_bridge_.cancelOutput().createStream(rtt_roscomm::topic(topic_namespace + "cancel")); + action_bridge_.resultInput().createStream(rtt_roscomm::topic(topic_namespace + "result")); + action_bridge_.statusInput().createStream(rtt_roscomm::topic(topic_namespace + "status")); + action_bridge_.feedbackInput().createStream(rtt_roscomm::topic(topic_namespace + "feedback")); + } + + return true; +} + +template +bool RTTActionClient::ready() const +{ + return action_bridge_.allConnected(); +} + +template +void RTTActionClient::sendGoalFunc(const RTTActionClient::ActionGoalConstPtr &action_goal) +{ + action_bridge_.goalOutput().write(*action_goal); +} + +template +void RTTActionClient::sendCancelFunc(const actionlib_msgs::GoalID &cancel_msg) +{ + action_bridge_.cancelOutput().write(cancel_msg); +} + +template +void RTTActionClient::resultCallback(RTT::base::PortInterface *) +{ + auto actionResult = boost::make_shared(); + + if (action_bridge_.resultInput().read(*actionResult) == RTT::NewData) + { + manager_.updateResults(std::move(actionResult)); + } +} + +template +void RTTActionClient::statusCallback(RTT::base::PortInterface *) +{ + auto goalStatus = boost::make_shared(); + + if (action_bridge_.statusInput().read(*goalStatus) == RTT::NewData) + { + manager_.updateStatuses(std::move(goalStatus)); + } +} + +template +void RTTActionClient::feedbackCallback(RTT::base::PortInterface *) +{ + auto actionFeedback = boost::make_shared(); + + if (action_bridge_.feedbackInput().read(*actionFeedback) == RTT::NewData) + { + manager_.updateFeedbacks(std::move(actionFeedback)); + } +} + +template +typename RTTActionClient::GoalHandle +RTTActionClient::sendGoal(const Goal &goal, + RTTActionClient::TransitionCallback transition_cb, + RTTActionClient::FeedbackCallback feedback_cb) +{ + return manager_.initGoal(goal, transition_cb, feedback_cb); +} + +template +void RTTActionClient::cancelAllGoals() +{ + actionlib_msgs::GoalID cancel_msg; + // CancelAll policy encoded by stamp=0, id=0 + cancel_msg.stamp = ros::Time(0, 0); + cancel_msg.id = ""; + action_bridge_.cancelOutput().write(cancel_msg); +} + +template +void RTTActionClient::cancelGoalsAtAndBeforeTime(const ros::Time &time) +{ + actionlib_msgs::GoalID cancel_msg; + cancel_msg.stamp = time; + cancel_msg.id = ""; + action_bridge_.cancelOutput().write(cancel_msg); +} + +} // namespace rtt_actionlib. + +#endif // __RTT_ACTION_CLIENT_H diff --git a/rtt_actionlib/include/rtt_actionlib/rtt_simple_action_client.h b/rtt_actionlib/include/rtt_actionlib/rtt_simple_action_client.h new file mode 100644 index 00000000..e4ee6303 --- /dev/null +++ b/rtt_actionlib/include/rtt_actionlib/rtt_simple_action_client.h @@ -0,0 +1,295 @@ +#ifndef RTT_SIMPLE_ACTION_CLIENT_HPP +#define RTT_SIMPLE_ACTION_CLIENT_HPP + +#include +#include +#include +#include + +#include "rtt_action_client.h" + +namespace rtt_actionlib +{ + +template +class RTTSimpleActionClient +{ +public: + // Generates typedefs that make our lives easier + ACTION_DEFINITION(ActionSpec); + typedef actionlib::ClientGoalHandle GoalHandle; + typedef boost::function + SimpleDoneCallback; + typedef boost::function SimpleActiveCallback; + typedef boost::function SimpleFeedbackCallback; + +public: + explicit RTTSimpleActionClient(boost::shared_ptr owner_service); + ~RTTSimpleActionClient() = default; + + /** + * Check all connections. + */ + bool ready() const { return action_client_.ready(); } + + /** + * \brief Sends a goal to the ActionServer. Discard old goal if exists. + */ + void sendGoal(const Goal &goal); + + /** + * \brief Get the state information for this goal + * + * Possible States Are: PENDING, ACTIVE, RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST. + * \return The goal's state. Returns LOST if this SimpleActionClient isn't tracking a goal. + */ + actionlib::SimpleClientGoalState getState() const; + + /** + * \brief Cancel the goal that we are currently pursuing + */ + void cancelGoal(); + + /** + * @brief Setup active hook. + * @param resultHook A function, being called if current goal become active. + **/ + void setActiveHook(SimpleActiveCallback _activeHook) { activeHook_ = std::move(_activeHook); } + + /** + * @brief Setup result hook. + * @param resultHook A function, being called if the goal is done. + **/ + void setDoneHook(SimpleDoneCallback _doneHook) { doneHook_ = std::move(_doneHook); } + + /** + * @brief Setup feedback hook. + * @param feedbackHook A function, being called when active goal has a feedback. + **/ + void setFeedbackHook(SimpleFeedbackCallback _feedbackHook) { feedbackHook_ = std::move(_feedbackHook); } + +private: + void handleFeedback(GoalHandle gh, const FeedbackConstPtr &feedback); + void handleTransition(GoalHandle gh); + +private: + RTTActionClient action_client_; + + GoalHandle gh_; + actionlib::SimpleGoalState cur_simple_state_; + + SimpleActiveCallback activeHook_; + SimpleDoneCallback doneHook_; + SimpleFeedbackCallback feedbackHook_; +}; + +template +RTTSimpleActionClient::RTTSimpleActionClient(boost::shared_ptr owner_service) + : cur_simple_state_ {actionlib::SimpleGoalState::PENDING} +{ + if (!owner_service) + throw std::invalid_argument("RTTSimpleActionClient: owner pointer must be valid."); + action_client_.addPorts(owner_service); +} + +template +void RTTSimpleActionClient::sendGoal(const Goal &goal) +{ + gh_.reset(); + cur_simple_state_ = actionlib::SimpleGoalState::PENDING; + gh_ = action_client_.sendGoal(goal, + boost::bind(&RTTSimpleActionClient::handleTransition, this, _1), + boost::bind(&RTTSimpleActionClient::handleFeedback, this, _1, _2)); +} + +template +void RTTSimpleActionClient::cancelGoal() +{ + if (gh_.isExpired()) + { + RTT::Logger::In in {"RTTSimpleActionClient"}; + RTT::log(RTT::Error) + << "Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient" + << RTT::endlog(); + } + + gh_.cancel(); +} + +template +actionlib::SimpleClientGoalState RTTSimpleActionClient::getState() const +{ + // Code copied from actionlib/client/simple_action_client.h + RTT::Logger::In in {"RTTSimpleActionClient"}; + + if (gh_.isExpired()) + { + RTT::log(RTT::Error) + << "Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient"; + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::LOST); + } + + actionlib::CommState comm_state_ = gh_.getCommState(); + + switch (comm_state_.state_) + { + case actionlib::CommState::WAITING_FOR_GOAL_ACK: + case actionlib::CommState::PENDING: + case actionlib::CommState::RECALLING: + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::PENDING); + case actionlib::CommState::ACTIVE: + case actionlib::CommState::PREEMPTING: + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::ACTIVE); + case actionlib::CommState::DONE: + { + switch (gh_.getTerminalState().state_) + { + case actionlib::TerminalState::RECALLED: + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::RECALLED, + gh_.getTerminalState().text_); + case actionlib::TerminalState::REJECTED: + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::REJECTED, + gh_.getTerminalState().text_); + case actionlib::TerminalState::PREEMPTED: + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::PREEMPTED, + gh_.getTerminalState().text_); + case actionlib::TerminalState::ABORTED: + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::ABORTED, + gh_.getTerminalState().text_); + case actionlib::TerminalState::SUCCEEDED: + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::SUCCEEDED, + gh_.getTerminalState().text_); + case actionlib::TerminalState::LOST: + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::LOST, + gh_.getTerminalState().text_); + default: + RTT::log(RTT::Error) << "Unknown terminal state [" << gh_.getTerminalState().state_ + << "]. This is a bug in RTTSimpleActionClient" << RTT::endlog(); + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::LOST, + gh_.getTerminalState().text_); + } + } + case actionlib::CommState::WAITING_FOR_RESULT: + case actionlib::CommState::WAITING_FOR_CANCEL_ACK: + { + switch (cur_simple_state_.state_) + { + case actionlib::SimpleGoalState::PENDING: + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::PENDING); + case actionlib::SimpleGoalState::ACTIVE: + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::ACTIVE); + case actionlib::SimpleGoalState::DONE: + RTT::log(RTT::Error) << "In WAITING_FOR_RESULT or WAITING_FOR_CANCEL_ACK, yet we are in " + "SimpleGoalState DONE. This is a bug in RTTSimpleActionClient" + << RTT::endlog(); + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::LOST); + default: + RTT::log(RTT::Error) << "\"Got a SimpleGoalState of [" << cur_simple_state_.state_ + << "]. This is a bug in SimpleActionClient\"" << RTT::endlog(); + } + } + default: break; + } + RTT::log(RTT::Error) << "Error trying to interpret CommState - " << comm_state_.state_ << RTT::endlog(); + return actionlib::SimpleClientGoalState(actionlib::SimpleClientGoalState::LOST); +} + +template +void RTTSimpleActionClient::handleFeedback(RTTSimpleActionClient::GoalHandle gh, + const RTTSimpleActionClient::FeedbackConstPtr &feedback) +{ + if (gh_ != gh) + { + RTT::Logger::In in {"RTTSimpleActionClient"}; + RTT::log(RTT::Error) << "Got a callback on a goalHandle that we're not tracking. \ + This is an internal rtt_action bug. \ + This could also be a GoalID collision" + << RTT::endlog(); + } + + if (feedbackHook_) + feedbackHook_(*feedback); +} + +template +void RTTSimpleActionClient::handleTransition(RTTSimpleActionClient::GoalHandle gh) +{ + // Code copied from actionlib/client/simple_action_client.h + + RTT::Logger::In in {"RTTSimpleActionClient"}; + actionlib::CommState comm_state_ = gh.getCommState(); + switch (comm_state_.state_) + { + case actionlib::CommState::WAITING_FOR_GOAL_ACK: + RTT::log(RTT::Error) << "BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK" + << RTT::endlog(); + break; + case actionlib::CommState::PENDING: + if (cur_simple_state_ != actionlib::SimpleGoalState::PENDING) + RTT::log(RTT::Error) << "BUG: Got a transition to CommState [" << comm_state_.toString() + << "] when our in SimpleGoalState [" << cur_simple_state_.toString() << "]" + << RTT::endlog(); + break; + case actionlib::CommState::ACTIVE: + switch (cur_simple_state_.state_) + { + case actionlib::SimpleGoalState::PENDING: + cur_simple_state_ = actionlib::SimpleGoalState::ACTIVE; + if (activeHook_) + activeHook_(); + break; + case actionlib::SimpleGoalState::ACTIVE: break; + case actionlib::SimpleGoalState::DONE: + RTT::log(RTT::Error) << "BUG: Got a transition to CommState [" << comm_state_.toString() + << "] when our in SimpleGoalState [" << cur_simple_state_.toString() << "]" + << RTT::endlog(); + break; + } + break; + case actionlib::CommState::WAITING_FOR_RESULT: break; + case actionlib::CommState::WAITING_FOR_CANCEL_ACK: break; + case actionlib::CommState::RECALLING: + RTT::log(RTT::Error) << "BUG: Got a transition to CommState [" << comm_state_.toString() + << "] when our in SimpleGoalState [" << cur_simple_state_.toString() << "]" + << RTT::endlog(); + break; + case actionlib::CommState::PREEMPTING: + switch (cur_simple_state_.state_) + { + case actionlib::SimpleGoalState::PENDING: + cur_simple_state_ = actionlib::SimpleGoalState::ACTIVE; + if (activeHook_) + activeHook_(); + break; + case actionlib::SimpleGoalState::ACTIVE: break; + case actionlib::SimpleGoalState::DONE: + RTT::log(RTT::Error) << "BUG: Got a transition to CommState [" << comm_state_.toString() + << "] when our in SimpleGoalState [" << cur_simple_state_.toString() << "]" + << RTT::endlog(); + break; + } + break; + case actionlib::CommState::DONE: + switch (cur_simple_state_.state_) + { + case actionlib::SimpleGoalState::PENDING: + case actionlib::SimpleGoalState::ACTIVE: + cur_simple_state_ = actionlib::SimpleGoalState::DONE; + + if (doneHook_) + doneHook_(getState(), *gh.getResult()); + break; + case actionlib::SimpleGoalState::DONE: + RTT::log(RTT::Error) << "BUG: Got a second transition to DONE" << RTT::endlog(); + break; + } + break; + default: + RTT::log(RTT::Error) << "Unknown CommState received [%u]" << comm_state_.state_ << RTT::endlog(); + break; + } +} + +} // namespace rtt_actionlib. + +#endif // RTT_SIMPLE_ACTION_CLIENT_HPP From d15bde52f1b40a3c4c5ab21bbc3b6263c8ef9a63 Mon Sep 17 00:00:00 2001 From: Hugo Laloge Date: Thu, 14 Oct 2021 17:31:04 +0200 Subject: [PATCH 2/2] rtt_actionlib: add some documentation for the action client --- rtt_actionlib/README.md | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/rtt_actionlib/README.md b/rtt_actionlib/README.md index fa8df70b..c3dde7b2 100644 --- a/rtt_actionlib/README.md +++ b/rtt_actionlib/README.md @@ -45,14 +45,16 @@ binds the goal and cancel callbacks to RTT event ports. ### Calling Actions from Orocos -TBD +The RTTActionClient is implemented as a mirror of RTTActionServer. The main +function is sendGoal, which allow to send a new goal and subscribe for +transition and feebacks. Usage ----- First the appropriate RTT ports need to be created in a given service (or subservice) of a TaskContext. These can be easily creted by delegating to an -RTTActionServer. The RTTActionServer will create the necessary RTT ports and +RTTActionServer or RTTActionClient. It will create the necessary RTT ports and bind them to the user-supplied callbacks. For example, to add an actionlib interface to a given compnent, you could do something similar to the following: @@ -233,9 +235,3 @@ Future Work * Add operation to actionlib service which connects goal/cancel callbacks to given RTT operations so that any RTT component with operations with the right types can be bound to an actionlib service -* Add action client support. - - - - -