From 2c6dc8c8e794bd35239f2b0646a458ef0070a40c Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Sun, 20 Jun 2021 21:52:32 +0200
Subject: [PATCH] Absolem: fix bad merge.

---
 .../urdf/basler_cameras.xacro                 | 123 ++++++++++++++++++
 .../urdf/nifti_robot.xacro                    | 113 +++++++++++++---
 .../urdf/utils.xacro                          |  16 +++
 3 files changed, 235 insertions(+), 17 deletions(-)
 create mode 100644 submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/basler_cameras.xacro

diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/basler_cameras.xacro b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/basler_cameras.xacro
new file mode 100644
index 00000000..0dad1d67
--- /dev/null
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/basler_cameras.xacro
@@ -0,0 +1,123 @@
+<?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>
+                        <xacro:unless value="${fisheye and '$(arg rendering_target)' == 'gz'}">
+                            <lens>
+                                <intrinsics>
+                                    <xacro:property name="f" value="${resolution_x / (2 * tan(radians(hfov_deg)/2))}" />
+                                    <fx>${f}</fx>
+                                    <fy>${f}</fy>
+                                    <cx>${(resolution_x - 1) / 2}</cx>
+                                    <cy>${(resolution_y - 1) / 2}</cy>
+                                    <s>0</s>
+                                </intrinsics>
+                            </lens>
+                        </xacro:unless>
+                    </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>
\ No newline at end of file
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/nifti_robot.xacro b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/nifti_robot.xacro
index 30fccaec..5bd60a65 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/nifti_robot.xacro
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/nifti_robot.xacro
@@ -440,6 +440,9 @@
 	<xacro:if value="$(arg has_boson_thermocam)">
 		<xacro:include filename="$(find ctu_cras_norlab_absolem_sensor_config_1)/urdf/boson_thermocam.xacro" />
 	</xacro:if>
+	<xacro:if value="$(arg has_omnicam_vras)">
+		<xacro:include filename="$(find ctu_cras_norlab_absolem_sensor_config_1)/urdf/basler_cameras.xacro" />
+	</xacro:if>
 
 	<!-- NIFTI ROBOT LINKS AND JOINTS SPECIFICATION -->
 
@@ -718,6 +721,14 @@
 												filename="package://ctu_cras_norlab_absolem_sensor_config_1/meshes/${top_box_mesh_name}"/>
 							</geometry>
 					</visual>
+					<xacro:if value="${'$(arg revision)' == '2021' and $(arg has_omnicam_vras)}">
+						<visual>
+							<origin rpy="0 ${3*pi/2} 0" xyz="-0.075 0.0 0.12" />
+							<geometry>
+								<mesh filename="package://ctu_cras_norlab_absolem_sensor_config_1/meshes/reflector.dae" />
+							</geometry>
+						</visual>
+					</xacro:if>
 					<xacro:if value="${$(arg simplified_collision)==0}">
 							<collision>
 									<origin xyz="0 0 0" rpy="0 0 0" />
@@ -792,7 +803,37 @@
 				<!-- Origin of top box is 0.0635 m from the front side. And top_box_j_x is distance from robot's back to top box' front. -->
 				<xacro:fixed_joint name="top_box_j" parent="$(arg prefix)rear_right_box" child="$(arg prefix)top_box" xyz="${top_box_j_x-0.0635} ${$(arg rover_robotWidthBody)/2} 0.0" rpy="0 0 0"/>
 				<xacro:if value="$(arg has_omnicam_vras)">
-					<!-- -->
+					<link name="$(arg prefix)top_box_step">
+						<xacro:property name="top_box_step_width" value="0.12" />
+						<xacro:property name="top_box_step_height" value="0.1" />
+						<visual>
+							<origin rpy="0 0 0" xyz="0 0 ${top_box_step_height/2}"/>
+							<geometry>
+								<box size="0.1270 ${top_box_step_width} ${top_box_step_height}" />
+							</geometry>
+							<material name="body_color">
+								<color rgba="0 ${94/255.0} ${184/255.0} 1.0"/>
+							</material>
+						</visual>
+						<visual>
+							<origin rpy="0 $(arg ouster_rpy_p) 0" xyz="${$(arg ouster_pos_x) - 0.01} 0 ${$(arg ouster_pos_z) - 0.075}"/>
+							<geometry>
+								<cylinder radius="0.025" length="0.05" />
+							</geometry>
+							<material name="body_color">
+								<color rgba="0 ${94/255.0} ${184/255.0} 1.0"/>
+							</material>
+						</visual>
+						<collision>
+							<origin rpy="0 0 0" xyz="0 0 ${top_box_step_height/2}"/>
+							<geometry>
+								<box size="0.1270 ${top_box_step_width} ${top_box_step_height}" />
+							</geometry>
+						</collision>
+						<xacro:box_inertial mass="1.0" width="${top_box_step_width}" height="${top_box_step_height}" depth="0.1270" xyz="0 0 ${top_box_step_height/2}" />
+					</link>
+					<xacro:fixed_joint name="top_box_step_j" parent="$(arg prefix)top_box" child="$(arg prefix)top_box_step" xyz="0 0 0.06252" />
+					<xacro:solid_color_link_material link="$(arg prefix)top_box_step" r="0" g="${94/255.0}" b="${184/255.0}" />
 				</xacro:if>
 			</xacro:if>
 			<xacro:unless value="${'$(arg revision)' == '2021'}">
@@ -852,7 +893,12 @@
 			</inertial>
 	</link>
 	<xacro:if value="${'$(arg revision)' == '2021'}">
-		<xacro:fixed_joint name="antenna_j" parent="$(arg prefix)base_link" child="$(arg prefix)antenna" xyz="${-$(arg rover_robotLengthBody)/2 + 0.12} 0.0789 0.1551" rpy="0 0 0" />
+		<xacro:if value="$(arg has_omnicam)">
+			<xacro:fixed_joint name="antenna_j" parent="$(arg prefix)base_link" child="$(arg prefix)antenna" xyz="${-$(arg rover_robotLengthBody)/2 + 0.12} 0.0789 0.1551" rpy="0 0 0" />
+		</xacro:if>
+		<xacro:if value="$(arg has_omnicam_vras)">
+			<xacro:fixed_joint name="antenna_j" parent="$(arg prefix)base_link" child="$(arg prefix)antenna" xyz="${-$(arg rover_robotLengthBody)/2 + 0.12} 0.1 0.1551" rpy="0 0 0" />
+		</xacro:if>
 	</xacro:if>
 	<xacro:unless value="${'$(arg revision)' == '2021'}">
 		<xacro:fixed_joint name="antenna_j" parent="$(arg prefix)base_link" child="$(arg prefix)antenna" xyz="-0.3720 0.0789 0.1551" rpy="0 0 0" />
@@ -1294,22 +1340,49 @@
 
 	</xacro:if>
 
+	<!-- Omnicamera -->
+	<xacro:if value="$(arg has_omnicam_vras)">
+		<xacro:basler_ace_omnicam name="camera_0" simulate="$(arg simulate_omnicam_vras)" visualize="$(arg visualize_sensors)" />
+		<xacro:fixed_joint name="camera_0_j" parent="$(arg prefix)rear_right_box" child="$(arg prefix)camera_0"
+						   xyz="${$(arg rover_robotLengthBody) - 0.11} ${$(arg rover_robotWidthBody)/2} 0.01" lump="0" />
+		<link name="$(arg prefix)front_camera_holder"><visual><geometry><box size="0.04 0.025 0.01" /></geometry></visual></link>
+		<xacro:fixed_joint name="front_camera_holder_j" parent="$(arg prefix)camera_0" child="$(arg prefix)front_camera_holder" xyz="0.02 0 -0.005" />
+
+		<xacro:basler_ace_omnicam name="camera_1" simulate="$(arg simulate_omnicam_vras)" visualize="$(arg visualize_sensors)" />
+		<xacro:fixed_joint name="camera_1_j" parent="$(arg prefix)rear_right_box" child="$(arg prefix)camera_1"
+						   xyz="${$(arg rover_robotLengthBody)/2-0.02} 0.083 0" rpy="0 0 ${-pi/2}" lump="0" />
+
+		<xacro:basler_ace_omnicam name="camera_2" simulate="$(arg simulate_omnicam_vras)" visualize="$(arg visualize_sensors)" />
+		<xacro:fixed_joint name="camera_2_j" parent="$(arg prefix)rear_right_box" child="$(arg prefix)camera_2"
+						   xyz="0.05 ${$(arg rover_robotWidthBody)/2} 0" rpy="0 0 ${-pi}" lump="0" />
+
+		<xacro:basler_ace_omnicam name="camera_3" simulate="$(arg simulate_omnicam_vras)" visualize="$(arg visualize_sensors)" />
+		<xacro:fixed_joint name="camera_3_j" parent="$(arg prefix)rear_right_box" child="$(arg prefix)camera_3"
+						   xyz="${$(arg rover_robotLengthBody)/2-0.02} ${$(arg rover_robotWidthBody) - 0.083} 0" rpy="0 0 ${-3*pi/2}" lump="0" />
+
+		<xacro:basler_ace_omnicam name="camera_4" simulate="$(arg simulate_omnicam_vras)" visualize="$(arg visualize_sensors)" />
+		<xacro:fixed_joint name="camera_4_j" parent="$(arg prefix)top_box" child="$(arg prefix)camera_4"
+						   xyz="-0.048 -0.0145 ${0.06252 + top_box_step_height - 0.08}" rpy="${pi} ${-pi/2} ${pi/2}" lump="0" />
+	</xacro:if>
+
 	<!-- 3D lidar -->
 	<xacro:if value="$(arg has_ouster_lidar)">
 		<xacro:ouster_os0_128 name="laser" simulate="$(arg simulate_ouster_lidar)" visualize="$(arg visualize_sensors)" />
 		<xacro:calibrated_fixed_joint name="laser_j" parent="top_box" child="laser" prefix="ouster" lump="0"/>
-		<link name="$(arg prefix)ouster_holder">
-			<visual>
-				<origin xyz="0 0 0" />
-				<geometry>
-					<mesh filename="package://ctu_cras_norlab_absolem_sensor_config_1/meshes/ouster_holder_ladybug.dae" />
-				</geometry>
-			</visual>
-			<xacro:cylinder_inertial mass="0.2" radius="0.07" length="0.02"
-								xyz="0 0 -0.01" />
-		</link>
-		<xacro:fixed_joint name="ouster_holder_j" parent="$(arg prefix)omnicam_top" child="$(arg prefix)ouster_holder"
-			xyz="0 0 0.005" rpy="0 0 ${radians(36)}" />
+		<xacro:if value="$(arg has_omnicam)">
+			<link name="$(arg prefix)ouster_holder">
+				<visual>
+					<origin xyz="0 0 0" />
+					<geometry>
+						<mesh filename="package://ctu_cras_norlab_absolem_sensor_config_1/meshes/ouster_holder_ladybug.dae" />
+					</geometry>
+				</visual>
+				<xacro:cylinder_inertial mass="0.2" radius="0.07" length="0.02"
+										 xyz="0 0 -0.01" />
+			</link>
+			<xacro:fixed_joint name="ouster_holder_j" parent="$(arg prefix)omnicam_top" child="$(arg prefix)ouster_holder"
+							   xyz="0 0 0.005" rpy="0 0 ${radians(36)}" />
+		</xacro:if>
 	</xacro:if>
 
 	<!-- Thermal camera -->
@@ -1643,9 +1716,15 @@
 
 	<!-- Mote deployer -->
     <xacro:if value="$(arg has_mote_deployer)">
-        <!-- The link is included automatically in the payload xacro -->
-        <xacro:fixed_joint name="mote_deployer_j" parent="$(arg prefix)rear_right_box" child="$(arg prefix)mote_deployer"
-            xyz="0.09 0.16 -0.01" rpy="0 0 ${pi}" />
+		<!-- The link is included automatically in the payload xacro -->
+		<xacro:if value="$(arg has_omnicam)">
+			<xacro:fixed_joint name="mote_deployer_j" parent="$(arg prefix)rear_right_box" child="$(arg prefix)mote_deployer"
+							   xyz="0.09 0.16 -0.01" rpy="0 0 ${pi}" />
+		</xacro:if>
+		<xacro:if value="$(arg has_omnicam_vras)">
+			<xacro:fixed_joint name="mote_deployer_j" parent="$(arg prefix)rear_right_box" child="$(arg prefix)mote_deployer"
+							   xyz="0.09 0.18 -0.01" rpy="0 0 ${pi}" />
+		</xacro:if>
     </xacro:if>
 
     <xacro:macro name="stiff_joint_tags">
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/utils.xacro b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/utils.xacro
index ccab9e53..4bcec06e 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/utils.xacro
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/utils.xacro
@@ -66,4 +66,20 @@
             </gazebo>
         </xacro:if>
     </xacro:macro>
+
+    <xacro:macro name="solid_color_link_material" params="link r g b">
+        <xacro:unless value="${'$(arg rendering_target)' == 'urdf'}">
+            <gazebo reference="${link}">
+                <visual>
+                    <material>
+                        <ambient>0 0 0 0</ambient>
+                        <!-- Ignition Gazebo seems to use much brighter colors... -->
+                        <diffuse>${max(0, r - 60/255.0)} ${max(0, g - 60/255.0)} ${max(0, b - 60/255.0)} 1</diffuse>
+                        <specular>0 0 0 0</specular>
+                        <emissive>0 0 0 0</emissive>
+                    </material>
+                </visual>
+            </gazebo>
+        </xacro:unless>
+    </xacro:macro>
 </root>
\ No newline at end of file