469 task rewrite thruster interface

[albertomors]

No description provided.

[Andeshog]

Remove the python package and change the name of the new cpp package back to the original name

[Andeshog]

document

[jorgenfj]

Try to avoid using auto in general. Especially when it is not easy to deduce what type it is

[chrstrom]

I suggest following ES.11: Use auto to avoid redundant repetition of type names

[albertomors]

that variable should be a std::vector, like the other parameters passed as std::vector. The problem is that .as_integer_array() methods returns a long int and to do what we want to do i think it would require an explicit cast from long int to int that, imo, looks like inelegant and verbose. So i use auto. If i try to compile using explicit typing i get this:

error: conversion from ‘vector<long int,allocator>’ to non-scalar type ‘vector<int,allocator>’ requested
34 | this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping")
| ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
35 | .as_integer_array();

[chrstrom]

To be fair, if you end up not caring about system operational voltage, you can probably instantiate the left/right coefficients in the constructor?

Also, if you implement my suggested change and let your input be a parameter struct of some sorts, you can do this casting / validation immediately in the constructor.

Note that while long int -> int is a narrowing conversion, the thruster-to-pin mapping int will never be larger than what a normal int can hold, so I would actually suggest doing this cast

[jorgenfj]

Name of subscriber and publisher should be exposed as a ros param and read from a config file

[jorgenfj]

Should have a ros param boolean flag that only publishes if the flag is set so we dont publish when not needed. The boolean flag can also conditionally initialize the publisher so we don't initialize it if not needed. It would be nice to be able to toggle this during runtime. You can have a look at https://github.com/vortexntnu/vortex-image-filtering/blob/main/image-filtering/src/image_filtering_ros.cpp for inspiration on how this can be done

[jorgenfj]

Also update the thrust_update_rate: 10.0 # [Hz] in orca.yaml
Should be higher than 10.0

[chrstrom]

First pass of the C++ stuff.

As a general note, you might want to remove most of the inline comments. As a general rule of thumb, you should have a very good reason for adding inline codes (if you can't avoid some bit magic or some specific equation etc), as the code itself should do the explaining: Comments may lie (and/or become outdated), but the code wont!

This can most notably be seen in the suggestions to use std::algorithm, as that makes things a lot more explicit (especially when defining the lambdas outside the algorithm call and giving it a fitting name!

Also note that the suggestions here are free-balled, so make sure everything compiles and works as expected 😅

[chrstrom]

This is a constructor that's in the category of "too big". I would suggest making structs to hold some of the data, like

struct ThrusterParameters
{
    std::uint8_t thruster_no;
    std::uint8_t direction;
    std::uint16_t pwm_offset;
    std::uint16_t pwm_min;
    std::uint16_t pwm_max;
};

then passing in const std::vector<ThrusterParameters>&

You might find you can do something similar with the other parameters as well!

[chrstrom]

As a side note, good that you're passing the vectors/map by const reference!

[chrstrom]

Another side note, when constructing the ThrusterParameters objects, you can do additional checks to ensure that values are within legal range!

[chrstrom]

Avoid ALL_CAPS name except for static constexpr member variables. Use either the m_ prefix or _ suffix (i.e. m_bus_fd or bus_fd_), not sure what your conventions are 😄

[chrstrom]

1. Do you need ThrusterInterfaceAUVDriver to be default constructable? It appears to me that you'd explicitly want it not to be.

2. cppcoreguidelines say E.16: Destructors, deallocation, swap, and exception type copy/move construction must never fail, might be a good idea to explicitly define the destructor as noexcept, i.e. as ~ThrusterInterfaceAUVDriver() noexcept;, depending on your conventions. (I think it's handled implicitly by default?)

[albertomors]

yeah we dont actively use this in the code, it's there such that it compiles. This is because in the .hpp file for the node class i declare a driver class as field member of the node class and doing that it triggers a constructor, requesting me to create an "empty" constructor to handle this. Is there a better way to do it?

[chrstrom]

Ah, you'd want to hold the ThrusterInterfaceAUVDriver object in a unique_ptr. That's the usual way of dealing with classes that need other (fairly big) class objects as part of their members. Especially in this case, where having a default-constructed ThrusterInterfaceAUVDriver is not "allowed" (in the sense that it will not be a valid object unless you make it with the appropriate constructor) So something like

std::unique_ptr<ThrusterInterfaceAUVDriver> thruster_interface_

And then using std::make_unique where you define it. That will allow you to remove the default constructor.

[albertomors]

could you be more specific? What i did (and is not compiling with the same errors as when i hadn't the default constructor) is replacing

1. ThrusterInterfaceAUVDriver thruster_driver_ with std::unique_ptr<ThrusterInterfaceAUVDriver> thruster_driver_ as member of the class "blablabla_node"
2. removing the default constructor ThrusterInterfaceAUVDriver();
3. and replacing the line of code that instantiate the object passing the arguments with this->thruster_driver_ = std::make_unique<ThrusterInterfaceAUVDriver>(arguments....

The error is error: no declaration matches ‘ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver()’ 3 | ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver() {} | ^~~~~~~~~~~~~~~~~~~~~~~~~~ In file included from /home/alberto/vortex/auto_ws/src/vortex-auv/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp:1: /home/alberto/vortex/auto_ws/src/vortex-auv/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp:30:7: note: candidates are: ‘ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver(const ThrusterInterfaceAUVDriver&)’ 30 | class ThrusterInterfaceAUVDriver { | ^~~~~~~~~~~~~~~~~~~~~~~~~~ /home/alberto/vortex/auto_ws/src/vortex-auv/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp:57:5: note: ‘ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver(short int, int, const std::vector<short int>&, const std::vector<short int>&, const std::vector<int>&, const std::vector<int>&, const std::vector<double>&, const std::vector<double>&)’ 57 | ThrusterInterfaceAUVDriver(short I2C_BUS, | ^~~~~~~~~~~~~~~~~~~~~~~~~~ /home/alberto/vortex/auto_ws/src/vortex-auv/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp:30:7: note: ‘class ThrusterInterfaceAUVDriver’ defined here 30 | class ThrusterInterfaceAUVDriver { | ^~~~~~~~~~~~~~~~~~~~~~~~~~

[chrstrom]

I would suggest creating a free function for this, maybe something like

int discretize_system_operational_voltage(const double voltage)
{
    // TODO: Short description of equation since it's kinda hard to digest :)
    return std::min<int>(20.0, std::max(10.0, 2.0 * std::floor((voltage + 1.0) / 2.0)));
}

[albertomors]

completely removed this part from the code since we discovered there's no handling on the SYSTEM OP VOLTAGE. We dont read it from any topic since we dont measure it, so this piece becomes an unuseful piece of trash. Now we dont pass SYSTEM OP VOLTAGE anymore since it's 16V and not something else. ALWAYS. This mean we dont need anymore the map containing the pair of coeffs for any of them, just the 16V ones. Left all the coeffs in the yaml file for future uses, removed them from the code for saving resources

[chrstrom]

Would it make sense to subscribe to a voltage topic and do this lookup for every thrust output then?

[albertomors]

it would make sense if we had a way to measure it but we don't. And voltage always stays around 16V, maybe it's 15.5 but it rounds up to 16 so anyway would result in using the 16V values.

[chrstrom]

Fair enough, then there's not much you can do! However, not having a battery voltage measurement available is major haram, might want to whip electronics/embedded to get that up and running. Speaking from experience, if you do not have an alarm system set up to kill everything when the battery voltage dips too low for too long, you WILL destroy batteries 😂

[albertomors]

('im totally not sure about this @Andeshog correct me) but we should have a BMS, it's just that's it's purely enclosed in his purpose and doesnt talk with the other systems and so we dont have a "digital" access on it that we could use for this purpose.

[chrstrom]

Maybe the constructor should throw if you fail to open the I2C bus?

[chrstrom]

Avoid "raw loops" like the plague (i.e. for loops that have the type i = 0; i < .... syntax)😨

The #include library has a LOT of really nice functions you can use instead. In this case (defining the lambda outside lets us name it as well

auto to_kg = [](double force) { return force / 9.80665; };
std::transform(thruster_forces_array.begin(), thruster_forces_array.end(), forces_in_kg.begin(), to_kg);

[albertomors]

i put it in the local scope of the driver class as static constexpr directly in the .hpp class, instead of declaring it and defining it in the .cpp, since it's a single line of code. Is it okay? Compiling is compiling and working too.

[chrstrom]

EDIT: The original link here doesn't really apply, but instead;

Catching all (...) removes all info about what error was thrown. It would be nice to at least catch std::exception and print the value resulting from it, so you know what failed

[chrstrom]

Suggestion:

constexpr std::size_t i2c_data_size = 8 * 2;  // 8 thrusters * (1xMSB + 1xLSB)
std::uint8_t i2c_data_array[i2c_data_size];
auto pwm_to_i2c_data = [](std::int16_t pwm) -> std::array<std::uint8_t, 2> {
    return {static_cast<std::uint8_t>((pwm >> 8) & 0xFF), static_cast<std::uint8_t>(pwm & 0xFF)};
};
std::transform(thruster_pwm_array.begin(), thruster_pwm_array.end(), i2c_data_array, pwm_to_i2c_data);