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..2f6e79cb --- /dev/null +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml @@ -0,0 +1,76 @@ +depth_camera: + camera1: + dim: + length: 0.05 + width: 0.05 + height: 0.05 + parent_link: chassis + offset: + x: 0.61 + y: 0 + 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: + length: 0.05 + width: 0.05 + height: 0.05 + parent_link: chassis + offset: + x: -0.61 + y: 0 + z: 0 + + r_rot: 0 + p_rot: 0 + y_rot: 3.14159265359 + camera_properties: + horizontal_fov: 1.047198 + image: + width: 640 + height: 480 + format: R8G8B8 + clip: + near: 0.05 + far: 3 + +lidar: + lidar1: + dim: + length: 0.05 + width: 0.05 + height: 0.05 + parent_link: chassis + offset: + x: 0.61 + y: 0 + z: 0.1 + + 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/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..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 @@ -2,10 +2,13 @@ + + + @@ -40,4 +43,17 @@ + + + + + + + + + + + + + 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..31da8895 --- /dev/null +++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/sensors.macro.xacro @@ -0,0 +1,113 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 30.0 + + ${camera_props['horizontal_fov']} + + ${camera_props['image']['width']} + ${camera_props['image']['height']} + ${camera_props['image']['format']} + + + ${camera_props['clip']['near']} + ${camera_props['clip']['far']} + + + + ${camera_link_name} + ${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 + true + 0.0 + camera_depth_frame + 0.5 + 3.0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + + + + true + true + ${lidar_props['update_rate']} + + + + ${lidar_props['scan']['samples']} + ${lidar_props['scan']['resolution']} + ${lidar_props['scan']['min_angle']} + ${lidar_props['scan']['max_angle']} + + + + ${lidar_props['range']['min']} + ${lidar_props['range']['max']} + ${lidar_props['range']['resolution']} + + + gaussian + 0.0 + 0.01 + + + + 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 + + + +