Skip to content
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

ESC Telemetry plugin: add diagnostics support #1705

Open
wants to merge 3 commits into
base: master
Choose a base branch
from

Conversation

amilcarlucas
Copy link
Contributor

This create separated diagnostics for:

  • voltage
  • current
  • rpm
  • temperature
  • messages (stale telemetry)

Should I instead create one diagnostic per ESC?

Copy link
Member

@vooon vooon left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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
Copy link
Member

vooon commented Feb 16, 2022

Also you can move limit init to custom diag class.

@amilcarlucas
Copy link
Contributor Author

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
Copy link
Member

vooon commented Feb 16, 2022

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
Copy link
Contributor Author

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 Outdated Show resolved Hide resolved
mavros_extras/src/plugins/esc_telemetry.cpp Outdated Show resolved Hide resolved
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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fixed it, but now it has other issues :(

mavros_extras/src/plugins/esc_telemetry.cpp Show resolved Hide resolved
mavros_msgs/msg/ESCTelemetry.msg Outdated Show resolved Hide resolved
@amilcarlucas amilcarlucas force-pushed the esc_telemetry_diagnostics branch 3 times, most recently from c39664b to 6f2276c Compare February 17, 2022 09:02
int rpm_min_nok;
int rpm_min_ok;
int rpm_max_nok;
int rpm_max_ok;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

lim(lim_)
{}

const Limits& lim;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

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

vooon commented Feb 17, 2022

@amilcarlucas could you please post gcc error here?

@amilcarlucas
Copy link
Contributor Author

amilcarlucas commented Feb 17, 2022

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
Copy link
Member

vooon commented Feb 17, 2022

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

@amilcarlucas
Copy link
Contributor Author

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
Copy link
Member

vooon commented Feb 17, 2022

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

@amilcarlucas
Copy link
Contributor Author

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
Copy link
Member

vooon commented Feb 18, 2022

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants