Merge pull request #415 from husarion/ros2-lynx-remove-version

Delete panther_version argument and fix controller launch to use robot_model arg
husarion · Sep 20, 2024 · 08ec819 · 08ec819
2 parents 2430738 + 9c0a8c1
commit 08ec819
Show file tree

Hide file tree

Showing 15 changed files with 49 additions and 55 deletions.
diff --git a/lynx_description/launch/load_urdf.launch.py b/lynx_description/launch/load_urdf.launch.py
@@ -127,14 +127,13 @@ def generate_launch_description():
         choices=["WH05", "custom"],
     )
 
-    imu_localization_x = os.environ.get("LYNX_IMU_LOCALIZATION_X", "0.168")
-    imu_localization_y = os.environ.get("LYNX_IMU_LOCALIZATION_Y", "0.028")
-    imu_localization_z = os.environ.get("LYNX_IMU_LOCALIZATION_Z", "0.083")
-    imu_orientation_r = os.environ.get("LYNX_IMU_ORIENTATION_R", "3.14")
-    imu_orientation_p = os.environ.get("LYNX_IMU_ORIENTATION_P", "-1.57")
-    imu_orientation_y = os.environ.get("LYNX_IMU_ORIENTATION_Y", "0.0")
-
     # Get URDF via xacro
+    imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168")
+    imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028")
+    imu_pos_z = os.environ.get("ROBOT_IMU_LOCALIZATION_Z", "0.083")
+    imu_rot_r = os.environ.get("ROBOT_IMU_ORIENTATION_R", "3.14")
+    imu_rot_p = os.environ.get("ROBOT_IMU_ORIENTATION_P", "-1.57")
+    imu_rot_y = os.environ.get("ROBOT_IMU_ORIENTATION_Y", "0.0")
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -151,9 +150,9 @@ def generate_launch_description():
             " battery_config_file:=",
             battery_config_path,
             " imu_xyz:=",
-            f'"{imu_localization_x} {imu_localization_y} {imu_localization_z}"',
+            f"'{imu_pos_x} {imu_pos_y} {imu_pos_z}'",
             " imu_rpy:=",
-            f'"{imu_orientation_r} {imu_orientation_p} {imu_orientation_y}"',
+            f"'{imu_rot_r} {imu_rot_p} {imu_rot_y}'",
             " namespace:=",
             namespace,
             " components_config_path:=",

diff --git a/panther_battery/launch/battery.launch.py b/panther_battery/launch/battery.launch.py
@@ -28,13 +28,10 @@ def generate_launch_description():
         description="Add namespace to all launched nodes.",
     )
 
-    panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0")
-
     battery_driver_node = Node(
         package="panther_battery",
         executable="battery_driver_node",
         name="battery_driver",
-        parameters=[{"panther_version": panther_version}],
         namespace=namespace,
         remappings=[("/diagnostics", "diagnostics")],
         emulate_tty=True,

diff --git a/panther_battery/src/battery_driver_node.cpp b/panther_battery/src/battery_driver_node.cpp
@@ -41,7 +41,6 @@ BatteryDriverNode::BatteryDriverNode(
 {
   RCLCPP_INFO(this->get_logger(), "Constructing node.");
 
-  this->declare_parameter<float>("panther_version", 1.2);
   this->declare_parameter<int>("ma_window_len/voltage", 10);
   this->declare_parameter<int>("ma_window_len/current", 10);
 
@@ -58,17 +57,14 @@ void BatteryDriverNode::Initialize()
 {
   RCLCPP_INFO(this->get_logger(), "Initializing.");
 
-  const float panther_version = this->get_parameter("panther_version").as_double();
-  if (panther_version >= (1.2f - std::numeric_limits<float>::epsilon())) {
-    try {
-      InitializeWithADCBattery();
-      return;
-    } catch (const std::runtime_error & e) {
-      RCLCPP_WARN_STREAM(
-        this->get_logger(), "An exception occurred while initializing with ADC: "
-                              << e.what()
-                              << " Falling back to using Roboteq drivers to publish battery data.");
-    }
+  try {
+    InitializeWithADCBattery();
+    return;
+  } catch (const std::runtime_error & e) {
+    RCLCPP_WARN_STREAM(
+      this->get_logger(), "An exception occurred while initializing with ADC: "
+                            << e.what()
+                            << " Falling back to using Roboteq drivers to publish battery data.");
   }
   InitializeWithRoboteqBattery();
 

diff --git a/panther_battery/test/test_battery_driver_node_adc_dual.cpp b/panther_battery/test/test_battery_driver_node_adc_dual.cpp
@@ -25,7 +25,7 @@
 class TestBatteryNodeADCDual : public TestBatteryNode
 {
 public:
-  TestBatteryNodeADCDual() : TestBatteryNode(1.2, true) {}
+  TestBatteryNodeADCDual() : TestBatteryNode(true, true) {}
 };
 
 TEST_F(TestBatteryNodeADCDual, BatteryValues)

diff --git a/panther_battery/test/test_battery_driver_node_adc_single.cpp b/panther_battery/test/test_battery_driver_node_adc_single.cpp
@@ -25,7 +25,7 @@
 class TestBatteryNodeADCSingle : public TestBatteryNode
 {
 public:
-  TestBatteryNodeADCSingle() : TestBatteryNode(1.2, false) {}
+  TestBatteryNodeADCSingle() : TestBatteryNode(true, false) {}
 };
 
 TEST_F(TestBatteryNodeADCSingle, BatteryValues)

diff --git a/panther_battery/test/test_battery_driver_node_roboteq.cpp b/panther_battery/test/test_battery_driver_node_roboteq.cpp
@@ -27,7 +27,7 @@
 class TestBatteryNodeRoboteq : public TestBatteryNode
 {
 public:
-  TestBatteryNodeRoboteq() : TestBatteryNode(1.0) {}
+  TestBatteryNodeRoboteq() : TestBatteryNode(false) {}
 };
 
 TEST_F(TestBatteryNodeRoboteq, BatteryValues)

diff --git a/panther_battery/test/utils/test_battery_driver_node.hpp b/panther_battery/test/utils/test_battery_driver_node.hpp
@@ -40,7 +40,7 @@ using IOStateMsg = panther_msgs::msg::IOState;
 class TestBatteryNode : public testing::Test
 {
 public:
-  TestBatteryNode(const float panther_version = 1.2, const bool dual_battery = false);
+  TestBatteryNode(const bool use_adc_battery = true, const bool dual_battery = false);
   ~TestBatteryNode();
 
 protected:
@@ -63,12 +63,11 @@ class TestBatteryNode : public testing::Test
   rclcpp::Publisher<RobotDriverStateMsg>::SharedPtr driver_state_pub_;
 };
 
-TestBatteryNode::TestBatteryNode(const float panther_version, const bool dual_battery)
+TestBatteryNode::TestBatteryNode(const bool use_adc_battery, const bool dual_battery)
 {
   std::vector<rclcpp::Parameter> params;
-  params.push_back(rclcpp::Parameter("panther_version", panther_version));
 
-  if (panther_version >= 1.2 - std::numeric_limits<float>::epsilon()) {
+  if (use_adc_battery) {
     device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0;
     device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1;
 

diff --git a/panther_controller/launch/controller.launch.py b/panther_controller/launch/controller.launch.py
@@ -112,12 +112,13 @@ def generate_launch_description():
         choices=["True", "true", "False", "false"],
     )
 
+    robot_description_pkg = PythonExpression(["'", robot_model, "_description'"])
     wheel_config_path = LaunchConfiguration("wheel_config_path")
     declare_wheel_config_path_arg = DeclareLaunchArgument(
         "wheel_config_path",
         default_value=PathJoinSubstitution(
             [
-                FindPackageShare("panther_description"),
+                FindPackageShare(robot_description_pkg),
                 "config",
                 PythonExpression(["'", wheel_type, ".yaml'"]),
             ]
@@ -142,19 +143,25 @@ def generate_launch_description():
     )
 
     # Get URDF via xacro
+    robot_description_pkg = PythonExpression(["'", robot_model, "_description'"])
+    robot_description_file = PythonExpression(["'", robot_model, ".urdf.xacro'"])
+    imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168")
+    imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028")
+    imu_pos_z = os.environ.get("ROBOT_IMU_LOCALIZATION_Z", "0.083")
+    imu_rot_r = os.environ.get("ROBOT_IMU_ORIENTATION_R", "3.14")
+    imu_rot_p = os.environ.get("ROBOT_IMU_ORIENTATION_P", "-1.57")
+    imu_rot_y = os.environ.get("ROBOT_IMU_ORIENTATION_Y", "0.0")
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
             PathJoinSubstitution(
                 [
-                    FindPackageShare("panther_description"),
+                    FindPackageShare(robot_description_pkg),
                     "urdf",
-                    "panther.urdf.xacro",
+                    robot_description_file,
                 ]
             ),
-            " panther_version:=",
-            os.environ.get("PANTHER_ROBOT_VERSION", "1.0"),
             " use_sim:=",
             use_sim,
             " wheel_config_file:=",
@@ -164,9 +171,9 @@ def generate_launch_description():
             " battery_config_file:=",
             battery_config_path,
             " imu_xyz:=",
-            f"\"{os.environ.get('PANTHER_IMU_LOCALIZATION_X', '0.168')} {os.environ.get('PANTHER_IMU_LOCALIZATION_Y', '0.028')} {os.environ.get('PANTHER_IMU_LOCALIZATION_Z', '0.083')}\"",
+            f"'{imu_pos_x} {imu_pos_y} {imu_pos_z}'",
             " imu_rpy:=",
-            f"\"{os.environ.get('PANTHER_IMU_ORIENTATION_R', '3.14')} {os.environ.get('PANTHER_IMU_ORIENTATION_P', '-1.57')} {os.environ.get('PANTHER_IMU_ORIENTATION_Y', '0.0')}\"",
+            f"'{imu_rot_r} {imu_rot_p} {imu_rot_y}'",
             " namespace:=",
             namespace,
             " components_config_path:=",

diff --git a/panther_controller/package.xml b/panther_controller/package.xml
@@ -22,6 +22,7 @@
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
+  <exec_depend>lynx_description</exec_depend>
   <exec_depend>mecanum_drive_controller</exec_depend>
   <exec_depend>panther_description</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == hardware">panther_hardware_interfaces</exec_depend>

diff --git a/panther_description/launch/load_urdf.launch.py b/panther_description/launch/load_urdf.launch.py
@@ -126,17 +126,20 @@ def generate_launch_description():
         choices=["WH01", "WH02", "WH04", "custom"],
     )
 
-    panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0")
     # Get URDF via xacro
+    imu_pos_x = os.environ.get("ROBOT_IMU_LOCALIZATION_X", "0.168")
+    imu_pos_y = os.environ.get("ROBOT_IMU_LOCALIZATION_Y", "0.028")
+    imu_pos_z = os.environ.get("ROBOT_IMU_LOCALIZATION_Z", "0.083")
+    imu_rot_r = os.environ.get("ROBOT_IMU_ORIENTATION_R", "3.14")
+    imu_rot_p = os.environ.get("ROBOT_IMU_ORIENTATION_P", "-1.57")
+    imu_rot_y = os.environ.get("ROBOT_IMU_ORIENTATION_Y", "0.0")
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
             PathJoinSubstitution(
                 [FindPackageShare("panther_description"), "urdf", "panther.urdf.xacro"]
             ),
-            " panther_version:=",
-            panther_version,
             " use_sim:=",
             use_sim,
             " wheel_config_file:=",
@@ -146,9 +149,9 @@ def generate_launch_description():
             " battery_config_file:=",
             battery_config_path,
             " imu_xyz:=",
-            f"\"{os.environ.get('PANTHER_IMU_LOCALIZATION_X', '0.168')} {os.environ.get('PANTHER_IMU_LOCALIZATION_Y', '0.028')} {os.environ.get('PANTHER_IMU_LOCALIZATION_Z', '0.083')}\"",
+            f"'{imu_pos_x} {imu_pos_y} {imu_pos_z}'",
             " imu_rpy:=",
-            f"\"{os.environ.get('PANTHER_IMU_ORIENTATION_R', '3.14')} {os.environ.get('PANTHER_IMU_ORIENTATION_P', '-1.57')} {os.environ.get('PANTHER_IMU_ORIENTATION_Y', '0.0')}\"",
+            f"'{imu_rot_r} {imu_rot_p} {imu_rot_y}'",
             " namespace:=",
             namespace,
             " components_config_path:=",

diff --git a/panther_description/urdf/panther.urdf.xacro b/panther_description/urdf/panther.urdf.xacro
@@ -1,7 +1,6 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot name="panther" xmlns:xacro="http://wiki.ros.org/xacro">
 
-  <xacro:arg name="panther_version" default="1.0" />
   <xacro:arg name="use_sim" default="false" />
   <!-- if lidars have to work correctly, use_gpu has to be true. -->
   <xacro:arg name="use_gpu" default="true" />
@@ -18,7 +17,6 @@
 
   <xacro:include filename="$(find panther_description)/urdf/panther_macro.urdf.xacro" ns="husarion" />
   <xacro:husarion.panther_robot
-    panther_version="$(arg panther_version)"
     use_sim="$(arg use_sim)"
     imu_xyz="$(arg imu_xyz)"
     imu_rpy="$(arg imu_rpy)"

diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro
@@ -3,8 +3,7 @@
 
   <xacro:macro
     name="panther_robot"
-    params="panther_version
-            wheel_config_file
+    params="wheel_config_file
             controller_config_file
             battery_config_file
             use_sim:=false
@@ -64,8 +63,6 @@
         <xacro:unless value="${use_sim}">
           <plugin>panther_hardware_interfaces/PantherSystem</plugin>
 
-          <param name="panther_version">${panther_version}</param>
-
           <param name="encoder_resolution">1600</param>
 
           <param name="gear_ratio">30.08</param>

diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml
@@ -31,6 +31,7 @@
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">husarion_gz_worlds</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">launch</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">launch_ros</exec_depend>
+  <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">lynx_description</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">nav2_common</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_controller</exec_depend>
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">panther_description</exec_depend>

diff --git a/panther_hardware_interfaces/test/utils/test_constants.hpp b/panther_hardware_interfaces/test/utils/test_constants.hpp
@@ -67,7 +67,6 @@ const std::string kPluginName =
 )";
 
 const std::map<std::string, std::string> kDefaultParamMap = {
-  {"panther_version", "1.2"},
   {"encoder_resolution", "1600"},
   {"gear_ratio", "30.08"},
   {"gearbox_efficiency", "0.75"},

diff --git a/panther_manager/launch/manager.launch.py b/panther_manager/launch/manager.launch.py
@@ -16,20 +16,18 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition
+from launch.conditions import UnlessCondition
 from launch.substitutions import (
     EnvironmentVariable,
     LaunchConfiguration,
     PathJoinSubstitution,
-    PythonExpression,
 )
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
 
-    panther_version = EnvironmentVariable(name="PANTHER_ROBOT_VERSION", default_value="1.0")
     panther_manager_dir = FindPackageShare("panther_manager")
 
     lights_bt_project_path = LaunchConfiguration("lights_bt_project_path")
@@ -55,7 +53,6 @@ def generate_launch_description():
             [panther_manager_dir, "behavior_trees", "PantherSafetyBT.btproj"]
         ),
         description="Path to BehaviorTree project file, responsible for safety and shutdown management.",
-        condition=IfCondition(PythonExpression([panther_version, ">=", "1.2"])),
     )
 
     shutdown_hosts_config_path = LaunchConfiguration("shutdown_hosts_config_path")
@@ -103,7 +100,7 @@ def generate_launch_description():
         ],
         namespace=namespace,
         emulate_tty=True,
-        condition=IfCondition(PythonExpression([panther_version, ">=", "1.2 and not ", use_sim])),
+        condition=UnlessCondition(use_sim),
     )
 
     actions = [