Connection with multiple crazyflies (swarm)

ruimscarvalho98 commented 3 years ago

Connection with two (or more) crazyflies

Hardware description: Crazyflie 2.1
RTOS: freeRTOS
Installation type: micro_ros_setup
Version or commit hash: foxy

Steps to reproduce the issue

Running a simple micro-ros app with an int32 subscriber and int32 publisher
Modify the python bridge with an additional CrazyradioBridge() instance to be able to connect to multiple crazyflies with a single radio. (Not the best solution i know just to test ;) ) and write to a different virtual serial port depending on the crazyflie (i used /dev/ttyS11 and /dev/ttyS12.
Create two separate socat bridges: /dev/ttyS10 <-> /dev/ttyS11 and /dev/ttyS10 <-> /dev/ttyS12

Expected behavior

Multiple crazyflies (micro-ros clients) connect to a single micro-ros agent

Actual behavior

No error happens in the agent and the first crazyflie manages to connect with the agent but the second one fails resulting in an hardfault. Connection is possible by running two different agents with independent serial ports: /dev/ttyS10 <-> /dev/ttyS11 and /dev/ttyS20 <-> /dev/ttyS12

Additional information

Trying to use the same /dev/ttySXX does not work probably due to socat's bidirectional communication but then again im not sure which alternative approach I can take.

pablogs9 commented 3 years ago

Hello, @ruimscarvalho98 thanks a lot for doing that.

Some points:

Do you know if there is any C++ library for interfacing with Crazyradio? I think that this is a great opportunity to use the new micro-ROS custom transports.
If the former option is not possible, we are planning to implement a micro-ROS Agent that is able to open multiple serial ports using just one executable.

CC: @FranFin @jamoralp @Acuadros95 guys, can we plan this internally?

ruimscarvalho98 commented 3 years ago

Hey @pablogs9, Thanks for the quick reply!

Yeah there is the crazyflie_cpp library which is the library used in the crazyflie_ros package.

So if I understood correctly, your idea would be to use this library (or a similar one) to develop a custom transport for the agent which uses CRTP to send and receive data in a custom port (9 and 10) like you have done for the client right? Would this remove the need for the CRTP->Serial bridge entirely?

pablogs9 commented 3 years ago

That's it! I think that is a super interesting feature to implement. We will need to refactor the Crazyflie transport to just use CRTP stream to send the micro-ROS middleware data to the agent who will receive it using this C++ library.

Let me know if you make progress on this.

ruimscarvalho98 commented 3 years ago

Hey @pablogs9,

I've been looking up on this topic and I found another library that i think is really promising.

Its a native cpp implementation (as well as python bindings) with support for multiple drones and has been developed by bitcraze and whoenig (one of the guys responsible for the crazyflie_cpp and crazyflie_ros libraries i mentioned above) and has a simpler API which might be easier to integrate in a custom agent (here's a very simple example on how to read console data - i suppose for micro-ros it would be a matter of using port 9 instead of 0.

A (very) recent blog post has been published about it here

pablogs9 commented 3 years ago

Yes, this looks super interesting. I guess that the optimal way of communicating the Crazyflie with micro-ROS should be this way. But I'm wondering, what happens if we connect the Crazyflie to the micro-ROS agent via some CRTP (not necessarily the 10), where will be the control stream for controlling the drone movement?

Are you planning to start doing these tests? Let us know if you prepare a repo or a fork of this one.

ruimscarvalho98 commented 3 years ago

In my specific use-case I am only concerned with autonomous flight and for that i was thinking of using micro-ros to send the flight commands as well (i.e. create services dedicated to take off, landing, uploading trajectories etc. which call the High Level Commander of the crazyflie inside the micro-ros app) so I suppose I would only need to ignore the packets whose port is not the one used by micro-ros.

But yeah, in the case of using the cpp library for the agent I am not seeing an easy way to control the drone using another client because both programs would have to share the crazyradio.

I am working on other parts of the project right now but down the line I would definitely like to have a go at trying to make a custom agent and I'll let u guys know if i make any progress. Btw, do you have any expected date for releasing the agent that uses multiple serial ports?

pablogs9 commented 3 years ago

That's sounds super nice.

We have not planned the multiserial agent yet, but it should be in our mid-term roadmap.

CC: @FranFin @jamoralp @Acuadros95

ruimscarvalho98 commented 3 years ago

Hey @pablogs9,

Like we talked, I have started making a custom CRTP XRCE-DDS agent which you can find here if you are interested and I can confirm it is working (with a few bugs). Just have a few questions:

If i understood correctly the main difference between the XRCE-DDS agent and micro_ros_agent is that you guys include a ros2 graph manager. Is there any documentation you can share on how to use/integrate custom transports in the micro_ros_agent package?

When I try running the crazyflie_position_publisher app I get a few "deserialization errors" (bellow) but when I publish other types (i have tested uint32, strings and even a custom type) this does not occur... Is it because it's a geometry_msg? Do I need to have any special consideration when using non-standard XRCE types because im not using the micro_ros_agent package?


[1624967095.777059] error    | InputMessage.cpp   | log_error                | deserialization error  | buffer: 
[1624967095.777114] debug    | CustomAgent.cpp    | recv_message             | [==>> CRTP <<==]       | client_key: 0x4D90AD0E, len: 0, data: 
[1624967095.777790] debug    | CustomAgent.cpp    | recv_message             | [==>> CRTP <<==]       | client_key: 0x4D90AD0E, len: 24, data: 
0000: 81 01 06 00 07 01 10 00 00 20 00 15 B1 5F 1B BE EB 93 CB BE 98 8D 2A 40
[1624967095.777853] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 12, data: 
0000: B1 5F 1B BE EB 93 CB BE 98 8D 2A 40
[1624967095.779187] error    | InputMessage.cpp   | log_error                | deserialization error  | buffer: 
[1624967095.779248] debug    | CustomAgent.cpp    | recv_message             | [==>> CRTP <<==]       | client_key: 0x4D90AD0E, len: 0, data: 
[1624967095.779894] debug    | CustomAgent.cpp    | recv_message             | [==>> CRTP <<==]       | client_key: 0x4D90AD0E, len: 24, data: 
0000: 81 01 07 00 07 01 10 00 00 21 00 05 A9 23 A2 BC 4E C5 0C BE F5 D7 DF 3C
[1624967095.780014] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000000, len: 12, data: 
0000: A9 23 A2 BC 4E C5 0C BE F5 D7 DF 3C


Can confirm I can still echo the topics using `ros2 topic echo ....`

pablogs9 commented 3 years ago

It is super nice to hear about an Agent with custom transports for Crazyflie! I guess that you are the first community member that archives that. I wonder if you want to participate in the Embedded Working Group (CC: @mamerlan) for sharing with the community your procedure for creating this and explaining your use case.

If I understood correctly the main difference between the XRCE-DDS agent and micro_ros_agent is that you guys include a ros2 graph manager. Is there any documentation you can share on how to use/integrate custom transports in the micro_ros_agent package?

That's correct, by now the only difference between the Micro XRCE-DDS Agent and the micro-ROS Agent is the graph manager. By now there is no API for including custom transports in the micro-ROS Agent for sure you can fork the micro-ROS Agent and use the XRCE-DDS API for custom transport in the main file. Maybe we can schedule a meeting to talk about how to polish this in order to have a nice API for using it directly in micro-ROS Agent (CC: @mamerlan).

When I try running the crazyflie_position_publisher app I get a few "deserialization errors" (bellow) but when I publish other types (i have tested uint32, strings and even a custom type) this does not occur... Is it because it's a geometry_msg? Do I need to have any special consideration when using non-standard XRCE types because im not using the micro_ros_agent package?

It seems to be a flaw in the transports, are you sure that you receiving the same amount of bytes that you are sending? A serialization error occurs when the CDR buffer can't deserialize the whole message... Maybe if you provide the code I can replicate it in a debug environment.

Thanks a lot!

ruimscarvalho98 commented 3 years ago

Thanks a lot for your interest in our project!

It is super nice to hear about an Agent with custom transports for Crazyflie! I guess that you are the first community member that archives that. I wonder if you want to participate in the Embedded Working Group (CC: @mamerlan) for sharing with the community your procedure for creating this and explaining your use case.

Sure, me and my co-worker @matheus-ps would love to share our project with the community. We just have to discuss with our manager about NDAs and stuff like that.

Maybe we can schedule a meeting to talk about how to polish this in order to have a nice API for using it directly in micro-ROS Agent (CC: @mamerlan).

Yeah, we're always open to discuss and help out so if you want to schedule a meeting just let us know :)

It seems to be a flaw in the transports, are you sure that you receiving the same amount of bytes that you are sending? A serialization error occurs when the CDR buffer can't deserialize the whole message...

About this I've been reviewing my transport implementation and I think it's related to the message size. In this case as we are using a Point32 its 4 + 4 + 4 = 12Bytes. With serial framing I suppose it is something like:

As CRTP only allows 30Bytes payload per packet the CRC is fragmented and I believe the state machine that handles the StreamFramingProtocol does not support this.... Is this intentional? I've tried decreasing the maximum number of bytes sent per packet in client-side transport (for the fragmentation to occur in the payload not in the crc) and can confirm the error no longer occurs.

Maybe if you provide the code I can replicate it in a debug environment.

The code is available in this repo

mamerlan commented 3 years ago

Hi there! Thanks for your contribution @ruimscarvalho98 and @matheus-ps ! Please, reach out to me when you are certain about dissemination. FYI- We are hosting an EWG in July before the summer break and we will be back in September.

Sure, me and my co-worker @matheus-ps would love to share our project with the community. We just have to discuss with our manager about NDAs and stuff like that.

pablogs9 commented 3 years ago

Regarding the XRCE encapsulation in CRTP: you do not have to try to encapsulate the whole XRCE packet in the CRTP frame. Just assume that CRTP is a stream of continuous data between the client and the agent.

So an approach can be:

Your client's custom transport (with framing enabled) write function receives more than 30 Bytes, just split them in multiple CRTP packets.
We assume that those packets are received in the agent ordered.
In the agent, the custom transport function just read all the CRTP packets that the radio link can provide and buffers them internally (maybe a static byte array in your transport). The receive function just has to copy len data in the provided buf, this data can be taken from the mentioned internal buffer.

On the other way (agent -> client) a similar approach should be taken, but consider that if your micro-ROS client exclusively has publishers, the communication from the agent to the client will be just ACK and heartbeats (probably less that 30 B) so maybe you can follow another approach.

Let me know if this solves your questions.

ruimscarvalho98 commented 3 years ago

Hey @pablogs9 , thanks for the suggestions!

I am actually already using CRTP as a stream (with framing enabled).

I think I might actually have an idea of what was causing the issue, in the CustomAgent.cpp the read function is defined:

        if (framing_)
        {
            uint8_t remote_addr = 0x00;
            recv_bytes = framing_io_.read_framed_msg(
                buffer_, SERVER_BUFFER_SIZE, remote_addr, timeout, transport_rc);
        }
        else
        {
            recv_bytes = custom_recv_msg_func_(
                recv_endpoint_, buffer_, SERVER_BUFFER_SIZE, timeout, transport_rc);
        }

        bool success = (0 <= recv_bytes && TransportRc::ok == transport_rc);
        if (success)
        {
            // User must have filled all the members of the endpoint.
            recv_endpoint_->check_non_empty_members();

            input_packet.message.reset(
                new eprosima::uxr::InputMessage(
                    buffer_, static_cast<size_t>(recv_bytes)));
            input_packet.source = *recv_endpoint_;

            uint32_t raw_client_key = 0u;
            this->get_client_key(input_packet.source, raw_client_key);

            std::stringstream ss;
            ss << UXR_COLOR_YELLOW << "[==>> " << name_ << " <<==]" << UXR_COLOR_RESET;
            UXR_AGENT_LOG_MESSAGE(
                ss.str(),
                raw_client_key,
                input_packet.message->get_buf(),
                input_packet.message->get_len());
        }

Shouldn't the success condition require the recv_bytes to be greater and not greater or equal to zero (similiarly to the logic in SerialAgentLinux.cpp)? Because if the fragmentation occurs during the CRC (i.e. due to a timeout) the return value of the the read_framed_msg function will be zero (while the buffer is not empty) and the TrasnportRC is still OK triggering the success condition...

Can that cause a seriliazation error when the InputMessage constructor is called? I've tried modifying the file to only run the code inside the "success condition" (and therefore returning true) if the recv_bytes > 0 and I no longer get any errors.

pablogs9 commented 3 years ago

Something like https://github.com/eProsima/Micro-XRCE-DDS-Agent/pull/259?

I'm going to prepare a CI test in order to check if this passes all tests. IMO you are right, if transport (framed or not) gives 0 B, it is not a success: https://github.com/eProsima/Micro-XRCE-DDS/pull/98

Only one thing, what happens if recv_message returns false? Does it make an error in the upper layer? Maybe we should set success to true if transport_rc is a timeout because the custom transport should be able to read nothing at some point...

ruimscarvalho98 commented 3 years ago

Something like eProsima/Micro-XRCE-DDS-Agent#259?

I've tried this one and it doesn't work for me... I might be doing something wrong because when I can't read buffer_length bytes but still read some bytes I set transport_rc to TransportRc::ok, should I set it to TransportRc::timeout_error?? Because if I set it to OK, the success condition is false but the else if (TransportRc::timeout_error != transport_rc) condition is trggered causing a server error.

I can make a PR with the solution thats working for me if thats not the case.

pablogs9 commented 3 years ago

In general, if framing is enabled the transport should read up to length bytes. Check docu.

Yes please open the PR and I will check it.

ruimscarvalho98 commented 3 years ago

Sry, had some trouble with signing the pull-request and opened a new one: eProsima/Micro-XRCE-DDS-Agent#263. It passed the CI tests. Appreciate it if you could check/review it @pablogs9

Acuadros95 commented 2 years ago

Closing due to inactivity, reopen if needed.

micro-ROS / micro-ROS_crazyflie_demo