power_node2.h
Go to the documentation of this file.
1 
2 #pragma once
3 
4 #include <time.h>
5 #include <string>
6 #include "ros/ros.h"
7 #include "pr2_power_board/PowerBoardCommand.h"
8 #include "pr2_power_board/PowerBoardCommand2.h"
9 #include "pr2_msgs/BatteryServer2.h"
10 #include "boost/thread/mutex.hpp"
11 
12 class Interface
13 {
14  public:
15 
16  int recv_sock;
17  int send_sock;
18  Interface();
20  void Close();
21  int Init( const std::string &address_str);
22  int InitReceive(const std::string &address_str);
23  void AddToReadSet(fd_set &set, int &max_sock) const;
24  bool IsReadSet(fd_set set) const;
25 };
26 
27 
28 class Device
29 {
30  public:
31  ros::Time message_time;
32 
33  const TransitionMessage &getTransitionMessage()
34  {
35  return tmsg;
36  }
37  void setTransitionMessage(const TransitionMessage &newtmsg);
38 
39  const PowerMessage &getPowerMessage()
40  {
41  return pmsg;
42  }
43  void setPowerMessage(const PowerMessage &newpmsg);
44 
45  Device();
46  ~Device() { };
47  private:
48  bool tmsgset;
49  TransitionMessage tmsg;
50  bool pmsgset;
51  PowerMessage pmsg; //last power message recived from device
52 };
53 
54 
55 class PowerBoard
56 {
57  public:
58  PowerBoard( const ros::NodeHandle node_handle, const std::string &address_str );
59  bool commandCallback( pr2_power_board::PowerBoardCommand::Request &req_,
60  pr2_power_board::PowerBoardCommand::Response &res_);
61  bool commandCallback2( pr2_power_board::PowerBoardCommand2::Request &req_,
62  pr2_power_board::PowerBoardCommand2::Response &res_);
63  void init();
64  void collectMessages();
65  void sendMessages();
66  int collect_messages();
67  int process_message(const PowerMessage *msg, int len);
68  int process_transition_message(const TransitionMessage *msg, int len);
69  const char* master_state_to_str(char state);
70  const char* cb_state_to_str(char state);
71  int list_devices(void);
72  int send_command(int circuit_breaker, const std::string &command, unsigned flags);
73  int requestMessage(const unsigned int message);
74 
75  void checkFanSpeed(); // Check battery temperatures and send command for fan speed
76 
77  private:
78  ros::NodeHandle node_handle;
79  ros::ServiceServer service;
81  ros::Publisher diags_pub;
82  ros::Publisher state_pub;
84 
85  std::map<int, float> battery_temps_;
86  bool fan_high_;
87 
88  int getFanDuty(); // Duty cycle to send to fan. 0 if no fan command
89 
90  void batteryCB(const pr2_msgs::BatteryServer2::ConstPtr &msgPtr);
91 
92  pr2_power_board::PowerBoardCommand::Request req_;
93  pr2_power_board::PowerBoardCommand::Response res_;
94  boost::mutex library_lock_;
95  uint64_t ip_address;
96 
97 };
msg
int Init(sockaddr_in *port_address, sockaddr_in *broadcast_address)
Definition: power_node.cpp:202
void init(const M_string &remappings)
int send_sock
Definition: power_node.h:15
std::map< int, float > battery_temps_
Definition: power_node2.h:85
uint64_t ip_address
Definition: power_node2.h:95
int InitReceive()
Definition: power_node.cpp:162
const TransitionMessage & getTransitionMessage()
Definition: power_node2.h:33
bool fan_high_
Definition: power_node2.h:86
bool IsReadSet(fd_set set) const
Definition: power_node.cpp:281
int recv_sock
Definition: power_node.h:14
void sendMessages()
Definition: power_node.cpp:885
const PowerMessage & getPowerMessage()
Definition: power_node2.h:39
list command
Definition: test_power.py:44
void AddToReadSet(fd_set &set, int &max_sock) const
Definition: power_node.cpp:275
~Device()
Definition: power_node2.h:46
void Close()
Definition: power_node.cpp:150
ros::ServiceServer service2
Definition: power_node2.h:80
ros::Subscriber battery_sub_
Definition: power_node2.h:83


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