$search
00001 // ROS includes 00002 #include <ros/ros.h> 00003 00004 // ROS message includes 00005 #include <pr2_msgs/PowerBoardState.h> 00006 #include <pr2_msgs/PowerState.h> 00007 #include <cob_relayboard/EmergencyStopState.h> 00008 00009 #include <cob_voltage_control_common.cpp> 00010 00011 00012 class cob_voltage_control_ros 00013 { 00014 public: 00015 ros::NodeHandle n_; 00016 00017 ros::Publisher pub_em_stop_state__; 00018 ros::Publisher pub_powerstate__; 00019 ros::Publisher pub_relayboard_state__; 00020 00021 00022 00023 00024 cob_voltage_control_data component_data_; 00025 cob_voltage_control_config component_config_; 00026 cob_voltage_control_impl component_implementation_; 00027 00028 cob_voltage_control_ros() 00029 { 00030 00031 pub_em_stop_state__ = n_.advertise<pr2_msgs::PowerBoardState>("pub_em_stop_state_", 1); 00032 pub_powerstate__ = n_.advertise<pr2_msgs::PowerState>("pub_powerstate_", 1); 00033 pub_relayboard_state__ = n_.advertise<cob_relayboard::EmergencyStopState>("pub_relayboard_state_", 1); 00034 00035 n_.param("battery_max_voltage", component_config_.max_voltage, 50.0); 00036 n_.param("battery_min_voltage", component_config_.min_voltage, 44.0); 00037 n_.param("robot_max_voltage", component_config_.max_voltage_res, 70.0); 00038 n_.param("voltage_analog_port", component_config_.num_voltage_port, 1); 00039 n_.param("em_stop_dio_port", component_config_.num_em_stop_port, 0); 00040 n_.param("scanner_stop_dio_port", component_config_.num_scanner_em_port, 1); 00041 00042 00043 } 00044 00045 void configure() 00046 { 00047 component_implementation_.configure(); 00048 } 00049 00050 void update() 00051 { 00052 component_implementation_.update(component_data_, component_config_); 00053 pub_em_stop_state__.publish(component_data_.out_pub_em_stop_state_); 00054 pub_powerstate__.publish(component_data_.out_pub_powerstate_); 00055 pub_relayboard_state__.publish(component_data_.out_pub_relayboard_state); 00056 } 00057 }; 00058 00059 int main(int argc, char** argv) 00060 { 00061 00062 ros::init(argc, argv, "cob_voltage_control"); 00063 00064 cob_voltage_control_ros node; 00065 ROS_INFO("blub"); 00066 node.configure(); 00067 00068 ros::Rate loop_rate(100); // Hz // if cycle time == 0 do a spin() here without calling node.update() 00069 00070 while(node.n_.ok()) 00071 { 00072 node.update(); 00073 loop_rate.sleep(); 00074 ros::spinOnce(); 00075 } 00076 return 0; 00077 }