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/include/play_motion/play_motion.h b/play_motion/include/play_motion/play_motion.h index 95de578..71153a7 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: @@ -95,13 +95,14 @@ namespace play_motion int active_controllers; Callback cb; ControllerList controllers; + bool canceled; ~Goal(); void cancel(); 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 +116,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 f86a979..210280f 100644 --- a/play_motion/src/move_joint_group.cpp +++ b/play_motion/src/move_joint_group.cpp @@ -53,15 +53,43 @@ 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(); } + 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()) diff --git a/play_motion/src/play_motion.cpp b/play_motion/src/play_motion.cpp index 7c02767..c322374 100644 --- a/play_motion/src/play_motion.cpp +++ b/play_motion/src/play_motion.cpp @@ -51,6 +51,82 @@ #define foreach BOOST_FOREACH +namespace +{ + typedef play_motion::PlayMotion::GoalHandle GoalHandle; + 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, SCGS ctrl_state) + { + typedef control_msgs::FollowJointTrajectoryResult JTR; + switch (error_code) + { + case JTR::PATH_TOLERANCE_VIOLATED: + goal_hdl->error_code = PMR::TRAJECTORY_ERROR; + break; + case JTR::GOAL_TOLERANCE_VIOLATED: + goal_hdl->error_code = PMR::GOAL_NOT_REACHED; + break; + default: + std::ostringstream os; + goal_hdl->error_code = PMR::OTHER_ERROR; + 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, const MoveJointGroupPtr& ctrl) + { + 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_STREAM("return from joint group " << ctrl->getName() << ", " + << goal_hdl->controllers.size() << " active controllers, " + "error: " << error_code); + + if (goal_hdl->canceled) + { + ROS_DEBUG("The Goal was canceled, not calling Motion callback."); + return; + } + + goal_hdl->error_code = PMR::SUCCEEDED; + if (error_code != 0 || ctrl->getState() != actionlib::SimpleClientGoalState::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 + 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) : @@ -61,8 +137,16 @@ namespace play_motion ctrlr_updater_.registerUpdateCb(boost::bind(&PlayMotion::updateControllersCb, this, _1, _2)); } + PlayMotion::Goal::Goal(const Callback& cbk) + : error_code(0) + , active_controllers(0) + , cb(cbk) + , canceled(false) + {} + void PlayMotion::Goal::cancel() { + canceled = true; foreach (MoveJointGroupPtr mjg, controllers) mjg->cancel(); } @@ -70,13 +154,11 @@ namespace play_motion void PlayMotion::Goal::addController(const MoveJointGroupPtr& ctrl) { controllers.push_back(ctrl); - active_controllers++; } PlayMotion::Goal::~Goal() { - if (active_controllers) - cancel(); + cancel(); } void PlayMotion::updateControllersCb(const ControllerUpdater::ControllerStates& states, @@ -93,49 +175,6 @@ namespace play_motion } } - static void generateErrorCode(PlayMotion::GoalHandle goal_hdl, int error_code) - { - typedef control_msgs::FollowJointTrajectoryResult JTR; - switch (error_code) - { - case JTR::PATH_TOLERANCE_VIOLATED: - goal_hdl->error_code = PMR::TRAJECTORY_ERROR; - break; - case JTR::GOAL_TOLERANCE_VIOLATED: - goal_hdl->error_code = PMR::GOAL_NOT_REACHED; - break; - default: - std::ostringstream os; - goal_hdl->error_code = PMR::OTHER_ERROR; - os << "got error code " << error_code << ", motion aborted"; - goal_hdl->error_string = os.str(); - } - } - - void PlayMotion::controllerCb(int error_code, GoalHandle goal_hdl) - { - if (goal_hdl->active_controllers < 1) - return; - - ROS_DEBUG("return from joint group, %d active controllers, error: %d", - goal_hdl->active_controllers - 1, error_code); - - if (error_code != 0) - { - generateErrorCode(goal_hdl, error_code); - goal_hdl->cancel(); - goal_hdl->active_controllers = 1; // terminate goal immediately - } - - goal_hdl->active_controllers--; - if (goal_hdl->active_controllers == 0) - { - if (!goal_hdl->error_code) - goal_hdl->error_code = PMR::SUCCEEDED; - goal_hdl->cb(goal_hdl); - } - }; - void PlayMotion::jointStateCb(const sensor_msgs::JointStatePtr& msg) { joint_states_.clear(); @@ -243,16 +282,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 +322,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, p.first)); 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)