Skip to content

Commit

Permalink
PERCEPTION: Setting up sensors (#252)
Browse files Browse the repository at this point in the history 
* add macros to simulate sensors

* add more params to yaml

* add lidar at front
  • Loading branch information
@meshvaD
meshvaD authored Sep 29, 2023
1 parent 8016946 commit d917392
Show file tree
Hide file tree
Showing 7 changed files with 454 additions and 1 deletion.
2 changes: 2 additions & 0 deletions uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,15 @@ cmake_minimum_required(VERSION 3.5)
project(uwrt_mars_rover_drivetrain_description)

find_package(ament_cmake REQUIRED)
find_package(sensor_msgs REQUIRED)

install(
DIRECTORY
config
launch
rviz
urdf
world
DESTINATION
share/${PROJECT_NAME}
)
Expand Down
76 changes: 76 additions & 0 deletions ...ars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/config/sensor_parameters.yaml
View file
Original file line number Diff line number Diff line change
@@ -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
5 changes: 4 additions & 1 deletion uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@


from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import xacro


Expand All @@ -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)
Expand Down Expand Up @@ -53,4 +56,4 @@ def generate_launch_description():
gazebo,
node_robot_state_publisher,
spawn_entity
])
])
1 change: 1 addition & 0 deletions uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
<exec_depend>ros2launch</exec_depend>
<exec_depend>rviz2</exec_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>sensor_msgs</exec_depend>

<test_depend>ament_cmake_flake8</test_depend>
<test_depend>ament_cmake_xmllint</test_depend>
Expand Down
16 changes: 16 additions & 0 deletions uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,13 @@
<robot name="drivetrain" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- import macros file -->
<xacro:include filename="$(find uwrt_mars_rover_drivetrain_description)/urdf/drivetrain.macro.xacro"/>
<xacro:include filename="$(find uwrt_mars_rover_drivetrain_description)/urdf/sensors.macro.xacro"/>

<!-- load model data from yamls -->
<xacro:load_model_data
physical_parameters_file="$(find uwrt_mars_rover_drivetrain_description)/config/physical_parameters.yaml"/>

<xacro:property name="sensor_paramater_file" value="$(find uwrt_mars_rover_drivetrain_description)/config/sensor_parameters.yaml"/>

<!-- calculate parameters based on model data -->
<xacro:property name="wheelbase_length" value="${chassis_width + wheel_width}"/>
Expand Down Expand Up @@ -40,4 +43,17 @@
<xacro:wheel prefix="left" suffix="middle" parent_link="chassis" reflect="1" x_position="0"/>
<xacro:wheel prefix="left" suffix="back" parent_link="chassis" reflect="1" x_position="${-chassis_length/2}"/>


<!-- sensors -->
<xacro:property name="sensors_parameters" value="${load_yaml(sensor_paramater_file)}"/>

<xacro:generate_sensor_frame sensor_params="${sensors_parameters['depth_camera']['camera1']}" name="camera1"/>
<xacro:default_depth_camera_frame camera_link_name="camera1" camera_props="${sensors_parameters['depth_camera']['camera1']['camera_properties']}"/>

<xacro:generate_sensor_frame sensor_params="${sensors_parameters['depth_camera']['camera2']}" name="camera2"/>
<xacro:default_depth_camera_frame camera_link_name="camera2" camera_props="${sensors_parameters['depth_camera']['camera2']['camera_properties']}"/>

<xacro:generate_sensor_frame sensor_params="${sensors_parameters['lidar']['lidar1']}" name="lidar1"/>
<xacro:default_lidar_frame lidar_link_name="lidar1" lidar_props="${sensors_parameters['lidar']['lidar1']['lidar_properties']}"/>

</robot>
113 changes: 113 additions & 0 deletions uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/sensors.macro.xacro
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,113 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">

<!-- import macros file -->
<xacro:include filename="$(find uwrt_mars_rover_drivetrain_description)/urdf/drivetrain.macro.xacro"/>

<xacro:macro name="generate_sensor_frame" params="sensor_params name">
<link name="${name}">
<xacro:identical_visual_collision>
<identical_blocks>
<geometry>
<box size="${sensor_params['dim']['length']} ${sensor_params['dim']['width']} ${sensor_params['dim']['height']}"/>
</geometry>
<material name="blue">
<color rgba="0 0 0.8 1.0"/>
</material>
</identical_blocks>
</xacro:identical_visual_collision>
<xacro:default_inertial/>
</link>

<joint name="${sensor_params['parent_link']}_to_${name}" type="fixed">
<parent link="${sensor_params['parent_link']}"/>
<child link="${name}"/>
<origin xyz="${sensor_params['offset']['x']} ${sensor_params['offset']['y']} ${sensor_params['offset']['z']} "
rpy="${sensor_params['offset']['r_rot']} ${sensor_params['offset']['p_rot']} ${sensor_params['offset']['y_rot']} "/>
</joint>

</xacro:macro>


<xacro:macro name="default_depth_camera_frame" params="camera_link_name camera_props">
<gazebo reference="${camera_link_name}">
<sensor name="${camera_link_name}_sensor" type="depth">
<visualize>true</visualize>
<update_rate>30.0</update_rate>
<camera name="camera">
<horizontal_fov>${camera_props['horizontal_fov']}</horizontal_fov>
<image>
<width>${camera_props['image']['width']}</width>
<height>${camera_props['image']['height']}</height>
<format>${camera_props['image']['format']}</format>
</image>
<clip>
<near>${camera_props['clip']['near']}</near>
<far>${camera_props['clip']['far']}</far>
</clip>
</camera>
<plugin name="depth_camera_controller" filename="libgazebo_ros_camera.so">
<cameraName>${camera_link_name}</cameraName>
<imageTopicName>${camera_link_name}_ir/image_raw</imageTopicName>
<cameraInfoTopicName>${camera_link_name}_ir/camera_info</cameraInfoTopicName>
<depthImageTopicName>${camera_link_name}_depth/image_raw</depthImageTopicName>
<depthImageCameraInfoTopicName>${camera_link_name}_depth/camera_info</depthImageCameraInfoTopicName>
<pointCloudTopicName>${camera_link_name}_depth/points</pointCloudTopicName>
<frameName>${camera_link_name}_optical_frame</frameName>

<baseline>0.2</baseline>
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<frame_name>camera_depth_frame</frame_name>
<pointCloudCutoff>0.5</pointCloudCutoff>
<pointCloudCutoffMax>3.0</pointCloudCutoffMax>
<distortionK1>0</distortionK1>
<distortionK2>0</distortionK2>
<distortionK3>0</distortionK3>
<distortionT1>0</distortionT1>
<distortionT2>0</distortionT2>
<CxPrime>0</CxPrime>
<Cx>0</Cx>
<Cy>0</Cy>
<focalLength>0</focalLength>
<hackBaseline>0</hackBaseline>
</plugin>
</sensor>
</gazebo>
</xacro:macro>

<xacro:macro name="default_lidar_frame" params="lidar_link_name lidar_props">
<gazebo reference="${lidar_link_name}">
<sensor name="${lidar_link_name}_sensor" type="ray">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>${lidar_props['update_rate']}</update_rate>
<ray>
<scan>
<horizontal>
<samples>${lidar_props['scan']['samples']}</samples>
<resolution>${lidar_props['scan']['resolution']}</resolution>
<min_angle>${lidar_props['scan']['min_angle']}</min_angle>
<max_angle>${lidar_props['scan']['max_angle']}</max_angle>
</horizontal>
</scan>
<range>
<min>${lidar_props['range']['min']}</min>
<max>${lidar_props['range']['max']}</max>
<resolution>${lidar_props['range']['resolution']}</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="lidar_controller" filename="libgazebo_ros_ray_sensor.so">
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>${lidar_link_name}</frame_name>
</plugin>
</sensor>
</gazebo>
</xacro:macro>

</robot>
Loading

0 comments on commit d917392

Please sign in to comment.