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 #include <ros/ros.h>
00031 #include <pr2_msgs/PowerBoardState.h>
00032 #include <std_srvs/Empty.h>
00033 #include <pr2_power_board/PowerBoardCommand.h>
00034
00035
00036 static bool last_e_stop_state_ = true;
00037
00038 void powerStateCallback(const pr2_msgs::PowerBoardStateConstPtr& msg)
00039 {
00040 if (!last_e_stop_state_ && msg->run_stop){
00041 ROS_INFO("Run stop re-enabled. Bringing up power and resetting motors.");
00042
00043
00044 if (!ros::service::waitForService("power_board/control", ros::Duration(5.0))){
00045 ROS_ERROR("Could not find power board control service");
00046 return;
00047 }
00048 pr2_power_board::PowerBoardCommand power_board_cmd;
00049 power_board_cmd.request.serial_number = msg->serial_num;
00050 power_board_cmd.request.command = "start";
00051 for (unsigned int i=0; i<3; i++){
00052 power_board_cmd.request.breaker_number = i;
00053 ros::service::call("power_board/control", power_board_cmd);
00054 ROS_INFO(" - Enabling breaker %i: %i",i, power_board_cmd.response.retval);
00055 }
00056
00057
00058 ros::Duration(2.0).sleep();
00059 if (!ros::service::waitForService("pr2_etherCAT/reset_motors", ros::Duration(5.0))){
00060 ROS_ERROR("Could not find reset motors service");
00061 return;
00062 }
00063 std_srvs::Empty empty_cmd;
00064 ros::service::call("pr2_etherCAT/reset_motors", empty_cmd);
00065 ROS_INFO(" - Reset motors");
00066 }
00067
00068 last_e_stop_state_ = msg->run_stop;
00069 }
00070
00071
00072 int main(int argc, char** argv)
00073 {
00074 ros::init(argc, argv, "run_stop_helper");
00075 ros::NodeHandle node;
00076
00077
00078 ros::Subscriber power_state_sub = node.subscribe("power_board/state", 1, powerStateCallback);
00079
00080 ros::spin();
00081
00082 return 0;
00083 }