From 982262eda21036cf80b0dc86170c3480468cd0d7 Mon Sep 17 00:00:00 2001
From: yasuohayashibara <yasuo.hayashibara@it-chiba.ac.jp>
Date: Sun, 1 Sep 2024 21:57:36 +0900
Subject: [PATCH 1/5] add caster models

---
 .../gazebo_simulator_active_caster.launch.py  |  88 +++
 .../gazebo_simulator_caster_trail.launch.py   |  81 +++
 .../controller_position.yaml                  |  11 +
 .../models/ai_car_active_caster/model.config  |  16 +
 .../models/ai_car_active_caster/model.sdf     | 450 +++++++++++++
 .../models/ai_car_caster_trail/model.config   |  16 +
 .../models/ai_car_caster_trail/model.sdf      | 446 +++++++++++++
 .../shihou_course/ai_car_active_caster.model  | 619 ++++++++++++++++++
 .../shihou_course/ai_car_caster_trail.model   | 619 ++++++++++++++++++
 ...fstatic_broadcaster_no_namespace.launch.py | 108 +++
 .../vehicle/xacro/ai_car_active_caster.xacro  |  62 ++
 .../vehicle/xacro/ai_car_caster_trail.xacro   |  62 ++
 .../xacro/parts/active_caster_macro.xacro     |  66 ++
 .../xacro/parts/caster_trail_macro.xacro      |  53 ++
 14 files changed, 2697 insertions(+)
 create mode 100644 simulator/simulator/launch/gazebo_simulator_active_caster.launch.py
 create mode 100644 simulator/simulator/launch/gazebo_simulator_caster_trail.launch.py
 create mode 100644 simulator/simulator/models/ai_car_active_caster/controller_position.yaml
 create mode 100644 simulator/simulator/models/ai_car_active_caster/model.config
 create mode 100644 simulator/simulator/models/ai_car_active_caster/model.sdf
 create mode 100644 simulator/simulator/models/ai_car_caster_trail/model.config
 create mode 100644 simulator/simulator/models/ai_car_caster_trail/model.sdf
 create mode 100644 simulator/simulator/worlds/shihou_course/ai_car_active_caster.model
 create mode 100644 simulator/simulator/worlds/shihou_course/ai_car_caster_trail.model
 create mode 100644 simulator/vehicle/launch/extrinsic_tfstatic_broadcaster_no_namespace.launch.py
 create mode 100644 simulator/vehicle/xacro/ai_car_active_caster.xacro
 create mode 100644 simulator/vehicle/xacro/ai_car_caster_trail.xacro
 create mode 100644 simulator/vehicle/xacro/parts/active_caster_macro.xacro
 create mode 100644 simulator/vehicle/xacro/parts/caster_trail_macro.xacro

diff --git a/simulator/simulator/launch/gazebo_simulator_active_caster.launch.py b/simulator/simulator/launch/gazebo_simulator_active_caster.launch.py
new file mode 100644
index 0000000..e83ddfe
--- /dev/null
+++ b/simulator/simulator/launch/gazebo_simulator_active_caster.launch.py
@@ -0,0 +1,88 @@
+import os.path as osp
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import (
+    DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess
+)
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    VEHICLE_NAME = "ai_car_active_caster"
+    launch_args = (
+        DeclareLaunchArgument(
+            "world_name",
+            default_value="shihou_course",
+            description="World Name",
+        ),
+        DeclareLaunchArgument(
+            "vehicle_name",
+            default_value="ai_car_active_caster",
+            description="Vehicle Name",
+        ),
+        DeclareLaunchArgument(
+            "world_path",
+            default_value=[
+                osp.join(get_package_share_directory("simulator"), "worlds", ""),
+                LaunchConfiguration("world_name"), "/",
+                LaunchConfiguration("vehicle_name"), ".model"
+            ],
+            description="Path to world file.",
+        ),
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true",
+            description="If true, use simulation (Gazebo) clock",
+        ),
+    )
+
+    tf_static_publisher = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            osp.join(get_package_share_directory("vehicle"),
+                     "launch/extrinsic_tfstatic_broadcaster_no_namespace.launch.py"),
+        ),
+        launch_arguments={
+            "vehicle_name": VEHICLE_NAME,
+            "use_sim_time": LaunchConfiguration("use_sim_time"),
+        }.items(),
+    )
+    # ExecuteProcess(
+    # cmd=["export", "GAZEBO_MODEL_PATH=$(ros2 pkg prefix vehicle)/share/vehicle/xacro"],
+    # shell=True),
+    gzserver = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            osp.join(get_package_share_directory("gazebo_ros"),
+                     "launch/gzserver.launch.py"),
+        ),
+        launch_arguments={
+            "world": LaunchConfiguration("world_path"),
+            "world_name": LaunchConfiguration("world_name"),
+        }.items(),
+    )
+    gzclient = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            osp.join(get_package_share_directory("gazebo_ros"),
+                     "launch/gzclient.launch.py"),
+        ),
+    )
+    set_use_sim_time = ExecuteProcess(
+        cmd=['ros2', 'param', 'set', '/gazebo',
+             'use_sim_time', LaunchConfiguration("use_sim_time")],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        *launch_args,
+        tf_static_publisher,
+        gzserver,
+        gzclient,
+        set_use_sim_time,
+        Node(
+            package='controller_manager',
+            executable='spawner.py',
+            arguments=['position_controller'],
+            output='screen'
+        )
+    ])
diff --git a/simulator/simulator/launch/gazebo_simulator_caster_trail.launch.py b/simulator/simulator/launch/gazebo_simulator_caster_trail.launch.py
new file mode 100644
index 0000000..494d119
--- /dev/null
+++ b/simulator/simulator/launch/gazebo_simulator_caster_trail.launch.py
@@ -0,0 +1,81 @@
+import os.path as osp
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess
+)
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    VEHICLE_NAME = "ai_car_caster_trail"
+    launch_args = (
+        DeclareLaunchArgument(
+            "world_name",
+            default_value="shihou_course",
+            description="World Name",
+        ),
+        DeclareLaunchArgument(
+            "vehicle_name",
+            default_value="ai_car_caster_trail",
+            description="Vehicle Name",
+        ),
+        DeclareLaunchArgument(
+            "world_path",
+            default_value=[
+                osp.join(get_package_share_directory("simulator"), "worlds", ""),
+                LaunchConfiguration("world_name"), "/",
+                LaunchConfiguration("vehicle_name"), ".model"
+            ],
+            description="Path to world file.",
+        ),
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true",
+            description="If true, use simulation (Gazebo) clock",
+        ),
+    )
+
+    tf_static_publisher = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            osp.join(get_package_share_directory("vehicle"),
+                     "launch/extrinsic_tfstatic_broadcaster_no_namespace.launch.py"),
+        ),
+        launch_arguments={
+            "vehicle_name": VEHICLE_NAME,
+            "use_sim_time": LaunchConfiguration("use_sim_time"),
+        }.items(),
+    )
+    # ExecuteProcess(
+    # cmd=["export", "GAZEBO_MODEL_PATH=$(ros2 pkg prefix vehicle)/share/vehicle/xacro"],
+    # shell=True),
+    gzserver = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            osp.join(get_package_share_directory("gazebo_ros"),
+                     "launch/gzserver.launch.py"),
+        ),
+        launch_arguments={
+            "world": LaunchConfiguration("world_path"),
+            "world_name": LaunchConfiguration("world_name"),
+        }.items(),
+    )
+    gzclient = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            osp.join(get_package_share_directory("gazebo_ros"),
+                     "launch/gzclient.launch.py"),
+        ),
+    )
+    set_use_sim_time = ExecuteProcess(
+        cmd=['ros2', 'param', 'set', '/gazebo',
+             'use_sim_time', LaunchConfiguration("use_sim_time")],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        *launch_args,
+        tf_static_publisher,
+        gzserver,
+        gzclient,
+        set_use_sim_time,
+    ])
diff --git a/simulator/simulator/models/ai_car_active_caster/controller_position.yaml b/simulator/simulator/models/ai_car_active_caster/controller_position.yaml
new file mode 100644
index 0000000..8ac952d
--- /dev/null
+++ b/simulator/simulator/models/ai_car_active_caster/controller_position.yaml
@@ -0,0 +1,11 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 30  # Hz
+
+    position_controller:
+      type: position_controllers/JointGroupPositionController
+
+position_controller:
+  ros__parameters:
+    joints:
+      - caster_back_steering_joint
\ No newline at end of file
diff --git a/simulator/simulator/models/ai_car_active_caster/model.config b/simulator/simulator/models/ai_car_active_caster/model.config
new file mode 100644
index 0000000..5e2fd74
--- /dev/null
+++ b/simulator/simulator/models/ai_car_active_caster/model.config
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>ai_car_active_caster</name>
+  <version>2.0</version>
+  <sdf version="1.5">model.sdf</sdf>
+
+  <author>
+    <name>Masaya Okada</name>
+    <email>masaya_okada@jp.honda</email>
+  </author>
+
+  <description>
+    ai_car_active_caster
+  </description>
+</model>
diff --git a/simulator/simulator/models/ai_car_active_caster/model.sdf b/simulator/simulator/models/ai_car_active_caster/model.sdf
new file mode 100644
index 0000000..059ac5d
--- /dev/null
+++ b/simulator/simulator/models/ai_car_active_caster/model.sdf
@@ -0,0 +1,450 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="ai_car_active_caster">  
+  <pose>0 0 0 0 0 0</pose>
+
+    <link name="base_footprint"/>
+
+    <joint name="base_joint" type="fixed">
+      <parent>base_footprint</parent>
+      <child>base_link</child>
+      <pose>0 0 0.15 0 0 0</pose>
+    </joint>
+
+    <link name="base_link">
+      <inertial>
+        <pose>0 0 0 0 0 3.14</pose>   
+        <inertia>
+          <ixx>1.73333</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>4.56667</iyy>
+          <iyz>0</iyz>
+          <izz>4.56667</izz>
+        </inertia>
+        <mass>30.0</mass>
+      </inertial>
+
+      <collision name="base_collision">
+        <pose>0 0 0 0 0 3.14</pose>
+        <geometry>
+          <mesh>
+             <uri>model://ai_car1/meshes/AIF_body.dae</uri>
+          </mesh>
+        </geometry>
+      </collision>
+
+      <visual name="base_visual">
+        <pose>0 0 0 0 0 3.14</pose>
+        <geometry>
+          <mesh>
+            <uri>model://ai_car1/meshes/AIF_body.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+<!-- Left Wheel -->
+    <joint name="wheel_left_joint" type="revolute">
+      <parent>base_link</parent>
+      <child>wheel_left_link</child>
+      <pose> 0 0.3 0.0 1.57 0 1.57</pose>
+      <axis>
+        <xyz>1 0 0</xyz>
+      </axis>
+    </joint>
+
+    <link name="wheel_left_link">
+      <inertial>
+        <pose> 0 0.3 0.0 1.57 0 1.57</pose>
+        <inertia>
+          <ixx>0.04778</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.04778</iyy>
+          <iyz>0</iyz>
+          <izz>0.07875</izz>
+        </inertia>
+        <mass>7.0</mass>
+      </inertial>
+      <collision name="wheel_left_collision">
+        <pose> 0 0.3 0.0 1.57 0 1.57</pose>
+        <geometry>
+         <mesh>
+            <uri>model://ai_car1/meshes/tire.dae</uri>
+            <scale>0.05 0.12 0.12</scale>      
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.7</mu>
+              <mu2>0.7</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0.0</slip1>
+              <slip2>0.0</slip2>
+            </ode>
+          </friction>
+          <contact>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+5</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+      <visual name="wheel_left_visual">
+        <pose> 0 0.3 0.0 1.57 0 1.57</pose>
+        <geometry>
+          <mesh>
+            <uri>model://ai_car1/meshes/tire.dae</uri>
+            <scale>0.05 0.12 0.12</scale>      
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+<!-- Right Wheel -->
+
+    <joint name="wheel_right_joint" type="revolute">
+      <parent>base_link</parent>
+      <child>wheel_right_link</child>
+      <pose>0 -0.3 0.0 1.57 0 1.57</pose>
+      <axis>
+        <xyz>1 0 0</xyz>
+      </axis>
+    </joint>
+
+    <link name="wheel_right_link">
+      <inertial>
+        <pose>0 -0.3 0 1.57 0 1.57</pose>
+        <inertia>
+          <ixx>0.04778</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.04778</iyy>
+          <iyz>0</iyz>
+          <izz>0.07875</izz>
+        </inertia>
+        <mass>7.0</mass>
+      </inertial>
+      <collision name="wheel_right_collision">
+        <pose>0 -0.3 0 1.57 0 1.57</pose>
+        <geometry>
+          <mesh>
+            <uri>model://ai_car1/meshes/tire.dae</uri>
+            <scale>0.05 0.12 0.12</scale>      
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.7</mu>
+              <mu2>0.7</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0.0</slip1>
+              <slip2>0.0</slip2>
+            </ode>
+          </friction>
+          <contact>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+5</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+      <visual name="wheel_right_visual">
+        <pose>0 -0.3 0 1.57 0 1.57</pose>
+        <geometry>
+          <mesh>
+            <uri>model://ai_car1/meshes/tire.dae</uri>
+            <scale>0.05 0.12 0.12</scale>      
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+<!-- wheel caster -->
+
+   <joint name='caster_back_steering_joint' type="revolute">
+      <parent>base_link</parent>
+      <child>caster_back_steering_link</child>
+      <pose>-0.8 0 -0.02 1.57 0 1.57</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-1.57</lower>
+          <upper>1.57</upper>
+        </limit>
+      </axis>
+   </joint>
+
+   <link name="caster_back_steering_link">
+      <inertial>
+        <pose>-0.8 0 -0.02 1.57 0 1.57</pose>
+        <mass>0.8</mass>
+      </inertial>
+  </link>
+
+   <joint name='caster_back_joint' type="revolute">
+      <parent>caster_back_steering_link</parent>
+      <child>caster_back_link</child>
+      <pose>-0.8 0 -0.02 1.57 0 1.57</pose>
+      <axis>
+        <xyz>1 0 0</xyz>
+      </axis>
+   </joint>
+
+   <link name="caster_back_link">
+      <inertial>
+        <pose>-0.8 0 -0.02 1.57 0 1.57</pose>
+        <inertia>
+          <ixx>0.04778</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.04778</iyy>
+          <iyz>0</iyz>
+          <izz>0.07875</izz>
+        </inertia>
+        <mass>10.0</mass>
+      </inertial>
+
+      <collision name="wheel_caster_collision">
+        <pose>-0.8 0 -0.02 1.57 0 1.57</pose>
+        <geometry>
+          <mesh>
+            <uri>model://ai_car1/meshes/tire.dae</uri>
+            <scale>0.1 0.1 0.1</scale>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.7</mu>
+              <mu2>0.7</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0.0</slip1>
+              <slip2>0.0</slip2>
+            </ode>
+          </friction>
+          <contact>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+5</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+
+      <visual name="wheel_caster_visual">
+        <pose>-0.8 0 -0.02 1.57 0 1.57</pose>
+        <geometry>
+          <mesh>
+            <uri>model://ai_car1/meshes/tire.dae</uri>
+            <scale>0.1 0.1 0.1</scale>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="camera_joint" type="fixed">
+      <parent>base_link</parent>
+      <child>camera_link</child>
+      <pose>0.35 0 1.5 0 0.087 0</pose>
+      <!-- <pose>0.0 0.35 0.42 0 0 1.57</pose> -->
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis> 
+    </joint>
+
+    <joint name="camera_rgb_joint" type="fixed">
+      <parent>camera_link</parent>
+      <child>camera_rgb_frame</child>
+      <pose>0.35 0 1.5 0 0.087 0</pose>
+      <!-- <pose>0.0 0.35 0.42 0 0 1.57</pose> -->
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <link name="camera_link"/>
+
+    <link name="camera_rgb_frame">
+      <inertial>
+        <pose>0.35 0 1.5 0 0.087 0</pose>
+        <!-- <pose>0.0 0.35 0.42 0 0 1.57</pose> -->
+        <inertia>
+          <ixx>0.001</ixx>
+          <ixy>0.000</ixy>
+          <ixz>0.000</ixz>
+          <iyy>0.001</iyy>
+          <iyz>0.000</iyz>
+          <izz>0.001</izz>
+        </inertia>
+        <mass>0.035</mass>
+      </inertial>
+
+      <pose>0.35 0 1.5 0 0.087 0</pose>
+      <!-- <pose>0.0 0.35 0.42 0 0 1.57</pose> -->
+      <sensor name="camera" type="camera">
+        <always_on>true</always_on>
+        <visualize>true</visualize>
+        <update_rate>30</update_rate>
+        <camera name="intel_realsense_r200">
+          <horizontal_fov>1.02974</horizontal_fov>
+          <image>
+            <width>1920</width>
+            <height>1080</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.01</near>
+            <far>300</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <!-- Noise is sampled independently per pixel on each frame.
+                  That pixel's noise value is added to each of its color
+                  channels, which at that point lie in the range [0,1]. -->
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+          <plugin name="camera_driver" filename="libgazebo_ros_camera.so">
+            <ros>
+              <!-- <namespace>test_cam</namespace> -->
+              <!-- <remapping>image_raw:=image_demo</remapping> -->
+              <!-- <remapping>camera_info:=camera_info_demo</remapping> -->
+            </ros>
+            <!-- camera_name>omit so it defaults to sensor name</camera_name-->
+            <!-- frame_name>omit so it defaults to link name</frameName-->
+            <!-- <hack_baseline>0.07</hack_baseline> -->
+          </plugin>
+      </sensor>
+    </link>    
+
+    <joint name="imu_joint" type="fixed">
+      <parent>base_link</parent>
+      <child>imu_link</child>
+      <pose>-0.032 0 0.068 0 0 0</pose>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>    
+
+    <link name="imu_link">
+      <sensor name="tb3_imu" type="imu">
+        <always_on>true</always_on>
+        <update_rate>200</update_rate>
+        <imu>
+          <angular_velocity>
+            <x>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+              </noise>
+            </z>
+          </angular_velocity>
+          <linear_acceleration>
+            <x>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+              </noise>
+            </z>
+          </linear_acceleration>
+        </imu>
+        <plugin name="turtlebot3_imu" filename="libgazebo_ros_imu_sensor.so">
+          <ros>
+            <!-- <namespace>/tb3</namespace> -->
+            <remapping>~/out:=imu</remapping>
+          </ros>
+        </plugin>
+      </sensor>
+    </link>
+
+    <plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so">
+
+      <ros>
+        <!-- <namespace>/tb3</namespace> -->
+      </ros>
+
+      <update_rate>30</update_rate>
+
+      <!-- wheels -->
+      <left_joint>wheel_left_joint</left_joint>
+      <right_joint>wheel_right_joint</right_joint>
+
+      <!-- kinematics -->
+      <wheel_separation>0.60</wheel_separation>
+      <wheel_diameter>0.30</wheel_diameter>
+
+      <!-- limits -->
+      <max_wheel_torque>9.6</max_wheel_torque>
+      <max_wheel_acceleration>1.0</max_wheel_acceleration>
+
+      <command_topic>cmd_vel</command_topic>
+
+      <!-- output -->
+      <publish_odom>true</publish_odom>
+      <publish_odom_tf>true</publish_odom_tf>
+      <publish_wheel_tf>false</publish_wheel_tf>
+
+      <odometry_topic>odom</odometry_topic>
+      <odometry_frame>odom</odometry_frame>
+      <robot_base_frame>base_footprint</robot_base_frame>
+
+    </plugin>
+
+    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+      <parameters>/home/haya/ros2_ws/src/AIFormula_private/simulator/simulator/models/ai_car_active_caster/controller_position.yaml</parameters>
+    </plugin>
+
+    <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
+      <ros>
+        <!-- <namespace>/tsimulator/models/ai_car1/model.sdf simulator/models/ai_car1/model.configb3</namespace> -->
+      </ros>
+      <update_rate>30</update_rate>
+      <joint_name>wheel_left_joint</joint_name>
+      <joint_name>wheel_right_joint</joint_name>
+    </plugin>    
+
+  </model>
+</sdf>
diff --git a/simulator/simulator/models/ai_car_caster_trail/model.config b/simulator/simulator/models/ai_car_caster_trail/model.config
new file mode 100644
index 0000000..a54c4f7
--- /dev/null
+++ b/simulator/simulator/models/ai_car_caster_trail/model.config
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>ai_car_caster_trail</name>
+  <version>2.0</version>
+  <sdf version="1.5">model.sdf</sdf>
+
+  <author>
+    <name>Masaya Okada</name>
+    <email>masaya_okada@jp.honda</email>
+  </author>
+
+  <description>
+    ai_car_caster_trail
+  </description>
+</model>
diff --git a/simulator/simulator/models/ai_car_caster_trail/model.sdf b/simulator/simulator/models/ai_car_caster_trail/model.sdf
new file mode 100644
index 0000000..08f43e0
--- /dev/null
+++ b/simulator/simulator/models/ai_car_caster_trail/model.sdf
@@ -0,0 +1,446 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="ai_car_caster_trail">  
+  <pose>0 0 0 0 0 0</pose>
+
+    <link name="base_footprint"/>
+
+    <joint name="base_joint" type="fixed">
+      <parent>base_footprint</parent>
+      <child>base_link</child>
+      <pose>0 0 0.15 0 0 0</pose>
+    </joint>
+
+    <link name="base_link">
+      <inertial>
+        <pose>0 0 0 0 0 3.14</pose>   
+        <inertia>
+          <ixx>1.73333</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>4.56667</iyy>
+          <iyz>0</iyz>
+          <izz>4.56667</izz>
+        </inertia>
+        <mass>30.0</mass>
+      </inertial>
+
+      <collision name="base_collision">
+        <pose>0 0 0 0 0 3.14</pose>
+        <geometry>
+          <mesh>
+             <uri>model://ai_car1/meshes/AIF_body.dae</uri>
+          </mesh>
+        </geometry>
+      </collision>
+
+      <visual name="base_visual">
+        <pose>0 0 0 0 0 3.14</pose>
+        <geometry>
+          <mesh>
+            <uri>model://ai_car1/meshes/AIF_body.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+<!-- Left Wheel -->
+    <joint name="wheel_left_joint" type="revolute">
+      <parent>base_link</parent>
+      <child>wheel_left_link</child>
+      <pose> 0 0.3 0.0 1.57 0 1.57</pose>
+      <axis>
+        <xyz>1 0 0</xyz>
+      </axis>
+    </joint>
+
+    <link name="wheel_left_link">
+      <inertial>
+        <pose> 0 0.3 0.0 1.57 0 1.57</pose>
+        <inertia>
+          <ixx>0.04778</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.04778</iyy>
+          <iyz>0</iyz>
+          <izz>0.07875</izz>
+        </inertia>
+        <mass>7.0</mass>
+      </inertial>
+      <collision name="wheel_left_collision">
+        <pose> 0 0.3 0.0 1.57 0 1.57</pose>
+        <geometry>
+         <mesh>
+            <uri>model://ai_car1/meshes/tire.dae</uri>
+            <scale>0.05 0.12 0.12</scale>      
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.7</mu>
+              <mu2>0.7</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0.0</slip1>
+              <slip2>0.0</slip2>
+            </ode>
+          </friction>
+          <contact>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+5</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+      <visual name="wheel_left_visual">
+        <pose> 0 0.3 0.0 1.57 0 1.57</pose>
+        <geometry>
+          <mesh>
+            <uri>model://ai_car1/meshes/tire.dae</uri>
+            <scale>0.05 0.12 0.12</scale>      
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+<!-- Right Wheel -->
+
+    <joint name="wheel_right_joint" type="revolute">
+      <parent>base_link</parent>
+      <child>wheel_right_link</child>
+      <pose>0 -0.3 0.0 1.57 0 1.57</pose>
+      <axis>
+        <xyz>1 0 0</xyz>
+      </axis>
+    </joint>
+
+    <link name="wheel_right_link">
+      <inertial>
+        <pose>0 -0.3 0 1.57 0 1.57</pose>
+        <inertia>
+          <ixx>0.04778</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.04778</iyy>
+          <iyz>0</iyz>
+          <izz>0.07875</izz>
+        </inertia>
+        <mass>7.0</mass>
+      </inertial>
+      <collision name="wheel_right_collision">
+        <pose>0 -0.3 0 1.57 0 1.57</pose>
+        <geometry>
+          <mesh>
+            <uri>model://ai_car1/meshes/tire.dae</uri>
+            <scale>0.05 0.12 0.12</scale>      
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.7</mu>
+              <mu2>0.7</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0.0</slip1>
+              <slip2>0.0</slip2>
+            </ode>
+          </friction>
+          <contact>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+5</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+      <visual name="wheel_right_visual">
+        <pose>0 -0.3 0 1.57 0 1.57</pose>
+        <geometry>
+          <mesh>
+            <uri>model://ai_car1/meshes/tire.dae</uri>
+            <scale>0.05 0.12 0.12</scale>      
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+<!-- wheel caster -->
+
+   <joint name='wheel_caster_steering_joint' type="revolute">
+      <parent>base_link</parent>
+      <child>wheel_caster_steering_link</child>
+      <pose>-0.8 0 -0.02 1.57 0 1.57</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-1.57</lower>
+          <upper>1.57</upper>
+        </limit>
+      </axis>
+   </joint>
+
+   <link name="wheel_caster_steering_link">
+      <inertial>
+        <pose>-0.8 0 -0.02 1.57 0 1.57</pose>
+        <mass>1.0</mass>
+      </inertial>
+  </link>
+
+   <joint name='wheel_caster_joint' type="revolute">
+      <parent>wheel_caster_steering_link</parent>
+      <child>wheel_caster_link</child>
+      <pose>-1.0 0 -0.02 1.57 0 1.57</pose>
+      <axis>
+        <xyz>1 0 0</xyz>
+      </axis>
+   </joint>
+
+   <link name="wheel_caster_link">
+      <inertial>
+        <pose>-1.0 0 -0.02 1.57 0 1.57</pose>
+        <inertia>
+          <ixx>0.04778</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.04778</iyy>
+          <iyz>0</iyz>
+          <izz>0.07875</izz>
+        </inertia>
+        <mass>10.0</mass>
+      </inertial>
+
+      <collision name="wheel_caster_collision">
+        <pose>-1.0 0 -0.02 1.57 0 1.57</pose>
+        <geometry>
+          <mesh>
+            <uri>model://ai_car1/meshes/tire.dae</uri>
+            <scale>0.1 0.1 0.1</scale>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.7</mu>
+              <mu2>0.7</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0.0</slip1>
+              <slip2>0.0</slip2>
+            </ode>
+          </friction>
+          <contact>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+5</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+
+      <visual name="wheel_caster_visual">
+        <pose>-1.0 0 -0.02 1.57 0 1.57</pose>
+        <geometry>
+          <mesh>
+            <uri>model://ai_car1/meshes/tire.dae</uri>
+            <scale>0.1 0.1 0.1</scale>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="camera_joint" type="fixed">
+      <parent>base_link</parent>
+      <child>camera_link</child>
+      <pose>0.35 0 1.5 0 0.087 0</pose>
+      <!-- <pose>0.0 0.35 0.42 0 0 1.57</pose> -->
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis> 
+    </joint>
+
+    <joint name="camera_rgb_joint" type="fixed">
+      <parent>camera_link</parent>
+      <child>camera_rgb_frame</child>
+      <pose>0.35 0 1.5 0 0.087 0</pose>
+      <!-- <pose>0.0 0.35 0.42 0 0 1.57</pose> -->
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <link name="camera_link"/>
+
+    <link name="camera_rgb_frame">
+      <inertial>
+        <pose>0.35 0 1.5 0 0.087 0</pose>
+        <!-- <pose>0.0 0.35 0.42 0 0 1.57</pose> -->
+        <inertia>
+          <ixx>0.001</ixx>
+          <ixy>0.000</ixy>
+          <ixz>0.000</ixz>
+          <iyy>0.001</iyy>
+          <iyz>0.000</iyz>
+          <izz>0.001</izz>
+        </inertia>
+        <mass>0.035</mass>
+      </inertial>
+
+      <pose>0.35 0 1.5 0 0.087 0</pose>
+      <!-- <pose>0.0 0.35 0.42 0 0 1.57</pose> -->
+      <sensor name="camera" type="camera">
+        <always_on>true</always_on>
+        <visualize>true</visualize>
+        <update_rate>30</update_rate>
+        <camera name="intel_realsense_r200">
+          <horizontal_fov>1.02974</horizontal_fov>
+          <image>
+            <width>1920</width>
+            <height>1080</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.01</near>
+            <far>300</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <!-- Noise is sampled independently per pixel on each frame.
+                  That pixel's noise value is added to each of its color
+                  channels, which at that point lie in the range [0,1]. -->
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+          <plugin name="camera_driver" filename="libgazebo_ros_camera.so">
+            <ros>
+              <!-- <namespace>test_cam</namespace> -->
+              <!-- <remapping>image_raw:=image_demo</remapping> -->
+              <!-- <remapping>camera_info:=camera_info_demo</remapping> -->
+            </ros>
+            <!-- camera_name>omit so it defaults to sensor name</camera_name-->
+            <!-- frame_name>omit so it defaults to link name</frameName-->
+            <!-- <hack_baseline>0.07</hack_baseline> -->
+          </plugin>
+      </sensor>
+    </link>    
+
+    <joint name="imu_joint" type="fixed">
+      <parent>base_link</parent>
+      <child>imu_link</child>
+      <pose>-0.032 0 0.068 0 0 0</pose>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>    
+
+    <link name="imu_link">
+      <sensor name="tb3_imu" type="imu">
+        <always_on>true</always_on>
+        <update_rate>200</update_rate>
+        <imu>
+          <angular_velocity>
+            <x>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+              </noise>
+            </z>
+          </angular_velocity>
+          <linear_acceleration>
+            <x>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+              </noise>
+            </z>
+          </linear_acceleration>
+        </imu>
+        <plugin name="turtlebot3_imu" filename="libgazebo_ros_imu_sensor.so">
+          <ros>
+            <!-- <namespace>/tb3</namespace> -->
+            <remapping>~/out:=imu</remapping>
+          </ros>
+        </plugin>
+      </sensor>
+    </link>
+
+    <plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so">
+
+      <ros>
+        <!-- <namespace>/tb3</namespace> -->
+      </ros>
+
+      <update_rate>30</update_rate>
+
+      <!-- wheels -->
+      <left_joint>wheel_left_joint</left_joint>
+      <right_joint>wheel_right_joint</right_joint>
+
+      <!-- kinematics -->
+      <wheel_separation>0.60</wheel_separation>
+      <wheel_diameter>0.30</wheel_diameter>
+
+      <!-- limits -->
+      <max_wheel_torque>9.6</max_wheel_torque>
+      <max_wheel_acceleration>1.0</max_wheel_acceleration>
+
+      <command_topic>cmd_vel</command_topic>
+
+      <!-- output -->
+      <publish_odom>true</publish_odom>
+      <publish_odom_tf>true</publish_odom_tf>
+      <publish_wheel_tf>false</publish_wheel_tf>
+
+      <odometry_topic>odom</odometry_topic>
+      <odometry_frame>odom</odometry_frame>
+      <robot_base_frame>base_footprint</robot_base_frame>
+
+    </plugin>
+
+    <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
+      <ros>
+        <!-- <namespace>/tsimulator/models/ai_car1/model.sdf simulator/models/ai_car1/model.configb3</namespace> -->
+      </ros>
+      <update_rate>30</update_rate>
+      <joint_name>wheel_left_joint</joint_name>
+      <joint_name>wheel_right_joint</joint_name>
+    </plugin>    
+
+  </model>
+</sdf>
diff --git a/simulator/simulator/worlds/shihou_course/ai_car_active_caster.model b/simulator/simulator/worlds/shihou_course/ai_car_active_caster.model
new file mode 100644
index 0000000..27e2421
--- /dev/null
+++ b/simulator/simulator/worlds/shihou_course/ai_car_active_caster.model
@@ -0,0 +1,619 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="default">
+
+    <model name="ground_plane">
+      <static>true</static>
+      <pose>0 0 -0.001 0 0 0</pose>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>200 200</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+        <visual name="visual">
+          <cast_shadows>false</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>200 200</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <scene>
+      <shadows>false</shadows>
+      <sky>
+        <clouds>12</clouds>
+      </sky>
+    </scene>
+
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+
+    <physics type="ode">
+      <real_time_update_rate>1000.0</real_time_update_rate>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>150</iters>
+          <precon_iters>0</precon_iters>
+          <sor>1.400000</sor>
+          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
+        </solver>
+        <constraints>
+          <cfm>0.00001</cfm>
+          <erp>0.2</erp>
+          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
+          <contact_surface_layer>0.01000</contact_surface_layer>
+        </constraints>
+      </ode>
+    </physics>
+    
+    <road name="my_road">
+      <width>9.0</width>
+      <point>  0       1.800 0</point>
+      <point>-19.15 1.8 0</point>
+      <point>-20.762 1.73 0</point>
+      <point>-22.362 1.519 0</point>
+      <point>-23.938 1.17 0</point>
+      <point>-25.477 0.684 0</point>
+      <point>-26.968 0.067 0</point>
+      <point>-28.4 -0.679 0</point>
+      <point>-29.761 -1.546 0</point>
+      <point>-31.042 -2.528 0</point>
+      <point>-32.231 -3.619 0</point>
+      <point>-33.322 -4.808 0</point>
+      <point>-34.304 -6.089 0</point>
+      <point>-35.171 -7.45 0</point>
+      <point>-35.917 -8.882 0</point>
+      <point>-36.534 -10.373 0</point>
+      <point>-37.02 -11.912 0</point>
+      <point>-37.369 -13.488 0</point>
+      <point>-37.58 -15.088 0</point>
+      <point>-37.65 -16.7 0</point>
+      <point>-37.65 -37.7 0</point>
+      <point>-37.58 -39.312 0</point>
+      <point>-37.369 -40.912 0</point>
+      <point>-37.02 -42.488 0</point>
+      <point>-36.534 -44.027 0</point>
+      <point>-35.917 -45.518 0</point>
+      <point>-35.171 -46.95 0</point>
+      <point>-34.304 -48.311 0</point>
+      <point>-33.322 -49.592 0</point>
+      <point>-32.231 -50.781 0</point>
+      <point>-31.042 -51.872 0</point>
+      <point>-29.761 -52.854 0</point>
+      <point>-28.4 -53.721 0</point>
+      <point>-26.968 -54.467 0</point>
+      <point>-25.477 -55.084 0</point>
+      <point>20.173 -73.584 0</point>
+      <point>21.712 -74.07 0</point>
+      <point>23.288 -74.419 0</point>
+      <point>24.888 -74.63 0</point>
+      <point>26.5 -74.7 0</point>
+      <point>28.112 -74.63 0</point>
+      <point>29.712 -74.419 0</point>
+      <point>31.288 -74.07 0</point>
+      <point>32.827 -73.584 0</point>
+      <point>34.318 -72.967 0</point>
+      <point>35.75 -72.221 0</point>
+      <point>37.111 -71.354 0</point>
+      <point>38.392 -70.372 0</point>
+      <point>39.581 -69.281 0</point>
+      <point>40.672 -68.092 0</point>
+      <point>41.654 -66.811 0</point>
+      <point>42.521 -65.45 0</point>
+      <point>43.267 -64.018 0</point>
+      <point>43.884 -62.527 0</point>
+      <point>44.37 -60.988 0</point>
+      <point>44.719 -59.412 0</point>
+      <point>44.93 -57.812 0</point>
+      <point>45 -56.2 0</point>
+      <point>45 -16.7 0</point>
+      <point>44.93 -15.088 0</point>
+      <point>44.719 -13.488 0</point>
+      <point>44.37 -11.912 0</point>
+      <point>43.884 -10.373 0</point>
+      <point>43.267 -8.882 0</point>
+      <point>42.521 -7.45 0</point>
+      <point>41.654 -6.089 0</point>
+      <point>40.672 -4.808 0</point>
+      <point>39.581 -3.619 0</point>
+      <point>38.392 -2.528 0</point>
+      <point>37.111 -1.546 0</point>
+      <point>35.75 -0.679 0</point>
+      <point>34.318 0.067 0</point>
+      <point>32.827 0.684 0</point>
+      <point>31.288 1.17 0</point>
+      <point>29.712 1.519 0</point>
+      <point>28.112 1.73 0</point>
+      <point>26.5 1.8 0</point>
+      <point>  0      1.8 0</point>
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Tertiary</name>
+        </script>
+      </material>
+    </road>
+
+  
+
+    <road name="my_road_center">
+      <width>0.2</width>
+      <point>  0       1.800 0.001</point>
+      <point>-19.15 1.8 0.001</point>
+      <point>-20.762 1.73 0.001</point>
+      <point>-22.362 1.519 0.001</point>
+      <point>-23.938 1.17 0.001</point>
+      <point>-25.477 0.684 0.001</point>
+      <point>-26.968 0.067 0.001</point>
+      <point>-28.4 -0.679 0.001</point>
+      <point>-29.761 -1.546 0.001</point>
+      <point>-31.042 -2.528 0.001</point>
+      <point>-32.231 -3.619 0.001</point>
+      <point>-33.322 -4.808 0.001</point>
+      <point>-34.304 -6.089 0.001</point>
+      <point>-35.171 -7.45 0.001</point>
+      <point>-35.917 -8.882 0.001</point>
+      <point>-36.534 -10.373 0.001</point>
+      <point>-37.02 -11.912 0.001</point>
+      <point>-37.369 -13.488 0.001</point>
+      <point>-37.58 -15.088 0.001</point>
+      <point>-37.65 -16.7 0.001</point>
+      <point>-37.65 -37.7 0.001</point>
+      <point>-37.58 -39.312 0.001</point>
+      <point>-37.369 -40.912 0.001</point>
+      <point>-37.02 -42.488 0.001</point>
+      <point>-36.534 -44.027 0.001</point>
+      <point>-35.917 -45.518 0.001</point>
+      <point>-35.171 -46.95 0.001</point>
+      <point>-34.304 -48.311 0.001</point>
+      <point>-33.322 -49.592 0.001</point>
+      <point>-32.231 -50.781 0.001</point>
+      <point>-31.042 -51.872 0.001</point>
+      <point>-29.761 -52.854 0.001</point>
+      <point>-28.4 -53.721 0.001</point>
+      <point>-26.968 -54.467 0.001</point>
+      <point>-25.477 -55.084 0.001</point>
+      <point>20.173 -73.584 0.001</point>
+      <point>21.712 -74.07 0.001</point>
+      <point>23.288 -74.419 0.001</point>
+      <point>24.888 -74.63 0.001</point>
+      <point>26.5 -74.7 0.001</point>
+      <point>28.112 -74.63 0.001</point>
+      <point>29.712 -74.419 0.001</point>
+      <point>31.288 -74.07 0.001</point>
+      <point>32.827 -73.584 0.001</point>
+      <point>34.318 -72.967 0.001</point>
+      <point>35.75 -72.221 0.001</point>
+      <point>37.111 -71.354 0.001</point>
+      <point>38.392 -70.372 0.001</point>
+      <point>39.581 -69.281 0.001</point>
+      <point>40.672 -68.092 0.001</point>
+      <point>41.654 -66.811 0.001</point>
+      <point>42.521 -65.45 0.001</point>
+      <point>43.267 -64.018 0.001</point>
+      <point>43.884 -62.527 0.001</point>
+      <point>44.37 -60.988 0.001</point>
+      <point>44.719 -59.412 0.001</point>
+      <point>44.93 -57.812 0.001</point>
+      <point>45 -56.2 0.001</point>
+      <point>45 -16.7 0.001</point>
+      <point>44.93 -15.088 0.001</point>
+      <point>44.719 -13.488 0.001</point>
+      <point>44.37 -11.912 0.001</point>
+      <point>43.884 -10.373 0.001</point>
+      <point>43.267 -8.882 0.001</point>
+      <point>42.521 -7.45 0.001</point>
+      <point>41.654 -6.089 0.001</point>
+      <point>40.672 -4.808 0.001</point>
+      <point>39.581 -3.619 0.001</point>
+      <point>38.392 -2.528 0.001</point>
+      <point>37.111 -1.546 0.001</point>
+      <point>35.75 -0.679 0.001</point>
+      <point>34.318 0.067 0.001</point>
+      <point>32.827 0.684 0.001</point>
+      <point>31.288 1.17 0.001</point>
+      <point>29.712 1.519 0.001</point>
+      <point>28.112 1.73 0.001</point>
+      <point>26.5 1.8 0.001</point>
+      <point>  0      1.8 0.001</point>
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/White</name>
+        </script>
+      </material>
+    </road>
+    
+    <road name="my_road_left">
+      <width>0.2</width>
+      <point>0 -1.5 0.001</point>
+      <point>-19.15 -1.5 0.001</point>
+      <point>-20.475 -1.558 0.001</point>
+      <point>-21.789 -1.731 0.001</point>
+      <point>-23.084 -2.018 0.001</point>
+      <point>-24.349 -2.417 0.001</point>
+      <point>-25.574 -2.924 0.001</point>
+      <point>-26.75 -3.536 0.001</point>
+      <point>-27.868 -4.249 0.001</point>
+      <point>-28.92 -5.056 0.001</point>
+      <point>-29.898 -5.952 0.001</point>
+      <point>-30.794 -6.93 0.001</point>
+      <point>-31.601 -7.982 0.001</point>
+      <point>-32.314 -9.1 0.001</point>
+      <point>-32.926 -10.276 0.001</point>
+      <point>-33.433 -11.501 0.001</point>
+      <point>-33.832 -12.766 0.001</point>
+      <point>-34.119 -14.061 0.001</point>
+      <point>-34.292 -15.375 0.001</point>
+      <point>-34.35 -16.7 0.001</point>
+      <point>-34.35 -37.7 0.001</point>
+      <point>-34.292 -39.025 0.001</point>
+      <point>-34.119 -40.339 0.001</point>
+      <point>-33.832 -41.634 0.001</point>
+      <point>-33.433 -42.899 0.001</point>
+      <point>-32.926 -44.124 0.001</point>
+      <point>-32.314 -45.3 0.001</point>
+      <point>-31.601 -46.418 0.001</point>
+      <point>-30.794 -47.47 0.001</point>
+      <point>-29.898 -48.448 0.001</point>
+      <point>-28.92 -49.344 0.001</point>
+      <point>-27.868 -50.151 0.001</point>
+      <point>-26.75 -50.864 0.001</point>
+      <point>-25.574 -51.476 0.001</point>
+      <point>-24.349 -51.983 0.001</point>
+      <point>21.301 -70.483 0.001</point>
+      <point>22.566 -70.882 0.001</point>
+      <point>23.861 -71.169 0.001</point>
+      <point>25.175 -71.342 0.001</point>
+      <point>26.5 -71.4 0.001</point>
+      <point>27.825 -71.342 0.001</point>
+      <point>29.139 -71.169 0.001</point>
+      <point>30.434 -70.882 0.001</point>
+      <point>31.699 -70.483 0.001</point>
+      <point>32.924 -69.976 0.001</point>
+      <point>34.1 -69.364 0.001</point>
+      <point>35.218 -68.651 0.001</point>
+      <point>36.27 -67.844 0.001</point>
+      <point>37.248 -66.948 0.001</point>
+      <point>38.144 -65.97 0.001</point>
+      <point>38.951 -64.918 0.001</point>
+      <point>39.664 -63.8 0.001</point>
+      <point>40.276 -62.624 0.001</point>
+      <point>40.783 -61.399 0.001</point>
+      <point>41.182 -60.134 0.001</point>
+      <point>41.469 -58.839 0.001</point>
+      <point>41.642 -57.525 0.001</point>
+      <point>41.7 -56.2 0.001</point>
+      <point>41.7 -16.7 0.001</point>
+      <point>41.642 -15.375 0.001</point>
+      <point>41.469 -14.061 0.001</point>
+      <point>41.182 -12.766 0.001</point>
+      <point>40.783 -11.501 0.001</point>
+      <point>40.276 -10.276 0.001</point>
+      <point>39.664 -9.1 0.001</point>
+      <point>38.951 -7.982 0.001</point>
+      <point>38.144 -6.93 0.001</point>
+      <point>37.248 -5.952 0.001</point>
+      <point>36.27 -5.056 0.001</point>
+      <point>35.218 -4.249 0.001</point>
+      <point>34.1 -3.536 0.001</point>
+      <point>32.924 -2.924 0.001</point>
+      <point>31.699 -2.417 0.001</point>
+      <point>30.434 -2.018 0.001</point>
+      <point>29.139 -1.731 0.001</point>
+      <point>27.825 -1.558 0.001</point>
+      <point>26.5 -1.5 0.001</point>
+      <point>0 -1.5 0.001</point>
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/White</name>
+        </script>
+      </material>
+    </road>
+    
+    <road name="my_road_right">
+      <width>0.2</width>
+      <point>0 5.1 0.001</point>
+      <point>-19.15 5.1 0.001</point>
+      <point>-21.05 5.017 0.001</point>
+      <point>-22.936 4.769 0.001</point>
+      <point>-24.792 4.357 0.001</point>
+      <point>-26.606 3.785 0.001</point>
+      <point>-28.363 3.058 0.001</point>
+      <point>-30.05 2.179 0.001</point>
+      <point>-31.654 1.158 0.001</point>
+      <point>-33.163 0 0.001</point>
+      <point>-34.565 -1.285 0.001</point>
+      <point>-35.85 -2.687 0.001</point>
+      <point>-37.008 -4.196 0.001</point>
+      <point>-38.029 -5.8 0.001</point>
+      <point>-38.908 -7.487 0.001</point>
+      <point>-39.635 -9.244 0.001</point>
+      <point>-40.207 -11.058 0.001</point>
+      <point>-40.619 -12.914 0.001</point>
+      <point>-40.867 -14.8 0.001</point>
+      <point>-40.95 -16.7 0.001</point>
+      <point>-40.95 -37.7 0.001</point>
+      <point>-40.867 -39.6 0.001</point>
+      <point>-40.619 -41.486 0.001</point>
+      <point>-40.207 -43.342 0.001</point>
+      <point>-39.635 -45.156 0.001</point>
+      <point>-38.908 -46.913 0.001</point>
+      <point>-38.029 -48.6 0.001</point>
+      <point>-37.008 -50.204 0.001</point>
+      <point>-35.85 -51.713 0.001</point>
+      <point>-34.565 -53.115 0.001</point>
+      <point>-33.163 -54.4 0.001</point>
+      <point>-31.654 -55.558 0.001</point>
+      <point>-30.05 -56.579 0.001</point>
+      <point>-28.363 -57.458 0.001</point>
+      <point>-26.606 -58.185 0.001</point>
+      <point>19.044 -76.685 0.001</point>
+      <point>20.858 -77.257 0.001</point>
+      <point>22.714 -77.669 0.001</point>
+      <point>24.6 -77.917 0.001</point>
+      <point>26.5 -78 0.001</point>
+      <point>28.4 -77.917 0.001</point>
+      <point>30.286 -77.669 0.001</point>
+      <point>32.142 -77.257 0.001</point>
+      <point>33.956 -76.685 0.001</point>
+      <point>35.713 -75.958 0.001</point>
+      <point>37.4 -75.079 0.001</point>
+      <point>39.004 -74.058 0.001</point>
+      <point>40.513 -72.9 0.001</point>
+      <point>41.915 -71.615 0.001</point>
+      <point>43.2 -70.213 0.001</point>
+      <point>44.358 -68.704 0.001</point>
+      <point>45.379 -67.1 0.001</point>
+      <point>46.258 -65.413 0.001</point>
+      <point>46.985 -63.656 0.001</point>
+      <point>47.557 -61.842 0.001</point>
+      <point>47.969 -59.986 0.001</point>
+      <point>48.217 -58.1 0.001</point>
+      <point>48.3 -56.2 0.001</point>
+      <point>48.3 -16.7 0.001</point>
+      <point>48.217 -14.8 0.001</point>
+      <point>47.969 -12.914 0.001</point>
+      <point>47.557 -11.058 0.001</point>
+      <point>46.985 -9.244 0.001</point>
+      <point>46.258 -7.487 0.001</point>
+      <point>45.379 -5.8 0.001</point>
+      <point>44.358 -4.196 0.001</point>
+      <point>43.2 -2.687 0.001</point>
+      <point>41.915 -1.285 0.001</point>
+      <point>40.513 0 0.001</point>
+      <point>39.004 1.158 0.001</point>
+      <point>37.4 2.179 0.001</point>
+      <point>35.713 3.058 0.001</point>
+      <point>33.956 3.785 0.001</point>
+      <point>32.142 4.357 0.001</point>
+      <point>30.286 4.769 0.001</point>
+      <point>28.4 5.017 0.001</point>
+      <point>26.5 5.1 0.001</point>
+      <point>0 5.1 0.001</point>
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/White</name>
+        </script>
+      </material>
+    </road>
+
+  <road name="my_walkway_out">
+      <width>5.0</width>
+      <point>  0       8.800 0</point>
+      <point>-19.15 8.8 0.001</point>
+      <point>-21.372 8.703 0.001</point>
+      <point>-23.578 8.413 0.001</point>
+      <point>-25.75 7.931 0.001</point>
+      <point>-27.872 7.262 0.001</point>
+      <point>-29.927 6.411 0.001</point>
+      <point>-31.9 5.384 0.001</point>
+      <point>-33.776 4.188 0.001</point>
+      <point>-35.541 2.834 0.001</point>
+      <point>-37.181 1.331 0.001</point>
+      <point>-38.684 -0.309 0.001</point>
+      <point>-40.038 -2.074 0.001</point>
+      <point>-41.234 -3.95 0.001</point>
+      <point>-42.261 -5.923 0.001</point>
+      <point>-43.112 -7.978 0.001</point>
+      <point>-43.781 -10.1 0.001</point>
+      <point>-44.263 -12.272 0.001</point>
+      <point>-44.553 -14.478 0.001</point>
+      <point>-44.65 -16.7 0.001</point>
+      <point>-44.65 -37.7 0.001</point>
+      <point>-44.553 -39.922 0.001</point>
+      <point>-44.263 -42.128 0.001</point>
+      <point>-43.781 -44.3 0.001</point>
+      <point>-43.112 -46.422 0.001</point>
+      <point>-42.261 -48.477 0.001</point>
+      <point>-41.234 -50.45 0.001</point>
+      <point>-40.038 -52.326 0.001</point>
+      <point>-38.684 -54.091 0.001</point>
+      <point>-37.181 -55.731 0.001</point>
+      <point>-35.541 -57.234 0.001</point>
+      <point>-33.776 -58.588 0.001</point>
+      <point>-31.9 -59.784 0.001</point>
+      <point>-29.927 -60.811 0.001</point>
+      <point>-27.872 -61.662 0.001</point>
+      <point>17.778 -80.162 0.001</point>
+      <point>19.9 -80.831 0.001</point>
+      <point>22.072 -81.313 0.001</point>
+      <point>24.278 -81.603 0.001</point>
+      <point>26.5 -81.7 0.001</point>
+      <point>28.722 -81.603 0.001</point>
+      <point>30.928 -81.313 0.001</point>
+      <point>33.1 -80.831 0.001</point>
+      <point>35.222 -80.162 0.001</point>
+      <point>37.277 -79.311 0.001</point>
+      <point>39.25 -78.284 0.001</point>
+      <point>41.126 -77.088 0.001</point>
+      <point>42.891 -75.734 0.001</point>
+      <point>44.531 -74.231 0.001</point>
+      <point>46.034 -72.591 0.001</point>
+      <point>47.388 -70.826 0.001</point>
+      <point>48.584 -68.95 0.001</point>
+      <point>49.611 -66.977 0.001</point>
+      <point>50.462 -64.922 0.001</point>
+      <point>51.131 -62.8 0.001</point>
+      <point>51.613 -60.628 0.001</point>
+      <point>51.903 -58.422 0.001</point>
+      <point>52 -56.2 0.001</point>
+      <point>52 -16.7 0.001</point>
+      <point>51.903 -14.478 0.001</point>
+      <point>51.613 -12.272 0.001</point>
+      <point>51.131 -10.1 0.001</point>
+      <point>50.462 -7.978 0.001</point>
+      <point>49.611 -5.923 0.001</point>
+      <point>48.584 -3.95 0.001</point>
+      <point>47.388 -2.074 0.001</point>
+      <point>46.034 -0.309 0.001</point>
+      <point>44.531 1.331 0.001</point>
+      <point>42.891 2.834 0.001</point>
+      <point>41.126 4.188 0.001</point>
+      <point>39.25 5.384 0.001</point>
+      <point>37.277 6.411 0.001</point>
+      <point>35.222 7.262 0.001</point>
+      <point>33.1 7.931 0.001</point>
+      <point>30.928 8.413 0.001</point>
+      <point>28.722 8.703 0.001</point>
+      <point>26.5 8.8 0.001</point>
+      <point>  0      8.8 0</point>
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Green</name>
+        </script>
+      </material>
+    </road>   
+
+    <road name="my_walkway_in">
+      <width>5.0</width>
+          <point>  0  -5.2 0</point>
+          <point>-19.15 -5.2 0.001</point>
+          <point>-20.152 -5.244 0.001</point>
+          <point>-21.147 -5.375 0.001</point>
+          <point>-22.126 -5.592 0.001</point>
+          <point>-23.083 -5.894 0.001</point>
+          <point>-24.01 -6.277 0.001</point>
+          <point>-24.9 -6.741 0.001</point>
+          <point>-25.746 -7.28 0.001</point>
+          <point>-26.542 -7.89 0.001</point>
+          <point>-27.282 -8.568 0.001</point>
+          <point>-27.96 -9.308 0.001</point>
+          <point>-28.57 -10.104 0.001</point>
+          <point>-29.109 -10.95 0.001</point>
+          <point>-29.573 -11.84 0.001</point>
+          <point>-29.956 -12.767 0.001</point>
+          <point>-30.258 -13.724 0.001</point>
+          <point>-30.475 -14.703 0.001</point>
+          <point>-30.606 -15.698 0.001</point>
+          <point>-30.65 -16.7 0.001</point>
+          <point>-30.65 -37.7 0.001</point>
+          <point>-30.606 -38.702 0.001</point>
+          <point>-30.475 -39.697 0.001</point>
+          <point>-30.258 -40.676 0.001</point>
+          <point>-29.956 -41.633 0.001</point>
+          <point>-29.573 -42.56 0.001</point>
+          <point>-29.109 -43.45 0.001</point>
+          <point>-28.57 -44.296 0.001</point>
+          <point>-27.96 -45.092 0.001</point>
+          <point>-27.282 -45.832 0.001</point>
+          <point>-26.542 -46.51 0.001</point>
+          <point>-25.746 -47.12 0.001</point>
+          <point>-24.9 -47.659 0.001</point>
+          <point>-24.01 -48.123 0.001</point>
+          <point>-23.083 -48.506 0.001</point>
+          <point>22.567 -67.006 0.001</point>
+          <point>23.524 -67.308 0.001</point>
+          <point>24.503 -67.525 0.001</point>
+          <point>25.498 -67.656 0.001</point>
+          <point>26.5 -67.7 0.001</point>
+          <point>27.502 -67.656 0.001</point>
+          <point>28.497 -67.525 0.001</point>
+          <point>29.476 -67.308 0.001</point>
+          <point>30.433 -67.006 0.001</point>
+          <point>31.36 -66.623 0.001</point>
+          <point>32.25 -66.159 0.001</point>
+          <point>33.096 -65.62 0.001</point>
+          <point>33.892 -65.01 0.001</point>
+          <point>34.632 -64.332 0.001</point>
+          <point>35.31 -63.592 0.001</point>
+          <point>35.92 -62.796 0.001</point>
+          <point>36.459 -61.95 0.001</point>
+          <point>36.923 -61.06 0.001</point>
+          <point>37.306 -60.133 0.001</point>
+          <point>37.608 -59.176 0.001</point>
+          <point>37.825 -58.197 0.001</point>
+          <point>37.956 -57.202 0.001</point>
+          <point>38 -56.2 0.001</point>
+          <point>38 -16.7 0.001</point>
+          <point>37.956 -15.698 0.001</point>
+          <point>37.825 -14.703 0.001</point>
+          <point>37.608 -13.724 0.001</point>
+          <point>37.306 -12.767 0.001</point>
+          <point>36.923 -11.84 0.001</point>
+          <point>36.459 -10.95 0.001</point>
+          <point>35.92 -10.104 0.001</point>
+          <point>35.31 -9.308 0.001</point>
+          <point>34.632 -8.568 0.001</point>
+          <point>33.892 -7.89 0.001</point>
+          <point>33.096 -7.28 0.001</point>
+          <point>32.25 -6.741 0.001</point>
+          <point>31.36 -6.277 0.001</point>
+          <point>30.433 -5.894 0.001</point>
+          <point>29.476 -5.592 0.001</point>
+          <point>28.497 -5.375 0.001</point>
+          <point>27.502 -5.244 0.001</point>
+          <point>26.5 -5.2 0.001</point>
+          <point> 0 -5.2 0</point>
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Green</name>
+        </script>
+      </material>
+    </road>   
+
+    <include>
+      <pose>0.0 0.0 0.01 0.0 0.0 </pose>
+      <uri>model://ai_car_active_caster</uri>
+    </include>
+
+  </world>
+</sdf>
diff --git a/simulator/simulator/worlds/shihou_course/ai_car_caster_trail.model b/simulator/simulator/worlds/shihou_course/ai_car_caster_trail.model
new file mode 100644
index 0000000..913c276
--- /dev/null
+++ b/simulator/simulator/worlds/shihou_course/ai_car_caster_trail.model
@@ -0,0 +1,619 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="default">
+
+    <model name="ground_plane">
+      <static>true</static>
+      <pose>0 0 -0.001 0 0 0</pose>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>200 200</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+        <visual name="visual">
+          <cast_shadows>false</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>200 200</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <scene>
+      <shadows>false</shadows>
+      <sky>
+        <clouds>12</clouds>
+      </sky>
+    </scene>
+
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+
+    <physics type="ode">
+      <real_time_update_rate>1000.0</real_time_update_rate>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>150</iters>
+          <precon_iters>0</precon_iters>
+          <sor>1.400000</sor>
+          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
+        </solver>
+        <constraints>
+          <cfm>0.00001</cfm>
+          <erp>0.2</erp>
+          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
+          <contact_surface_layer>0.01000</contact_surface_layer>
+        </constraints>
+      </ode>
+    </physics>
+    
+    <road name="my_road">
+      <width>9.0</width>
+      <point>  0       1.800 0</point>
+      <point>-19.15 1.8 0</point>
+      <point>-20.762 1.73 0</point>
+      <point>-22.362 1.519 0</point>
+      <point>-23.938 1.17 0</point>
+      <point>-25.477 0.684 0</point>
+      <point>-26.968 0.067 0</point>
+      <point>-28.4 -0.679 0</point>
+      <point>-29.761 -1.546 0</point>
+      <point>-31.042 -2.528 0</point>
+      <point>-32.231 -3.619 0</point>
+      <point>-33.322 -4.808 0</point>
+      <point>-34.304 -6.089 0</point>
+      <point>-35.171 -7.45 0</point>
+      <point>-35.917 -8.882 0</point>
+      <point>-36.534 -10.373 0</point>
+      <point>-37.02 -11.912 0</point>
+      <point>-37.369 -13.488 0</point>
+      <point>-37.58 -15.088 0</point>
+      <point>-37.65 -16.7 0</point>
+      <point>-37.65 -37.7 0</point>
+      <point>-37.58 -39.312 0</point>
+      <point>-37.369 -40.912 0</point>
+      <point>-37.02 -42.488 0</point>
+      <point>-36.534 -44.027 0</point>
+      <point>-35.917 -45.518 0</point>
+      <point>-35.171 -46.95 0</point>
+      <point>-34.304 -48.311 0</point>
+      <point>-33.322 -49.592 0</point>
+      <point>-32.231 -50.781 0</point>
+      <point>-31.042 -51.872 0</point>
+      <point>-29.761 -52.854 0</point>
+      <point>-28.4 -53.721 0</point>
+      <point>-26.968 -54.467 0</point>
+      <point>-25.477 -55.084 0</point>
+      <point>20.173 -73.584 0</point>
+      <point>21.712 -74.07 0</point>
+      <point>23.288 -74.419 0</point>
+      <point>24.888 -74.63 0</point>
+      <point>26.5 -74.7 0</point>
+      <point>28.112 -74.63 0</point>
+      <point>29.712 -74.419 0</point>
+      <point>31.288 -74.07 0</point>
+      <point>32.827 -73.584 0</point>
+      <point>34.318 -72.967 0</point>
+      <point>35.75 -72.221 0</point>
+      <point>37.111 -71.354 0</point>
+      <point>38.392 -70.372 0</point>
+      <point>39.581 -69.281 0</point>
+      <point>40.672 -68.092 0</point>
+      <point>41.654 -66.811 0</point>
+      <point>42.521 -65.45 0</point>
+      <point>43.267 -64.018 0</point>
+      <point>43.884 -62.527 0</point>
+      <point>44.37 -60.988 0</point>
+      <point>44.719 -59.412 0</point>
+      <point>44.93 -57.812 0</point>
+      <point>45 -56.2 0</point>
+      <point>45 -16.7 0</point>
+      <point>44.93 -15.088 0</point>
+      <point>44.719 -13.488 0</point>
+      <point>44.37 -11.912 0</point>
+      <point>43.884 -10.373 0</point>
+      <point>43.267 -8.882 0</point>
+      <point>42.521 -7.45 0</point>
+      <point>41.654 -6.089 0</point>
+      <point>40.672 -4.808 0</point>
+      <point>39.581 -3.619 0</point>
+      <point>38.392 -2.528 0</point>
+      <point>37.111 -1.546 0</point>
+      <point>35.75 -0.679 0</point>
+      <point>34.318 0.067 0</point>
+      <point>32.827 0.684 0</point>
+      <point>31.288 1.17 0</point>
+      <point>29.712 1.519 0</point>
+      <point>28.112 1.73 0</point>
+      <point>26.5 1.8 0</point>
+      <point>  0      1.8 0</point>
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Tertiary</name>
+        </script>
+      </material>
+    </road>
+
+  
+
+    <road name="my_road_center">
+      <width>0.2</width>
+      <point>  0       1.800 0.001</point>
+      <point>-19.15 1.8 0.001</point>
+      <point>-20.762 1.73 0.001</point>
+      <point>-22.362 1.519 0.001</point>
+      <point>-23.938 1.17 0.001</point>
+      <point>-25.477 0.684 0.001</point>
+      <point>-26.968 0.067 0.001</point>
+      <point>-28.4 -0.679 0.001</point>
+      <point>-29.761 -1.546 0.001</point>
+      <point>-31.042 -2.528 0.001</point>
+      <point>-32.231 -3.619 0.001</point>
+      <point>-33.322 -4.808 0.001</point>
+      <point>-34.304 -6.089 0.001</point>
+      <point>-35.171 -7.45 0.001</point>
+      <point>-35.917 -8.882 0.001</point>
+      <point>-36.534 -10.373 0.001</point>
+      <point>-37.02 -11.912 0.001</point>
+      <point>-37.369 -13.488 0.001</point>
+      <point>-37.58 -15.088 0.001</point>
+      <point>-37.65 -16.7 0.001</point>
+      <point>-37.65 -37.7 0.001</point>
+      <point>-37.58 -39.312 0.001</point>
+      <point>-37.369 -40.912 0.001</point>
+      <point>-37.02 -42.488 0.001</point>
+      <point>-36.534 -44.027 0.001</point>
+      <point>-35.917 -45.518 0.001</point>
+      <point>-35.171 -46.95 0.001</point>
+      <point>-34.304 -48.311 0.001</point>
+      <point>-33.322 -49.592 0.001</point>
+      <point>-32.231 -50.781 0.001</point>
+      <point>-31.042 -51.872 0.001</point>
+      <point>-29.761 -52.854 0.001</point>
+      <point>-28.4 -53.721 0.001</point>
+      <point>-26.968 -54.467 0.001</point>
+      <point>-25.477 -55.084 0.001</point>
+      <point>20.173 -73.584 0.001</point>
+      <point>21.712 -74.07 0.001</point>
+      <point>23.288 -74.419 0.001</point>
+      <point>24.888 -74.63 0.001</point>
+      <point>26.5 -74.7 0.001</point>
+      <point>28.112 -74.63 0.001</point>
+      <point>29.712 -74.419 0.001</point>
+      <point>31.288 -74.07 0.001</point>
+      <point>32.827 -73.584 0.001</point>
+      <point>34.318 -72.967 0.001</point>
+      <point>35.75 -72.221 0.001</point>
+      <point>37.111 -71.354 0.001</point>
+      <point>38.392 -70.372 0.001</point>
+      <point>39.581 -69.281 0.001</point>
+      <point>40.672 -68.092 0.001</point>
+      <point>41.654 -66.811 0.001</point>
+      <point>42.521 -65.45 0.001</point>
+      <point>43.267 -64.018 0.001</point>
+      <point>43.884 -62.527 0.001</point>
+      <point>44.37 -60.988 0.001</point>
+      <point>44.719 -59.412 0.001</point>
+      <point>44.93 -57.812 0.001</point>
+      <point>45 -56.2 0.001</point>
+      <point>45 -16.7 0.001</point>
+      <point>44.93 -15.088 0.001</point>
+      <point>44.719 -13.488 0.001</point>
+      <point>44.37 -11.912 0.001</point>
+      <point>43.884 -10.373 0.001</point>
+      <point>43.267 -8.882 0.001</point>
+      <point>42.521 -7.45 0.001</point>
+      <point>41.654 -6.089 0.001</point>
+      <point>40.672 -4.808 0.001</point>
+      <point>39.581 -3.619 0.001</point>
+      <point>38.392 -2.528 0.001</point>
+      <point>37.111 -1.546 0.001</point>
+      <point>35.75 -0.679 0.001</point>
+      <point>34.318 0.067 0.001</point>
+      <point>32.827 0.684 0.001</point>
+      <point>31.288 1.17 0.001</point>
+      <point>29.712 1.519 0.001</point>
+      <point>28.112 1.73 0.001</point>
+      <point>26.5 1.8 0.001</point>
+      <point>  0      1.8 0.001</point>
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/White</name>
+        </script>
+      </material>
+    </road>
+    
+    <road name="my_road_left">
+      <width>0.2</width>
+      <point>0 -1.5 0.001</point>
+      <point>-19.15 -1.5 0.001</point>
+      <point>-20.475 -1.558 0.001</point>
+      <point>-21.789 -1.731 0.001</point>
+      <point>-23.084 -2.018 0.001</point>
+      <point>-24.349 -2.417 0.001</point>
+      <point>-25.574 -2.924 0.001</point>
+      <point>-26.75 -3.536 0.001</point>
+      <point>-27.868 -4.249 0.001</point>
+      <point>-28.92 -5.056 0.001</point>
+      <point>-29.898 -5.952 0.001</point>
+      <point>-30.794 -6.93 0.001</point>
+      <point>-31.601 -7.982 0.001</point>
+      <point>-32.314 -9.1 0.001</point>
+      <point>-32.926 -10.276 0.001</point>
+      <point>-33.433 -11.501 0.001</point>
+      <point>-33.832 -12.766 0.001</point>
+      <point>-34.119 -14.061 0.001</point>
+      <point>-34.292 -15.375 0.001</point>
+      <point>-34.35 -16.7 0.001</point>
+      <point>-34.35 -37.7 0.001</point>
+      <point>-34.292 -39.025 0.001</point>
+      <point>-34.119 -40.339 0.001</point>
+      <point>-33.832 -41.634 0.001</point>
+      <point>-33.433 -42.899 0.001</point>
+      <point>-32.926 -44.124 0.001</point>
+      <point>-32.314 -45.3 0.001</point>
+      <point>-31.601 -46.418 0.001</point>
+      <point>-30.794 -47.47 0.001</point>
+      <point>-29.898 -48.448 0.001</point>
+      <point>-28.92 -49.344 0.001</point>
+      <point>-27.868 -50.151 0.001</point>
+      <point>-26.75 -50.864 0.001</point>
+      <point>-25.574 -51.476 0.001</point>
+      <point>-24.349 -51.983 0.001</point>
+      <point>21.301 -70.483 0.001</point>
+      <point>22.566 -70.882 0.001</point>
+      <point>23.861 -71.169 0.001</point>
+      <point>25.175 -71.342 0.001</point>
+      <point>26.5 -71.4 0.001</point>
+      <point>27.825 -71.342 0.001</point>
+      <point>29.139 -71.169 0.001</point>
+      <point>30.434 -70.882 0.001</point>
+      <point>31.699 -70.483 0.001</point>
+      <point>32.924 -69.976 0.001</point>
+      <point>34.1 -69.364 0.001</point>
+      <point>35.218 -68.651 0.001</point>
+      <point>36.27 -67.844 0.001</point>
+      <point>37.248 -66.948 0.001</point>
+      <point>38.144 -65.97 0.001</point>
+      <point>38.951 -64.918 0.001</point>
+      <point>39.664 -63.8 0.001</point>
+      <point>40.276 -62.624 0.001</point>
+      <point>40.783 -61.399 0.001</point>
+      <point>41.182 -60.134 0.001</point>
+      <point>41.469 -58.839 0.001</point>
+      <point>41.642 -57.525 0.001</point>
+      <point>41.7 -56.2 0.001</point>
+      <point>41.7 -16.7 0.001</point>
+      <point>41.642 -15.375 0.001</point>
+      <point>41.469 -14.061 0.001</point>
+      <point>41.182 -12.766 0.001</point>
+      <point>40.783 -11.501 0.001</point>
+      <point>40.276 -10.276 0.001</point>
+      <point>39.664 -9.1 0.001</point>
+      <point>38.951 -7.982 0.001</point>
+      <point>38.144 -6.93 0.001</point>
+      <point>37.248 -5.952 0.001</point>
+      <point>36.27 -5.056 0.001</point>
+      <point>35.218 -4.249 0.001</point>
+      <point>34.1 -3.536 0.001</point>
+      <point>32.924 -2.924 0.001</point>
+      <point>31.699 -2.417 0.001</point>
+      <point>30.434 -2.018 0.001</point>
+      <point>29.139 -1.731 0.001</point>
+      <point>27.825 -1.558 0.001</point>
+      <point>26.5 -1.5 0.001</point>
+      <point>0 -1.5 0.001</point>
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/White</name>
+        </script>
+      </material>
+    </road>
+    
+    <road name="my_road_right">
+      <width>0.2</width>
+      <point>0 5.1 0.001</point>
+      <point>-19.15 5.1 0.001</point>
+      <point>-21.05 5.017 0.001</point>
+      <point>-22.936 4.769 0.001</point>
+      <point>-24.792 4.357 0.001</point>
+      <point>-26.606 3.785 0.001</point>
+      <point>-28.363 3.058 0.001</point>
+      <point>-30.05 2.179 0.001</point>
+      <point>-31.654 1.158 0.001</point>
+      <point>-33.163 0 0.001</point>
+      <point>-34.565 -1.285 0.001</point>
+      <point>-35.85 -2.687 0.001</point>
+      <point>-37.008 -4.196 0.001</point>
+      <point>-38.029 -5.8 0.001</point>
+      <point>-38.908 -7.487 0.001</point>
+      <point>-39.635 -9.244 0.001</point>
+      <point>-40.207 -11.058 0.001</point>
+      <point>-40.619 -12.914 0.001</point>
+      <point>-40.867 -14.8 0.001</point>
+      <point>-40.95 -16.7 0.001</point>
+      <point>-40.95 -37.7 0.001</point>
+      <point>-40.867 -39.6 0.001</point>
+      <point>-40.619 -41.486 0.001</point>
+      <point>-40.207 -43.342 0.001</point>
+      <point>-39.635 -45.156 0.001</point>
+      <point>-38.908 -46.913 0.001</point>
+      <point>-38.029 -48.6 0.001</point>
+      <point>-37.008 -50.204 0.001</point>
+      <point>-35.85 -51.713 0.001</point>
+      <point>-34.565 -53.115 0.001</point>
+      <point>-33.163 -54.4 0.001</point>
+      <point>-31.654 -55.558 0.001</point>
+      <point>-30.05 -56.579 0.001</point>
+      <point>-28.363 -57.458 0.001</point>
+      <point>-26.606 -58.185 0.001</point>
+      <point>19.044 -76.685 0.001</point>
+      <point>20.858 -77.257 0.001</point>
+      <point>22.714 -77.669 0.001</point>
+      <point>24.6 -77.917 0.001</point>
+      <point>26.5 -78 0.001</point>
+      <point>28.4 -77.917 0.001</point>
+      <point>30.286 -77.669 0.001</point>
+      <point>32.142 -77.257 0.001</point>
+      <point>33.956 -76.685 0.001</point>
+      <point>35.713 -75.958 0.001</point>
+      <point>37.4 -75.079 0.001</point>
+      <point>39.004 -74.058 0.001</point>
+      <point>40.513 -72.9 0.001</point>
+      <point>41.915 -71.615 0.001</point>
+      <point>43.2 -70.213 0.001</point>
+      <point>44.358 -68.704 0.001</point>
+      <point>45.379 -67.1 0.001</point>
+      <point>46.258 -65.413 0.001</point>
+      <point>46.985 -63.656 0.001</point>
+      <point>47.557 -61.842 0.001</point>
+      <point>47.969 -59.986 0.001</point>
+      <point>48.217 -58.1 0.001</point>
+      <point>48.3 -56.2 0.001</point>
+      <point>48.3 -16.7 0.001</point>
+      <point>48.217 -14.8 0.001</point>
+      <point>47.969 -12.914 0.001</point>
+      <point>47.557 -11.058 0.001</point>
+      <point>46.985 -9.244 0.001</point>
+      <point>46.258 -7.487 0.001</point>
+      <point>45.379 -5.8 0.001</point>
+      <point>44.358 -4.196 0.001</point>
+      <point>43.2 -2.687 0.001</point>
+      <point>41.915 -1.285 0.001</point>
+      <point>40.513 0 0.001</point>
+      <point>39.004 1.158 0.001</point>
+      <point>37.4 2.179 0.001</point>
+      <point>35.713 3.058 0.001</point>
+      <point>33.956 3.785 0.001</point>
+      <point>32.142 4.357 0.001</point>
+      <point>30.286 4.769 0.001</point>
+      <point>28.4 5.017 0.001</point>
+      <point>26.5 5.1 0.001</point>
+      <point>0 5.1 0.001</point>
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/White</name>
+        </script>
+      </material>
+    </road>
+
+  <road name="my_walkway_out">
+      <width>5.0</width>
+      <point>  0       8.800 0</point>
+      <point>-19.15 8.8 0.001</point>
+      <point>-21.372 8.703 0.001</point>
+      <point>-23.578 8.413 0.001</point>
+      <point>-25.75 7.931 0.001</point>
+      <point>-27.872 7.262 0.001</point>
+      <point>-29.927 6.411 0.001</point>
+      <point>-31.9 5.384 0.001</point>
+      <point>-33.776 4.188 0.001</point>
+      <point>-35.541 2.834 0.001</point>
+      <point>-37.181 1.331 0.001</point>
+      <point>-38.684 -0.309 0.001</point>
+      <point>-40.038 -2.074 0.001</point>
+      <point>-41.234 -3.95 0.001</point>
+      <point>-42.261 -5.923 0.001</point>
+      <point>-43.112 -7.978 0.001</point>
+      <point>-43.781 -10.1 0.001</point>
+      <point>-44.263 -12.272 0.001</point>
+      <point>-44.553 -14.478 0.001</point>
+      <point>-44.65 -16.7 0.001</point>
+      <point>-44.65 -37.7 0.001</point>
+      <point>-44.553 -39.922 0.001</point>
+      <point>-44.263 -42.128 0.001</point>
+      <point>-43.781 -44.3 0.001</point>
+      <point>-43.112 -46.422 0.001</point>
+      <point>-42.261 -48.477 0.001</point>
+      <point>-41.234 -50.45 0.001</point>
+      <point>-40.038 -52.326 0.001</point>
+      <point>-38.684 -54.091 0.001</point>
+      <point>-37.181 -55.731 0.001</point>
+      <point>-35.541 -57.234 0.001</point>
+      <point>-33.776 -58.588 0.001</point>
+      <point>-31.9 -59.784 0.001</point>
+      <point>-29.927 -60.811 0.001</point>
+      <point>-27.872 -61.662 0.001</point>
+      <point>17.778 -80.162 0.001</point>
+      <point>19.9 -80.831 0.001</point>
+      <point>22.072 -81.313 0.001</point>
+      <point>24.278 -81.603 0.001</point>
+      <point>26.5 -81.7 0.001</point>
+      <point>28.722 -81.603 0.001</point>
+      <point>30.928 -81.313 0.001</point>
+      <point>33.1 -80.831 0.001</point>
+      <point>35.222 -80.162 0.001</point>
+      <point>37.277 -79.311 0.001</point>
+      <point>39.25 -78.284 0.001</point>
+      <point>41.126 -77.088 0.001</point>
+      <point>42.891 -75.734 0.001</point>
+      <point>44.531 -74.231 0.001</point>
+      <point>46.034 -72.591 0.001</point>
+      <point>47.388 -70.826 0.001</point>
+      <point>48.584 -68.95 0.001</point>
+      <point>49.611 -66.977 0.001</point>
+      <point>50.462 -64.922 0.001</point>
+      <point>51.131 -62.8 0.001</point>
+      <point>51.613 -60.628 0.001</point>
+      <point>51.903 -58.422 0.001</point>
+      <point>52 -56.2 0.001</point>
+      <point>52 -16.7 0.001</point>
+      <point>51.903 -14.478 0.001</point>
+      <point>51.613 -12.272 0.001</point>
+      <point>51.131 -10.1 0.001</point>
+      <point>50.462 -7.978 0.001</point>
+      <point>49.611 -5.923 0.001</point>
+      <point>48.584 -3.95 0.001</point>
+      <point>47.388 -2.074 0.001</point>
+      <point>46.034 -0.309 0.001</point>
+      <point>44.531 1.331 0.001</point>
+      <point>42.891 2.834 0.001</point>
+      <point>41.126 4.188 0.001</point>
+      <point>39.25 5.384 0.001</point>
+      <point>37.277 6.411 0.001</point>
+      <point>35.222 7.262 0.001</point>
+      <point>33.1 7.931 0.001</point>
+      <point>30.928 8.413 0.001</point>
+      <point>28.722 8.703 0.001</point>
+      <point>26.5 8.8 0.001</point>
+      <point>  0      8.8 0</point>
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Green</name>
+        </script>
+      </material>
+    </road>   
+
+    <road name="my_walkway_in">
+      <width>5.0</width>
+          <point>  0  -5.2 0</point>
+          <point>-19.15 -5.2 0.001</point>
+          <point>-20.152 -5.244 0.001</point>
+          <point>-21.147 -5.375 0.001</point>
+          <point>-22.126 -5.592 0.001</point>
+          <point>-23.083 -5.894 0.001</point>
+          <point>-24.01 -6.277 0.001</point>
+          <point>-24.9 -6.741 0.001</point>
+          <point>-25.746 -7.28 0.001</point>
+          <point>-26.542 -7.89 0.001</point>
+          <point>-27.282 -8.568 0.001</point>
+          <point>-27.96 -9.308 0.001</point>
+          <point>-28.57 -10.104 0.001</point>
+          <point>-29.109 -10.95 0.001</point>
+          <point>-29.573 -11.84 0.001</point>
+          <point>-29.956 -12.767 0.001</point>
+          <point>-30.258 -13.724 0.001</point>
+          <point>-30.475 -14.703 0.001</point>
+          <point>-30.606 -15.698 0.001</point>
+          <point>-30.65 -16.7 0.001</point>
+          <point>-30.65 -37.7 0.001</point>
+          <point>-30.606 -38.702 0.001</point>
+          <point>-30.475 -39.697 0.001</point>
+          <point>-30.258 -40.676 0.001</point>
+          <point>-29.956 -41.633 0.001</point>
+          <point>-29.573 -42.56 0.001</point>
+          <point>-29.109 -43.45 0.001</point>
+          <point>-28.57 -44.296 0.001</point>
+          <point>-27.96 -45.092 0.001</point>
+          <point>-27.282 -45.832 0.001</point>
+          <point>-26.542 -46.51 0.001</point>
+          <point>-25.746 -47.12 0.001</point>
+          <point>-24.9 -47.659 0.001</point>
+          <point>-24.01 -48.123 0.001</point>
+          <point>-23.083 -48.506 0.001</point>
+          <point>22.567 -67.006 0.001</point>
+          <point>23.524 -67.308 0.001</point>
+          <point>24.503 -67.525 0.001</point>
+          <point>25.498 -67.656 0.001</point>
+          <point>26.5 -67.7 0.001</point>
+          <point>27.502 -67.656 0.001</point>
+          <point>28.497 -67.525 0.001</point>
+          <point>29.476 -67.308 0.001</point>
+          <point>30.433 -67.006 0.001</point>
+          <point>31.36 -66.623 0.001</point>
+          <point>32.25 -66.159 0.001</point>
+          <point>33.096 -65.62 0.001</point>
+          <point>33.892 -65.01 0.001</point>
+          <point>34.632 -64.332 0.001</point>
+          <point>35.31 -63.592 0.001</point>
+          <point>35.92 -62.796 0.001</point>
+          <point>36.459 -61.95 0.001</point>
+          <point>36.923 -61.06 0.001</point>
+          <point>37.306 -60.133 0.001</point>
+          <point>37.608 -59.176 0.001</point>
+          <point>37.825 -58.197 0.001</point>
+          <point>37.956 -57.202 0.001</point>
+          <point>38 -56.2 0.001</point>
+          <point>38 -16.7 0.001</point>
+          <point>37.956 -15.698 0.001</point>
+          <point>37.825 -14.703 0.001</point>
+          <point>37.608 -13.724 0.001</point>
+          <point>37.306 -12.767 0.001</point>
+          <point>36.923 -11.84 0.001</point>
+          <point>36.459 -10.95 0.001</point>
+          <point>35.92 -10.104 0.001</point>
+          <point>35.31 -9.308 0.001</point>
+          <point>34.632 -8.568 0.001</point>
+          <point>33.892 -7.89 0.001</point>
+          <point>33.096 -7.28 0.001</point>
+          <point>32.25 -6.741 0.001</point>
+          <point>31.36 -6.277 0.001</point>
+          <point>30.433 -5.894 0.001</point>
+          <point>29.476 -5.592 0.001</point>
+          <point>28.497 -5.375 0.001</point>
+          <point>27.502 -5.244 0.001</point>
+          <point>26.5 -5.2 0.001</point>
+          <point> 0 -5.2 0</point>
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Green</name>
+        </script>
+      </material>
+    </road>   
+
+    <include>
+      <pose>0.0 0.0 0.01 0.0 0.0 </pose>
+      <uri>model://ai_car_caster_trail</uri>
+    </include>
+
+  </world>
+</sdf>
diff --git a/simulator/vehicle/launch/extrinsic_tfstatic_broadcaster_no_namespace.launch.py b/simulator/vehicle/launch/extrinsic_tfstatic_broadcaster_no_namespace.launch.py
new file mode 100644
index 0000000..f0f2934
--- /dev/null
+++ b/simulator/vehicle/launch/extrinsic_tfstatic_broadcaster_no_namespace.launch.py
@@ -0,0 +1,108 @@
+import os.path as osp
+from typing import Tuple
+from launch import LaunchDescription, LaunchContext
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition
+from ament_index_python.packages import get_package_share_directory
+from vehicle.vehicle_util import convert_xacro_to_urdf
+
+
+def get_robot_state_publisher(
+    context: LaunchContext,
+    vehicle_name_lc: LaunchConfiguration,
+    use_sim_time_lc: LaunchConfiguration
+) -> Tuple[Node]:
+    vehicle_name = context.perform_substitution(vehicle_name_lc)
+    xacro_path = osp.join(get_package_share_directory("vehicle"),
+                          "xacro", vehicle_name + ".xacro")
+    urdf_path = osp.join(get_package_share_directory("vehicle"),
+                         "xacro", vehicle_name + ".urdf")
+    convert_xacro_to_urdf(xacro_path, urdf_path)
+    with open(urdf_path, 'r') as infp:
+        robot_description = infp.read()
+
+    return (
+        Node(
+            package="robot_state_publisher",
+            executable="robot_state_publisher",
+            name="robot_state_publisher",
+            #namespace="/aiformula_sensing",
+            parameters=[{
+                "robot_description": robot_description,
+                "use_sim_time": use_sim_time_lc,
+            }],
+            arguments=["--ros-args", "--log-level", "WARN"],  # No output
+        ),
+    )
+
+
+def get_joint_state_publisher(
+    context: LaunchContext,
+    use_joint_state_publisher_lc: LaunchConfiguration,
+    use_gui_lc: LaunchConfiguration,
+    use_sim_time_lc: LaunchConfiguration
+) -> Tuple[Node]:
+    use_gui = context.perform_substitution(use_gui_lc).lower() == "true"
+    node_name = "joint_state_publisher_gui" if use_gui else "joint_state_publisher"
+    return (
+        Node(
+            package=node_name,
+            executable=node_name,
+            name=node_name,
+            #namespace="/aiformula_sensing",
+            parameters=[{
+                "use_sim_time": use_sim_time_lc,
+            }],
+            arguments=["--ros-args", "--log-level", "WARN"],  # No output
+            condition=IfCondition(use_joint_state_publisher_lc),
+        ),
+    )
+
+
+def generate_launch_description():
+    launch_args = (
+        DeclareLaunchArgument(
+            "vehicle_name",
+            default_value="ai_car1",
+            description="Vehicle Name",
+        ),
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="false",
+            description="If true, use simulation (Gazebo) clock",
+        ),
+        DeclareLaunchArgument(
+            "use_joint_state_publisher",
+            default_value="true",
+            description="If true, use joint_state_publisher",
+        ),
+        DeclareLaunchArgument(
+            "use_gui",
+            default_value="false",
+            description="If true, use joint_state_publisher_gui",
+        ),
+    )
+
+    robot_state_publisher = OpaqueFunction(
+        function=get_robot_state_publisher,
+        args=[
+            LaunchConfiguration("vehicle_name"),
+            LaunchConfiguration("use_sim_time"),
+        ],
+    )
+    joint_state_publisher = OpaqueFunction(
+        function=get_joint_state_publisher,
+        args=[
+            LaunchConfiguration("use_joint_state_publisher"),
+            LaunchConfiguration("use_gui"),
+            LaunchConfiguration("use_sim_time"),
+        ],
+    )
+
+    return LaunchDescription([
+        *launch_args,
+        robot_state_publisher,
+        joint_state_publisher,
+    ])
diff --git a/simulator/vehicle/xacro/ai_car_active_caster.xacro b/simulator/vehicle/xacro/ai_car_active_caster.xacro
new file mode 100644
index 0000000..b772827
--- /dev/null
+++ b/simulator/vehicle/xacro/ai_car_active_caster.xacro
@@ -0,0 +1,62 @@
+<?xml version="1.0" ?>
+<robot name="ai_car1" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:include filename="parts/base.xacro"/>
+  <xacro:include filename="parts/wheel_macro.xacro"/>
+  <xacro:include filename="parts/active_caster_macro.xacro"/>
+  <xacro:include filename="parts/zed_macro.xacro"/>
+
+  <xacro:property name="PI" value="3.1415926"/>
+  <xacro:property name="D2R" value="${PI/180}"/>
+  <xacro:property name="WHEEL_RADIUS" value="0.12"/>
+  <xacro:property name="CASTER_RADIUS" value="0.10"/>
+
+  <xacro:property name="BASE_FOOTPRINT" value="base_footprint"/>
+  <xacro:property name="BASE" value="base"/>
+  <xacro:property name="ZEDX" value="zed"/>
+  <xacro:property name="WHEEL_LEFT" value="wheel_left"/>
+  <xacro:property name="WHEEL_RIGHT" value="wheel_right"/>
+  <xacro:property name="CASTER_BACK" value="caster_back"/>
+
+  <xacro:property name="ZED_SERIAL_NUMBER" value="SN48442725"/>
+
+  <!-- base_footprint -->
+  <link name="${BASE_FOOTPRINT}"/>
+
+  <!-- base -->
+  <xacro:base_macro
+    base="${BASE}" parent_link="${BASE_FOOTPRINT}"
+    x="0.0" y="0.0" z="${WHEEL_RADIUS}"
+    roll="0.0" pitch="0.0" yaw="${180.0*D2R}"/>
+
+  <!-- zedx -->
+  <xacro:zed_macro
+    camera="${ZEDX}" parent="${BASE}" zed_serial_number="${ZED_SERIAL_NUMBER}"/>
+
+  <!-- wheel -->
+  <xacro:wheel_joint_macro
+    wheel="${WHEEL_LEFT}" parent="${BASE}"
+    x="0.0" y="0.3" z="${WHEEL_RADIUS}"
+    roll="${90.0*D2R}" pitch="0.0" yaw="${90.0*D2R}"/>
+  <xacro:wheel_link_macro
+    wheel="${WHEEL_LEFT}"
+    x="0.0" y="0.0" z="0.0"
+    roll="0.0" pitch="0.0" yaw="0.0"/>
+  <xacro:wheel_joint_macro
+    wheel="${WHEEL_RIGHT}" parent="${BASE}"
+    x="0.0" y="-0.3" z="${WHEEL_RADIUS}"
+    roll="${90.0*D2R}" pitch="0.0" yaw="${90.0*D2R}"/>
+  <xacro:wheel_link_macro
+    wheel="${WHEEL_RIGHT}"
+    x="0.0" y="0.0" z="0.0"
+    roll="0.0" pitch="0.0" yaw="0.0"/>
+
+  <!-- caster_back -->
+  <xacro:caster_back_joint_macro
+    caster_back="${CASTER_BACK}" parent="${BASE}"
+    x="-0.76" y="0.0" z="${CASTER_RADIUS}"
+    roll="0.0" pitch="0.0" yaw="0.0"/>
+  <xacro:caster_back_link_macro
+    caster_back="${CASTER_BACK}"
+    x="0.0" y="0.0" z="0.0"
+    roll="0.0" pitch="0.0" yaw="0.0"/>
+</robot>
diff --git a/simulator/vehicle/xacro/ai_car_caster_trail.xacro b/simulator/vehicle/xacro/ai_car_caster_trail.xacro
new file mode 100644
index 0000000..e3845dd
--- /dev/null
+++ b/simulator/vehicle/xacro/ai_car_caster_trail.xacro
@@ -0,0 +1,62 @@
+<?xml version="1.0" ?>
+<robot name="ai_car1" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:include filename="parts/base.xacro"/>
+  <xacro:include filename="parts/wheel_macro.xacro"/>
+  <xacro:include filename="parts/caster_trail_macro.xacro"/>
+  <xacro:include filename="parts/zed_macro.xacro"/>
+
+  <xacro:property name="PI" value="3.1415926"/>
+  <xacro:property name="D2R" value="${PI/180}"/>
+  <xacro:property name="WHEEL_RADIUS" value="0.12"/>
+  <xacro:property name="CASTER_RADIUS" value="0.10"/>
+
+  <xacro:property name="BASE_FOOTPRINT" value="base_footprint"/>
+  <xacro:property name="BASE" value="base"/>
+  <xacro:property name="ZEDX" value="zed"/>
+  <xacro:property name="WHEEL_LEFT" value="wheel_left"/>
+  <xacro:property name="WHEEL_RIGHT" value="wheel_right"/>
+  <xacro:property name="CASTER_BACK" value="caster_back"/>
+
+  <xacro:property name="ZED_SERIAL_NUMBER" value="SN48442725"/>
+
+  <!-- base_footprint -->
+  <link name="${BASE_FOOTPRINT}"/>
+
+  <!-- base -->
+  <xacro:base_macro
+    base="${BASE}" parent_link="${BASE_FOOTPRINT}"
+    x="0.0" y="0.0" z="${WHEEL_RADIUS}"
+    roll="0.0" pitch="0.0" yaw="${180.0*D2R}"/>
+
+  <!-- zedx -->
+  <xacro:zed_macro
+    camera="${ZEDX}" parent="${BASE}" zed_serial_number="${ZED_SERIAL_NUMBER}"/>
+
+  <!-- wheel -->
+  <xacro:wheel_joint_macro
+    wheel="${WHEEL_LEFT}" parent="${BASE}"
+    x="0.0" y="0.3" z="${WHEEL_RADIUS}"
+    roll="${90.0*D2R}" pitch="0.0" yaw="${90.0*D2R}"/>
+  <xacro:wheel_link_macro
+    wheel="${WHEEL_LEFT}"
+    x="0.0" y="0.0" z="0.0"
+    roll="0.0" pitch="0.0" yaw="0.0"/>
+  <xacro:wheel_joint_macro
+    wheel="${WHEEL_RIGHT}" parent="${BASE}"
+    x="0.0" y="-0.3" z="${WHEEL_RADIUS}"
+    roll="${90.0*D2R}" pitch="0.0" yaw="${90.0*D2R}"/>
+  <xacro:wheel_link_macro
+    wheel="${WHEEL_RIGHT}"
+    x="0.0" y="0.0" z="0.0"
+    roll="0.0" pitch="0.0" yaw="0.0"/>
+
+  <!-- caster_back -->
+  <xacro:caster_back_joint_macro
+    caster_back="${CASTER_BACK}" parent="${BASE}"
+    x="-0.76" y="0.0" z="${CASTER_RADIUS}"
+    roll="0.0" pitch="0.0" yaw="0.0"/>
+  <xacro:caster_back_link_macro
+    caster_back="${CASTER_BACK}"
+    x="0.0" y="0.0" z="0.0"
+    roll="0.0" pitch="0.0" yaw="0.0"/>
+</robot>
diff --git a/simulator/vehicle/xacro/parts/active_caster_macro.xacro b/simulator/vehicle/xacro/parts/active_caster_macro.xacro
new file mode 100644
index 0000000..d85aece
--- /dev/null
+++ b/simulator/vehicle/xacro/parts/active_caster_macro.xacro
@@ -0,0 +1,66 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:property name="PI" value="3.1415926"/>
+  <xacro:macro name="caster_back_joint_macro"
+               params="caster_back parent
+                       x y z roll pitch yaw">
+    <joint name="${caster_back}_steering_joint" type="continuous">
+      <parent link="${parent}_link"/>
+      <child  link="${caster_back}_steering_link"/>
+      <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="1000.0" lower="-1.57" upper="1.57" velocity="10"/>
+    </joint>
+    <joint name="${caster_back}_joint" type="continuous">
+      <parent link="${caster_back}_steering_link"/>
+      <child  link="${caster_back}_link"/>
+      <origin xyz="-0.2 0 0" rpy="0 0 0"/>
+      <axis xyz="0 1 0"/>
+    </joint>
+  </xacro:macro>
+
+  <xacro:macro name="caster_back_link_macro"
+               params="caster_back
+                       x y z roll pitch yaw">
+    <xacro:property name="MESH_FILE" value="package://vehicle/xacro/meshes/tire.dae"/>
+    <xacro:property name="SCALE" value="0.10"/>
+    <link name="${caster_back}_steering_link"/>
+    <link name="${caster_back}_link">
+      <visual>
+        <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw+PI/2}"/>
+        <geometry>
+          <mesh filename="${MESH_FILE}" scale="${SCALE} ${SCALE} ${SCALE}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw+PI/2}"/>
+        <geometry>
+          <mesh filename="${MESH_FILE}" scale="${SCALE} ${SCALE} ${SCALE}"/>
+        </geometry>
+        <!-- <origin xyz="0.0  0.001  0.0" rpy="0.0  0.0  0.0"/>
+        <geometry>
+          <box size="0.030  0.009  0.020"/>
+        </geometry> -->
+      </collision>
+      <inertial>
+        <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw+PI/2}"/>
+        <mass value="0.005" />
+        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+        <!-- <mass value="5.0" />
+        <inertia ixx="0.01517" ixy="0.0" ixz="0.0" iyy="0.01517" iyz="0.0" izz="0.02500"/> -->
+      </inertial>
+    </link>
+  </xacro:macro>
+  <ros2_control name="GazeboSystem" type="system">
+    <hardware>
+      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+    </hardware>
+    <joint name="caster_back_steering_joint">
+      <command_interface name="position">
+        <param name="min">-1.57</param>
+        <param name="max">1.57</param>
+      </command_interface>
+      <state_interface name="position"/>
+    </joint>
+  </ros2_control>
+</robot>
diff --git a/simulator/vehicle/xacro/parts/caster_trail_macro.xacro b/simulator/vehicle/xacro/parts/caster_trail_macro.xacro
new file mode 100644
index 0000000..9ba9ff6
--- /dev/null
+++ b/simulator/vehicle/xacro/parts/caster_trail_macro.xacro
@@ -0,0 +1,53 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:property name="PI" value="3.1415926"/>
+  <xacro:macro name="caster_back_joint_macro"
+               params="caster_back parent
+                       x y z roll pitch yaw">
+    <joint name="${caster_back}_steering_joint" type="continuous">
+      <parent link="${parent}_link"/>
+      <child  link="${caster_back}_steering_link"/>
+      <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
+      <axis xyz="0 0 1"/>
+    </joint>
+    <joint name="${caster_back}_joint" type="continuous">
+      <parent link="${caster_back}_steering_link"/>
+      <child  link="${caster_back}_link"/>
+      <origin xyz="-0.2 0 0" rpy="0 0 0"/>
+      <axis xyz="0 1 0"/>
+    </joint>
+  </xacro:macro>
+
+  <xacro:macro name="caster_back_link_macro"
+               params="caster_back
+                       x y z roll pitch yaw">
+    <xacro:property name="MESH_FILE" value="package://vehicle/xacro/meshes/tire.dae"/>
+    <xacro:property name="SCALE" value="0.10"/>
+    <link name="${caster_back}_steering_link"/>
+    <link name="${caster_back}_link">
+      <visual>
+        <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw+PI/2}"/>
+        <geometry>
+          <mesh filename="${MESH_FILE}" scale="${SCALE} ${SCALE} ${SCALE}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw+PI/2}"/>
+        <geometry>
+          <mesh filename="${MESH_FILE}" scale="${SCALE} ${SCALE} ${SCALE}"/>
+        </geometry>
+        <!-- <origin xyz="0.0  0.001  0.0" rpy="0.0  0.0  0.0"/>
+        <geometry>
+          <box size="0.030  0.009  0.020"/>
+        </geometry> -->
+      </collision>
+      <inertial>
+        <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw+PI/2}"/>
+        <mass value="0.005" />
+        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+        <!-- <mass value="5.0" />
+        <inertia ixx="0.01517" ixy="0.0" ixz="0.0" iyy="0.01517" iyz="0.0" izz="0.02500"/> -->
+      </inertial>
+    </link>
+  </xacro:macro>  
+</robot>

From eb7eebb515824ec22007e8f6c4aacd1fbf824db2 Mon Sep 17 00:00:00 2001
From: yasuohayashibara <yasuo.hayashibara@it-chiba.ac.jp>
Date: Tue, 3 Sep 2024 17:41:55 +0900
Subject: [PATCH 2/5] add caster_controller

---
 .../gazebo_simulator_active_caster.launch.py  | 21 +++++++----
 simulator/simulator/setup.py                  |  1 +
 .../simulator/caster_controller_sim.py        | 35 +++++++++++++++++++
 3 files changed, 51 insertions(+), 6 deletions(-)
 create mode 100644 simulator/simulator/simulator/caster_controller_sim.py

diff --git a/simulator/simulator/launch/gazebo_simulator_active_caster.launch.py b/simulator/simulator/launch/gazebo_simulator_active_caster.launch.py
index e83ddfe..6a94b01 100644
--- a/simulator/simulator/launch/gazebo_simulator_active_caster.launch.py
+++ b/simulator/simulator/launch/gazebo_simulator_active_caster.launch.py
@@ -73,16 +73,25 @@ def generate_launch_description():
         output='screen'
     )
 
+    controller_manager = Node(
+        package='controller_manager',
+        executable='spawner.py',
+        arguments=['position_controller'],
+        output='screen'
+    )
+
+    caster_controller_sim = Node(
+        package='simulator',
+        executable='caster_controller_sim',
+        output='screen'
+    )
+
     return LaunchDescription([
         *launch_args,
         tf_static_publisher,
         gzserver,
         gzclient,
         set_use_sim_time,
-        Node(
-            package='controller_manager',
-            executable='spawner.py',
-            arguments=['position_controller'],
-            output='screen'
-        )
+        controller_manager,
+        caster_controller_sim
     ])
diff --git a/simulator/simulator/setup.py b/simulator/simulator/setup.py
index ee7d3a5..7091c1f 100644
--- a/simulator/simulator/setup.py
+++ b/simulator/simulator/setup.py
@@ -17,6 +17,7 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'caster_controller_sim = simulator.caster_controller_sim:main',
         ],
     },
 )
diff --git a/simulator/simulator/simulator/caster_controller_sim.py b/simulator/simulator/simulator/caster_controller_sim.py
new file mode 100644
index 0000000..86dfa05
--- /dev/null
+++ b/simulator/simulator/simulator/caster_controller_sim.py
@@ -0,0 +1,35 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float64MultiArray
+import math
+
+class CasterControllerSim(Node):
+    def __init__(self):
+        super().__init__("caster_controller_sim_node")
+        self.wheelbase = 0.65
+        self.create_subscription(Twist, "/cmd_vel", self.callback_cmd_vel, 1)
+        self.publisher = self.create_publisher(Float64MultiArray, '/position_controller/commands', 1)
+        self.get_logger().info("caster_controller_sim_node has been started.")
+
+    def callback_cmd_vel(self, msg):
+        pub_msg = Float64MultiArray()
+        if msg.linear.x == 0:
+            pub_msg.data = [0.0]
+        else:
+            print("self.wheelbase: " + str(self.wheelbase))
+            print("msg.angular.z: " + str(msg.angular.z))
+            print("msg.linear.x: " + str(msg.linear.x))
+            print("self.wheelbase * msg.angular.z / msg.linear.x: " + str(self.wheelbase * msg.angular.z / msg.linear.x))
+            pub_msg.data = [-math.asin(max(min(self.wheelbase * msg.angular.z / msg.linear.x, 1.0), -1.0))]
+        self.publisher.publish(pub_msg)
+
+def main():
+    rclpy.init()
+    node = CasterControllerSim()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

From 15cbb7be01aacb1eeea1df3c311c7bf93611559e Mon Sep 17 00:00:00 2001
From: yasuohayashibara <yasuo.hayashibara@it-chiba.ac.jp>
Date: Sun, 8 Sep 2024 07:23:09 +0900
Subject: [PATCH 3/5] relative path

---
 simulator/simulator/models/ai_car_active_caster/model.sdf | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/simulator/simulator/models/ai_car_active_caster/model.sdf b/simulator/simulator/models/ai_car_active_caster/model.sdf
index 059ac5d..995470b 100644
--- a/simulator/simulator/models/ai_car_active_caster/model.sdf
+++ b/simulator/simulator/models/ai_car_active_caster/model.sdf
@@ -434,7 +434,7 @@
     </plugin>
 
     <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
-      <parameters>/home/haya/ros2_ws/src/AIFormula_private/simulator/simulator/models/ai_car_active_caster/controller_position.yaml</parameters>
+      <parameters>../models/ai_car_active_caster/controller_position.yaml</parameters>
     </plugin>
 
     <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">

From f3e04c5c92d2a144782c688f98e0d975db3d1bd2 Mon Sep 17 00:00:00 2001
From: yasuohayashibara <yasuo.hayashibara@it-chiba.ac.jp>
Date: Sun, 8 Sep 2024 18:15:30 +0900
Subject: [PATCH 4/5] add rosdep

---
 simulator/simulator/models/ai_car_active_caster/model.sdf | 2 +-
 simulator/simulator/package.xml                           | 5 +++++
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/simulator/simulator/models/ai_car_active_caster/model.sdf b/simulator/simulator/models/ai_car_active_caster/model.sdf
index 995470b..6ac6710 100644
--- a/simulator/simulator/models/ai_car_active_caster/model.sdf
+++ b/simulator/simulator/models/ai_car_active_caster/model.sdf
@@ -434,7 +434,7 @@
     </plugin>
 
     <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
-      <parameters>../models/ai_car_active_caster/controller_position.yaml</parameters>
+      <parameters>/home/mirai/ros2_ws/src/aiformula/simulator/simulator/models/ai_car_active_caster/controller_position.yaml</parameters>
     </plugin>
 
     <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
diff --git a/simulator/simulator/package.xml b/simulator/simulator/package.xml
index a199534..7807c71 100644
--- a/simulator/simulator/package.xml
+++ b/simulator/simulator/package.xml
@@ -9,6 +9,11 @@
 
   <depend>common_python</depend>
   <depend>vehicle</depend>
+  <depend>gazebo_ros</depend>
+  <depend>xacro</depend>
+  <depend>gazebo_ros2_control</depend>
+  <depend>joint_state_publisher</depend>
+  <depend>gazebo_ros_pkgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

From 21778729c193fa66dc37a0c8ee218f567f331492 Mon Sep 17 00:00:00 2001
From: yasuohayashibara <yasuo.hayashibara@it-chiba.ac.jp>
Date: Sun, 8 Sep 2024 18:47:08 +0900
Subject: [PATCH 5/5] add rosdep

---
 simulator/simulator/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/simulator/simulator/package.xml b/simulator/simulator/package.xml
index 7807c71..cb2fedc 100644
--- a/simulator/simulator/package.xml
+++ b/simulator/simulator/package.xml
@@ -14,6 +14,7 @@
   <depend>gazebo_ros2_control</depend>
   <depend>joint_state_publisher</depend>
   <depend>gazebo_ros_pkgs</depend>
+  <depend>ros2_controllers</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>