00001 00002 #pragma once 00003 00004 #include <time.h> 00005 #include <string> 00006 #include "ros/ros.h" 00007 #include "pr2_power_board/PowerBoardCommand.h" 00008 #include "pr2_power_board/PowerBoardCommand2.h" 00009 #include "pr2_msgs/BatteryServer2.h" 00010 #include "boost/thread/mutex.hpp" 00011 00012 class Interface 00013 { 00014 public: 00015 00016 int recv_sock; 00017 int send_sock; 00018 Interface(); 00019 ~Interface() {Close();} 00020 void Close(); 00021 int Init( const std::string &address_str); 00022 int InitReceive(const std::string &address_str); 00023 void AddToReadSet(fd_set &set, int &max_sock) const; 00024 bool IsReadSet(fd_set set) const; 00025 }; 00026 00027 00028 class Device 00029 { 00030 public: 00031 ros::Time message_time; 00032 00033 const TransitionMessage &getTransitionMessage() 00034 { 00035 return tmsg; 00036 } 00037 void setTransitionMessage(const TransitionMessage &newtmsg); 00038 00039 const PowerMessage &getPowerMessage() 00040 { 00041 return pmsg; 00042 } 00043 void setPowerMessage(const PowerMessage &newpmsg); 00044 00045 Device(); 00046 ~Device() { }; 00047 private: 00048 bool tmsgset; 00049 TransitionMessage tmsg; 00050 bool pmsgset; 00051 PowerMessage pmsg; //last power message recived from device 00052 }; 00053 00054 00055 class PowerBoard 00056 { 00057 public: 00058 PowerBoard( const ros::NodeHandle node_handle, const std::string &address_str ); 00059 bool commandCallback( pr2_power_board::PowerBoardCommand::Request &req_, 00060 pr2_power_board::PowerBoardCommand::Response &res_); 00061 bool commandCallback2( pr2_power_board::PowerBoardCommand2::Request &req_, 00062 pr2_power_board::PowerBoardCommand2::Response &res_); 00063 void init(); 00064 void collectMessages(); 00065 void sendMessages(); 00066 int collect_messages(); 00067 int process_message(const PowerMessage *msg, int len); 00068 int process_transition_message(const TransitionMessage *msg, int len); 00069 const char* master_state_to_str(char state); 00070 const char* cb_state_to_str(char state); 00071 int list_devices(void); 00072 int send_command(int circuit_breaker, const std::string &command, unsigned flags); 00073 int requestMessage(const unsigned int message); 00074 00075 void checkFanSpeed(); // Check battery temperatures and send command for fan speed 00076 00077 private: 00078 ros::NodeHandle node_handle; 00079 ros::ServiceServer service; 00080 ros::ServiceServer service2; 00081 ros::Publisher diags_pub; 00082 ros::Publisher state_pub; 00083 ros::Subscriber battery_sub_; 00084 00085 std::map<int, float> battery_temps_; 00086 bool fan_high_; 00087 00088 int getFanDuty(); // Duty cycle to send to fan. 0 if no fan command 00089 00090 void batteryCB(const pr2_msgs::BatteryServer2::ConstPtr &msgPtr); 00091 00092 pr2_power_board::PowerBoardCommand::Request req_; 00093 pr2_power_board::PowerBoardCommand::Response res_; 00094 boost::mutex library_lock_; 00095 uint64_t ip_address; 00096 00097 };