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


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