From 1a48fc03acfd5c96a859ac6285e82b1c7a9120f8 Mon Sep 17 00:00:00 2001 From: Gregoire Linard Date: Thu, 3 Jan 2019 00:14:21 +0100 Subject: [PATCH] VehicleInfo : add srv into sys_status plugin to request basic info from vehicle --- mavros/src/plugins/sys_status.cpp | 110 +++++++++++++++++++++++++++++ mavros_msgs/CMakeLists.txt | 2 + mavros_msgs/msg/VehicleInfo.msg | 18 +++++ mavros_msgs/srv/VehicleInfoGet.srv | 14 ++++ 4 files changed, 144 insertions(+) create mode 100644 mavros_msgs/msg/VehicleInfo.msg create mode 100644 mavros_msgs/srv/VehicleInfoGet.srv diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp index eba823413..db6c1f48e 100644 --- a/mavros/src/plugins/sys_status.cpp +++ b/mavros/src/plugins/sys_status.cpp @@ -22,6 +22,9 @@ #include #include #include +#include +#include + #ifdef HAVE_SENSOR_MSGS_BATTERYSTATE_MSG #include @@ -471,6 +474,7 @@ class SystemStatusPlugin : public plugin::PluginBase statustext_sub = nh.subscribe("statustext/send", 10, &SystemStatusPlugin::statustext_cb, this); rate_srv = nh.advertiseService("set_stream_rate", &SystemStatusPlugin::set_rate_cb, this); mode_srv = nh.advertiseService("set_mode", &SystemStatusPlugin::set_mode_cb, this); + vehicle_info_get_srv = nh.advertiseService("vehicle_info_get", &SystemStatusPlugin::vehicle_info_get_cb, this); // init state topic publish_disconnection(); @@ -509,6 +513,7 @@ class SystemStatusPlugin : public plugin::PluginBase ros::Subscriber statustext_sub; ros::ServiceServer rate_srv; ros::ServiceServer mode_srv; + ros::ServiceServer vehicle_info_get_srv; static constexpr int RETRIES_COUNT = 6; int version_retries; @@ -516,7 +521,15 @@ class SystemStatusPlugin : public plugin::PluginBase bool has_battery_status; float battery_voltage; + std::unordered_map vehicles; + /* -*- mid-level helpers -*- */ + + //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; + } /** * Sent STATUSTEXT message to rosout @@ -639,6 +652,35 @@ 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 + it = res.first; + } + + ROS_ASSERT(it != vehicles.end()); + + //Existing vehicle found, update data + 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); + 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 if (!m_uas->is_my_target(msg->sysid)) { ROS_DEBUG_NAMED("sys", "HEARTBEAT from [%d, %d] dropped.", msg->sysid, msg->compid); return; @@ -747,6 +789,31 @@ class SystemStatusPlugin : public plugin::PluginBase process_autopilot_version_apm_quirk(apv, msg->sysid, msg->compid); 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 + it = res.first; + } + + ROS_ASSERT(it != vehicles.end()); + + //Existing vehicle found, update data + it->second.capabilities = apv.capabilities; + it->second.flight_sw_version = apv.flight_sw_version; + it->second.middleware_sw_version = apv.middleware_sw_version; + it->second.os_sw_version = apv.os_sw_version; + it->second.board_version = apv.board_version; + it->second.vendor_id = apv.vendor_id; + it->second.product_id = apv.product_id; + it->second.uid = apv.uid; } void handle_battery_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::BATTERY_STATUS &bs) @@ -977,6 +1044,49 @@ class SystemStatusPlugin : public plugin::PluginBase res.mode_sent = true; return true; } + + 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) { + 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; + + 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); + auto it = vehicles.find(key); + + if (it == vehicles.end()) { + //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; + } }; } // namespace std_plugins } // namespace mavros diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt index 1cbce40b9..12d5a667b 100644 --- a/mavros_msgs/CMakeLists.txt +++ b/mavros_msgs/CMakeLists.txt @@ -39,6 +39,7 @@ add_message_files( Thrust.msg VFR_HUD.msg Vibration.msg + VehicleInfo.msg Waypoint.msg WaypointList.msg WaypointReached.msg @@ -71,6 +72,7 @@ add_service_files( SetMavFrame.srv SetMode.srv StreamRate.srv + VehicleInfoGet.srv WaypointClear.srv WaypointPull.srv WaypointPush.srv diff --git a/mavros_msgs/msg/VehicleInfo.msg b/mavros_msgs/msg/VehicleInfo.msg new file mode 100644 index 000000000..28c318a2a --- /dev/null +++ b/mavros_msgs/msg/VehicleInfo.msg @@ -0,0 +1,18 @@ +# Vehicle Info msg +uint8 sysid #SYSTEM ID +uint8 compid #COMPONENT ID +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 \ No newline at end of file diff --git a/mavros_msgs/srv/VehicleInfoGet.srv b/mavros_msgs/srv/VehicleInfoGet.srv new file mode 100644 index 000000000..9e3223649 --- /dev/null +++ b/mavros_msgs/srv/VehicleInfoGet.srv @@ -0,0 +1,14 @@ +# Request the Vehicle Info +# use this to request the current target sysid / compid defined in mavros +# set get_all = True to request all available vehicles + +uint8 GET_MY_SYSID = 0 +uint8 GET_MY_COMPID = 0 + +uint8 sysid +uint8 compid +bool get_all +--- +bool success +mavros_msgs/VehicleInfo[] vehicles +