Merge pull request #304 from husarion/ros2-control-fix

Ros2 control fix
husarion · May 15, 2024 · 5eee217 · 5eee217
2 parents d2cb9a3 + 9c0ff22
commit 5eee217
Show file tree

Hide file tree

Showing 8 changed files with 9 additions and 16 deletions.
diff --git a/README.md b/README.md
@@ -38,7 +38,9 @@ if [ "$HUSARION_ROS_BUILD_TYPE" == "simulation" ]; then
   vcs import src < src/panther_ros/panther/panther_simulation.repos
 fi
 
-cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_controllers
+cp -r src/ros2_controllers/diff_drive_controller src
+cp -r src/ros2_controllers/imu_sensor_broadcaster src
+rm -rf src/ros2_controllers
 
 sudo rosdep init
 rosdep update --rosdistro $ROS_DISTRO

diff --git a/panther_controller/config/WH01_controller.yaml b/panther_controller/config/WH01_controller.yaml
@@ -13,7 +13,7 @@
   # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications
   imu_broadcaster:
     ros__parameters:
-      use_namespace_as_sensor_name_prefix: false
+      use_namespace_as_sensor_name_prefix: true
 
       sensor_name: imu
       frame_id: imu_link
@@ -28,7 +28,6 @@
     ros__parameters:
       left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"]
       right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"]
-      tf_frame_prefix_enable: false
 
       wheel_separation: 0.697
       wheel_radius: 0.1825

diff --git a/panther_controller/config/WH02_controller.yaml b/panther_controller/config/WH02_controller.yaml
@@ -13,7 +13,7 @@
   # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications
   imu_broadcaster:
     ros__parameters:
-      use_namespace_as_sensor_name_prefix: false
+      use_namespace_as_sensor_name_prefix: true
 
       sensor_name: imu
       frame_id: imu_link
@@ -30,7 +30,6 @@
       front_right_wheel_name: fr_wheel_joint
       rear_left_wheel_name: rl_wheel_joint
       rear_right_wheel_name: rr_wheel_joint
-      tf_frame_prefix_enable: false
 
       wheel_separation_x: 0.44
       wheel_separation_y: 0.6785

diff --git a/panther_controller/config/WH04_controller.yaml b/panther_controller/config/WH04_controller.yaml
@@ -13,7 +13,7 @@
   # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications
   imu_broadcaster:
     ros__parameters:
-      use_namespace_as_sensor_name_prefix: false
+      use_namespace_as_sensor_name_prefix: true
 
       sensor_name: imu
       frame_id: imu_link
@@ -28,7 +28,6 @@
     ros__parameters:
       left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"]
       right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"]
-      tf_frame_prefix_enable: false
 
       wheel_separation: 0.616
       wheel_radius: 0.1016

diff --git a/panther_description/urdf/gazebo.urdf.xacro b/panther_description/urdf/gazebo.urdf.xacro
@@ -76,7 +76,7 @@
     </xacro:unless>
 
     <gazebo reference="${reference_frame}">
-      <sensor name="imu" type="imu">
+      <sensor name="${ns}imu" type="imu">
         <plugin filename="ignition-gazebo-imu-system" name="ignition::gazebo::systems::Imu" />
         <always_on>true</always_on>
         <update_rate>50</update_rate>

diff --git a/panther_description/urdf/panther_macro.urdf.xacro b/panther_description/urdf/panther_macro.urdf.xacro
@@ -138,7 +138,7 @@
       </joint>
 
       <xacro:if value="${use_sim}">
-        <sensor name="imu">
+        <sensor name="${ns}imu">
           <state_interface name="orientation.x" />
           <state_interface name="orientation.y" />
           <state_interface name="orientation.z" />
@@ -174,7 +174,7 @@
           <param name="world_frame">enu</param>
         </hardware>
 
-        <sensor name="imu">
+        <sensor name="${ns}imu">
           <state_interface name="orientation.x" />
           <state_interface name="orientation.y" />
           <state_interface name="orientation.z" />

diff --git a/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_imu.hpp b/panther_hardware_interfaces/include/panther_hardware_interfaces/panther_imu.hpp
@@ -144,7 +144,6 @@ class PantherImuSensor : public hardware_interface::SensorInterface
   std::vector<double> imu_sensor_state_;
   rclcpp::Logger logger_{rclcpp::get_logger("PantherImuSensor")};
 
-  inline static const std::string kImuSensorName = "imu";
   inline static const std::array<std::string, kImuInterfacesSize> kImuInterfacesNames = {
     "orientation.x",         "orientation.y",         "orientation.z",      "orientation.w",
     "angular_velocity.x",    "angular_velocity.y",    "angular_velocity.z", "linear_acceleration.x",

diff --git a/panther_hardware_interfaces/src/panther_imu.cpp b/panther_hardware_interfaces/src/panther_imu.cpp
@@ -176,11 +176,6 @@ void PantherImuSensor::CheckSensorName() const
   if (!info_.sensors.size()) {
     throw std::runtime_error("Sensor is not defined in urdf!");
   }
-
-  if (info_.sensors[0].name != kImuSensorName) {
-    throw std::runtime_error(
-      "Wrong sensor name: '" + info_.sensors[0].name + "', '" + kImuSensorName + "' expected.");
-  }
 }
 
 void PantherImuSensor::CheckStatesSize() const