ESC Telemetry plugin: add diagnostics support #1705

amilcarlucas · 2022-02-15T10:28:32Z

This create separated diagnostics for:

voltage
current
rpm
temperature
messages (stale telemetry)

Should I instead create one diagnostic per ESC?

vooon

I would rather having separate diagnostics for each ESC than each value.
Config part looks ugly. You can use private node handle to shorten paths.

auto pnh = ros::NodeHandle(ros::NodeHandle("~esc_telemetry"), "diagnostics")

pnh.param("enabled", diag_enabled, false);
...

vooon · 2022-02-16T17:03:55Z

Also you can move limit init to custom diag class.

amilcarlucas · 2022-02-16T18:16:29Z

I would rather having separate diagnostics for each ESC than each value.

I will work on that than.

you can move limit init to custom diag class.

Will that not create a bunch independent of limits per ESC? That is a bit too much "configurability"!

vooon · 2022-02-16T18:56:47Z

Will that not create a bunch independent of limits per ESC?

Not necessary to use different names. E.g.

struct Limits {

double min_x;

explicit Limits(ros::NodeHandle &pnh) {
  pnh.param("min_x", this->min_x, 1.0);
  ...
}
};

class Diag() {
  Limits lim;
};

amilcarlucas · 2022-02-16T20:28:59Z

I did some of the changes you requested, but I think I did not use it the way you planned. Can you take a look at it?

mavros_extras/src/plugins/esc_telemetry.cpp

vooon · 2022-02-16T21:11:33Z

mavros_extras/src/plugins/esc_telemetry.cpp

+			pnh.param("curr_max/nok", this->_curr_max_nok, 10.0f);
+			pnh.param("curr_max/ok", this->_curr_max_ok, 8.0f);
+			pnh.param("rpm_max/nok", this->_rpm_max_nok, 12000);
+			pnh.param("rpm_max/ok", this->_rpm_max_ok, 9000);


Perhaps i'd generate that list by cog.

// [[[cog: // fields = [ // ("temp", (1.0, 0.0), (85.0, 90.0), ), // ... // ] // // for field, min_, max_ = range fields: // for fm, lim = zip(("min", "max"), (min_, max_)): // for fn, l = zip(("ok", "nok"), lim): // cog.outl(f"""pnh.param("{field}_{fm}/{fn}", this->{field}_{fm}_{fn}, {l});""") // ]]]

done, now I just need to find out how to run the cog compiler

cog -cr <file>

https://github.com/mavlink/mavros/blob/ros2/cog-requirements.txt

Ah, sorry, please use for _ in fields. = range is a golangism.

Fixed it, but now it has other issues :(

mavros_extras/src/plugins/esc_telemetry.cpp

mavros_msgs/msg/ESCTelemetry.msg

vooon · 2022-02-17T09:15:12Z

mavros_extras/src/plugins/esc_telemetry.cpp

+	int rpm_min_nok;
+	int rpm_min_ok;
+	int rpm_max_nok;
+	int rpm_max_ok;


We can generate most of that fields by cog also. Will need to move fields list and extend it to have type.

vooon · 2022-02-17T09:19:23Z

mavros_extras/src/plugins/esc_telemetry.cpp

+		lim(lim_)
+	{}
+
+	const Limits& lim;


Ah no, we can't really use const ref on temporary object. https://stackoverflow.com/questions/15513734/const-reference-as-class-member

Try to add limits object to the plugin class and pass it to diags.

vooon · 2022-02-17T14:43:38Z

@amilcarlucas could you please post gcc error here?

amilcarlucas · 2022-02-17T16:46:22Z

It is deep and ugly:

/usr/include/c++/7/ext/new_allocator.h: In instantiation of ‘void __gnu_cxx::new_allocator<_Tp>::construct(_Up*, _Args&& ...) [with _Up = mavros::extra_plugins::ESCDiag; _Args = {std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >}; _Tp = mavros::extra_plugins::ESCDiag]’:
/usr/include/c++/7/bits/alloc_traits.h:475:4:   required from ‘static void std::allocator_traits<std::allocator<_CharT> >::construct(std::allocator_traits<std::allocator<_CharT> >::allocator_type&, _Up*, _Args&& ...) [with _Up = mavros::extra_plugins::ESCDiag; _Args = {std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >}; _Tp = mavros::extra_plugins::ESCDiag; std::allocator_traits<std::allocator<_CharT> >::allocator_type = std::allocator<mavros::extra_plugins::ESCDiag>]’
/usr/include/c++/7/bits/vector.tcc:100:30:   required from ‘void std::vector<_Tp, _Alloc>::emplace_back(_Args&& ...) [with _Args = {std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >}; _Tp = mavros::extra_plugins::ESCDiag; _Alloc = std::allocator<mavros::extra_plugins::ESCDiag>]’
..../mavros/mavros_extras/src/plugins/esc_telemetry.cpp:252:44:   required from here
/usr/include/c++/7/ext/new_allocator.h:136:4: error: invalid new-expression of abstract class type ‘mavros::extra_plugins::ESCDiag’
  { ::new((void *)__p) _Up(std::forward<_Args>(__args)...); }
    ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
..../mavros/mavros_extras/src/plugins/esc_telemetry.cpp:101:7: note:   because the following virtual functions are pure within ‘mavros::extra_plugins::ESCDiag’:
 class ESCDiag : public diagnostic_updater::DiagnosticTask
       ^~~~~~~
In file included from ..../mavros/mavros/include/mavros/mavros_plugin.h:23:0,
                 from ..../mavros/mavros_extras/src/plugins/esc_telemetry.cpp:21:
/opt/ros/melodic/include/diagnostic_updater/diagnostic_updater.h:85:20: note:   virtual void diagnostic_updater::DiagnosticTask::run(diagnostic_updater::DiagnosticStatusWrapper&)
       virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat) = 0;
                    ^~~
In file included from /usr/include/c++/7/vector:62:0,
                 from ..../mavros/mavros/include/mavros/mavros_plugin.h:21,
                 from ..../mavros/mavros_extras/src/plugins/esc_telemetry.cpp:21:
/usr/include/c++/7/bits/stl_construct.h: In instantiation of ‘void std::_Construct(_T1*, _Args&& ...) [with _T1 = mavros::extra_plugins::ESCDiag; _Args = {mavros::extra_plugins::ESCDiag}]’:
/usr/include/c++/7/bits/stl_uninitialized.h:83:18:   required from ‘static _ForwardIterator std::__uninitialized_copy<_TrivialValueTypes>::__uninit_copy(_InputIterator, _InputIterator, _ForwardIterator) [with _InputIterator = std::move_iterator<mavros::extra_plugins::ESCDiag*>; _ForwardIterator = mavros::extra_plugins::ESCDiag*; bool _TrivialValueTypes = false]’
/usr/include/c++/7/bits/stl_uninitialized.h:134:15:   required from ‘_ForwardIterator std::uninitialized_copy(_InputIterator, _InputIterator, _ForwardIterator) [with _InputIterator = std::move_iterator<mavros::extra_plugins::ESCDiag*>; _ForwardIterator = mavros::extra_plugins::ESCDiag*]’
/usr/include/c++/7/bits/stl_uninitialized.h:289:37:   required from ‘_ForwardIterator std::__uninitialized_copy_a(_InputIterator, _InputIterator, _ForwardIterator, std::allocator<_Tp>&) [with _InputIterator = std::move_iterator<mavros::extra_plugins::ESCDiag*>; _ForwardIterator = mavros::extra_plugins::ESCDiag*; _Tp = mavros::extra_plugins::ESCDiag]’
/usr/include/c++/7/bits/stl_uninitialized.h:311:2:   required from ‘_ForwardIterator std::__uninitialized_move_if_noexcept_a(_InputIterator, _InputIterator, _ForwardIterator, _Allocator&) [with _InputIterator = mavros::extra_plugins::ESCDiag*; _ForwardIterator = mavros::extra_plugins::ESCDiag*; _Allocator = std::allocator<mavros::extra_plugins::ESCDiag>]’
/usr/include/c++/7/bits/vector.tcc:426:6:   required from ‘void std::vector<_Tp, _Alloc>::_M_realloc_insert(std::vector<_Tp, _Alloc>::iterator, _Args&& ...) [with _Args = {std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >}; _Tp = mavros::extra_plugins::ESCDiag; _Alloc = std::allocator<mavros::extra_plugins::ESCDiag>; std::vector<_Tp, _Alloc>::iterator = __gnu_cxx::__normal_iterator<mavros::extra_plugins::ESCDiag*, std::vector<mavros::extra_plugins::ESCDiag> >; typename std::_Vector_base<_Tp, _Alloc>::pointer = mavros::extra_plugins::ESCDiag*]’
/usr/include/c++/7/bits/vector.tcc:105:21:   required from ‘void std::vector<_Tp, _Alloc>::emplace_back(_Args&& ...) [with _Args = {std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >}; _Tp = mavros::extra_plugins::ESCDiag; _Alloc = std::allocator<mavros::extra_plugins::ESCDiag>]’
..../mavros/mavros_extras/src/plugins/esc_telemetry.cpp:252:44:   required from here
/usr/include/c++/7/bits/stl_construct.h:75:7: error: invalid new-expression of abstract class type ‘mavros::extra_plugins::ESCDiag’
     { ::new(static_cast<void*>(__p)) _T1(std::forward<_Args>(__args)...); }
       ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /usr/include/c++/7/functional:58:0,
                 from ..../mavros/mavros/include/mavros/mavros_plugin.h:22,
                 from ..../mavros/mavros_extras/src/plugins/esc_telemetry.cpp:21:
/usr/include/c++/7/bits/std_function.h:685:7: error: ‘std::function<_Res(_ArgTypes ...)>::function(_Functor) [with _Functor = mavros::plugin::PluginBase::make_handler(void (_C::*)(const mavlink_message_t*, _T&)) [with _C = mavros::extra_plugins::ESCTelemetryPlugin; _T = mavlink::ardupilotmega::msg::ESC_TELEMETRY_1_TO_4; mavros::plugin::PluginBase::HandlerInfo = std::tuple<unsigned int, const char*, long unsigned int, std::function<void(const mavlink::__mavlink_message*, mavconn::Framing)> >; mavlink::mavlink_message_t = mavlink::__mavlink_message]::<lambda(const mavlink_message_t*, mavconn::Framing)>; <template-parameter-2-2> = void; <template-parameter-2-3> = void; _Res = void; _ArgTypes = {const mavlink::__mavlink_message*, mavconn::Framing}]’, declared using local type ‘mavros::plugin::PluginBase::make_handler(void (_C::*)(const mavlink_message_t*, _T&)) [with _C = mavros::extra_plugins::ESCTelemetryPlugin; _T = mavlink::ardupilotmega::msg::ESC_TELEMETRY_1_TO_4; mavros::plugin::PluginBase::HandlerInfo = std::tuple<unsigned int, const char*, long unsigned int, std::function<void(const mavlink::__mavlink_message*, mavconn::Framing)> >; mavlink::mavlink_message_t = mavlink::__mavlink_message]::<lambda(const mavlink_message_t*, mavconn::Framing)>’, is used but never defined [-fpermissive]
       function<_Res(_ArgTypes...)>::
       ^~~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/include/c++/7/bits/std_function.h:685:7: error: ‘std::function<_Res(_ArgTypes ...)>::function(_Functor) [with _Functor = mavros::plugin::PluginBase::make_handler(void (_C::*)(const mavlink_message_t*, _T&)) [with _C = mavros::extra_plugins::ESCTelemetryPlugin; _T = mavlink::ardupilotmega::msg::ESC_TELEMETRY_5_TO_8; mavros::plugin::PluginBase::HandlerInfo = std::tuple<unsigned int, const char*, long unsigned int, std::function<void(const mavlink::__mavlink_message*, mavconn::Framing)> >; mavlink::mavlink_message_t = mavlink::__mavlink_message]::<lambda(const mavlink_message_t*, mavconn::Framing)>; <template-parameter-2-2> = void; <template-parameter-2-3> = void; _Res = void; _ArgTypes = {const mavlink::__mavlink_message*, mavconn::Framing}]’, declared using local type ‘mavros::plugin::PluginBase::make_handler(void (_C::*)(const mavlink_message_t*, _T&)) [with _C = mavros::extra_plugins::ESCTelemetryPlugin; _T = mavlink::ardupilotmega::msg::ESC_TELEMETRY_5_TO_8; mavros::plugin::PluginBase::HandlerInfo = std::tuple<unsigned int, const char*, long unsigned int, std::function<void(const mavlink::__mavlink_message*, mavconn::Framing)> >; mavlink::mavlink_message_t = mavlink::__mavlink_message]::<lambda(const mavlink_message_t*, mavconn::Framing)>’, is used but never defined [-fpermissive]
/usr/include/c++/7/bits/std_function.h:685:7: error: ‘std::function<_Res(_ArgTypes ...)>::function(_Functor) [with _Functor = mavros::plugin::PluginBase::make_handler(void (_C::*)(const mavlink_message_t*, _T&)) [with _C = mavros::extra_plugins::ESCTelemetryPlugin; _T = mavlink::ardupilotmega::msg::ESC_TELEMETRY_9_TO_12; mavros::plugin::PluginBase::HandlerInfo = std::tuple<unsigned int, const char*, long unsigned int, std::function<void(const mavlink::__mavlink_message*, mavconn::Framing)> >; mavlink::mavlink_message_t = mavlink::__mavlink_message]::<lambda(const mavlink_message_t*, mavconn::Framing)>; <template-parameter-2-2> = void; <template-parameter-2-3> = void; _Res = void; _ArgTypes = {const mavlink::__mavlink_message*, mavconn::Framing}]’, declared using local type ‘mavros::plugin::PluginBase::make_handler(void (_C::*)(const mavlink_message_t*, _T&)) [with _C = mavros::extra_plugins::ESCTelemetryPlugin; _T = mavlink::ardupilotmega::msg::ESC_TELEMETRY_9_TO_12; mavros::plugin::PluginBase::HandlerInfo = std::tuple<unsigned int, const char*, long unsigned int, std::function<void(const mavlink::__mavlink_message*, mavconn::Framing)> >; mavlink::mavlink_message_t = mavlink::__mavlink_message]::<lambda(const mavlink_message_t*, mavconn::Framing)>’, is used but never defined [-fpermissive]

Thanks for your review and help

vooon · 2022-02-17T16:52:48Z

Add void run(diagnostic_updater::DiagnosticStatusWrapper &stat) override {} to ESCDiag class.

amilcarlucas · 2022-02-17T17:24:28Z

Thanks! It compiles now.

But cog does not:

  File "mavros_extras/src/plugins/esc_telemetry.cpp+62", line 12
    cog.outl(f"""pnh.param("{field}_{fm}/{fn}", this->{field}_{fm}_{fn}, {l});""")
                                                                                ^
SyntaxError: invalid syntax

vooon · 2022-02-17T20:22:26Z

What version of python do you use? For f-strings you need >= 3.6.

amilcarlucas · 2022-02-18T09:42:15Z

My bad I was using cog -cr ... instead of cog.py -cr ... It run fine now. Thanks.

I will move the diagnostics to the dynamically created ESC objects ... soonish

vooon · 2022-02-18T12:30:03Z

cog and cog.py should be the same. Perhaps you somehow has cog to be run by python2?

…tiple ESC instances

vooon added enhancement extras labels Feb 16, 2022

vooon reviewed Feb 16, 2022

View reviewed changes

amilcarlucas force-pushed the esc_telemetry_diagnostics branch 3 times, most recently from c39664b to 6f2276c Compare February 17, 2022 09:02

vooon reviewed Feb 17, 2022

View reviewed changes

amilcarlucas force-pushed the esc_telemetry_diagnostics branch 3 times, most recently from 9841fa9 to 7c45aef Compare February 17, 2022 14:10

amilcarlucas force-pushed the esc_telemetry_diagnostics branch from 9dd8487 to 613f38d Compare February 17, 2022 17:25

amilcarlucas force-pushed the esc_telemetry_diagnostics branch from 613f38d to f56d42c Compare February 18, 2022 09:38

amilcarlucas added 3 commits February 21, 2022 16:44

esc_telemetry.cpp: Add ROS diagnostics support

e20a31b

esc_telemetry.cpp: changes requested by vooon.

03fdc99

esc_telemetry.cpp: Remove the limits class and dynamically create mul…

14fc11d

…tiple ESC instances

amilcarlucas force-pushed the esc_telemetry_diagnostics branch from f56d42c to 14fc11d Compare February 21, 2022 15:45

amilcarlucas added APM plugin labels Feb 21, 2022

ESC Telemetry plugin: add diagnostics support #1705

Are you sure you want to change the base?

ESC Telemetry plugin: add diagnostics support #1705

Uh oh!

Conversation

amilcarlucas commented Feb 15, 2022

Uh oh!

vooon left a comment

Choose a reason for hiding this comment

Uh oh!

vooon commented Feb 16, 2022

Uh oh!

amilcarlucas commented Feb 16, 2022

Uh oh!

vooon commented Feb 16, 2022

Uh oh!

amilcarlucas commented Feb 16, 2022

Uh oh!

Uh oh!

Uh oh!

vooon Feb 16, 2022

Choose a reason for hiding this comment

Uh oh!

amilcarlucas Feb 16, 2022

Choose a reason for hiding this comment

Uh oh!

vooon Feb 17, 2022

Choose a reason for hiding this comment

Uh oh!

vooon Feb 17, 2022

Choose a reason for hiding this comment

Uh oh!

amilcarlucas Feb 17, 2022

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

vooon Feb 17, 2022

Choose a reason for hiding this comment

Uh oh!

vooon Feb 17, 2022

Choose a reason for hiding this comment

Uh oh!

vooon commented Feb 17, 2022

Uh oh!

amilcarlucas commented Feb 17, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

vooon commented Feb 17, 2022

Uh oh!

amilcarlucas commented Feb 17, 2022

Uh oh!

vooon commented Feb 17, 2022

Uh oh!

amilcarlucas commented Feb 18, 2022

Uh oh!

vooon commented Feb 18, 2022

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

amilcarlucas commented Feb 17, 2022 •

edited

Loading