Go to the documentation of this file.00001
00002 #include <ros/ros.h>
00003
00004
00005 #include <pr2_msgs/PowerBoardState.h>
00006 #include <pr2_msgs/PowerState.h>
00007 #include <cob_relayboard/EmergencyStopState.h>
00008 #include <std_msgs/Bool.h>
00009 #include <std_msgs/Float64.h>
00010
00011 #include <cob_voltage_control_common.cpp>
00012
00013
00014
00015
00016
00017 class cob_voltage_control_ros
00018 {
00019 public:
00020 ros::NodeHandle n_;
00021
00022 ros::Publisher pub_em_stop_state__;
00023 ros::Publisher pub_powerstate__;
00024 ros::Publisher pub_relayboard_state__;
00025 ros::Publisher topicPub_Voltage;
00026
00027 CPhidgetInterfaceKitHandle IFK;
00028
00029
00030
00031 cob_voltage_control_data component_data_;
00032 cob_voltage_control_config component_config_;
00033 cob_voltage_control_impl component_implementation_;
00034
00035 cob_voltage_control_ros()
00036 {
00037
00038
00039
00040
00041 topicPub_Voltage = n_.advertise<std_msgs::Float64>("/power_board/voltage", 10);
00042
00043 n_.param("battery_max_voltage", component_config_.max_voltage, 50.0);
00044 n_.param("battery_min_voltage", component_config_.min_voltage, 44.0);
00045 n_.param("robot_max_voltage", component_config_.max_voltage_res, 61.0);
00046 n_.param("voltage_analog_port", component_config_.num_voltage_port, 1);
00047 n_.param("em_stop_dio_port", component_config_.num_em_stop_port, 0);
00048 n_.param("scanner_stop_dio_port", component_config_.num_scanner_em_port, 1);
00049
00050
00051 }
00052
00053 void configure()
00054 {
00055 component_implementation_.configure();
00056 }
00057
00058 void update()
00059 {
00060 component_implementation_.update(component_data_, component_config_);
00061 topicPub_Voltage.publish(component_data_.out_pub_voltage);
00062 }
00063 };
00064
00065 int main(int argc, char** argv)
00066 {
00067
00068 ros::init(argc, argv, "cob_voltage_control");
00069
00070 cob_voltage_control_ros node;
00071 ROS_INFO("blub");
00072 node.configure();
00073
00074 ros::Rate loop_rate(100);
00075
00076 while(node.n_.ok())
00077 {
00078 node.update();
00079 loop_rate.sleep();
00080 ros::spinOnce();
00081 }
00082 return 0;
00083 }