Add localizer package back in and add base-link in properly

.gitignore

@@ -207,3 +207,5 @@ cython_debug/
 *.out
 *.app
 
+.DS_Store
+*.mcap

dependencies.sh

@@ -4,6 +4,8 @@ set -euo pipefail
 
 pkgs=(
     python3-colcon-common-extensions
+    libgeographic-dev
+    geographiclib-tools
 )
 
 for pkg in "${pkgs[@]}"; do

src/description/CMakeLists.txt

@@ -11,9 +11,11 @@ install(DIRECTORY
     config
     launch
     meshes
+    models
     rviz
     ros2_control
     urdf
+    worlds
     DESTINATION share/${PROJECT_NAME}/
 )
 

src/description/rviz/athena_drive.rviz

@@ -62,7 +62,7 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        chassis_base_link:
+        base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -170,7 +170,7 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: true
-        chassis_base_link:
+        base_link:
           Value: true
         propulsion_1_link:
           Value: true
@@ -214,7 +214,7 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        chassis_base_link:
+        base_link:
           suspension_front_left_link:
             swerve_mount_1_link:
               steer_1_link:
@@ -242,7 +242,7 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: chassis_base_link
+    Fixed Frame: base_link
     Frame Rate: 30
   Name: root
   Tools:

src/description/rviz/new_athena_drive.rviz

@@ -86,7 +86,7 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        chassis_base_link:
+        base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false

src/description/urdf/athena_drive.urdf.xacro

@@ -14,6 +14,7 @@
   <xacro:include filename="$(find description)/urdf/athena_drive/athena_drive_macro.ros2_control.xacro"/>
   <xacro:include filename="imu.xacro"/>
   <xacro:include filename="gps.xacro"/>
+  <xacro:include filename="ground_truth_odom.xacro"/>
 
 
   <!-- Load robot's macro with parameters -->
@@ -37,6 +38,17 @@
   <!-- Contains description of drive -->
   <xacro:description/>
 
+
+  <xacro:imu_sensor parent="base_link">
+    <origin xyz="0 0 0.0" rpy="0 0 0"/>
+  </xacro:imu_sensor>
+
+  <xacro:gps_sensor parent="base_link">
+    <origin xyz="0 0 0.0" rpy="0 0 0"/>
+  </xacro:gps_sensor>
+
+  <xacro:ground_truth_odometry/>
+
   <xacro:if value="$(arg use_mock_hardware)">
     <xacro:athena_drive_ros2_control
     name="athena_drive"

src/description/urdf/athena_drive_description.urdf.xacro

@@ -4,7 +4,7 @@
      For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro name="description">
-        <link name="chassis_base_link">
+        <link name="base_link">
             <inertial>
                 <origin xyz="0.775305042176281 -0.0692748019047986 -0.475175318827691" rpy="0 0 0" />
                 <mass value="44.0454429414672" />
@@ -52,7 +52,7 @@
 
         <joint name="suspension_front_left_joint" type="fixed">
             <origin xyz="0.61263 0.22146 -0.74592" rpy="1.5708 0 0.094743" />
-            <parent link="chassis_base_link" />
+            <parent link="base_link" />
             <child link="suspension_front_left_link" />
             <axis xyz="0 0 -1" />
             <limit lower="-1.0471975512" upper="1.0471975512" effort="200" velocity="3" />
@@ -325,7 +325,7 @@
 
         <joint name="suspension_front_right_joint" type="fixed">
             <origin xyz="0.66803 -0.36146 -0.74592" rpy="1.5708 0 0.094743" />
-            <parent link="chassis_base_link" />
+            <parent link="base_link" />
             <child link="suspension_front_right_link" />
             <axis xyz="0 0 1" />
             <limit lower="-1.0471975512" upper="1.0471975512" effort="200" velocity="3" />

src/description/urdf/gps.xacro

@@ -1,13 +1,13 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <xacro:macro name="gps_sensor" params="parent prefix *origin">
-        <joint name="${prefix}_gps_joint" type="fixed">
+    <xacro:macro name="gps_sensor" params="parent *origin">
+        <joint name="gnss_joint" type="fixed">
             <xacro:insert_block name="origin" />
             <parent link="${parent}" />
-            <child link="${prefix}_gps_link" />
+            <child link="gnss_link" />
         </joint>
 
-        <link name="${prefix}_gps_link">
+        <link name="gnss_link">
             <visual>
                 <geometry>
                     <box size="0.05 0.05 0.05" />
@@ -18,13 +18,13 @@
             </visual>
         </link>
 
-        <gazebo reference="${prefix}_gps_link">
-            <sensor name="${prefix}_navsat" type="navsat">
+        <gazebo reference="gnss_link">
+            <sensor name="navsat" type="navsat">
                 <always_on>true</always_on>
-                <update_rate>10</update_rate>
-                <topic>/gps</topic>
-                <plugin filename="libgz-sim-navsat-system.so" name="gz::sim::systems::NavSat">
-                </plugin>
+                <update_rate>50</update_rate>
+                <topic>/gps/fix</topic>
+                <ign_frame_id>gnss_link</ign_frame_id>
+                <plugin filename="ignition-gazebo-navsat-system" name="ignition::gazebo::systems::NavSat"/>
             </sensor>
         </gazebo>
     </xacro:macro>

src/description/urdf/ground_truth_odom.xacro

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="ground_truth_odometry" params="odom_frame:=odom robot_base_frame:=base_link odom_topic:=/odom/ground_truth update_rate:=50">
+        <gazebo>
+            <plugin filename="ignition-gazebo-odometry-publisher-system" name="ignition::gazebo::systems::OdometryPublisher">
+                <odom_frame>${odom_frame}</odom_frame>
+                <robot_base_frame>${robot_base_frame}</robot_base_frame>
+                <odom_topic>${odom_topic}</odom_topic>
+                <odom_covariance_topic>${odom_topic}/covariance</odom_covariance_topic>
+                <tf_topic>${odom_topic}/tf</tf_topic>
+                <dimensions>3</dimensions>
+                <odom_publish_frequency>${update_rate}</odom_publish_frequency>
+            </plugin>
+        </gazebo>
+    </xacro:macro>
+</robot>

src/description/urdf/imu.xacro

@@ -1,13 +1,13 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <xacro:macro name="imu_sensor" params="parent prefix *origin">
-        <joint name="${prefix}_imu_joint" type="fixed">
+    <xacro:macro name="imu_sensor" params="parent *origin">
+        <joint name="imu_joint" type="fixed">
             <xacro:insert_block name="origin" />
             <parent link="${parent}" />
-            <child link="${prefix}_imu_link" />
+            <child link="imu_link" />
         </joint>
 
-        <link name="${prefix}_imu_link">
+        <link name="imu_link">
             <visual>
                 <geometry>
                     <box size="0.05 0.05 0.05" />
@@ -18,12 +18,13 @@
             </visual>
         </link>
 
-        <gazebo reference="${prefix}_imu_link">
-            <sensor name="${prefix}_imu" type="imu">
+        <gazebo reference="imu_link">
+            <sensor name="imu" type="imu">
                 <always_on>true</always_on>
-                <update_rate>100</update_rate>
+                <update_rate>200</update_rate>
                 <topic>/imu</topic>
-                <plugin filename="libgz-sim-imu-system.so" name="gz::sim::systems::Imu"/>
+                <ign_frame_id>imu_link</ign_frame_id>
+                <plugin filename="libignition-gazebo-imu-system.so" name="ignition::gazebo::systems::Imu"/>
             </sensor>
         </gazebo>
     </xacro:macro>

src/description/worlds/empty.sdf

@@ -0,0 +1,72 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+  <world name="default">
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <world_frame_orientation>ENU</world_frame_orientation>
+      <latitude_deg>38.42391162772634</latitude_deg>
+      <longitude_deg>-110.78490558433397</longitude_deg>
+      <elevation>0.0</elevation>
+      <heading_deg>0.0</heading_deg>
+    </spherical_coordinates>
+
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="ignition::gazebo::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>

src/simulation/launch/bridge.launch.py

@@ -29,7 +29,17 @@ def generate_launch_description():
             name='clock_bridge',
             output='screen',
             arguments=[
-                '/clock@rosgraph_msgs/msg/Clock@gz.msgs.Clock',
+                '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
+            ]
+        ),
+
+        Node(
+            package='ros_gz_bridge',
+            executable='parameter_bridge',
+            name='ground_truth_odom_bridge',
+            output='screen',
+            arguments=[
+                '/odom/ground_truth@nav_msgs/msg/Odometry@gz.msgs.Odometry',
             ]
         ),
     ])

src/simulation/launch/gz_sim.launch.py

@@ -20,15 +20,22 @@ def generate_launch_description():
 
     gz_resource_path = SetEnvironmentVariable(
         name='GZ_SIM_RESOURCE_PATH',
-        value=[str(Path(pkg_description).parent.resolve())])
-
+        value=[
+            str(Path(pkg_description).parent.resolve()),
+            ':',
+            os.path.join(pkg_description, 'models')
+        ])
+
     gz_sim_launch = PathJoinSubstitution(
         [pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'])
-
+
+    world_path = PathJoinSubstitution(
+        [pkg_description, 'worlds', LaunchConfiguration('world')])
+
     gazebo_sim = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([gz_sim_launch]),
         launch_arguments=[
-            ('gz_args', [LaunchConfiguration('world'),
+            ('gz_args', [world_path,
                         ' -r',
                         ' -v 4']
             )