power_node.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_node.h"
59 #include "diagnostic_msgs/DiagnosticArray.h"
61 #include "pr2_msgs/PowerBoardState.h"
63 #include "ros/ros.h"
64 
65 using namespace std;
66 namespace po = boost::program_options;
67 
68 // Keep a pointer to the last message received for
69 // Each board.
70 static std::vector<Device*> Devices;
71 static std::vector<Interface*> SendInterfaces;
74 
75 static const ros::Duration TIMEOUT = ros::Duration(1,0);
76 
77 
78 void Device::setTransitionMessage(const TransitionMessage &newtmsg)
79 {
80  if (tmsgset)
81  {
82 #define PRINT_IF_CHANGED(val) \
83 for (int counter = 0; counter < 3; counter++) \
84 { \
85  if (tmsg.cb[counter].val##_count != newtmsg.cb[counter].val##_count) \
86  { \
87  ROS_INFO("Power board: CB%i "#val" event count changed to %i.", counter, newtmsg.cb[counter].val##_count);\
88  } \
89 }
90 
91  PRINT_IF_CHANGED(stop);
92  PRINT_IF_CHANGED(estop);
93  PRINT_IF_CHANGED(trip);
94  PRINT_IF_CHANGED(fail_18V);
95  PRINT_IF_CHANGED(disable);
97  PRINT_IF_CHANGED(pump_fail);
98  PRINT_IF_CHANGED(reset);
99 
100 #undef PRINT_IF_CHANGED
101  }
102  else
103  tmsgset = 1;
104 
105  tmsg = newtmsg;
106 }
107 
108 void Device::setPowerMessage(const PowerMessage &newpmsg)
109 {
110  if (pmsgset)
111  {
112 #define PRINT_IF_CHANGED(val) \
113 if (pmsg.status.val##_status != newpmsg.status.val##_status) \
114 { \
115  ROS_INFO("Power board: status of "#val" changed to %i.", newpmsg.status.val##_status); \
116 }
117 
118  PRINT_IF_CHANGED(CB0);
119  PRINT_IF_CHANGED(CB1);
120  PRINT_IF_CHANGED(CB2);
121  PRINT_IF_CHANGED(estop_button);
122  PRINT_IF_CHANGED(estop);
123 
124 #undef PRINT_IF_CHANGED
125  }
126  else
127  pmsgset = 1;
128 
129  if(newpmsg.header.message_revision == 2) //migrate old messages to new structure
130  {
131  memcpy( &pmsg.header, &newpmsg.header, sizeof(MessageHeader));
132 
133  //Copy the contents of the Rev2 message into the current structure.
134  memcpy( &pmsg.status, &newpmsg.status, sizeof(StatusStruct_Rev2));
135  }
136  else
137  pmsg = newpmsg;
138 
139 }
140 
141 Interface::Interface(const char* ifname) :
142  recv_sock(-1),
143  send_sock(-1) {
144  memset(&interface, 0, sizeof(interface));
145  assert(strlen(ifname) <= sizeof(interface.ifr_name));
146  strncpy(interface.ifr_name, ifname, IFNAMSIZ);
147 }
148 
149 
151  if (recv_sock != -1) {
152  close(recv_sock);
153  recv_sock = -1;
154  }
155  if (send_sock != -1) {
156  close(send_sock);
157  send_sock = -1;
158  }
159 }
160 
161 
163 {
164 
165  recv_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
166  if (recv_sock == -1) {
167  perror("Couldn't create recv socket");
168  return -1;
169  }
170 
171  // Allow reuse of receive port
172  int opt = 1;
173  if (setsockopt(recv_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) {
174  perror("Couldn't set reuse addr on recv socket\n");
175  Close();
176  return -1;
177  }
178 
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 
187  // Bind socket to receive packets on <UDP_STATUS_PORT> from any address/interface
188  struct sockaddr_in sin;
189  memset(&sin, 0, sizeof(sin));
190  sin.sin_family = AF_INET;
191  sin.sin_port = htons(POWER_PORT);
192  sin.sin_addr.s_addr = (INADDR_ANY);
193  if (bind(recv_sock, (struct sockaddr*)&sin, sizeof(sin))) {
194  perror("Couldn't Bind socket to port");
195  Close();
196  return -1;
197  }
198 
199  return 0;
200 }
201 
202 int Interface::Init(sockaddr_in *port_address, sockaddr_in *broadcast_address)
203 {
204 
205  memcpy( &ifc_address, broadcast_address, sizeof(sockaddr_in));
206 #if 0
207  recv_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
208  if (recv_sock == -1) {
209  perror("Couldn't create recv socket");
210  return -1;
211  }
212 #endif
213  send_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
214  if (send_sock == -1) {
215  Close();
216  perror("Couldn't create send socket");
217  return -1;
218  }
219 
220 
221  // Allow reuse of receive port
222  int opt;
223 #if 0
224  opt = 1;
225  if (setsockopt(recv_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) {
226  perror("Couldn't set reuse addr on recv socket\n");
227  Close();
228  return -1;
229  }
230 
231  // Allow broadcast on send socket
232  opt = 1;
233  if (setsockopt(recv_sock, SOL_SOCKET, SO_BROADCAST, &opt, sizeof(opt))) {
234  perror("Setting broadcast option on recv");
235  Close();
236  return -1;
237  }
238 #endif
239  // All recieving packets sent to broadcast address
240  opt = 1;
241  if (setsockopt(send_sock, SOL_SOCKET, SO_BROADCAST, &opt, sizeof(opt))) {
242  perror("Setting broadcast option on send");
243  Close();
244  return -1;
245  }
246 
247  // Bind socket to receive packets on <UDP_STATUS_PORT> from any address/interface
248  struct sockaddr_in sin;
249  memset(&sin, 0, sizeof(sin));
250  sin.sin_family = AF_INET;
251 #if 0
252  sin.sin_port = htons(POWER_PORT);
253  sin.sin_addr.s_addr = (INADDR_ANY);
254  //sin.sin_addr= port_address->sin_addr;
255  if (bind(recv_sock, (struct sockaddr*)&sin, sizeof(sin))) {
256  perror("Couldn't Bind socket to port");
257  Close();
258  return -1;
259  }
260 #endif
261 
262  // Connect send socket to use broadcast address and same port as receive sock
263  sin.sin_port = htons(POWER_PORT);
264  //sin.sin_addr.s_addr = INADDR_BROADCAST; //inet_addr("192.168.10.255");
265  sin.sin_addr= broadcast_address->sin_addr;
266  if (connect(send_sock, (struct sockaddr*)&sin, sizeof(sin))) {
267  perror("Connect'ing socket failed");
268  Close();
269  return -1;
270  }
271 
272  return 0;
273 }
274 
275 void Interface::AddToReadSet(fd_set &set, int &max_sock) const {
276  FD_SET(recv_sock,&set);
277  if (recv_sock > max_sock)
278  max_sock = recv_sock;
279 }
280 
281 bool Interface::IsReadSet(fd_set set) const {
282  return FD_ISSET(recv_sock,&set);
283 }
284 
285 int PowerBoard::send_command(unsigned int serial_number, int circuit_breaker, const std::string &command, unsigned flags)
286 {
287  if (Devices.size() == 0) {
288  fprintf(stderr,"No devices to send command to\n");
289  return -1;
290  }
291 
292  int selected_device = -1;
293 
294  if(serial_number == 0) {
295  if(Devices.size() > 1) {
296  fprintf(stderr,"Too many devices to send command to using serial_number=0\n");
297  return -1;
298  }
299  selected_device = 0;
300  } else {
301  // Look for device serial number in list of devices...
302  for (unsigned i = 0; i<Devices.size(); ++i) {
303  if (Devices[i]->getPowerMessage().header.serial_num == serial_number) {
304  selected_device = i;
305  break;
306  }
307  }
308  }
309 
310  if ((selected_device < 0) || (selected_device >= (int)Devices.size())) {
311  fprintf(stderr, "Device number must be between 0 and %u\n", (int)Devices.size()-1);
312  return -1;
313  }
314 
315  Device* device = Devices[selected_device];
316  assert(device != NULL);
317 
318  if ((circuit_breaker < 0) || (circuit_breaker > 2)) {
319  fprintf(stderr, "Circuit breaker number must be between 0 and 2\n");
320  return -1;
321  }
322 
323  ROS_DEBUG("circuit=%d command=%s flags=%x\n", circuit_breaker, command.c_str(), flags);
324 
325  // Determine what command to send
326  char command_enum = NONE;
327  if (command == "start") {
328  command_enum = COMMAND_START;
329  }
330  else if (command == "stop") {
331  command_enum = COMMAND_STOP;
332  }
333  else if (command == "reset") {
334  command_enum = COMMAND_RESET;
335  }
336  else if (command == "disable") {
337  command_enum = COMMAND_DISABLE;
338  }
339  else if (command == "none") {
340  command_enum = NONE;
341  }
342  else {
343  ROS_ERROR("invalid command '%s'", command.c_str());
344  return -1;
345  }
346  //" -c : command to send to device : 'start', 'stop', 'reset', 'disable'\n"
347 
348 
349  // Build command message
350  CommandMessage cmdmsg;
351  memset(&cmdmsg, 0, sizeof(cmdmsg));
352  cmdmsg.header.message_revision = COMMAND_MESSAGE_REVISION;
353  cmdmsg.header.message_id = MESSAGE_ID_COMMAND;
354  cmdmsg.header.serial_num = device->getPowerMessage().header.serial_num;
355  //cmdmsg.header.serial_num = 0x12345678;
356  strncpy(cmdmsg.header.text, "power command message", sizeof(cmdmsg.header.text));
357  cmdmsg.command.CB0_command = NONE;
358  cmdmsg.command.CB1_command = NONE;
359  cmdmsg.command.CB2_command = NONE;
360  if (circuit_breaker==0) {
361  cmdmsg.command.CB0_command = command_enum;
362  }
363  else if (circuit_breaker==1) {
364  cmdmsg.command.CB1_command = command_enum;
365  }
366  else if (circuit_breaker==2) {
367  cmdmsg.command.CB2_command = command_enum;
368  }
369  else if (circuit_breaker==-1) {
370  cmdmsg.command.CB0_command = command_enum;
371  cmdmsg.command.CB1_command = command_enum;
372  cmdmsg.command.CB2_command = command_enum;
373  }
374 
375  cmdmsg.command.flags = flags;
376 
377  errno = 0;
378  for (unsigned xx = 0; xx < SendInterfaces.size(); ++xx)
379  {
380  //ROS_INFO("Send on %s", inet_ntoa(SendInterfaces[xx]->ifc_address.sin_addr));
381  int result = send(SendInterfaces[xx]->send_sock, &cmdmsg, sizeof(cmdmsg), 0);
382  if (result == -1) {
383  ROS_ERROR("Error sending");
384  return -1;
385  } else if (result != sizeof(cmdmsg)) {
386  ROS_WARN("Error sending : send only took %d of %d bytes\n",
387  result, (int)sizeof(cmdmsg));
388  }
389  }
390 
391  ROS_DEBUG("Send to Serial=%u, revision=%u", cmdmsg.header.serial_num, cmdmsg.header.message_revision);
392  ROS_DEBUG("Sent command %s(%d) to device %d, circuit %d",
393  command.c_str(), command_enum, selected_device, circuit_breaker);
394 
395  return 0;
396 }
397 
398 
400 {
401  //enum CB_State { STATE_NOPOWER, STATE_STANDBY, STATE_PUMPING, STATE_ON, STATE_DISABLED };
402  switch(state) {
403  case STATE_NOPOWER:
404  return "no-power";
405  case STATE_STANDBY:
406  return "Standby";
407  case STATE_PUMPING:
408  return "pumping";
409  case STATE_ON:
410  return "On";
411  case STATE_DISABLED:
412  return "Disabled";
413  }
414  return "???";
415 }
416 
418 {
419  //enum CB_State { STATE_NOPOWER, STATE_STANDBY, STATE_PUMPING, STATE_ON, STATE_DISABLED };
420  switch(state) {
421  case MASTER_NOPOWER:
422  return "no-power";
423  case MASTER_STANDBY:
424  return "stand-by";
425  case MASTER_ON:
426  return "on";
427  case MASTER_OFF:
428  return "off";
429  case MASTER_SHUTDOWN:
430  return "shutdown";
431  }
432  return "???";
433 }
434 
435 
436 // Determine if a record of the device already exists...
437 // If it does use newest message a fill in pointer to old one .
438 // If it does not.. use
439 int PowerBoard::process_message(const PowerMessage *msg, int len)
440 {
441  if ((msg->header.message_revision > CURRENT_MESSAGE_REVISION) || (msg->header.message_revision < MINIMUM_MESSAGE_REVISION)) {
442  ROS_WARN("Got message with incorrect message revision %u\n", msg->header.message_revision);
443  return -1;
444  }
445 
446  if ((msg->header.message_revision == CURRENT_MESSAGE_REVISION) && (len != CURRENT_MESSAGE_SIZE))
447  ROS_ERROR("received message of incorrect size %d for rev=%d\n", len, msg->header.message_revision);
448 
449  if ((msg->header.message_revision == MINIMUM_MESSAGE_REVISION) && (len != REVISION_2_MESSAGE_SIZE))
450  ROS_ERROR("received message of incorrect size %d for rev=%d\n", len, msg->header.message_revision);
451 
452 
453  // Look for device serial number in list of devices...
454  if(serial_number != 0) // when a specific serial is called out, ignore everything else
455  {
456  if(serial_number == msg->header.serial_num) //this should be our number
457  {
458  boost::mutex::scoped_lock(library_lock_);
459  Devices[0]->message_time = ros::Time::now();
460  Devices[0]->setPowerMessage(*msg);
461  }
462  }
463  else
464  {
465  for (unsigned i = 0; i<Devices.size(); ++i) {
466  if (Devices[i]->getPowerMessage().header.serial_num == msg->header.serial_num) {
467  boost::mutex::scoped_lock(library_lock_);
468  Devices[i]->message_time = ros::Time::now();
469  Devices[i]->setPowerMessage(*msg);
470  return 0;
471  }
472  }
473 
474  // Add new device to list
475  Device *newDevice = new Device();
476  Devices.push_back(newDevice);
477  newDevice->message_time = ros::Time::now();
478  newDevice->setPowerMessage(*msg);
479  }
480 
481  return 0;
482 }
483 
484 int PowerBoard::process_transition_message(const TransitionMessage *msg, int len)
485 {
486  if (msg->header.message_revision != TRANSITION_MESSAGE_REVISION) {
487  ROS_WARN("Got message with incorrect message revision %u\n", msg->header.message_revision);
488  return -1;
489  }
490 
491  if (len != sizeof(TransitionMessage)) {
492  ROS_ERROR("received message of incorrect size %d\n", len);
493  return -2;
494  }
495 
496  // Look for device serial number in list of devices...
497  if(serial_number != 0) // when a specific serial is called out, ignore everything else
498  {
499  if(serial_number == msg->header.serial_num) //this should be our number
500  {
501  boost::mutex::scoped_lock(library_lock_);
502  Devices[0]->message_time = ros::Time::now();
503  Devices[0]->setTransitionMessage(*msg);
504  }
505  }
506  else
507  {
508  for (unsigned i = 0; i<Devices.size(); ++i) {
509  if (Devices[i]->getPowerMessage().header.serial_num == msg->header.serial_num) {
510  boost::mutex::scoped_lock(library_lock_);
511  Devices[i]->setTransitionMessage(*msg);
512  return 0;
513  }
514  }
515 
516  // Add new device to list
517  Device *newDevice = new Device();
518  Devices.push_back(newDevice);
519  newDevice->setTransitionMessage(*msg);
520  }
521 
522  return 0;
523 }
524 
525 // collect status packets for 0.5 seconds. Keep the last packet sent
526 // from each seperate power device.
528 {
529  PowerMessage *power_msg;
530  TransitionMessage *transition_msg;
531  MessageHeader *header;
532  char tmp_buf[256]; //bigger than our max size we expect
533 
534  //ROS_DEBUG("PowerMessage size=%u", sizeof(PowerMessage));
535  //ROS_DEBUG("TransitionMessage size=%u", sizeof(TransitionMessage));
536 
537  timeval timeout; //timeout once a second to check if we should die or not.
538  timeout.tv_sec = 1;
539  timeout.tv_usec = 0;
540 
541  while (1)
542  {
543  // Wait for packets to arrive on socket.
544  fd_set read_set;
545  int max_sock = -1;
546  FD_ZERO(&read_set);
547  //for (unsigned i = 0; i<SendInterfaces.size(); ++i)
548  ReceiveInterface->AddToReadSet(read_set,max_sock);
549 
550  int result = select(max_sock+1, &read_set, NULL, NULL, &timeout);
551 
552  //fprintf(stderr,"*");
553 
554  if (result == -1) {
555  perror("Select");
556  return -1;
557  }
558  else if (result == 0) {
559  return 0;
560  }
561  else if (result >= 1) {
562  Interface *recvInterface = ReceiveInterface;
563 #if 0
564  for (unsigned i = 0; i<SendInterfaces.size(); ++i) {
565  //figure out which interface we received on
566  if (SendInterfaces[i]->IsReadSet(read_set)) {
567  recvInterface = SendInterfaces[i];
568  //ROS_INFO("Receive index=%d", i);
569  break;
570  }
571  }
572 #endif
573 
574  //ROS_INFO("Receive on %s", inet_ntoa(((struct sockaddr_in *)(&recvInterface->interface.ifr_dstaddr))->sin_addr));
575  int len = recv(recvInterface->recv_sock, tmp_buf, sizeof(tmp_buf), 0);
576  if (len == -1) {
577  ROS_ERROR("Error recieving on socket");
578  return -1;
579  }
580  else if (len < (int)sizeof(MessageHeader)) {
581  ROS_ERROR("received message of incorrect size %d\n", len);
582  }
583 
584  header = (MessageHeader*)tmp_buf;
585  {
586 
587  //ROS_DEBUG("Header type=%d", header->message_id);
588  if(header->message_id == MESSAGE_ID_POWER)
589  {
590  power_msg = (PowerMessage*)tmp_buf;
591  if (len == -1) {
592  ROS_ERROR("Error recieving on socket");
593  return -1;
594  }
595 /*
596  else if (len != (int)sizeof(PowerMessage)) {
597  ROS_ERROR("received message of incorrect size %d\n", len);
598  }
599 */
600  else {
601  if (process_message(power_msg, len))
602  return -1;
603  }
604  }
605  else if(header->message_id == MESSAGE_ID_TRANSITION)
606  {
607  transition_msg = (TransitionMessage *)tmp_buf;
608  if (len == -1) {
609  ROS_ERROR("Error recieving on socket");
610  return -1;
611  }
612  else {
613  if (process_transition_message(transition_msg, len))
614  return -1;
615  }
616  }
617  else
618  {
619  ROS_DEBUG("Discard message len=%d", len);
620  }
621  }
622  }
623  else {
624  ROS_ERROR("Unexpected select result %d\n", result);
625  return -1;
626  }
627  }
628 
629  return 0;
630 }
631 
632 PowerBoard::PowerBoard( const ros::NodeHandle node_handle, unsigned int serial_number ) : node_handle(node_handle)
633 {
635  log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
636  fprintf(stderr, "Logger Name: %s\n", ROSCONSOLE_DEFAULT_NAME);
637 
638  if( my_logger->getLevel() == 0 ) //has anyone set our level??
639  {
640  // Set the ROS logger
641  my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Info]);
642  }
643 
644  this->serial_number = serial_number;
645 }
646 
648 {
649  if(serial_number != 0)
650  {
651  ROS_INFO("PowerBoard: created with serial number = %d", serial_number);
652  Device *newDevice = new Device();
653  Devices.push_back(newDevice);
654  }
655 
657 
658  diags_pub = node_handle.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 2);
659  state_pub = node_handle.advertise<pr2_msgs::PowerBoardState>("state", 2, true);
660 }
661 
662 
663 bool PowerBoard::commandCallback(pr2_power_board::PowerBoardCommand::Request &req_,
664  pr2_power_board::PowerBoardCommand::Response &res_)
665 {
666  res_.retval = send_command( req_.serial_number, req_.breaker_number, req_.command, req_.flags);
667 
668  return true;
669 }
670 
672 {
673  while(node_handle.ok())
674  {
676  //ROS_DEBUG("*");
677  }
678 }
679 
681 {
682  ros::Rate r(1);
683  while(node_handle.ok())
684  {
685  r.sleep();
686  //ROS_DEBUG("-");
687  boost::mutex::scoped_lock(library_lock_);
688 
689  for (unsigned i = 0; i<Devices.size(); ++i)
690  {
691  diagnostic_msgs::DiagnosticArray msg_out;
693 
694  Device *device = Devices[i];
695  const PowerMessage *pmesg = &device->getPowerMessage();
696 
697  ostringstream ss, ss2;
698  ss << "Power board " << pmesg->header.serial_num;
699  ss2 << "68050070" << pmesg->header.serial_num;
700  stat.name = ss.str();
701  stat.hardware_id = ss2.str();
702 
703  if( (ros::Time::now() - device->message_time) > TIMEOUT )
704  {
705  stat.summary(2, "No Updates");
706  }
707  else
708  {
709  stat.summary(0, "Running");
710  }
711  const StatusStruct *status = &pmesg->status;
712 
713  ROS_DEBUG("Device %u", i);
714  ROS_DEBUG(" Serial = %u", pmesg->header.serial_num);
715 
716  stat.add("Serial Number", pmesg->header.serial_num);
717 
718  ROS_DEBUG(" Current = %f", status->input_current);
719  stat.add("Input Current", status->input_current);
720 
721  ROS_DEBUG(" Voltages:");
722  ROS_DEBUG(" Input = %f", status->input_voltage);
723  stat.add("Input Voltage", status->input_voltage);
724 
725  ROS_DEBUG(" DCDC 12 aux = %f", status->DCDC_12V_aux);
726  stat.add("DCDC 12V aux", status->DCDC_12V_aux);
727 
728  ROS_DEBUG(" DCDC 12V cpu0 = %f", status->DCDC_12V_cpu0);
729  stat.add("DCDC 12V cpu0", status->DCDC_12V_cpu0);
730 
731  ROS_DEBUG(" CB0 (Base) = %f", status->CB0_voltage);
732  stat.add("Breaker 0 Voltage", status->CB0_voltage);
733 
734  ROS_DEBUG(" CB1 (R-arm) = %f", status->CB1_voltage);
735  stat.add("Breaker 1 Voltage", status->CB1_voltage);
736 
737  ROS_DEBUG(" CB2 (L-arm) = %f", status->CB2_voltage);
738  stat.add("Breaker 2 Voltage", status->CB2_voltage);
739 
740  ROS_DEBUG(" Board Temp = %f", status->ambient_temp);
741  stat.add("Board Temperature", status->ambient_temp);
742 
743  ROS_DEBUG(" Fan Speeds:");
744  ROS_DEBUG(" Fan 0 = %u", status->fan0_speed);
745  stat.add("Fan 0 Speed", status->fan0_speed);
746 
747  ROS_DEBUG(" Fan 1 = %u", status->fan1_speed);
748  stat.add("Fan 1 Speed", status->fan1_speed);
749 
750  ROS_DEBUG(" Fan 2 = %u", status->fan2_speed);
751  stat.add("Fan 2 Speed", status->fan2_speed);
752 
753  ROS_DEBUG(" Fan 3 = %u", status->fan3_speed);
754  stat.add("Fan 3 Speed", status->fan3_speed);
755 
756  ROS_DEBUG(" State:");
757  ROS_DEBUG(" CB0 (Base) = %s", cb_state_to_str(status->CB0_state));
758  stat.add("Breaker 0 State", cb_state_to_str(status->CB0_state));
759 
760  ROS_DEBUG(" CB1 (R-arm) = %s", cb_state_to_str(status->CB1_state));
761  stat.add("Breaker 1 State", cb_state_to_str(status->CB1_state));
762 
763  ROS_DEBUG(" CB2 (L-arm) = %s", cb_state_to_str(status->CB2_state));
764  stat.add("Breaker 2 State", cb_state_to_str(status->CB2_state));
765 
766  ROS_DEBUG(" DCDC = %s", master_state_to_str(status->DCDC_state));
767  stat.add("DCDC state", master_state_to_str(status->DCDC_state));
768 
769  ROS_DEBUG(" Status:");
770  ROS_DEBUG(" CB0 (Base) = %s", (status->CB0_status) ? "On" : "Off");
771  stat.add("Breaker 0 Status", (status->CB0_status) ? "On" : "Off");
772 
773  ROS_DEBUG(" CB1 (R-arm) = %s", (status->CB1_status) ? "On" : "Off");
774  stat.add("Breaker 1 Status", (status->CB1_status) ? "On" : "Off");
775 
776  ROS_DEBUG(" CB2 (L-arm) = %s", (status->CB2_status) ? "On" : "Off");
777  stat.add("Breaker 2 Status", (status->CB2_status) ? "On" : "Off");
778 
779  ROS_DEBUG(" estop_button= %x", (status->estop_button_status));
780  stat.add("RunStop Button Status", (status->estop_button_status ? "True":"False"));
781 
782  ROS_DEBUG(" estop_status= %x", (status->estop_status));
783  stat.add("RunStop Status", (status->estop_status ? "True":"False"));
784 
785  ROS_DEBUG(" Revisions:");
786  ROS_DEBUG(" PCA = %c", status->pca_rev);
787  stat.add("PCA", status->pca_rev);
788 
789  ROS_DEBUG(" PCB = %c", status->pcb_rev);
790  stat.add("PCB", status->pcb_rev);
791 
792  ROS_DEBUG(" Major = %c", status->major_rev);
793  stat.add("Major Revision", status->major_rev);
794 
795  ROS_DEBUG(" Minor = %c", status->minor_rev);
796  stat.add("Minor Revision", status->minor_rev);
797 
798  stat.add("Min Voltage", status->min_input_voltage);
799  stat.add("Max Current", status->max_input_current);
800 
801  ROS_DEBUG(" DCDC 12V cpu1 = %f", status->DCDC_12V_cpu1);
802  stat.add("DCDC 12V cpu1", status->DCDC_12V_cpu1);
803 
804  ROS_DEBUG(" DCDC 12V user = %f", status->DCDC_12V_user);
805  stat.add("DCDC 12V user", status->DCDC_12V_user);
806 
807  for( int xx = 0; xx < 4; ++xx)
808  {
809  ROS_DEBUG(" Battery %d voltage=%f", xx, status->battery_voltage[xx]);
810  ss.str("");
811  ss << "Battery " << xx << " voltage=";
812  stat.add(ss.str(), status->battery_voltage[xx]);
813  }
814 
815 
816  const TransitionMessage *tmsg = &device->getTransitionMessage();
817  for(int cb_index=0; cb_index < 3; ++cb_index)
818  {
819  const TransitionCount *trans = &tmsg->cb[cb_index];
820  ROS_DEBUG("Transition: CB%d", cb_index);
821  ss.str("");
822  ss << "CB" << cb_index << " Stop Count";
823  stat.add(ss.str(), (int)trans->stop_count);
824  //ROS_DEBUG(" Stop Count=%d", trans->stop_count);
825 
826  ss.str("");
827  ss << "CB" << cb_index << " E-Stop Count";
828  stat.add(ss.str(), (int)trans->estop_count);
829 
830  ss.str("");
831  ss << "CB" << cb_index << " Trip Count";
832  stat.add(ss.str(), (int)trans->trip_count);
833 
834  ss.str("");
835  ss << "CB" << cb_index << " 18V Fail Count";
836  stat.add(ss.str(), (int)trans->fail_18V_count);
837 
838  ss.str("");
839  ss << "CB" << cb_index << " Disable Count";
840  stat.add(ss.str(), (int)trans->disable_count);
841 
842  ss.str("");
843  ss << "CB" << cb_index << " Start Count";
844  stat.add(ss.str(), (int)trans->start_count);
845 
846  ss.str("");
847  ss << "CB" << cb_index << " Pump Fail Count";
848  stat.add(ss.str(), (int)trans->pump_fail_count);
849 
850  ss.str("");
851  ss << "CB" << cb_index << " Reset Count";
852  stat.add(ss.str(), (int)trans->reset_count);
853  }
854 
855  msg_out.status.push_back(stat);
856  msg_out.header.stamp = ros::Time::now();
857 
858  //ROS_DEBUG("Publishing ");
859  diags_pub.publish(msg_out);
860 
861  pr2_msgs::PowerBoardState state_msg;
862  state_msg.name = stat.name;
863  state_msg.serial_num = pmesg->header.serial_num;
864  state_msg.input_voltage = status->input_voltage;
865  state_msg.circuit_voltage[0] = status->CB0_voltage;
866  state_msg.circuit_voltage[1] = status->CB1_voltage;
867  state_msg.circuit_voltage[2] = status->CB2_voltage;
868  state_msg.master_state = status->DCDC_state;
869  state_msg.circuit_state[0] = status->CB0_state;
870  state_msg.circuit_state[1] = status->CB1_state;
871  state_msg.circuit_state[2] = status->CB2_state;
872  state_msg.run_stop = status->estop_status;
873  state_msg.wireless_stop = status->estop_button_status;
874  state_msg.header.stamp = ros::Time::now();
875  state_pub.publish(state_msg);
876  }
877  }
878 }
879 
881 {
883 }
884 
886 {
888 }
889 
890 // CloseAll
891 void CloseAllInterfaces(void) {
892  for (unsigned i=0; i<SendInterfaces.size(); ++i){
893  if (SendInterfaces[i] != NULL) {
894  delete SendInterfaces[i];
895  }
896  }
897 
898  delete ReceiveInterface;
899 }
900 void CloseAllDevices(void) {
901  for (unsigned i=0; i<Devices.size(); ++i){
902  if (Devices[i] != NULL) {
903  delete Devices[i];
904  }
905  }
906 }
907 
909 {
910  Interface *newInterface = new Interface("rcv");
911  newInterface->InitReceive();
912  ReceiveInterface = newInterface;
913 }
914 
915 
916 // Build list of interfaces
918 {
919  //
920  int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
921  if (sock == -1) {
922  ROS_ERROR("Couldn't create socket for ioctl requests");
923  return -1;
924  }
925 
926  struct ifconf get_io;
927  get_io.ifc_req = new ifreq[10];
928  get_io.ifc_len = sizeof(ifreq) * 10;
929 
930  if(ioctl( sock, SIOCGIFCONF, &get_io ) == 0)
931  {
932  int num_interfaces = get_io.ifc_len / sizeof(ifreq);
933  ROS_DEBUG("Got %d interfaces", num_interfaces);
934  for(int yy=0; yy < num_interfaces; ++yy)
935  {
936  ROS_DEBUG("interface=%s", get_io.ifc_req[yy].ifr_name);
937  if(get_io.ifc_req[yy].ifr_addr.sa_family == AF_INET)
938  {
939  //ROS_DEBUG("ioctl: family=%d", get_io.ifc_req[yy].ifr_addr.sa_family);
940  sockaddr_in *addr = (sockaddr_in*)&get_io.ifc_req[yy].ifr_addr;
941  ROS_DEBUG("address=%s", inet_ntoa(addr->sin_addr) );
942 
943  if ((strncmp("lo", get_io.ifc_req[yy].ifr_name, strlen(get_io.ifc_req[yy].ifr_name)) == 0) ||
944  (strncmp("tun", get_io.ifc_req[yy].ifr_name, 3) == 0) ||
945  (strncmp("vmnet", get_io.ifc_req[yy].ifr_name, 5) == 0))
946  {
947  ROS_INFO("Ignoring interface %*s", (int)strlen(get_io.ifc_req[yy].ifr_name), get_io.ifc_req[yy].ifr_name);
948  continue;
949  }
950  else
951  {
952  ROS_INFO("Found interface %*s", (int)strlen(get_io.ifc_req[yy].ifr_name), get_io.ifc_req[yy].ifr_name);
953  Interface *newInterface = new Interface(get_io.ifc_req[yy].ifr_name);
954  assert(newInterface != NULL);
955  if (newInterface == NULL)
956  {
957  continue;
958  }
959 
960 
961  struct ifreq *ifr = &get_io.ifc_req[yy];
962  if(ioctl(sock, SIOCGIFBRDADDR, ifr) == 0)
963  {
964  if (ifr->ifr_broadaddr.sa_family == AF_INET)
965  {
966  struct sockaddr_in *br_addr = (struct sockaddr_in *) &ifr->ifr_dstaddr;
967 
968  ROS_DEBUG ("Broadcast addess %s", inet_ntoa(br_addr->sin_addr));
969 
970  if (newInterface->Init(addr, br_addr))
971  {
972  ROS_ERROR("Error initializing interface %*s", (int)sizeof(get_io.ifc_req[yy].ifr_name), get_io.ifc_req[yy].ifr_name);
973  delete newInterface;
974  newInterface = NULL;
975  continue;
976  }
977  else
978  {
979  // Interface is good add it to interface list
980  SendInterfaces.push_back(newInterface);
981 
982  }
983 
984  }
985 
986  }
987 
988 
989  }
990  }
991  }
992  }
993  else
994  {
995  ROS_ERROR("Bad ioctl status");
996  }
997 
998  delete[] get_io.ifc_req;
999 
1000  setupReceive();
1001  //ROS_INFO("Found %d usable interfaces", Interfaces.size());
1002 
1003  return 0;
1004 }
1005 
1006 int main(int argc, char** argv)
1007 {
1008  ros::init(argc, argv, "power_board");
1009 
1010  unsigned int serial_option;
1011  po::options_description desc("Allowed options");
1012  desc.add_options()
1013  ("help", "this help message")
1014  ("serial", po::value<unsigned int>(&serial_option)->default_value(0), "filter a specific serial number");
1015 
1016  po::variables_map vm;
1017  po::store(po::parse_command_line( argc, argv, desc), vm);
1018  po::notify(vm);
1019 
1020  if( vm.count("help"))
1021  {
1022  cout << desc << "\n";
1023  return 1;
1024  }
1025 
1027 
1028  ros::NodeHandle handle("~");
1029  myBoard = new PowerBoard( handle, serial_option);
1030  myBoard->init();
1031 
1032  boost::thread getThread( &getMessages );
1033  boost::thread sendThread( &sendMessages );
1034 
1035  ros::spin(); //wait for ros to shut us down
1036 
1037  sendThread.join();
1038  getThread.join();
1039 
1040  CloseAllDevices();
1042 
1043 
1044  delete myBoard;
1045  return 0;
1046 
1047 }
1048 
1049 Device::Device(): message_time(0,0)
1050 {
1051  pmsgset = false;
1052  tmsgset = false;
1053  memset( &pmsg, 0, sizeof(PowerMessage));
1054  memset( &tmsg, 0, sizeof(TransitionMessage));
1055 };
Device::pmsg
PowerMessage pmsg
Definition: power_node.h:50
test_power.flags
int flags
Definition: test_power.py:36
Interface::recv_sock
int recv_sock
Definition: power_node.h:14
msg
msg
Interface::InitReceive
int InitReceive()
Definition: power_node.cpp:162
PowerBoard::node_handle
ros::NodeHandle node_handle
Definition: power_node.h:72
ReceiveInterface
static Interface * ReceiveInterface
Definition: power_node.cpp:73
diagnostic_updater::DiagnosticStatusWrapper::add
void add(const std::string &key, const bool &b)
PowerBoard::cb_state_to_str
const char * cb_state_to_str(char state)
Definition: power_node.cpp:399
STATE_PUMPING
@ STATE_PUMPING
Definition: power_comm.h:49
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
test_power.state
state
Definition: test_power.py:28
SendInterfaces
static std::vector< Interface * > SendInterfaces
Definition: power_node.cpp:71
Device::message_time
ros::Time message_time
Definition: power_node.h:30
ros.h
PowerBoard::commandCallback
bool commandCallback(pr2_power_board::PowerBoardCommand::Request &req_, pr2_power_board::PowerBoardCommand::Response &res_)
Definition: power_node.cpp:663
COMMAND_START
@ COMMAND_START
Definition: power_comm.h:50
CURRENT_MESSAGE_REVISION
static const unsigned CURRENT_MESSAGE_REVISION
Definition: power_comm.h:33
CloseAllInterfaces
void CloseAllInterfaces(void)
Definition: power_node.cpp:891
MESSAGE_ID_TRANSITION
static const unsigned MESSAGE_ID_TRANSITION
Definition: power_comm.h:44
PRINT_IF_CHANGED
#define PRINT_IF_CHANGED(val)
PowerBoard::collectMessages
void collectMessages()
Definition: power_node.cpp:671
Interface::Interface
Interface()
Definition: power_node2.cpp:143
POWER_PORT
static const unsigned POWER_PORT
Definition: power_comm.h:46
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
PowerBoard::library_lock_
boost::mutex library_lock_
Definition: power_node.h:79
PowerBoard::res_
pr2_power_board::PowerBoardCommand::Response res_
Definition: power_node.h:78
PowerBoard::PowerBoard
PowerBoard(const ros::NodeHandle node_handle, unsigned int serial_number=0)
Definition: power_node.cpp:632
Device
Definition: power_node.h:27
MASTER_NOPOWER
@ MASTER_NOPOWER
Definition: power_comm.h:48
Device::pmsgset
bool pmsgset
Definition: power_node.h:49
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
test_power.command
list command
Definition: test_power.py:44
Device::tmsg
TransitionMessage tmsg
Definition: power_node.h:48
ROSCONSOLE_AUTOINIT
#define ROSCONSOLE_AUTOINIT
STATE_STANDBY
@ STATE_STANDBY
Definition: power_comm.h:49
PowerBoard::state_pub
ros::Publisher state_pub
Definition: power_node.h:75
COMMAND_RESET
@ COMMAND_RESET
Definition: power_comm.h:50
COMMAND_DISABLE
@ COMMAND_DISABLE
Definition: power_comm.h:50
PowerBoard
Definition: power_node.h:54
Interface::Init
int Init(sockaddr_in *port_address, sockaddr_in *broadcast_address)
Definition: power_node.cpp:202
setupReceive
void setupReceive()
Definition: power_node.cpp:908
diagnostic_updater::DiagnosticStatusWrapper::summary
void summary(const diagnostic_msgs::DiagnosticStatus &src)
PowerBoard::process_transition_message
int process_transition_message(const TransitionMessage *msg, int len)
Definition: power_node.cpp:484
PowerBoard::collect_messages
int collect_messages()
Definition: power_node.cpp:527
Interface::Close
void Close()
Definition: power_node.cpp:150
PowerBoard::process_message
int process_message(const PowerMessage *msg, int len)
Definition: power_node.cpp:439
ROS_DEBUG
#define ROS_DEBUG(...)
MINIMUM_MESSAGE_REVISION
static const unsigned MINIMUM_MESSAGE_REVISION
Definition: power_comm.h:35
Interface::AddToReadSet
void AddToReadSet(fd_set &set, int &max_sock) const
Definition: power_node.cpp:275
MASTER_STANDBY
@ MASTER_STANDBY
Definition: power_comm.h:48
NONE
@ NONE
Definition: power_comm.h:50
TRANSITION_MESSAGE_REVISION
static const unsigned TRANSITION_MESSAGE_REVISION
Definition: power_comm.h:38
DiagnosticStatusWrapper.h
ROS_WARN
#define ROS_WARN(...)
COMMAND_STOP
@ COMMAND_STOP
Definition: power_comm.h:50
myBoard
static PowerBoard * myBoard
Definition: power_node.cpp:72
PowerBoard::master_state_to_str
const char * master_state_to_str(char state)
Definition: power_node.cpp:417
Device::getPowerMessage
const PowerMessage & getPowerMessage()
Definition: power_node.h:38
Interface::IsReadSet
bool IsReadSet(fd_set set) const
Definition: power_node.cpp:281
COMMAND_MESSAGE_REVISION
static const unsigned COMMAND_MESSAGE_REVISION
Definition: power_comm.h:39
STATE_NOPOWER
@ STATE_NOPOWER
Definition: power_comm.h:49
ros::Rate::sleep
bool sleep()
ros::console::levels::Info
Info
STATE_DISABLED
@ STATE_DISABLED
Definition: power_comm.h:49
set
ROSCPP_DECL void set(const std::string &key, bool b)
start
ROSCPP_DECL void start()
TIMEOUT
static const ros::Duration TIMEOUT
Definition: power_node.cpp:75
CURRENT_MESSAGE_SIZE
#define CURRENT_MESSAGE_SIZE
Definition: power_comm.h:34
macros_generated.h
ros::NodeHandle::ok
bool ok() const
STATE_ON
@ STATE_ON
Definition: power_comm.h:49
Device::setPowerMessage
void setPowerMessage(const PowerMessage &newpmsg)
Definition: power_node.cpp:108
Interface
Definition: power_node.h:9
main
int main(int argc, char **argv)
Definition: power_node.cpp:1006
MASTER_OFF
@ MASTER_OFF
Definition: power_comm.h:48
std
MESSAGE_ID_COMMAND
static const unsigned MESSAGE_ID_COMMAND
Definition: power_comm.h:43
Device::getTransitionMessage
const TransitionMessage & getTransitionMessage()
Definition: power_node.h:32
ROS_ERROR
#define ROS_ERROR(...)
getMessages
void getMessages()
Definition: power_node.cpp:880
Interface::send_sock
int send_sock
Definition: power_node.h:15
PowerBoard::req_
pr2_power_board::PowerBoardCommand::Request req_
Definition: power_node.h:77
CloseAllDevices
void CloseAllDevices(void)
Definition: power_node.cpp:900
power_comm.h
Device::tmsgset
bool tmsgset
Definition: power_node.h:45
diagnostic_updater::DiagnosticStatusWrapper
MASTER_SHUTDOWN
@ MASTER_SHUTDOWN
Definition: power_comm.h:48
MESSAGE_ID_POWER
static const unsigned MESSAGE_ID_POWER
Definition: power_comm.h:42
power_node.h
ros::Rate
Devices
static std::vector< Device * > Devices
Definition: power_node.cpp:70
ros::spin
ROSCPP_DECL void spin()
PowerBoard::init
void init()
Definition: power_node.cpp:647
PowerBoard::serial_number
unsigned int serial_number
Definition: power_node.h:80
Interface::interface
struct ifreq interface
Definition: power_node.h:13
REVISION_2_MESSAGE_SIZE
#define REVISION_2_MESSAGE_SIZE
Definition: power_comm.h:36
Interface::ifc_address
sockaddr_in ifc_address
Definition: power_node.h:23
PowerBoard::send_command
int send_command(unsigned int serial_number, int circuit_breaker, const std::string &command, unsigned flags)
Definition: power_node.cpp:285
ROSCONSOLE_DEFAULT_NAME
#define ROSCONSOLE_DEFAULT_NAME
PowerBoard::service
ros::ServiceServer service
Definition: power_node.h:73
header
const std::string header
assert.h
ROS_INFO
#define ROS_INFO(...)
sendMessages
void sendMessages()
Definition: power_node.cpp:885
Device::setTransitionMessage
void setTransitionMessage(const TransitionMessage &newtmsg)
Definition: power_node.cpp:78
ros::Duration
Device::Device
Device()
Definition: power_node.cpp:1049
MASTER_ON
@ MASTER_ON
Definition: power_comm.h:48
CreateAllInterfaces
int CreateAllInterfaces(void)
Definition: power_node.cpp:917
PowerBoard::sendMessages
void sendMessages()
Definition: power_node.cpp:680
PowerBoard::diags_pub
ros::Publisher diags_pub
Definition: power_node.h:74
ros::NodeHandle
ros::Time::now
static Time now()


pr2_power_board
Author(s): Curt Meyers, Blaise Gassend
autogenerated on Tue Mar 7 2023 03:19:35