batteries_monitor.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include <iostream>
#include "komodo_batteries_monitor/batteries_monitor.h"
#include "std_msgs/Float32.h"
#include "dynamixel_msgs/MotorStateList.h"
#include "dynamixel_msgs/MotorState.h"

ros::Publisher batteries_pub;

ros::Timer rover_timer;
bool rover_timer_running = true;

ros::Timer arm_timer;
bool arm_timer_running = true;

ros::Timer pc_timer;
bool pc_timer_running = true;
bool pc_first = true, arm_first = true, rover_first = true;

komodo_batteries_monitor::batteries_monitor bat_array;

double pc_bat_voltage_divider, pc_bat_min_voltage, pc_bat_max_voltage,
                arm_bat_min_voltage, arm_bat_max_voltage, rover_bat_min_voltage,
                rover_bat_max_voltage;
bool turn_off_enable = true;
int countdown = 29;

void rover_timerCallback(const ros::TimerEvent&) {

        //ROS_ERROR("ROVER BATTERY UNDER VOLTAGE!!!!");
        ROS_ERROR("Rover battery warnning! LOW VOLTAGE!!! [V=%.1f]",
                        bat_array.rover_bat_v);
        rover_timer_running = false;

}
void arm_timerCallback(const ros::TimerEvent&) {

        //ROS_ERROR("ARM BATTERY UNDER VOLTAGE!!!!");
        ROS_ERROR("Arm battery warnning! LOW VOLTAGE!!! [V=%.1f]",
                        bat_array.arm_bat_v);
        arm_timer_running = false;

}
void pc_timerCallback(const ros::TimerEvent&) {

        if (turn_off_enable) {

                if (countdown > 0) {
                        ROS_ERROR(
                                        "PC battery warnning! LOW VOLTAGE!!! Turning computer off in %d seconds...  [V=%.1f]",
                                        countdown, bat_array.pc_bat_v);
                        countdown = countdown - 1;

                } else {
                        ROS_ERROR(
                                        "PC battery warnning! LOW VOLTAGE!!! Turning computer off now...  [V=%.1f]",
                                        bat_array.pc_bat_v);
                        //system("sudo shutdown -c");
                        system("sudo shutdown -h now");
                        pc_timer.stop();
                        pc_timer_running = false;
                }
        } else {

                if (countdown > 0) {
                        ROS_ERROR(
                                        "PC battery warnning! LOW VOLTAGE!!! Turning computer off in %d seconds...  [V=%.1f]",
                                        countdown, bat_array.pc_bat_v);
                        countdown = countdown - 1;

                } else {

                        ROS_ERROR(
                                        "PC battery warnning! LOW VOLTAGE!!! Turning computer off disabled...  [V=%.1f]",
                                        bat_array.pc_bat_v);
                        pc_timer_running = false;
                        countdown = 29;
                }
        }

}

void pc_bat_Callback(const std_msgs::Float32::ConstPtr& bat) {
        bat_array.pc_bat_v = bat->data * (pc_bat_voltage_divider);
        bat_array.pc_bat_time = ros::Time::now();

        if (bat_array.pc_bat_v < pc_bat_min_voltage) {
                if (pc_timer_running == false) {
                        if (pc_first)
                                pc_first = false;
                        pc_timer_running = true;
                        pc_timer.stop();
                        pc_timer.start();
                        ROS_ERROR(
                                        "PC battery warnning! LOW VOLTAGE!!! Turning computer off in 30 seconds...  [V=%.1f]",
                                        bat_array.pc_bat_v);
                } else {
                        if (pc_first) {
                                pc_first = false;
                                pc_timer_running = true;
                                pc_timer.stop();
                                pc_timer.start();
                                ROS_ERROR(
                                                "PC battery warnning! LOW VOLTAGE!!! Turning computer off in 30 seconds...  [V=%.1f]",
                                                bat_array.pc_bat_v);
                        }
                }
        } else {
                if (pc_timer_running == true) {
                        pc_timer.stop();
                        pc_timer_running = false;
                        ROS_INFO("PC battery OK [V=%.1f]", bat_array.pc_bat_v);
                        countdown = 29;
                }

        }

        batteries_pub.publish(bat_array);

}

void rover_bat_Callback(const std_msgs::Float32::ConstPtr& bat) {
        bat_array.rover_bat_v = bat->data;
        bat_array.rover_bat_time = ros::Time::now();

        if (bat_array.rover_bat_v < rover_bat_min_voltage) {
                if (rover_timer_running == false) {
                        if (rover_first)
                                rover_first = false;
                        rover_timer_running = true;
                        rover_timer.stop();
                        rover_timer.start();
                } else {
                        if (rover_first) {
                                rover_first = false;
                                rover_timer_running = true;
                                rover_timer.stop();
                                rover_timer.start();
                        }
                        //ROS_ERROR("You should turn the rover off...[V=%.1f]", bat_array.rover_bat_v);
                }
        } else {
                if (rover_timer_running == true) {
                        rover_timer.stop();
                        rover_timer_running = false;
                        ROS_INFO("Rover battery OK [V=%.1f]", bat_array.rover_bat_v);
                }

        }
        //      ROS_INFO("array:%f   %f   %f",bat_array.pc_bat_v, bat_array.arm_bat_v , bat_array.rover_bat_v);
        batteries_pub.publish(bat_array);

}
void arm_bat_Callback(const dynamixel_msgs::MotorStateList::ConstPtr& bat) {

        dynamixel_msgs::MotorState m1 = bat->motor_states[0];
        bat_array.arm_bat_v = m1.voltage;
        bat_array.arm_bat_time = ros::Time::now();

        //ROS_INFO("array:%f   %f   %f", bat_array.pc_bat_v, bat_array.arm_bat_v,bat_array.rover_bat_v);

        if (bat_array.arm_bat_v < arm_bat_min_voltage) {
                if (arm_timer_running == false) {
                        if (arm_first)
                                arm_first = false;
                        arm_timer_running = true;
                        arm_timer.stop();
                        arm_timer.start();
                } else {
                        if (arm_first) {
                                arm_first = false;
                                arm_timer_running = true;
                                arm_timer.stop();
                                arm_timer.start();
                        }
                        //ROS_ERROR("You should turn the arm off...[V=%.1f]",bat_array.arm_bat_v);
                }
        } else {
                if (arm_timer_running == true) {
                        arm_timer.stop();
                        arm_timer_running = false;
                        ROS_INFO("Arm battery OK [V=%.1f]", bat_array.arm_bat_v);
                }

        }
        batteries_pub.publish(bat_array);
}

int main(int argc, char **argv) {

        // ros::init(argc, argv, "apm_talker");
        ros::init(argc, argv, "batteries_monitor_node");

        ros::NodeHandle n("~");

        n.param("pc_bat_voltage_divider", pc_bat_voltage_divider, 3.0);
        n.param("pc_bat_min_voltage", pc_bat_min_voltage, 11.0);
        //n.param("pc_bat_max_voltage", pc_bat_max_voltage, 25.0);

        n.param("arm_bat_min_voltage", arm_bat_min_voltage, 11.0);
        //n.param("arm_bat_max_voltage", arm_bat_max_voltage, 25.0);

        n.param("rover_bat_min_voltage", rover_bat_min_voltage, 22.0);
        //n.param("rover_bat_max_voltage", rover_bat_max_voltage, 25.0);
        n.param("turn_off_enable", turn_off_enable, true);

        ros::Subscriber pc_bat_sub = n.subscribe("/pc_bat", 10, pc_bat_Callback);

        ros::Subscriber rover_bat_sub = n.subscribe("/rover_bat", 10,
                        rover_bat_Callback);

        ros::Subscriber arm_bat_sub = n.subscribe("/motor_states/arm_port", 10,
                        arm_bat_Callback);

        batteries_pub = n.advertise < komodo_batteries_monitor::batteries_monitor
                        > ("/batteries_monitor", 10);

        rover_timer = n.createTimer(ros::Duration(5), rover_timerCallback, true);
        rover_timer.stop();
        pc_timer = n.createTimer(ros::Duration(1), pc_timerCallback, false);
        pc_timer.stop();
        arm_timer = n.createTimer(ros::Duration(5), arm_timerCallback, true);
        arm_timer.stop();
        //bat_array.header=;
        bat_array.pc_bat_v = -1;
        bat_array.arm_bat_v = -1;
        bat_array.rover_bat_v = -1;

        //ROS_INFO("array:%f   %f   %f",bat_array.pc_bat_v, bat_array.arm_bat_v , bat_array.rover_bat_v);
        ros::spin();

        return 0;
}