35 #include <sys/select.h> 36 #include <sys/types.h> 43 #include <boost/thread/thread.hpp> 44 #include <boost/program_options.hpp> 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> 53 #include <sys/ioctl.h> 55 #include <log4cxx/logger.h> 59 #include "diagnostic_msgs/DiagnosticArray.h" 61 #include "pr2_msgs/PowerBoardState.h" 66 namespace po = boost::program_options;
82 #define PRINT_IF_CHANGED(val) \ 83 for (int counter = 0; counter < 3; counter++) \ 85 if (tmsg.cb[counter].val##_count != newtmsg.cb[counter].val##_count) \ 87 ROS_INFO("Power board: CB%i "#val" event count changed to %i.", counter, newtmsg.cb[counter].val##_count);\ 100 #undef PRINT_IF_CHANGED 112 #define PRINT_IF_CHANGED(val) \ 113 if (pmsg.status.val##_status != newpmsg.status.val##_status) \ 115 ROS_INFO("Power board: status of "#val" changed to %i.", newpmsg.status.val##_status); \ 124 #undef PRINT_IF_CHANGED 129 if(newpmsg.header.message_revision == 2)
131 memcpy( &pmsg.header, &newpmsg.header,
sizeof(MessageHeader));
134 memcpy( &pmsg.status, &newpmsg.status,
sizeof(StatusStruct_Rev2));
145 assert(strlen(ifname) <=
sizeof(
interface.ifr_name));
146 strncpy(
interface.ifr_name, ifname, IFNAMSIZ);
165 recv_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
167 perror(
"Couldn't create recv socket");
173 if (setsockopt(
recv_sock, SOL_SOCKET, SO_REUSEADDR, &opt,
sizeof(opt))) {
174 perror(
"Couldn't set reuse addr on recv socket\n");
181 if (setsockopt(
recv_sock, SOL_SOCKET, SO_BROADCAST, &opt,
sizeof(opt))) {
182 perror(
"Setting broadcast option on recv");
188 struct sockaddr_in sin;
189 memset(&sin, 0,
sizeof(sin));
190 sin.sin_family = AF_INET;
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");
205 memcpy( &
ifc_address, broadcast_address,
sizeof(sockaddr_in));
207 recv_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
209 perror(
"Couldn't create recv socket");
213 send_sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
216 perror(
"Couldn't create send socket");
225 if (setsockopt(
recv_sock, SOL_SOCKET, SO_REUSEADDR, &opt,
sizeof(opt))) {
226 perror(
"Couldn't set reuse addr on recv socket\n");
233 if (setsockopt(
recv_sock, SOL_SOCKET, SO_BROADCAST, &opt,
sizeof(opt))) {
234 perror(
"Setting broadcast option on recv");
241 if (setsockopt(
send_sock, SOL_SOCKET, SO_BROADCAST, &opt,
sizeof(opt))) {
242 perror(
"Setting broadcast option on send");
248 struct sockaddr_in sin;
249 memset(&sin, 0,
sizeof(sin));
250 sin.sin_family = AF_INET;
253 sin.sin_addr.s_addr = (INADDR_ANY);
255 if (bind(
recv_sock, (
struct sockaddr*)&sin,
sizeof(sin))) {
256 perror(
"Couldn't Bind socket to port");
265 sin.sin_addr= broadcast_address->sin_addr;
266 if (connect(
send_sock, (
struct sockaddr*)&sin,
sizeof(sin))) {
267 perror(
"Connect'ing socket failed");
288 fprintf(stderr,
"No devices to send command to\n");
292 int selected_device = -1;
294 if(serial_number == 0) {
296 fprintf(stderr,
"Too many devices to send command to using serial_number=0\n");
302 for (
unsigned i = 0; i<
Devices.size(); ++i) {
303 if (
Devices[i]->getPowerMessage().header.serial_num == serial_number) {
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);
316 assert(device != NULL);
318 if ((circuit_breaker < 0) || (circuit_breaker > 2)) {
319 fprintf(stderr,
"Circuit breaker number must be between 0 and 2\n");
323 ROS_DEBUG(
"circuit=%d command=%s flags=%x\n", circuit_breaker, command.c_str(),
flags);
326 char command_enum =
NONE;
327 if (command ==
"start") {
330 else if (command ==
"stop") {
333 else if (command ==
"reset") {
336 else if (command ==
"disable") {
339 else if (command ==
"none") {
343 ROS_ERROR(
"invalid command '%s'", command.c_str());
350 CommandMessage cmdmsg;
351 memset(&cmdmsg, 0,
sizeof(cmdmsg));
354 cmdmsg.header.serial_num = device->
getPowerMessage().header.serial_num;
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;
363 else if (circuit_breaker==1) {
364 cmdmsg.command.CB1_command = command_enum;
366 else if (circuit_breaker==2) {
367 cmdmsg.command.CB2_command = command_enum;
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;
375 cmdmsg.command.flags =
flags;
385 }
else if (result !=
sizeof(cmdmsg)) {
386 ROS_WARN(
"Error sending : send only took %d of %d bytes\n",
387 result, (
int)
sizeof(cmdmsg));
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);
442 ROS_WARN(
"Got message with incorrect message revision %u\n", msg->header.message_revision);
447 ROS_ERROR(
"received message of incorrect size %d for rev=%d\n", len, msg->header.message_revision);
450 ROS_ERROR(
"received message of incorrect size %d for rev=%d\n", len, msg->header.message_revision);
454 if(serial_number != 0)
456 if(serial_number == msg->header.serial_num)
458 boost::mutex::scoped_lock(library_lock_);
460 Devices[0]->setPowerMessage(*msg);
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_);
469 Devices[i]->setPowerMessage(*msg);
487 ROS_WARN(
"Got message with incorrect message revision %u\n", msg->header.message_revision);
491 if (len !=
sizeof(TransitionMessage)) {
492 ROS_ERROR(
"received message of incorrect size %d\n", len);
497 if(serial_number != 0)
499 if(serial_number == msg->header.serial_num)
501 boost::mutex::scoped_lock(library_lock_);
503 Devices[0]->setTransitionMessage(*msg);
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);
529 PowerMessage *power_msg;
530 TransitionMessage *transition_msg;
550 int result = select(max_sock+1, &read_set, NULL, NULL, &timeout);
558 else if (result == 0) {
561 else if (result >= 1) {
575 int len = recv(recvInterface->
recv_sock, tmp_buf,
sizeof(tmp_buf), 0);
580 else if (len < (
int)
sizeof(MessageHeader)) {
581 ROS_ERROR(
"received message of incorrect size %d\n", len);
584 header = (MessageHeader*)tmp_buf;
590 power_msg = (PowerMessage*)tmp_buf;
601 if (process_message(power_msg, len))
607 transition_msg = (TransitionMessage *)tmp_buf;
613 if (process_transition_message(transition_msg, len))
619 ROS_DEBUG(
"Discard message len=%d", len);
624 ROS_ERROR(
"Unexpected select result %d\n", result);
638 if( my_logger->getLevel() == 0 )
664 pr2_power_board::PowerBoardCommand::Response &
res_)
666 res_.retval =
send_command( req_.serial_number, req_.breaker_number, req_.command, req_.flags);
689 for (
unsigned i = 0; i<
Devices.size(); ++i)
691 diagnostic_msgs::DiagnosticArray msg_out;
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();
711 const StatusStruct *status = &pmesg->status;
714 ROS_DEBUG(
" Serial = %u", pmesg->header.serial_num);
716 stat.
add(
"Serial Number", pmesg->header.serial_num);
718 ROS_DEBUG(
" Current = %f", status->input_current);
719 stat.
add(
"Input Current", status->input_current);
722 ROS_DEBUG(
" Input = %f", status->input_voltage);
723 stat.
add(
"Input Voltage", status->input_voltage);
725 ROS_DEBUG(
" DCDC 12 aux = %f", status->DCDC_12V_aux);
726 stat.
add(
"DCDC 12V aux", status->DCDC_12V_aux);
728 ROS_DEBUG(
" DCDC 12V cpu0 = %f", status->DCDC_12V_cpu0);
729 stat.
add(
"DCDC 12V cpu0", status->DCDC_12V_cpu0);
731 ROS_DEBUG(
" CB0 (Base) = %f", status->CB0_voltage);
732 stat.
add(
"Breaker 0 Voltage", status->CB0_voltage);
734 ROS_DEBUG(
" CB1 (R-arm) = %f", status->CB1_voltage);
735 stat.
add(
"Breaker 1 Voltage", status->CB1_voltage);
737 ROS_DEBUG(
" CB2 (L-arm) = %f", status->CB2_voltage);
738 stat.
add(
"Breaker 2 Voltage", status->CB2_voltage);
740 ROS_DEBUG(
" Board Temp = %f", status->ambient_temp);
741 stat.
add(
"Board Temperature", status->ambient_temp);
744 ROS_DEBUG(
" Fan 0 = %u", status->fan0_speed);
745 stat.
add(
"Fan 0 Speed", status->fan0_speed);
747 ROS_DEBUG(
" Fan 1 = %u", status->fan1_speed);
748 stat.
add(
"Fan 1 Speed", status->fan1_speed);
750 ROS_DEBUG(
" Fan 2 = %u", status->fan2_speed);
751 stat.
add(
"Fan 2 Speed", status->fan2_speed);
753 ROS_DEBUG(
" Fan 3 = %u", status->fan3_speed);
754 stat.
add(
"Fan 3 Speed", status->fan3_speed);
770 ROS_DEBUG(
" CB0 (Base) = %s", (status->CB0_status) ?
"On" :
"Off");
771 stat.
add(
"Breaker 0 Status", (status->CB0_status) ?
"On" :
"Off");
773 ROS_DEBUG(
" CB1 (R-arm) = %s", (status->CB1_status) ?
"On" :
"Off");
774 stat.
add(
"Breaker 1 Status", (status->CB1_status) ?
"On" :
"Off");
776 ROS_DEBUG(
" CB2 (L-arm) = %s", (status->CB2_status) ?
"On" :
"Off");
777 stat.
add(
"Breaker 2 Status", (status->CB2_status) ?
"On" :
"Off");
779 ROS_DEBUG(
" estop_button= %x", (status->estop_button_status));
780 stat.
add(
"RunStop Button Status", (status->estop_button_status ?
"True":
"False"));
782 ROS_DEBUG(
" estop_status= %x", (status->estop_status));
783 stat.
add(
"RunStop Status", (status->estop_status ?
"True":
"False"));
787 stat.
add(
"PCA", status->pca_rev);
790 stat.
add(
"PCB", status->pcb_rev);
792 ROS_DEBUG(
" Major = %c", status->major_rev);
793 stat.
add(
"Major Revision", status->major_rev);
795 ROS_DEBUG(
" Minor = %c", status->minor_rev);
796 stat.
add(
"Minor Revision", status->minor_rev);
798 stat.
add(
"Min Voltage", status->min_input_voltage);
799 stat.
add(
"Max Current", status->max_input_current);
801 ROS_DEBUG(
" DCDC 12V cpu1 = %f", status->DCDC_12V_cpu1);
802 stat.
add(
"DCDC 12V cpu1", status->DCDC_12V_cpu1);
804 ROS_DEBUG(
" DCDC 12V user = %f", status->DCDC_12V_user);
805 stat.
add(
"DCDC 12V user", status->DCDC_12V_user);
807 for(
int xx = 0; xx < 4; ++xx)
809 ROS_DEBUG(
" Battery %d voltage=%f", xx, status->battery_voltage[xx]);
811 ss <<
"Battery " << xx <<
" voltage=";
812 stat.
add(ss.str(), status->battery_voltage[xx]);
817 for(
int cb_index=0; cb_index < 3; ++cb_index)
819 const TransitionCount *trans = &tmsg->cb[cb_index];
822 ss <<
"CB" << cb_index <<
" Stop Count";
823 stat.
add(ss.str(), (int)trans->stop_count);
827 ss <<
"CB" << cb_index <<
" E-Stop Count";
828 stat.
add(ss.str(), (int)trans->estop_count);
831 ss <<
"CB" << cb_index <<
" Trip Count";
832 stat.
add(ss.str(), (int)trans->trip_count);
835 ss <<
"CB" << cb_index <<
" 18V Fail Count";
836 stat.
add(ss.str(), (int)trans->fail_18V_count);
839 ss <<
"CB" << cb_index <<
" Disable Count";
840 stat.
add(ss.str(), (int)trans->disable_count);
843 ss <<
"CB" << cb_index <<
" Start Count";
844 stat.
add(ss.str(), (int)trans->start_count);
847 ss <<
"CB" << cb_index <<
" Pump Fail Count";
848 stat.
add(ss.str(), (int)trans->pump_fail_count);
851 ss <<
"CB" << cb_index <<
" Reset Count";
852 stat.
add(ss.str(), (int)trans->reset_count);
855 msg_out.status.push_back(stat);
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;
901 for (
unsigned i=0; i<
Devices.size(); ++i){
912 ReceiveInterface = newInterface;
920 int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
922 ROS_ERROR(
"Couldn't create socket for ioctl requests");
926 struct ifconf get_io;
927 get_io.ifc_req =
new ifreq[10];
928 get_io.ifc_len =
sizeof(ifreq) * 10;
930 if(ioctl( sock, SIOCGIFCONF, &get_io ) == 0)
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)
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)
940 sockaddr_in *addr = (sockaddr_in*)&get_io.ifc_req[yy].ifr_addr;
941 ROS_DEBUG(
"address=%s", inet_ntoa(addr->sin_addr) );
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))
947 ROS_INFO(
"Ignoring interface %*s", (
int)strlen(get_io.ifc_req[yy].ifr_name), get_io.ifc_req[yy].ifr_name);
952 ROS_INFO(
"Found interface %*s", (
int)strlen(get_io.ifc_req[yy].ifr_name), get_io.ifc_req[yy].ifr_name);
954 assert(newInterface != NULL);
955 if (newInterface == NULL)
961 struct ifreq *ifr = &get_io.ifc_req[yy];
962 if(ioctl(sock, SIOCGIFBRDADDR, ifr) == 0)
964 if (ifr->ifr_broadaddr.sa_family == AF_INET)
966 struct sockaddr_in *br_addr = (
struct sockaddr_in *) &ifr->ifr_dstaddr;
968 ROS_DEBUG (
"Broadcast addess %s", inet_ntoa(br_addr->sin_addr));
970 if (newInterface->
Init(addr, br_addr))
972 ROS_ERROR(
"Error initializing interface %*s", (
int)
sizeof(get_io.ifc_req[yy].ifr_name), get_io.ifc_req[yy].ifr_name);
998 delete[] get_io.ifc_req;
1010 unsigned int serial_option;
1011 po::options_description desc(
"Allowed options");
1013 (
"help",
"this help message")
1014 (
"serial", po::value<unsigned int>(&serial_option)->default_value(0),
"filter a specific serial number");
1016 po::variables_map vm;
1017 po::store(po::parse_command_line( argc, argv, desc), vm);
1020 if( vm.count(
"help"))
1022 cout << desc <<
"\n";
1029 myBoard =
new PowerBoard( handle, serial_option);
1053 memset( &
pmsg, 0,
sizeof(PowerMessage));
1054 memset( &
tmsg, 0,
sizeof(TransitionMessage));
static const unsigned COMMAND_MESSAGE_REVISION
const char * master_state_to_str(char state)
int Init(sockaddr_in *port_address, sockaddr_in *broadcast_address)
static std::vector< Device * > Devices
void summary(unsigned char lvl, const std::string s)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define CURRENT_MESSAGE_SIZE
unsigned int serial_number
#define ROSCONSOLE_AUTOINIT
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
int send_command(unsigned int serial_number, int circuit_breaker, const std::string &command, unsigned flags)
pr2_power_board::PowerBoardCommand::Request req_
ros::ServiceServer service
static Interface * ReceiveInterface
static const unsigned MESSAGE_ID_COMMAND
void publish(const boost::shared_ptr< M > &message) const
#define REVISION_2_MESSAGE_SIZE
#define PRINT_IF_CHANGED(val)
void CloseAllInterfaces(void)
int main(int argc, char **argv)
void setPowerMessage(const PowerMessage &newpmsg)
static const ros::Duration TIMEOUT
ros::NodeHandle node_handle
static const unsigned MESSAGE_ID_POWER
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const TransitionMessage & getTransitionMessage()
static const unsigned MINIMUM_MESSAGE_REVISION
static const unsigned TRANSITION_MESSAGE_REVISION
const char * cb_state_to_str(char state)
bool IsReadSet(fd_set set) const
int CreateAllInterfaces(void)
boost::mutex library_lock_
bool commandCallback(pr2_power_board::PowerBoardCommand::Request &req_, pr2_power_board::PowerBoardCommand::Response &res_)
const PowerMessage & getPowerMessage()
void setTransitionMessage(const TransitionMessage &newtmsg)
static const unsigned MESSAGE_ID_TRANSITION
void AddToReadSet(fd_set &set, int &max_sock) const
static const unsigned CURRENT_MESSAGE_REVISION
pr2_power_board::PowerBoardCommand::Response res_
PowerBoard(const ros::NodeHandle node_handle, unsigned int serial_number=0)
static PowerBoard * myBoard
static const unsigned POWER_PORT
void add(const std::string &key, const T &val)
static std::vector< Interface * > SendInterfaces
#define ROSCONSOLE_DEFAULT_NAME
void CloseAllDevices(void)
int process_message(const PowerMessage *msg, int len)
int process_transition_message(const TransitionMessage *msg, int len)