31 #include <pr2_msgs/PowerBoardState.h> 32 #include <std_srvs/Empty.h> 33 #include <pr2_power_board/PowerBoardCommand.h> 41 ROS_INFO(
"Run stop re-enabled. Bringing up power and resetting motors.");
45 ROS_ERROR(
"Could not find power board control service");
48 pr2_power_board::PowerBoardCommand power_board_cmd;
49 power_board_cmd.request.serial_number = msg->serial_num;
50 power_board_cmd.request.command =
"start";
51 for (
unsigned int i=0; i<3; i++){
52 power_board_cmd.request.breaker_number = i;
54 ROS_INFO(
" - Enabling breaker %i: %i",i, power_board_cmd.response.retval);
60 ROS_ERROR(
"Could not find reset motors service");
63 std_srvs::Empty empty_cmd;
72 int main(
int argc,
char** argv)
void powerStateCallback(const pr2_msgs::PowerBoardStateConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool call(const std::string &service_name, MReq &req, MRes &res)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
static bool last_e_stop_state_
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)