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 #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 }