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; +} +