Skip to content

Commit

Permalink
Absolem: Added sensor configs 6, 7 and 8.
Browse files Browse the repository at this point in the history
  • Loading branch information
peci1 committed Apr 4, 2021
1 parent 2572b4e commit 2598957
Show file tree
Hide file tree
Showing 43 changed files with 13,888 additions and 46 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@
<arg name="revision" default="2014" />
<arg name="has_cliff_sensors" default="0" />
<arg name="has_thermal_camera" default="0" />
<arg name="has_omnicam" default="1" />
<arg name="has_omnicam_vras" default="0" />

<include file="$(dirname)/description.launch" pass_all_args="true">
<arg name="print_command" value="$(arg description_print_command)" if="$(eval description_print_command != '')" />
Expand Down Expand Up @@ -111,41 +113,75 @@

<!-- Omnicamera -->

<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_0" />
<arg name="gazebo_topic" value="$(arg link_prefix)/base_link/sensor/omnicam_sensor0" />
<arg name="ros_topic" value="omni/camera_0" />
</include>
<group if="$(arg has_omnicam)">
<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_0" />
<arg name="gazebo_topic" value="$(arg link_prefix)/base_link/sensor/omnicam_sensor0" />
<arg name="ros_topic" value="omni/camera_0" />
</include>

<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_1" />
<arg name="gazebo_topic" value="$(arg link_prefix)/base_link/sensor/omnicam_sensor1" />
<arg name="ros_topic" value="omni/camera_1" />
</include>
<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_1" />
<arg name="gazebo_topic" value="$(arg link_prefix)/base_link/sensor/omnicam_sensor1" />
<arg name="ros_topic" value="omni/camera_1" />
</include>

<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_2" />
<arg name="gazebo_topic" value="$(arg link_prefix)/base_link/sensor/omnicam_sensor2" />
<arg name="ros_topic" value="omni/camera_2" />
</include>
<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_2" />
<arg name="gazebo_topic" value="$(arg link_prefix)/base_link/sensor/omnicam_sensor2" />
<arg name="ros_topic" value="omni/camera_2" />
</include>

<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_3" />
<arg name="gazebo_topic" value="$(arg link_prefix)/base_link/sensor/omnicam_sensor3" />
<arg name="ros_topic" value="omni/camera_3" />
</include>
<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_3" />
<arg name="gazebo_topic" value="$(arg link_prefix)/base_link/sensor/omnicam_sensor3" />
<arg name="ros_topic" value="omni/camera_3" />
</include>

<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_4" />
<arg name="gazebo_topic" value="$(arg link_prefix)/base_link/sensor/omnicam_sensor4" />
<arg name="ros_topic" value="omni/camera_4" />
</include>
<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_4" />
<arg name="gazebo_topic" value="$(arg link_prefix)/base_link/sensor/omnicam_sensor4" />
<arg name="ros_topic" value="omni/camera_4" />
</include>

<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_5" />
<arg name="gazebo_topic" value="$(arg link_prefix)/base_link/sensor/omnicam_sensor5" />
<arg name="ros_topic" value="omni/camera_5" />
</include>
<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_5" />
<arg name="gazebo_topic" value="$(arg link_prefix)/base_link/sensor/omnicam_sensor5" />
<arg name="ros_topic" value="omni/camera_5" />
</include>
</group>

<group if="$(arg has_omnicam_vras)">
<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_vras_front" />
<arg name="gazebo_topic" value="$(arg link_prefix)/camera_0/sensor/camera_0" />
<arg name="ros_topic" value="omni/camera_0" />
</include>

<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_vras_right" />
<arg name="gazebo_topic" value="$(arg link_prefix)/camera_1/sensor/camera_1" />
<arg name="ros_topic" value="omni/camera_1" />
</include>

<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_vras_rear" />
<arg name="gazebo_topic" value="$(arg link_prefix)/camera_2/sensor/camera_2" />
<arg name="ros_topic" value="omni/camera_2" />
</include>

<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_vras_left" />
<arg name="gazebo_topic" value="$(arg link_prefix)/camera_3/sensor/camera_3" />
<arg name="ros_topic" value="omni/camera_3" />
</include>

<include file="$(find subt_ros)/launch/models_common/rgb_camera.launch">
<arg name="node_name_suffix" value="omnicam_vras_up" />
<arg name="gazebo_topic" value="$(arg link_prefix)/camera_4/sensor/camera_4" />
<arg name="ros_topic" value="omni/camera_4" />
</include>
</group>

<!-- Thermal camera (if used) -->
<include if="$(arg has_thermal_camera)" file="$(find subt_ros)/launch/models_common/thermal_camera.launch">
Expand Down

Large diffs are not rendered by default.

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
@@ -0,0 +1,111 @@
<?xml version="1.0"?>
<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Mass properties from datasheet -->
<xacro:property name="basler_ace_mass" value="0.09" scope="global" />
<xacro:property name="basler_ace2_pro_mass" value="0.105" scope="global" />
<xacro:property name="fisheye_lens_mass" value="0.05" scope="global" />
<xacro:property name="omnicam_lens_mass" value="0.127" scope="global" />
<xacro:property name="c_mount_flange_distance" value="0.017526" scope="global" />
<xacro:property name="c_mount_radius" value="${0.025/2}" scope="global" />

<xacro:macro name="basler_ace_base" params="name hfov_deg frame_rate resolution_x resolution_y fisheye:=0 camera_length origin_to_camera_front_dist camera_mass camera_mesh lens_length lens_back_focal_distance lens_entrance_pupil_position lens_mass lens_mesh near_clip:=-1 simulate:=1 visualize:=0">
<xacro:property name="camera_width" value="0.029"/>
<xacro:property name="camera_height" value="0.029"/>

<xacro:property name="sensor_chip_pos" value="${origin_to_camera_front_dist - c_mount_flange_distance}" />
<xacro:property name="lens_back_pos" value="${sensor_chip_pos + lens_back_focal_distance}" />

<link name="$(arg prefix)${name}">
<visual>
<origin xyz="0 0 ${camera_height/2}" rpy="0 0 0" />
<geometry>
<!-- The origin of the mesh is center above tripod screw -->
<mesh filename="${camera_mesh}"/>
</geometry>
</visual>
<visual>
<!-- The origin of the mesh is at the back of the lens -->
<origin xyz="${lens_back_pos} 0 ${camera_height/2}" rpy="0 0 0" />
<geometry>
<mesh filename="${lens_mesh}"/>
</geometry>
</visual>
<collision name="${name}_collision">
<origin rpy="0 0 0" xyz="${origin_to_camera_front_dist - camera_length/2} 0 ${camera_height/2}"/>
<geometry>
<box size="${camera_length} ${camera_width} ${camera_height}" />
</geometry>
</collision>
<collision name="${name}_lens_collision">
<origin rpy="0 ${pi/2} 0" xyz="${lens_back_pos + lens_length/2} 0 ${camera_height/2}"/>
<geometry>
<cylinder length="${lens_length}" radius="${c_mount_radius}" />
</geometry>
</collision>
<xacro:box_inertial mass="${camera_mass + lens_mass}"
width="${camera_width}" height="${camera_height}" depth="${camera_length + lens_length - (c_mount_flange_distance - lens_back_focal_distance)}"
xyz="${origin_to_camera_front_dist + (lens_length - (c_mount_flange_distance - lens_back_focal_distance))/2 - camera_length/2} 0 ${camera_height/2}" />
</link>
<xacro:if value="${simulate}">
<gazebo reference="$(arg prefix)${name}">
<!-- Workaround for https://github.com/ignitionrobotics/ign-sensors/issues/24 -->
<xacro:if value="${fisheye == 0 or '$(arg rendering_target)' == 'ign'}">
<xacro:property name="cam_type" value="camera"/>
</xacro:if>
<xacro:unless value="${fisheye == 0 or '$(arg rendering_target)' == 'ign'}">
<xacro:property name="cam_type" value="wideanglecamera"/>
</xacro:unless>
<sensor name="${name}" type="${cam_type}">
<!-- I think the simulated sensor should be placed in the middle of the entrance pupil. -->
<xacro:property name="virtual_lens_origin" value="${lens_back_pos + lens_length - lens_entrance_pupil_position}" />
<pose>${virtual_lens_origin} 0 ${camera_height/2} 0 0 0</pose>
<update_rate>${frame_rate}</update_rate>
<visualize>${visualize}</visualize>
<camera>
<horizontal_fov>${radians(hfov_deg)}</horizontal_fov>
<image>
<width>${resolution_x}</width>
<height>${resolution_y}</height>
<format>R8G8B8</format>
</image>
<clip>
<xacro:property name="clip" value="${lens_entrance_pupil_position * 1.1 if near_clip == -1 else near_clip}" />
<near>${clip}</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
<xacro:if value="${fisheye and '$(arg rendering_target)' == 'gz'}">
<lens>
<type>stereographic</type>
<scale_to_hfov>true</scale_to_hfov>
<cutoff_angle>${pi}</cutoff_angle>
<env_texture_size>1024</env_texture_size>
</lens>
</xacro:if>
</camera>
</sensor>
</gazebo>
</xacro:if>
</xacro:macro>

<xacro:macro name="basler_ace2_pro_link" params="name hfov_deg fisheye:=0 lens_length lens_back_focal_distance lens_entrance_pupil_position lens_mass lens_mesh near_clip:=-1 simulate:=1 visualize:=0">
<xacro:basler_ace_base name="${name}" hfov_deg="${hfov_deg}" frame_rate="9" resolution_x="1920" resolution_y="1200"
fisheye="${fisheye}" simulate="${simulate}" visualize="${visualize}"
camera_length="0.0555" origin_to_camera_front_dist="0.0524" camera_mass="${basler_ace2_pro_mass}"
camera_mesh="package://ctu_cras_norlab_absolem_sensor_config_1/meshes/basler_ace2_pro.dae"
lens_length="${lens_length}" lens_back_focal_distance="${lens_back_focal_distance}"
lens_entrance_pupil_position="${lens_entrance_pupil_position}" lens_mass="${lens_mass}" lens_mesh="${lens_mesh}"
near_clip="${near_clip}"
/>
</xacro:macro>

<xacro:macro name="basler_ace_omnicam" params="name simulate:=1 visualize:=0">
<xacro:basler_ace2_pro_link name="${name}" hfov_deg="86.5" simulate="${simulate}" visualize="${visualize}"
lens_back_focal_distance="0.00809" lens_entrance_pupil_position="0.01066" lens_length="0.03974" lens_mass="${omnicam_lens_mass}"
lens_mesh="package://ctu_cras_norlab_absolem_sensor_config_1/meshes/evetar_lens.dae" />
</xacro:macro>
</root>
Loading

0 comments on commit 2598957

Please sign in to comment.