From b24165bee08b2975e2f68232b227425fa6e0e82b Mon Sep 17 00:00:00 2001 From: MeshvaD Date: Thu, 20 Jul 2023 21:49:19 -0400 Subject: [PATCH 1/3] add macros to simulate sensors --- .../CMakeLists.txt | 2 + .../config/sensor_parameters.yaml | 46 ++++ .../launch/gazebo.launch.py | 5 +- .../package.xml | 1 + .../urdf/drivetrain.urdf.xacro | 13 + .../urdf/sensors.macro.xacro | 116 +++++++++ .../world/my_world.sdf | 242 ++++++++++++++++++ 7 files changed, 424 insertions(+), 1 deletion(-) create mode 100644 uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml create mode 100644 uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/sensors.macro.xacro create mode 100644 uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/world/my_world.sdf diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/CMakeLists.txt b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/CMakeLists.txt index 95d8abea..bf5c1967 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/CMakeLists.txt +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(uwrt_mars_rover_drivetrain_description) find_package(ament_cmake REQUIRED) +find_package(sensor_msgs REQUIRED) install( DIRECTORY @@ -9,6 +10,7 @@ install( launch rviz urdf + world DESTINATION share/${PROJECT_NAME} ) diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml new file mode 100644 index 00000000..59cf52ea --- /dev/null +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml @@ -0,0 +1,46 @@ +depth_camera: + camera1: + dim: + length: 0.05 + width: 0.05 + height: 0.05 + parent_link: chassis + offset: + x: 0 + y: 0 + z: 0.1 + + r_rot: 0 + p_rot: 0 + y_rot: 0 + + camera2: + dim: + length: 0.05 + width: 0.05 + height: 0.05 + parent_link: chassis + offset: + x: 0.585 + y: 0 + z: 0.1 + + r_rot: 0 + p_rot: 0 + y_rot: 0 + +lidar: + lidar1: + dim: + length: 0.05 + width: 0.05 + height: 0.05 + parent_link: chassis + offset: + x: 0.4 + y: 0.3 + z: 0.1 + + r_rot: 0 + p_rot: 0 + y_rot: 0 diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo.launch.py b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo.launch.py index 7b736be3..a8e71f59 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo.launch.py +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo.launch.py @@ -6,6 +6,7 @@ from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare import xacro @@ -15,6 +16,8 @@ def generate_launch_description(): pkg_name = 'uwrt_mars_rover_drivetrain_description' file_subpath = 'urdf/drivetrain.urdf.xacro' + pkg_share = FindPackageShare(package=pkg_name).find(pkg_name) + world_path=os.path.join(pkg_share, 'world/my_world.sdf') # Use xacro to process the file xacro_file = os.path.join(get_package_share_directory(pkg_name),file_subpath) @@ -53,4 +56,4 @@ def generate_launch_description(): gazebo, node_robot_state_publisher, spawn_entity - ]) \ No newline at end of file + ]) diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/package.xml b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/package.xml index edcfff59..229c4cca 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/package.xml +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/package.xml @@ -20,6 +20,7 @@ ros2launch rviz2 xacro + sensor_msgs ament_cmake_flake8 ament_cmake_xmllint diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro index 9225e7f3..90cb0d7e 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro @@ -2,10 +2,13 @@ + + + @@ -40,4 +43,14 @@ + + + + + + + + + + diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/sensors.macro.xacro b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/sensors.macro.xacro new file mode 100644 index 00000000..d5c1e4f2 --- /dev/null +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/sensors.macro.xacro @@ -0,0 +1,116 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 30.0 + + 1.047198 + + 640 + 480 + R8G8B8 + + + 0.05 + 3 + + + + ${camera_link_name} + ir/image_raw + ir/camera_info + depth/image_raw + depth/camera_info + depth/points + ${camera_link_name}_optical_frame + + 0.2 + true + 0.0 + camera_depth_frame + 0.5 + 3.0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + + + + true + true + 5 + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 3.5 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + ~/out:=${lidar_link_name}_sensor/scan + + sensor_msgs/LaserScan + ${lidar_link_name} + + + + + + diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/world/my_world.sdf b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/world/my_world.sdf new file mode 100644 index 00000000..4348150e --- /dev/null +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/world/my_world.sdf @@ -0,0 +1,242 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1.51271 -0.181418 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 0 + + + + -1.89496 2.36764 0.5 0 -0 0 + + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 0 0 0 0 -0 0 + + + + + 0.5 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + + + + + + + 0 + 0 + 0 + + + + 0 0 + 0 0 + 1626668720 808592627 + 0 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 1.51272 -0.181418 0.499995 0 1e-05 0 + 1 1 1 + + 1.51272 -0.181418 0.499995 0 1e-05 0 + 0 0 0 0 -0 0 + 0.010615 -0.006191 -9.78231 0.012424 0.021225 1.8e-05 + 0.010615 -0.006191 -9.78231 0 -0 0 + + + + -0.725833 1.36206 0.5 0 -0 0 + 1 1 1 + + -0.944955 1.09802 0.5 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 3.17226 -5.10401 6.58845 0 0.739643 2.19219 + orbit + perspective + + + + From ad49bd02377b2413689c729a06860bb4d3a17a19 Mon Sep 17 00:00:00 2001 From: MeshvaD Date: Mon, 18 Sep 2023 22:37:01 -0400 Subject: [PATCH 2/3] add more params to yaml --- .../config/sensor_parameters.yaml | 42 ++++++++++++++--- .../urdf/drivetrain.urdf.xacro | 9 ++-- .../urdf/sensors.macro.xacro | 45 +++++++++---------- 3 files changed, 63 insertions(+), 33 deletions(-) diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml index 59cf52ea..5dc6746f 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml @@ -6,13 +6,22 @@ depth_camera: height: 0.05 parent_link: chassis offset: - x: 0 + x: 0.61 y: 0 - z: 0.1 + z: 0 r_rot: 0 p_rot: 0 y_rot: 0 + camera_properties: + horizontal_fov: 1.047198 + image: + width: 640 + height: 480 + format: R8G8B8 + clip: + near: 0.05 + far: 3 camera2: dim: @@ -21,13 +30,22 @@ depth_camera: height: 0.05 parent_link: chassis offset: - x: 0.585 + x: -0.61 y: 0 - z: 0.1 - + z: 0 + r_rot: 0 p_rot: 0 - y_rot: 0 + y_rot: 3.14159265359pi + camera_properties: + horizontal_fov: 1.047198 + image: + width: 640 + height: 480 + format: R8G8B8 + clip: + near: 0.05 + far: 3 lidar: lidar1: @@ -44,3 +62,15 @@ lidar: r_rot: 0 p_rot: 0 y_rot: 0 + + lidar_properties: + update_rate: 5 + scan: + samples: 360 + resolution: 1.000000 + min_angle: 0.000000 + max_angle: 6.280000 + range: + min: 0.120000 + max: 3.5 + resolution: 0.015000 diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro index 90cb0d7e..ade069e5 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro @@ -48,9 +48,12 @@ - + - - + + + + diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/sensors.macro.xacro b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/sensors.macro.xacro index d5c1e4f2..31da8895 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/sensors.macro.xacro +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/sensors.macro.xacro @@ -29,30 +29,30 @@ - + true 30.0 - 1.047198 + ${camera_props['horizontal_fov']} - 640 - 480 - R8G8B8 + ${camera_props['image']['width']} + ${camera_props['image']['height']} + ${camera_props['image']['format']} - 0.05 - 3 + ${camera_props['clip']['near']} + ${camera_props['clip']['far']} ${camera_link_name} - ir/image_raw - ir/camera_info - depth/image_raw - depth/camera_info - depth/points + ${camera_link_name}_ir/image_raw + ${camera_link_name}_ir/camera_info + ${camera_link_name}_depth/image_raw + ${camera_link_name}_depth/camera_info + ${camera_link_name}_depth/points ${camera_link_name}_optical_frame 0.2 @@ -76,25 +76,25 @@ - + true true - 5 + ${lidar_props['update_rate']} - 360 - 1.000000 - 0.000000 - 6.280000 + ${lidar_props['scan']['samples']} + ${lidar_props['scan']['resolution']} + ${lidar_props['scan']['min_angle']} + ${lidar_props['scan']['max_angle']} - 0.120000 - 3.5 - 0.015000 + ${lidar_props['range']['min']} + ${lidar_props['range']['max']} + ${lidar_props['range']['resolution']} gaussian @@ -103,9 +103,6 @@ - - ~/out:=${lidar_link_name}_sensor/scan - sensor_msgs/LaserScan ${lidar_link_name} From 915b5bdb495872589cfb82233edd4e96ab50f9fb Mon Sep 17 00:00:00 2001 From: MeshvaD Date: Mon, 18 Sep 2023 22:44:34 -0400 Subject: [PATCH 3/3] add lidar at front --- .../config/sensor_parameters.yaml | 6 +++--- .../urdf/drivetrain.urdf.xacro | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml index 5dc6746f..2f6e79cb 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml @@ -36,7 +36,7 @@ depth_camera: r_rot: 0 p_rot: 0 - y_rot: 3.14159265359pi + y_rot: 3.14159265359 camera_properties: horizontal_fov: 1.047198 image: @@ -55,8 +55,8 @@ lidar: height: 0.05 parent_link: chassis offset: - x: 0.4 - y: 0.3 + x: 0.61 + y: 0 z: 0.1 r_rot: 0 diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro index ade069e5..897b1cdd 100644 --- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro @@ -53,7 +53,7 @@ - + +