Skip to content

Commit

Permalink
sys_state: Small cleanup of #1150
Browse files Browse the repository at this point in the history
  • Loading branch information
@vooon
vooon committed Jan 3, 2019
1 parent 42d0a00 commit dd8fa19
Show file tree
Hide file tree
Showing 2 changed files with 83 additions and 71 deletions.
112 changes: 56 additions & 56 deletions mavros/src/plugins/sys_status.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -534,14 +534,34 @@ class SystemStatusPlugin : public plugin::PluginBase
bool has_battery_status;
float battery_voltage;

std::unordered_map<uint16_t,mavros_msgs::VehicleInfo> vehicles;
using M_VehicleInfo = std::unordered_map<uint16_t, mavros_msgs::VehicleInfo>;
M_VehicleInfo vehicles;

/* -*- mid-level helpers -*- */
//Get vehicle key for the unordered map containing all vehicles

// Get vehicle key for the unordered map containing all vehicles
inline uint16_t get_vehicle_key(uint8_t sysid,uint8_t compid) {
uint16_t key = sysid << 8 | compid;
return key;
return sysid << 8 | compid;
}

// Find or create vehicle info
inline M_VehicleInfo::iterator find_or_create_vehicle_info(uint8_t sysid, uint8_t compid) {
key = get_vehicle_key(sysid, compid);
M_VehicleInfo::iterator ret = vehicles.find(key);

if (ret == vehicles.end()) {
// Not found
mavros_msgs::VehicleInfo v;
v.sysid = sysid;
v.compid = compid;
v.available_info = 0;

auto res = vehicles.emplace(key, v); //-> pair<iterator, bool>
ret = res.first;
}

ROS_ASSERT(ret != vehicles.end());
return ret;
}

/**
Expand Down Expand Up @@ -665,35 +685,29 @@ class SystemStatusPlugin : public plugin::PluginBase
{
using mavlink::common::MAV_MODE_FLAG;

//Store generic info of all heartbeats seen
uint16_t key = get_vehicle_key(msg->sysid,msg->compid);
auto it = vehicles.find(key);

if (it == vehicles.end()) {
//Vehicle not found, create a new one
mavros_msgs::VehicleInfo new_vehicle;
new_vehicle.sysid = msg->sysid;
new_vehicle.compid = msg->compid;
auto res = vehicles.emplace(key, new_vehicle); //gives a pair <iterator,bool>
it = res.first;
}
// Store generic info of all heartbeats seen
auto it = find_or_create_vehicle_info(msg->sysid, msg->compid);

ROS_ASSERT(it != vehicles.end());
auto vehicle_mode = m_uas->str_mode_v10(hb.base_mode, hb.custom_mode);
auto stamp = ros::Time::now();

//Existing vehicle found, update data
// Update vehicle data
it->second.header.stamp = stamp;
it->second.available_info |= mavros_msgs::VehicleInfo::HAVE_INFO_HEARTBEAT;
it->second.autopilot = hb.autopilot;
it->second.type = hb.type;
it->second.system_status = hb.system_status;
it->second.base_mode = hb.base_mode;
it->second.custom_mode = hb.custom_mode;
it->second.mode = m_uas->str_mode_v10(hb.base_mode, hb.custom_mode);
it->second.mode = vehicle_mode;

if (!(hb.base_mode & enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED))) {
it->second.mode_id = hb.base_mode;
} else {
it->second.mode_id = hb.custom_mode;
}
//Continue from here only if vehicle is my target

// Continue from here only if vehicle is my target
if (!m_uas->is_my_target(msg->sysid)) {
ROS_DEBUG_NAMED("sys", "HEARTBEAT from [%d, %d] dropped.", msg->sysid, msg->compid);
return;
Expand All @@ -707,11 +721,11 @@ class SystemStatusPlugin : public plugin::PluginBase

// build state message after updating uas
auto state_msg = boost::make_shared<mavros_msgs::State>();
state_msg->header.stamp = ros::Time::now();
state_msg->header.stamp = stamp;
state_msg->connected = true;
state_msg->armed = !!(hb.base_mode & enum_value(MAV_MODE_FLAG::SAFETY_ARMED));
state_msg->guided = !!(hb.base_mode & enum_value(MAV_MODE_FLAG::GUIDED_ENABLED));
state_msg->mode = m_uas->str_mode_v10(hb.base_mode, hb.custom_mode);
state_msg->mode = vehicle_mode;
state_msg->system_status = hb.system_status;

state_pub.publish(state_msg);
Expand Down Expand Up @@ -803,22 +817,12 @@ class SystemStatusPlugin : public plugin::PluginBase
else
process_autopilot_version_normal(apv, msg->sysid, msg->compid);

//Store generic info of all autopilot seen
uint16_t key = get_vehicle_key(msg->sysid,msg->compid);
auto it = vehicles.find(key);

if (it == vehicles.end()) {
//Vehicle not found, create a new one
mavros_msgs::VehicleInfo new_vehicle;
new_vehicle.sysid = msg->sysid;
new_vehicle.compid = msg->compid;
auto res = vehicles.emplace(key, new_vehicle); //gives a pair <iterator,bool>
it = res.first;
}

ROS_ASSERT(it != vehicles.end());
// Store generic info of all autopilot seen
auto it = find_or_create_vehicle_info(msg->sysid, msg->compid);

//Existing vehicle found, update data
// Update vehicle data
it->second.header.stamp = ros::Time::now();
it->second.available_info |= mavros_msgs::VehicleInfo::HAVE_INFO_AUTOPILOT_VERSION;
it->second.capabilities = apv.capabilities;
it->second.flight_sw_version = apv.flight_sw_version;
it->second.middleware_sw_version = apv.middleware_sw_version;
Expand Down Expand Up @@ -990,6 +994,9 @@ class SystemStatusPlugin : public plugin::PluginBase
if (!connected) {
// publish connection change
publish_disconnection();

// Clear known vehicles
vehicles.clear();
}
}

Expand Down Expand Up @@ -1061,41 +1068,34 @@ class SystemStatusPlugin : public plugin::PluginBase
bool vehicle_info_get_cb(mavros_msgs::VehicleInfoGet::Request &req,
mavros_msgs::VehicleInfoGet::Response &res)
{

if(req.get_all){
//Send all vehicles
for (auto &got : vehicles) {
if (req.get_all) {
// Send all vehicles
for (const auto &got : vehicles) {
res.vehicles.emplace_back(got.second);
}
//ROS_INFO_NAMED("sys", "Number of vehicles : %u", (uint8_t)vehicles.size());

res.success = true;
return res.success;
}

uint8_t req_sysid = 0;
uint8_t req_compid = 0;
uint8_t req_sysid = req.sysid;
uint8_t req_compid = req.compid;

if(req.sysid == 0 && req.compid == 0){
//use target
if (req.sysid == 0 && req.compid == 0) {
// use target
req_sysid = m_uas->get_tgt_system();
req_compid = m_uas->get_tgt_component();
} else {
req_sysid = req.sysid;
req_compid = req.compid;
}

uint16_t key = get_vehicle_key(req_sysid,req_compid);
uint16_t key = get_vehicle_key(req_sysid, req_compid);
auto it = vehicles.find(key);

if (it == vehicles.end()) {
//Vehicle not found
// Vehicle not found
res.success = false;
return res.success;
}

ROS_ASSERT(it != vehicles.end());

res.vehicles.emplace_back(it->second);
res.success = true;
return res.success;
Expand Down
42 changes: 27 additions & 15 deletions mavros_msgs/msg/VehicleInfo.msg
View file
Original file line number Diff line number Diff line change
@@ -1,18 +1,30 @@
# Vehicle Info msg
uint8 sysid #SYSTEM ID
uint8 compid #COMPONENT ID
uint8 autopilot #MAV_AUTOPILOT
uint8 type #MAV_TYPE
uint8 system_status #MAV_STATE

Header header

uint8 HAVE_INFO_HEARTBEAT = 1
uint8 HAVE_INFO_AUTOPILOT_VERSION = 2
uint8 available_info # Bitmap shows what info is available

# Vehicle address
uint8 sysid # SYSTEM ID
uint8 compid # COMPONENT ID

# -*- Heartbeat info -*-
uint8 autopilot # MAV_AUTOPILOT
uint8 type # MAV_TYPE
uint8 system_status # MAV_STATE
uint8 base_mode
uint32 custom_mode
string mode #MAV_MODE string
uint32 mode_id #MAV_MODE number
uint64 capabilities #MAV_PROTOCOL_CAPABILITY
uint32 flight_sw_version #Firmware version number
uint32 middleware_sw_version #Middleware version number
uint32 os_sw_version #Operating system version number
uint32 board_version #HW / board version (last 8 bytes should be silicon ID, if any)
uint16 vendor_id #ID of the board vendor
uint16 product_id #ID of the product
uint64 uid #UID if provided by hardware
string mode # MAV_MODE string
uint32 mode_id # MAV_MODE number

# -*- Autopilot version -*-
uint64 capabilities # MAV_PROTOCOL_CAPABILITY
uint32 flight_sw_version # Firmware version number
uint32 middleware_sw_version # Middleware version number
uint32 os_sw_version # Operating system version number
uint32 board_version # HW / board version (last 8 bytes should be silicon ID, if any)
uint16 vendor_id # ID of the board vendor
uint16 product_id # ID of the product
uint64 uid # UID if provided by hardware

0 comments on commit dd8fa19

Please sign in to comment.