Skip to content

Commit

Permalink
Revert "Revert "Merge pull request ros-industrial#113 from simonschme…
Browse files Browse the repository at this point in the history
…isser/indigo""

This reverts commit fb28e26.
  • Loading branch information
shaun-edwards committed Jun 25, 2015
1 parent f7029f0 commit bcad8cf
Show file tree
Hide file tree
Showing 2 changed files with 30 additions and 34 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,11 @@ class JointTrajectoryAction
*/
ros::Timer watchdog_timer_;

/**
* \brief Controller was alive during the last watchdog interval
*/
bool controller_alive_;

/**
* \brief Indicates action has an active goal
*/
Expand Down Expand Up @@ -144,12 +149,6 @@ class JointTrajectoryAction
*/
control_msgs::FollowJointTrajectoryFeedbackConstPtr last_trajectory_state_;

/**
* \brief Indicates trajectory state has been received. Used by
* watchdog to determine if the robot driver is responding.
*/
bool trajectory_state_recvd_;

/**
* \brief Cache of the last subscribed status message
*/
Expand All @@ -158,7 +157,7 @@ class JointTrajectoryAction
/**
* \brief The watchdog period (seconds)
*/
static const double WATCHD0G_PERIOD_;// = 1.0;
static const double WATCHDOG_PERIOD_;// = 1.0;

/**
* \brief Watch dog callback, used to detect robot driver failures
Expand Down
51 changes: 24 additions & 27 deletions industrial_robot_client/src/joint_trajectory_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,12 +39,13 @@ namespace industrial_robot_client
namespace joint_trajectory_action
{

const double JointTrajectoryAction::WATCHD0G_PERIOD_ = 1.0;
const double JointTrajectoryAction::WATCHDOG_PERIOD_ = 1.0;
const double JointTrajectoryAction::DEFAULT_GOAL_THRESHOLD_ = 0.01;

JointTrajectoryAction::JointTrajectoryAction() :
action_server_(node_, "joint_trajectory_action", boost::bind(&JointTrajectoryAction::goalCB, this, _1),
boost::bind(&JointTrajectoryAction::cancelCB, this, _1), false), has_active_goal_(false)
boost::bind(&JointTrajectoryAction::cancelCB, this, _1), false), has_active_goal_(false),
controller_alive_(false)
{
ros::NodeHandle pn("~");

Expand All @@ -62,7 +63,7 @@ JointTrajectoryAction::JointTrajectoryAction() :
sub_trajectory_state_ = node_.subscribe("feedback_states", 1, &JointTrajectoryAction::controllerStateCB, this);
sub_robot_status_ = node_.subscribe("robot_status", 1, &JointTrajectoryAction::robotStatusCB, this);

watchdog_timer_ = node_.createTimer(ros::Duration(WATCHD0G_PERIOD_), &JointTrajectoryAction::watchdog, this);
watchdog_timer_ = node_.createTimer(ros::Duration(WATCHDOG_PERIOD_), &JointTrajectoryAction::watchdog, this, true);
action_server_.start();
}

Expand All @@ -82,42 +83,35 @@ void JointTrajectoryAction::watchdog(const ros::TimerEvent &e)
{
ROS_DEBUG("Waiting for subscription to joint trajectory state");
}
if (!trajectory_state_recvd_)
{
ROS_DEBUG("Trajectory state not received since last watchdog");
}

ROS_WARN("Trajectory state not received for %f seconds", WATCHDOG_PERIOD_);
controller_alive_ = false;


// Aborts the active goal if the controller does not appear to be active.
if (has_active_goal_)
{
if (!trajectory_state_recvd_)
// last_trajectory_state_ is null if the subscriber never makes a connection
if (!last_trajectory_state_)
{
// last_trajectory_state_ is null if the subscriber never makes a connection
if (!last_trajectory_state_)
{
ROS_WARN("Aborting goal because we have never heard a controller state message.");
}
else
{
ROS_WARN_STREAM(
"Aborting goal because we haven't heard from the controller in " << WATCHD0G_PERIOD_ << " seconds");
}

abortGoal();

ROS_WARN("Aborting goal because we have never heard a controller state message.");
}
else
{
ROS_WARN_STREAM(
"Aborting goal because we haven't heard from the controller in " << WATCHDOG_PERIOD_ << " seconds");
}
}

// Reset the trajectory state received flag
trajectory_state_recvd_ = false;
abortGoal();
}
}

void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh)
{
ROS_INFO("Received new goal");

// reject all goals as long as we haven't heard from the remote controller
if (!trajectory_state_recvd_)
if (!controller_alive_)
{
ROS_ERROR("Joint trajectory action rejected: waiting for (initial) feedback from controller");
control_msgs::FollowJointTrajectoryResult rslt;
Expand Down Expand Up @@ -216,9 +210,12 @@ void JointTrajectoryAction::cancelCB(JointTractoryActionServer::GoalHandle & gh)

void JointTrajectoryAction::controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
{
//ROS_DEBUG("Checking controller state feedback");
ROS_DEBUG("Checking controller state feedback");
last_trajectory_state_ = msg;
trajectory_state_recvd_ = true;
controller_alive_ = true;

watchdog_timer_.stop();
watchdog_timer_.start();

if (!has_active_goal_)
{
Expand Down

0 comments on commit bcad8cf

Please sign in to comment.