void controllerStateCB(const pr2_gripper_sensor_msgs::PR2GripperFindContactDataConstPtr &msg)
{
last_controller_state_ = msg;
ros::Time now = ros::Time::now();
if (!has_active_goal_)
return;
pr2_gripper_sensor_msgs::PR2GripperFindContactGoal goal;
goal.command = active_goal_.getGoal()->command;
pr2_gripper_sensor_msgs::PR2GripperFindContactFeedback feedback;
feedback.data = *msg;
pr2_gripper_sensor_msgs::PR2GripperFindContactResult result;
result.data = *msg;
// do not check until some dT has expired from message start
double dT = 0.2;
if(feedback.data.stamp.toSec() < action_start_time.toSec()+dT ){}
// if we are actually in a find_contact control state or positoin control state
else if(feedback.data.rtstate.realtime_controller_state == 5 || feedback.data.rtstate.realtime_controller_state == 3)
{
if(feedback.data.contact_conditions_met)
{
active_goal_.setSucceeded(result);
has_active_goal_ = false;
}
}
else
has_active_goal_ = false;
active_goal_.publishFeedback(feedback);
}
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);
}
