From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Thu, 8 May 2025 02:39:54 +0000 (-0300)
Subject: Port to Jazzy.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=af88b953723d61f30faa22afbcfb35c60c7a6a18;p=twil.git

Port to Jazzy.
---

diff --git a/twil_2dnav/launch/gazebo.launch.xml b/twil_2dnav/launch/gazebo.launch.xml
index 3afe2ad..d494253 100644
--- a/twil_2dnav/launch/gazebo.launch.xml
+++ b/twil_2dnav/launch/gazebo.launch.xml
@@ -1,7 +1,7 @@
 <!--******************************************************************************
 	                     Twil 2D Navigation
 	                     Gazebo Launch File
-          Copyright (C) 2019, 2022 Walter Fetter Lages <w.fetter@ieee.org>
+          Copyright (C) 2019..2024 Walter Fetter Lages <w.fetter@ieee.org>
 
         This program is free software: you can redistribute it and/or modify
         it under the terms of the GNU General Public License as published by
@@ -20,7 +20,7 @@
 *******************************************************************************-->
 
 <launch>
-	<arg name="pause" default="true"/>
+	<arg name="pause" default="false"/>
 	<arg name="gui" default="true"/>
 	<arg name="use_sim_time" default="true"/>
 	<arg name="wam" default="false"/>
diff --git a/twil_bringup/config/dynamics_linearizing_controller.yaml b/twil_bringup/config/dynamics_linearizing_controller.yaml
index 6fa5662..f13808a 100644
--- a/twil_bringup/config/dynamics_linearizing_controller.yaml
+++ b/twil_bringup/config/dynamics_linearizing_controller.yaml
@@ -2,6 +2,7 @@
 
 joint_state_broadcaster:
   ros__parameters:
+    type: joint_state_broadcaster/JointStateBroadcaster
     extra_joints:
       - caster_wheel_joint
       - caster_base_joint
@@ -11,6 +12,7 @@ joint_state_broadcaster:
 
 dynamics_linearizing_controller:
   ros__parameters:
+    type: effort_controllers/DynamicsLinearizingController
     joints:
       - left_wheel_joint
       - right_wheel_joint
diff --git a/twil_bringup/config/joint_effort_controller.yaml b/twil_bringup/config/joint_effort_controller.yaml
index a27a7ec..d0dd1c2 100644
--- a/twil_bringup/config/joint_effort_controller.yaml
+++ b/twil_bringup/config/joint_effort_controller.yaml
@@ -1,5 +1,6 @@
 joint_state_broadcaster:
         ros__parameters:
+                type: joint_state_broadcaster/JointStateBroadcaster
                 extra_joints:
                         - caster_wheel_joint
                         - caster_base_joint
@@ -9,10 +10,12 @@ joint_state_broadcaster:
 
 left_wheel_joint_effort_controller:
         ros__parameters:
+                type: forward_command_controller/ForwardCommandController
                 joints: [left_wheel_joint]
                 interface_name: effort
 
 right_wheel_joint_effort_controller:
         ros__parameters:
+                type: forward_command_controller/ForwardCommandController
                 joints: [right_wheel_joint]
                 interface_name: effort
diff --git a/twil_bringup/config/joint_velocity_controller.yaml b/twil_bringup/config/joint_velocity_controller.yaml
index 43f0748..c2fa9c8 100644
--- a/twil_bringup/config/joint_velocity_controller.yaml
+++ b/twil_bringup/config/joint_velocity_controller.yaml
@@ -1,5 +1,6 @@
 joint_state_broadcaster:
         ros__parameters:
+                type: joint_state_broadcaster/JointStateBroadcaster
                 extra_joints:
                         - caster_wheel_joint
                         - caster_base_joint
@@ -9,12 +10,14 @@ joint_state_broadcaster:
 
 left_wheel_joint_velocity_controller:
         ros__parameters:
+                type: effort_controllers/JointVelocityController
                 joint: left_wheel_joint
                 pid: {p: 0.0, i: 0.0, d: 0.0}
                 interface_name: effort
 
 right_wheel_joint_velocity_controller:
         ros__parameters:
+                type: effort_controllers/JointVelocityController
                 joint: right_wheel_joint
                 pid: {p: 0.0, i: 0.0, d: 0.0}
                 interface_name: effort
diff --git a/twil_bringup/config/nonsmooth_backstep_controller.yaml b/twil_bringup/config/nonsmooth_backstep_controller.yaml
index 83aef65..d2d9afd 100644
--- a/twil_bringup/config/nonsmooth_backstep_controller.yaml
+++ b/twil_bringup/config/nonsmooth_backstep_controller.yaml
@@ -2,6 +2,7 @@
 
 joint_state_broadcaster:
   ros__parameters:
+    type: joint_state_broadcaster/JointStateBroadcaster
     extra_joints:
       - caster_wheel_joint
       - caster_base_joint
@@ -11,6 +12,7 @@ joint_state_broadcaster:
 
 nonsmooth_backstep_controller:
   ros__parameters:
+    type: effort_controllers/NonSmoothBackstepController
     joints:
       - left_wheel_joint
       - right_wheel_joint
diff --git a/twil_bringup/config/nonsmooth_backstep_controller_step.yaml b/twil_bringup/config/nonsmooth_backstep_controller_step.yaml
index 1e745e9..83b3474 100644
--- a/twil_bringup/config/nonsmooth_backstep_controller_step.yaml
+++ b/twil_bringup/config/nonsmooth_backstep_controller_step.yaml
@@ -2,6 +2,7 @@
 
 joint_state_broadcaster:
   ros__parameters:
+    type: joint_state_broadcaster/JointStateBroadcaster
     extra_joints:
       - caster_wheel_joint
       - caster_base_joint
@@ -11,6 +12,7 @@ joint_state_broadcaster:
 
 nonsmooth_backstep_controller:
   ros__parameters:
+    type: effort_controllers/NonSmoothBackstepController
     joints:
       - left_wheel_joint
       - right_wheel_joint
diff --git a/twil_bringup/config/twist_mrac_linearizing_controller.yaml b/twil_bringup/config/twist_mrac_linearizing_controller.yaml
index acfedd0..981dc91 100644
--- a/twil_bringup/config/twist_mrac_linearizing_controller.yaml
+++ b/twil_bringup/config/twist_mrac_linearizing_controller.yaml
@@ -2,6 +2,7 @@
 
 joint_state_broadcaster:
   ros__parameters:
+    type: joint_state_broadcaster/JointStateBroadcaster
     extra_joints:
       - caster_wheel_joint
       - caster_base_joint
@@ -11,6 +12,7 @@ joint_state_broadcaster:
 
 twist_mrac_linearizing_controller:
   ros__parameters:
+    type: effort_controllers/TwistMracLinearizingController
     joints:
       - left_wheel_joint
       - right_wheel_joint
diff --git a/twil_bringup/launch/dynamics_linearizing_controller.launch.xml b/twil_bringup/launch/dynamics_linearizing_controller.launch.xml
index d21e33a..da83b8a 100644
--- a/twil_bringup/launch/dynamics_linearizing_controller.launch.xml
+++ b/twil_bringup/launch/dynamics_linearizing_controller.launch.xml
@@ -23,8 +23,8 @@
 	<arg name="config" default="$(find-pkg-share twil_bringup)/config/dynamics_linearizing_controller.yaml"/>
 
 	<node name="controller_spawner" pkg="controller_manager" exec="spawner"
-		args="-t effort_controllers/DynamicsLinearizingController -p $(var config) dynamics_linearizing_controller"/>
+		args="-p $(var config) dynamics_linearizing_controller"/>
 
 	<node name="joint_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
-		args="-t joint_state_broadcaster/JointStateBroadcaster -p $(var config) joint_state_broadcaster"/>
+		args="-p $(var config) joint_state_broadcaster"/>
 </launch>
diff --git a/twil_bringup/launch/gazebo.launch.xml b/twil_bringup/launch/gazebo.launch.xml
index 2e5d498..ee9e592 100644
--- a/twil_bringup/launch/gazebo.launch.xml
+++ b/twil_bringup/launch/gazebo.launch.xml
@@ -25,7 +25,8 @@
 	<arg name="use_sim_time" default="true"/>
 	<arg name="wam" default="false"/>
 	<arg name="gps" default="false"/>
-	<arg name="world" default ="worlds/empty_sky.world"/>
+	<arg name="imu" default="false"/>
+	<arg name="world" default ="$(find-pkg-share ufrgs_gazebo)/worlds/empty_sky_porto_alegre.world"/>
 
 	<arg name="controller" default="joint_effort_controller"/>
 	<arg name="config" default="$(find-pkg-share twil_bringup)/config/$(var controller).yaml"/>
@@ -36,6 +37,7 @@
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
 		<arg name="wam" value="$(var wam)"/>
 		<arg name="gps" value="$(var gps)"/>
+		<arg name="imu" value="$(var imu)"/>
 		<arg name="world" value="$(var world)"/>
 	</include>
 
diff --git a/twil_bringup/launch/gazebo8.launch.xml b/twil_bringup/launch/gazebo8.launch.xml
index fa3a592..4ee590f 100644
--- a/twil_bringup/launch/gazebo8.launch.xml
+++ b/twil_bringup/launch/gazebo8.launch.xml
@@ -25,6 +25,7 @@
 	<arg name="use_sim_time" default="true"/>
 	<arg name="wam" default="false"/>
 	<arg name="gps" default="false"/>
+	<arg name="imu" default="false"/>
 	<arg name="world" default ="worlds/empty_sky.world"/>
 
 	<arg name="controller" default="nonsmooth_backstep_controller"/>
@@ -36,6 +37,7 @@
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
 		<arg name="wam" value="$(var wam)"/>
 		<arg name="gps" value="$(var gps)"/>
+		<arg name="imu" value="$(var imu)"/>
 		<arg name="world" value="$(var world)"/>
 		<arg name="controller" value="$(var controller)"/>
 		<arg name="config" value="$(var config)"/>
diff --git a/twil_bringup/launch/joint_effort_controller.launch.xml b/twil_bringup/launch/joint_effort_controller.launch.xml
index 830c08d..5e0f4bb 100644
--- a/twil_bringup/launch/joint_effort_controller.launch.xml
+++ b/twil_bringup/launch/joint_effort_controller.launch.xml
@@ -23,11 +23,11 @@
 	<arg name="config" default="$(find-pkg-share twil_bringup)/config/joint_effort_controller.yaml"/>
 	
 	<node name="left_wheel_controller_spawner" pkg="controller_manager" exec="spawner"
-		args="-t forward_command_controller/ForwardCommandController -p $(var config) left_wheel_joint_effort_controller"/>
+		args="-p $(var config) left_wheel_joint_effort_controller"/>
 
 	<node name="right_wheel_controller_spawner" pkg="controller_manager" exec="spawner"
-		args="-t forward_command_controller/ForwardCommandController -p $(var config) right_wheel_joint_effort_controller"/>
+		args="-p $(var config) right_wheel_joint_effort_controller"/>
 
 	<node name="joint_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
-		args="-t joint_state_broadcaster/JointStateBroadcaster -p $(var config) joint_state_broadcaster"/>
+		args="-p $(var config) joint_state_broadcaster"/>
 </launch>
diff --git a/twil_bringup/launch/joint_velocity_controller.launch.xml b/twil_bringup/launch/joint_velocity_controller.launch.xml
index cd7d396..163e91a 100644
--- a/twil_bringup/launch/joint_velocity_controller.launch.xml
+++ b/twil_bringup/launch/joint_velocity_controller.launch.xml
@@ -23,11 +23,11 @@
 	<arg name="config" default="$(find-pkg-share twil_bringup)/config/joint_velocity_controller.yaml"/>
 
 	<node name="left_wheel_controller_spawner" pkg="controller_manager" exec="spawner"
-		args="-t effort_controllers/JointVelocityController -p $(var config) left_wheel_velocity_controller"/>
+		args="-p $(var config) left_wheel_velocity_controller"/>
 
 	<node name="right_wheel_controller_spawner" pkg="controller_manager" exec="spawner"
-		args="-t effort_controllers/JointVelocityController -p $(var config) right_wheel_velocity_controller"/>
+		args="-p $(var config) right_wheel_velocity_controller"/>
 		
 	<node name="joint_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
-		args="-t joint_state_broadcaster/JointStateBroadcaster -p $(var config) joint_state_broadcaster"/>
+		args="-p $(var config) joint_state_broadcaster"/>
 </launch>
diff --git a/twil_bringup/launch/nonsmooth_backstep_controller.launch.xml b/twil_bringup/launch/nonsmooth_backstep_controller.launch.xml
index 44026cf..31c8ebf 100644
--- a/twil_bringup/launch/nonsmooth_backstep_controller.launch.xml
+++ b/twil_bringup/launch/nonsmooth_backstep_controller.launch.xml
@@ -23,8 +23,8 @@
 	<arg name="config" default="$(find-pkg-share twil_bringup)/config/nonsmooth_backstep_controller.yaml"/>
 
 	<node name="controller_spawner" pkg="controller_manager" exec="spawner"
-		args="-t effort_controllers/NonSmoothBackstepController -p $(var config) nonsmooth_backstep_controller"/>
+		args="-p $(var config) nonsmooth_backstep_controller"/>
 
 	<node name="joint_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
-		args="-t joint_state_broadcaster/JointStateBroadcaster -p $(var config) joint_state_broadcaster"/>
+		args="-p $(var config) joint_state_broadcaster"/>
 </launch>
diff --git a/twil_bringup/launch/twist_mrac_linearizing_controller.launch.xml b/twil_bringup/launch/twist_mrac_linearizing_controller.launch.xml
index 3cf65f9..5b77893 100644
--- a/twil_bringup/launch/twist_mrac_linearizing_controller.launch.xml
+++ b/twil_bringup/launch/twist_mrac_linearizing_controller.launch.xml
@@ -23,8 +23,8 @@
 	<arg name="config" default="$(find-pkg-share twil_bringup)/config/twist_mrac_linearizing_controller.yaml"/>
 
 	<node name="controller_spawner" pkg="controller_manager" exec="spawner"
-		args="-t effort_controllers/TwistMracLinearizingController -p $(var config) twist_mrac_linearizing_controller"/>
+		args="-p $(var config) twist_mrac_linearizing_controller"/>
 
 	<node name="joint_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
-		args="-t joint_state_broadcaster/JointStateBroadcaster -p $(var config) joint_state_broadcaster"/>
+		args="-p $(var config) joint_state_broadcaster"/>
 </launch>
diff --git a/twil_description/config/ros_gz_bridge.yaml b/twil_description/config/ros_gz_bridge.yaml
new file mode 100644
index 0000000..57b38d9
--- /dev/null
+++ b/twil_description/config/ros_gz_bridge.yaml
@@ -0,0 +1,61 @@
+- ros_topic_name: "/clock"
+  gz_topic_name: "/clock"
+  ros_type_name: "rosgraph_msgs/msg/Clock"
+  gz_type_name: "gz.msgs.Clock"
+#  subscriber_queue: 5       # Default 10
+#  publisher_queue: 6        # Default 10
+#  lazy: true                # Default "false"
+  direction: GZ_TO_ROS      # Default "BIDIRECTIONAL" - Bridge both directions
+                            # "GZ_TO_ROS" - Bridge Ignition topic to ROS
+                            # "ROS_TO_GZ" - Bridge ROS topic to Ignition
+
+# nav_msgs/msg/Odometry has a covariance field.  There is no motive to take
+# the version without covariance from Gazebo
+#- ros_topic_name: "/ground_truth"
+#  gz_topic_name: "/model/twil/odometry"
+#  ros_type_name: "nav_msgs/msg/Odometry"
+#  gz_type_name: "gz.msgs.Odometry"
+#  direction: GZ_TO_ROS
+
+- ros_topic_name: "/ground_truth"
+  gz_topic_name: "/model/twil/odometry_with_covariance"
+  ros_type_name: "nav_msgs/msg/Odometry"
+  gz_type_name: "gz.msgs.OdometryWithCovariance"
+  direction: GZ_TO_ROS
+  
+- ros_topic_name: "/pose"
+  gz_topic_name: "/model/twil/pose"
+  ros_type_name: "geometry_msgs/msg/PoseArray"
+  gz_type_name: "gz.msgs.Pose_V"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/gnss/fix"
+  gz_topic_name: "/gnss/fix"
+  ros_type_name: "sensor_msgs/msg/NavSatFix"
+  gz_type_name: "gz.msgs.NavSat"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/imu/data"
+  gz_topic_name: "/imu/data"
+  ros_type_name: "sensor_msgs/msg/Imu"
+  gz_type_name: "gz.msgs.IMU"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/imu/mag"
+  gz_topic_name: "/imu/mag"
+  ros_type_name: "sensor_msgs/msg/MagneticField"
+  gz_type_name: "gz.msgs.Magnetometer"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/depth_camera/camera_info"
+  gz_topic_name: "/depth_camera/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/depth_camera/image_raw"
+  gz_topic_name: "/depth_camera/image_raw"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
+  
\ No newline at end of file
diff --git a/twil_description/launch/gazebo.launch.xml b/twil_description/launch/gazebo.launch.xml
index 1f53b26..f29b4f4 100644
--- a/twil_description/launch/gazebo.launch.xml
+++ b/twil_description/launch/gazebo.launch.xml
@@ -1,7 +1,7 @@
 <!--******************************************************************************
 	                    Twil Description
-	                   Gazebo Launch File
-          Copyright (C) 2014..2022 Walter Fetter Lages <w.fetter@ieee.org>
+	                  Gazebo Sim Launch File
+          Copyright (C) 2014..2025 Walter Fetter Lages <w.fetter@ieee.org>
 
         This program is free software: you can redistribute it and/or modify
         it under the terms of the GNU General Public License as published by
@@ -20,20 +20,24 @@
 *******************************************************************************-->
 
 <launch>
-	<arg name="pause" default="true"/>
+	<arg name="pause" default="false"/>
 	<arg name="gui" default="true"/>
 	<arg name="use_sim_time" default="true"/>
 	<arg name="wam" default="false"/>
 	<arg name="d435" default="false"/>
 	<arg name="gps" default="false"/>
-	<arg name="world" default ="worlds/empty_sky.world"/>
+	<arg name="imu" default="false"/>
+	<arg name="world" default ="empty.sdf"/>
 
-	<include file="$(find-pkg-share gazebo_ros)/launch/gazebo.launch.py">
-		<arg name="pause" value="$(var pause)"/>
-		<arg name="gui" value="$(var gui)"/>
+	<arg unless="$(var pause)" name="gz_pause" default="-r"/>
+	<arg if="$(var pause)" name="gz_pause" default=""/>
+
+	<arg unless="$(var gui)" name="gz_gui" default="-s"/>
+	<arg if="$(var gui)" name="gz_gui" default=""/>
+
+	<include file="$(find-pkg-share ros_gz_sim)/launch/gz_sim.launch.py">
+		<arg name="gz_args" value="$(var gz_pause) $(var gz_gui) $(var world)"/>
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
-		<arg name="world" value="$(var world)"/>
-		<arg name="extra_gazebo_args" value="--ros-args --params-file $(find-pkg-share twil_description)/config/gazebo.yaml"/>
 	</include>
 
 	<include file="$(find-pkg-share twil_description)/launch/twil.launch.xml" >
@@ -41,7 +45,13 @@
 		<arg name="wam" value="$(var wam)"/>
 		<arg name="d435" value="$(var d435)"/>
 		<arg name="gps" value="$(var gps)"/>
+		<arg name="imu" value="$(var imu)"/>
 	</include>
 
-	<node name="twil_spawner" pkg="gazebo_ros" exec="spawn_entity.py" args="-topic robot_description -entity twil"/>
+	<node name="twil_spawner" pkg="ros_gz_sim" exec="create" args="-topic robot_description -name twil"/>
+
+	<node name="ros_gz_bridge" pkg="ros_gz_bridge" exec="parameter_bridge">
+		<param name="config_file" value="$(find-pkg-share twil_description)/config/ros_gz_bridge.yaml"/>
+		<param name="use_sim_time" value="$(var use_sim_time)"/>
+	</node>
 </launch>
diff --git a/twil_description/launch/twil.launch.xml b/twil_description/launch/twil.launch.xml
index 8b56e4b..ce406a7 100644
--- a/twil_description/launch/twil.launch.xml
+++ b/twil_description/launch/twil.launch.xml
@@ -1,6 +1,6 @@
 <!--******************************************************************************
 	                    Twil Description
-	                    Gazebo Launch File
+	                    Twil Launch File
           Copyright (C) 2014..2022 Walter Fetter Lages <w.fetter@ieee.org>
 
         This program is free software: you can redistribute it and/or modify
@@ -24,9 +24,10 @@
 	<arg name="wam" default="false"/>
 	<arg name="d435" default="false"/>
 	<arg name="gps" default="false"/>
+	<arg name="imu" default="false"/>
 
 	<node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
-		<param name="robot_description" value="$(command 'xacro $(find-pkg-share twil_description)/xacro/twil.urdf.xacro wam:=$(var wam) d435:=$(var d435) add_plug:=true gps:=$(var gps)')" type="str"/>
+		<param name="robot_description" value="$(command 'xacro $(find-pkg-share twil_description)/xacro/twil.urdf.xacro wam:=$(var wam) d435:=$(var d435) add_plug:=true gps:=$(var gps) imu:=$(var imu)')" type="str"/>
 		<param name="use_sim_time" value="$(var use_sim_time)"/>
 	</node>
 </launch>
diff --git a/twil_description/package.xml b/twil_description/package.xml
index 78af7c1..1913f55 100644
--- a/twil_description/package.xml
+++ b/twil_description/package.xml
@@ -18,6 +18,6 @@
   <export>
     <build_type>ament_cmake</build_type>
     <gazebo_ros gazebo_model_path="${prefix}/.."/>
-    <gazebo_ros gazebo_model_path="/opt/ros/humble/share/realsense2_description//.."/>
+    <gazebo_ros gazebo_model_path="/opt/ros/jazzy/share/realsense2_description/.."/>
   </export>
 </package>
diff --git a/twil_description/xacro/battery_bosch_12v.urdf.xacro b/twil_description/xacro/battery_bosch_12v.urdf.xacro
index 289089b..e9d5c57 100644
--- a/twil_description/xacro/battery_bosch_12v.urdf.xacro
+++ b/twil_description/xacro/battery_bosch_12v.urdf.xacro
@@ -27,7 +27,7 @@
     </collision>
   </link>
 
-  <joint name="${name}" type="fixed">
+  <joint name="${name}_joint" type="fixed">
     <xacro:insert_block name="origin" />
     <parent link="${parent}"/>
     <child link="${name}" />
diff --git a/twil_description/xacro/caster_support.urdf.xacro b/twil_description/xacro/caster_support.urdf.xacro
index dac2725..b32b34f 100644
--- a/twil_description/xacro/caster_support.urdf.xacro
+++ b/twil_description/xacro/caster_support.urdf.xacro
@@ -27,7 +27,7 @@
     </collision>
   </link>
 
-  <joint name="${name}" type="fixed">
+  <joint name="${name}_joint" type="fixed">
     <xacro:insert_block name="origin" />
     <parent link="${parent}"/>
     <child link="${name}" />
diff --git a/twil_description/xacro/eurocard.urdf.xacro b/twil_description/xacro/eurocard.urdf.xacro
index b28b461..d69d84e 100644
--- a/twil_description/xacro/eurocard.urdf.xacro
+++ b/twil_description/xacro/eurocard.urdf.xacro
@@ -32,7 +32,7 @@
     </collision>
   </link>
 
-  <joint name="${name}" type="fixed">
+  <joint name="${name}_joint" type="fixed">
     <xacro:insert_block name="origin" />
     <parent link="${parent}"/>
     <child link="${name}" />
diff --git a/twil_description/xacro/fixed_wheel.urdf.xacro b/twil_description/xacro/fixed_wheel.urdf.xacro
index a432dec..82522ed 100644
--- a/twil_description/xacro/fixed_wheel.urdf.xacro
+++ b/twil_description/xacro/fixed_wheel.urdf.xacro
@@ -2,8 +2,6 @@
 
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:property name="M_PI" value="3.1415926535897931" />
-
   <xacro:macro name="fixed_wheel" params="name parent *origin">
 
   <link name="${name}">
@@ -34,7 +32,7 @@
     <parent link="${parent}" />
     <child link="${name}" />
     <axis xyz="0 1 0" />
-    <limit effort="100.0" velocity="${2*M_PI}" />
+    <limit effort="100.0" velocity="${2*pi}" />
   </joint>
 
   <transmission name="${name}_transmission">
diff --git a/twil_description/xacro/fixed_wheel_support.urdf.xacro b/twil_description/xacro/fixed_wheel_support.urdf.xacro
index 1346e56..78db149 100644
--- a/twil_description/xacro/fixed_wheel_support.urdf.xacro
+++ b/twil_description/xacro/fixed_wheel_support.urdf.xacro
@@ -27,7 +27,7 @@
     </collision>
   </link>
 
-  <joint name="${name}" type="fixed">
+  <joint name="${name}_joint" type="fixed">
     <xacro:insert_block name="origin" />
     <parent link="${parent}"/>
     <child link="${name}" />
diff --git a/twil_description/xacro/gps.urdf.xacro b/twil_description/xacro/gps.urdf.xacro
index b36ceae..e395b6d 100644
--- a/twil_description/xacro/gps.urdf.xacro
+++ b/twil_description/xacro/gps.urdf.xacro
@@ -4,44 +4,80 @@
 
 <xacro:macro name="sensor_gps" params="name parent *origin">
 
-  <link name="${name}_link" />
+  <link name="${name}_link">
+	<visual>
+                <origin xyz="0 0 0.0" rpy="0 0 0"/>
+                <geometry>
+                        <cylinder radius="${0.061/2}" length="0.0195"/>
+                </geometry>
+                <material name="black">
+                        <color rgba="0 0 0 1" />
+                </material>
+	</visual>
+        <colision>
+                <origin xyz="0 0 0.0" rpy="0 0 0"/>
+                <geometry>
+                        <cylinder radius="${0.061/2}" length="0.0195"/>
+                </geometry>
+        </colision>
+        <inertial>
+		<origin xyz="0 0 0" rpy="0 0 ${pi/2}"/>
+		<mass value="0.0539"/>
+		<inertia ixx="${(1/12) * 0.0539 * (3*(0.0613/2)*(0.0613/2) + 0.0195*0.0195)}" ixy="0.0" ixz="0.0"
+			iyy="${(1/12) * 0.0539 * (3*(0.0613/2)*(0.0613/2) + 0.0195*0.0195)}" iyz="0.0"
+			izz="${(1/2) * 0.0539 * ((0.0613/2)*(0.0613/2))}"/>
+        </inertial>
+  </link>
 
   <joint name="${name}_joint" type="fixed">
 	<parent link="${parent}"/>
 	<child link="${name}_link" />
-	<xacro:insert_block name="origin" />
+	<xacro:insert_block name="origin"/>
   </joint>
 
   <gazebo reference="${name}_link">
-	<sensor name="${name}_sensor" type="gps">
+	<sensor name="${name}_sensor" type="navsat">
 		<always_on>true</always_on>
 		<update_rate>5.0</update_rate>
-		<gps>
+		<topic>gnss/fix</topic>
+		<gz_frame_id>${name}_link</gz_frame_id>
+		<navsat>
 			<position_sensing>
 				<horizontal>
 					<noise type="gaussian">
+						<!-- They should be in meters, but appears to be in degrees, SDF bug? -->
 						<mean>0.0</mean>
-						<stddev>2e-4</stddev>
+						<stddev>1.5e-5</stddev>
 					</noise>
 				</horizontal>
 				<vertical>
 					<noise type="gaussian">
 						<mean>0.0</mean>
-						<stddev>2e-4</stddev>
+						<stddev>1.5</stddev>
 					</noise>
 				</vertical>
 			</position_sensing>
-		</gps>
-	
-		<plugin name="${name}_controller" filename="libgazebo_ros_gps_sensor.so">
-			<ros>
-				<!-- <namespace>/namespace</namespace> -->
-				<remapping>${name}_controller/out:=${name}/fix</remapping>
-			</ros>
-			<frame_name>${name}_link</frame_name>
-		</plugin>
+			<velocity_sensing>
+				<horizontal>
+					<noise type="gaussian">
+						<mean>0.0</mean>
+						<stddev>0.0514444</stddev>
+					</noise>
+				</horizontal>
+				<vertical>
+					<noise type="gaussian">
+						<mean>0.0</mean>
+						<stddev>0.0514444</stddev>
+					</noise>
+				</vertical>
+			</velocity_sensing>
+		</navsat>
 	</sensor>
- 	<material>Gazebo/Grey</material>
+	<material>Gazebo/Black</material>
+  </gazebo>
+
+  <gazebo>
+	<plugin filename="gz-sim-navsat-system" name="gz::sim::systems::NavSat"/>
   </gazebo>
  </xacro:macro>
 
diff --git a/twil_description/xacro/ground_truth.urdf.xacro b/twil_description/xacro/ground_truth.urdf.xacro
new file mode 100644
index 0000000..1a404d4
--- /dev/null
+++ b/twil_description/xacro/ground_truth.urdf.xacro
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <gazebo>
+	<!-- Publish Ignition (not ROS!) topics -->
+	<plugin filename="gz-sim-odometry-publisher-system" name="gz::sim::systems::OdometryPublisher">
+		<dimensions>3</dimensions>
+		<odom_frame>map</odom_frame>
+		<xyz_offset>0.0 0.0 0.0</xyz_offset>
+		<rpy_offset>0.0 0.0 0.0</rpy_offset>
+		<gaussian_noise>0.0</gaussian_noise>
+		<robot_base_frame>twil_origin</robot_base_frame>
+	</plugin>
+  </gazebo>
+</robot>
diff --git a/twil_description/xacro/gz_ros2_control.urdf.xacro b/twil_description/xacro/gz_ros2_control.urdf.xacro
new file mode 100644
index 0000000..725b8dc
--- /dev/null
+++ b/twil_description/xacro/gz_ros2_control.urdf.xacro
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <gazebo>
+	<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+		<parameters>$(find twil_description)/config/controller_manager.yaml</parameters>
+	</plugin>
+  </gazebo>
+</robot>
diff --git a/twil_description/xacro/ros2_control.urdf.xacro b/twil_description/xacro/ros2_control.urdf.xacro
new file mode 100644
index 0000000..9014bd3
--- /dev/null
+++ b/twil_description/xacro/ros2_control.urdf.xacro
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <ros2_control name="TwilSystem" type="system">
+  	<hardware>
+		<plugin>gz_ros2_control/GazeboSimSystem</plugin>
+	</hardware>
+
+	<joint name="right_wheel_joint">
+		<command_interface name="effort">
+			<param name="min">-100</param>
+			<param name="max">100</param>
+		</command_interface>
+		<state_interface name="position"/>
+		<state_interface name="velocity"/>
+		<state_interface name="effort"/>
+	</joint>
+
+	<joint name="left_wheel_joint">
+		<command_interface name="effort">
+			<param name="min">-100</param>
+			<param name="max">100</param>
+		</command_interface>
+		<state_interface name="position"/>
+		<state_interface name="velocity"/>
+		<state_interface name="effort"/>
+	</joint>
+  </ros2_control>
+</robot>
diff --git a/twil_description/xacro/sonar.urdf.xacro b/twil_description/xacro/sonar.urdf.xacro
index 10c47ae..026b3e9 100644
--- a/twil_description/xacro/sonar.urdf.xacro
+++ b/twil_description/xacro/sonar.urdf.xacro
@@ -2,7 +2,6 @@
 
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:property name="M_PI" value="3.1415926535897931" />
   <xacro:property name="ABS_DENSITY" value="1200" />
 
   <xacro:macro name="sonar" params="name parent *origin">
diff --git a/twil_description/xacro/step_motor.urdf.xacro b/twil_description/xacro/step_motor.urdf.xacro
index e4f1324..a0a8f9f 100644
--- a/twil_description/xacro/step_motor.urdf.xacro
+++ b/twil_description/xacro/step_motor.urdf.xacro
@@ -2,8 +2,6 @@
 
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:property name="M_PI" value="3.1415926535897931" />
-
   <xacro:macro name="step_motor" params="name parent *origin">
 
    <link name="${name}">
diff --git a/twil_description/xacro/tower.urdf.xacro b/twil_description/xacro/tower.urdf.xacro
index d5c16f7..ecdbaa9 100644
--- a/twil_description/xacro/tower.urdf.xacro
+++ b/twil_description/xacro/tower.urdf.xacro
@@ -2,8 +2,6 @@
 
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:property name="M_PI" value="3.1415926535897931" />
-
   <xacro:macro name="tower" params="name parent *origin">
 
   <link name="${name}">
@@ -35,7 +33,7 @@
     <parent link="${parent}" />
     <child link="${name}" />
     <axis xyz="0 0 1" />
-    <limit effort="100.0" velocity="${1.8*M_PI/180*1000}" />
+    <limit effort="100.0" velocity="${1.8*pi/180*1000}" />
     <dynamics friction="1.0"/>
   </joint>
 
diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro
index 8107882..2f25bbc 100644
--- a/twil_description/xacro/twil.urdf.xacro
+++ b/twil_description/xacro/twil.urdf.xacro
@@ -1,35 +1,36 @@
 <?xml version="1.0"?>
 <robot name="twil" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
+  <xacro:arg name="wam" default="false" />
+  <xacro:arg name="gps" default="false" />
+  <xacro:arg name="imu" default="false" />
   <xacro:arg name="d435" default="false" />
 
-  <xacro:property name="M_PI" value="3.1415926535897931" />  
-
-  <xacro:include filename="$(find twil_description)/xacro/chassis.urdf.xacro" />
-  <xacro:include filename="$(find twil_description)/xacro/fixed_wheel_support.urdf.xacro" />
-  <xacro:include filename="$(find twil_description)/xacro/fixed_wheel.urdf.xacro" />
-  <xacro:include filename="$(find twil_description)/xacro/caster_support.urdf.xacro" />
-  <xacro:include filename="$(find twil_description)/xacro/caster_base.urdf.xacro" />
-  <xacro:include filename="$(find twil_description)/xacro/caster_wheel.urdf.xacro" />
-  <xacro:include filename="$(find twil_description)/xacro/battery_bosch_12v.urdf.xacro" />
-  <xacro:include filename="$(find twil_description)/xacro/cpu.urdf.xacro" />
-  <xacro:include filename="$(find twil_description)/xacro/eurocard.urdf.xacro" />
-  <xacro:include filename="$(find twil_description)/xacro/tower.urdf.xacro" />
-  <xacro:include filename="$(find twil_description)/xacro/step_motor.urdf.xacro" />
-  <xacro:include filename="$(find twil_description)/xacro/sonar.urdf.xacro" />
-
-  <link name="twil_origin" />
+  <xacro:include filename="chassis.urdf.xacro" />
+  <xacro:include filename="fixed_wheel_support.urdf.xacro" />
+  <xacro:include filename="fixed_wheel.urdf.xacro" />
+  <xacro:include filename="caster_support.urdf.xacro" />
+  <xacro:include filename="caster_base.urdf.xacro" />
+  <xacro:include filename="caster_wheel.urdf.xacro" />
+  <xacro:include filename="battery_bosch_12v.urdf.xacro" />
+  <xacro:include filename="cpu.urdf.xacro" />
+  <xacro:include filename="eurocard.urdf.xacro" />
+  <xacro:include filename="tower.urdf.xacro" />
+  <xacro:include filename="step_motor.urdf.xacro" />
+  <xacro:include filename="sonar.urdf.xacro" />
+
+  <link name="twil_origin"/>
 
   <xacro:chassis name="chassis" parent="twil_origin">
     <origin xyz="0 0 0.172" rpy="0 0 0"/>
   </xacro:chassis>
 
   <xacro:fixed_wheel_support name="right_wheel_support" parent="chassis">
-    <origin xyz="0 -0.161 -0.002" rpy="0 0 ${M_PI}" />
+    <origin xyz="0 -0.161 -0.002" rpy="0 0 ${pi}" />
   </xacro:fixed_wheel_support>
 
   <xacro:fixed_wheel name="right_wheel" parent="right_wheel_support">
-    <origin xyz="0 0 -0.095" rpy="0 0 ${-M_PI}" />
+    <origin xyz="0 0 -0.095" rpy="0 0 ${-pi}" />
   </xacro:fixed_wheel>
 
   <xacro:fixed_wheel_support name="left_wheel_support" parent="chassis">
@@ -81,11 +82,11 @@
   </xacro:sonar>
 
   <!--xacro:battery_bosch_12v name="battery" parent="chassis">
-    <origin xyz="-0.15 0.0 ${0.0015+0.0875}" rpy="0.0 0.0 ${M_PI/2}" />
+    <origin xyz="-0.15 0.0 ${0.0015+0.0875}" rpy="0.0 0.0 ${pi/2}" />
   </xacro:battery_bosch_12v-->
 
   <xacro:battery_bosch_12v name="battery" parent="chassis">
-    <origin xyz="0.0 0.0 ${0.0015+0.0875}" rpy="0.0 0.0 ${M_PI/2}" />
+    <origin xyz="0.0 0.0 ${0.0015+0.0875}" rpy="0.0 0.0 ${pi/2}" />
   </xacro:battery_bosch_12v>
 
   <xacro:eurocard name="fan" parent="chassis">
@@ -105,7 +106,7 @@
   </xacro:eurocard>
 
   <xacro:cpu name="cpu" parent="chassis">
-    <origin xyz="0 0.0 ${0.0015+0.3+0.003+0.127+0.003+0.17}" rpy="0.0 0.0 ${M_PI/2}" />
+    <origin xyz="0 0.0 ${0.0015+0.3+0.003+0.127+0.003+0.17}" rpy="0.0 0.0 ${pi/2}" />
   </xacro:cpu>
 
   <link name="chassis_top" />
@@ -113,72 +114,45 @@
   <joint name="chassis_top_joint" type="fixed">
     <parent link="chassis"/>
     <child link="chassis_top" />
-    <origin xyz="0.0 0.0 0.787" rpy="0.0 0.0 0.0" />
+    <origin xyz="0.0 0.0 0.787" rpy="0.0 0.0 0.0"/>
   </joint>
 
   <xacro:if value="$(arg wam)">
     <xacro:arg name="bhand" default="true"/>
-    <xacro:include filename="$(find wam_description)/xacro/wam.urdf.xacro" />
+    <xacro:include filename="$(find wam_description)/xacro/wam.urdf.xacro"/>
     <joint name="twil_wam" type="fixed">
       <parent link="chassis_top"/>
-      <child link="wam_origin" />
-      <origin xyz="-0.220 -0.140 0.0" rpy="0.0 0.0 0.0" />
+      <child link="wam_origin"/>
+      <origin xyz="-0.220 -0.140 0.0" rpy="0.0 0.0 0.0"/>
     </joint>
   </xacro:if>
 
-  <ros2_control name="GazeboSystem" type="system">
-  	<hardware>
-		<plugin>gazebo_ros2_control/GazeboSystem</plugin>
-	</hardware>
-
-	<joint name="right_wheel_joint">
-		<command_interface name="effort">
-			<param name="min">-100</param>
-			<param name="max">100</param>
-		</command_interface>
-		<state_interface name="position"/>
-		<state_interface name="velocity"/>
-		<state_interface name="effort"/>
-	</joint>
+  <xacro:include filename="ros2_control.urdf.xacro"/>
 
-	<joint name="left_wheel_joint">
-		<command_interface name="effort">
-			<param name="min">-100</param>
-			<param name="max">100</param>
-		</command_interface>
-		<state_interface name="position"/>
-		<state_interface name="velocity"/>
-		<state_interface name="effort"/>
-	</joint>
-  </ros2_control>
+  <xacro:include filename="gz_ros2_control.urdf.xacro"/>
+
+  <xacro:include filename="ground_truth.urdf.xacro"/>
   
-  <gazebo>
-	<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-		<robot_param>robot_description</robot_param>
-		<robot_param_node>robot_state_publisher</robot_param_node>
-		<parameters>$(find twil_description)/config/controller_manager.yaml</parameters>
-	</plugin>
-
-	<plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
-		<frame_name>map</frame_name>
-		<body_name>twil_origin</body_name>
-		<ros>
-			<remapping>odom:=ground_truth</remapping>
-		</ros>
-		<!--update_rate>100</update_rate-->
-	</plugin>
-
-  </gazebo>
 
   <xacro:if value="$(arg gps)">
-	<xacro:include filename="$(find twil_description)/xacro/gps.urdf.xacro"/>
-	<xacro:sensor_gps name="gps" parent="twil_origin">
-		<origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
+	<xacro:include filename="gps.urdf.xacro"/>
+	<xacro:sensor_gps name="gps" parent="chassis_top">
+		<origin xyz="0 0 0" rpy="0.0 0.0 0.0"/>
 	</xacro:sensor_gps>
   </xacro:if>
 
-  <xacro:if value="$(arg d435)">
-	<xacro:include filename="$(find twil_description)/xacro/d435.urdf.xacro"/>
+  <xacro:if value="$(arg imu)">
+	<xacro:include filename="$(find bno055_shuttle_description)/urdf/bno055_shuttle.xacro"/>
+	<xacro:bno055_shuttle name="imu"/>
+	<joint name="chassis_imu_joint" type="fixed">
+		<parent link="chassis_top"/>
+		<child link="imu_origin"/>
+		<origin xyz="0.1 0 0" rpy="0.0 0.0 0.0"/>
+	</joint>
   </xacro:if>
 
+<!--  <xacro:if value="$(arg d435)">
+	<xacro:include filename="d435.urdf.xacro"/>
+  </xacro:if>
+ -->
 </robot>