Feature/battery warning msi laptop

CMakeLists.txt

@@ -1,10 +1,25 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(hero_bringup)
 
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  sensor_msgs
+)
+
+include_directories(${catkin_INCLUDE_DIRS})
 
 catkin_add_env_hooks(99.hero_functions SHELLS bash DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks)
 
 catkin_python_setup()
 
 catkin_package()
+
+###########
+## Build ##
+###########
+
+add_executable(battery_manager_laptop src/battery_manager_laptop.cpp)
+add_dependencies(battery_manager_laptop ${catkin_EXPORTED_TARGETS})
+target_link_libraries(battery_manager_laptop
+  ${catkin_LIBRARIES}
+)

launch/battery_manager_hero2.launch

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="namespace" default="/hero" />
+
+    <group ns="$(arg namespace)">
+        <!-- Battery warning parameters -->
+        <rosparam file="$(find hero_bringup)/parameters/hardware/battery_manager_hero2.yaml" command="load" />
+        <node name="battery_manager_hero2" pkg="hero_bringup" type="battery_manager_laptop" />
+    </group>
+</launch>

launch/hero_bringup.launch

@@ -59,7 +59,6 @@
         <include file="$(find robot_launch_files)/launch/interaction/mobile_ui_server.launch" unless="$(optenv ROBOT_REAL false)" />
     </group>
 
-
     <!-- Real hardware -->
     <group if="$(optenv ROBOT_REAL false)" >
 

package.xml

@@ -14,6 +14,9 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+
   <exec_depend>gazebo_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>hero_bridge</exec_depend>
@@ -23,6 +26,8 @@
   <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-yaml</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-yaml</exec_depend>
   <exec_depend>robot_launch_files</exec_depend>
+  <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
 
 </package>

parameters/hardware/battery_manager_hero2.yaml

@@ -0,0 +1,4 @@
+robot_location: "hero2"
+battery_empty: 10
+battery_low: 20
+battery_middle: 50

scripts/services/hero2-demo/hero2-demo-battery-manager.service

@@ -0,0 +1,11 @@
+[Unit]
+Description=running battery_manager_laptop
+
+[Install]
+WantedBy=multi-user.target
+
+[Service]
+User=demo
+Restart=on-failure
+ExecStart=/bin/bash -c 'source /home/demo/.tue/setup.bash && \
+                        roslaunch hero_bringup battery_manager_hero2.launch'

scripts/services/hero2/hero2-battery-manager.service

@@ -0,0 +1,11 @@
+[Unit]
+Description=running battery_manager_laptop
+
+[Install]
+WantedBy=multi-user.target
+
+[Service]
+User=amigo
+Restart=on-failure
+ExecStart=/bin/bash -c 'source /home/amigo/.tue/setup.bash && \
+                        roslaunch hero_bringup battery_manager_hero2.launch'

src/battery_manager_laptop.cpp

@@ -0,0 +1,75 @@
+#include <ros/ros.h>
+#include <sensor_msgs/BatteryState.h>
+#include <std_msgs/String.h>
+#include <string>
+#include <sstream>
+
+int middle;
+int low;
+int empty;
+std::string robot_location = "";
+std::string old_message = "";
+ros::Publisher speech_pub;
+
+void batteryCallback(const sensor_msgs::BatteryState::ConstPtr& msg)
+{
+    int percentage;
+    std::string message = "";
+    std::string location = "";
+    std::ostringstream string_message;
+
+    if (msg->location == robot_location)
+    {
+        percentage = (msg->percentage)*100;
+        location = msg->location;
+
+        // check battery status
+        if (percentage < empty)
+        {
+            string_message << "The battery on" << location <<  " is at"  << percentage << " percent. Charge me now!";
+        }
+        else if (percentage < low)
+        {
+            string_message << "The battery on" << location <<  " is at"  << percentage << " percent. Don't forget to charge me";
+        }
+        else if (percentage < middle)
+        {
+            string_message << "The battery on" << location <<  " is at"  << percentage << " percent. Keep an I on the batteries";
+        }
+
+        message = string_message.str();
+
+        // publish voice message
+        if (old_message != message && message != "")
+        {
+            std_msgs::String speech_msg;
+            speech_msg.data = message;
+            speech_pub.publish(speech_msg);
+            old_message = message;
+        }
+    }
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "Battery_manager_laptop");
+    ros::NodeHandle gn;
+
+    ros::param::get("/battery_middle", middle);
+    ros::param::get("/battery_low", low);
+    ros::param::get("/battery_empty", empty);
+    ros::param::get("/robot_location", robot_location);
+
+    ros::Subscriber battery_sub = gn.subscribe("battery", 1, batteryCallback);
+    speech_pub = gn.advertise<std_msgs::String>("text_to_speech/input", 10);
+
+    ros::Rate loop_rate(1.0);
+
+    while(gn.ok())
+    {
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+    return 0;
+}
+