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); }