Sim refactoring

rktl_autonomy/launch/rocket_league/rocket_league_train.launch

@@ -18,7 +18,7 @@
     </group>
 
     <!-- environment -->
-    <include file="$(find rktl_launch)/launch/rocket_league_sim.launch">
+    <include file="$(find rktl_launch)/launch/rocket_league.launch">
         <arg name="render"      value="$(arg render)"/>
         <arg name="sim_mode"    value="$(arg sim_mode)"/>
         <arg name="agent_type"  value="$(arg agent_type)"/>

rktl_launch/config/global_params.yaml

@@ -6,18 +6,19 @@ field:
   length: 4.25
   wall_thickness: 0.25
   goal:
-    width: 1
+    width: 1.0
 
 # car dimensions and physical constants
 ball:
   radius: 0.0508 # (2 in)
 
 # car dimensions and physical constants
 cars:
-  length: 0.12  # front to rear wheel center to center, meters
+  width: 0.0762 # (3 in)
+  length: 0.12 # (8 in)
   steering:
-    max_throw: 0.1826  # center to side, radians
-    rate: 0.9128  # rad/s
+    max_throw: 0.1826 # 15 degrees
+    rate: 0.9128 # 30 degrees/s
   throttle:
-    max_speed: 2.3  # m/s
-    tau: 0.2  # time constant, seconds
+    max_speed: 2.3 # m/s
+    tau: 0.2 # (approx 1 sec to reach max speed)

rktl_launch/launch/car.launch

@@ -0,0 +1,29 @@
+<launch>
+    <arg name="car_id" default="0"/>
+    <arg name="agent_type" default="planner"/> <!-- none, planner or autonomy -->
+    <arg name="autonomy_weights" default="model"/>
+
+    <arg name="sim_mode" default="realistic"/> <!-- none, realistic or ideal -->
+    <arg name="perception_delay" default="0.15"/>
+
+    <include unless="$(eval sim_mode == 'ideal')" 
+        file="$(find rktl_control)/launch/agent_control.launch">
+        <arg name="car_name" value="car$(arg car_id)"/>
+    </include>
+
+    <node if="$(eval sim_mode == 'realistic')" ns="cars/car$(arg car_id)"
+        pkg="rktl_control" type="topic_delay" name="pose_delay"
+        args="pose_sync_early pose_sync
+        geometry_msgs/PoseWithCovarianceStamped $(arg perception_delay)"/>
+
+    <include if="$(eval agent_type == 'planner')" 
+        file="$(find rktl_planner)/launch/simple_agent.launch">
+        <arg name="agent_name" value="agent$(arg car_id)"/>
+        <arg name="car_name" value="car$(arg car_id)"/>
+    </include>
+
+    <include if="$(eval agent_type == 'autonomy')" 
+        file="$(find rktl_autonomy)/launch/rocket_league/rocket_league_agent.launch">
+        <arg name="weights_name" value="$(arg autonomy_weights)"/>
+    </include>
+</launch>

rktl_launch/launch/rocket_league.launch

@@ -1,37 +1,54 @@
 <launch>
     <arg name="render"  default="true"/>
 
-    <arg name="agent_type"          default="patrol"/> <!-- Either planner, autonomy, or patrol-->
+    <arg name="sim_mode"            default="realistic"/> <!-- none, realistic, or ideal -->
+    <arg name="perception_delay"    default="0.15"/>
+
+    <arg name="agent_type"          default="planner"/> <!-- none, planner or autonomy -->
     <arg name="autonomy_weights"    default="model"/>
 
     <rosparam command="load" file="$(find rktl_launch)/config/global_params.yaml"/>
 
     <!-- Visualizer -->
     <include if="$(arg render)" file="$(find rktl_sim)/launch/visualizer.launch"/>
 
+    <!-- Simulator -->
+    <include unless="$(eval sim_mode == 'none')" file="$(find rktl_sim)/launch/simulator.launch">
+        <arg name="sim_mode" value="$(arg sim_mode)"/>
+    </include>
+
     <!-- Game Manager -->
     <include file="$(find rktl_game)/launch/game.launch"/>
 
     <!-- Control GUI -->
     <node pkg="rqt_gui" type="rqt_gui" name="rqt_gui" args="--perspective-file $(find rktl_launch)/rqt/rktl.perspective"/>
 
     <!-- Ball -->
-    <include file="$(find rktl_control)/launch/ball.launch"/>
+    <group if="$(eval sim_mode == 'realistic')">
+        <node ns="ball" pkg="rktl_control" type="topic_delay" name="pose_delay"
+            args="pose_sync_early pose_sync
+            geometry_msgs/PoseWithCovarianceStamped $(arg perception_delay)"/>
+        <include file="$(find rktl_control)/launch/ball_filter.launch"/>
+    </group>
 
     <!-- Cars -->
-    <include file="$(find rktl_control)/launch/car.launch">
-        <arg name="car_name" value="car0"/>
-    </include>
-    <include file="$(find rktl_control)/launch/hardware_interface.launch"/>
+    <!-- TODO: Dynamic configuration of cars on launch-->
+    <include if="$(eval sim_mode == 'none')"
+        file="$(find rktl_control)/launch/hardware_interface.launch"/>
 
-    <include if="$(eval agent_type == 'planner')" file="$(find rktl_planner)/launch/simple_agent.launch">
-        <arg name="agent_name" value="agent0"/>
-        <arg name="car_name" value="car0"/>
-    </include>
-    <include if="$(eval agent_type == 'autonomy')" file="$(find rktl_autonomy)/launch/rocket_league/rocket_league_agent.launch">
-        <arg name="weights_name" value="$(arg autonomy_weights)"/>
-    </include>
-    <include if="$(eval agent_type == 'patrol')" file="$(find rktl_planner)/launch/patrol_agent.launch">
-        <arg name="car_name" value="car0"/>
+    <include file="$(find rktl_launch)/launch/car.launch">
+        <arg name="car_id" value="0"/>
+        <arg name="agent_type" value="$(arg agent_type)"/>
+        <arg name="autonomy_weights" value="$(arg autonomy_weights)"/>
+        <arg name="sim_mode" value="$(arg sim_mode)"/>
+        <arg name="perception_delay" value="$(arg perception_delay)"/>
     </include>
+
+    <!-- <include file="$(find rktl_launch)/launch/car.launch">
+        <arg name="car_id" value="1"/>
+        <arg name="agent_type" value="none"/>
+        <arg name="autonomy_weights" value="$(arg autonomy_weights)"/>
+        <arg name="sim_mode" value="$(arg sim_mode)"/>
+        <arg name="perception_delay" value="$(arg perception_delay)"/>
+    </include> -->
 </launch>

rktl_sim/CMakeLists.txt

@@ -2,11 +2,27 @@ cmake_minimum_required(VERSION 3.0.2)
 project(rktl_sim)
 
 # find catkin so we can use our macros
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  geometry_msgs
+  message_generation)
 
 # install python module(s)
 catkin_python_setup()
 
+# services
+add_service_files(
+  FILES
+  CreateCar.srv
+  DeleteCar.srv
+)
+
+# messages
+generate_messages(
+  DEPENDENCIES
+  geometry_msgs
+)
+
 # generate variables for installation
 catkin_package()
 

rktl_sim/config/simulation.yaml

@@ -1,20 +1,82 @@
+#
+# Simulation settings
+#
+
 rate: 10
 spawn_height: 0.06
 
-car:
-  # init_pose:
-  #   pos: [0.5, 0.0, 0.06]
-  sensor_noise:
+# Directly export to setPhysicsEngineParameters()
+engine:
+  fixedTimeStep: 0.001
+  restitutionVelocityThreshold: 0.0001
+
+# Directly exported to changeDynamics()
+dynamics:
+  ball:
+    mass: 0.2
+    lateralFriction: 0.4
+    restitution: 0.7
+    rollingFriction: 0.0001
+    spinningFriction: 0.001
+    contactDamping: 0.0
+    contactStiffness: 0.0
+    collisionMargin: 0.0
+  car:
+    mass: 0.0
+    lateralFriction: 0.0
+    restitution: 0.0
+    rollingFriction: 0.0
+    spinningFriction: 0.0
+    contactDamping: 0.0
+    contactStiffness: 0.0
+    collisionMargin: 0.0
+  walls:
+    mass: 0.0
+    lateralFriction: 0.0
+    restitution: 0.0
+    rollingFriction: 0.0
+    spinningFriction: 0.0
+    contactDamping: 0.0
+    contactStiffness: 0.0
+    collisionMargin: 0.0
+  floor:
+    mass: 0.0
+    lateralFriction: 0.0
+    restitution: 0.0
+    rollingFriction: 0.0
+    spinningFriction: 0.0
+    contactDamping: 0.0
+    contactStiffness: 0.0
+    collisionMargin: 0.0
+
+#
+# Object instances
+#
+
+sensor_noise:
+  car:
     pos: [0.01, 0.01, 0.00]
     orient: [0.0, 0.0, 0.09]
-    dropout: 0.15
+    dropout: 0.0
+  ball:
+    pos: [0.01, 0.01, 0.0]
+    orient: [0.0, 0.0, 0.0]
+    dropout: 0.0
+
+cars:
+  - name: "car0"
+    # init_pose:
+    #   pos: [0.5, 0.0, 0.06]
+
+  # - name: "car1"
+  #   # init_pose:
+  #   #   pos: [0.5, 0.0, 0.06]
+  #   sensor_noise:
+  #     pos: [0.01, 0.01, 0.00]
+  #     orient: [0.0, 0.0, 0.09]
 
 ball:
   init_speed: 0.0
   # init_pose:
   #   pos: [0.0, 0.5, 0.06]
   #   orient: [0.0, 0.0, 0.0]
-  sensor_noise:
-    pos: [0.01, 0.01, 0.0]
-    orient: [0.0, 0.0, 0.0]
-    dropout: 0.1

rktl_sim/launch/adapter.launch

@@ -0,0 +1,20 @@
+<launch>
+    <rosparam command="load" file="$(find rktl_launch)/config/global_params.yaml"/>
+    <node pkg="rktl_sim" type="adapter" name="adapter" output="screen" required="true">
+        <param name="bag_file" value="$(find rktl_sim)/bag/2022-04-17-21-38-02.bag"/>
+        <param name="mode" value="realistic"/>
+        <param name="urdf/ball"     value="$(find rktl_sim)/urdf/ball.urdf"/>
+        <param name="urdf/car"      value="$(find rktl_sim)/urdf/car.urdf"/>
+        <param name="urdf/goal_a"   value="$(find rktl_sim)/urdf/goal_a.urdf"/>
+        <param name="urdf/goal_b"   value="$(find rktl_sim)/urdf/goal_b.urdf"/>
+        <param name="urdf/walls"    value="$(find rktl_sim)/urdf/walls.urdf"/>
+        <param name="urdf/plane"    value="$(find rktl_sim)/urdf/plane.urdf"/>
+    </node>
+    <node pkg="rktl_sim" type="visualizer" name="visualizer" output="screen">
+        <rosparam command="load" file="$(find rktl_sim)/config/visualization.yaml"/>
+        <param name="media/ball"    value="$(find rktl_sim)/media/ball.png"/>
+        <param name="media/car"     value="$(find rktl_sim)/media/car.png"/>
+        <param name="media/goal"    value="$(find rktl_sim)/media/goal.png"/>
+        <param name="media/field"   value="$(find rktl_sim)/media/field.jpg"/>
+    </node>
+</launch>

rktl_sim/launch/simulator.launch

@@ -2,15 +2,17 @@
     <arg name="pybullet_render" default="false"/>
     <arg name="sim_mode" default="realistic"/> <!--Either realistic or ideal (check docs) -->
 
+    <node pkg="rktl_sim" type="build_xacro.sh" name="build_xacro" output="screen" />
+
     <node pkg="rktl_sim" type="simulator" name="simulator" output="screen" required="true">
         <rosparam command="load" file="$(find rktl_sim)/config/simulation.yaml"/>
         <param name="mode"          value="$(arg sim_mode)"/>
         <param name="render"        value="$(arg pybullet_render)"/>
         <param name="urdf/ball"     value="$(find rktl_sim)/urdf/ball.urdf"/>
         <param name="urdf/car"      value="$(find rktl_sim)/urdf/car.urdf"/>
-        <param name="urdf/goal"     value="$(find rktl_sim)/urdf/goal.urdf"/>
-        <param name="urdf/sidewall" value="$(find rktl_sim)/urdf/sidewall.urdf"/>
-        <param name="urdf/backwall" value="$(find rktl_sim)/urdf/backwall.urdf"/>
+        <param name="urdf/goal_a"   value="$(find rktl_sim)/urdf/goal_a.urdf"/>
+        <param name="urdf/goal_b"   value="$(find rktl_sim)/urdf/goal_b.urdf"/>
+        <param name="urdf/walls"    value="$(find rktl_sim)/urdf/walls.urdf"/>
         <param name="urdf/plane"    value="$(find rktl_sim)/urdf/plane.urdf"/>
     </node>
 </launch>