ADD: Changes to run in ROS2 Humble on Ubuntu 22.04 (#40)

* FIX: fix building for ROS2 Humble on Ubuntu 22.04

* FIX: do not need to add .py in Humble

refer to https://discourse.articulatedrobotics.xyz/t/ros2-control-spawner-py-not-found/124/2

* FIX: fix std::future_error

when running `ros2 run rl_sar rl_sim`, get:

```bash
terminate called after throwing an instance of 'std::future_error'
  what():  std::future_error: No associated state
```

* fix: multi ros version support

* FIX: typo

* FIX: let CMake find Python version that corresponds to ROS distro

* ADD: Update CONTRIBUTORS.md

---------

Co-authored-by: fan-ziqi <fanziqi614@gmail.com>

CONTRIBUTORS.md

@@ -16,6 +16,7 @@ Guidelines for modifications:
 ## Contributors
 
 * Jijie Huang
+* Yufeng Chi (-T.K.-)
 
 ## Acknowledgements
 

src/rl_sar/CMakeLists.txt

@@ -18,7 +18,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
 find_package(gazebo REQUIRED)
 
 find_package(ament_cmake REQUIRED)
-find_package(joint_state_controller REQUIRED)
+find_package(joint_state_broadcaster REQUIRED)
 find_package(robot_state_publisher REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(gazebo_ros REQUIRED)
@@ -29,8 +29,19 @@ find_package(rclpy REQUIRED)
 find_package(gazebo_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 
-# Make sure to find Python 3.8
-find_package(Python3 3.8 EXACT COMPONENTS Interpreter Development REQUIRED)
+# Make sure to find correct Python version based on ROS distro
+if(DEFINED ENV{ROS_DISTRO})
+    set(ROS_DISTRO_ENV $ENV{ROS_DISTRO})
+    if(ROS_DISTRO_ENV STREQUAL "foxy")
+      find_package(Python3 3.8 EXACT COMPONENTS Interpreter Development REQUIRED)
+    elseif(ROS_DISTRO_ENV STREQUAL "humble")
+      find_package(Python3 3.10 EXACT COMPONENTS Interpreter Development REQUIRED)
+    else()
+      # Not very sure what would the default case be. A fuzzy match?
+      find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
+    endif()
+endif()
+
 
 link_directories(/usr/local/lib)
 include_directories(${YAML_CPP_INCLUDE_DIR})
@@ -101,7 +112,7 @@ target_link_libraries(rl_real_go2
 )
 
 ament_target_dependencies(rl_sim
-  joint_state_controller
+  joint_state_broadcaster
   robot_state_publisher
   rclcpp
   gazebo_ros

src/rl_sar/launch/gazebo.launch.py

@@ -52,14 +52,14 @@ def generate_launch_description():
 
     joint_state_broadcaster_node = Node(
         package="controller_manager",
-        executable="spawner.py",
+        executable='spawner.py' if os.environ.get('ROS_DISTRO', '') == 'foxy' else 'spawner',
         arguments=["joint_state_broadcaster"],
         output="screen",
     )
 
     robot_joint_controller_node = Node(
         package="controller_manager",
-        executable="spawner.py",
+        executable='spawner.py' if os.environ.get('ROS_DISTRO', '') == 'foxy' else 'spawner',
         arguments=["robot_joint_controller"],
         output="screen",
     )

src/rl_sar/scripts/rl_sim.py

@@ -19,7 +19,7 @@ from ament_index_python.packages import get_package_share_directory
 from rl_sdk import *
 from observation_buffer import *
 
-CSV_LOGGER = True
+CSV_LOGGER = False
 
 class RL_Sim(RL, Node):
     def __init__(self):

src/rl_sar/src/rl_sim.cpp

@@ -1,7 +1,7 @@
 #include "rl_sim.hpp"
 
 // #define PLOT
-#define CSV_LOGGER
+// #define CSV_LOGGER
 
 RL_Sim::RL_Sim()
     : rclcpp::Node("rl_sim_node")
@@ -12,31 +12,39 @@ RL_Sim::RL_Sim()
     param_client = this->create_client<rcl_interfaces::srv::GetParameters>("/param_node/get_parameters");
     while (!param_client->wait_for_service(std::chrono::seconds(1)))
     {
+        if (!rclcpp::ok()) {
+            std::cout << LOGGER::ERROR << "Interrupted while waiting for param_node service. Exiting." << std::endl;
+            return;
+        }
         std::cout << LOGGER::WARNING << "Waiting for param_node service to be available..." << std::endl;
     }
     auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
-    request->names.push_back("robot_name");
-    request->names.push_back("gazebo_model_name");
+    request->names = {"robot_name", "gazebo_model_name"};
+
+    // Use a timeout for the future
     auto future = param_client->async_send_request(request);
-    rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);
-    if (future.get()->values.size() < 2)
+    auto status = rclcpp::spin_until_future_complete(this->get_node_base_interface(), future, std::chrono::seconds(5));
+
+    if (status == rclcpp::FutureReturnCode::SUCCESS)
     {
-        std::cout << LOGGER::WARNING << "Failed to get all parameters from param_node" << std::endl;
+        auto result = future.get();
+        if (result->values.size() < 2)
+        {
+            std::cout << LOGGER::ERROR << "Failed to get all parameters from param_node" << std::endl;
+        }
+        else
+        {
+            this->robot_name = result->values[0].string_value;
+            this->gazebo_model_name = result->values[1].string_value;
+            std::cout << LOGGER::INFO << "Get param robot_name: " << this->robot_name << std::endl;
+            std::cout << LOGGER::INFO << "Get param gazebo_model_name: " << this->gazebo_model_name << std::endl;
+        }
     }
     else
     {
-        this->robot_name = future.get()->values[0].string_value;
-        this->gazebo_model_name = future.get()->values[1].string_value;
-        std::cout << LOGGER::INFO << "Get param robot_name: " << this->robot_name << std::endl;
-        std::cout << LOGGER::INFO << "Get param gazebo_model_name: " << this->gazebo_model_name << std::endl;
+        std::cout << LOGGER::ERROR << "Failed to call param_node service" << std::endl;
     }
 
-    // // get params from this node
-    // this->declare_parameter<std::string>("robot_name", "");
-    // this->get_parameter("robot_name", this->robot_name);
-    // this->declare_parameter<std::string>("gazebo_model_name", "");
-    // this->get_parameter("gazebo_model_name", this->gazebo_model_name);
-
     // read params from yaml
     this->ReadYaml(this->robot_name);
     for (std::string &observation : this->params.observations)

src/robot_joint_controller/include/robot_joint_controller.hpp

@@ -37,7 +37,11 @@ typedef struct
 
 namespace robot_joint_controller
 {
-using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+#if defined(ROS_DISTRO_FOXY)
+    using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+#elif defined(ROS_DISTRO_HUMBLE)
+    using CallbackReturn = controller_interface::CallbackReturn;
+#endif
 
 class RobotJointController : public controller_interface::ControllerInterface
 {
@@ -51,13 +55,13 @@ public:
 #elif defined(ROS_DISTRO_HUMBLE)
     ROBOT_JOINT_CONTROLLER_PUBLIC
     controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    CallbackReturn on_init() override;
 #endif
     ROBOT_JOINT_CONTROLLER_PUBLIC
     controller_interface::InterfaceConfiguration command_interface_configuration() const override;
     ROBOT_JOINT_CONTROLLER_PUBLIC
     controller_interface::InterfaceConfiguration state_interface_configuration() const override;
-    // ROBOT_JOINT_CONTROLLER_PUBLIC
-    // CallbackReturn on_init() override;
     ROBOT_JOINT_CONTROLLER_PUBLIC
     CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override;
     ROBOT_JOINT_CONTROLLER_PUBLIC

src/robot_joint_controller/include/robot_joint_controller_group.hpp

@@ -37,7 +37,11 @@ typedef struct
 
 namespace robot_joint_controller
 {
-using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+#if defined(ROS_DISTRO_FOXY)
+    using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+#elif defined(ROS_DISTRO_HUMBLE)
+    using CallbackReturn = controller_interface::CallbackReturn;
+#endif
 
 class RobotJointControllerGroup : public controller_interface::ControllerInterface
 {
@@ -51,13 +55,13 @@ public:
 #elif defined(ROS_DISTRO_HUMBLE)
     ROBOT_JOINT_CONTROLLER_PUBLIC
     controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    CallbackReturn on_init() override;
 #endif
     ROBOT_JOINT_CONTROLLER_PUBLIC
     controller_interface::InterfaceConfiguration command_interface_configuration() const override;
     ROBOT_JOINT_CONTROLLER_PUBLIC
     controller_interface::InterfaceConfiguration state_interface_configuration() const override;
-    // ROBOT_JOINT_CONTROLLER_PUBLIC
-    // CallbackReturn on_init() override;
     ROBOT_JOINT_CONTROLLER_PUBLIC
     CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override;
     ROBOT_JOINT_CONTROLLER_PUBLIC

src/robot_joint_controller/src/robot_joint_controller.cpp

@@ -18,10 +18,12 @@ RobotJointController::RobotJointController()
     memset(&servo_command_, 0, sizeof(ServoCommand));
 }
 
-// CallbackReturn RobotJointController::on_init()
-// {
-//     return CallbackReturn::SUCCESS;
-// }
+#if defined(ROS_DISTRO_HUMBLE)
+CallbackReturn RobotJointController::on_init()
+{
+    return CallbackReturn::SUCCESS;
+}
+#endif
 
 CallbackReturn RobotJointController::on_configure(const rclcpp_lifecycle::State &previous_state)
 {

src/robot_joint_controller/src/robot_joint_controller_group.cpp

@@ -16,10 +16,12 @@ RobotJointControllerGroup::RobotJointControllerGroup()
     memset(&servo_command_, 0, sizeof(ServoCommand));
 }
 
-// CallbackReturn RobotJointControllerGroup::on_init()
-// {
-//     return CallbackReturn::SUCCESS;
-// }
+#if defined(ROS_DISTRO_HUMBLE)
+CallbackReturn RobotJointControllerGroup::on_init()
+{
+    return CallbackReturn::SUCCESS;
+}
+#endif
 
 CallbackReturn RobotJointControllerGroup::on_configure(const rclcpp_lifecycle::State &previous_state)
 {

src/robots/a1_description/launch/group_gazebo.launch.py

@@ -38,14 +38,14 @@ def generate_launch_description():
 
     joint_state_broadcaster_node = Node(
         package="controller_manager",
-        executable="spawner.py",
+        executable='spawner.py' if os.environ.get('ROS_DISTRO', '') == 'foxy' else 'spawner',
         arguments=["joint_state_broadcaster"],
         output='screen',
     )
 
     robot_joint_controller_node = Node(
         package="controller_manager",
-        executable="spawner.py",
+        executable='spawner.py' if os.environ.get('ROS_DISTRO', '') == 'foxy' else 'spawner',
         arguments=["robot_joint_controller"],
         output='screen',
     )

src/robots/a1_description/launch/single_gazebo.launch.py

@@ -46,7 +46,7 @@ def generate_launch_description():
 
     controller_nodes = [Node(
         package="controller_manager",
-        executable="spawner.py",
+        executable='spawner.py' if os.environ.get('ROS_DISTRO', '') == 'foxy' else 'spawner',
         arguments=[name],
         output='screen',
     ) for name in controller_names]