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..6a94b01
--- /dev/null
+++ b/simulator/simulator/launch/gazebo_simulator_active_caster.launch.py
@@ -0,0 +1,97 @@
+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'
+ )
+
+ 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,
+ controller_manager,
+ caster_controller_sim
+ ])
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 @@
+
+
+
+ ai_car_active_caster
+ 2.0
+ model.sdf
+
+
+ Masaya Okada
+ masaya_okada@jp.honda
+
+
+
+ ai_car_active_caster
+
+
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..6ac6710
--- /dev/null
+++ b/simulator/simulator/models/ai_car_active_caster/model.sdf
@@ -0,0 +1,450 @@
+
+
+
+ 0 0 0 0 0 0
+
+
+
+
+ base_footprint
+ base_link
+ 0 0 0.15 0 0 0
+
+
+
+
+ 0 0 0 0 0 3.14
+
+ 1.73333
+ 0
+ 0
+ 4.56667
+ 0
+ 4.56667
+
+ 30.0
+
+
+
+ 0 0 0 0 0 3.14
+
+
+ model://ai_car1/meshes/AIF_body.dae
+
+
+
+
+
+ 0 0 0 0 0 3.14
+
+
+ model://ai_car1/meshes/AIF_body.dae
+
+
+
+
+
+
+
+ base_link
+ wheel_left_link
+ 0 0.3 0.0 1.57 0 1.57
+
+ 1 0 0
+
+
+
+
+
+ 0 0.3 0.0 1.57 0 1.57
+
+ 0.04778
+ 0
+ 0
+ 0.04778
+ 0
+ 0.07875
+
+ 7.0
+
+
+ 0 0.3 0.0 1.57 0 1.57
+
+
+ model://ai_car1/meshes/tire.dae
+ 0.05 0.12 0.12
+
+
+
+
+
+ 0.7
+ 0.7
+ 0 0 0
+ 0.0
+ 0.0
+
+
+
+
+ 0
+ 0.2
+ 1e+5
+ 1
+ 0.01
+ 0.001
+
+
+
+
+
+ 0 0.3 0.0 1.57 0 1.57
+
+
+ model://ai_car1/meshes/tire.dae
+ 0.05 0.12 0.12
+
+
+
+
+
+
+
+
+ base_link
+ wheel_right_link
+ 0 -0.3 0.0 1.57 0 1.57
+
+ 1 0 0
+
+
+
+
+
+ 0 -0.3 0 1.57 0 1.57
+
+ 0.04778
+ 0
+ 0
+ 0.04778
+ 0
+ 0.07875
+
+ 7.0
+
+
+ 0 -0.3 0 1.57 0 1.57
+
+
+ model://ai_car1/meshes/tire.dae
+ 0.05 0.12 0.12
+
+
+
+
+
+ 0.7
+ 0.7
+ 0 0 0
+ 0.0
+ 0.0
+
+
+
+
+ 0
+ 0.2
+ 1e+5
+ 1
+ 0.01
+ 0.001
+
+
+
+
+
+ 0 -0.3 0 1.57 0 1.57
+
+
+ model://ai_car1/meshes/tire.dae
+ 0.05 0.12 0.12
+
+
+
+
+
+
+
+
+ base_link
+ caster_back_steering_link
+ -0.8 0 -0.02 1.57 0 1.57
+
+ 0 1 0
+
+ -1.57
+ 1.57
+
+
+
+
+
+
+ -0.8 0 -0.02 1.57 0 1.57
+ 0.8
+
+
+
+
+ caster_back_steering_link
+ caster_back_link
+ -0.8 0 -0.02 1.57 0 1.57
+
+ 1 0 0
+
+
+
+
+
+ -0.8 0 -0.02 1.57 0 1.57
+
+ 0.04778
+ 0
+ 0
+ 0.04778
+ 0
+ 0.07875
+
+ 10.0
+
+
+
+ -0.8 0 -0.02 1.57 0 1.57
+
+
+ model://ai_car1/meshes/tire.dae
+ 0.1 0.1 0.1
+
+
+
+
+
+ 0.7
+ 0.7
+ 0 0 0
+ 0.0
+ 0.0
+
+
+
+
+ 0
+ 0.2
+ 1e+5
+ 1
+ 0.01
+ 0.001
+
+
+
+
+
+
+ -0.8 0 -0.02 1.57 0 1.57
+
+
+ model://ai_car1/meshes/tire.dae
+ 0.1 0.1 0.1
+
+
+
+
+
+
+ base_link
+ camera_link
+ 0.35 0 1.5 0 0.087 0
+
+
+ 0 0 1
+
+
+
+
+ camera_link
+ camera_rgb_frame
+ 0.35 0 1.5 0 0.087 0
+
+
+ 0 0 1
+
+
+
+
+
+
+
+ 0.35 0 1.5 0 0.087 0
+
+
+ 0.001
+ 0.000
+ 0.000
+ 0.001
+ 0.000
+ 0.001
+
+ 0.035
+
+
+ 0.35 0 1.5 0 0.087 0
+
+
+ true
+ true
+ 30
+
+ 1.02974
+
+ 1920
+ 1080
+ R8G8B8
+
+
+ 0.01
+ 300
+
+
+ gaussian
+
+ 0.0
+ 0.007
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ base_link
+ imu_link
+ -0.032 0 0.068 0 0 0
+
+ 0 0 1
+
+
+
+
+
+ true
+ 200
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+
+
+
+ ~/out:=imu
+
+
+
+
+
+
+
+
+
+
+
+ 30
+
+
+ wheel_left_joint
+ wheel_right_joint
+
+
+ 0.60
+ 0.30
+
+
+ 9.6
+ 1.0
+
+ cmd_vel
+
+
+ true
+ true
+ false
+
+ odom
+ odom
+ base_footprint
+
+
+
+
+ /home/mirai/ros2_ws/src/aiformula/simulator/simulator/models/ai_car_active_caster/controller_position.yaml
+
+
+
+
+
+
+ 30
+ wheel_left_joint
+ wheel_right_joint
+
+
+
+
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 @@
+
+
+
+ ai_car_caster_trail
+ 2.0
+ model.sdf
+
+
+ Masaya Okada
+ masaya_okada@jp.honda
+
+
+
+ ai_car_caster_trail
+
+
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 @@
+
+
+
+ 0 0 0 0 0 0
+
+
+
+
+ base_footprint
+ base_link
+ 0 0 0.15 0 0 0
+
+
+
+
+ 0 0 0 0 0 3.14
+
+ 1.73333
+ 0
+ 0
+ 4.56667
+ 0
+ 4.56667
+
+ 30.0
+
+
+
+ 0 0 0 0 0 3.14
+
+
+ model://ai_car1/meshes/AIF_body.dae
+
+
+
+
+
+ 0 0 0 0 0 3.14
+
+
+ model://ai_car1/meshes/AIF_body.dae
+
+
+
+
+
+
+
+ base_link
+ wheel_left_link
+ 0 0.3 0.0 1.57 0 1.57
+
+ 1 0 0
+
+
+
+
+
+ 0 0.3 0.0 1.57 0 1.57
+
+ 0.04778
+ 0
+ 0
+ 0.04778
+ 0
+ 0.07875
+
+ 7.0
+
+
+ 0 0.3 0.0 1.57 0 1.57
+
+
+ model://ai_car1/meshes/tire.dae
+ 0.05 0.12 0.12
+
+
+
+
+
+ 0.7
+ 0.7
+ 0 0 0
+ 0.0
+ 0.0
+
+
+
+
+ 0
+ 0.2
+ 1e+5
+ 1
+ 0.01
+ 0.001
+
+
+
+
+
+ 0 0.3 0.0 1.57 0 1.57
+
+
+ model://ai_car1/meshes/tire.dae
+ 0.05 0.12 0.12
+
+
+
+
+
+
+
+
+ base_link
+ wheel_right_link
+ 0 -0.3 0.0 1.57 0 1.57
+
+ 1 0 0
+
+
+
+
+
+ 0 -0.3 0 1.57 0 1.57
+
+ 0.04778
+ 0
+ 0
+ 0.04778
+ 0
+ 0.07875
+
+ 7.0
+
+
+ 0 -0.3 0 1.57 0 1.57
+
+
+ model://ai_car1/meshes/tire.dae
+ 0.05 0.12 0.12
+
+
+
+
+
+ 0.7
+ 0.7
+ 0 0 0
+ 0.0
+ 0.0
+
+
+
+
+ 0
+ 0.2
+ 1e+5
+ 1
+ 0.01
+ 0.001
+
+
+
+
+
+ 0 -0.3 0 1.57 0 1.57
+
+
+ model://ai_car1/meshes/tire.dae
+ 0.05 0.12 0.12
+
+
+
+
+
+
+
+
+ base_link
+ wheel_caster_steering_link
+ -0.8 0 -0.02 1.57 0 1.57
+
+ 0 1 0
+
+ -1.57
+ 1.57
+
+
+
+
+
+
+ -0.8 0 -0.02 1.57 0 1.57
+ 1.0
+
+
+
+
+ wheel_caster_steering_link
+ wheel_caster_link
+ -1.0 0 -0.02 1.57 0 1.57
+
+ 1 0 0
+
+
+
+
+
+ -1.0 0 -0.02 1.57 0 1.57
+
+ 0.04778
+ 0
+ 0
+ 0.04778
+ 0
+ 0.07875
+
+ 10.0
+
+
+
+ -1.0 0 -0.02 1.57 0 1.57
+
+
+ model://ai_car1/meshes/tire.dae
+ 0.1 0.1 0.1
+
+
+
+
+
+ 0.7
+ 0.7
+ 0 0 0
+ 0.0
+ 0.0
+
+
+
+
+ 0
+ 0.2
+ 1e+5
+ 1
+ 0.01
+ 0.001
+
+
+
+
+
+
+ -1.0 0 -0.02 1.57 0 1.57
+
+
+ model://ai_car1/meshes/tire.dae
+ 0.1 0.1 0.1
+
+
+
+
+
+
+ base_link
+ camera_link
+ 0.35 0 1.5 0 0.087 0
+
+
+ 0 0 1
+
+
+
+
+ camera_link
+ camera_rgb_frame
+ 0.35 0 1.5 0 0.087 0
+
+
+ 0 0 1
+
+
+
+
+
+
+
+ 0.35 0 1.5 0 0.087 0
+
+
+ 0.001
+ 0.000
+ 0.000
+ 0.001
+ 0.000
+ 0.001
+
+ 0.035
+
+
+ 0.35 0 1.5 0 0.087 0
+
+
+ true
+ true
+ 30
+
+ 1.02974
+
+ 1920
+ 1080
+ R8G8B8
+
+
+ 0.01
+ 300
+
+
+ gaussian
+
+ 0.0
+ 0.007
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ base_link
+ imu_link
+ -0.032 0 0.068 0 0 0
+
+ 0 0 1
+
+
+
+
+
+ true
+ 200
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+ 0.0
+ 2e-4
+
+
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+ 0.0
+ 1.7e-2
+
+
+
+
+
+
+
+ ~/out:=imu
+
+
+
+
+
+
+
+
+
+
+
+ 30
+
+
+ wheel_left_joint
+ wheel_right_joint
+
+
+ 0.60
+ 0.30
+
+
+ 9.6
+ 1.0
+
+ cmd_vel
+
+
+ true
+ true
+ false
+
+ odom
+ odom
+ base_footprint
+
+
+
+
+
+
+
+ 30
+ wheel_left_joint
+ wheel_right_joint
+
+
+
+
diff --git a/simulator/simulator/package.xml b/simulator/simulator/package.xml
index a199534..cb2fedc 100644
--- a/simulator/simulator/package.xml
+++ b/simulator/simulator/package.xml
@@ -9,6 +9,12 @@
common_python
vehicle
+ gazebo_ros
+ xacro
+ gazebo_ros2_control
+ joint_state_publisher
+ gazebo_ros_pkgs
+ ros2_controllers
ament_copyright
ament_flake8
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()
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 @@
+
+
+
+
+
+ true
+ 0 0 -0.001 0 0 0
+
+
+
+
+ 0 0 1
+ 200 200
+
+
+
+
+
+ 1
+ 1
+
+
+
+
+
+ false
+
+
+ 0 0 1
+ 200 200
+
+
+
+
+
+
+
+
+
+
+ model://sun
+
+
+
+ false
+
+ 12
+
+
+
+
+
+ 0.319654 -0.235002 9.29441 0 1.5138 0.009599
+ orbit
+ perspective
+
+
+
+
+ 1000.0
+ 0.001
+ 1
+
+
+ quick
+ 150
+ 0
+ 1.400000
+ 1
+
+
+ 0.00001
+ 0.2
+ 2000.000000
+ 0.01000
+
+
+
+
+
+ 9.0
+ 0 1.800 0
+ -19.15 1.8 0
+ -20.762 1.73 0
+ -22.362 1.519 0
+ -23.938 1.17 0
+ -25.477 0.684 0
+ -26.968 0.067 0
+ -28.4 -0.679 0
+ -29.761 -1.546 0
+ -31.042 -2.528 0
+ -32.231 -3.619 0
+ -33.322 -4.808 0
+ -34.304 -6.089 0
+ -35.171 -7.45 0
+ -35.917 -8.882 0
+ -36.534 -10.373 0
+ -37.02 -11.912 0
+ -37.369 -13.488 0
+ -37.58 -15.088 0
+ -37.65 -16.7 0
+ -37.65 -37.7 0
+ -37.58 -39.312 0
+ -37.369 -40.912 0
+ -37.02 -42.488 0
+ -36.534 -44.027 0
+ -35.917 -45.518 0
+ -35.171 -46.95 0
+ -34.304 -48.311 0
+ -33.322 -49.592 0
+ -32.231 -50.781 0
+ -31.042 -51.872 0
+ -29.761 -52.854 0
+ -28.4 -53.721 0
+ -26.968 -54.467 0
+ -25.477 -55.084 0
+ 20.173 -73.584 0
+ 21.712 -74.07 0
+ 23.288 -74.419 0
+ 24.888 -74.63 0
+ 26.5 -74.7 0
+ 28.112 -74.63 0
+ 29.712 -74.419 0
+ 31.288 -74.07 0
+ 32.827 -73.584 0
+ 34.318 -72.967 0
+ 35.75 -72.221 0
+ 37.111 -71.354 0
+ 38.392 -70.372 0
+ 39.581 -69.281 0
+ 40.672 -68.092 0
+ 41.654 -66.811 0
+ 42.521 -65.45 0
+ 43.267 -64.018 0
+ 43.884 -62.527 0
+ 44.37 -60.988 0
+ 44.719 -59.412 0
+ 44.93 -57.812 0
+ 45 -56.2 0
+ 45 -16.7 0
+ 44.93 -15.088 0
+ 44.719 -13.488 0
+ 44.37 -11.912 0
+ 43.884 -10.373 0
+ 43.267 -8.882 0
+ 42.521 -7.45 0
+ 41.654 -6.089 0
+ 40.672 -4.808 0
+ 39.581 -3.619 0
+ 38.392 -2.528 0
+ 37.111 -1.546 0
+ 35.75 -0.679 0
+ 34.318 0.067 0
+ 32.827 0.684 0
+ 31.288 1.17 0
+ 29.712 1.519 0
+ 28.112 1.73 0
+ 26.5 1.8 0
+ 0 1.8 0
+
+
+
+
+
+
+
+
+ 0.2
+ 0 1.800 0.001
+ -19.15 1.8 0.001
+ -20.762 1.73 0.001
+ -22.362 1.519 0.001
+ -23.938 1.17 0.001
+ -25.477 0.684 0.001
+ -26.968 0.067 0.001
+ -28.4 -0.679 0.001
+ -29.761 -1.546 0.001
+ -31.042 -2.528 0.001
+ -32.231 -3.619 0.001
+ -33.322 -4.808 0.001
+ -34.304 -6.089 0.001
+ -35.171 -7.45 0.001
+ -35.917 -8.882 0.001
+ -36.534 -10.373 0.001
+ -37.02 -11.912 0.001
+ -37.369 -13.488 0.001
+ -37.58 -15.088 0.001
+ -37.65 -16.7 0.001
+ -37.65 -37.7 0.001
+ -37.58 -39.312 0.001
+ -37.369 -40.912 0.001
+ -37.02 -42.488 0.001
+ -36.534 -44.027 0.001
+ -35.917 -45.518 0.001
+ -35.171 -46.95 0.001
+ -34.304 -48.311 0.001
+ -33.322 -49.592 0.001
+ -32.231 -50.781 0.001
+ -31.042 -51.872 0.001
+ -29.761 -52.854 0.001
+ -28.4 -53.721 0.001
+ -26.968 -54.467 0.001
+ -25.477 -55.084 0.001
+ 20.173 -73.584 0.001
+ 21.712 -74.07 0.001
+ 23.288 -74.419 0.001
+ 24.888 -74.63 0.001
+ 26.5 -74.7 0.001
+ 28.112 -74.63 0.001
+ 29.712 -74.419 0.001
+ 31.288 -74.07 0.001
+ 32.827 -73.584 0.001
+ 34.318 -72.967 0.001
+ 35.75 -72.221 0.001
+ 37.111 -71.354 0.001
+ 38.392 -70.372 0.001
+ 39.581 -69.281 0.001
+ 40.672 -68.092 0.001
+ 41.654 -66.811 0.001
+ 42.521 -65.45 0.001
+ 43.267 -64.018 0.001
+ 43.884 -62.527 0.001
+ 44.37 -60.988 0.001
+ 44.719 -59.412 0.001
+ 44.93 -57.812 0.001
+ 45 -56.2 0.001
+ 45 -16.7 0.001
+ 44.93 -15.088 0.001
+ 44.719 -13.488 0.001
+ 44.37 -11.912 0.001
+ 43.884 -10.373 0.001
+ 43.267 -8.882 0.001
+ 42.521 -7.45 0.001
+ 41.654 -6.089 0.001
+ 40.672 -4.808 0.001
+ 39.581 -3.619 0.001
+ 38.392 -2.528 0.001
+ 37.111 -1.546 0.001
+ 35.75 -0.679 0.001
+ 34.318 0.067 0.001
+ 32.827 0.684 0.001
+ 31.288 1.17 0.001
+ 29.712 1.519 0.001
+ 28.112 1.73 0.001
+ 26.5 1.8 0.001
+ 0 1.8 0.001
+
+
+
+
+
+
+ 0.2
+ 0 -1.5 0.001
+ -19.15 -1.5 0.001
+ -20.475 -1.558 0.001
+ -21.789 -1.731 0.001
+ -23.084 -2.018 0.001
+ -24.349 -2.417 0.001
+ -25.574 -2.924 0.001
+ -26.75 -3.536 0.001
+ -27.868 -4.249 0.001
+ -28.92 -5.056 0.001
+ -29.898 -5.952 0.001
+ -30.794 -6.93 0.001
+ -31.601 -7.982 0.001
+ -32.314 -9.1 0.001
+ -32.926 -10.276 0.001
+ -33.433 -11.501 0.001
+ -33.832 -12.766 0.001
+ -34.119 -14.061 0.001
+ -34.292 -15.375 0.001
+ -34.35 -16.7 0.001
+ -34.35 -37.7 0.001
+ -34.292 -39.025 0.001
+ -34.119 -40.339 0.001
+ -33.832 -41.634 0.001
+ -33.433 -42.899 0.001
+ -32.926 -44.124 0.001
+ -32.314 -45.3 0.001
+ -31.601 -46.418 0.001
+ -30.794 -47.47 0.001
+ -29.898 -48.448 0.001
+ -28.92 -49.344 0.001
+ -27.868 -50.151 0.001
+ -26.75 -50.864 0.001
+ -25.574 -51.476 0.001
+ -24.349 -51.983 0.001
+ 21.301 -70.483 0.001
+ 22.566 -70.882 0.001
+ 23.861 -71.169 0.001
+ 25.175 -71.342 0.001
+ 26.5 -71.4 0.001
+ 27.825 -71.342 0.001
+ 29.139 -71.169 0.001
+ 30.434 -70.882 0.001
+ 31.699 -70.483 0.001
+ 32.924 -69.976 0.001
+ 34.1 -69.364 0.001
+ 35.218 -68.651 0.001
+ 36.27 -67.844 0.001
+ 37.248 -66.948 0.001
+ 38.144 -65.97 0.001
+ 38.951 -64.918 0.001
+ 39.664 -63.8 0.001
+ 40.276 -62.624 0.001
+ 40.783 -61.399 0.001
+ 41.182 -60.134 0.001
+ 41.469 -58.839 0.001
+ 41.642 -57.525 0.001
+ 41.7 -56.2 0.001
+ 41.7 -16.7 0.001
+ 41.642 -15.375 0.001
+ 41.469 -14.061 0.001
+ 41.182 -12.766 0.001
+ 40.783 -11.501 0.001
+ 40.276 -10.276 0.001
+ 39.664 -9.1 0.001
+ 38.951 -7.982 0.001
+ 38.144 -6.93 0.001
+ 37.248 -5.952 0.001
+ 36.27 -5.056 0.001
+ 35.218 -4.249 0.001
+ 34.1 -3.536 0.001
+ 32.924 -2.924 0.001
+ 31.699 -2.417 0.001
+ 30.434 -2.018 0.001
+ 29.139 -1.731 0.001
+ 27.825 -1.558 0.001
+ 26.5 -1.5 0.001
+ 0 -1.5 0.001
+
+
+
+
+
+
+ 0.2
+ 0 5.1 0.001
+ -19.15 5.1 0.001
+ -21.05 5.017 0.001
+ -22.936 4.769 0.001
+ -24.792 4.357 0.001
+ -26.606 3.785 0.001
+ -28.363 3.058 0.001
+ -30.05 2.179 0.001
+ -31.654 1.158 0.001
+ -33.163 0 0.001
+ -34.565 -1.285 0.001
+ -35.85 -2.687 0.001
+ -37.008 -4.196 0.001
+ -38.029 -5.8 0.001
+ -38.908 -7.487 0.001
+ -39.635 -9.244 0.001
+ -40.207 -11.058 0.001
+ -40.619 -12.914 0.001
+ -40.867 -14.8 0.001
+ -40.95 -16.7 0.001
+ -40.95 -37.7 0.001
+ -40.867 -39.6 0.001
+ -40.619 -41.486 0.001
+ -40.207 -43.342 0.001
+ -39.635 -45.156 0.001
+ -38.908 -46.913 0.001
+ -38.029 -48.6 0.001
+ -37.008 -50.204 0.001
+ -35.85 -51.713 0.001
+ -34.565 -53.115 0.001
+ -33.163 -54.4 0.001
+ -31.654 -55.558 0.001
+ -30.05 -56.579 0.001
+ -28.363 -57.458 0.001
+ -26.606 -58.185 0.001
+ 19.044 -76.685 0.001
+ 20.858 -77.257 0.001
+ 22.714 -77.669 0.001
+ 24.6 -77.917 0.001
+ 26.5 -78 0.001
+ 28.4 -77.917 0.001
+ 30.286 -77.669 0.001
+ 32.142 -77.257 0.001
+ 33.956 -76.685 0.001
+ 35.713 -75.958 0.001
+ 37.4 -75.079 0.001
+ 39.004 -74.058 0.001
+ 40.513 -72.9 0.001
+ 41.915 -71.615 0.001
+ 43.2 -70.213 0.001
+ 44.358 -68.704 0.001
+ 45.379 -67.1 0.001
+ 46.258 -65.413 0.001
+ 46.985 -63.656 0.001
+ 47.557 -61.842 0.001
+ 47.969 -59.986 0.001
+ 48.217 -58.1 0.001
+ 48.3 -56.2 0.001
+ 48.3 -16.7 0.001
+ 48.217 -14.8 0.001
+ 47.969 -12.914 0.001
+ 47.557 -11.058 0.001
+ 46.985 -9.244 0.001
+ 46.258 -7.487 0.001
+ 45.379 -5.8 0.001
+ 44.358 -4.196 0.001
+ 43.2 -2.687 0.001
+ 41.915 -1.285 0.001
+ 40.513 0 0.001
+ 39.004 1.158 0.001
+ 37.4 2.179 0.001
+ 35.713 3.058 0.001
+ 33.956 3.785 0.001
+ 32.142 4.357 0.001
+ 30.286 4.769 0.001
+ 28.4 5.017 0.001
+ 26.5 5.1 0.001
+ 0 5.1 0.001
+
+
+
+
+
+
+ 5.0
+ 0 8.800 0
+ -19.15 8.8 0.001
+ -21.372 8.703 0.001
+ -23.578 8.413 0.001
+ -25.75 7.931 0.001
+ -27.872 7.262 0.001
+ -29.927 6.411 0.001
+ -31.9 5.384 0.001
+ -33.776 4.188 0.001
+ -35.541 2.834 0.001
+ -37.181 1.331 0.001
+ -38.684 -0.309 0.001
+ -40.038 -2.074 0.001
+ -41.234 -3.95 0.001
+ -42.261 -5.923 0.001
+ -43.112 -7.978 0.001
+ -43.781 -10.1 0.001
+ -44.263 -12.272 0.001
+ -44.553 -14.478 0.001
+ -44.65 -16.7 0.001
+ -44.65 -37.7 0.001
+ -44.553 -39.922 0.001
+ -44.263 -42.128 0.001
+ -43.781 -44.3 0.001
+ -43.112 -46.422 0.001
+ -42.261 -48.477 0.001
+ -41.234 -50.45 0.001
+ -40.038 -52.326 0.001
+ -38.684 -54.091 0.001
+ -37.181 -55.731 0.001
+ -35.541 -57.234 0.001
+ -33.776 -58.588 0.001
+ -31.9 -59.784 0.001
+ -29.927 -60.811 0.001
+ -27.872 -61.662 0.001
+ 17.778 -80.162 0.001
+ 19.9 -80.831 0.001
+ 22.072 -81.313 0.001
+ 24.278 -81.603 0.001
+ 26.5 -81.7 0.001
+ 28.722 -81.603 0.001
+ 30.928 -81.313 0.001
+ 33.1 -80.831 0.001
+ 35.222 -80.162 0.001
+ 37.277 -79.311 0.001
+ 39.25 -78.284 0.001
+ 41.126 -77.088 0.001
+ 42.891 -75.734 0.001
+ 44.531 -74.231 0.001
+ 46.034 -72.591 0.001
+ 47.388 -70.826 0.001
+ 48.584 -68.95 0.001
+ 49.611 -66.977 0.001
+ 50.462 -64.922 0.001
+ 51.131 -62.8 0.001
+ 51.613 -60.628 0.001
+ 51.903 -58.422 0.001
+ 52 -56.2 0.001
+ 52 -16.7 0.001
+ 51.903 -14.478 0.001
+ 51.613 -12.272 0.001
+ 51.131 -10.1 0.001
+ 50.462 -7.978 0.001
+ 49.611 -5.923 0.001
+ 48.584 -3.95 0.001
+ 47.388 -2.074 0.001
+ 46.034 -0.309 0.001
+ 44.531 1.331 0.001
+ 42.891 2.834 0.001
+ 41.126 4.188 0.001
+ 39.25 5.384 0.001
+ 37.277 6.411 0.001
+ 35.222 7.262 0.001
+ 33.1 7.931 0.001
+ 30.928 8.413 0.001
+ 28.722 8.703 0.001
+ 26.5 8.8 0.001
+ 0 8.8 0
+
+
+
+
+
+
+ 5.0
+ 0 -5.2 0
+ -19.15 -5.2 0.001
+ -20.152 -5.244 0.001
+ -21.147 -5.375 0.001
+ -22.126 -5.592 0.001
+ -23.083 -5.894 0.001
+ -24.01 -6.277 0.001
+ -24.9 -6.741 0.001
+ -25.746 -7.28 0.001
+ -26.542 -7.89 0.001
+ -27.282 -8.568 0.001
+ -27.96 -9.308 0.001
+ -28.57 -10.104 0.001
+ -29.109 -10.95 0.001
+ -29.573 -11.84 0.001
+ -29.956 -12.767 0.001
+ -30.258 -13.724 0.001
+ -30.475 -14.703 0.001
+ -30.606 -15.698 0.001
+ -30.65 -16.7 0.001
+ -30.65 -37.7 0.001
+ -30.606 -38.702 0.001
+ -30.475 -39.697 0.001
+ -30.258 -40.676 0.001
+ -29.956 -41.633 0.001
+ -29.573 -42.56 0.001
+ -29.109 -43.45 0.001
+ -28.57 -44.296 0.001
+ -27.96 -45.092 0.001
+ -27.282 -45.832 0.001
+ -26.542 -46.51 0.001
+ -25.746 -47.12 0.001
+ -24.9 -47.659 0.001
+ -24.01 -48.123 0.001
+ -23.083 -48.506 0.001
+ 22.567 -67.006 0.001
+ 23.524 -67.308 0.001
+ 24.503 -67.525 0.001
+ 25.498 -67.656 0.001
+ 26.5 -67.7 0.001
+ 27.502 -67.656 0.001
+ 28.497 -67.525 0.001
+ 29.476 -67.308 0.001
+ 30.433 -67.006 0.001
+ 31.36 -66.623 0.001
+ 32.25 -66.159 0.001
+ 33.096 -65.62 0.001
+ 33.892 -65.01 0.001
+ 34.632 -64.332 0.001
+ 35.31 -63.592 0.001
+ 35.92 -62.796 0.001
+ 36.459 -61.95 0.001
+ 36.923 -61.06 0.001
+ 37.306 -60.133 0.001
+ 37.608 -59.176 0.001
+ 37.825 -58.197 0.001
+ 37.956 -57.202 0.001
+ 38 -56.2 0.001
+ 38 -16.7 0.001
+ 37.956 -15.698 0.001
+ 37.825 -14.703 0.001
+ 37.608 -13.724 0.001
+ 37.306 -12.767 0.001
+ 36.923 -11.84 0.001
+ 36.459 -10.95 0.001
+ 35.92 -10.104 0.001
+ 35.31 -9.308 0.001
+ 34.632 -8.568 0.001
+ 33.892 -7.89 0.001
+ 33.096 -7.28 0.001
+ 32.25 -6.741 0.001
+ 31.36 -6.277 0.001
+ 30.433 -5.894 0.001
+ 29.476 -5.592 0.001
+ 28.497 -5.375 0.001
+ 27.502 -5.244 0.001
+ 26.5 -5.2 0.001
+ 0 -5.2 0
+
+
+
+
+
+
+ 0.0 0.0 0.01 0.0 0.0
+ model://ai_car_active_caster
+
+
+
+
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 @@
+
+
+
+
+
+ true
+ 0 0 -0.001 0 0 0
+
+
+
+
+ 0 0 1
+ 200 200
+
+
+
+
+
+ 1
+ 1
+
+
+
+
+
+ false
+
+
+ 0 0 1
+ 200 200
+
+
+
+
+
+
+
+
+
+
+ model://sun
+
+
+
+ false
+
+ 12
+
+
+
+
+
+ 0.319654 -0.235002 9.29441 0 1.5138 0.009599
+ orbit
+ perspective
+
+
+
+
+ 1000.0
+ 0.001
+ 1
+
+
+ quick
+ 150
+ 0
+ 1.400000
+ 1
+
+
+ 0.00001
+ 0.2
+ 2000.000000
+ 0.01000
+
+
+
+
+
+ 9.0
+ 0 1.800 0
+ -19.15 1.8 0
+ -20.762 1.73 0
+ -22.362 1.519 0
+ -23.938 1.17 0
+ -25.477 0.684 0
+ -26.968 0.067 0
+ -28.4 -0.679 0
+ -29.761 -1.546 0
+ -31.042 -2.528 0
+ -32.231 -3.619 0
+ -33.322 -4.808 0
+ -34.304 -6.089 0
+ -35.171 -7.45 0
+ -35.917 -8.882 0
+ -36.534 -10.373 0
+ -37.02 -11.912 0
+ -37.369 -13.488 0
+ -37.58 -15.088 0
+ -37.65 -16.7 0
+ -37.65 -37.7 0
+ -37.58 -39.312 0
+ -37.369 -40.912 0
+ -37.02 -42.488 0
+ -36.534 -44.027 0
+ -35.917 -45.518 0
+ -35.171 -46.95 0
+ -34.304 -48.311 0
+ -33.322 -49.592 0
+ -32.231 -50.781 0
+ -31.042 -51.872 0
+ -29.761 -52.854 0
+ -28.4 -53.721 0
+ -26.968 -54.467 0
+ -25.477 -55.084 0
+ 20.173 -73.584 0
+ 21.712 -74.07 0
+ 23.288 -74.419 0
+ 24.888 -74.63 0
+ 26.5 -74.7 0
+ 28.112 -74.63 0
+ 29.712 -74.419 0
+ 31.288 -74.07 0
+ 32.827 -73.584 0
+ 34.318 -72.967 0
+ 35.75 -72.221 0
+ 37.111 -71.354 0
+ 38.392 -70.372 0
+ 39.581 -69.281 0
+ 40.672 -68.092 0
+ 41.654 -66.811 0
+ 42.521 -65.45 0
+ 43.267 -64.018 0
+ 43.884 -62.527 0
+ 44.37 -60.988 0
+ 44.719 -59.412 0
+ 44.93 -57.812 0
+ 45 -56.2 0
+ 45 -16.7 0
+ 44.93 -15.088 0
+ 44.719 -13.488 0
+ 44.37 -11.912 0
+ 43.884 -10.373 0
+ 43.267 -8.882 0
+ 42.521 -7.45 0
+ 41.654 -6.089 0
+ 40.672 -4.808 0
+ 39.581 -3.619 0
+ 38.392 -2.528 0
+ 37.111 -1.546 0
+ 35.75 -0.679 0
+ 34.318 0.067 0
+ 32.827 0.684 0
+ 31.288 1.17 0
+ 29.712 1.519 0
+ 28.112 1.73 0
+ 26.5 1.8 0
+ 0 1.8 0
+
+
+
+
+
+
+
+
+ 0.2
+ 0 1.800 0.001
+ -19.15 1.8 0.001
+ -20.762 1.73 0.001
+ -22.362 1.519 0.001
+ -23.938 1.17 0.001
+ -25.477 0.684 0.001
+ -26.968 0.067 0.001
+ -28.4 -0.679 0.001
+ -29.761 -1.546 0.001
+ -31.042 -2.528 0.001
+ -32.231 -3.619 0.001
+ -33.322 -4.808 0.001
+ -34.304 -6.089 0.001
+ -35.171 -7.45 0.001
+ -35.917 -8.882 0.001
+ -36.534 -10.373 0.001
+ -37.02 -11.912 0.001
+ -37.369 -13.488 0.001
+ -37.58 -15.088 0.001
+ -37.65 -16.7 0.001
+ -37.65 -37.7 0.001
+ -37.58 -39.312 0.001
+ -37.369 -40.912 0.001
+ -37.02 -42.488 0.001
+ -36.534 -44.027 0.001
+ -35.917 -45.518 0.001
+ -35.171 -46.95 0.001
+ -34.304 -48.311 0.001
+ -33.322 -49.592 0.001
+ -32.231 -50.781 0.001
+ -31.042 -51.872 0.001
+ -29.761 -52.854 0.001
+ -28.4 -53.721 0.001
+ -26.968 -54.467 0.001
+ -25.477 -55.084 0.001
+ 20.173 -73.584 0.001
+ 21.712 -74.07 0.001
+ 23.288 -74.419 0.001
+ 24.888 -74.63 0.001
+ 26.5 -74.7 0.001
+ 28.112 -74.63 0.001
+ 29.712 -74.419 0.001
+ 31.288 -74.07 0.001
+ 32.827 -73.584 0.001
+ 34.318 -72.967 0.001
+ 35.75 -72.221 0.001
+ 37.111 -71.354 0.001
+ 38.392 -70.372 0.001
+ 39.581 -69.281 0.001
+ 40.672 -68.092 0.001
+ 41.654 -66.811 0.001
+ 42.521 -65.45 0.001
+ 43.267 -64.018 0.001
+ 43.884 -62.527 0.001
+ 44.37 -60.988 0.001
+ 44.719 -59.412 0.001
+ 44.93 -57.812 0.001
+ 45 -56.2 0.001
+ 45 -16.7 0.001
+ 44.93 -15.088 0.001
+ 44.719 -13.488 0.001
+ 44.37 -11.912 0.001
+ 43.884 -10.373 0.001
+ 43.267 -8.882 0.001
+ 42.521 -7.45 0.001
+ 41.654 -6.089 0.001
+ 40.672 -4.808 0.001
+ 39.581 -3.619 0.001
+ 38.392 -2.528 0.001
+ 37.111 -1.546 0.001
+ 35.75 -0.679 0.001
+ 34.318 0.067 0.001
+ 32.827 0.684 0.001
+ 31.288 1.17 0.001
+ 29.712 1.519 0.001
+ 28.112 1.73 0.001
+ 26.5 1.8 0.001
+ 0 1.8 0.001
+
+
+
+
+
+
+ 0.2
+ 0 -1.5 0.001
+ -19.15 -1.5 0.001
+ -20.475 -1.558 0.001
+ -21.789 -1.731 0.001
+ -23.084 -2.018 0.001
+ -24.349 -2.417 0.001
+ -25.574 -2.924 0.001
+ -26.75 -3.536 0.001
+ -27.868 -4.249 0.001
+ -28.92 -5.056 0.001
+ -29.898 -5.952 0.001
+ -30.794 -6.93 0.001
+ -31.601 -7.982 0.001
+ -32.314 -9.1 0.001
+ -32.926 -10.276 0.001
+ -33.433 -11.501 0.001
+ -33.832 -12.766 0.001
+ -34.119 -14.061 0.001
+ -34.292 -15.375 0.001
+ -34.35 -16.7 0.001
+ -34.35 -37.7 0.001
+ -34.292 -39.025 0.001
+ -34.119 -40.339 0.001
+ -33.832 -41.634 0.001
+ -33.433 -42.899 0.001
+ -32.926 -44.124 0.001
+ -32.314 -45.3 0.001
+ -31.601 -46.418 0.001
+ -30.794 -47.47 0.001
+ -29.898 -48.448 0.001
+ -28.92 -49.344 0.001
+ -27.868 -50.151 0.001
+ -26.75 -50.864 0.001
+ -25.574 -51.476 0.001
+ -24.349 -51.983 0.001
+ 21.301 -70.483 0.001
+ 22.566 -70.882 0.001
+ 23.861 -71.169 0.001
+ 25.175 -71.342 0.001
+ 26.5 -71.4 0.001
+ 27.825 -71.342 0.001
+ 29.139 -71.169 0.001
+ 30.434 -70.882 0.001
+ 31.699 -70.483 0.001
+ 32.924 -69.976 0.001
+ 34.1 -69.364 0.001
+ 35.218 -68.651 0.001
+ 36.27 -67.844 0.001
+ 37.248 -66.948 0.001
+ 38.144 -65.97 0.001
+ 38.951 -64.918 0.001
+ 39.664 -63.8 0.001
+ 40.276 -62.624 0.001
+ 40.783 -61.399 0.001
+ 41.182 -60.134 0.001
+ 41.469 -58.839 0.001
+ 41.642 -57.525 0.001
+ 41.7 -56.2 0.001
+ 41.7 -16.7 0.001
+ 41.642 -15.375 0.001
+ 41.469 -14.061 0.001
+ 41.182 -12.766 0.001
+ 40.783 -11.501 0.001
+ 40.276 -10.276 0.001
+ 39.664 -9.1 0.001
+ 38.951 -7.982 0.001
+ 38.144 -6.93 0.001
+ 37.248 -5.952 0.001
+ 36.27 -5.056 0.001
+ 35.218 -4.249 0.001
+ 34.1 -3.536 0.001
+ 32.924 -2.924 0.001
+ 31.699 -2.417 0.001
+ 30.434 -2.018 0.001
+ 29.139 -1.731 0.001
+ 27.825 -1.558 0.001
+ 26.5 -1.5 0.001
+ 0 -1.5 0.001
+
+
+
+
+
+
+ 0.2
+ 0 5.1 0.001
+ -19.15 5.1 0.001
+ -21.05 5.017 0.001
+ -22.936 4.769 0.001
+ -24.792 4.357 0.001
+ -26.606 3.785 0.001
+ -28.363 3.058 0.001
+ -30.05 2.179 0.001
+ -31.654 1.158 0.001
+ -33.163 0 0.001
+ -34.565 -1.285 0.001
+ -35.85 -2.687 0.001
+ -37.008 -4.196 0.001
+ -38.029 -5.8 0.001
+ -38.908 -7.487 0.001
+ -39.635 -9.244 0.001
+ -40.207 -11.058 0.001
+ -40.619 -12.914 0.001
+ -40.867 -14.8 0.001
+ -40.95 -16.7 0.001
+ -40.95 -37.7 0.001
+ -40.867 -39.6 0.001
+ -40.619 -41.486 0.001
+ -40.207 -43.342 0.001
+ -39.635 -45.156 0.001
+ -38.908 -46.913 0.001
+ -38.029 -48.6 0.001
+ -37.008 -50.204 0.001
+ -35.85 -51.713 0.001
+ -34.565 -53.115 0.001
+ -33.163 -54.4 0.001
+ -31.654 -55.558 0.001
+ -30.05 -56.579 0.001
+ -28.363 -57.458 0.001
+ -26.606 -58.185 0.001
+ 19.044 -76.685 0.001
+ 20.858 -77.257 0.001
+ 22.714 -77.669 0.001
+ 24.6 -77.917 0.001
+ 26.5 -78 0.001
+ 28.4 -77.917 0.001
+ 30.286 -77.669 0.001
+ 32.142 -77.257 0.001
+ 33.956 -76.685 0.001
+ 35.713 -75.958 0.001
+ 37.4 -75.079 0.001
+ 39.004 -74.058 0.001
+ 40.513 -72.9 0.001
+ 41.915 -71.615 0.001
+ 43.2 -70.213 0.001
+ 44.358 -68.704 0.001
+ 45.379 -67.1 0.001
+ 46.258 -65.413 0.001
+ 46.985 -63.656 0.001
+ 47.557 -61.842 0.001
+ 47.969 -59.986 0.001
+ 48.217 -58.1 0.001
+ 48.3 -56.2 0.001
+ 48.3 -16.7 0.001
+ 48.217 -14.8 0.001
+ 47.969 -12.914 0.001
+ 47.557 -11.058 0.001
+ 46.985 -9.244 0.001
+ 46.258 -7.487 0.001
+ 45.379 -5.8 0.001
+ 44.358 -4.196 0.001
+ 43.2 -2.687 0.001
+ 41.915 -1.285 0.001
+ 40.513 0 0.001
+ 39.004 1.158 0.001
+ 37.4 2.179 0.001
+ 35.713 3.058 0.001
+ 33.956 3.785 0.001
+ 32.142 4.357 0.001
+ 30.286 4.769 0.001
+ 28.4 5.017 0.001
+ 26.5 5.1 0.001
+ 0 5.1 0.001
+
+
+
+
+
+
+ 5.0
+ 0 8.800 0
+ -19.15 8.8 0.001
+ -21.372 8.703 0.001
+ -23.578 8.413 0.001
+ -25.75 7.931 0.001
+ -27.872 7.262 0.001
+ -29.927 6.411 0.001
+ -31.9 5.384 0.001
+ -33.776 4.188 0.001
+ -35.541 2.834 0.001
+ -37.181 1.331 0.001
+ -38.684 -0.309 0.001
+ -40.038 -2.074 0.001
+ -41.234 -3.95 0.001
+ -42.261 -5.923 0.001
+ -43.112 -7.978 0.001
+ -43.781 -10.1 0.001
+ -44.263 -12.272 0.001
+ -44.553 -14.478 0.001
+ -44.65 -16.7 0.001
+ -44.65 -37.7 0.001
+ -44.553 -39.922 0.001
+ -44.263 -42.128 0.001
+ -43.781 -44.3 0.001
+ -43.112 -46.422 0.001
+ -42.261 -48.477 0.001
+ -41.234 -50.45 0.001
+ -40.038 -52.326 0.001
+ -38.684 -54.091 0.001
+ -37.181 -55.731 0.001
+ -35.541 -57.234 0.001
+ -33.776 -58.588 0.001
+ -31.9 -59.784 0.001
+ -29.927 -60.811 0.001
+ -27.872 -61.662 0.001
+ 17.778 -80.162 0.001
+ 19.9 -80.831 0.001
+ 22.072 -81.313 0.001
+ 24.278 -81.603 0.001
+ 26.5 -81.7 0.001
+ 28.722 -81.603 0.001
+ 30.928 -81.313 0.001
+ 33.1 -80.831 0.001
+ 35.222 -80.162 0.001
+ 37.277 -79.311 0.001
+ 39.25 -78.284 0.001
+ 41.126 -77.088 0.001
+ 42.891 -75.734 0.001
+ 44.531 -74.231 0.001
+ 46.034 -72.591 0.001
+ 47.388 -70.826 0.001
+ 48.584 -68.95 0.001
+ 49.611 -66.977 0.001
+ 50.462 -64.922 0.001
+ 51.131 -62.8 0.001
+ 51.613 -60.628 0.001
+ 51.903 -58.422 0.001
+ 52 -56.2 0.001
+ 52 -16.7 0.001
+ 51.903 -14.478 0.001
+ 51.613 -12.272 0.001
+ 51.131 -10.1 0.001
+ 50.462 -7.978 0.001
+ 49.611 -5.923 0.001
+ 48.584 -3.95 0.001
+ 47.388 -2.074 0.001
+ 46.034 -0.309 0.001
+ 44.531 1.331 0.001
+ 42.891 2.834 0.001
+ 41.126 4.188 0.001
+ 39.25 5.384 0.001
+ 37.277 6.411 0.001
+ 35.222 7.262 0.001
+ 33.1 7.931 0.001
+ 30.928 8.413 0.001
+ 28.722 8.703 0.001
+ 26.5 8.8 0.001
+ 0 8.8 0
+
+
+
+
+
+
+ 5.0
+ 0 -5.2 0
+ -19.15 -5.2 0.001
+ -20.152 -5.244 0.001
+ -21.147 -5.375 0.001
+ -22.126 -5.592 0.001
+ -23.083 -5.894 0.001
+ -24.01 -6.277 0.001
+ -24.9 -6.741 0.001
+ -25.746 -7.28 0.001
+ -26.542 -7.89 0.001
+ -27.282 -8.568 0.001
+ -27.96 -9.308 0.001
+ -28.57 -10.104 0.001
+ -29.109 -10.95 0.001
+ -29.573 -11.84 0.001
+ -29.956 -12.767 0.001
+ -30.258 -13.724 0.001
+ -30.475 -14.703 0.001
+ -30.606 -15.698 0.001
+ -30.65 -16.7 0.001
+ -30.65 -37.7 0.001
+ -30.606 -38.702 0.001
+ -30.475 -39.697 0.001
+ -30.258 -40.676 0.001
+ -29.956 -41.633 0.001
+ -29.573 -42.56 0.001
+ -29.109 -43.45 0.001
+ -28.57 -44.296 0.001
+ -27.96 -45.092 0.001
+ -27.282 -45.832 0.001
+ -26.542 -46.51 0.001
+ -25.746 -47.12 0.001
+ -24.9 -47.659 0.001
+ -24.01 -48.123 0.001
+ -23.083 -48.506 0.001
+ 22.567 -67.006 0.001
+ 23.524 -67.308 0.001
+ 24.503 -67.525 0.001
+ 25.498 -67.656 0.001
+ 26.5 -67.7 0.001
+ 27.502 -67.656 0.001
+ 28.497 -67.525 0.001
+ 29.476 -67.308 0.001
+ 30.433 -67.006 0.001
+ 31.36 -66.623 0.001
+ 32.25 -66.159 0.001
+ 33.096 -65.62 0.001
+ 33.892 -65.01 0.001
+ 34.632 -64.332 0.001
+ 35.31 -63.592 0.001
+ 35.92 -62.796 0.001
+ 36.459 -61.95 0.001
+ 36.923 -61.06 0.001
+ 37.306 -60.133 0.001
+ 37.608 -59.176 0.001
+ 37.825 -58.197 0.001
+ 37.956 -57.202 0.001
+ 38 -56.2 0.001
+ 38 -16.7 0.001
+ 37.956 -15.698 0.001
+ 37.825 -14.703 0.001
+ 37.608 -13.724 0.001
+ 37.306 -12.767 0.001
+ 36.923 -11.84 0.001
+ 36.459 -10.95 0.001
+ 35.92 -10.104 0.001
+ 35.31 -9.308 0.001
+ 34.632 -8.568 0.001
+ 33.892 -7.89 0.001
+ 33.096 -7.28 0.001
+ 32.25 -6.741 0.001
+ 31.36 -6.277 0.001
+ 30.433 -5.894 0.001
+ 29.476 -5.592 0.001
+ 28.497 -5.375 0.001
+ 27.502 -5.244 0.001
+ 26.5 -5.2 0.001
+ 0 -5.2 0
+
+
+
+
+
+
+ 0.0 0.0 0.01 0.0 0.0
+ model://ai_car_caster_trail
+
+
+
+
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ gazebo_ros2_control/GazeboSystem
+
+
+
+ -1.57
+ 1.57
+
+
+
+
+
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+