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


pr2_power_board
Author(s): Curt Meyers, Blaise Gassend
autogenerated on Thu Jun 6 2019 21:11:02