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:
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.
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_);
}
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:
poll
system call. Because of this, when we can't read any data afterread_poll_ms
, we return an erroneous 0 bytes read, instead of an error.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: