7965506471

Committed 19 Feb 2024 09:51PM CUT coverage: 36.22% (+0.008%) from 36.212%

Build # 7965506471

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2223 from mavlink/pr-absl-fix

Fix illegal instruction on RPi 4

Run Details

10035 of 27706 relevant lines covered (36.22%)

127.69 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

0.0

/src/mavsdk/plugins/component_information/component_information_impl.cpp

#include "component_information_impl.h"
#include "callback_list.tpp"

#include <utility>
#include <filesystem>
#include <fstream>
#include <random>
#include <json/json.h>

namespace mavsdk {

namespace fs = std::filesystem;

template class CallbackList<ComponentInformation::FloatParamUpdate>;

ComponentInformationImpl::ComponentInformationImpl(System& system) : PluginImplBase(system)
{
    _system_impl->register_plugin(this);
}

ComponentInformationImpl::ComponentInformationImpl(std::shared_ptr<System> system) :
    PluginImplBase(std::move(system))
{
    _system_impl->register_plugin(this);
}

ComponentInformationImpl::~ComponentInformationImpl()
{
    _system_impl->unregister_plugin(this);
}

void ComponentInformationImpl::init() {}

void ComponentInformationImpl::deinit() {}

void ComponentInformationImpl::enable()
{
    // TODO: iterate through components!

    _system_impl->request_message().request(
        MAVLINK_MSG_ID_COMPONENT_INFORMATION,
        MAV_COMP_ID_PATHPLANNER,
        [this](auto&& result, auto&& message) { receive_component_information(result, message); });
}

void ComponentInformationImpl::disable() {}

void ComponentInformationImpl::receive_component_information(
    MavlinkCommandSender::Result result, const mavlink_message_t& message)
{
    if (result != MavlinkCommandSender::Result::Success) {
        LogWarn() << "Requesting component information failed with " << static_cast<int>(result);
        return;
    }

    mavlink_component_information_t component_information;
    mavlink_msg_component_information_decode(&message, &component_information);

    component_information
        .general_metadata_uri[sizeof(component_information.general_metadata_uri) - 1] = '\0';
    const auto general_metadata_uri = std::string(component_information.general_metadata_uri);

    download_file_async(
        general_metadata_uri, [this](std::string path) { parse_metadata_file(path); });
}

void ComponentInformationImpl::download_file_async(
    const std::string& uri, std::function<void(std::string path)> callback)
{
    // TODO: check CRC

    if (uri.empty()) {
        LogErr() << "No component information URI provided";
        return;

    } else if (uri.find("mftp://") == 0) {
        LogDebug() << "Found mftp URI, using MAVLink FTP to download file";

        const auto path = uri.substr(strlen("mftp://"));

        const auto maybe_tmp_path = create_tmp_directory("mavsdk-component-information-tmp-files");
        const auto path_to_download = maybe_tmp_path ? maybe_tmp_path.value() : "./";

        _system_impl->mavlink_ftp_client().download_async(
            path,
            path_to_download,
            false, // Don't use burst for now
            [path_to_download, callback, path](
                MavlinkFtpClient::ClientResult download_result,
                MavlinkFtpClient::ProgressData progress_data) {
                if (download_result == MavlinkFtpClient::ClientResult::Next) {
                    LogDebug() << "File download progress: " << progress_data.bytes_transferred
                               << '/' << progress_data.total_bytes;
                } else {
                    LogDebug() << "File download ended with result " << download_result;
                    if (download_result == MavlinkFtpClient::ClientResult::Success) {
                        LogDebug() << "Received file " << path_to_download + "/" + path;
                        callback(path_to_download + "/" + path);
                    }
                }
            });
    } else if (uri.find("http://") == 0 || uri.find("https://") == 0) {
        LogWarn() << "Download using http(s) not implemented yet";
    } else {
        LogWarn() << "Unknown URI protocol";
    }
}

void ComponentInformationImpl::parse_metadata_file(const std::string& path)
{
    std::ifstream f(path);
    if (f.bad()) {
        LogErr() << "Could not open json metadata file.";
        return;
    }

    Json::Value metadata;
    f >> metadata;

    if (!metadata.isMember("version")) {
        LogErr() << "version not found";
        return;
    }

    if (metadata["version"].asInt() != 1) {
        LogWarn() << "version " << metadata["version"].asInt() << " not supported";
    }

    if (!metadata.isMember("metadataTypes")) {
        LogErr() << "metadataTypes not found";
        return;
    }

    for (auto& metadata_type : metadata["metadataTypes"]) {
        if (!metadata_type.isMember("type")) {
            LogErr() << "type missing";
            return;
        }
        if (!metadata_type.isMember("uri")) {
            LogErr() << "uri missing";
            return;
        }

        if (metadata_type["type"].asInt() == COMP_METADATA_TYPE_PARAMETER) {
            download_file_async(
                metadata_type["uri"].asString(), [this](const std::string& parameter_file_path) {
                    LogDebug() << "Found parameter file at: " << parameter_file_path;
                    parse_parameter_file(parameter_file_path);
                });
        }
    }
}

void ComponentInformationImpl::parse_parameter_file(const std::string& path)
{
    std::ifstream f(path);
    if (f.bad()) {
        LogErr() << "Could not open json parameter file.";
        return;
    }

    Json::Value parameters;
    f >> parameters;

    if (!parameters.isMember("version")) {
        LogErr() << "version not found";
        return;
    }

    if (parameters["version"].asInt() != 1) {
        LogWarn() << "version " << parameters["version"].asInt() << " not supported";
    }

    if (!parameters.isMember("parameters")) {
        LogErr() << "parameters not found";
        return;
    }

    std::lock_guard<std::mutex> lock(_mutex);
    _float_params.clear();

    for (auto& param : parameters["parameters"]) {
        if (!param.isMember("type")) {
            LogErr() << "type not found";
            return;
        }

        if (param["type"].asString() == "Float") {
            _float_params.push_back(ComponentInformation::FloatParam{
                param["name"].asString(),
                param["shortDesc"].asString(),
                param["longDesc"].asString(),
                param["units"].asString(),
                param["decimalPlaces"].asInt(),
                NAN,
                param["default"].asFloat(),
                param["min"].asFloat(),
                param["max"].asFloat()});

            const auto name = param["name"].asString();

            _system_impl->get_param_float_async(
                name,
                [this, name](MavlinkParameterClient::Result result, float value) {
                    get_float_param_result(name, result, value);
                },
                this);

            _system_impl->subscribe_param_float(
                name, [this, name](float value) { param_update(name, value); }, this);

        } else {
            LogWarn() << "Ignoring type " << param["type"].asString() << " for now.";
        }
    }
}

void ComponentInformationImpl::get_float_param_result(
    const std::string& name, MavlinkParameterClient::Result result, float value)
{
    if (result != MavlinkParameterClient::Result::Success) {
        LogWarn() << "Getting float param result: " << static_cast<int>(result);
        return;
    }

    std::lock_guard<std::mutex> lock(_mutex);
    for (auto& param : _float_params) {
        if (param.name == name) {
            param.start_value = value;
            LogDebug() << "Received value " << value << " for " << name;
        }
    }
}

void ComponentInformationImpl::param_update(const std::string& name, float new_value)
{
    std::lock_guard<std::mutex> lock(_mutex);

    for (auto& param : _float_params) {
        if (param.name == name) {
            param.start_value = new_value;
            LogDebug() << "Received value " << new_value << " for " << name;
        }
    }

    const auto param_update = ComponentInformation::FloatParamUpdate{name, new_value};

    _float_param_update_callbacks.queue(
        param_update, [this](const auto& func) { _system_impl->call_user_callback(func); });
}

std::pair<ComponentInformation::Result, std::vector<ComponentInformation::FloatParam>>
ComponentInformationImpl::access_float_params()
{
    return {ComponentInformation::Result::Success, _float_params};
}

ComponentInformation::FloatParamHandle ComponentInformationImpl::subscribe_float_param(
    const ComponentInformation::FloatParamCallback& callback)
{
    std::lock_guard<std::mutex> lock(_mutex);
    return _float_param_update_callbacks.subscribe(callback);
}

void ComponentInformationImpl::unsubscribe_float_param(
    ComponentInformation::FloatParamHandle handle)
{
    std::lock_guard<std::mutex> lock(_mutex);
    _float_param_update_callbacks.unsubscribe(handle);
}

std::optional<std::string> ComponentInformationImpl::create_tmp_directory(const std::string& prefix)
{
    // Inspired by https://stackoverflow.com/a/58454949/8548472
    const auto tmp_dir = fs::temp_directory_path();

    std::random_device dev;
    std::mt19937 prng(dev());
    std::uniform_int_distribution<uint32_t> rand(0);

    static constexpr unsigned max_tries = 100;

    for (unsigned i = 0; i < max_tries; ++i) {
        std::stringstream ss;
        ss << prefix << '-' << std::hex << rand(prng);
        auto path = tmp_dir / ss.str();

        const auto created = fs::create_directory(path);
        if (created) {
            return {path.string()};
        }
    }

    LogErr() << "Could not create a temporary directory, aborting.";
    return {};
}

} // namespace mavsdk

1	#include "component_information_impl.h"
2	#include "callback_list.tpp"
3
4	#include <utility>
5	#include <filesystem>
6	#include <fstream>
7	#include <random>
8	#include <json/json.h>
9
10	namespace mavsdk {
11
12	namespace fs = std::filesystem;
13
14	template class CallbackList<ComponentInformation::FloatParamUpdate>;
15
16	ComponentInformationImpl::ComponentInformationImpl(System& system) : PluginImplBase(system)	×
17	{
18	_system_impl->register_plugin(this);	×
19	}	×
20
21	ComponentInformationImpl::ComponentInformationImpl(std::shared_ptr<System> system) :	×
22	PluginImplBase(std::move(system))	×
23	{
24	_system_impl->register_plugin(this);	×
25	}	×
26
27	ComponentInformationImpl::~ComponentInformationImpl()	×
28	{
29	_system_impl->unregister_plugin(this);	×
30	}	×
31
32	void ComponentInformationImpl::init() {}	×
33
34	void ComponentInformationImpl::deinit() {}	×
35
36	void ComponentInformationImpl::enable()	×
37	{
38	// TODO: iterate through components!
39
40	_system_impl->request_message().request(	×
41	MAVLINK_MSG_ID_COMPONENT_INFORMATION,
42	MAV_COMP_ID_PATHPLANNER,
43	[this](auto&& result, auto&& message) { receive_component_information(result, message); });	×
44	}	×
45
46	void ComponentInformationImpl::disable() {}	×
47
48	void ComponentInformationImpl::receive_component_information(	×
49	MavlinkCommandSender::Result result, const mavlink_message_t& message)
50	{
51	if (result != MavlinkCommandSender::Result::Success) {	×
52	LogWarn() << "Requesting component information failed with " << static_cast<int>(result);	×
53	return;	×
54	}
55
56	mavlink_component_information_t component_information;	×
57	mavlink_msg_component_information_decode(&message, &component_information);	×
58
59	component_information
60	.general_metadata_uri[sizeof(component_information.general_metadata_uri) - 1] = '\0';	×
61	const auto general_metadata_uri = std::string(component_information.general_metadata_uri);	×
62
63	download_file_async(	×
64	general_metadata_uri, [this](std::string path) { parse_metadata_file(path); });	×
65	}
66
67	void ComponentInformationImpl::download_file_async(	×
68	const std::string& uri, std::function<void(std::string path)> callback)
69	{
70	// TODO: check CRC
71
72	if (uri.empty()) {	×
73	LogErr() << "No component information URI provided";	×
74	return;	×
75
76	} else if (uri.find("mftp://") == 0) {	×
77	LogDebug() << "Found mftp URI, using MAVLink FTP to download file";	×
78
79	const auto path = uri.substr(strlen("mftp://"));	×
80
81	const auto maybe_tmp_path = create_tmp_directory("mavsdk-component-information-tmp-files");	×
82	const auto path_to_download = maybe_tmp_path ? maybe_tmp_path.value() : "./";	×
83
84	_system_impl->mavlink_ftp_client().download_async(	×
85	path,
86	path_to_download,
87	false, // Don't use burst for now
88	[path_to_download, callback, path](	×
89	MavlinkFtpClient::ClientResult download_result,
90	MavlinkFtpClient::ProgressData progress_data) {
91	if (download_result == MavlinkFtpClient::ClientResult::Next) {	×
92	LogDebug() << "File download progress: " << progress_data.bytes_transferred	×
93	<< '/' << progress_data.total_bytes;	×
94	} else {
95	LogDebug() << "File download ended with result " << download_result;	×
96	if (download_result == MavlinkFtpClient::ClientResult::Success) {	×
97	LogDebug() << "Received file " << path_to_download + "/" + path;	×
98	callback(path_to_download + "/" + path);	×
99	}
100	}
101	});	×
102	} else if (uri.find("http://") == 0 \|\| uri.find("https://") == 0) {	×
103	LogWarn() << "Download using http(s) not implemented yet";	×
104	} else {
105	LogWarn() << "Unknown URI protocol";	×
106	}
107	}
108
109	void ComponentInformationImpl::parse_metadata_file(const std::string& path)	×
110	{
111	std::ifstream f(path);	×
112	if (f.bad()) {	×
113	LogErr() << "Could not open json metadata file.";	×
114	return;	×
115	}
116
117	Json::Value metadata;	×
118	f >> metadata;	×
119
120	if (!metadata.isMember("version")) {	×
121	LogErr() << "version not found";	×
122	return;	×
123	}
124
125	if (metadata["version"].asInt() != 1) {	×
126	LogWarn() << "version " << metadata["version"].asInt() << " not supported";	×
127	}
128
129	if (!metadata.isMember("metadataTypes")) {	×
130	LogErr() << "metadataTypes not found";	×
131	return;	×
132	}
133
134	for (auto& metadata_type : metadata["metadataTypes"]) {	×
135	if (!metadata_type.isMember("type")) {	×
136	LogErr() << "type missing";	×
137	return;	×
138	}
139	if (!metadata_type.isMember("uri")) {	×
140	LogErr() << "uri missing";	×
141	return;	×
142	}
143
144	if (metadata_type["type"].asInt() == COMP_METADATA_TYPE_PARAMETER) {	×
145	download_file_async(	×
146	metadata_type["uri"].asString(), [this](const std::string& parameter_file_path) {	×
147	LogDebug() << "Found parameter file at: " << parameter_file_path;	×
148	parse_parameter_file(parameter_file_path);	×
149	});	×
150	}
151	}
152	}
153
154	void ComponentInformationImpl::parse_parameter_file(const std::string& path)	×
155	{
156	std::ifstream f(path);	×
157	if (f.bad()) {	×
158	LogErr() << "Could not open json parameter file.";	×
159	return;	×
160	}
161
162	Json::Value parameters;	×
163	f >> parameters;	×
164
165	if (!parameters.isMember("version")) {	×
166	LogErr() << "version not found";	×
167	return;	×
168	}
169
170	if (parameters["version"].asInt() != 1) {	×
171	LogWarn() << "version " << parameters["version"].asInt() << " not supported";	×
172	}
173
174	if (!parameters.isMember("parameters")) {	×
175	LogErr() << "parameters not found";	×
176	return;	×
177	}
178
179	std::lock_guard<std::mutex> lock(_mutex);	×
180	_float_params.clear();	×
181
182	for (auto& param : parameters["parameters"]) {	×
183	if (!param.isMember("type")) {	×
184	LogErr() << "type not found";	×
185	return;	×
186	}
187
188	if (param["type"].asString() == "Float") {	×
189	_float_params.push_back(ComponentInformation::FloatParam{	×
190	param["name"].asString(),	×
191	param["shortDesc"].asString(),	×
192	param["longDesc"].asString(),	×
193	param["units"].asString(),	×
194	param["decimalPlaces"].asInt(),	×
195	NAN,
196	param["default"].asFloat(),	×
197	param["min"].asFloat(),	×
198	param["max"].asFloat()});	×
199
200	const auto name = param["name"].asString();	×
201
202	_system_impl->get_param_float_async(	×
203	name,
204	[this, name](MavlinkParameterClient::Result result, float value) {	×
205	get_float_param_result(name, result, value);	×
206	},	×
207	this);
208
209	_system_impl->subscribe_param_float(	×
210	name, [this, name](float value) { param_update(name, value); }, this);	×
211
212	} else {
213	LogWarn() << "Ignoring type " << param["type"].asString() << " for now.";	×
214	}
215	}
216	}
217
218	void ComponentInformationImpl::get_float_param_result(	×
219	const std::string& name, MavlinkParameterClient::Result result, float value)
220	{
221	if (result != MavlinkParameterClient::Result::Success) {	×
222	LogWarn() << "Getting float param result: " << static_cast<int>(result);	×
223	return;	×
224	}
225
226	std::lock_guard<std::mutex> lock(_mutex);	×
227	for (auto& param : _float_params) {	×
228	if (param.name == name) {	×
229	param.start_value = value;	×
230	LogDebug() << "Received value " << value << " for " << name;	×
231	}
232	}
233	}
234
235	void ComponentInformationImpl::param_update(const std::string& name, float new_value)	×
236	{
237	std::lock_guard<std::mutex> lock(_mutex);	×
238
239	for (auto& param : _float_params) {	×
240	if (param.name == name) {	×
241	param.start_value = new_value;	×
242	LogDebug() << "Received value " << new_value << " for " << name;	×
243	}
244	}
245
246	const auto param_update = ComponentInformation::FloatParamUpdate{name, new_value};	×
247
248	_float_param_update_callbacks.queue(	×
249	param_update, [this](const auto& func) { _system_impl->call_user_callback(func); });	×
250	}	×
251
252	std::pair<ComponentInformation::Result, std::vector<ComponentInformation::FloatParam>>
253	ComponentInformationImpl::access_float_params()	×
254	{
255	return {ComponentInformation::Result::Success, _float_params};	×
256	}
257
258	ComponentInformation::FloatParamHandle ComponentInformationImpl::subscribe_float_param(	×
259	const ComponentInformation::FloatParamCallback& callback)
260	{
261	std::lock_guard<std::mutex> lock(_mutex);	×
262	return _float_param_update_callbacks.subscribe(callback);	×
263	}
264
265	void ComponentInformationImpl::unsubscribe_float_param(	×
266	ComponentInformation::FloatParamHandle handle)
267	{
268	std::lock_guard<std::mutex> lock(_mutex);	×
269	_float_param_update_callbacks.unsubscribe(handle);	×
270	}	×
271
272	std::optional<std::string> ComponentInformationImpl::create_tmp_directory(const std::string& prefix)	×
273	{
274	// Inspired by https://stackoverflow.com/a/58454949/8548472
275	const auto tmp_dir = fs::temp_directory_path();	×
276
277	std::random_device dev;	×
278	std::mt19937 prng(dev());	×
279	std::uniform_int_distribution<uint32_t> rand(0);	×
280
281	static constexpr unsigned max_tries = 100;
282
283	for (unsigned i = 0; i < max_tries; ++i) {	×
284	std::stringstream ss;	×
285	ss << prefix << '-' << std::hex << rand(prng);	×
286	auto path = tmp_dir / ss.str();	×
287
288	const auto created = fs::create_directory(path);	×
289	if (created) {	×
290	return {path.string()};	×
291	}
292	}
293
294	LogErr() << "Could not create a temporary directory, aborting.";	×
295	return {};	×
296	}
297
298	} // namespace mavsdk

mavlink / MAVSDK / 7965506471

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous