ESC Telemetry plugin: add diagnostics support

[amilcarlucas]

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]

Also you can move limit init to custom diag class.

[amilcarlucas]

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]

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]

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?

[vooon]

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});""")
// ]]]

[amilcarlucas]

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

[vooon]

cog -cr <file>

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

[vooon]

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

[amilcarlucas]

Fixed it, but now it has other issues :(

[vooon]

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

[vooon]

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]

@amilcarlucas could you please post gcc error here?

[amilcarlucas]

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]

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

[amilcarlucas]

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]

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

[amilcarlucas]

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]

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