visualizer & simulation node added

purdue-arc · Apr 20, 2024 · 29c6035 · 29c6035
1 parent 1824dec
commit 29c6035
Show file tree

Hide file tree

Showing 4 changed files with 143 additions and 5 deletions.
diff --git a/src/rktl_launch/launch/test.launch.py b/src/rktl_launch/launch/test.launch.py
@@ -118,6 +118,15 @@ def generate_launch_description():
 
     )
 
+    simulator_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory('rktl_sim'),
+                'launch',
+                'test_simulator.launch.py'
+            )
+        )
+    )
 
     return LaunchDescription([
         render_launch_arg,
@@ -128,7 +137,8 @@ def generate_launch_description():
         group_action1,
         ball_node,
         car_node,
-        rqt_gui_node
+        rqt_gui_node,
+        simulator_launch
     ])
 
 

diff --git a/src/rktl_sim/launch/test_simulator.launch.py b/src/rktl_sim/launch/test_simulator.launch.py
@@ -0,0 +1,95 @@
+import os
+import sys
+
+import launch
+import launch_ros.actions
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    pybullet_render_launch_arg = DeclareLaunchArgument(
+        name='pybullet_render',
+        default_value='false'
+    )
+
+    sim_mode_launch_arg = DeclareLaunchArgument(
+        name='sim_mode',
+        default_value='realistic'
+    )
+
+    simulation_config = os.path.join(
+        get_package_share_directory('rktl_sim'),
+                'config',
+                'simulation.yaml'
+    )
+
+    urdf_ball_param = os.path.join(
+        get_package_share_directory('rktl_sim'),
+                'urdf',
+                'ball.urdf'
+    )
+
+    urdf_car_param = os.path.join(
+        get_package_share_directory('rktl_sim'),
+                'urdf',
+                'car.urdf'
+    )
+
+    urdf_goal_param = os.path.join(
+        get_package_share_directory('rktl_sim'),
+                'urdf',
+                'goal.urdf'
+    )
+
+    urdf_sidewall_param = os.path.join(
+        get_package_share_directory('rktl_sim'),
+                'urdf',
+                'sidewall.urdf'
+    )
+
+    urdf_backwall_param = os.path.join(
+        get_package_share_directory('rktl_sim'),
+                'urdf',
+                'backwall.urdf'
+    )
+
+    urdf_plane_param = os.path.join(
+        get_package_share_directory('rktl_sim'),
+                'urdf',
+                'plane.urdf'
+    )
+
+    simulator_node = Node(
+        package='rktl_sim',
+        executable='simulator',
+        name='simulator',
+        parameters=[
+            simulation_config,
+            {
+                'mode' : LaunchConfiguration('sim_mode'),
+                'render' : LaunchConfiguration('pybullet_render'),
+                'urdf/ball' : urdf_ball_param,
+                'urdf/car' : urdf_car_param,
+                'urdf/goal' : urdf_goal_param,
+                'urdf/sidewall' : urdf_sidewall_param,
+                'urdf/backwall' : urdf_backwall_param,
+                'urdf/plane' : urdf_plane_param
+            }
+        ]
+
+    )
+
+
+    return LaunchDescription([
+        pybullet_render_launch_arg,
+        sim_mode_launch_arg,
+        simulator_node
+    ])
+
+if __name__ == '__main__':
+    generate_launch_description()
diff --git a/src/rktl_sim/launch/test_visualizer.launch.py b/src/rktl_sim/launch/test_visualizer.launch.py
@@ -25,14 +25,45 @@ def generate_launch_description():
                 'vizualization.yaml'
     )
 
+    media_ball_param = os.path.join(
+        get_package_share_directory('rktl_sim'),
+                'media',
+                'ball.png'
+    )
+
+    media_car_param = os.path.join(
+        get_package_share_directory('rktl_sim'),
+                'media',
+                'car.png'
+    )
+
+    media_goal_param = os.path.join(
+        get_package_share_directory('rktl_sim'),
+                'media',
+                'goal.png'
+    )
+
+    media_field_param = os.path.join(
+        get_package_share_directory('rktl_sim'),
+                'media',
+                'field.png'
+    )
+
     visualizer_node = Node(
         package='rktl_sim',
         executable='visualizer',
         namespace='test',
         name='visualizer',
         output='screen',
         parameters=[
-            global_config, visualizer_config
+            global_config, 
+            visualizer_config,
+            {
+                'media/ball' : media_ball_param,
+                'media/car' : media_car_param,
+                'media/goal' : media_goal_param,
+                'media/field' : media_field_param 
+            }
         ]
     )
 

diff --git a/src/rktl_sim/rktl_sim/vis_visualizer.py b/src/rktl_sim/rktl_sim/vis_visualizer.py
@@ -61,7 +61,9 @@ def __init__(self):
         # Setting up field
         id = 0
         # field_img_path = rospy.get_param("~media/field", None)
-        field_img_path = node.declare_parameter("~media/field", None).value
+        field_img_path = node.declare_parameter("media/field", None).get_parameter_value().string_value
+
+        print(f"field_img_path: {field_img_path}")
 
         if field_img_path is not None:
             self.window.createAsset(
@@ -106,7 +108,7 @@ def __init__(self):
         id += 1
 
         # Setting up car
-        car_img_path = node.declare_parameter("~media/car", None).value
+        car_img_path = node.declare_parameter("media/car", None).get_parameter_value().string_value
 
 
         if car_img_path is not None:
@@ -116,7 +118,7 @@ def __init__(self):
             id += 1
 
         # Setting up ball
-        ball_img_path = node.declare_parameter("~media/ball", None).value
+        ball_img_path = node.declare_parameter("media/ball", None).get_parameter_value().string_value
 
         if ball_img_path is not None:
             self.ball_id = id