From 6b6cd1b5b7047090b3a461cdacab6b452af73369 Mon Sep 17 00:00:00 2001 From: Paul Mathieu Date: Fri, 21 Feb 2014 13:17:07 +0100 Subject: [PATCH 1/3] Refactor and document MoveJointGroup --- .../include/play_motion/move_joint_group.h | 65 +++++++++++++++---- play_motion/src/move_joint_group.cpp | 31 +++++++++ 2 files changed, 82 insertions(+), 14 deletions(-) diff --git a/play_motion/include/play_motion/move_joint_group.h b/play_motion/include/play_motion/move_joint_group.h index 863f972..f2d0168 100644 --- a/play_motion/include/play_motion/move_joint_group.h +++ b/play_motion/include/play_motion/move_joint_group.h @@ -56,7 +56,7 @@ namespace play_motion { private: typedef actionlib::SimpleActionClient - ActionClient; + ActionClient; typedef control_msgs::FollowJointTrajectoryGoal ActionGoal; typedef control_msgs::FollowJointTrajectoryResult ActionResult; typedef boost::shared_ptr ActionResultPtr; @@ -64,26 +64,63 @@ namespace play_motion public: MoveJointGroup(const std::string& controller_name, const JointNames& joint_names); + + /** + * \brief Send a trajectory goal to the associated controller. + * \param traj The trajectory to send + * \param duration (misnamed) Time to reach the first point + */ bool sendGoal(const std::vector& traj, const ros::Duration& duration); + + /** + * \brief Returns true if the specified joint is controlled by the controller. + * \pram joint_name Joint name + */ bool isControllingJoint(const std::string& joint_name); - bool isIdle() { return !busy_; } - void cancel() { busy_ = false; client_.cancelAllGoals(); } - void setCallback(const Callback& cb) { active_cb_ = cb; } - const JointNames& getJointNames() const {return joint_names_;} - actionlib::SimpleClientGoalState getState() {return client_.getState();} - const std::string& getName() { return controller_name_; } + /** + * \brief Returns true if the MoveJointGroup is ready to accept a new goal. + * \note This is independent from the state of the controller, but if the + * controller cannot be reached, isIdle() will always return false. + */ + bool isIdle() const; + + /** + * \brief Cancel the current goal + */ + void cancel(); + + /** + * \brief Register the callback to be called when the action is finished. + * \param cb The callback + */ + void setCallback(const Callback& cb); + + /** + * \brief Returns the list of associated joints + */ + const JointNames& getJointNames() const; + + /** + * \brief Returns the action client state + */ + actionlib::SimpleClientGoalState getState(); + + /** + * \brief Returns the name that was used when creating the MoveJointGroup + */ + const std::string& getName() const; private: void alCallback(); - bool busy_; - ros::NodeHandle nh_; ///< Default node handle. - std::string controller_name_; ///< Controller name. - JointNames joint_names_; ///< Names of controller joints. - ActionClient client_; ///< Action client used to trigger motions. - Callback active_cb_; ///< Call this when we are called back from the controller - ros::Timer configure_timer_; ///< To periodically check for controller actionlib server + bool busy_; + ros::NodeHandle nh_; ///< Default node handle. + std::string controller_name_; ///< Controller name. XXX: is this needed? + JointNames joint_names_; ///< Names of controller joints. + ActionClient client_; ///< Action client used to trigger motions. + Callback active_cb_; ///< Call this when we are called back from the controller + ros::Timer configure_timer_; ///< To periodically check for controller actionlib server }; } diff --git a/play_motion/src/move_joint_group.cpp b/play_motion/src/move_joint_group.cpp index f86a979..30e3e97 100644 --- a/play_motion/src/move_joint_group.cpp +++ b/play_motion/src/move_joint_group.cpp @@ -62,6 +62,37 @@ namespace play_motion active_cb_.clear(); } + bool MoveJointGroup::isIdle() const + { + return !busy_; + } + + void MoveJointGroup::cancel() + { + busy_ = false; + client_.cancelAllGoals(); + } + + void MoveJointGroup::setCallback(const Callback& cb) + { + active_cb_ = cb; + } + + const JointNames& MoveJointGroup::getJointNames() const + { + return joint_names_; + } + + actionlib::SimpleClientGoalState MoveJointGroup::getState() + { + return client_.getState(); + } + + const std::string& MoveJointGroup::getName() const + { + return controller_name_; + } + bool MoveJointGroup::isControllingJoint(const std::string& joint_name) { if (!client_.isServerConnected()) From 51e498c71b067c809c6c5b13fcc1451e8a3f7928 Mon Sep 17 00:00:00 2001 From: Paul Mathieu Date: Fri, 21 Feb 2014 17:07:16 +0100 Subject: [PATCH 2/3] Refactor some stuff in play_motion.cpp controllerCb had no business inside PlayMotion class --- play_motion/include/play_motion/play_motion.h | 7 +- play_motion/src/move_joint_group.cpp | 3 - play_motion/src/play_motion.cpp | 124 ++++++++++-------- 3 files changed, 71 insertions(+), 63 deletions(-) diff --git a/play_motion/include/play_motion/play_motion.h b/play_motion/include/play_motion/play_motion.h index 95de578..b78df94 100644 --- a/play_motion/include/play_motion/play_motion.h +++ b/play_motion/include/play_motion/play_motion.h @@ -39,7 +39,7 @@ #define REACHPOSE_H #include -#include +#include #include #include #include @@ -81,7 +81,7 @@ namespace play_motion typedef boost::shared_ptr GoalHandle; private: typedef boost::shared_ptr MoveJointGroupPtr; - typedef std::vector ControllerList; + typedef std::list ControllerList; typedef boost::function Callback; public: @@ -101,7 +101,7 @@ namespace play_motion void addController(const MoveJointGroupPtr& ctrl); private: - Goal(const Callback& cbk) : error_code(0), active_controllers(0), cb(cbk) {} + Goal(const Callback& cbk); }; PlayMotion(ros::NodeHandle& nh); @@ -115,7 +115,6 @@ namespace play_motion private: void jointStateCb(const sensor_msgs::JointStatePtr& msg); - void controllerCb(int error_code, GoalHandle goal_hdl); bool getGroupTraj(MoveJointGroupPtr move_joint_group, const JointNames& motion_joints, diff --git a/play_motion/src/move_joint_group.cpp b/play_motion/src/move_joint_group.cpp index 30e3e97..210280f 100644 --- a/play_motion/src/move_joint_group.cpp +++ b/play_motion/src/move_joint_group.cpp @@ -53,11 +53,8 @@ namespace play_motion void MoveJointGroup::alCallback() { - bool success = getState() == actionlib::SimpleClientGoalState::SUCCEEDED; busy_ = false; ActionResultPtr r = client_.getResult(); - if (!success) - ROS_WARN_STREAM("controller " << controller_name_ << " failed with err " << client_.getResult()->error_code); active_cb_(r->error_code); active_cb_.clear(); } diff --git a/play_motion/src/play_motion.cpp b/play_motion/src/play_motion.cpp index 7c02767..5c3c565 100644 --- a/play_motion/src/play_motion.cpp +++ b/play_motion/src/play_motion.cpp @@ -51,49 +51,14 @@ #define foreach BOOST_FOREACH -namespace play_motion +namespace { - PlayMotion::PlayMotion(ros::NodeHandle& nh) : - nh_(nh), - joint_states_sub_(nh_.subscribe("joint_states", 10, &PlayMotion::jointStateCb, this)), - ctrlr_updater_(nh_) - { - ctrlr_updater_.registerUpdateCb(boost::bind(&PlayMotion::updateControllersCb, this, _1, _2)); - } - - void PlayMotion::Goal::cancel() - { - foreach (MoveJointGroupPtr mjg, controllers) - mjg->cancel(); - } - - void PlayMotion::Goal::addController(const MoveJointGroupPtr& ctrl) - { - controllers.push_back(ctrl); - active_controllers++; - } - - PlayMotion::Goal::~Goal() - { - if (active_controllers) - cancel(); - } - - void PlayMotion::updateControllersCb(const ControllerUpdater::ControllerStates& states, - const ControllerUpdater::ControllerJoints& joints) - { - typedef std::pair ctrlr_state_pair_t; - move_joint_groups_.clear(); - foreach (const ctrlr_state_pair_t& p, states) - { - if (p.second != ControllerUpdater::RUNNING) - continue; - move_joint_groups_.push_back(MoveJointGroupPtr(new MoveJointGroup(p.first, joints.at(p.first)))); - ROS_DEBUG_STREAM("controller '" << p.first << "' with " << joints.at(p.first).size() << " joints"); - } - } + typedef play_motion::PlayMotion::GoalHandle GoalHandle; + typedef boost::shared_ptr MoveJointGroupPtr; + typedef std::list ControllerList; + typedef play_motion::PMR PMR; - static void generateErrorCode(PlayMotion::GoalHandle goal_hdl, int error_code) + void generateErrorCode(GoalHandle goal_hdl, int error_code) { typedef control_msgs::FollowJointTrajectoryResult JTR; switch (error_code) @@ -112,7 +77,7 @@ namespace play_motion } } - void PlayMotion::controllerCb(int error_code, GoalHandle goal_hdl) + void controllerCb(int error_code, GoalHandle goal_hdl) { if (goal_hdl->active_controllers < 1) return; @@ -134,7 +99,64 @@ namespace play_motion goal_hdl->error_code = PMR::SUCCEEDED; goal_hdl->cb(goal_hdl); } - }; + } + + template + bool hasNonNullIntersection(const std::vector& v1, const std::vector& v2) + { + foreach (const T& e1, v1) + foreach (const T& e2, v2) + if (e1 == e2) + return true; + return false; + } +} // unnamed namespace + +namespace play_motion +{ + PlayMotion::PlayMotion(ros::NodeHandle& nh) : + nh_(nh), + joint_states_sub_(nh_.subscribe("joint_states", 10, &PlayMotion::jointStateCb, this)), + ctrlr_updater_(nh_) + { + ctrlr_updater_.registerUpdateCb(boost::bind(&PlayMotion::updateControllersCb, this, _1, _2)); + } + + PlayMotion::Goal::Goal(const Callback& cbk) + : error_code(0) + , active_controllers(0) + , cb(cbk) + {} + + void PlayMotion::Goal::cancel() + { + foreach (MoveJointGroupPtr mjg, controllers) + mjg->cancel(); + } + + void PlayMotion::Goal::addController(const MoveJointGroupPtr& ctrl) + { + controllers.push_back(ctrl); + } + + PlayMotion::Goal::~Goal() + { + cancel(); + } + + void PlayMotion::updateControllersCb(const ControllerUpdater::ControllerStates& states, + const ControllerUpdater::ControllerJoints& joints) + { + typedef std::pair ctrlr_state_pair_t; + move_joint_groups_.clear(); + foreach (const ctrlr_state_pair_t& p, states) + { + if (p.second != ControllerUpdater::RUNNING) + continue; + move_joint_groups_.push_back(MoveJointGroupPtr(new MoveJointGroup(p.first, joints.at(p.first)))); + ROS_DEBUG_STREAM("controller '" << p.first << "' with " << joints.at(p.first).size() << " joints"); + } + } void PlayMotion::jointStateCb(const sensor_msgs::JointStatePtr& msg) { @@ -243,16 +265,6 @@ next_joint:; } - template - static bool hasNonNullIntersection(const std::vector& v1, const std::vector& v2) - { - foreach (const T& e1, v1) - foreach (const T& e2, v2) - if (e1 == e2) - return true; - return false; - } - bool PlayMotion::run(const std::string& motion_name, const ros::Duration& duration, GoalHandle& goal_hdl, const Callback& cb) { @@ -293,10 +305,10 @@ next_joint:; foreach (const traj_pair_t& p, joint_group_traj) { goal_hdl->addController(p.first); - p.first->setCallback(boost::bind(&PlayMotion::controllerCb, this, _1, goal_hdl)); + p.first->setCallback(boost::bind(controllerCb, _1, goal_hdl)); if (!p.first->sendGoal(p.second, duration)) - throw PMException("controller '" + p.first->getName() + "' did not accept trajectory, " - "canceling everything"); + throw PMException("controller '" + p.first->getName() + + "' did not accept trajectory, canceling everything"); } } catch (const PMException& e) From 82e443a703be1025dfd1ea5c9c21b969f48cbd84 Mon Sep 17 00:00:00 2001 From: Paul Mathieu Date: Fri, 21 Feb 2014 19:01:18 +0100 Subject: [PATCH 3/3] Propagate controller action state to internal API So that a proper message can be displayed, and appropriate measures be taken. Fixes #15 --- play_motion/include/play_motion/play_motion.h | 1 + play_motion/src/play_motion.cpp | 45 +++++++++++++------ 2 files changed, 32 insertions(+), 14 deletions(-) diff --git a/play_motion/include/play_motion/play_motion.h b/play_motion/include/play_motion/play_motion.h index b78df94..71153a7 100644 --- a/play_motion/include/play_motion/play_motion.h +++ b/play_motion/include/play_motion/play_motion.h @@ -95,6 +95,7 @@ namespace play_motion int active_controllers; Callback cb; ControllerList controllers; + bool canceled; ~Goal(); void cancel(); diff --git a/play_motion/src/play_motion.cpp b/play_motion/src/play_motion.cpp index 5c3c565..c322374 100644 --- a/play_motion/src/play_motion.cpp +++ b/play_motion/src/play_motion.cpp @@ -57,8 +57,9 @@ namespace typedef boost::shared_ptr MoveJointGroupPtr; typedef std::list ControllerList; typedef play_motion::PMR PMR; + typedef actionlib::SimpleClientGoalState SCGS; - void generateErrorCode(GoalHandle goal_hdl, int error_code) + void generateErrorCode(GoalHandle goal_hdl, int error_code, SCGS ctrl_state) { typedef control_msgs::FollowJointTrajectoryResult JTR; switch (error_code) @@ -75,30 +76,44 @@ namespace os << "got error code " << error_code << ", motion aborted"; goal_hdl->error_string = os.str(); } + //TODO: add handling for controller state } - void controllerCb(int error_code, GoalHandle goal_hdl) + void controllerCb(int error_code, GoalHandle goal_hdl, const MoveJointGroupPtr& ctrl) { - if (goal_hdl->active_controllers < 1) + ControllerList::iterator it = std::find(goal_hdl->controllers.begin(), + goal_hdl->controllers.end(), ctrl); + if (it == goal_hdl->controllers.end()) + { + ROS_ERROR_STREAM("Something is wrong in the controller callback handling. " + << ctrl->getName() << " called a goal callback while no " + "motion goal was alive for it."); return; + } + goal_hdl->controllers.erase(it); - ROS_DEBUG("return from joint group, %d active controllers, error: %d", - goal_hdl->active_controllers - 1, error_code); + ROS_DEBUG_STREAM("return from joint group " << ctrl->getName() << ", " + << goal_hdl->controllers.size() << " active controllers, " + "error: " << error_code); - if (error_code != 0) + if (goal_hdl->canceled) { - generateErrorCode(goal_hdl, error_code); - goal_hdl->cancel(); - goal_hdl->active_controllers = 1; // terminate goal immediately + ROS_DEBUG("The Goal was canceled, not calling Motion callback."); + return; } - goal_hdl->active_controllers--; - if (goal_hdl->active_controllers == 0) + goal_hdl->error_code = PMR::SUCCEEDED; + if (error_code != 0 || ctrl->getState() != actionlib::SimpleClientGoalState::SUCCEEDED) { - if (!goal_hdl->error_code) - goal_hdl->error_code = PMR::SUCCEEDED; + ROS_ERROR_STREAM("Controller " << ctrl->getName() << " aborted."); + goal_hdl->cancel(); + generateErrorCode(goal_hdl, error_code, ctrl->getState()); goal_hdl->cb(goal_hdl); + return; } + + if (goal_hdl->controllers.empty()) + goal_hdl->cb(goal_hdl); } template @@ -126,10 +141,12 @@ namespace play_motion : error_code(0) , active_controllers(0) , cb(cbk) + , canceled(false) {} void PlayMotion::Goal::cancel() { + canceled = true; foreach (MoveJointGroupPtr mjg, controllers) mjg->cancel(); } @@ -305,7 +322,7 @@ next_joint:; foreach (const traj_pair_t& p, joint_group_traj) { goal_hdl->addController(p.first); - p.first->setCallback(boost::bind(controllerCb, _1, goal_hdl)); + p.first->setCallback(boost::bind(controllerCb, _1, goal_hdl, p.first)); if (!p.first->sendGoal(p.second, duration)) throw PMException("controller '" + p.first->getName() + "' did not accept trajectory, canceling everything");