Added new topic for charge state in mwh

cmu-mars · Apr 24, 2018 · 70199d2 · 70199d2
1 parent c67ebb4
commit 70199d2
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 1 deletion.
diff --git a/src/battery_discharge.cpp b/src/battery_discharge.cpp
@@ -77,6 +77,7 @@ void BatteryPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
     // Publish a topic for motor power and charge level
     this->motor_power = this->rosNode->advertise<kobuki_msgs::MotorPower>("/mobile_base/commands/motor_power", 1);
     this->charge_state = this->rosNode->advertise<std_msgs::Float64>("/mobile_base/commands/charge_level", 1);
+    this->charge_state_mwh = this->rosNode->advertise<std_msgs::Float64>("/mobile_base/commands/charge_level_mwh", 1);
 
     this->set_charging = this->rosNode->advertiseService(this->model->GetName() + "/set_charging", &BatteryPlugin::SetCharging, this);
     this->set_charging_rate = this->rosNode->advertiseService(this->model->GetName() + "/set_charge_rate", &BatteryPlugin::SetChargingRate, this);
@@ -195,10 +196,13 @@ double BatteryPlugin::OnUpdateVoltage(const common::BatteryPtr &_battery)
         this->q = this->c;
     }
 
-    std_msgs::Float64 charge_msg;
+    std_msgs::Float64 charge_msg, charge_msg_mwh;
     charge_msg.data = this->q;
+    charge_msg_mwh.data = this->q * 1000 * this-> et;
+
     lock.lock();
     this->charge_state.publish(charge_msg);
+    this->charge_state_mwh.publish(charge_msg_mwh);
     lock.unlock();
 
     return et;

diff --git a/src/battery_discharge.hh b/src/battery_discharge.hh
@@ -100,6 +100,7 @@ namespace gazebo
     protected: std::unique_ptr<ros::NodeHandle> rosNode;
 
     protected: ros::Publisher charge_state;
+    protected: ros::Publisher charge_state_mwh;
     protected: ros::Publisher motor_power;
 
     protected: ros::ServiceServer set_charging;