diff --git a/CMakeLists.txt b/CMakeLists.txt
index d77907a5..3e47dfc3 100644
--- a/CMakeLists.txt
+++ b/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}
+)
diff --git a/launch/battery_manager_hero2.launch b/launch/battery_manager_hero2.launch
new file mode 100644
index 00000000..7ea31019
--- /dev/null
+++ b/launch/battery_manager_hero2.launch
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/hero_bringup.launch b/launch/hero_bringup.launch
index f011819c..8823149a 100644
--- a/launch/hero_bringup.launch
+++ b/launch/hero_bringup.launch
@@ -59,7 +59,6 @@
-
diff --git a/package.xml b/package.xml
index 5de6bce5..416f6f16 100644
--- a/package.xml
+++ b/package.xml
@@ -14,6 +14,9 @@
catkin
+ roscpp
+ sensor_msgs
+
gazebo_msgs
geometry_msgs
hero_bridge
@@ -23,6 +26,8 @@
python-yaml
python3-yaml
robot_launch_files
+ roscpp
rospy
+ sensor_msgs
diff --git a/parameters/hardware/battery_manager_hero2.yaml b/parameters/hardware/battery_manager_hero2.yaml
new file mode 100644
index 00000000..5da72e9b
--- /dev/null
+++ b/parameters/hardware/battery_manager_hero2.yaml
@@ -0,0 +1,4 @@
+robot_location: "hero2"
+battery_empty: 10
+battery_low: 20
+battery_middle: 50
diff --git a/scripts/services/hero2-demo/hero2-demo-battery-manager.service b/scripts/services/hero2-demo/hero2-demo-battery-manager.service
new file mode 100644
index 00000000..4d550ee5
--- /dev/null
+++ b/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'
diff --git a/scripts/services/hero2/hero2-battery-manager.service b/scripts/services/hero2/hero2-battery-manager.service
new file mode 100644
index 00000000..3df5e31b
--- /dev/null
+++ b/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'
diff --git a/src/battery_manager_laptop.cpp b/src/battery_manager_laptop.cpp
new file mode 100644
index 00000000..ba273ff0
--- /dev/null
+++ b/src/battery_manager_laptop.cpp
@@ -0,0 +1,75 @@
+#include
+#include
+#include
+#include
+#include
+
+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("text_to_speech/input", 10);
+
+ ros::Rate loop_rate(1.0);
+
+ while(gn.ok())
+ {
+ ros::spinOnce();
+ loop_rate.sleep();
+ }
+ return 0;
+}
+