mutac:simulate battery discharge without events

multi_robot_simulator/include/simulator/flightmare_simulator.h

@@ -99,6 +99,8 @@ class Simulator {
     std::map<int, image_transport::Publisher> rgb_pubs;
     std::map<int, std::shared_ptr<RGBCamera>> rgb_cameras;
 
+    double battery_each_min;
+
 private:
     void connectROS();
     void configDrones();

multi_robot_simulator/include/unexpected_events/drone.h

@@ -37,6 +37,7 @@
 #include "geometry_msgs/PoseStamped.h"
 #include "geometry_msgs/Pose.h"
 #include "geometry_msgs/Twist.h"
+#include "sensor_msgs/BatteryState.h"
 
 #include "polynomial_trajectories/minimum_snap_trajectories.h"
 #include "polynomial_trajectories/polynomial_trajectories_common.h"
@@ -71,7 +72,7 @@ class Drone {
 
 private:
     int id;
-    int battery;
+    double battery;
     std::vector<double> position;
 
     int stopped_time;
@@ -94,9 +95,12 @@ class Drone {
 
     ros::Time timer;
 
+    ros::NodeHandle nh;
+    ros::Publisher battery_pub;
+
 public: 
     int getId() {return id;}
-    int getBattery() {return battery;}
+    double getBattery() {return battery;}
     std::vector<double> getPosition() {return position;}
 
     int getStoppedTime() {return stopped_time;}
@@ -122,7 +126,7 @@ class Drone {
     ros::Time getTimer() {return timer;}
 
 public:
-    void setBattery(int battery) {this->battery = battery;}
+    void setBattery(double battery) {this->battery = battery;}
     void setPosition(std::vector<double> pos) {position[0] = pos[0]; position[1] = pos[1]; position[2] = pos[2];}
 
     void setStoppedTime(int stopped_time) {this->stopped_time = stopped_time;}
@@ -153,6 +157,8 @@ class Drone {
     void addPoint(std::vector<double> point){trj_points.push_back(point);}
     void clearTrjPoints(){trj_points.clear();}
 
+    void batteryDischarge(double percentage);
+
 private:
     bool changeTrajectory();
     double calculateSegmentTime(Eigen::Vector3d p1, Eigen::Vector3d p2);

multi_robot_simulator/include/unexpected_events/event_controller.h

@@ -64,7 +64,7 @@ class EventController {
     ros::Timer timer;
 
     ros::NodeHandle nh;
-    ros::Publisher battery_pub;
+    //ros::Publisher battery_pub;
     ros::Publisher alarm_pub;
 
 public:
@@ -73,7 +73,7 @@ class EventController {
 private:
     void blindCamera();
     void goHomeBase();
-    void batteryDischarge(int percentage);
+    //void batteryDischarge(double percentage);
     void fallDown();
     void stop(int secs);
     void rectilinearMotion();

multi_robot_simulator/src/simulator/flightmare_simulator.cpp

@@ -66,6 +66,9 @@ Simulator::Simulator() {
         homebases.push_back(pos);
     }
 
+    nh.param<double>("/mutac/battery_each_min", battery_each_min, 1);
+
+
     // ROS communication
     connectROS();
 
@@ -142,6 +145,9 @@ void Simulator::start() {
                 rgb_msg->header.stamp = time;
                 rgb_pubs[i].publish(rgb_msg);
             }
+            if(drones[i]->getState().first != MotionState::NOT_STARTED) {
+                drones[i]->batteryDischarge(battery_each_min/600);
+            }
         }
 
         unity_bridge_ptr->getRender(frame_id);

multi_robot_simulator/src/unexpected_events/drone.cpp

@@ -48,6 +48,9 @@ Drone::Drone(int id, std::vector<double> init_pos) {
 
     acceleration << 0.0, 0.0, 0.0;
     velocity << 0.0, 0.0, 0.0;
+
+    nh = ros::NodeHandle("");
+    battery_pub = nh.advertise<sensor_msgs::BatteryState>("drone" + std::to_string(id+1) + "/battery_state", 100);
 }
 
 Drone::Drone(const Drone &other) :
@@ -135,3 +138,22 @@ double Drone::calculateSegmentTime(Eigen::Vector3d p1, Eigen::Vector3d p2) {
     double dist = std::sqrt(std::pow((p1[0] - p2[0]), 2) + std::pow((p1[1] - p2[1]), 2) + std::pow((p1[2] - p2[2]), 2));
     return dist / trj_velocity;
 }
+
+void Drone::batteryDischarge(double percentage) {
+    sensor_msgs::BatteryState msg = sensor_msgs::BatteryState();
+    double real_battery = getBattery();
+    double discharged_battery = real_battery - percentage;
+    std::cout << "Dron" << id << " battery: " << real_battery << " (now: " << discharged_battery << ")" << std::endl;
+    setBattery(discharged_battery);
+
+    if(ceil(real_battery) != ceil(discharged_battery)) {
+        msg.percentage = ceil(discharged_battery);
+
+        if (msg.percentage <= 0) {
+            msg.percentage = 0;
+        }
+
+        //setBattery(msg.percentage);
+        battery_pub.publish(msg);
+    }
+}

multi_robot_simulator/src/unexpected_events/event_controller.cpp

@@ -33,14 +33,15 @@ EventController::EventController(std::shared_ptr<Drone> drone, std::vector<doubl
     drone(drone), homebase(homebase), floorZ(floorZ) {
         nh = ros::NodeHandle("");
         alarm_pub = nh.advertise<mutac_msgs::Alarm>("drone_alarm", 100);
-        battery_pub = nh.advertise<sensor_msgs::BatteryState>("drone" + to_string(drone->getId()+1) + "/battery_state", 100);
+        //battery_pub = nh.advertise<sensor_msgs::BatteryState>("drone" + to_string(drone->getId()+1) + "/battery_state", 100);
 }
 
 EventController::EventController(const EventController &other) :
-    drone(other.drone), homebase(other.homebase), floorZ(other.floorZ), timer(other.timer), nh(other.nh), battery_pub(other.battery_pub),
+    drone(other.drone), homebase(other.homebase), floorZ(other.floorZ), timer(other.timer), nh(other.nh), //battery_pub(other.battery_pub),
     alarm_pub(other.alarm_pub) {}
 
 void EventController::startEvent(Event event) {
+    double instantBattery;
     switch (event.getType()) {
     case EventType::BLIND_CAMERA:
         std::cout << "Drone: " << drone->getId() << " Event: BLIND_CAMERA" << std::endl;
@@ -52,7 +53,11 @@ void EventController::startEvent(Event event) {
         break;
     case EventType::BATTERY_DISCHARGE:
         std::cout << "Drone: " << drone->getId() << " Event: BATTERY_DISCHARGE" << std::endl;
-        batteryDischarge(event.getParam());
+        drone->batteryDischarge(event.getParam());
+        instantBattery = drone->getBattery();
+        if(instantBattery <= 0) {
+            fallDown();
+        }
         break;
     case EventType::FALL_DOWN:
         std::cout << "Drone: " << drone->getId() << " Event: FALL_DOWN" << std::endl;
@@ -100,10 +105,10 @@ void EventController::goHomeBase() {
 
     drone->setTrjChanged(true);
 }
-
-void EventController::batteryDischarge(int percentage) {
+/*
+void EventController::batteryDischarge(double percentage) {
     sensor_msgs::BatteryState msg = sensor_msgs::BatteryState();
-    msg.percentage = drone->getBattery() - percentage;
+    msg.percentage = percentage;//drone->getBattery() - percentage;
     
     if (msg.percentage <= 0) {
         msg.percentage = 0;
@@ -112,7 +117,7 @@ void EventController::batteryDischarge(int percentage) {
 
     drone->setBattery(msg.percentage);
     battery_pub.publish(msg);
-}
+}*/
 
 void EventController::fallDown() {
     drone->setMotionState(MotionState::FALLING_DOWN_FAILURE);