power_node2.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <stdio.h>
31 #include <stdint.h>
32 #include <string.h>
33 #include <unistd.h>
34 #include <stdlib.h>
35 #include <sys/select.h>
36 #include <sys/types.h>
37 #include <assert.h>
38 #include <errno.h>
39 #include <signal.h>
40 #include <vector>
41 #include <sstream>
42 #include <iostream>
43 #include <boost/thread/thread.hpp>
44 #include <boost/program_options.hpp>
45 
46 // Internet/Socket stuff
47 #include <sys/types.h>
48 #include <sys/socket.h>
49 #include <netinet/in.h>
50 #include <arpa/inet.h>
51 #include <sys/socket.h>
52 #include <net/if.h>
53 #include <sys/ioctl.h>
54 
55 #include <log4cxx/logger.h>
56 
57 #include "power_comm.h"
58 #include "power_node2.h"
59 #include "diagnostic_msgs/DiagnosticArray.h"
61 #include "pr2_msgs/PowerBoardState.h"
63 #include "ros/ros.h"
64 
65 #define TEMP_WARN 60
66 
67 using namespace std;
68 namespace po = boost::program_options;
69 
70 // Keep a pointer to the last message received for
71 // Each board.
72 static Device *devicePtr;
76 
77 static const ros::Duration TIMEOUT = ros::Duration(1,0);
78 
79 
80 void Device::setTransitionMessage(const TransitionMessage &newtmsg)
81 {
82  if (tmsgset)
83  {
84 #define PRINT_IF_CHANGED(val) \
85 for (int counter = 0; counter < 3; counter++) \
86 { \
87  if (tmsg.cb[counter].val##_count != newtmsg.cb[counter].val##_count) \
88  { \
89  ROS_INFO("Power board: CB%i "#val" event count changed to %i.", counter, newtmsg.cb[counter].val##_count);\
90  } \
91 }
92 
93  PRINT_IF_CHANGED(stop);
94  PRINT_IF_CHANGED(estop);
95  PRINT_IF_CHANGED(trip);
96  PRINT_IF_CHANGED(fail_18V);
97  PRINT_IF_CHANGED(disable);
98  PRINT_IF_CHANGED(start);
99  PRINT_IF_CHANGED(pump_fail);
100  PRINT_IF_CHANGED(reset);
101 
102 #undef PRINT_IF_CHANGED
103  }
104  else
105  tmsgset = 1;
106 
107  tmsg = newtmsg;
108 }
109 
110 void Device::setPowerMessage(const PowerMessage &newpmsg)
111 {
112  if (pmsgset)
113  {
114 #define PRINT_IF_CHANGED(val) \
115 if (pmsg.status.val##_status != newpmsg.status.val##_status) \
116 { \
117  ROS_INFO("Power board: status of "#val" changed to %i.", newpmsg.status.val##_status); \
118 }
119 
120  PRINT_IF_CHANGED(CB0);
121  PRINT_IF_CHANGED(CB1);
122  PRINT_IF_CHANGED(CB2);
123  PRINT_IF_CHANGED(estop_button);
124  PRINT_IF_CHANGED(estop);
125 
126 #undef PRINT_IF_CHANGED
127  }
128  else
129  pmsgset = 1;
130 
131  if(newpmsg.header.message_revision == 2) //migrate old messages to new structure
132  {
133  memcpy( &pmsg.header, &newpmsg.header, sizeof(MessageHeader));
134 
135  //Copy the contents of the Rev2 message into the current structure.
136  memcpy( &pmsg.status, &newpmsg.status, sizeof(StatusStruct_Rev2));
137  }
138  else
139  pmsg = newpmsg;
140 
141 }
142 
143 Interface::Interface() : recv_sock(-1), send_sock(-1)
144 {
145 
146 }
147 
148 
149 void Interface::Close() {
150  if (recv_sock != -1) {
151  close(recv_sock);
152  recv_sock = -1;
153  }
154  if (send_sock != -1) {
155  close(send_sock);
156  send_sock = -1;
157  }
158 }
159 
160 
161 int Interface::InitReceive(const std::string &address_str)
162 {
163 
164  recv_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
165  if (recv_sock == -1) {
166  perror("Couldn't create recv socket");
167  return -1;
168  }
169 
170  // Allow reuse of receive port
171  int opt = 1;
172  if (setsockopt(recv_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) {
173  perror("Couldn't set reuse addr on recv socket\n");
174  Close();
175  return -1;
176  }
177 
178 #if 0
179  // Allow broadcast on send socket
180  opt = 1;
181  if (setsockopt(recv_sock, SOL_SOCKET, SO_BROADCAST, &opt, sizeof(opt))) {
182  perror("Setting broadcast option on recv");
183  Close();
184  return -1;
185  }
186 #endif
187 
188  // Bind socket to receive packets on <UDP_STATUS_PORT> from any address/interface
189  struct sockaddr_in sin;
190  memset(&sin, 0, sizeof(sin));
191  sin.sin_family = AF_INET;
192  sin.sin_port = htons(POWER_PORT);
193  sin.sin_addr.s_addr = (INADDR_ANY);
194  //inet_pton( AF_INET, address_str.c_str(), &sin.sin_addr);
195  //inet_pton( AF_INET, "192.168.10.10", &sin.sin_addr);
196  if (bind(recv_sock, (struct sockaddr*)&sin, sizeof(sin))) {
197  perror("Couldn't Bind socket to port");
198  Close();
199  return -1;
200  }
201 
202  return 0;
203 }
204 
205 int Interface::Init( const std::string &address_str)
206 {
207 
208  send_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
209  if (send_sock == -1) {
210  Close();
211  perror("Couldn't create send socket");
212  return -1;
213  }
214 
215 
216  int opt;
217 
218  // Allow reuse of receive port
219  opt = 1;
220  if (setsockopt(send_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) {
221  perror("Error allowing socket reuse");
222  Close();
223  return -1;
224  }
225 
226  struct sockaddr_in sin;
227  memset(&sin, 0, sizeof(sin));
228  sin.sin_family = AF_INET;
229 
230  sin.sin_port = htons(POWER_PORT);
231  //sin.sin_addr.s_addr = inet_addr("192.168.13.19");
232  inet_pton( AF_INET, address_str.c_str(), &sin.sin_addr);
233  if (connect(send_sock, (struct sockaddr*)&sin, sizeof(sin))) {
234  perror("Connect'ing socket failed");
235  Close();
236  return -1;
237  }
238 
239  return 0;
240 }
241 
242 void Interface::AddToReadSet(fd_set &set, int &max_sock) const {
243  FD_SET(recv_sock,&set);
244  if (recv_sock > max_sock)
245  max_sock = recv_sock;
246 }
247 
248 bool Interface::IsReadSet(fd_set set) const {
249  return FD_ISSET(recv_sock,&set);
250 }
251 
252 int PowerBoard::send_command( int circuit_breaker, const std::string &command, unsigned flags)
253 {
254  assert(devicePtr != NULL);
255 
256  // Build command message
257  CommandMessage cmdmsg;
258  memset(&cmdmsg, 0, sizeof(cmdmsg));
259  cmdmsg.header.message_revision = COMMAND_MESSAGE_REVISION;
260  cmdmsg.header.message_id = MESSAGE_ID_COMMAND;
261  cmdmsg.header.serial_num = devicePtr->getPowerMessage().header.serial_num;
262  //cmdmsg.header.serial_num = 0x12345678;
263  strncpy(cmdmsg.header.text, "power command message", sizeof(cmdmsg.header.text));
264 
265  if(command == "fan")
266  {
267  //use circuit_breaker to choose which fan.
268  //use flags to set the duty cycle.
269 
270  if((circuit_breaker > 3) || (circuit_breaker < 0))
271  {
272  fprintf(stderr, "Fan must be between 0 and 3\n" );
273  return -1;
274  }
275 
276  switch(circuit_breaker)
277  {
278  default:
279  case 0:
280  cmdmsg.command.fan0_command = flags;
281  break;
282  case 1:
283  cmdmsg.command.fan1_command = flags;
284  break;
285  case 2:
286  cmdmsg.command.fan2_command = flags;
287  break;
288  case 3:
289  cmdmsg.command.fan3_command = flags;
290  break;
291  }
292 
293  ROS_DEBUG("Set fan %d to %d%% output", circuit_breaker, flags);
294  }
295  else
296  {
297  if ((circuit_breaker < 0) || (circuit_breaker >= pr2_power_board::PowerBoardCommand2::Request::NUMBER_OF_CIRCUITS)) {
298  fprintf(stderr, "Circuit breaker number must be between 0 and %d\n", pr2_power_board::PowerBoardCommand2::Request::NUMBER_OF_CIRCUITS);
299  return -1;
300  }
301 
302  ROS_DEBUG("circuit=%d command=%s flags=%x\n", circuit_breaker, command.c_str(), flags);
303 
304  // Set fan duty based on battery temperature. #4763
305  cmdmsg.command.fan0_command = getFanDuty();
306 
307  // Determine what command to send
308  char command_enum = NONE;
309  if (command == "start") {
310  command_enum = COMMAND_START;
311  }
312  else if (command == "stop") {
313  command_enum = COMMAND_STOP;
314  }
315  else if (command == "reset") {
316  command_enum = COMMAND_RESET;
317  }
318  else if (command == "disable") {
319  command_enum = COMMAND_DISABLE;
320  }
321  else if (command == "none") {
322  command_enum = NONE;
323  }
324  else {
325  ROS_ERROR("invalid command '%s'", command.c_str());
326  return -1;
327  }
328  //" -c : command to send to device : 'start', 'stop', 'reset', 'disable'\n"
329 
330 
331  if (circuit_breaker==0) {
332  cmdmsg.command.CB0_command = command_enum;
333  }
334  else if (circuit_breaker==1) {
335  cmdmsg.command.CB1_command = command_enum;
336  }
337  else if (circuit_breaker==2) {
338  cmdmsg.command.CB2_command = command_enum;
339  }
340  else if (circuit_breaker==-1) {
341  cmdmsg.command.CB0_command = command_enum;
342  cmdmsg.command.CB1_command = command_enum;
343  cmdmsg.command.CB2_command = command_enum;
344  }
345 
346  cmdmsg.command.flags = flags;
347  ROS_DEBUG("Sent command %s(%d), circuit %d", command.c_str(), command_enum, circuit_breaker);
348 
349  }
350 
351 
352 
353  errno = 0;
354  //ROS_INFO("Send on %s", inet_ntoa(SendInterfaces[xx]->ifc_address.sin_addr));
355  int result = send(SendInterface->send_sock, &cmdmsg, sizeof(cmdmsg), 0);
356  if (result == -1) {
357  ROS_ERROR("Error sending");
358  return -1;
359  } else if (result != sizeof(cmdmsg)) {
360  ROS_WARN("Error sending : send only took %d of %lu bytes\n",
361  result, sizeof(cmdmsg));
362  }
363 
364  ROS_DEBUG("Send to Serial=%u, revision=%u", cmdmsg.header.serial_num, cmdmsg.header.message_revision);
365 
366 
367  return 0;
368 }
369 
370 
371 const char* PowerBoard::cb_state_to_str(char state)
372 {
373  //enum CB_State { STATE_NOPOWER, STATE_STANDBY, STATE_PUMPING, STATE_ON, STATE_DISABLED };
374  switch(state) {
375  case STATE_NOPOWER:
376  return "no-power";
377  case STATE_STANDBY:
378  return "Standby";
379  case STATE_PUMPING:
380  return "pumping";
381  case STATE_ON:
382  return "Enabled";
383  case STATE_DISABLED:
384  return "Disabled";
385  }
386  return "???";
387 }
388 
389 const char* PowerBoard::master_state_to_str(char state)
390 {
391  //enum CB_State { STATE_NOPOWER, STATE_STANDBY, STATE_PUMPING, STATE_ON, STATE_DISABLED };
392  switch(state) {
393  case MASTER_NOPOWER:
394  return "no-power";
395  case MASTER_STANDBY:
396  return "stand-by";
397  case MASTER_ON:
398  return "on";
399  case MASTER_OFF:
400  return "off";
401  case MASTER_SHUTDOWN:
402  return "shutdown";
403  }
404  return "???";
405 }
406 
407 
408 // Determine if a record of the device already exists...
409 // If it does use newest message a fill in pointer to old one .
410 // If it does not.. use
411 int PowerBoard::process_message(const PowerMessage *msg, int len)
412 {
413  if ((msg->header.message_revision > CURRENT_MESSAGE_REVISION) || (msg->header.message_revision < MINIMUM_MESSAGE_REVISION)) {
414  ROS_WARN("Got message with incorrect message revision %u\n", msg->header.message_revision);
415  return -1;
416  }
417 
418  if ((msg->header.message_revision == CURRENT_MESSAGE_REVISION) && (len != CURRENT_MESSAGE_SIZE))
419  ROS_ERROR("received message of incorrect size %d for rev=%d\n", len, msg->header.message_revision);
420 
421  if ((msg->header.message_revision == MINIMUM_MESSAGE_REVISION) && (len != REVISION_2_MESSAGE_SIZE))
422  ROS_ERROR("received message of incorrect size %d for rev=%d\n", len, msg->header.message_revision);
423 
424 
425  {
426  boost::mutex::scoped_lock(library_lock_);
427  devicePtr->message_time = ros::Time::now();
428  devicePtr->setPowerMessage(*msg);
429  }
430 
431  return 0;
432 }
433 
434 int PowerBoard::process_transition_message(const TransitionMessage *msg, int len)
435 {
436  if (msg->header.message_revision != TRANSITION_MESSAGE_REVISION) {
437  ROS_WARN("Got message with incorrect message revision %u\n", msg->header.message_revision);
438  return -1;
439  }
440 
441  if (len != sizeof(TransitionMessage)) {
442  ROS_ERROR("received message of incorrect size %d\n", len);
443  return -2;
444  }
445 
446  {
447  boost::mutex::scoped_lock(library_lock_);
448  devicePtr->message_time = ros::Time::now();
449  devicePtr->setTransitionMessage(*msg);
450  }
451 
452  return 0;
453 }
454 
455 // collect status packets for 0.5 seconds. Keep the last packet sent
456 // from each seperate power device.
458 {
459  PowerMessage *power_msg;
460  TransitionMessage *transition_msg;
461  char tmp_buf[256]; //bigger than our max size we expect
462 
463  //ROS_DEBUG("PowerMessage size=%u", sizeof(PowerMessage));
464  //ROS_DEBUG("TransitionMessage size=%u", sizeof(TransitionMessage));
465 
466  timeval timeout; //timeout once a second to check if we should die or not.
467  timeout.tv_sec = 1;
468  timeout.tv_usec = 0;
469 
470  while (1)
471  {
472  // Wait for packets to arrive on socket.
473  fd_set read_set;
474  int max_sock = -1;
475  FD_ZERO(&read_set);
476  //for (unsigned i = 0; i<SendInterfaces.size(); ++i)
477  ReceiveInterface->AddToReadSet(read_set,max_sock);
478 
479  int result = select(max_sock+1, &read_set, NULL, NULL, &timeout);
480 
481  //fprintf(stderr,"*");
482 
483  if (result == -1) {
484  perror("Select");
485  return -1;
486  }
487  else if (result == 0) {
488  return 0;
489  }
490  else if (result >= 1) {
491  Interface *recvInterface = ReceiveInterface;
492 
493  struct sockaddr_in crap;
494  socklen_t sock_len = sizeof(crap);
495 
496  //ROS_INFO("Receive on %s", inet_ntoa(((struct sockaddr_in *)(&recvInterface->interface.ifr_dstaddr))->sin_addr));
497  int len = recvfrom(recvInterface->recv_sock, tmp_buf, sizeof(tmp_buf), 0, (sockaddr*)&crap, &sock_len);
498  if (len == -1) {
499  ROS_ERROR("Error recieving on socket");
500  return -1;
501  }
502  else if (len < (int)sizeof(MessageHeader)) {
503  ROS_ERROR("received message of incorrect size %d\n", len);
504  }
505 
506 #if 0
507  char str[INET_ADDRSTRLEN];
508  inet_ntop(AF_INET, &(crap.sin_addr), str, INET_ADDRSTRLEN);
509  ROS_DEBUG("received from = %s", str);
510 #endif
511 
512  if(crap.sin_addr.s_addr == ip_address )
513  {
514  MessageHeader *header;
515  header = (MessageHeader*)tmp_buf;
516 
517  //ROS_DEBUG("Header type=%d", header->message_id);
518  if(header->message_id == MESSAGE_ID_POWER)
519  {
520  power_msg = (PowerMessage*)tmp_buf;
521  if (len == -1) {
522  ROS_ERROR("Error recieving on socket");
523  return -1;
524  }
525 /*
526  else if (len != (int)sizeof(PowerMessage)) {
527  ROS_ERROR("received message of incorrect size %d\n", len);
528  }
529 */
530  else {
531  if (process_message(power_msg, len))
532  return -1;
533  }
534  }
535  else if(header->message_id == MESSAGE_ID_TRANSITION)
536  {
537  transition_msg = (TransitionMessage *)tmp_buf;
538  if (len == -1) {
539  ROS_ERROR("Error recieving on socket");
540  return -1;
541  }
542  else {
543  if (process_transition_message(transition_msg, len))
544  return -1;
545  }
546  }
547  else
548  {
549  ROS_DEBUG("Discard message len=%d", len);
550  }
551  }
552  }
553  else {
554  ROS_ERROR("Unexpected select result %d\n", result);
555  return -1;
556  }
557  }
558 
559  return 0;
560 }
561 
562 PowerBoard::PowerBoard( const ros::NodeHandle node_handle, const std::string &address_str ) :
563  node_handle(node_handle),
564  fan_high_(false)
565 {
567  log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
568  fprintf(stderr, "Logger Name: %s\n", ROSCONSOLE_DEFAULT_NAME);
569 
570  if( my_logger->getLevel() == 0 ) //has anyone set our level??
571  {
572  // Set the ROS logger
573  my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Info]);
574  }
575 
576  sockaddr_in sin;
577  inet_pton( AF_INET, address_str.c_str(), &sin.sin_addr);
578  ip_address = sin.sin_addr.s_addr;
579 
580 }
581 
582 void PowerBoard::init()
583 {
584  devicePtr = new Device();
585 
588 
589  diags_pub = node_handle.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 2);
590  state_pub = node_handle.advertise<pr2_msgs::PowerBoardState>("state", 2, true);
591 
592  ros::NodeHandle main_handle;
593  battery_sub_ = main_handle.subscribe("battery/server2", 10, &PowerBoard::batteryCB, this);
594 }
595 
596 void PowerBoard::batteryCB(const pr2_msgs::BatteryServer2::ConstPtr &msgPtr)
597 {
598  float max_temp = -1.0f;
599 
600  std::vector<pr2_msgs::BatteryState2>::const_iterator it;
601  for (it = msgPtr->battery.begin(); it != msgPtr->battery.end(); ++it)
602  {
603  // Convert to celcius from 0.1K
604  float batt_temp = ((float) it->battery_register[0x08]) / 10.0f - 273.15;
605 
606  max_temp = max(max_temp, batt_temp);
607  }
608 
609  ROS_DEBUG("Logging battery server %d with temperature %f", msgPtr->id, max_temp);
610  battery_temps_[msgPtr->id] = max_temp;
611 }
612 
614 {
615  // Find max battery temp
616  float max_temp = -1.0f;
617 
618  std::map<int, float>::const_iterator it;
619  for (it = battery_temps_.begin(); it != battery_temps_.end(); ++it)
620  max_temp = max(max_temp, it->second);
621 
622  // Find the appropriate duty cycle based on battery temp
623  // Turn on fan when temp hits 46C, turn off when temp drops to 44C
624  int battDuty = 0;
625  if (max_temp > 46.0f)
626  battDuty = 100;
627  else if (max_temp > 44.0f && fan_high_)
628  battDuty = 100; // Hysteresis in fan controller
629  else // (max_temp < 44.0f) || (max_temp > 44.0f && !fan_high_ && max_temp < 46.0f)
630  battDuty = 0;
631 
632  fan_high_ = battDuty > 0;
633 
634  ROS_DEBUG("Fan duty cycle based on battery temperature: %d, Battery temp: %.1f", battDuty, max_temp);
635  return battDuty;
636 }
637 
639 {
640  send_command(0, "fan", getFanDuty());
641 }
642 
643 bool PowerBoard::commandCallback(pr2_power_board::PowerBoardCommand::Request &req_,
644  pr2_power_board::PowerBoardCommand::Response &res_)
645 {
646  res_.retval = send_command( req_.breaker_number, req_.command, req_.flags);
649 
650  return true;
651 }
652 
653 bool PowerBoard::commandCallback2(pr2_power_board::PowerBoardCommand2::Request &req_,
654  pr2_power_board::PowerBoardCommand2::Response &res_)
655 {
656 
657  unsigned flags = 0;
658  if(req_.reset_stats)
659  flags |= 0x1;
660  if(req_.reset_circuits)
661  flags |= 0x2;
662 
663  res_.success = send_command( req_.circuit, req_.command, flags);
666 
667  return true;
668 }
669 
671 {
672  while(node_handle.ok())
673  {
675  //ROS_DEBUG("*");
676  }
677 }
678 
680 {
681  ros::Rate r(5);
682  r.sleep();
683  r = 1;
684  r.reset();
685 
686  while(node_handle.ok())
687  {
688  r.sleep();
689  //ROS_DEBUG("-");
690  boost::mutex::scoped_lock(library_lock_);
691 
692  {
693  diagnostic_msgs::DiagnosticArray msg_out;
695 
696  const PowerMessage *pmesg = &devicePtr->getPowerMessage();
697 
698  ostringstream ss, ss2;
699  ss << "Power board " << pmesg->header.serial_num;
700  ss2 << "68050070" << pmesg->header.serial_num;
701  stat.name = ss.str();
702  stat.hardware_id = ss2.str();
703 
704  if( (ros::Time::now() - devicePtr->message_time) > TIMEOUT )
705  {
706  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No Updates");
707  }
708  else
709  {
710  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Running");
711  }
712  const StatusStruct *status = &pmesg->status;
713 
714  if (status->fan0_speed == 0)
715  {
716  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Base Fan Off");
717  }
718 
719  //ROS_DEBUG("Device %u", i);
720  //ROS_DEBUG(" Serial = %u", pmesg->header.serial_num);
721 
722  stat.add("Serial Number", pmesg->header.serial_num);
723 
724  //ROS_DEBUG(" Current = %f", status->input_current);
725  stat.add("Input Current", status->input_current);
726 
727  //ROS_DEBUG(" Voltages:");
728  //ROS_DEBUG(" Input = %f", status->input_voltage);
729  stat.add("Input Voltage", status->input_voltage);
730 
731  //ROS_DEBUG(" DCDC 12 aux = %f", status->DCDC_12V_aux);
732  stat.add("DCDC 12V aux", status->DCDC_12V_aux);
733 
734  //ROS_DEBUG(" DCDC 12V cpu0 = %f", status->DCDC_12V_cpu0);
735  stat.add("DCDC 12V c1", status->DCDC_12V_cpu0);
736 
737  //ROS_DEBUG(" CB0 (Base) = %f", status->CB0_voltage);
738  stat.add("Breaker 0 Voltage", status->CB0_voltage);
739 
740  //ROS_DEBUG(" CB1 (R-arm) = %f", status->CB1_voltage);
741  stat.add("Breaker 1 Voltage", status->CB1_voltage);
742 
743  //ROS_DEBUG(" CB2 (L-arm) = %f", status->CB2_voltage);
744  stat.add("Breaker 2 Voltage", status->CB2_voltage);
745 
746  //ROS_DEBUG(" Board Temp = %f", status->ambient_temp);
747  stat.add("Board Temperature", status->ambient_temp);
748 
749  if (status->ambient_temp > TEMP_WARN)
750  {
751  stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "High Temp");
752  }
753 
754  //ROS_DEBUG(" Fan Speeds:");
755  //ROS_DEBUG(" Fan 0 = %u", status->fan0_speed);
756  stat.add("Base Fan Speed", status->fan0_speed);
757 
758  //ROS_DEBUG(" Fan 1 = %u", status->fan1_speed);
759  stat.add("Expansion Fan 1 Speed", status->fan1_speed);
760 
761  //ROS_DEBUG(" Fan 2 = %u", status->fan2_speed);
762  stat.add("Expansion Fan 2 Speed", status->fan2_speed);
763 
764  //ROS_DEBUG(" Fan 3 = %u", status->fan3_speed);
765  stat.add("Expansion Fan 3 Speed", status->fan3_speed);
766 
767  //ROS_DEBUG(" State:");
768  //ROS_DEBUG(" CB0 (Base) = %s", cb_state_to_str(status->CB0_state));
769  stat.add("Breaker 0 (Left Arm) State", cb_state_to_str(status->CB0_state));
770 
771  //ROS_DEBUG(" CB1 (R-arm) = %s", cb_state_to_str(status->CB1_state));
772  stat.add("Breaker 1 (Base/Body) State", cb_state_to_str(status->CB1_state));
773 
774  //ROS_DEBUG(" CB2 (L-arm) = %s", cb_state_to_str(status->CB2_state));
775  stat.add("Breaker 2 (Right Arm) State", cb_state_to_str(status->CB2_state));
776 
777  //ROS_DEBUG(" DCDC = %s", master_state_to_str(status->DCDC_state));
778  stat.add("DCDC state", master_state_to_str(status->DCDC_state));
779 
780 
781  //ROS_DEBUG(" estop_button= %x", (status->estop_button_status));
782  stat.add("RunStop Button Status", (status->estop_button_status ? "True":"False"));
783 
784  //ROS_DEBUG(" estop_status= %x", (status->estop_status));
785  stat.add("RunStop Wireless Status", (status->estop_status ? "True":"False"));
786 
787  //ROS_DEBUG(" Revisions:");
788  //ROS_DEBUG(" PCA = %c", status->pca_rev);
789  stat.add("Circuit assembly revision", status->pca_rev);
790 
791  //ROS_DEBUG(" PCB = %c", status->pcb_rev);
792  stat.add("Circuit board revision", status->pcb_rev);
793 
794  //ROS_DEBUG(" Major = %c", status->major_rev);
795  stat.add("Major Revision", status->major_rev);
796 
797  //ROS_DEBUG(" Minor = %c", status->minor_rev);
798  stat.add("Minor Revision", status->minor_rev);
799 
800  stat.add("Min Voltage", status->min_input_voltage);
801  stat.add("Max Current", status->max_input_current);
802 
803  //ROS_DEBUG(" DCDC 12V cpu1 = %f", status->DCDC_12V_cpu1);
804  stat.add("DCDC 12V c2", status->DCDC_12V_cpu1);
805 
806  //ROS_DEBUG(" DCDC 12V user = %f", status->DCDC_12V_user);
807  stat.add("DCDC 12V user", status->DCDC_12V_user);
808 
809  for( int xx = 0; xx < 4; ++xx)
810  {
811  //ROS_DEBUG(" Battery %d voltage=%f", xx, status->battery_voltage[xx]);
812  ss.str("");
813  ss << "Battery " << xx << " voltage=";
814  stat.add(ss.str(), status->battery_voltage[xx]);
815  }
816 
817 
818  const TransitionMessage *tmsg = &devicePtr->getTransitionMessage();
819  for(int cb_index=0; cb_index < 3; ++cb_index)
820  {
821  const TransitionCount *trans = &tmsg->cb[cb_index];
822  //ROS_DEBUG("Transition: CB%d", cb_index);
823  ss.str("");
824  ss << "CB" << cb_index << " Stop Count";
825  stat.add(ss.str(), (int)trans->stop_count);
827 
828  ss.str("");
829  ss << "CB" << cb_index << " E-Stop Count";
830  stat.add(ss.str(), (int)trans->estop_count);
831 
832  ss.str("");
833  ss << "CB" << cb_index << " Trip Count";
834  stat.add(ss.str(), (int)trans->trip_count);
835 
836  ss.str("");
837  ss << "CB" << cb_index << " 18V Fail Count";
838  stat.add(ss.str(), (int)trans->fail_18V_count);
839 
840  ss.str("");
841  ss << "CB" << cb_index << " Disable Count";
842  stat.add(ss.str(), (int)trans->disable_count);
843 
844  ss.str("");
845  ss << "CB" << cb_index << " Start Count";
846  stat.add(ss.str(), (int)trans->start_count);
847 
848  ss.str("");
849  ss << "CB" << cb_index << " Pump Fail Count";
850  stat.add(ss.str(), (int)trans->pump_fail_count);
851 
852  ss.str("");
853  ss << "CB" << cb_index << " Reset Count";
854  stat.add(ss.str(), (int)trans->reset_count);
855  }
856 
857  msg_out.status.push_back(stat);
858  msg_out.header.stamp = ros::Time::now();
859  //ROS_DEBUG("Publishing ");
860  diags_pub.publish(msg_out);
861 
862  // Only publish a message if we've received data recently. #3877
863  if ((ros::Time::now() - devicePtr->message_time) < TIMEOUT )
864  {
865  pr2_msgs::PowerBoardState state_msg;
866  state_msg.name = stat.name;
867  state_msg.serial_num = pmesg->header.serial_num;
868  state_msg.input_voltage = status->input_voltage;
869  state_msg.circuit_voltage[0] = status->CB0_voltage;
870  state_msg.circuit_voltage[1] = status->CB1_voltage;
871  state_msg.circuit_voltage[2] = status->CB2_voltage;
872  state_msg.master_state = status->DCDC_state;
873  state_msg.circuit_state[0] = status->CB0_state;
874  state_msg.circuit_state[1] = status->CB1_state;
875  state_msg.circuit_state[2] = status->CB2_state;
876  state_msg.run_stop = status->estop_status;
877  state_msg.wireless_stop = status->estop_button_status;
878  state_msg.header.stamp = ros::Time::now();
879  state_pub.publish(state_msg);
880  }
881  }
882  }
883 }
884 
885 int PowerBoard::requestMessage(const unsigned int message)
886 {
887 
888  GetMessage cmdmsg;
889  memset(&cmdmsg, 0, sizeof(cmdmsg));
890  cmdmsg.header.message_revision = STATUS_MESSAGE_REVISION;
891  cmdmsg.header.message_id = MESSAGE_ID_STATUS;
892  cmdmsg.header.serial_num = devicePtr->getPowerMessage().header.serial_num;
893  strncpy(cmdmsg.header.text, "power status message", sizeof(cmdmsg.header.text));
894 
895  cmdmsg.message_to_get = message;
896 
897  errno = 0;
898  int result = send(SendInterface->send_sock, &cmdmsg, sizeof(cmdmsg), 0);
899  if (result == -1) {
900  ROS_ERROR("Error sending");
901  return -1;
902  } else if (result != sizeof(cmdmsg)) {
903  ROS_WARN("Error sending : send only took %d of %lu bytes\n", result, sizeof(cmdmsg));
904  }
905 
906  ROS_DEBUG("requestMessage: to Serial=%u, revision=%u", cmdmsg.header.serial_num, cmdmsg.header.message_revision);
907  return 0;
908 }
909 
910 
912 {
913  myBoard->collectMessages();
914 }
915 
917 {
918  myBoard->sendMessages();
919 }
920 
921 // CloseAll
923 {
924 
925  delete SendInterface;
926  delete ReceiveInterface;
927 }
928 
929 void CloseAllDevices(void)
930 {
931  delete devicePtr;
932 }
933 
934 
935 // Build list of interfaces
936 int CreateAllInterfaces(const std::string &address_str)
937 {
938  //
939  int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
940  if (sock == -1) {
941  ROS_ERROR("Couldn't create socket for ioctl requests");
942  return -1;
943  }
944 
945  SendInterface = new Interface();
946  if (SendInterface->Init(address_str))
947  {
948  ROS_ERROR("Error initializing interface");
949  delete SendInterface;
950  SendInterface = NULL;
951  }
952 
953 
954  ReceiveInterface = new Interface();
955  ReceiveInterface->InitReceive(address_str);
956 
957  return 0;
958 }
959 
960 int main(int argc, char** argv)
961 {
962  ros::init(argc, argv, "power_board");
963 
964  std::string address_str;
965  po::options_description desc("Allowed options");
966  desc.add_options()
967  ("help", "this help message")
968  ("address", po::value<std::string>(&address_str), "IP address for specific Power Board");
969 
970  po::variables_map vm;
971  po::store(po::parse_command_line( argc, argv, desc), vm);
972  po::notify(vm);
973 
974  if( vm.count("help"))
975  {
976  cout << desc << "\n";
977  return 1;
978  }
979 
980  if( vm.count("address") == 0 )
981  {
982  ROS_ERROR("PowerNode: Error you did not specify the IP address, use --address= to specify the address of the power board.");
983  exit(-1);
984  }
985 
986  ROS_INFO("PowerNode:Using Address=%s", address_str.c_str());
987 
988 
989  CreateAllInterfaces(address_str);
990 
991  ros::NodeHandle private_handle("~");
992  myBoard = new PowerBoard(private_handle, address_str );
993  myBoard->init();
994 
995  boost::thread getThread( &getMessages );
996  boost::thread sendThread( &sendMessages );
997 
998  double sample_frequency = 10; //In Hertz
999  double transition_frequency = 0.1; //In Hertz
1000  private_handle.getParam( "sample_frequency", sample_frequency );
1001  ROS_INFO("Using sampling frequency %fHz", sample_frequency);
1002  private_handle.getParam( "transition_frequency", transition_frequency );
1003  ROS_INFO("Using transition frequency %fHz", transition_frequency);
1004 
1005  ros::Time last_msg( 0, 0);
1006  ros::Time last_transition( 0, 0);
1007  ros::Time last_batt_check(0, 0);
1008 
1009  double ros_rate = 10; //(Hz) need to run the "spin" loop at some number of Hertz to handle ros things.
1010 
1011  if(sample_frequency > ros_rate)
1012  ros_rate = sample_frequency;
1013 
1014  ros::Rate r(ros_rate);
1015  const ros::Duration MSG_RATE(1/sample_frequency);
1016  const ros::Duration TRANSITION_RATE(1/transition_frequency);
1017  const ros::Duration BATT_CHECK_RATE(1 / 0.1);
1018 
1019  while(private_handle.ok())
1020  {
1021  r.sleep();
1022  if( (ros::Time::now() - last_msg) > MSG_RATE )
1023  {
1024  myBoard->requestMessage(MESSAGE_ID_POWER);
1025  //ROS_INFO("Send ");
1026  last_msg = ros::Time::now();
1027  }
1028 
1029  if( (ros::Time::now() - last_transition) > TRANSITION_RATE )
1030  {
1032  last_transition = ros::Time::now();
1033  }
1034 
1035  if (ros::Time::now() - last_batt_check > BATT_CHECK_RATE)
1036  {
1037  last_batt_check = ros::Time::now();
1038  myBoard->checkFanSpeed();
1039  }
1040 
1041  ros::spinOnce();
1042  }
1043 
1044  sendThread.join();
1045  getThread.join();
1046 
1047  CloseAllDevices();
1049 
1050 
1051  delete myBoard;
1052  return 0;
1053 
1054 }
1055 
1056 Device::Device(): message_time(0,0)
1057 {
1058  pmsgset = false;
1059  tmsgset = false;
1060  memset( &pmsg, 0, sizeof(PowerMessage));
1061  memset( &tmsg, 0, sizeof(TransitionMessage));
1062 };
#define PRINT_IF_CHANGED(val)
msg
static const unsigned COMMAND_MESSAGE_REVISION
Definition: power_comm.h:39
const char * master_state_to_str(char state)
Definition: power_node.cpp:417
static const unsigned STATUS_MESSAGE_REVISION
Definition: power_comm.h:40
int Init(sockaddr_in *port_address, sockaddr_in *broadcast_address)
Definition: power_node.cpp:202
void CloseAllDevices(void)
f
void summary(unsigned char lvl, const std::string s)
void getMessages()
#define TEMP_WARN
Definition: power_node2.cpp:65
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
static Interface * ReceiveInterface
Definition: power_node2.cpp:74
#define CURRENT_MESSAGE_SIZE
Definition: power_comm.h:34
void init()
Definition: power_node.cpp:647
int send_sock
Definition: power_node.h:15
#define ROSCONSOLE_AUTOINIT
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
int getFanDuty()
int send_command(unsigned int serial_number, int circuit_breaker, const std::string &command, unsigned flags)
Definition: power_node.cpp:285
#define ROS_WARN(...)
bool commandCallback2(pr2_power_board::PowerBoardCommand2::Request &req_, pr2_power_board::PowerBoardCommand2::Response &res_)
pr2_power_board::PowerBoardCommand::Request req_
Definition: power_node.h:77
ros::ServiceServer service
Definition: power_node.h:73
ros::Time message_time
Definition: power_node.h:30
static const unsigned MESSAGE_ID_COMMAND
Definition: power_comm.h:43
int CreateAllInterfaces(const std::string &address_str)
void reset()
void batteryCB(const pr2_msgs::BatteryServer2::ConstPtr &msgPtr)
std::map< int, float > battery_temps_
Definition: power_node2.h:85
void publish(const boost::shared_ptr< M > &message) const
#define REVISION_2_MESSAGE_SIZE
Definition: power_comm.h:36
uint64_t ip_address
Definition: power_node2.h:95
void sendMessages()
Definition: power_node.cpp:680
void setPowerMessage(const PowerMessage &newpmsg)
Definition: power_node.cpp:108
#define ROS_INFO(...)
bool getParam(const std::string &key, std::string &s) const
ros::Publisher diags_pub
Definition: power_node.h:74
static Device * devicePtr
Definition: power_node2.cpp:72
ros::NodeHandle node_handle
Definition: power_node.h:72
void checkFanSpeed()
static const unsigned MESSAGE_ID_POWER
Definition: power_comm.h:42
int InitReceive()
Definition: power_node.cpp:162
static PowerBoard * myBoard
Definition: power_node2.cpp:75
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const TransitionMessage & getTransitionMessage()
Definition: power_node.h:32
bool fan_high_
Definition: power_node2.h:86
static const unsigned MINIMUM_MESSAGE_REVISION
Definition: power_comm.h:35
static const unsigned TRANSITION_MESSAGE_REVISION
Definition: power_comm.h:38
const char * cb_state_to_str(char state)
Definition: power_node.cpp:399
bool sleep()
bool IsReadSet(fd_set set) const
Definition: power_node.cpp:281
void collectMessages()
Definition: power_node.cpp:671
boost::mutex library_lock_
Definition: power_node.h:79
bool commandCallback(pr2_power_board::PowerBoardCommand::Request &req_, pr2_power_board::PowerBoardCommand::Response &res_)
Definition: power_node.cpp:663
int recv_sock
Definition: power_node.h:14
const PowerMessage & getPowerMessage()
Definition: power_node.h:38
void setTransitionMessage(const TransitionMessage &newtmsg)
Definition: power_node.cpp:78
static const unsigned MESSAGE_ID_TRANSITION
Definition: power_comm.h:44
list command
Definition: test_power.py:44
void AddToReadSet(fd_set &set, int &max_sock) const
Definition: power_node.cpp:275
static const unsigned CURRENT_MESSAGE_REVISION
Definition: power_comm.h:33
pr2_power_board::PowerBoardCommand::Response res_
Definition: power_node.h:78
PowerBoard(const ros::NodeHandle node_handle, unsigned int serial_number=0)
Definition: power_node.cpp:632
static Time now()
const std::string header
static const unsigned POWER_PORT
Definition: power_comm.h:46
void add(const std::string &key, const T &val)
void CloseAllInterfaces(void)
int requestMessage(const unsigned int message)
ROSCPP_DECL void spinOnce()
bool ok() const
static Interface * SendInterface
Definition: power_node2.cpp:73
#define ROS_ERROR(...)
#define ROSCONSOLE_DEFAULT_NAME
ros::Publisher state_pub
Definition: power_node.h:75
void Close()
Definition: power_node.cpp:150
ros::ServiceServer service2
Definition: power_node2.h:80
static const unsigned MESSAGE_ID_STATUS
Definition: power_comm.h:45
static const ros::Duration TIMEOUT
Definition: power_node2.cpp:77
ros::Subscriber battery_sub_
Definition: power_node2.h:83
#define ROS_DEBUG(...)
int process_message(const PowerMessage *msg, int len)
Definition: power_node.cpp:439
int process_transition_message(const TransitionMessage *msg, int len)
Definition: power_node.cpp:484
int collect_messages()
Definition: power_node.cpp:527


pr2_power_board
Author(s): Curt Meyers, Blaise Gassend
autogenerated on Wed Mar 8 2023 03:37:51