void watchdog(const ros::TimerEvent &e)
{
ros::Time now = ros::Time::now();
// Aborts the active goal if the controller does not appear to be active.
if (has_active_goal_)
{
bool should_abort = false;
if (!last_controller_state_)
{
should_abort = true;
ROS_WARN("Aborting goal because we have never heard a controller state message.");
}
else if ((now - last_controller_state_->stamp) > ros::Duration(5.0))
{
should_abort = true;
ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
(now - last_controller_state_->stamp).toSec());
}
if (should_abort)
{
// Marks the current goal as aborted.
active_goal_.setAborted();
has_active_goal_ = false;
}
}
}
void ServerGoalHandleDestructionTester::goalCallback(GoalHandle gh)
{
ROS_ERROR_NAMED("actionlib", "In callback");
//assign to our stored goal handle
*gh_ = gh;
TestGoal goal = *gh.getGoal();
switch (goal.goal)
{
case 1:
gh.setAccepted();
gh.setSucceeded(TestResult(), "The ref server has succeeded");
break;
case 2:
gh.setAccepted();
gh.setAborted(TestResult(), "The ref server has aborted");
break;
case 3:
gh.setRejected(TestResult(), "The ref server has rejected");
break;
default:
break;
}
ros::shutdown();
}
void controllerStateCB(const pr2_controllers_msgs::JointControllerStateConstPtr &msg)
{
last_controller_state_ = msg;
ros::Time now = ros::Time::now();
if (!has_active_goal_)
return;
// grab the goal threshold off the param server
if(!node_.getParam("position_servo_position_tolerance", goal_threshold_))
ROS_ERROR("No position_servo_position_tolerance given in namespace: '%s')", node_.getNamespace().c_str());
// Ensures that the controller is tracking my setpoint.
if (fabs(msg->set_point - active_goal_.getGoal()->command.position) > stall_velocity_threshold_)
{
if (now - goal_received_ < ros::Duration(1.0))
{
return;
}
else
{
ROS_ERROR("Cancelling goal: Controller is trying to achieve a different setpoint.");
active_goal_.setCanceled();
has_active_goal_ = false;
}
}
pr2_controllers_msgs::Pr2GripperCommandFeedback feedback;
feedback.position = msg->process_value;
feedback.effort = msg->command;
feedback.reached_goal = false;
feedback.stalled = false;
pr2_controllers_msgs::Pr2GripperCommandResult result;
result.position = msg->process_value;
result.effort = msg->command;
result.reached_goal = false;
result.stalled = false;
// check if we've reached the goal
if (fabs(msg->process_value - active_goal_.getGoal()->command.position) < goal_threshold_)
{
feedback.reached_goal = true;
result.reached_goal = true;
active_goal_.setSucceeded(result);
has_active_goal_ = false;
}
else
{
// Determines if the gripper has stalled.
if (fabs(msg->process_value_dot) > stall_velocity_threshold_)
{
last_movement_time_ = ros::Time::now();
}
else if ((ros::Time::now() - last_movement_time_).toSec() > stall_timeout_ &&
active_goal_.getGoal()->command.max_effort != 0.0)
{
feedback.stalled = true;
result.stalled = true;
active_goal_.setAborted(result);
has_active_goal_ = false;
}
}
active_goal_.publishFeedback(feedback);
}
