Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include "ros/ros.h"
00033
00034 #include <actionlib/server/action_server.h>
00035 #include <pr2_controllers_msgs/JointControllerState.h>
00036 #include <pr2_controllers_msgs/Pr2GripperCommand.h>
00037 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
00038 #include <std_srvs/Empty.h>
00039
00040 class Pr2GripperAction
00041 {
00042 private:
00043 typedef actionlib::ActionServer<pr2_controllers_msgs::Pr2GripperCommandAction> GAS;
00044 typedef GAS::GoalHandle GoalHandle;
00045
00046 public:
00047 Pr2GripperAction(ros::NodeHandle &n) :
00048 node_(n),
00049 action_server_(node_, "gripper_action",
00050 boost::bind(&Pr2GripperAction::goalCB, this, _1),
00051 boost::bind(&Pr2GripperAction::cancelCB, this, _1)),
00052 has_active_goal_(false)
00053 {
00054 ros::NodeHandle pn("~");
00055
00056
00057 pn.param("stall_velocity_threshold", stall_velocity_threshold_, 1e-6);
00058 pn.param("stall_timeout", stall_timeout_, 0.1);
00059
00060
00061 pub_controller_command_ =
00062 node_.advertise<pr2_controllers_msgs::Pr2GripperCommand>("command", 1);
00063 sub_controller_state_ =
00064 node_.subscribe("state", 1, &Pr2GripperAction::controllerStateCB, this);
00065
00066 watchdog_timer_ = node_.createTimer(ros::Duration(1.0), &Pr2GripperAction::watchdog, this);
00067 }
00068
00069 ~Pr2GripperAction()
00070 {
00071 pub_controller_command_.shutdown();
00072 sub_controller_state_.shutdown();
00073 watchdog_timer_.stop();
00074 }
00075
00076 private:
00077
00078 ros::NodeHandle node_;
00079 GAS action_server_;
00080 ros::Publisher pub_controller_command_;
00081 ros::Subscriber sub_controller_state_;
00082 ros::Timer watchdog_timer_;
00083
00084 bool has_active_goal_;
00085 GoalHandle active_goal_;
00086 ros::Time goal_received_;
00087
00088 double min_error_seen_;
00089 double goal_threshold_;
00090 double stall_velocity_threshold_;
00091 double stall_timeout_;
00092 ros::Time last_movement_time_;
00093
00094 void watchdog(const ros::TimerEvent &e)
00095 {
00096 ros::Time now = ros::Time::now();
00097
00098
00099 if (has_active_goal_)
00100 {
00101 bool should_abort = false;
00102 if (!last_controller_state_)
00103 {
00104 should_abort = true;
00105 ROS_WARN("Aborting goal because we have never heard a controller state message.");
00106 }
00107 else if ((now - last_controller_state_->header.stamp) > ros::Duration(5.0))
00108 {
00109 should_abort = true;
00110 ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
00111 (now - last_controller_state_->header.stamp).toSec());
00112 }
00113
00114 if (should_abort)
00115 {
00116
00117 active_goal_.setAborted();
00118 has_active_goal_ = false;
00119 }
00120 }
00121 }
00122
00123 void goalCB(GoalHandle gh)
00124 {
00125
00126 if (has_active_goal_)
00127 {
00128
00129 active_goal_.setCanceled();
00130 has_active_goal_ = false;
00131 }
00132
00133 gh.setAccepted();
00134 active_goal_ = gh;
00135 has_active_goal_ = true;
00136 goal_received_ = ros::Time::now();
00137 min_error_seen_ = 1e10;
00138
00139
00140 pub_controller_command_.publish(active_goal_.getGoal()->command);
00141 last_movement_time_ = ros::Time::now();
00142 }
00143
00144 void cancelCB(GoalHandle gh)
00145 {
00146 if (active_goal_ == gh)
00147 {
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159 std_srvs::Empty::Request req;
00160 std_srvs::Empty::Response resp;
00161 if(ros::service::exists("stop_motor_output",true))
00162 {
00163 ROS_INFO("Stopping Motor Output");
00164 ros::service::call("stop_motor_output",req,resp);
00165 }
00166
00167
00168 active_goal_.setCanceled();
00169 has_active_goal_ = false;
00170 }
00171 }
00172
00173
00174
00175 pr2_controllers_msgs::JointControllerStateConstPtr last_controller_state_;
00176 void controllerStateCB(const pr2_controllers_msgs::JointControllerStateConstPtr &msg)
00177 {
00178 last_controller_state_ = msg;
00179 ros::Time now = ros::Time::now();
00180
00181 if (!has_active_goal_)
00182 return;
00183
00184
00185 if(!node_.getParam("position_servo_position_tolerance", goal_threshold_))
00186 ROS_ERROR("No position_servo_position_tolerance given in namespace: '%s')", node_.getNamespace().c_str());
00187
00188
00189
00190 if (fabs(msg->set_point - active_goal_.getGoal()->command.position) > stall_velocity_threshold_)
00191 {
00192 if (now - goal_received_ < ros::Duration(1.0))
00193 {
00194 return;
00195 }
00196 else
00197 {
00198 ROS_ERROR("Cancelling goal: Controller is trying to achieve a different setpoint.");
00199 active_goal_.setCanceled();
00200 has_active_goal_ = false;
00201 }
00202 }
00203
00204
00205 pr2_controllers_msgs::Pr2GripperCommandFeedback feedback;
00206 feedback.position = msg->process_value;
00207 feedback.effort = msg->command;
00208 feedback.reached_goal = false;
00209 feedback.stalled = false;
00210
00211 pr2_controllers_msgs::Pr2GripperCommandResult result;
00212 result.position = msg->process_value;
00213 result.effort = msg->command;
00214 result.reached_goal = false;
00215 result.stalled = false;
00216
00217
00218 if (fabs(msg->process_value - active_goal_.getGoal()->command.position) < goal_threshold_)
00219 {
00220 feedback.reached_goal = true;
00221
00222 result.reached_goal = true;
00223 active_goal_.setSucceeded(result);
00224 has_active_goal_ = false;
00225 }
00226 else
00227 {
00228
00229 if (fabs(msg->process_value_dot) > stall_velocity_threshold_)
00230 {
00231 last_movement_time_ = ros::Time::now();
00232 }
00233 else if ((ros::Time::now() - last_movement_time_).toSec() > stall_timeout_ &&
00234 active_goal_.getGoal()->command.max_effort != 0.0)
00235 {
00236 feedback.stalled = true;
00237
00238 result.stalled = true;
00239 active_goal_.setAborted(result);
00240 has_active_goal_ = false;
00241 }
00242 }
00243 active_goal_.publishFeedback(feedback);
00244 }
00245 };
00246
00247
00248 int main(int argc, char** argv)
00249 {
00250 ros::init(argc, argv, "gripper_action_node");
00251 ros::NodeHandle node;
00252 Pr2GripperAction jte(node);
00253
00254 ros::spin();
00255
00256 return 0;
00257 }