osrf / ros2_serial_example Goto Github PK

A diagram would be helpful for sharing the approach.

Probably a simple block diagram in svg or dot would be helpful to explain how things are working.

We are currently using the "old" way of creating a node in https://github.com/osrf/ros2_serial_example/blob/master/src/ros2_to_serial_bridge.cpp#L216; that is, we create the node, then do operations on the node. The new, composable way to do things is to create a class that derives from rclcpp::Node and then does all of the operations internally. We should rewrite things to the latter method as it will make our node composable down the line.

The way the current serial protocol works is that an 8-bit quantity represents the "topic_ID", which is really a representation of the (topic_type, topic_name) tuple. The map of topic_ID -> (topic_type, topic_name) is currently given in a YAML file that is passed as ROS 2 parameters at ros2_to_serial_bridge startup time. It would be nice to allow this information to come either from the YAML file, or to be queried directly from the far end of the serial connection. That way the microcontroller on the far end could be completely self-describing and ros2_to_serial_bridge could be completely generic.

The current serial protocol is exactly equivalent to the one in https://github.com/PX4/px4_ros_com/blob/master/templates/microRTPS_transport.cpp#L151 . However, it is worthwhile to explore whether that is the best way to frame a serial protocol. Some observations and thoughts on serial framing design:

1. The payload should be a well-known format. We are using CDR.

2. There should be a header with a well-known sequence, a length at the beginning, and a CRC. We are currently using CRC16. The current protocol looks like this (the vertical bars are separators and not actually part of the protocol):

   >>>|topic_ID|seq|len_high|len_low|CRC_high|CRC_low|payload_start...payload_end|

3. Currently sends and receives to the serial port are "send it and forget it". It might be nice to have an ACK/NACK response.

4. Currently, messages that are larger than the ring buffer in the serial->ROS2 direction are just quietly dropped. It might be nice to have the ability to query for the largest size message that we accept. Aternatively, see 3 above.

5. We may want to think about using byte-stuffing for the serial protocol (see https://eli.thegreenplace.net/2009/08/12/framing-in-serial-communications/). In particular, think about the case where one of the first three > gets garbled in transmission, but there is >>> somewhere in the body of the message. In that case, we'll essentially be taking "random" data as the sequence, length, and CRC, and may make us do something unintended.

6. Even better, maybe we should look at using COBS instead: https://www.embeddedrelated.com/showarticle/113.php

Right now the YAML file for configuration looks like this:

ros2_to_serial_bridge:
    ros__parameters:
        device: /dev/pts/25
        serial_protocol: px4
        chatter:
            serial_mapping: 9
            type: std_msgs/String
            direction: SerialToROS2

The problem is that the topic -> serial mapping parts aren't in their own section, so they aren't cleanly separated from the rest of the configuration. We should move them down one level under a high-level "topics" sub-category to clearly delineate them.

All of the code currently in this repository are without namespaces. We should fix that to ensure we have no symbol collisions.

As it stands, ros2_serial_bridge_main.cpp does way too much work. This means that the node is not composable with other nodes. What we should probably do here is:

1. Remove the signal_handler and the running variable; I don't think they serve a purpose anymore.
2. Move the get_parameter for write_sleep_ms into the constructor (or get rid of it completely).
3. Think about how to get rid of the rate loop; if we code it properly, we should just be able to call rclcpp::spin on the constructed node.
4. Add a dependency on rclcpp_components to the package.xml and CMakeLists.txt.
5. Add the RCLCPP_COMPONENTS_REGISTER_NODE macro to the end of ros2_to_serial_bridge.cpp.
6. Add launch files showing how to launch the node in both composable and non-composable modes.

According to the documentation, setting dynamic_serial_mapping_ms to 0 should wait for a dynamic mapping indefinitely on startup. I clearly never tested this, as we needed to merge #74 to even allow the feature to work. However, with that in place, it still doesn't work, because the code to support doing it doesn't support 0.

The problem is in two parts:

1. In the low-level transporter (udp and uart), we aren't properly catching errors from the poll system call. Because of this, when we can't read any data after read_poll_ms, we return an erroneous 0 bytes read, instead of an error.
2. In dynamically_get_serial_mapping, the main loop that tries to get the mapping doesn't properly handle 0 timeout, nor does it handle errors from the underlying read implementation.

For indefinite wait to actually work, we need to fix both of these. The patch I have looks like this, but it needs more work and to be thoroughly tested:

diff --git a/ros2_serial_example/src/ros2_to_serial_bridge.cpp b/ros2_serial_example/src/ros2_to_serial_bridge.cpp
index 8fdcad3..a8283d5 100644
--- a/ros2_serial_example/src/ros2_to_serial_bridge.cpp
+++ b/ros2_serial_example/src/ros2_to_serial_bridge.cpp
@@ -122,8 +122,9 @@ ROS2ToSerialBridge::ROS2ToSerialBridge(const rclcpp::NodeOptions& node_options)
     }
 
     std::map<std::string, ros2_to_serial_bridge::pubsub::TopicMapping> topic_names_and_serialization;
-    if (dynamic_serial_mapping_ms > 0)
+    if (dynamic_serial_mapping_ms >= 0)
     {
+       fprintf(stderr, "Dynamically getting serial mapping\n");
         topic_names_and_serialization = dynamically_get_serial_mapping(dynamic_serial_mapping_ms);
     }
     else
@@ -310,9 +311,9 @@ std::map<std::string, ros2_to_serial_bridge::pubsub::TopicMapping> ROS2ToSerialB
     std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
     do
     {
-        ssize_t length = 0;
         topic_id_size_t topic_ID;
-        if ((length = transporter_->read(&topic_ID, data_buffer.get(), BUFFER_SIZE)) > 0)
+        ssize_t length = transporter_->read(&topic_ID, data_buffer.get(), BUFFER_SIZE);
+        if (length > 0)
         {
             if (topic_ID == 1)
             {
@@ -333,13 +334,18 @@ std::map<std::string, ros2_to_serial_bridge::pubsub::TopicMapping> ROS2ToSerialB
                 break;
             }
         }
+        else if (length != -ENODATA)
+        {
+          fprintf(stderr, "Length was %zd (%s)\n", length, strerror(errno));
+          break;
+        }
 
         if (wait_ms > 0)
         {
             std::chrono::time_point<std::chrono::system_clock> now = std::chrono::system_clock::now();
             diff_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - start);
         }
-    } while (diff_ms.count() < wait_ms);
+    } while (wait_ms == 0 || diff_ms.count() < wait_ms);
 
     if (!got_response)
     {
diff --git a/ros2_serial_example/src/udp_transporter.cpp b/ros2_serial_example/src/udp_transporter.cpp
index b5321f1..cb64a58 100644
--- a/ros2_serial_example/src/udp_transporter.cpp
+++ b/ros2_serial_example/src/udp_transporter.cpp
@@ -181,7 +181,11 @@ ssize_t UDPTransporter::node_read()
     ssize_t ret = 0;
     int r = ::poll(reinterpret_cast<struct pollfd *>(poll_fd_), 1, read_poll_ms_);
 
-    if (r == 1 && (poll_fd_[0].revents & POLLIN) != 0)
+    if (r < 0)
+    {
+      return r;
+    }
+    else if (r == 1 && (poll_fd_[0].revents & POLLIN) != 0)
     {
         ret = ringbuf_.read(recv_fd_);
     }

After searching around the Web I have not found (yet) any project that supports serial communication between ROS 2 machines and 8-bit microcontrollers like the Arduino Uno/Mega. I know of the microROS project, and the other projects that are referenced in the README, but all of them are targeting 32-bit boards.

I understand that this project is also focused on 32-bit boards (FreeRTOS / MicroCDR), so at this point I have a couple of questions:

1. Is the development of this repo still active? Or has everything being moved to microROS?

2. Is there anyone working on a library to offer support to smaller boards like the 8-bit Arduinos? I was planning to reuse an old project to build my own, based on HDLC or COBS with a basic ARQ feature. However, if there is an intention to provide official support from the OSRF I would not mind to help/collaborate to make it happen and avoid duplicate and unnecessary work.

This is probably related to #67, but it was closed due to inactivity and was not even focused on smaller micros...

We don't currently have any unit tests for the ROS2Topics class. These should be relatively straightforward to write, so we should probably do that.

Problem

I am trying to build ros2_serial bridge you provided with ros2 foxy. But it does not build for any packages like std_msgs, geometry_msgs , ...
What is the problem?

Error log

    colcon build --cmake-args -DROS2_SERIAL_PKGS="std_msgs"
    Starting >>> ros2_serial_msgs
    --- stderr: ros2_serial_msgs                              
    CMake Warning:
      Manually-specified variables were not used by the project:
    
        ROS2_SERIAL_PKGS
    
    
    ---
    Finished <<< ros2_serial_msgs [1.56s]
    Starting >>> ros2_serial_example
    --- stderr: ros2_serial_example                             
    /home/a/softwares/ros2_serial/src/ros2_serial_example/ros2_serial_example/src/dummy_udp.cpp:30:10: fatal error: ros2_serial_msgs/msg/serial_mapping__rosidl_typesupport_fastrtps_cpp.hpp: No such file or directory
       30 | #include "ros2_serial_msgs/msg/serial_mapping__rosidl_typesupport_fastrtps_cpp.hpp"
          |          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    compilation terminated.
    make[2]: *** [CMakeFiles/dummy_udp.dir/build.make:63: CMakeFiles/dummy_udp.dir/src/dummy_udp.cpp.o] Error 1
    make[2]: *** Waiting for unfinished jobs....
    /home/a/softwares/ros2_serial/src/ros2_serial_example/ros2_serial_example/src/dummy_serial.cpp:30:10: fatal error: ros2_serial_msgs/msg/serial_mapping__rosidl_typesupport_fastrtps_cpp.hpp: No such file or directory
       30 | #include "ros2_serial_msgs/msg/serial_mapping__rosidl_typesupport_fastrtps_cpp.hpp"
          |          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    compilation terminated.
    make[2]: *** [CMakeFiles/dummy_serial.dir/build.make:63: CMakeFiles/dummy_serial.dir/src/dummy_serial.cpp.o] Error 1
    make[1]: *** [CMakeFiles/Makefile2:369: CMakeFiles/dummy_serial.dir/all] Error 2
    make[1]: *** Waiting for unfinished jobs....
    /home/a/softwares/ros2_serial/build/ros2_serial_example/std_msgs_int8_pub_sub_type.cpp:24:10: fatal error: std_msgs/msg/int8__rosidl_typesupport_fastrtps_cpp.hpp: No such file or directory
       24 | #include <std_msgs/msg/int8__rosidl_typesupport_fastrtps_cpp.hpp>
          |          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    compilation terminated.
    make[2]: *** [CMakeFiles/bridge_gen.dir/build.make:250: CMakeFiles/bridge_gen.dir/std_msgs_int8_pub_sub_type.cpp.o] Error 1
    make[2]: *** Waiting for unfinished jobs....
    /home/a/softwares/ros2_serial/build/ros2_serial_example/std_msgs_int64_pub_sub_type.cpp:24:10: fatal error: std_msgs/msg/int64__rosidl_typesupport_fastrtps_cpp.hpp: No such file or directory
       24 | #include <std_msgs/msg/int64__rosidl_typesupport_fastrtps_cpp.hpp>
          |          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    compilation terminated.
    make[2]: *** [CMakeFiles/bridge_gen.dir/build.make:263: CMakeFiles/bridge_gen.dir/std_msgs_int64_pub_sub_type.cpp.o] Error 1
    /home/a/softwares/ros2_serial/build/ros2_serial_example/std_msgs_u_int32_multi_array_pub_sub_type.cpp:24:10: fatal error: std_msgs/msg/u_int32_multi_array__rosidl_typesupport_fastrtps_cpp.hpp: No such file or directory
       24 | #include <std_msgs/msg/u_int32_multi_array__rosidl_typesupport_fastrtps_cpp.hpp>
          |          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    compilation terminated.
    make[2]: *** [CMakeFiles/bridge_gen.dir/build.make:276: CMakeFiles/bridge_gen.dir/std_msgs_u_int32_multi_array_pub_sub_type.cpp.o] Error 1
    make[1]: *** [CMakeFiles/Makefile2:198: CMakeFiles/bridge_gen.dir/all] Error 2
    make[1]: *** [CMakeFiles/Makefile2:282: CMakeFiles/dummy_udp.dir/all] Error 2
    make: *** [Makefile:141: all] Error 2
    ---
    Failed   <<< ros2_serial_example [2.04s, exited with code 2]
    
    Summary: 1 package finished [3.76s]
      1 package failed: ros2_serial_example
      2 packages had stderr output: ros2_serial_example ros2_serial_msgs

There's currently a TODO at: https://github.com/osrf/ros2_serial_example/blob/master/CMakeLists.txt#L51 , which happens because CMake does not know to re-execute an execute_process if the script changes. I'm not quite sure how to fix it at the moment.

Currently the code generation for the individual messages all compile into a single header file (ros2_topics.hpp), which is then included into the main program. The problem is that for large number of message types, this single header file can grow quite large and eat up lots of RAM during compilation. In one test, I compiled all of ROS2 std_msgs into the application, and it took ~1.8GB of RAM during compilation. That's clearly too much.

One way to alleviate this would be to make each message its own compilation unit, and then have some kind of "registration" with a table so we can look it up later. That should significantly cut down on the RAM needed for each compilation unit.

This is something I could use yesterday. I'd like to work on it.

Possibly the readme.md file needs to say where discussions about the design take place.

I might be able to implement,

• the micro-controller side and
• a way to use this also with ROS 1.

On the uP side, I think there need to be functions like "publish()", "advertise()", "spin-once()" and so on. How to do this without duplicating code for each uP platform?

ROS_Serial does all this today but it uses 20K of ROM and 70% of an Arduino's RAM for "Hello World".