Delete panther_version argument and fix controller launch to use robo…

…t_model arg

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:=",

panther_battery/include/panther_battery/battery_driver_node.hpp

@@ -42,7 +42,6 @@ class BatteryDriverNode : public rclcpp::Node
   void BatteryPubTimerCB();
   void Initialize();
   void InitializeWithADCBattery();
-  void InitializeWithRoboteqBattery();
 
   static constexpr int kADCCurrentOffset = 625;
 

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,

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);
 
@@ -56,21 +55,14 @@ BatteryDriverNode::BatteryDriverNode(
 
 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();
+  } 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();
 
   RCLCPP_INFO(this->get_logger(), "Initialized successfully.");
 }
@@ -128,30 +120,6 @@ void BatteryDriverNode::InitializeWithADCBattery()
   RCLCPP_INFO(this->get_logger(), "Initialized battery driver using ADC data.");
 }
 
-void BatteryDriverNode::InitializeWithRoboteqBattery()
-{
-  RCLCPP_DEBUG(this->get_logger(), "Initializing with Roboteq data.");
-
-  this->declare_parameter<float>("roboteq/driver_state_timeout", 0.2);
-
-  const RoboteqBatteryParams battery_params = {
-    static_cast<float>(this->get_parameter("roboteq/driver_state_timeout").as_double()),
-    static_cast<std::size_t>(this->get_parameter("ma_window_len/voltage").as_int()),
-    static_cast<std::size_t>(this->get_parameter("ma_window_len/current").as_int()),
-  };
-
-  driver_state_sub_ = this->create_subscription<RobotDriverStateMsg>(
-    "hardware/robot_driver_state", 5,
-    [&](const RobotDriverStateMsg::SharedPtr msg) { driver_state_ = msg; });
-
-  battery_1_ = std::make_shared<RoboteqBattery>([&]() { return driver_state_; }, battery_params);
-
-  battery_publisher_ = std::make_shared<SingleBatteryPublisher>(
-    this->shared_from_this(), diagnostic_updater_, battery_1_);
-
-  RCLCPP_INFO(this->get_logger(), "Initialized battery driver using motor controllers data.");
-}
-
 void BatteryDriverNode::BatteryPubTimerCB()
 {
   if (!battery_publisher_) {

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 dual_battery = false);
   ~TestBatteryNode();
 
 protected:
@@ -63,43 +63,40 @@ 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 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()) {
-    device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0;
-    device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1;
-
-    params.push_back(rclcpp::Parameter("adc/device0", device0_path_.string()));
-    params.push_back(rclcpp::Parameter("adc/device1", device1_path_.string()));
-    params.push_back(rclcpp::Parameter("adc/path", testing::TempDir()));
-
-    // Create the device0 and device1 directories if they do not exist
-    std::filesystem::create_directory(device0_path_);
-    std::filesystem::create_directory(device1_path_);
-
-    // Create only files that are required for adc_node to start
-    int dual_bat_volt = dual_battery ? 800 : 1600;
-    WriteNumberToFile<int>(dual_bat_volt, std::filesystem::path(device0_path_ / "in_voltage0_raw"));
-    WriteNumberToFile<int>(800, std::filesystem::path(device0_path_ / "in_voltage1_raw"));
-    WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage2_raw"));
-    WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage3_raw"));
-    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage0_scale"));
-    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage1_scale"));
-    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage2_scale"));
-    WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage3_scale"));
-
-    WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage0_raw"));
-    WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage1_raw"));
-    WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage2_raw"));
-    WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage3_raw"));
-    WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage0_scale"));
-    WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage1_scale"));
-    WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage2_scale"));
-    WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage3_scale"));
-  }
+
+  device0_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice0;
+  device1_path_ = std::filesystem::path(testing::TempDir()) / kADCDevice1;
+
+  params.push_back(rclcpp::Parameter("adc/device0", device0_path_.string()));
+  params.push_back(rclcpp::Parameter("adc/device1", device1_path_.string()));
+  params.push_back(rclcpp::Parameter("adc/path", testing::TempDir()));
+
+  // Create the device0 and device1 directories if they do not exist
+  std::filesystem::create_directory(device0_path_);
+  std::filesystem::create_directory(device1_path_);
+
+  // Create only files that are required for adc_node to start
+  int dual_bat_volt = dual_battery ? 800 : 1600;
+  WriteNumberToFile<int>(dual_bat_volt, std::filesystem::path(device0_path_ / "in_voltage0_raw"));
+  WriteNumberToFile<int>(800, std::filesystem::path(device0_path_ / "in_voltage1_raw"));
+  WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage2_raw"));
+  WriteNumberToFile<int>(2, std::filesystem::path(device0_path_ / "in_voltage3_raw"));
+  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage0_scale"));
+  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage1_scale"));
+  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage2_scale"));
+  WriteNumberToFile<float>(2.0, std::filesystem::path(device0_path_ / "in_voltage3_scale"));
+
+  WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage0_raw"));
+  WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage1_raw"));
+  WriteNumberToFile<int>(600, std::filesystem::path(device1_path_ / "in_voltage2_raw"));
+  WriteNumberToFile<int>(1400, std::filesystem::path(device1_path_ / "in_voltage3_raw"));
+  WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage0_scale"));
+  WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage1_scale"));
+  WriteNumberToFile<float>(2.0, std::filesystem::path(device1_path_ / "in_voltage2_scale"));
+  WriteNumberToFile<float>(1.0, std::filesystem::path(device1_path_ / "in_voltage3_scale"));
 
   rclcpp::NodeOptions options;
   options.parameter_overrides(params);

panther_controller/launch/controller.launch.py

@@ -142,19 +142,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 +170,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:=",

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:=",

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)"

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>

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"},

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 = [