Skip to content

Civerachb cpr master #795

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Myzhar merged 4 commits into from
Nov 24, 2021
Merged

Civerachb cpr master #795

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
5d848e5
Create a macro that can be used to add multiple zed cameras to an exi…
civerachb-cpr Aug 17, 2021
186d662
Merge branch 'stereolabs:master' into master
civerachb-cpr Oct 26, 2021
a064324
Fix syntax errors in xacro file and improve URDF
Myzhar Nov 24, 2021
038fac5
Improve URDF link positions
Myzhar Nov 24, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
138 changes: 16 additions & 122 deletions zed_wrapper/urdf/zed_descr.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,126 +19,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->

<robot name="stereolabs_camera" xmlns:xacro="http://ros.org/wiki/xacro">

<!-- Includes -->
<xacro:include filename="$(find zed_wrapper)/urdf/include/materials.urdf.xacro" />

<!-- Arguments -->


<xacro:arg name="camera_name" default="zed" />
<xacro:arg name="camera_model" default="zed" />
<xacro:arg name="base_frame" default="base_link" />
<xacro:arg name="cam_pos_x" default="0.0" />
<xacro:arg name="cam_pos_y" default="0.0" />
<xacro:arg name="cam_pos_z" default="0.0" />
<xacro:arg name="cam_roll" default="0.0" />
<xacro:arg name="cam_pitch" default="0.0" />
<xacro:arg name="cam_yaw" default="0.0" />

<!-- Properties -->
<xacro:property name="M_PI" value="3.1415926535897931" />
<xacro:property name="model" value="$(arg camera_model)" />

<xacro:if value="${model == 'zed'}">
<xacro:property name="baseline" value="0.12" />
</xacro:if>
<xacro:if value="${model == 'zedm'}">
<xacro:property name="baseline" value="0.06" />
</xacro:if>
<xacro:if value="${model == 'zed2'}">
<xacro:property name="baseline" value="0.12" />
</xacro:if>
<xacro:if value="${model == 'zed2i'}">
<xacro:property name="baseline" value="0.12" />
</xacro:if>

<!-- base_link -->
<link name="$(arg base_frame)" />

<!-- Camera Center -->
<joint name="$(arg camera_name)_camera_center_joint" type="fixed">
<parent link="$(arg base_frame)"/>
<child link="$(arg camera_name)_camera_center"/>
<origin xyz="$(arg cam_pos_x) $(arg cam_pos_y) $(arg cam_pos_z)" rpy="$(arg cam_roll) $(arg cam_pitch) $(arg cam_yaw)" />
</joint>

<link name="$(arg camera_name)_camera_center">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://zed_interfaces/meshes/${model}.stl" />
</geometry>
<material name="${model}_mat" />
</visual>
</link>

<!-- Left Camera -->
<joint name="$(arg camera_name)_left_camera_joint" type="fixed">
<parent link="$(arg camera_name)_camera_center"/>
<child link="$(arg camera_name)_left_camera_frame"/>
<origin xyz="0 ${baseline/2} 0" rpy="0 0 0" />
</joint>

<link name="$(arg camera_name)_left_camera_frame" />

<joint name="$(arg camera_name)_left_camera_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
<parent link="$(arg camera_name)_left_camera_frame"/>
<child link="$(arg camera_name)_left_camera_optical_frame"/>
</joint>

<link name="$(arg camera_name)_left_camera_optical_frame"/>

<!-- Right Camera -->
<joint name="$(arg camera_name)_right_camera_joint" type="fixed">
<parent link="$(arg camera_name)_camera_center"/>
<child link="$(arg camera_name)_right_camera_frame"/>
<origin xyz="0 -${baseline/2} 0" rpy="0 0 0" />
</joint>

<link name="$(arg camera_name)_right_camera_frame" />

<joint name="$(arg camera_name)_right_camera_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
<parent link="$(arg camera_name)_right_camera_frame"/>
<child link="$(arg camera_name)_right_camera_optical_frame"/>
</joint>

<link name="$(arg camera_name)_right_camera_optical_frame"/>

<!-- ZED2 Sensors -->
<xacro:if value="${model == 'zed2'}">
<joint name="$(arg camera_name)_mag_joint" type="fixed">
<parent link="$(arg camera_name)_camera_center"/>
<child link="$(arg camera_name)_mag_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="$(arg camera_name)_mag_link" />

<joint name="$(arg camera_name)_baro_joint" type="fixed">
<parent link="$(arg camera_name)_camera_center"/>
<child link="$(arg camera_name)_baro_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="$(arg camera_name)_baro_link" />

<joint name="$(arg camera_name)_temp_left_joint" type="fixed">
<parent link="$(arg camera_name)_left_camera_frame"/>
<child link="$(arg camera_name)_temp_left_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="$(arg camera_name)_temp_left_link" />

<joint name="$(arg camera_name)_temp_right_joint" type="fixed">
<parent link="$(arg camera_name)_right_camera_frame"/>
<child link="$(arg camera_name)_temp_right_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="$(arg camera_name)_temp_right_link" />
</xacro:if>
<xacro:arg name="camera_name" default="zed" />
<xacro:arg name="camera_model" default="zed" />
<xacro:arg name="base_frame" default="base_link" />
<xacro:arg name="cam_pos_x" default="0.0" />
<xacro:arg name="cam_pos_y" default="0.0" />
<xacro:arg name="cam_pos_z" default="0.0" />
<xacro:arg name="cam_roll" default="0.0" />
<xacro:arg name="cam_pitch" default="0.0" />
<xacro:arg name="cam_yaw" default="0.0" />

<xacro:include filename="$(find zed_wrapper)/urdf/zed_macro.urdf.xacro" />
<xacro:zed_camera name="$(arg camera_name)" model="$(arg camera_model)" parent="$(arg base_frame)">
<origin xyz="$(arg cam_pos_x) $(arg cam_pos_y) $(arg cam_pos_z)" rpy="$(arg cam_roll) $(arg cam_pitch) $(arg cam_yaw)" />
</xacro:zed_camera>

<link name="$(arg base_frame)" />
</robot>
158 changes: 158 additions & 0 deletions zed_wrapper/urdf/zed_macro.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,158 @@
<?xml version="1.0"?>

<!--
Copyright (c) 2020, STEREOLABS.

All rights reserved.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->

<robot xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="$(find zed_wrapper)/urdf/include/materials.urdf.xacro" />
<xacro:property name="M_PI" value="3.1415926535897931" />

<!--
Parameters:
- name: the camera's name. should match the parameter sent to the launch file for this camera
- model: the tye of camera, one of zed, zedm, zed2, zed2i
- parent: the parent link of the camera. must be defined in the URDF file that uses this macro
- origin: the xyz/rpy offset from the parent link
-->
<xacro:macro name="zed_camera" params="name=zed model=zed parent *origin">
<xacro:if value="${model == 'zed'}">
<xacro:property name="baseline" value="0.12" />
<xacro:property name="height" value="0.033" />
<xacro:property name="bottom_slope" value="0.05" />
<xacro:property name="screw_offset_x" value="0.0" />
<xacro:property name="optical_offset_x" value="-0.01" />
</xacro:if>
<xacro:if value="${model == 'zedm'}">
<xacro:property name="baseline" value="0.06" />
<xacro:property name="height" value="0.0265" />
<xacro:property name="bottom_slope" value="0.0" />
<xacro:property name="screw_offset_x" value="0.0" />
<xacro:property name="optical_offset_x" value="0.0" />
</xacro:if>
<xacro:if value="${model == 'zed2'}">
<xacro:property name="baseline" value="0.12" />
<xacro:property name="height" value="0.033" />
<xacro:property name="bottom_slope" value="0.05" />
<xacro:property name="screw_offset_x" value="0.0" />
<xacro:property name="optical_offset_x" value="-0.01" />
</xacro:if>
<xacro:if value="${model == 'zed2i'}">
<xacro:property name="baseline" value="0.12" />
<xacro:property name="height" value="0.033" />
<xacro:property name="bottom_slope" value="0.0" />
<xacro:property name="screw_offset_x" value="-0.01" />
<xacro:property name="optical_offset_x" value="-0.01" />
</xacro:if>



<!-- Camera mounting point (the threaded screw hole in the bottom) -->
<link name="${name}_base_link" />
<joint name="${name}_base_joint" type="fixed">
<parent link="${parent}"/>
<child link="${name}_base_link"/>
<xacro:insert_block name="origin" />
</joint>


<!-- Camera Center -->
<link name="${name}_camera_center">
<visual>
<origin xyz="${screw_offset_x} 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://zed_interfaces/meshes/${model}.stl" />
</geometry>
<material name="${model}_mat" />
</visual>
<collision>
<geometry>
<mesh filename="package://zed_interfaces/meshes/${model}.stl" />
</geometry>
</collision>
</link>
<joint name="${name}_camera_center_joint" type="fixed">
<parent link="${name}_base_link"/>
<child link="${name}_camera_center"/>
<origin xyz="0 0 ${height/2}" rpy="0 ${bottom_slope} 0" />
</joint>


<!-- Left Camera -->
<link name="${name}_left_camera_frame" />
<joint name="${name}_left_camera_joint" type="fixed">
<parent link="${name}_camera_center"/>
<child link="${name}_left_camera_frame"/>
<origin xyz="0 ${baseline/2} 0" rpy="0 0 0" />
</joint>

<link name="${name}_left_camera_optical_frame"/>
<joint name="${name}_left_camera_optical_joint" type="fixed">
<origin xyz="${optical_offset_x} 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
<parent link="${name}_left_camera_frame"/>
<child link="${name}_left_camera_optical_frame"/>
</joint>


<!-- Right Camera -->
<link name="${name}_right_camera_frame" />
<joint name="${name}_right_camera_joint" type="fixed">
<parent link="${name}_camera_center"/>
<child link="${name}_right_camera_frame"/>
<origin xyz="0 -${baseline/2} 0" rpy="0 0 0" />
</joint>

<link name="${name}_right_camera_optical_frame"/>
<joint name="${name}_right_camera_optical_joint" type="fixed">
<origin xyz="${optical_offset_x} 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}"/>
<parent link="${name}_right_camera_frame"/>
<child link="${name}_right_camera_optical_frame"/>
</joint>


<!-- ZED2 Sensors -->
<xacro:if value="${model == 'zed2'}">
<link name="${name}_mag_link" />
<joint name="${name}_mag_joint" type="fixed">
<parent link="${name}_camera_center"/>
<child link="${name}_mag_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="${name}_baro_link" />
<joint name="${name}_baro_joint" type="fixed">
<parent link="${name}_camera_center"/>
<child link="${name}_baro_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="${name}_temp_left_link" />
<joint name="${name}_temp_left_joint" type="fixed">
<parent link="${name}_left_camera_frame"/>
<child link="${name}_temp_left_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="${name}_temp_right_link" />
<joint name="${name}_temp_right_joint" type="fixed">
<parent link="${name}_right_camera_frame"/>
<child link="${name}_temp_right_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
</xacro:if>
</xacro:macro>
</robot>