-
Notifications
You must be signed in to change notification settings - Fork 990
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
VehicleInfo : add srv into sys_status plugin to request vehicule basic info #1150
Changes from all commits
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -22,6 +22,9 @@ | |
#include <mavros_msgs/SetMode.h> | ||
#include <mavros_msgs/CommandLong.h> | ||
#include <mavros_msgs/StatusText.h> | ||
#include <mavros_msgs/VehicleInfo.h> | ||
#include <mavros_msgs/VehicleInfoGet.h> | ||
|
||
|
||
#ifdef HAVE_SENSOR_MSGS_BATTERYSTATE_MSG | ||
#include <sensor_msgs/BatteryState.h> | ||
|
@@ -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,14 +513,23 @@ 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; | ||
bool disable_diag; | ||
bool has_battery_status; | ||
float battery_voltage; | ||
|
||
std::unordered_map<uint16_t,mavros_msgs::VehicleInfo> 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 <iterator,bool> | ||
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 <iterator,bool> | ||
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); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Perhaps here no difference between |
||
} | ||
//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 | ||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I don't sure that this field is needed. |
||
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 |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
That search & create may be moved to common function.