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
00045
00046 #include <sys/types.h>
00047 #include <sys/socket.h>
00048 #include <netinet/in.h>
00049 #include <arpa/inet.h>
00050 #include <sys/socket.h>
00051 #include <net/if.h>
00052 #include <sys/ioctl.h>
00053
00054 #include "power_comm.h"
00055 #include "power_node.h"
00056 #include "diagnostic_msgs/DiagnosticMessage.h"
00057 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
00058 #include "rosconsole/macros_generated.h"
00059 #include "ros/console.h"
00060
00061 using namespace std;
00062
00063
00064
00065 static std::vector<Device*> Devices;
00066 static PowerBoard *myBoard;
00067
00068 void processSentMessage(CommandMessage *cmd);
00069
00070 void Device::setTransitionMessage(const TransitionMessage &newtmsg)
00071 {
00072 if (tmsgset)
00073 {
00074 #define PRINT_IF_CHANGED(val) \
00075 for (int counter = 0; counter < 3; counter++) \
00076 { \
00077 if (tmsg.cb[counter].val##_count != newtmsg.cb[counter].val##_count) \
00078 { \
00079 ROS_INFO("Power board: CB%i "#val" event count changed to %i.", counter, newtmsg.cb[counter].val##_count);\
00080 } \
00081 }
00082
00083 PRINT_IF_CHANGED(stop);
00084 PRINT_IF_CHANGED(estop);
00085 PRINT_IF_CHANGED(trip);
00086 PRINT_IF_CHANGED(fail_18V);
00087 PRINT_IF_CHANGED(disable);
00088 PRINT_IF_CHANGED(start);
00089 PRINT_IF_CHANGED(pump_fail);
00090 PRINT_IF_CHANGED(reset);
00091
00092 #undef PRINT_IF_CHANGED
00093 }
00094 else
00095 tmsgset = 1;
00096
00097 tmsg = newtmsg;
00098 }
00099
00100 void Device::setPowerMessage(const PowerMessage &newpmsg)
00101 {
00102 if (pmsgset)
00103 {
00104 #define PRINT_IF_CHANGED(val) \
00105 if (pmsg.status.val##_status != newpmsg.status.val##_status) \
00106 { \
00107 ROS_INFO("Power board: status of "#val" changed to %i.", newpmsg.status.val##_status); \
00108 }
00109
00110 PRINT_IF_CHANGED(CB0);
00111 PRINT_IF_CHANGED(CB1);
00112 PRINT_IF_CHANGED(CB2);
00113 PRINT_IF_CHANGED(estop_button);
00114 PRINT_IF_CHANGED(estop);
00115
00116 #undef PRINT_IF_CHANGED
00117 }
00118 else
00119 pmsgset = 1;
00120
00121 pmsg = newpmsg;
00122 }
00123
00124 int PowerBoard::send_command(unsigned int serial_number, int circuit_breaker, const std::string &command, unsigned flags)
00125 {
00126 if (Devices.size() == 0) {
00127 fprintf(stderr,"No devices to send command to\n");
00128 return -1;
00129 }
00130
00131 int selected_device = -1;
00132
00133 for (unsigned i = 0; i<Devices.size(); ++i) {
00134 if (Devices[i]->getPowerMessage().header.serial_num == serial_number) {
00135 selected_device = i;
00136 break;
00137 }
00138 }
00139
00140 if ((selected_device < 0) || (selected_device >= (int)Devices.size())) {
00141 fprintf(stderr, "Device number must be between 0 and %u\n", Devices.size()-1);
00142 return -1;
00143 }
00144
00145 Device* device = Devices[selected_device];
00146 assert(device != NULL);
00147
00148
00149 if ((circuit_breaker < 0) || (circuit_breaker > 2)) {
00150 fprintf(stderr, "Circuit breaker number must be between 0 and 2\n");
00151 return -1;
00152 }
00153
00154 ROS_DEBUG("circuit=%d command=%s flags=%x\n", circuit_breaker, command.c_str(), flags);
00155
00156
00157 char command_enum = NONE;
00158 if (command == "start") {
00159 command_enum = COMMAND_START;
00160 }
00161 else if (command == "stop") {
00162 command_enum = COMMAND_STOP;
00163 }
00164 else if (command == "reset") {
00165 command_enum = COMMAND_RESET;
00166 }
00167 else if (command == "disable") {
00168 command_enum = COMMAND_DISABLE;
00169 }
00170 else if (command == "none") {
00171 command_enum = NONE;
00172 }
00173
00174
00175
00176 else {
00177 ROS_ERROR("invalid command '%s'", command.c_str());
00178 return -1;
00179 }
00180
00181
00182
00183
00184 CommandMessage cmdmsg;
00185 memset(&cmdmsg, 0, sizeof(cmdmsg));
00186 cmdmsg.header.message_revision = CURRENT_MESSAGE_REVISION;
00187 cmdmsg.header.message_id = MESSAGE_ID_COMMAND;
00188 cmdmsg.header.serial_num = device->getPowerMessage().header.serial_num;
00189
00190 strncpy(cmdmsg.header.text, "power command message", sizeof(cmdmsg.header.text));
00191 cmdmsg.command.CB0_command = NONE;
00192 cmdmsg.command.CB1_command = NONE;
00193 cmdmsg.command.CB2_command = NONE;
00194 if (circuit_breaker==0) {
00195 cmdmsg.command.CB0_command = command_enum;
00196 }
00197 else if (circuit_breaker==1) {
00198 cmdmsg.command.CB1_command = command_enum;
00199 }
00200 else if (circuit_breaker==2) {
00201 cmdmsg.command.CB2_command = command_enum;
00202 }
00203 else if (circuit_breaker==-1) {
00204 cmdmsg.command.CB0_command = command_enum;
00205 cmdmsg.command.CB1_command = command_enum;
00206 cmdmsg.command.CB2_command = command_enum;
00207 }
00208
00209 cmdmsg.command.flags = flags;
00210
00211 errno = 0;
00212
00213 processSentMessage(&cmdmsg);
00214
00215 ROS_DEBUG("Send to Serial=%u, revision=%u", cmdmsg.header.serial_num, cmdmsg.header.message_revision);
00216 ROS_DEBUG("Sent command %s(%d) to device %d, circuit %d",
00217 command.c_str(), command_enum, selected_device, circuit_breaker);
00218
00219 return 0;
00220 }
00221
00222
00223 const char* PowerBoard::cb_state_to_str(char state)
00224 {
00225
00226 switch(state) {
00227 case STATE_NOPOWER:
00228 return "no-power";
00229 case STATE_STANDBY:
00230 return "Standby";
00231 case STATE_PUMPING:
00232 return "pumping";
00233 case STATE_ON:
00234 return "On";
00235 case STATE_DISABLED:
00236 return "Disabled";
00237 }
00238 return "???";
00239 }
00240
00241 const char* PowerBoard::master_state_to_str(char state)
00242 {
00243
00244 switch(state) {
00245 case MASTER_NOPOWER:
00246 return "no-power";
00247 case MASTER_STANDBY:
00248 return "stand-by";
00249 case MASTER_ON:
00250 return "on";
00251 case MASTER_OFF:
00252 return "off";
00253 }
00254 return "???";
00255 }
00256
00257
00258
00259
00260
00261 int PowerBoard::process_message(const PowerMessage *msg)
00262 {
00263 if (msg->header.message_revision != CURRENT_MESSAGE_REVISION) {
00264 ROS_WARN("Got message with incorrect message revision %u\n", msg->header.message_revision);
00265 return -1;
00266 }
00267
00268
00269 for (unsigned i = 0; i<Devices.size(); ++i) {
00270 if (Devices[i]->getPowerMessage().header.serial_num == msg->header.serial_num) {
00271 boost::mutex::scoped_lock(library_lock_);
00272 Devices[i]->message_time = ros::Time::now();
00273 Devices[i]->setPowerMessage(*msg);
00274 return 0;
00275 }
00276 }
00277
00278
00279 Device *newDevice = new Device();
00280 Devices.push_back(newDevice);
00281 newDevice->message_time = ros::Time::now();
00282 newDevice->setPowerMessage(*msg);
00283 return 0;
00284 }
00285
00286 int PowerBoard::process_transition_message(const TransitionMessage *msg)
00287 {
00288 if (msg->header.message_revision != CURRENT_MESSAGE_REVISION) {
00289 ROS_WARN("Got message with incorrect message revision %u\n", msg->header.message_revision);
00290 return -1;
00291 }
00292
00293
00294 for (unsigned i = 0; i<Devices.size(); ++i) {
00295 if (Devices[i]->getPowerMessage().header.serial_num == msg->header.serial_num) {
00296 boost::mutex::scoped_lock(library_lock_);
00297 Devices[i]->setTransitionMessage(*msg);
00298 return 0;
00299 }
00300 }
00301
00302
00303 Device *newDevice = new Device();
00304 Devices.push_back(newDevice);
00305 newDevice->setTransitionMessage(*msg);
00306 return 0;
00307 }
00308
00309 PowerBoard::PowerBoard( unsigned int serial_number ): ros::Node ("pr2_power_board")
00310 {
00311
00312 ROSCONSOLE_AUTOINIT;
00313 log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
00314
00315 if( my_logger->getLevel() == 0 )
00316 {
00317
00318 my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Info]);
00319 }
00320
00321 advertiseService("power_board_control", &PowerBoard::commandCallback);
00322 advertise<diagnostic_msgs::DiagnosticMessage>("/diagnostics", 2);
00323 }
00324
00325 bool PowerBoard::commandCallback(pr2_power_board::PowerBoardCommand::Request &req_,
00326 pr2_power_board::PowerBoardCommand::Response &res_)
00327 {
00328 res_.retval = send_command( req_.serial_number, req_.breaker_number, req_.command, req_.flags);
00329
00330 return true;
00331 }
00332
00333 void PowerBoard::sendDiagnostic()
00334 {
00335 ros::Rate r(1);
00336 while(node_handle.ok())
00337 {
00338 r.sleep();
00339
00340 boost::mutex::scoped_lock(library_lock_);
00341
00342 for (unsigned i = 0; i<Devices.size(); ++i)
00343 {
00344 diagnostic_msgs::DiagnosticArray msg_out;
00345 diagnostic_updater::DiagnosticStatusWrapper stat;
00346
00347 Device *device = Devices[i];
00348 const PowerMessage *pmesg = &device->getPowerMessage();
00349
00350 ostringstream ss;
00351 ss << "Power board " << pmesg->header.serial_num;
00352 stat.name = ss.str();
00353
00354 if( (ros::Time::now() - device->message_time) > TIMEOUT )
00355 {
00356 stat.summary(2, "No Updates");
00357 }
00358 else
00359 {
00360 stat.summary(0, "Running");
00361 }
00362 const StatusStruct *status = &pmesg->status;
00363
00364 ROS_DEBUG("Device %u", i);
00365 ROS_DEBUG(" Serial = %u", pmesg->header.serial_num);
00366
00367 stat.add("Serial Number", pmesg->header.serial_num);
00368
00369 ROS_DEBUG(" Current = %f", status->input_current);
00370 stat.add("Input Current", status->input_current);
00371
00372 ROS_DEBUG(" Voltages:");
00373 ROS_DEBUG(" Input = %f", status->input_voltage);
00374 stat.add("Input Voltage", status->input_voltage);
00375
00376 ROS_DEBUG(" DCDC 12 = %f", status->DCDC_12V_out_voltage);
00377 stat.add("DCDC12", status->DCDC_12V_out_voltage);
00378
00379 ROS_DEBUG(" DCDC 15 = %f", status->DCDC_19V_out_voltage);
00380 stat.add("DCDC 15", status->DCDC_19V_out_voltage);
00381
00382 ROS_DEBUG(" CB0 (Base) = %f", status->CB0_voltage);
00383 stat.add("Breaker 0 Voltage", status->CB0_voltage);
00384
00385 ROS_DEBUG(" CB1 (R-arm) = %f", status->CB1_voltage);
00386 stat.add("Breaker 1 Voltage", status->CB1_voltage);
00387
00388 ROS_DEBUG(" CB2 (L-arm) = %f", status->CB2_voltage);
00389 stat.add("Breaker 2 Voltage", status->CB2_voltage);
00390
00391 ROS_DEBUG(" Board Temp = %f", status->ambient_temp);
00392 stat.add("Board Temperature", status->ambient_temp);
00393
00394 ROS_DEBUG(" Fan Speeds:");
00395 ROS_DEBUG(" Fan 0 = %u", status->fan0_speed);
00396 stat.add("Fan 0 Speed", status->fan0_speed);
00397
00398 ROS_DEBUG(" Fan 1 = %u", status->fan1_speed);
00399 stat.add("Fan 1 Speed", status->fan1_speed);
00400
00401 ROS_DEBUG(" Fan 2 = %u", status->fan2_speed);
00402 stat.add("Fan 2 Speed", status->fan2_speed);
00403
00404 ROS_DEBUG(" Fan 3 = %u", status->fan3_speed);
00405 stat.add("Fan 3 Speed", status->fan3_speed);
00406
00407 ROS_DEBUG(" State:");
00408 ROS_DEBUG(" CB0 (Base) = %s", cb_state_to_str(status->CB0_state));
00409 stat.add("Breaker 0 State", cb_state_to_str(status->CB0_state));
00410
00411 ROS_DEBUG(" CB1 (R-arm) = %s", cb_state_to_str(status->CB1_state));
00412 stat.add("Breaker 1 State", cb_state_to_str(status->CB1_state));
00413
00414 ROS_DEBUG(" CB2 (L-arm) = %s", cb_state_to_str(status->CB2_state));
00415 stat.add("Breaker 2 State", cb_state_to_str(status->CB2_state));
00416
00417 ROS_DEBUG(" DCDC = %s", master_state_to_str(status->DCDC_state));
00418 stat.add("DCDC state", master_state_to_str(status->DCDC_state));
00419
00420 ROS_DEBUG(" Status:");
00421 ROS_DEBUG(" CB0 (Base) = %s", (status->CB0_status) ? "On" : "Off");
00422 stat.add("Breaker 0 Status", (status->CB0_status) ? "On" : "Off");
00423
00424 ROS_DEBUG(" CB1 (R-arm) = %s", (status->CB1_status) ? "On" : "Off");
00425 stat.add("Breaker 1 Status", (status->CB1_status) ? "On" : "Off");
00426
00427 ROS_DEBUG(" CB2 (L-arm) = %s", (status->CB2_status) ? "On" : "Off");
00428 stat.add("Breaker 2 Status", (status->CB2_status) ? "On" : "Off");
00429
00430 ROS_DEBUG(" estop_button= %x", (status->estop_button_status));
00431 stat.add("RunStop Button Status", status->estop_button_status);
00432
00433 ROS_DEBUG(" estop_status= %x", (status->estop_status));
00434 stat.add("RunStop Status", status->estop_status);
00435
00436 ROS_DEBUG(" Revisions:");
00437 ROS_DEBUG(" PCA = %c", status->pca_rev);
00438 stat.add("PCA", status->pca_rev);
00439
00440 ROS_DEBUG(" PCB = %c", status->pcb_rev);
00441 stat.add("PCB", status->pcb_rev);
00442
00443 ROS_DEBUG(" Major = %c", status->major_rev);
00444 stat.add("Major Revision", status->major_rev);
00445
00446 ROS_DEBUG(" Minor = %c", status->minor_rev);
00447 stat.add("Minor Revision", status->minor_rev);
00448
00449 stat.add("Min Voltage", status->min_input_voltage);
00450 stat.add("Max Current", status->max_input_current);
00451
00452 const TransitionMessage *tmsg = &device->getTransitionMessage();
00453 for(int cb_index=0; cb_index < 3; ++cb_index)
00454 {
00455 const TransitionCount *trans = &tmsg->cb[cb_index];
00456 ROS_DEBUG("Transition: CB%d", cb_index);
00457 ss.str("");
00458 ss << "CB" << cb_index << " Stop Count";
00459 stat.add(ss.str(), trans->stop_count);
00460
00461 ss.str("");
00462 ss << "CB" << cb_index << " E-Stop Count";
00463 stat.add(ss.str(), trans->estop_count);
00464
00465 ss.str("");
00466 ss << "CB" << cb_index << " Trip Count";
00467 stat.add(ss.str(), trans->trip_count);
00468
00469 ss.str("");
00470 ss << "CB" << cb_index << " 18V Fail Count";
00471 stat.add(ss.str(), trans->fail_18V_count);
00472
00473 ss.str("");
00474 ss << "CB" << cb_index << " Disable Count";
00475 stat.add(ss.str(), trans->disable_count);
00476
00477 ss.str("");
00478 ss << "CB" << cb_index << " Start Count";
00479 stat.add(ss.str(), trans->start_count);
00480
00481 ss.str("");
00482 ss << "CB" << cb_index << " Pump Fail Count";
00483 stat.add(ss.str(), trans->pump_fail_count);
00484
00485 ss.str("");
00486 ss << "CB" << cb_index << " Reset Count";
00487 stat.add(ss.str(), trans->reset_count);
00488 }
00489
00490 msg_out.status.push_back(stat);
00491
00492
00493 pub.publish(msg_out);
00494 }
00495
00496 }
00497 }
00498
00499 void sendMessages()
00500 {
00501 myBoard->sendDiagnostic();
00502 }
00503
00504 void CloseAllDevices(void) {
00505 for (unsigned i=0; i<Devices.size(); ++i){
00506 if (Devices[i] != NULL) {
00507 delete Devices[i];
00508 }
00509 }
00510 }
00511
00512
00513
00514 int CreateAllInterfaces(void)
00515 {
00516 return 0;
00517 }
00518
00519 void generateDeviceMessages()
00520 {
00521 static PowerMessage pm;
00522
00523 pm.header.message_revision = CURRENT_MESSAGE_REVISION;
00524 pm.header.serial_num = 0xDADABABA;
00525
00526 pm.header.message_id = MESSAGE_ID_POWER;
00527 pm.header.data_length = sizeof(pm.status);
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558 int i = 0;
00559 while (myBoard->ok())
00560 {
00561 i++;
00562 if (i % 200 < 100)
00563 {
00564 myBoard->process_message(&pm);
00565 ROS_INFO("Sent message");
00566 }
00567 else
00568 ROS_INFO("Idling");
00569 ros::Duration(0,1e8).sleep();
00570 }
00571 }
00572
00573 void processSentMessage(CommandMessage *cmd)
00574 {
00575 ROS_DEBUG("processSentMessage");
00576 }
00577
00578 int main(int argc, char** argv)
00579 {
00580 ros::init(argc, argv);
00581
00582 CreateAllInterfaces();
00583 myBoard = new PowerBoard();
00584
00585 boost::thread sendThread( &sendMessages );
00586 boost::thread fakeDeviceThread( &generateDeviceMessages );
00587
00588 myBoard->spin();
00589
00590 sendThread.join();
00591
00592 CloseAllDevices();
00593
00594
00595 delete myBoard;
00596 return 0;
00597
00598 }