pr2_power_board: pr2_power_board: power_node

00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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 // Internet/Socket stuff
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 // Keep a pointer to the last message recieved for
00064 // Each board.
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   // Look for device serial number in list of devices...
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   // Determine what command to send
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   /*else if (command == "terrible_hack_shutdown") {
00174     exit(0);
00175   }*/
00176   else {
00177     ROS_ERROR("invalid command '%s'", command.c_str());
00178     return -1;
00179   }
00180   //" -c : command to send to device : 'start', 'stop', 'reset', 'disable'\n"
00181 
00182 
00183   // Build command message
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   //cmdmsg.header.serial_num = 0x12345678;
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   //enum CB_State { STATE_NOPOWER, STATE_STANDBY, STATE_PUMPING, STATE_ON, STATE_DISABLED };
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   //enum CB_State { STATE_NOPOWER, STATE_STANDBY, STATE_PUMPING, STATE_ON, STATE_DISABLED };
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 // Determine if a record of the device already exists...
00259 // If it does use newest message a fill in pointer to old one .
00260 // If it does not.. use
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   // Look for device serial number in list of devices...
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   // Add new device to list
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   // Look for device serial number in list of devices...
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   // Add new device to list
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 )    //has anyone set our level??
00316   {
00317     // Set the ROS logger
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     //ROS_DEBUG("-");
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       //ROS_DEBUG("Publishing ");
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 // Build list of interfaces
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; //32 bit 
00524   pm.header.serial_num = 0xDADABABA;       //32 bit  Unique ID number
00525   //pm.header.text[32];         //Description identifier
00526   pm.header.message_id = MESSAGE_ID_POWER;
00527   pm.header.data_length = sizeof(pm.status);      //Length of the following structure
00528   
00529   /*pm.status.CB0_state;   // CB_State enum
00530   pm.status.CB1_state;
00531   pm.status.CB2_state;
00532   pm.status.DCDC_state;  // Master_State enum
00533   
00534   pm.status.input_voltage;
00535   pm.status.input_current;
00536   pm.status.DCDC_12V_out_voltage;
00537   pm.status.DCDC_19V_out_voltage;
00538   pm.status.CB0_voltage;
00539   pm.status.CB1_voltage;
00540   pm.status.CB2_voltage;
00541   pm.status.ambient_temp;
00542   pm.status.fan0_speed; 
00543   pm.status.fan1_speed;
00544   pm.status.fan2_speed;
00545   pm.status.fan3_speed;
00546   pm.status.CB0_status;  //0-off 1-on
00547   pm.status.CB1_status;
00548   pm.status.CB2_status;
00549   pm.status.estop_button_status;
00550   pm.status.estop_status;
00551   pm.status.pca_rev;
00552   pm.status.pcb_rev;
00553   pm.status.major_rev;
00554   pm.status.minor_rev;
00555   pm.status.min_input_voltage;
00556   pm.status.max_input_current;*/
00557 
00558   int i = 0;
00559   while (myBoard->ok())
00560   {
00561     i++;
00562     if (i % 200 < 100) // Alternate between sending and not sending messages to test the display.
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(); //wait for ros to shut us down
00589 
00590   sendThread.join();
00591 
00592   CloseAllDevices();
00593 
00594   
00595   delete myBoard;
00596   return 0;
00597 
00598 }
power_node_simulator.cpp
