power_node2.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 "power_comm.h"
00056 #include "power_node2.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 #define TEMP_WARN 60
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 Device *devicePtr;
00071 static Interface *SendInterface;
00072 static Interface *ReceiveInterface;
00073 static PowerBoard *myBoard;
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() : recv_sock(-1), send_sock(-1) 
00142 {
00143 
00144 }
00145 
00146 
00147 void Interface::Close() {
00148   if (recv_sock != -1) {
00149     close(recv_sock);
00150     recv_sock = -1;
00151   }
00152   if (send_sock != -1) {
00153     close(send_sock);
00154     send_sock = -1;
00155   }
00156 }
00157 
00158 
00159 int Interface::InitReceive(const std::string &address_str)
00160 {
00161 
00162   recv_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
00163   if (recv_sock == -1) {
00164     perror("Couldn't create recv socket");
00165     return -1;
00166   }
00167 
00168  // Allow reuse of receive port
00169   int opt = 1;
00170   if (setsockopt(recv_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) {
00171     perror("Couldn't set reuse addr on recv socket\n");
00172     Close();
00173     return -1;
00174   }
00175 
00176 #if 0
00177   // Allow broadcast on send socket
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 #endif
00185 
00186   // Bind socket to receive packets on <UDP_STATUS_PORT> from any address/interface
00187   struct sockaddr_in sin;
00188   memset(&sin, 0, sizeof(sin));
00189   sin.sin_family = AF_INET;
00190   sin.sin_port = htons(POWER_PORT);
00191   sin.sin_addr.s_addr = (INADDR_ANY);
00192   //inet_pton( AF_INET, address_str.c_str(), &sin.sin_addr);
00193   //inet_pton( AF_INET, "192.168.10.10", &sin.sin_addr);
00194   if (bind(recv_sock, (struct sockaddr*)&sin, sizeof(sin))) {
00195     perror("Couldn't Bind socket to port");
00196     Close();
00197     return -1;
00198   }
00199 
00200   return 0;
00201 }
00202 
00203 int Interface::Init( const std::string &address_str)
00204 {
00205 
00206   send_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
00207   if (send_sock == -1) {
00208     Close();
00209     perror("Couldn't create send socket");
00210     return -1;
00211   }
00212 
00213 
00214   int opt;
00215  
00216  // Allow reuse of receive port
00217   opt = 1;
00218   if (setsockopt(send_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) {
00219     perror("Error allowing socket reuse");
00220     Close();
00221     return -1;
00222   }
00223 
00224   struct sockaddr_in sin;
00225   memset(&sin, 0, sizeof(sin));
00226   sin.sin_family = AF_INET;
00227 
00228   sin.sin_port = htons(POWER_PORT);
00229   //sin.sin_addr.s_addr = inet_addr("192.168.13.19");
00230   inet_pton( AF_INET, address_str.c_str(), &sin.sin_addr);
00231   if (connect(send_sock, (struct sockaddr*)&sin, sizeof(sin))) {
00232     perror("Connect'ing socket failed");
00233     Close();
00234     return -1;
00235   }
00236 
00237   return 0;
00238 }
00239 
00240 void Interface::AddToReadSet(fd_set &set, int &max_sock) const {
00241   FD_SET(recv_sock,&set);
00242   if (recv_sock > max_sock)
00243     max_sock = recv_sock;
00244 }
00245 
00246 bool Interface::IsReadSet(fd_set set) const {
00247   return FD_ISSET(recv_sock,&set);
00248 }
00249 
00250 int PowerBoard::send_command( int circuit_breaker, const std::string &command, unsigned flags)
00251 {
00252   assert(devicePtr != NULL);
00253 
00254   // Build command message
00255   CommandMessage cmdmsg;
00256   memset(&cmdmsg, 0, sizeof(cmdmsg));
00257   cmdmsg.header.message_revision = COMMAND_MESSAGE_REVISION;
00258   cmdmsg.header.message_id = MESSAGE_ID_COMMAND;
00259   cmdmsg.header.serial_num = devicePtr->getPowerMessage().header.serial_num;
00260   //cmdmsg.header.serial_num = 0x12345678;
00261   strncpy(cmdmsg.header.text, "power command message", sizeof(cmdmsg.header.text));
00262 
00263   if(command == "fan")
00264   {
00265     //use circuit_breaker to choose which fan.
00266     //use flags to set the duty cycle.
00267 
00268     if((circuit_breaker > 3) || (circuit_breaker < 0))
00269     {
00270       fprintf(stderr, "Fan must be between 0 and 3\n" );
00271       return -1;
00272     }
00273 
00274     switch(circuit_breaker)
00275     {
00276       default:
00277       case 0:
00278         cmdmsg.command.fan0_command = flags;
00279         break;
00280       case 1:
00281         cmdmsg.command.fan1_command = flags;
00282         break;
00283       case 2:
00284         cmdmsg.command.fan2_command = flags;
00285         break;
00286       case 3:
00287         cmdmsg.command.fan3_command = flags;
00288         break;
00289     }
00290 
00291     ROS_DEBUG("Set fan %d to %d%% output", circuit_breaker, flags);
00292   }
00293   else
00294   {
00295     if ((circuit_breaker < 0) || (circuit_breaker >= pr2_power_board::PowerBoardCommand2::Request::NUMBER_OF_CIRCUITS)) {
00296       fprintf(stderr, "Circuit breaker number must be between 0 and %d\n", pr2_power_board::PowerBoardCommand2::Request::NUMBER_OF_CIRCUITS);
00297       return -1;
00298     }
00299 
00300     ROS_DEBUG("circuit=%d command=%s flags=%x\n", circuit_breaker, command.c_str(), flags);
00301 
00302     // Set fan duty based on battery temperature. #4763
00303     cmdmsg.command.fan0_command = getFanDuty();
00304     
00305     // Determine what command to send
00306     char command_enum = NONE;
00307     if (command == "start") {
00308       command_enum = COMMAND_START;
00309     }
00310     else if (command ==  "stop") {
00311       command_enum = COMMAND_STOP;
00312     }
00313     else if (command == "reset") {
00314       command_enum = COMMAND_RESET;
00315     }
00316     else if (command == "disable") {
00317       command_enum = COMMAND_DISABLE;
00318     }
00319     else if (command == "none") {
00320       command_enum = NONE;
00321     }
00322     else {
00323       ROS_ERROR("invalid command '%s'", command.c_str());
00324       return -1;
00325     }
00326     //" -c : command to send to device : 'start', 'stop', 'reset', 'disable'\n"
00327 
00328 
00329     if (circuit_breaker==0) {
00330       cmdmsg.command.CB0_command = command_enum;
00331     }
00332     else if (circuit_breaker==1) {
00333       cmdmsg.command.CB1_command = command_enum;
00334     }
00335     else if (circuit_breaker==2) {
00336       cmdmsg.command.CB2_command = command_enum;
00337     }
00338     else if (circuit_breaker==-1) {
00339       cmdmsg.command.CB0_command = command_enum;
00340       cmdmsg.command.CB1_command = command_enum;
00341       cmdmsg.command.CB2_command = command_enum;
00342     }
00343 
00344     cmdmsg.command.flags = flags;
00345     ROS_DEBUG("Sent command %s(%d), circuit %d", command.c_str(), command_enum, circuit_breaker);
00346 
00347   }
00348 
00349 
00350 
00351   errno = 0;
00352   //ROS_INFO("Send on %s", inet_ntoa(SendInterfaces[xx]->ifc_address.sin_addr));
00353   int result = send(SendInterface->send_sock, &cmdmsg, sizeof(cmdmsg), 0);
00354   if (result == -1) {
00355     ROS_ERROR("Error sending");
00356     return -1;
00357   } else if (result != sizeof(cmdmsg)) {
00358     ROS_WARN("Error sending : send only took %d of %lu bytes\n",
00359             result, sizeof(cmdmsg));
00360   }
00361 
00362   ROS_DEBUG("Send to Serial=%u, revision=%u", cmdmsg.header.serial_num, cmdmsg.header.message_revision);
00363 
00364 
00365   return 0;
00366 }
00367 
00368 
00369 const char* PowerBoard::cb_state_to_str(char state)
00370 {
00371   //enum CB_State { STATE_NOPOWER, STATE_STANDBY, STATE_PUMPING, STATE_ON, STATE_DISABLED };
00372   switch(state) {
00373   case STATE_NOPOWER:
00374     return "no-power";
00375   case STATE_STANDBY:
00376     return "Standby";
00377   case STATE_PUMPING:
00378     return "pumping";
00379   case STATE_ON:
00380     return "Enabled";
00381   case STATE_DISABLED:
00382     return "Disabled";
00383   }
00384   return "???";
00385 }
00386 
00387 const char* PowerBoard::master_state_to_str(char state)
00388 {
00389   //enum CB_State { STATE_NOPOWER, STATE_STANDBY, STATE_PUMPING, STATE_ON, STATE_DISABLED };
00390   switch(state) {
00391   case MASTER_NOPOWER:
00392     return "no-power";
00393   case MASTER_STANDBY:
00394     return "stand-by";
00395   case MASTER_ON:
00396     return "on";
00397   case MASTER_OFF:
00398     return "off";
00399   case MASTER_SHUTDOWN:
00400     return "shutdown";
00401   }
00402   return "???";
00403 }
00404 
00405 
00406 // Determine if a record of the device already exists...
00407 // If it does use newest message a fill in pointer to old one .
00408 // If it does not.. use
00409 int PowerBoard::process_message(const PowerMessage *msg, int len)
00410 {
00411   if ((msg->header.message_revision > CURRENT_MESSAGE_REVISION) || (msg->header.message_revision < MINIMUM_MESSAGE_REVISION)) {
00412     ROS_WARN("Got message with incorrect message revision %u\n", msg->header.message_revision);
00413     return -1;
00414   }
00415 
00416   if ((msg->header.message_revision == CURRENT_MESSAGE_REVISION) && (len != CURRENT_MESSAGE_SIZE))
00417     ROS_ERROR("received message of incorrect size %d for rev=%d\n", len, msg->header.message_revision);
00418 
00419   if ((msg->header.message_revision == MINIMUM_MESSAGE_REVISION) && (len != REVISION_2_MESSAGE_SIZE))
00420     ROS_ERROR("received message of incorrect size %d for rev=%d\n", len, msg->header.message_revision);
00421 
00422 
00423   {
00424     boost::mutex::scoped_lock(library_lock_);
00425     devicePtr->message_time = ros::Time::now();
00426     devicePtr->setPowerMessage(*msg);
00427   }
00428 
00429   return 0;
00430 }
00431 
00432 int PowerBoard::process_transition_message(const TransitionMessage *msg, int len)
00433 {
00434   if (msg->header.message_revision != TRANSITION_MESSAGE_REVISION) {
00435     ROS_WARN("Got message with incorrect message revision %u\n", msg->header.message_revision);
00436     return -1;
00437   }
00438 
00439   if (len != sizeof(TransitionMessage)) {
00440     ROS_ERROR("received message of incorrect size %d\n", len);
00441     return -2;
00442   }
00443 
00444   {
00445     boost::mutex::scoped_lock(library_lock_);
00446     devicePtr->message_time = ros::Time::now();
00447     devicePtr->setTransitionMessage(*msg);
00448   }
00449 
00450   return 0;
00451 }
00452 
00453 // collect status packets for 0.5 seconds.  Keep the last packet sent
00454 // from each seperate power device.
00455 int PowerBoard::collect_messages()
00456 {
00457   PowerMessage *power_msg;
00458   TransitionMessage *transition_msg;
00459   char tmp_buf[256];  //bigger than our max size we expect
00460 
00461   //ROS_DEBUG("PowerMessage size=%u", sizeof(PowerMessage));
00462   //ROS_DEBUG("TransitionMessage size=%u", sizeof(TransitionMessage));
00463 
00464   timeval timeout;  //timeout once a second to check if we should die or not.
00465   timeout.tv_sec = 1;
00466   timeout.tv_usec = 0;
00467 
00468   while (1)
00469   {
00470     // Wait for packets to arrive on socket.
00471     fd_set read_set;
00472     int max_sock = -1;
00473     FD_ZERO(&read_set);
00474     //for (unsigned i = 0; i<SendInterfaces.size(); ++i)
00475       ReceiveInterface->AddToReadSet(read_set,max_sock);
00476 
00477     int result = select(max_sock+1, &read_set, NULL, NULL, &timeout);
00478 
00479     //fprintf(stderr,"*");
00480 
00481     if (result == -1) {
00482       perror("Select");
00483       return -1;
00484     }
00485     else if (result == 0) {
00486       return 0;
00487     }
00488     else if (result >= 1) {
00489       Interface *recvInterface = ReceiveInterface;
00490 
00491       struct sockaddr_in crap;
00492       socklen_t sock_len = sizeof(crap);
00493 
00494       //ROS_INFO("Receive on %s", inet_ntoa(((struct sockaddr_in *)(&recvInterface->interface.ifr_dstaddr))->sin_addr));
00495       int len = recvfrom(recvInterface->recv_sock, tmp_buf, sizeof(tmp_buf), 0, (sockaddr*)&crap, &sock_len);
00496       if (len == -1) {
00497         ROS_ERROR("Error recieving on socket");
00498         return -1;
00499       }
00500       else if (len < (int)sizeof(MessageHeader)) {
00501         ROS_ERROR("received message of incorrect size %d\n", len);
00502       }
00503 
00504 #if 0
00505       char str[INET_ADDRSTRLEN];
00506       inet_ntop(AF_INET, &(crap.sin_addr), str, INET_ADDRSTRLEN);
00507       ROS_DEBUG("received from = %s", str);
00508 #endif
00509 
00510       if(crap.sin_addr.s_addr == ip_address )
00511       {
00512         MessageHeader *header;
00513         header = (MessageHeader*)tmp_buf;
00514 
00515         //ROS_DEBUG("Header type=%d", header->message_id);
00516         if(header->message_id == MESSAGE_ID_POWER)
00517         {
00518           power_msg = (PowerMessage*)tmp_buf;
00519           if (len == -1) {
00520             ROS_ERROR("Error recieving on socket");
00521             return -1;
00522           }
00523 /*
00524           else if (len != (int)sizeof(PowerMessage)) {
00525             ROS_ERROR("received message of incorrect size %d\n", len);
00526           }
00527 */
00528           else {
00529             if (process_message(power_msg, len))
00530               return -1;
00531           }
00532         }
00533         else if(header->message_id == MESSAGE_ID_TRANSITION)
00534         {
00535           transition_msg = (TransitionMessage *)tmp_buf;
00536           if (len == -1) {
00537             ROS_ERROR("Error recieving on socket");
00538             return -1;
00539           }
00540           else {
00541             if (process_transition_message(transition_msg, len))
00542               return -1;
00543           }
00544         }
00545         else
00546         {
00547           ROS_DEBUG("Discard message len=%d", len);
00548         }
00549       }
00550     }
00551     else {
00552       ROS_ERROR("Unexpected select result %d\n", result);
00553       return -1;
00554     }
00555   }
00556 
00557   return 0;
00558 }
00559 
00560 PowerBoard::PowerBoard( const ros::NodeHandle node_handle, const std::string &address_str ) : 
00561   node_handle(node_handle),
00562   fan_high_(false)
00563 {
00564   ROSCONSOLE_AUTOINIT;
00565   log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
00566   fprintf(stderr, "Logger Name: %s\n", ROSCONSOLE_DEFAULT_NAME);
00567 
00568   if( my_logger->getLevel() == 0 )    //has anyone set our level??
00569   {
00570     // Set the ROS logger
00571     my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Info]);
00572   }
00573 
00574   sockaddr_in sin;
00575   inet_pton( AF_INET, address_str.c_str(), &sin.sin_addr);
00576   ip_address = sin.sin_addr.s_addr;
00577 
00578 }
00579 
00580 void PowerBoard::init()
00581 {
00582   devicePtr = new Device();
00583 
00584   service = node_handle.advertiseService("control", &PowerBoard::commandCallback, this);
00585   service2 = node_handle.advertiseService("control2", &PowerBoard::commandCallback2, this);
00586 
00587   diags_pub = node_handle.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 2);
00588   state_pub = node_handle.advertise<pr2_msgs::PowerBoardState>("state", 2, true);
00589 
00590   ros::NodeHandle main_handle;
00591   battery_sub_ = main_handle.subscribe("battery/server2", 10, &PowerBoard::batteryCB, this);
00592 }
00593 
00594 void PowerBoard::batteryCB(const pr2_msgs::BatteryServer2::ConstPtr &msgPtr)
00595 {
00596   float max_temp = -1.0f;
00597   
00598   std::vector<pr2_msgs::BatteryState2>::const_iterator it;
00599   for (it = msgPtr->battery.begin(); it != msgPtr->battery.end(); ++it)
00600   {
00601     // Convert to celcius from 0.1K
00602     float batt_temp = ((float) it->battery_register[0x08]) / 10.0f - 273.15;
00603 
00604     max_temp = max(max_temp, batt_temp);
00605   }
00606 
00607   ROS_DEBUG("Logging battery server %d with temperature %f", msgPtr->id, max_temp);
00608   battery_temps_[msgPtr->id] = max_temp;
00609 }
00610 
00611 int PowerBoard::getFanDuty()
00612 {
00613   // Find max battery temp
00614   float max_temp = -1.0f;
00615 
00616   std::map<int, float>::const_iterator it;
00617   for (it = battery_temps_.begin(); it != battery_temps_.end(); ++it)
00618     max_temp = max(max_temp, it->second);
00619 
00620   // Find the appropriate duty cycle based on battery temp
00621   // Turn on fan when temp hits 46C, turn off when temp drops to 44C
00622   int battDuty = 0;
00623   if (max_temp > 46.0f)
00624     battDuty = 100;
00625   else if (max_temp > 44.0f && fan_high_)
00626     battDuty = 100; // Hysteresis in fan controller
00627   else // (max_temp < 44.0f) || (max_temp > 44.0f && !fan_high_ && max_temp < 46.0f) 
00628     battDuty = 0;
00629  
00630   fan_high_ = battDuty > 0;
00631 
00632   ROS_DEBUG("Fan duty cycle based on battery temperature: %d, Battery temp: %.1f", battDuty, max_temp);
00633   return battDuty;
00634 }
00635 
00636 void PowerBoard::checkFanSpeed()
00637 {
00638   send_command(0, "fan", getFanDuty());
00639 }
00640 
00641 bool PowerBoard::commandCallback(pr2_power_board::PowerBoardCommand::Request &req_,
00642                                  pr2_power_board::PowerBoardCommand::Response &res_)
00643 {
00644   res_.retval = send_command( req_.breaker_number, req_.command, req_.flags);
00645   requestMessage(MESSAGE_ID_POWER);
00646   requestMessage(MESSAGE_ID_TRANSITION);
00647 
00648   return true;
00649 }
00650 
00651 bool PowerBoard::commandCallback2(pr2_power_board::PowerBoardCommand2::Request &req_,
00652                                   pr2_power_board::PowerBoardCommand2::Response &res_)
00653 {
00654   
00655   unsigned flags = 0;
00656   if(req_.reset_stats)
00657     flags |= 0x1;
00658   if(req_.reset_circuits)
00659     flags |= 0x2;
00660 
00661   res_.success = send_command( req_.circuit, req_.command, flags);
00662   requestMessage(MESSAGE_ID_POWER);
00663   requestMessage(MESSAGE_ID_TRANSITION);
00664 
00665   return true;
00666 }
00667 
00668 void PowerBoard::collectMessages()
00669 {
00670   while(node_handle.ok())
00671   {
00672     collect_messages();
00673     //ROS_DEBUG("*");
00674   }
00675 }
00676 
00677 void PowerBoard::sendMessages()
00678 {
00679   ros::Rate r(5);
00680   r.sleep();
00681   r = 1;
00682   r.reset();
00683 
00684   while(node_handle.ok())
00685   {
00686     r.sleep();
00687     //ROS_DEBUG("-");
00688     boost::mutex::scoped_lock(library_lock_);
00689   
00690     {
00691       diagnostic_msgs::DiagnosticArray msg_out;
00692       diagnostic_updater::DiagnosticStatusWrapper stat;
00693 
00694       const PowerMessage *pmesg = &devicePtr->getPowerMessage();
00695       
00696       ostringstream ss, ss2;
00697       ss << "Power board " << pmesg->header.serial_num;
00698       ss2 << "68050070" << pmesg->header.serial_num;
00699       stat.name = ss.str();
00700       stat.hardware_id = ss2.str();
00701 
00702       if( (ros::Time::now() - devicePtr->message_time) > TIMEOUT )
00703       {
00704         stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No Updates");
00705       }
00706       else
00707       {
00708         stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Running");
00709       }
00710       const StatusStruct *status = &pmesg->status;
00711 
00712       if (status->fan0_speed == 0)
00713       {
00714         stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Base Fan Off");
00715       }
00716 
00717       //ROS_DEBUG("Device %u", i);
00718       //ROS_DEBUG(" Serial       = %u", pmesg->header.serial_num);
00719 
00720       stat.add("Serial Number", pmesg->header.serial_num);
00721 
00722       //ROS_DEBUG(" Current      = %f", status->input_current);
00723       stat.add("Input Current", status->input_current);
00724 
00725       //ROS_DEBUG(" Voltages:");
00726       //ROS_DEBUG("  Input       = %f", status->input_voltage);
00727       stat.add("Input Voltage", status->input_voltage);
00728       
00729       //ROS_DEBUG("  DCDC 12 aux = %f", status->DCDC_12V_aux);
00730       stat.add("DCDC 12V aux", status->DCDC_12V_aux);
00731       
00732       //ROS_DEBUG("  DCDC 12V cpu0   = %f", status->DCDC_12V_cpu0);
00733       stat.add("DCDC 12V c1", status->DCDC_12V_cpu0);
00734       
00735       //ROS_DEBUG("  CB0 (Base)  = %f", status->CB0_voltage);
00736       stat.add("Breaker 0 Voltage", status->CB0_voltage);
00737       
00738       //ROS_DEBUG("  CB1 (R-arm) = %f", status->CB1_voltage);
00739       stat.add("Breaker 1 Voltage", status->CB1_voltage);
00740       
00741       //ROS_DEBUG("  CB2 (L-arm) = %f", status->CB2_voltage);
00742       stat.add("Breaker 2 Voltage", status->CB2_voltage);
00743 
00744       //ROS_DEBUG(" Board Temp   = %f", status->ambient_temp);
00745       stat.add("Board Temperature", status->ambient_temp);
00746 
00747       if (status->ambient_temp > TEMP_WARN) 
00748       { 
00749         stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "High Temp");
00750       } 
00751 
00752       //ROS_DEBUG(" Fan Speeds:");
00753       //ROS_DEBUG("  Fan 0       = %u", status->fan0_speed);
00754       stat.add("Base Fan Speed", status->fan0_speed);
00755       
00756       //ROS_DEBUG("  Fan 1       = %u", status->fan1_speed);
00757       stat.add("Expansion Fan 1 Speed", status->fan1_speed);
00758       
00759       //ROS_DEBUG("  Fan 2       = %u", status->fan2_speed);
00760       stat.add("Expansion Fan 2 Speed", status->fan2_speed);
00761       
00762       //ROS_DEBUG("  Fan 3       = %u", status->fan3_speed);
00763       stat.add("Expansion Fan 3 Speed", status->fan3_speed);
00764 
00765       //ROS_DEBUG(" State:");
00766       //ROS_DEBUG("  CB0 (Base)  = %s", cb_state_to_str(status->CB0_state));
00767       stat.add("Breaker 0 (Left Arm) State", cb_state_to_str(status->CB0_state));
00768 
00769       //ROS_DEBUG("  CB1 (R-arm) = %s", cb_state_to_str(status->CB1_state));
00770       stat.add("Breaker 1 (Base/Body) State", cb_state_to_str(status->CB1_state));
00771 
00772       //ROS_DEBUG("  CB2 (L-arm) = %s", cb_state_to_str(status->CB2_state));
00773       stat.add("Breaker 2 (Right Arm) State", cb_state_to_str(status->CB2_state));
00774       
00775       //ROS_DEBUG("  DCDC        = %s", master_state_to_str(status->DCDC_state));
00776       stat.add("DCDC state", master_state_to_str(status->DCDC_state));
00777 
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 Wireless Status", (status->estop_status ? "True":"False"));
00784 
00785       //ROS_DEBUG(" Revisions:");
00786       //ROS_DEBUG("         PCA = %c", status->pca_rev);
00787       stat.add("Circuit assembly revision", status->pca_rev);
00788 
00789       //ROS_DEBUG("         PCB = %c", status->pcb_rev);
00790       stat.add("Circuit board revision", 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 c2", 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 = &devicePtr->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);
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       //ROS_DEBUG("Publishing ");
00858       diags_pub.publish(msg_out);
00859 
00860       // Only publish a message if we've received data recently. #3877
00861       if ((ros::Time::now() - devicePtr->message_time) < TIMEOUT )
00862       {
00863         pr2_msgs::PowerBoardState state_msg;
00864         state_msg.name = stat.name;
00865         state_msg.serial_num = pmesg->header.serial_num;
00866         state_msg.input_voltage = status->input_voltage;
00867         state_msg.circuit_voltage[0] = status->CB0_voltage;
00868         state_msg.circuit_voltage[1] = status->CB1_voltage;
00869         state_msg.circuit_voltage[2] = status->CB2_voltage;
00870         state_msg.master_state = status->DCDC_state;
00871         state_msg.circuit_state[0] = status->CB0_state;
00872         state_msg.circuit_state[1] = status->CB1_state;
00873         state_msg.circuit_state[2] = status->CB2_state;
00874         state_msg.run_stop = status->estop_status;
00875         state_msg.wireless_stop = status->estop_button_status;
00876         state_msg.header.stamp = ros::Time::now();
00877         state_pub.publish(state_msg);
00878       }
00879     }
00880   }
00881 }
00882 
00883 int PowerBoard::requestMessage(const unsigned int message)
00884 {
00885 
00886   GetMessage cmdmsg;
00887   memset(&cmdmsg, 0, sizeof(cmdmsg));
00888   cmdmsg.header.message_revision = STATUS_MESSAGE_REVISION;
00889   cmdmsg.header.message_id = MESSAGE_ID_STATUS;
00890   cmdmsg.header.serial_num = devicePtr->getPowerMessage().header.serial_num;
00891   strncpy(cmdmsg.header.text, "power status message", sizeof(cmdmsg.header.text));
00892 
00893   cmdmsg.message_to_get = message;
00894 
00895   errno = 0;
00896   int result = send(SendInterface->send_sock, &cmdmsg, sizeof(cmdmsg), 0);
00897   if (result == -1) {
00898     ROS_ERROR("Error sending");
00899     return -1;
00900   } else if (result != sizeof(cmdmsg)) {
00901     ROS_WARN("Error sending : send only took %d of %lu bytes\n", result, sizeof(cmdmsg));
00902   }
00903 
00904   ROS_DEBUG("requestMessage: to Serial=%u, revision=%u", cmdmsg.header.serial_num, cmdmsg.header.message_revision);
00905   return 0;
00906 }
00907 
00908 
00909 void getMessages()
00910 {
00911   myBoard->collectMessages();
00912 }
00913 
00914 void sendMessages()
00915 {
00916   myBoard->sendMessages();
00917 }
00918 
00919 // CloseAll
00920 void CloseAllInterfaces(void) 
00921 {
00922 
00923   delete SendInterface;
00924   delete ReceiveInterface;
00925 }
00926 
00927 void CloseAllDevices(void) 
00928 {
00929   delete devicePtr;
00930 }
00931 
00932 
00933 // Build list of interfaces
00934 int CreateAllInterfaces(const std::string &address_str)
00935 {
00936   //
00937   int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
00938   if (sock == -1) {
00939     ROS_ERROR("Couldn't create socket for ioctl requests");
00940     return -1;
00941   }
00942 
00943   SendInterface = new Interface();
00944   if (SendInterface->Init(address_str))
00945   {
00946     ROS_ERROR("Error initializing interface");
00947     delete SendInterface;
00948     SendInterface = NULL;
00949   }
00950 
00951 
00952   ReceiveInterface  = new Interface();
00953   ReceiveInterface->InitReceive(address_str);
00954 
00955   return 0;
00956 }
00957 
00958 int main(int argc, char** argv)
00959 {
00960   ros::init(argc, argv, "power_board");
00961 
00962   std::string address_str;
00963   po::options_description desc("Allowed options");
00964   desc.add_options()
00965     ("help", "this help message")
00966     ("address", po::value<std::string>(&address_str), "IP address for specific Power Board");
00967 
00968   po::variables_map vm;
00969   po::store(po::parse_command_line( argc, argv, desc), vm);
00970   po::notify(vm);
00971 
00972   if( vm.count("help"))
00973   {
00974     cout << desc << "\n";
00975     return 1;
00976   }
00977 
00978   if( vm.count("address") == 0 )
00979   {
00980     ROS_ERROR("PowerNode: Error you did not specify the IP address, use --address= to specify the address of the power board.");
00981     exit(-1);
00982   }
00983 
00984   ROS_INFO("PowerNode:Using Address=%s", address_str.c_str());
00985 
00986 
00987   CreateAllInterfaces(address_str);
00988 
00989   ros::NodeHandle private_handle("~");
00990   myBoard = new PowerBoard(private_handle, address_str );
00991   myBoard->init();
00992 
00993   boost::thread getThread( &getMessages );
00994   boost::thread sendThread( &sendMessages );
00995 
00996   double sample_frequency = 10; //In Hertz
00997   double transition_frequency = 0.1; //In Hertz
00998   private_handle.getParam( "sample_frequency", sample_frequency );
00999   ROS_INFO("Using sampling frequency %fHz", sample_frequency);
01000   private_handle.getParam( "transition_frequency", transition_frequency );
01001   ROS_INFO("Using transition frequency %fHz", transition_frequency);
01002 
01003   ros::Time last_msg( 0, 0);
01004   ros::Time last_transition( 0, 0);
01005   ros::Time last_batt_check(0, 0);
01006 
01007   double ros_rate = 10; //(Hz) need to run the "spin" loop at some number of Hertz to handle ros things.
01008 
01009   if(sample_frequency > ros_rate)
01010     ros_rate = sample_frequency;
01011 
01012   ros::Rate r(ros_rate);
01013   const ros::Duration MSG_RATE(1/sample_frequency);
01014   const ros::Duration TRANSITION_RATE(1/transition_frequency);
01015   const ros::Duration BATT_CHECK_RATE(1 / 0.1);
01016 
01017   while(private_handle.ok())
01018   {
01019     r.sleep();
01020     if( (ros::Time::now() - last_msg) > MSG_RATE )
01021     {
01022       myBoard->requestMessage(MESSAGE_ID_POWER);
01023       //ROS_INFO("Send ");
01024       last_msg = ros::Time::now();
01025     }
01026 
01027     if( (ros::Time::now() - last_transition) > TRANSITION_RATE )
01028     {
01029       myBoard->requestMessage(MESSAGE_ID_TRANSITION);
01030       last_transition = ros::Time::now();
01031     }
01032 
01033     if (ros::Time::now() - last_batt_check > BATT_CHECK_RATE)
01034     {
01035       last_batt_check = ros::Time::now();
01036       myBoard->checkFanSpeed();
01037     }
01038 
01039     ros::spinOnce();
01040   }
01041 
01042   sendThread.join();
01043   getThread.join();
01044 
01045   CloseAllDevices();
01046   CloseAllInterfaces();
01047 
01048   
01049   delete myBoard;
01050   return 0;
01051 
01052 }
01053 
01054 Device::Device(): message_time(0,0)
01055 { 
01056   pmsgset = false; 
01057   tmsgset = false; 
01058   memset( &pmsg, 0, sizeof(PowerMessage)); 
01059   memset( &tmsg, 0, sizeof(TransitionMessage)); 
01060 };


pr2_power_board
Author(s): Curt Meyers, Blaise Gassend
autogenerated on Tue Apr 22 2014 19:34:59