diff --git a/include/roscontroller.h b/include/roscontroller.h index 0f2bc2f..75ac1f9 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -12,7 +12,7 @@ #include "mavros_msgs/SetMode.h" #include "mavros_msgs/State.h" #include "mavros_msgs/BatteryStatus.h" -#include "sensor_msgs/BatteryState.h" +//#include "sensor_msgs/BatteryState.h" #include "mavros_msgs/Mavlink.h" #include "mavros_msgs/PositionTarget.h" #include "sensor_msgs/NavSatStatus.h" @@ -240,7 +240,7 @@ private: double gps_r_lat); /*battery status callback */ - void battery(const sensor_msgs::BatteryState::ConstPtr& msg); + void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); /*flight extended status callback*/ void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 071336b..d9aa603 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -352,7 +352,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) m_smTopic_infos.insert(pair(topic, "sensor_msgs/LaserScan")); node_handle.getParam("topics/battery", topic); - m_smTopic_infos.insert(pair(topic, "sensor_msgs/BatteryState")); + m_smTopic_infos.insert(pair(topic, "mavros_msgs/BatteryStatus")); node_handle.getParam("topics/status", topic); m_smTopic_infos.insert(pair(topic, "mavros_msgs/State")); @@ -504,7 +504,7 @@ void roscontroller::Subscribe(ros::NodeHandle& n_c) { flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); } - else if (it->second == "sensor_msgs/BatteryState") + else if (it->second == "mavros_msgs/BatteryStatus") { battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); } @@ -1035,12 +1035,13 @@ void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat)); }; -void roscontroller::battery(const sensor_msgs::BatteryState::ConstPtr& msg) +void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg) /* / Update battery status into BVM from subscriber /------------------------------------------------------*/ { - buzzuav_closures::set_battery(msg->voltage, msg->current, msg->percentage); + // buzzuav_closures::set_battery(msg->voltage, msg->current, msg->percentage); + buzzuav_closures::set_battery(msg->voltage, msg->current, msg->remaining*!00.0); // DEBUG // ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, // msg->current, msg ->remaining);