Merge pull request #10 from juraph-dev/fastdds_fixes

Fastdds fixes

README.md

@@ -23,9 +23,13 @@ https://user-images.githubusercontent.com/28330806/230512415-6c214e54-f59b-49c7-
 ### Prerequisites
 [Notcurses](https://github.com/dankamongmen/notcurses) \
 [ROS2 Humble or above](https://docs.ros.org/en/humble/index.html) \
-[Cyclone DDS](https://github.com/ros2/rmw_cyclonedds)
 
-FastRTPS isn't supported, as there doesn't appear to be introspection support for it [like there is for cyclonedds](https://github.com/ros2/rmw_cyclonedds/blob/2bbb80929e32d36a9e938c1ecb40f3fbf271b9f3/rmw_cyclonedds_cpp/src/TypeSupport.cpp), although it [appears to be in the works](https://github.com/ros2/rosidl_dynamic_typesupport_fastrtps), and I plan on adding support for it as soon as I get a chance. 
+FastRTPS's default configuration doesn't work, so you're going to need to do the following if you're using it:
+```sh
+apt-get install ros-${distro}-rmw-fastrtps-dynamic-cpp
+echo 'alias ornis="RMW_IMPLEMENTATION=rmw_fastrtps_dynamic_cpp ornis"' >> ~/.bashrc
+```
+CycloneDDS works out of the box, so no extra config required.
 
 Windows and mac also aren't currently supported. Mac might work, feel free to try it and let me know how it goes. 
 

src/main.cpp

@@ -29,19 +29,26 @@ int main(int argc, char* argv[])
     return 0;
   }
 
-  const int obj_ret = object_controller.spin();
+  int obj_ret = object_controller.spin();
 
+  if (obj_ret == -1)
+  {
+    (&object_controller)->~ObjectController();
+    new (&object_controller) ObjectController();
+    obj_ret = object_controller.spin();
+  }
   if (obj_ret == 1)
   {
     std::cerr << "Failed to get typesupport for std_msgs. Ensure your ros workspace is sourced\n";
   }
   else if (obj_ret == 2)
   {
-    std::cerr << "The ROS DDS does not appear to be configured to use typesupport introspection, \n \
-      If you're using fastrtps, export RMW_IMPLEMENTATION=rmw_fastrtps_cpp\n \
+    std::cerr << "Hello there, it looks like you're using fastrtps without dynamic typesupport, \n \
+      You can launch ORNIS with this functionality using: $RMW_IMPLEMENTATION=rmw_fastrtps_dynamic_cpp ornis\n \
      (You may also need to install the package: \n "
               << "#apt install ros-" << std::getenv("ROS_DISTRO") << "-rmw-fastrtps-dynamic-cpp)\n \
-      See this issue: https://gitlab.com/juraph/ornis/-/issues/5 for more information. \n Exiting...\n";
+      Maybe a handy alias: \"alias ornis=RMW_IMPLEMENTATION=rmw_fastrtps_dynamic_cpp ornis\" in the ~/.bashrc?\
+      \n Exiting...\n";
   }
 
   exit(0);

src/object_controller.cpp

@@ -1,6 +1,8 @@
 #include "ornis/object_controller.hpp"
 
 #include <cstdint>
+#include <cstdlib>
+#include <cstring>
 #include <rclcpp/utilities.hpp>
 #include <signal.h>
 #include <stdlib.h>
@@ -192,10 +194,9 @@ void ObjectController::checkUiRequests()
         // (Both to and from)
         stream_interface_map_[topic_name] = std::make_shared<StreamChannel>(topic_name);
         // Create the stream thread
-        stream_map_[topic_name] =
-            std::make_shared<TopicStreamer>(topic_name, topic_entry, topic_type, entry_type, entry_path,
-                                            stream_interface_map_[topic_name], ros_interface_node_, context_,
-                                            ui_.current_scheme_);
+        stream_map_[topic_name] = std::make_shared<TopicStreamer>(topic_name, topic_entry, topic_type, entry_type,
+                                                                  entry_path, stream_interface_map_[topic_name],
+                                                                  ros_interface_node_, context_, ui_.current_scheme_);
         interface_channel_->request_pending_ = false;
         interface_channel_->condition_variable_.notify_all();
         break;
@@ -220,14 +221,17 @@ void ObjectController::checkUiRequests()
 int ObjectController::spin()
 {
   char** argv = NULL;
-  context_ = rcl_get_zero_initialized_context();
   rcl_init_options_t options = rcl_get_zero_initialized_init_options();
+
   rcl_ret_t ret = rcl_init_options_init(&options, rcl_get_default_allocator());
+
+  context_ = rcl_get_zero_initialized_context();
+
   ret = rcl_init(0, argv, &options, &context_);
 
   if (ret != RCL_RET_OK)
   {
-    std::cerr << "FAILED RCL INIT" << ret << '\n';
+    std::cerr << "Failed rcl init: " << ret << '\n';
   }
 
   rcl_node_options_t node_options = rcl_node_get_default_options();
@@ -236,26 +240,31 @@ int ObjectController::spin()
 
   if (ret != RCL_RET_OK)
   {
-    std::cerr << "FAILED TO INITIALISE RCL NODE, ERROR: " << ret << '\n';
-    return 1;
+    std::cerr << "Failed to initialise rcl node, error: " << ret << '\n';
+    return -1;
+  }
+
+  if (!strcmp("rmw_fastrtps_cpp", rmw_get_implementation_identifier()))
+  {
+    rclcpp::shutdown();
+    ret = rcl_node_options_fini(&node_options);
+    return 2;
   }
 
   const auto ts = introspection::getMessageTypeSupport("std_msgs/msg/String",
                                                        rosidl_typesupport_introspection_cpp::typesupport_identifier);
 
   if (!ts)
   {
-    // If TS fails to initialise
+    rclcpp::shutdown();
+    ret = rcl_node_options_fini(&node_options);
     return 1;
   }
-  else if (ts->typesupport_identifier != rosidl_typesupport_introspection_cpp::typesupport_identifier)
-  {
-    return 2;
-  }
 
   while (spinUi() != 1)
     ;
 
+  rclcpp::shutdown();
   return rcl_node_options_fini(&node_options);
 }
 

src/options.cpp

@@ -175,7 +175,6 @@ void OptionsMenu::loadConfiguration()
   if (!std::filesystem::exists(config_dir))
   {
     createDefaultConfiguration();
-    return;
   }
 
   std::ifstream inputFile(config_dir);