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 <stdio.h>
00031 #include <stdint.h>
00032 #include <string.h>
00033 #include <unistd.h>
00034 #include <stdlib.h>
00035 #include <sys/select.h>
00036 #include <sys/types.h>
00037 #include <assert.h>
00038 #include <errno.h>
00039 #include <signal.h>
00040 #include <vector>
00041 #include <sstream>
00042 #include <iostream>
00043 #include <boost/thread/thread.hpp>
00044 #include <boost/program_options.hpp>
00045
00046
00047 #include <sys/types.h>
00048 #include <sys/socket.h>
00049 #include <netinet/in.h>
00050 #include <arpa/inet.h>
00051 #include <sys/socket.h>
00052 #include <net/if.h>
00053 #include <sys/ioctl.h>
00054
00055 #include "power_comm.h"
00056 #include "power_node.h"
00057 #include "diagnostic_msgs/DiagnosticArray.h"
00058 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
00059 #include "pr2_msgs/PowerBoardState.h"
00060 #include "rosconsole/macros_generated.h"
00061 #include "ros/ros.h"
00062
00063 using namespace std;
00064 namespace po = boost::program_options;
00065
00066
00067
00068 static std::vector<Device*> Devices;
00069 static std::vector<Interface*> SendInterfaces;
00070 static PowerBoard *myBoard;
00071 static Interface* ReceiveInterface;
00072
00073 static const ros::Duration TIMEOUT = ros::Duration(1,0);
00074
00075
00076 void Device::setTransitionMessage(const TransitionMessage &newtmsg)
00077 {
00078 if (tmsgset)
00079 {
00080 #define PRINT_IF_CHANGED(val) \
00081 for (int counter = 0; counter < 3; counter++) \
00082 { \
00083 if (tmsg.cb[counter].val##_count != newtmsg.cb[counter].val##_count) \
00084 { \
00085 ROS_INFO("Power board: CB%i "#val" event count changed to %i.", counter, newtmsg.cb[counter].val##_count);\
00086 } \
00087 }
00088
00089 PRINT_IF_CHANGED(stop);
00090 PRINT_IF_CHANGED(estop);
00091 PRINT_IF_CHANGED(trip);
00092 PRINT_IF_CHANGED(fail_18V);
00093 PRINT_IF_CHANGED(disable);
00094 PRINT_IF_CHANGED(start);
00095 PRINT_IF_CHANGED(pump_fail);
00096 PRINT_IF_CHANGED(reset);
00097
00098 #undef PRINT_IF_CHANGED
00099 }
00100 else
00101 tmsgset = 1;
00102
00103 tmsg = newtmsg;
00104 }
00105
00106 void Device::setPowerMessage(const PowerMessage &newpmsg)
00107 {
00108 if (pmsgset)
00109 {
00110 #define PRINT_IF_CHANGED(val) \
00111 if (pmsg.status.val##_status != newpmsg.status.val##_status) \
00112 { \
00113 ROS_INFO("Power board: status of "#val" changed to %i.", newpmsg.status.val##_status); \
00114 }
00115
00116 PRINT_IF_CHANGED(CB0);
00117 PRINT_IF_CHANGED(CB1);
00118 PRINT_IF_CHANGED(CB2);
00119 PRINT_IF_CHANGED(estop_button);
00120 PRINT_IF_CHANGED(estop);
00121
00122 #undef PRINT_IF_CHANGED
00123 }
00124 else
00125 pmsgset = 1;
00126
00127 if(newpmsg.header.message_revision == 2)
00128 {
00129 memcpy( &pmsg.header, &newpmsg.header, sizeof(MessageHeader));
00130
00131
00132 memcpy( &pmsg.status, &newpmsg.status, sizeof(StatusStruct_Rev2));
00133 }
00134 else
00135 pmsg = newpmsg;
00136
00137 }
00138
00139 Interface::Interface(const char* ifname) :
00140 recv_sock(-1),
00141 send_sock(-1) {
00142 memset(&interface, 0, sizeof(interface));
00143 assert(strlen(ifname) <= sizeof(interface.ifr_name));
00144 strncpy(interface.ifr_name, ifname, IFNAMSIZ);
00145 }
00146
00147
00148 void Interface::Close() {
00149 if (recv_sock != -1) {
00150 close(recv_sock);
00151 recv_sock = -1;
00152 }
00153 if (send_sock != -1) {
00154 close(send_sock);
00155 send_sock = -1;
00156 }
00157 }
00158
00159
00160 int Interface::InitReceive()
00161 {
00162
00163 recv_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
00164 if (recv_sock == -1) {
00165 perror("Couldn't create recv socket");
00166 return -1;
00167 }
00168
00169
00170 int opt = 1;
00171 if (setsockopt(recv_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) {
00172 perror("Couldn't set reuse addr on recv socket\n");
00173 Close();
00174 return -1;
00175 }
00176
00177
00178 opt = 1;
00179 if (setsockopt(recv_sock, SOL_SOCKET, SO_BROADCAST, &opt, sizeof(opt))) {
00180 perror("Setting broadcast option on recv");
00181 Close();
00182 return -1;
00183 }
00184
00185
00186 struct sockaddr_in sin;
00187 memset(&sin, 0, sizeof(sin));
00188 sin.sin_family = AF_INET;
00189 sin.sin_port = htons(POWER_PORT);
00190 sin.sin_addr.s_addr = (INADDR_ANY);
00191 if (bind(recv_sock, (struct sockaddr*)&sin, sizeof(sin))) {
00192 perror("Couldn't Bind socket to port");
00193 Close();
00194 return -1;
00195 }
00196
00197 return 0;
00198 }
00199
00200 int Interface::Init(sockaddr_in *port_address, sockaddr_in *broadcast_address)
00201 {
00202
00203 memcpy( &ifc_address, broadcast_address, sizeof(sockaddr_in));
00204 #if 0
00205 recv_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
00206 if (recv_sock == -1) {
00207 perror("Couldn't create recv socket");
00208 return -1;
00209 }
00210 #endif
00211 send_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
00212 if (send_sock == -1) {
00213 Close();
00214 perror("Couldn't create send socket");
00215 return -1;
00216 }
00217
00218
00219
00220 int opt;
00221 #if 0
00222 opt = 1;
00223 if (setsockopt(recv_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) {
00224 perror("Couldn't set reuse addr on recv socket\n");
00225 Close();
00226 return -1;
00227 }
00228
00229
00230 opt = 1;
00231 if (setsockopt(recv_sock, SOL_SOCKET, SO_BROADCAST, &opt, sizeof(opt))) {
00232 perror("Setting broadcast option on recv");
00233 Close();
00234 return -1;
00235 }
00236 #endif
00237
00238 opt = 1;
00239 if (setsockopt(send_sock, SOL_SOCKET, SO_BROADCAST, &opt, sizeof(opt))) {
00240 perror("Setting broadcast option on send");
00241 Close();
00242 return -1;
00243 }
00244
00245
00246 struct sockaddr_in sin;
00247 memset(&sin, 0, sizeof(sin));
00248 sin.sin_family = AF_INET;
00249 #if 0
00250 sin.sin_port = htons(POWER_PORT);
00251 sin.sin_addr.s_addr = (INADDR_ANY);
00252
00253 if (bind(recv_sock, (struct sockaddr*)&sin, sizeof(sin))) {
00254 perror("Couldn't Bind socket to port");
00255 Close();
00256 return -1;
00257 }
00258 #endif
00259
00260
00261 sin.sin_port = htons(POWER_PORT);
00262
00263 sin.sin_addr= broadcast_address->sin_addr;
00264 if (connect(send_sock, (struct sockaddr*)&sin, sizeof(sin))) {
00265 perror("Connect'ing socket failed");
00266 Close();
00267 return -1;
00268 }
00269
00270 return 0;
00271 }
00272
00273 void Interface::AddToReadSet(fd_set &set, int &max_sock) const {
00274 FD_SET(recv_sock,&set);
00275 if (recv_sock > max_sock)
00276 max_sock = recv_sock;
00277 }
00278
00279 bool Interface::IsReadSet(fd_set set) const {
00280 return FD_ISSET(recv_sock,&set);
00281 }
00282
00283 int PowerBoard::send_command(unsigned int serial_number, int circuit_breaker, const std::string &command, unsigned flags)
00284 {
00285 if (Devices.size() == 0) {
00286 fprintf(stderr,"No devices to send command to\n");
00287 return -1;
00288 }
00289
00290 int selected_device = -1;
00291
00292 if(serial_number == 0) {
00293 if(Devices.size() > 1) {
00294 fprintf(stderr,"Too many devices to send command to using serial_number=0\n");
00295 return -1;
00296 }
00297 selected_device = 0;
00298 } else {
00299
00300 for (unsigned i = 0; i<Devices.size(); ++i) {
00301 if (Devices[i]->getPowerMessage().header.serial_num == serial_number) {
00302 selected_device = i;
00303 break;
00304 }
00305 }
00306 }
00307
00308 if ((selected_device < 0) || (selected_device >= (int)Devices.size())) {
00309 fprintf(stderr, "Device number must be between 0 and %u\n", (int)Devices.size()-1);
00310 return -1;
00311 }
00312
00313 Device* device = Devices[selected_device];
00314 assert(device != NULL);
00315
00316 if ((circuit_breaker < 0) || (circuit_breaker > 2)) {
00317 fprintf(stderr, "Circuit breaker number must be between 0 and 2\n");
00318 return -1;
00319 }
00320
00321 ROS_DEBUG("circuit=%d command=%s flags=%x\n", circuit_breaker, command.c_str(), flags);
00322
00323
00324 char command_enum = NONE;
00325 if (command == "start") {
00326 command_enum = COMMAND_START;
00327 }
00328 else if (command == "stop") {
00329 command_enum = COMMAND_STOP;
00330 }
00331 else if (command == "reset") {
00332 command_enum = COMMAND_RESET;
00333 }
00334 else if (command == "disable") {
00335 command_enum = COMMAND_DISABLE;
00336 }
00337 else if (command == "none") {
00338 command_enum = NONE;
00339 }
00340 else {
00341 ROS_ERROR("invalid command '%s'", command.c_str());
00342 return -1;
00343 }
00344
00345
00346
00347
00348 CommandMessage cmdmsg;
00349 memset(&cmdmsg, 0, sizeof(cmdmsg));
00350 cmdmsg.header.message_revision = COMMAND_MESSAGE_REVISION;
00351 cmdmsg.header.message_id = MESSAGE_ID_COMMAND;
00352 cmdmsg.header.serial_num = device->getPowerMessage().header.serial_num;
00353
00354 strncpy(cmdmsg.header.text, "power command message", sizeof(cmdmsg.header.text));
00355 cmdmsg.command.CB0_command = NONE;
00356 cmdmsg.command.CB1_command = NONE;
00357 cmdmsg.command.CB2_command = NONE;
00358 if (circuit_breaker==0) {
00359 cmdmsg.command.CB0_command = command_enum;
00360 }
00361 else if (circuit_breaker==1) {
00362 cmdmsg.command.CB1_command = command_enum;
00363 }
00364 else if (circuit_breaker==2) {
00365 cmdmsg.command.CB2_command = command_enum;
00366 }
00367 else if (circuit_breaker==-1) {
00368 cmdmsg.command.CB0_command = command_enum;
00369 cmdmsg.command.CB1_command = command_enum;
00370 cmdmsg.command.CB2_command = command_enum;
00371 }
00372
00373 cmdmsg.command.flags = flags;
00374
00375 errno = 0;
00376 for (unsigned xx = 0; xx < SendInterfaces.size(); ++xx)
00377 {
00378
00379 int result = send(SendInterfaces[xx]->send_sock, &cmdmsg, sizeof(cmdmsg), 0);
00380 if (result == -1) {
00381 ROS_ERROR("Error sending");
00382 return -1;
00383 } else if (result != sizeof(cmdmsg)) {
00384 ROS_WARN("Error sending : send only took %d of %d bytes\n",
00385 result, (int)sizeof(cmdmsg));
00386 }
00387 }
00388
00389 ROS_DEBUG("Send to Serial=%u, revision=%u", cmdmsg.header.serial_num, cmdmsg.header.message_revision);
00390 ROS_DEBUG("Sent command %s(%d) to device %d, circuit %d",
00391 command.c_str(), command_enum, selected_device, circuit_breaker);
00392
00393 return 0;
00394 }
00395
00396
00397 const char* PowerBoard::cb_state_to_str(char state)
00398 {
00399
00400 switch(state) {
00401 case STATE_NOPOWER:
00402 return "no-power";
00403 case STATE_STANDBY:
00404 return "Standby";
00405 case STATE_PUMPING:
00406 return "pumping";
00407 case STATE_ON:
00408 return "On";
00409 case STATE_DISABLED:
00410 return "Disabled";
00411 }
00412 return "???";
00413 }
00414
00415 const char* PowerBoard::master_state_to_str(char state)
00416 {
00417
00418 switch(state) {
00419 case MASTER_NOPOWER:
00420 return "no-power";
00421 case MASTER_STANDBY:
00422 return "stand-by";
00423 case MASTER_ON:
00424 return "on";
00425 case MASTER_OFF:
00426 return "off";
00427 case MASTER_SHUTDOWN:
00428 return "shutdown";
00429 }
00430 return "???";
00431 }
00432
00433
00434
00435
00436
00437 int PowerBoard::process_message(const PowerMessage *msg, int len)
00438 {
00439 if ((msg->header.message_revision > CURRENT_MESSAGE_REVISION) || (msg->header.message_revision < MINIMUM_MESSAGE_REVISION)) {
00440 ROS_WARN("Got message with incorrect message revision %u\n", msg->header.message_revision);
00441 return -1;
00442 }
00443
00444 if ((msg->header.message_revision == CURRENT_MESSAGE_REVISION) && (len != CURRENT_MESSAGE_SIZE))
00445 ROS_ERROR("received message of incorrect size %d for rev=%d\n", len, msg->header.message_revision);
00446
00447 if ((msg->header.message_revision == MINIMUM_MESSAGE_REVISION) && (len != REVISION_2_MESSAGE_SIZE))
00448 ROS_ERROR("received message of incorrect size %d for rev=%d\n", len, msg->header.message_revision);
00449
00450
00451
00452 if(serial_number != 0)
00453 {
00454 if(serial_number == msg->header.serial_num)
00455 {
00456 boost::mutex::scoped_lock(library_lock_);
00457 Devices[0]->message_time = ros::Time::now();
00458 Devices[0]->setPowerMessage(*msg);
00459 }
00460 }
00461 else
00462 {
00463 for (unsigned i = 0; i<Devices.size(); ++i) {
00464 if (Devices[i]->getPowerMessage().header.serial_num == msg->header.serial_num) {
00465 boost::mutex::scoped_lock(library_lock_);
00466 Devices[i]->message_time = ros::Time::now();
00467 Devices[i]->setPowerMessage(*msg);
00468 return 0;
00469 }
00470 }
00471
00472
00473 Device *newDevice = new Device();
00474 Devices.push_back(newDevice);
00475 newDevice->message_time = ros::Time::now();
00476 newDevice->setPowerMessage(*msg);
00477 }
00478
00479 return 0;
00480 }
00481
00482 int PowerBoard::process_transition_message(const TransitionMessage *msg, int len)
00483 {
00484 if (msg->header.message_revision != TRANSITION_MESSAGE_REVISION) {
00485 ROS_WARN("Got message with incorrect message revision %u\n", msg->header.message_revision);
00486 return -1;
00487 }
00488
00489 if (len != sizeof(TransitionMessage)) {
00490 ROS_ERROR("received message of incorrect size %d\n", len);
00491 return -2;
00492 }
00493
00494
00495 if(serial_number != 0)
00496 {
00497 if(serial_number == msg->header.serial_num)
00498 {
00499 boost::mutex::scoped_lock(library_lock_);
00500 Devices[0]->message_time = ros::Time::now();
00501 Devices[0]->setTransitionMessage(*msg);
00502 }
00503 }
00504 else
00505 {
00506 for (unsigned i = 0; i<Devices.size(); ++i) {
00507 if (Devices[i]->getPowerMessage().header.serial_num == msg->header.serial_num) {
00508 boost::mutex::scoped_lock(library_lock_);
00509 Devices[i]->setTransitionMessage(*msg);
00510 return 0;
00511 }
00512 }
00513
00514
00515 Device *newDevice = new Device();
00516 Devices.push_back(newDevice);
00517 newDevice->setTransitionMessage(*msg);
00518 }
00519
00520 return 0;
00521 }
00522
00523
00524
00525 int PowerBoard::collect_messages()
00526 {
00527 PowerMessage *power_msg;
00528 TransitionMessage *transition_msg;
00529 MessageHeader *header;
00530 char tmp_buf[256];
00531
00532
00533
00534
00535 timeval timeout;
00536 timeout.tv_sec = 1;
00537 timeout.tv_usec = 0;
00538
00539 while (1)
00540 {
00541
00542 fd_set read_set;
00543 int max_sock = -1;
00544 FD_ZERO(&read_set);
00545
00546 ReceiveInterface->AddToReadSet(read_set,max_sock);
00547
00548 int result = select(max_sock+1, &read_set, NULL, NULL, &timeout);
00549
00550
00551
00552 if (result == -1) {
00553 perror("Select");
00554 return -1;
00555 }
00556 else if (result == 0) {
00557 return 0;
00558 }
00559 else if (result >= 1) {
00560 Interface *recvInterface = ReceiveInterface;
00561 #if 0
00562 for (unsigned i = 0; i<SendInterfaces.size(); ++i) {
00563
00564 if (SendInterfaces[i]->IsReadSet(read_set)) {
00565 recvInterface = SendInterfaces[i];
00566
00567 break;
00568 }
00569 }
00570 #endif
00571
00572
00573 int len = recv(recvInterface->recv_sock, tmp_buf, sizeof(tmp_buf), 0);
00574 if (len == -1) {
00575 ROS_ERROR("Error recieving on socket");
00576 return -1;
00577 }
00578 else if (len < (int)sizeof(MessageHeader)) {
00579 ROS_ERROR("received message of incorrect size %d\n", len);
00580 }
00581
00582 header = (MessageHeader*)tmp_buf;
00583 {
00584
00585
00586 if(header->message_id == MESSAGE_ID_POWER)
00587 {
00588 power_msg = (PowerMessage*)tmp_buf;
00589 if (len == -1) {
00590 ROS_ERROR("Error recieving on socket");
00591 return -1;
00592 }
00593
00594
00595
00596
00597
00598 else {
00599 if (process_message(power_msg, len))
00600 return -1;
00601 }
00602 }
00603 else if(header->message_id == MESSAGE_ID_TRANSITION)
00604 {
00605 transition_msg = (TransitionMessage *)tmp_buf;
00606 if (len == -1) {
00607 ROS_ERROR("Error recieving on socket");
00608 return -1;
00609 }
00610 else {
00611 if (process_transition_message(transition_msg, len))
00612 return -1;
00613 }
00614 }
00615 else
00616 {
00617 ROS_DEBUG("Discard message len=%d", len);
00618 }
00619 }
00620 }
00621 else {
00622 ROS_ERROR("Unexpected select result %d\n", result);
00623 return -1;
00624 }
00625 }
00626
00627 return 0;
00628 }
00629
00630 PowerBoard::PowerBoard( const ros::NodeHandle node_handle, unsigned int serial_number ) : node_handle(node_handle)
00631 {
00632 ROSCONSOLE_AUTOINIT;
00633 log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
00634 fprintf(stderr, "Logger Name: %s\n", ROSCONSOLE_DEFAULT_NAME);
00635
00636 if( my_logger->getLevel() == 0 )
00637 {
00638
00639 my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Info]);
00640 }
00641
00642 this->serial_number = serial_number;
00643 }
00644
00645 void PowerBoard::init()
00646 {
00647 if(serial_number != 0)
00648 {
00649 ROS_INFO("PowerBoard: created with serial number = %d", serial_number);
00650 Device *newDevice = new Device();
00651 Devices.push_back(newDevice);
00652 }
00653
00654 service = node_handle.advertiseService("control", &PowerBoard::commandCallback, this);
00655
00656 diags_pub = node_handle.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 2);
00657 state_pub = node_handle.advertise<pr2_msgs::PowerBoardState>("state", 2, true);
00658 }
00659
00660
00661 bool PowerBoard::commandCallback(pr2_power_board::PowerBoardCommand::Request &req_,
00662 pr2_power_board::PowerBoardCommand::Response &res_)
00663 {
00664 res_.retval = send_command( req_.serial_number, req_.breaker_number, req_.command, req_.flags);
00665
00666 return true;
00667 }
00668
00669 void PowerBoard::collectMessages()
00670 {
00671 while(node_handle.ok())
00672 {
00673 collect_messages();
00674
00675 }
00676 }
00677
00678 void PowerBoard::sendMessages()
00679 {
00680 ros::Rate r(1);
00681 while(node_handle.ok())
00682 {
00683 r.sleep();
00684
00685 boost::mutex::scoped_lock(library_lock_);
00686
00687 for (unsigned i = 0; i<Devices.size(); ++i)
00688 {
00689 diagnostic_msgs::DiagnosticArray msg_out;
00690 diagnostic_updater::DiagnosticStatusWrapper stat;
00691
00692 Device *device = Devices[i];
00693 const PowerMessage *pmesg = &device->getPowerMessage();
00694
00695 ostringstream ss, ss2;
00696 ss << "Power board " << pmesg->header.serial_num;
00697 ss2 << "68050070" << pmesg->header.serial_num;
00698 stat.name = ss.str();
00699 stat.hardware_id = ss2.str();
00700
00701 if( (ros::Time::now() - device->message_time) > TIMEOUT )
00702 {
00703 stat.summary(2, "No Updates");
00704 }
00705 else
00706 {
00707 stat.summary(0, "Running");
00708 }
00709 const StatusStruct *status = &pmesg->status;
00710
00711 ROS_DEBUG("Device %u", i);
00712 ROS_DEBUG(" Serial = %u", pmesg->header.serial_num);
00713
00714 stat.add("Serial Number", pmesg->header.serial_num);
00715
00716 ROS_DEBUG(" Current = %f", status->input_current);
00717 stat.add("Input Current", status->input_current);
00718
00719 ROS_DEBUG(" Voltages:");
00720 ROS_DEBUG(" Input = %f", status->input_voltage);
00721 stat.add("Input Voltage", status->input_voltage);
00722
00723 ROS_DEBUG(" DCDC 12 aux = %f", status->DCDC_12V_aux);
00724 stat.add("DCDC 12V aux", status->DCDC_12V_aux);
00725
00726 ROS_DEBUG(" DCDC 12V cpu0 = %f", status->DCDC_12V_cpu0);
00727 stat.add("DCDC 12V cpu0", status->DCDC_12V_cpu0);
00728
00729 ROS_DEBUG(" CB0 (Base) = %f", status->CB0_voltage);
00730 stat.add("Breaker 0 Voltage", status->CB0_voltage);
00731
00732 ROS_DEBUG(" CB1 (R-arm) = %f", status->CB1_voltage);
00733 stat.add("Breaker 1 Voltage", status->CB1_voltage);
00734
00735 ROS_DEBUG(" CB2 (L-arm) = %f", status->CB2_voltage);
00736 stat.add("Breaker 2 Voltage", status->CB2_voltage);
00737
00738 ROS_DEBUG(" Board Temp = %f", status->ambient_temp);
00739 stat.add("Board Temperature", status->ambient_temp);
00740
00741 ROS_DEBUG(" Fan Speeds:");
00742 ROS_DEBUG(" Fan 0 = %u", status->fan0_speed);
00743 stat.add("Fan 0 Speed", status->fan0_speed);
00744
00745 ROS_DEBUG(" Fan 1 = %u", status->fan1_speed);
00746 stat.add("Fan 1 Speed", status->fan1_speed);
00747
00748 ROS_DEBUG(" Fan 2 = %u", status->fan2_speed);
00749 stat.add("Fan 2 Speed", status->fan2_speed);
00750
00751 ROS_DEBUG(" Fan 3 = %u", status->fan3_speed);
00752 stat.add("Fan 3 Speed", status->fan3_speed);
00753
00754 ROS_DEBUG(" State:");
00755 ROS_DEBUG(" CB0 (Base) = %s", cb_state_to_str(status->CB0_state));
00756 stat.add("Breaker 0 State", cb_state_to_str(status->CB0_state));
00757
00758 ROS_DEBUG(" CB1 (R-arm) = %s", cb_state_to_str(status->CB1_state));
00759 stat.add("Breaker 1 State", cb_state_to_str(status->CB1_state));
00760
00761 ROS_DEBUG(" CB2 (L-arm) = %s", cb_state_to_str(status->CB2_state));
00762 stat.add("Breaker 2 State", cb_state_to_str(status->CB2_state));
00763
00764 ROS_DEBUG(" DCDC = %s", master_state_to_str(status->DCDC_state));
00765 stat.add("DCDC state", master_state_to_str(status->DCDC_state));
00766
00767 ROS_DEBUG(" Status:");
00768 ROS_DEBUG(" CB0 (Base) = %s", (status->CB0_status) ? "On" : "Off");
00769 stat.add("Breaker 0 Status", (status->CB0_status) ? "On" : "Off");
00770
00771 ROS_DEBUG(" CB1 (R-arm) = %s", (status->CB1_status) ? "On" : "Off");
00772 stat.add("Breaker 1 Status", (status->CB1_status) ? "On" : "Off");
00773
00774 ROS_DEBUG(" CB2 (L-arm) = %s", (status->CB2_status) ? "On" : "Off");
00775 stat.add("Breaker 2 Status", (status->CB2_status) ? "On" : "Off");
00776
00777 ROS_DEBUG(" estop_button= %x", (status->estop_button_status));
00778 stat.add("RunStop Button Status", (status->estop_button_status ? "True":"False"));
00779
00780 ROS_DEBUG(" estop_status= %x", (status->estop_status));
00781 stat.add("RunStop Status", (status->estop_status ? "True":"False"));
00782
00783 ROS_DEBUG(" Revisions:");
00784 ROS_DEBUG(" PCA = %c", status->pca_rev);
00785 stat.add("PCA", status->pca_rev);
00786
00787 ROS_DEBUG(" PCB = %c", status->pcb_rev);
00788 stat.add("PCB", status->pcb_rev);
00789
00790 ROS_DEBUG(" Major = %c", status->major_rev);
00791 stat.add("Major Revision", status->major_rev);
00792
00793 ROS_DEBUG(" Minor = %c", status->minor_rev);
00794 stat.add("Minor Revision", status->minor_rev);
00795
00796 stat.add("Min Voltage", status->min_input_voltage);
00797 stat.add("Max Current", status->max_input_current);
00798
00799 ROS_DEBUG(" DCDC 12V cpu1 = %f", status->DCDC_12V_cpu1);
00800 stat.add("DCDC 12V cpu1", status->DCDC_12V_cpu1);
00801
00802 ROS_DEBUG(" DCDC 12V user = %f", status->DCDC_12V_user);
00803 stat.add("DCDC 12V user", status->DCDC_12V_user);
00804
00805 for( int xx = 0; xx < 4; ++xx)
00806 {
00807 ROS_DEBUG(" Battery %d voltage=%f", xx, status->battery_voltage[xx]);
00808 ss.str("");
00809 ss << "Battery " << xx << " voltage=";
00810 stat.add(ss.str(), status->battery_voltage[xx]);
00811 }
00812
00813
00814 const TransitionMessage *tmsg = &device->getTransitionMessage();
00815 for(int cb_index=0; cb_index < 3; ++cb_index)
00816 {
00817 const TransitionCount *trans = &tmsg->cb[cb_index];
00818 ROS_DEBUG("Transition: CB%d", cb_index);
00819 ss.str("");
00820 ss << "CB" << cb_index << " Stop Count";
00821 stat.add(ss.str(), (int)trans->stop_count);
00822
00823
00824 ss.str("");
00825 ss << "CB" << cb_index << " E-Stop Count";
00826 stat.add(ss.str(), (int)trans->estop_count);
00827
00828 ss.str("");
00829 ss << "CB" << cb_index << " Trip Count";
00830 stat.add(ss.str(), (int)trans->trip_count);
00831
00832 ss.str("");
00833 ss << "CB" << cb_index << " 18V Fail Count";
00834 stat.add(ss.str(), (int)trans->fail_18V_count);
00835
00836 ss.str("");
00837 ss << "CB" << cb_index << " Disable Count";
00838 stat.add(ss.str(), (int)trans->disable_count);
00839
00840 ss.str("");
00841 ss << "CB" << cb_index << " Start Count";
00842 stat.add(ss.str(), (int)trans->start_count);
00843
00844 ss.str("");
00845 ss << "CB" << cb_index << " Pump Fail Count";
00846 stat.add(ss.str(), (int)trans->pump_fail_count);
00847
00848 ss.str("");
00849 ss << "CB" << cb_index << " Reset Count";
00850 stat.add(ss.str(), (int)trans->reset_count);
00851 }
00852
00853 msg_out.status.push_back(stat);
00854 msg_out.header.stamp = ros::Time::now();
00855
00856
00857 diags_pub.publish(msg_out);
00858
00859 pr2_msgs::PowerBoardState state_msg;
00860 state_msg.name = stat.name;
00861 state_msg.serial_num = pmesg->header.serial_num;
00862 state_msg.input_voltage = status->input_voltage;
00863 state_msg.circuit_voltage[0] = status->CB0_voltage;
00864 state_msg.circuit_voltage[1] = status->CB1_voltage;
00865 state_msg.circuit_voltage[2] = status->CB2_voltage;
00866 state_msg.master_state = status->DCDC_state;
00867 state_msg.circuit_state[0] = status->CB0_state;
00868 state_msg.circuit_state[1] = status->CB1_state;
00869 state_msg.circuit_state[2] = status->CB2_state;
00870 state_msg.run_stop = status->estop_status;
00871 state_msg.wireless_stop = status->estop_button_status;
00872 state_msg.header.stamp = ros::Time::now();
00873 state_pub.publish(state_msg);
00874 }
00875 }
00876 }
00877
00878 void getMessages()
00879 {
00880 myBoard->collectMessages();
00881 }
00882
00883 void sendMessages()
00884 {
00885 myBoard->sendMessages();
00886 }
00887
00888
00889 void CloseAllInterfaces(void) {
00890 for (unsigned i=0; i<SendInterfaces.size(); ++i){
00891 if (SendInterfaces[i] != NULL) {
00892 delete SendInterfaces[i];
00893 }
00894 }
00895
00896 delete ReceiveInterface;
00897 }
00898 void CloseAllDevices(void) {
00899 for (unsigned i=0; i<Devices.size(); ++i){
00900 if (Devices[i] != NULL) {
00901 delete Devices[i];
00902 }
00903 }
00904 }
00905
00906 void setupReceive()
00907 {
00908 Interface *newInterface = new Interface("rcv");
00909 newInterface->InitReceive();
00910 ReceiveInterface = newInterface;
00911 }
00912
00913
00914
00915 int CreateAllInterfaces(void)
00916 {
00917
00918 int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
00919 if (sock == -1) {
00920 ROS_ERROR("Couldn't create socket for ioctl requests");
00921 return -1;
00922 }
00923
00924 struct ifconf get_io;
00925 get_io.ifc_req = new ifreq[10];
00926 get_io.ifc_len = sizeof(ifreq) * 10;
00927
00928 if(ioctl( sock, SIOCGIFCONF, &get_io ) == 0)
00929 {
00930 int num_interfaces = get_io.ifc_len / sizeof(ifreq);
00931 ROS_DEBUG("Got %d interfaces", num_interfaces);
00932 for(int yy=0; yy < num_interfaces; ++yy)
00933 {
00934 ROS_DEBUG("interface=%s", get_io.ifc_req[yy].ifr_name);
00935 if(get_io.ifc_req[yy].ifr_addr.sa_family == AF_INET)
00936 {
00937
00938 sockaddr_in *addr = (sockaddr_in*)&get_io.ifc_req[yy].ifr_addr;
00939 ROS_DEBUG("address=%s", inet_ntoa(addr->sin_addr) );
00940
00941 if ((strncmp("lo", get_io.ifc_req[yy].ifr_name, strlen(get_io.ifc_req[yy].ifr_name)) == 0) ||
00942 (strncmp("tun", get_io.ifc_req[yy].ifr_name, 3) == 0) ||
00943 (strncmp("vmnet", get_io.ifc_req[yy].ifr_name, 5) == 0))
00944 {
00945 ROS_INFO("Ignoring interface %*s", (int)strlen(get_io.ifc_req[yy].ifr_name), get_io.ifc_req[yy].ifr_name);
00946 continue;
00947 }
00948 else
00949 {
00950 ROS_INFO("Found interface %*s", (int)strlen(get_io.ifc_req[yy].ifr_name), get_io.ifc_req[yy].ifr_name);
00951 Interface *newInterface = new Interface(get_io.ifc_req[yy].ifr_name);
00952 assert(newInterface != NULL);
00953 if (newInterface == NULL)
00954 {
00955 continue;
00956 }
00957
00958
00959 struct ifreq *ifr = &get_io.ifc_req[yy];
00960 if(ioctl(sock, SIOCGIFBRDADDR, ifr) == 0)
00961 {
00962 if (ifr->ifr_broadaddr.sa_family == AF_INET)
00963 {
00964 struct sockaddr_in *br_addr = (struct sockaddr_in *) &ifr->ifr_dstaddr;
00965
00966 ROS_DEBUG ("Broadcast addess %s", inet_ntoa(br_addr->sin_addr));
00967
00968 if (newInterface->Init(addr, br_addr))
00969 {
00970 ROS_ERROR("Error initializing interface %*s", (int)sizeof(get_io.ifc_req[yy].ifr_name), get_io.ifc_req[yy].ifr_name);
00971 delete newInterface;
00972 newInterface = NULL;
00973 continue;
00974 }
00975 else
00976 {
00977
00978 SendInterfaces.push_back(newInterface);
00979
00980 }
00981
00982 }
00983
00984 }
00985
00986
00987 }
00988 }
00989 }
00990 }
00991 else
00992 {
00993 ROS_ERROR("Bad ioctl status");
00994 }
00995
00996 delete[] get_io.ifc_req;
00997
00998 setupReceive();
00999
01000
01001 return 0;
01002 }
01003
01004 int main(int argc, char** argv)
01005 {
01006 ros::init(argc, argv, "power_board");
01007
01008 unsigned int serial_option;
01009 po::options_description desc("Allowed options");
01010 desc.add_options()
01011 ("help", "this help message")
01012 ("serial", po::value<unsigned int>(&serial_option)->default_value(0), "filter a specific serial number");
01013
01014 po::variables_map vm;
01015 po::store(po::parse_command_line( argc, argv, desc), vm);
01016 po::notify(vm);
01017
01018 if( vm.count("help"))
01019 {
01020 cout << desc << "\n";
01021 return 1;
01022 }
01023
01024 CreateAllInterfaces();
01025
01026 ros::NodeHandle handle("~");
01027 myBoard = new PowerBoard( handle, serial_option);
01028 myBoard->init();
01029
01030 boost::thread getThread( &getMessages );
01031 boost::thread sendThread( &sendMessages );
01032
01033 ros::spin();
01034
01035 sendThread.join();
01036 getThread.join();
01037
01038 CloseAllDevices();
01039 CloseAllInterfaces();
01040
01041
01042 delete myBoard;
01043 return 0;
01044
01045 }
01046
01047 Device::Device(): message_time(0,0)
01048 {
01049 pmsgset = false;
01050 tmsgset = false;
01051 memset( &pmsg, 0, sizeof(PowerMessage));
01052 memset( &tmsg, 0, sizeof(TransitionMessage));
01053 };