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