Skip to content

Commit

Permalink
Merge pull request #19 from pal-robotics/propagate-status
Browse files Browse the repository at this point in the history 
Propagate controller action final state
  • Loading branch information
@po1
po1 committed Feb 24, 2014
2 parents 22281a8 + 82e443a commit 576c597
Show file tree
Hide file tree
Showing 4 changed files with 174 additions and 80 deletions.
65 changes: 51 additions & 14 deletions play_motion/include/play_motion/move_joint_group.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,34 +56,71 @@ namespace play_motion
{
private:
typedef actionlib::SimpleActionClient
<control_msgs::FollowJointTrajectoryAction> ActionClient;
<control_msgs::FollowJointTrajectoryAction> ActionClient;
typedef control_msgs::FollowJointTrajectoryGoal ActionGoal;
typedef control_msgs::FollowJointTrajectoryResult ActionResult;
typedef boost::shared_ptr<const ActionResult> ActionResultPtr;
typedef boost::function<void(int)> Callback;

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<TrajPoint>& 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
};
}

Expand Down
8 changes: 4 additions & 4 deletions play_motion/include/play_motion/play_motion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@
#define REACHPOSE_H

#include <string>
#include <vector>
#include <list>
#include <map>
#include <ros/ros.h>
#include <boost/foreach.hpp>
Expand Down Expand Up @@ -81,7 +81,7 @@ namespace play_motion
typedef boost::shared_ptr<Goal> GoalHandle;
private:
typedef boost::shared_ptr<MoveJointGroup> MoveJointGroupPtr;
typedef std::vector<MoveJointGroupPtr> ControllerList;
typedef std::list<MoveJointGroupPtr> ControllerList;
typedef boost::function<void(const GoalHandle&)> Callback;

public:
Expand All @@ -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);
Expand All @@ -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,
Expand Down
34 changes: 31 additions & 3 deletions play_motion/src/move_joint_group.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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())
Expand Down
147 changes: 88 additions & 59 deletions play_motion/src/play_motion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,82 @@

#define foreach BOOST_FOREACH

namespace
{
typedef play_motion::PlayMotion::GoalHandle GoalHandle;
typedef boost::shared_ptr<play_motion::MoveJointGroup> MoveJointGroupPtr;
typedef std::list<MoveJointGroupPtr> 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 <class T>
bool hasNonNullIntersection(const std::vector<T>& v1, const std::vector<T>& 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) :
Expand All @@ -61,22 +137,28 @@ 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();
}

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,
Expand All @@ -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();
Expand Down Expand Up @@ -243,16 +282,6 @@ next_joint:;
}


template <class T>
static bool hasNonNullIntersection(const std::vector<T>& v1, const std::vector<T>& 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)
{
Expand Down Expand Up @@ -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)
Expand Down

0 comments on commit 576c597

Please sign in to comment.