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
00033
00034
00035
00036
00037 #include "ros/ros.h"
00038
00039 #include <actionlib/server/action_server.h>
00040 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoAction.h>
00041 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoData.h>
00042 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoCommand.h>
00043 #include <std_srvs/Empty.h>
00044
00045 class Pr2GripperForceServo
00046 {
00047 private:
00048 typedef actionlib::ActionServer<pr2_gripper_sensor_msgs::PR2GripperForceServoAction> GAS;
00049 typedef GAS::GoalHandle GoalHandle;
00050 public:
00051 Pr2GripperForceServo(ros::NodeHandle &n) :
00052 node_(n),
00053 action_server_(node_, "force_servo",
00054 boost::bind(&Pr2GripperForceServo::goalCB, this, _1),
00055 boost::bind(&Pr2GripperForceServo::cancelCB, this, _1)),
00056 has_active_goal_(false)
00057 {
00058 ros::NodeHandle pn("~");
00059
00060
00061 pub_controller_command_ =
00062 node_.advertise<pr2_gripper_sensor_msgs::PR2GripperForceServoCommand>("force_servo", 1);
00063 sub_controller_state_ =
00064 node_.subscribe("force_servo_state", 1, &Pr2GripperForceServo::controllerStateCB, this);
00065
00066 watchdog_timer_ = node_.createTimer(ros::Duration(1.0), &Pr2GripperForceServo::watchdog, this);
00067
00068 }
00069
00070 ~Pr2GripperForceServo()
00071 {
00072 pub_controller_command_.shutdown();
00073 sub_controller_state_.shutdown();
00074 watchdog_timer_.stop();
00075 }
00076
00077 private:
00078
00079 ros::NodeHandle node_;
00080 GAS action_server_;
00081 ros::Publisher pub_controller_command_;
00082 ros::Subscriber sub_controller_state_;
00083 ros::Timer watchdog_timer_;
00084
00085 bool has_active_goal_;
00086 bool firstDrop;
00087 GoalHandle active_goal_;
00088 ros::Time goal_received_;
00089
00090 double goal_threshold_;
00091 double stall_velocity_threshold_;
00092 double stall_timeout_;
00093 ros::Time last_movement_time_;
00094 ros::Time action_start_time;
00095
00096
00097
00098 void watchdog(const ros::TimerEvent &e)
00099 {
00100 ros::Time now = ros::Time::now();
00101
00102
00103 if (has_active_goal_)
00104 {
00105 bool should_abort = false;
00106 if (!last_controller_state_)
00107 {
00108 should_abort = true;
00109 ROS_WARN("Aborting goal because we have never heard a controller state message.");
00110 }
00111 else if ((now - last_controller_state_->stamp) > ros::Duration(5.0))
00112 {
00113 should_abort = true;
00114 ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
00115 (now - last_controller_state_->stamp).toSec());
00116 }
00117
00118 if (should_abort)
00119 {
00120
00121 active_goal_.setAborted();
00122 has_active_goal_ = false;
00123 }
00124 }
00125 }
00126
00127 void goalCB(GoalHandle gh)
00128 {
00129
00130 if (has_active_goal_)
00131 {
00132
00133 active_goal_.setCanceled();
00134 has_active_goal_ = false;
00135 }
00136
00137 gh.setAccepted();
00138 active_goal_ = gh;
00139 has_active_goal_ = true;
00140 firstDrop = true;
00141 goal_received_ = ros::Time::now();
00142
00143
00144 pub_controller_command_.publish(active_goal_.getGoal()->command);
00145
00146 last_movement_time_ = ros::Time::now();
00147 action_start_time = ros::Time::now();
00148 }
00149
00150 void cancelCB(GoalHandle gh)
00151 {
00152
00153 if (active_goal_ == gh)
00154 {
00155
00156 std_srvs::Empty::Request req;
00157 std_srvs::Empty::Response resp;
00158 if(ros::service::exists("stop_motor_output",true))
00159 {
00160 ROS_INFO("Stopping Motor Output");
00161 ros::service::call("stop_motor_output",req,resp);
00162 }
00163
00164
00165 active_goal_.setCanceled();
00166 has_active_goal_ = false;
00167 }
00168 }
00169
00170
00171 pr2_gripper_sensor_msgs::PR2GripperForceServoDataConstPtr last_controller_state_;
00172 void controllerStateCB(const pr2_gripper_sensor_msgs::PR2GripperForceServoDataConstPtr &msg)
00173 {
00174 last_controller_state_ = msg;
00175 ros::Time now = ros::Time::now();
00176
00177 if (!has_active_goal_)
00178 return;
00179
00180 pr2_gripper_sensor_msgs::PR2GripperForceServoGoal goal;
00181 goal.command = active_goal_.getGoal()->command;
00182
00183 pr2_gripper_sensor_msgs::PR2GripperForceServoFeedback feedback;
00184 feedback.data = *msg;
00185
00186 pr2_gripper_sensor_msgs::PR2GripperForceServoResult result;
00187 result.data = *msg;
00188
00189
00190
00191 double dT = 0.1;
00192 if(feedback.data.stamp.toSec() < action_start_time.toSec()+dT ){}
00193
00194
00195 else if(feedback.data.rtstate.realtime_controller_state == 4)
00196 {
00197
00198 if ( feedback.data.force_achieved )
00199 {
00200 active_goal_.setSucceeded(result);
00201 has_active_goal_ = false;
00202 }
00203
00204 }
00205
00206 else
00207 has_active_goal_ = false;
00208
00209 active_goal_.publishFeedback(feedback);
00210 }
00211 };
00212
00213
00214 int main(int argc, char** argv)
00215 {
00216 ros::init(argc, argv, "force_servo_node");
00217 ros::NodeHandle node;
00218 Pr2GripperForceServo jte(node);
00219
00220 ros::spin();
00221
00222 return 0;
00223 }