Skip to content
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.

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

Added URDF/XACRO for the Zivid One+ 3D Camera #17

Open
wants to merge 12 commits into
base: master
Choose a base branch
from
11 changes: 11 additions & 0 deletions zivid_description/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
cmake_minimum_required(VERSION 3.5)
project(zivid_description)

find_package(catkin REQUIRED COMPONENTS)

catkin_package()

install(
DIRECTORY config launch meshes urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
40 changes: 40 additions & 0 deletions zivid_description/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
# Zivid Description

## Usage
The URDF or the underlying macro can be included in your own project.

To view the Zivid One Plus:
```bash
roslaunch zivid_description view_zivid_one_plus_camera.launch type:='ZIVID_ONE_M'
```

The available types are: `ZIVID_ONE_S`, `ZIVID_ONE_M` (default), `ZIVID_ONE_L`

If an incorrect type is specified, the xacro will fail with the following error:
```bash
name 'optical_frame_angle' is not defined
when evaluating expression '0.5*pi + optical_frame_angle/180*pi'
```

To view the Zivid Two (Plus):
```
roslaunch zivid_description view_zivid_two_camera.launch type:='ZIVID_TWO_M70'
```

The available types are: `ZIVID_TWO_M70` (default), `ZIVID_TWO_M100`, `ZIVID_TWO_PLUS_L110`, `ZIVID_TWO_PLUS_M60`, `ZIVID_TWO_PLUS_M130`

Note: Currently the types do not actually influence the URDF in any way for the Zivid Two or Zivid Two Plus types.

## Launch files
- `load_zivid_one_plus_camera.launch`: Loads the Zivid One Plus camera
Arguments:
- `type` (string, options: `ZIVID_ONE_S`, `ZIVID_ONE_M`, `ZIVID_ONE_L`, default: `ZIVID_ONE_M`)
- `view_zivid_one_plus_camera.launch`: Visualizes the Zivid One Plus camera in RViz
Arguments:
- `type` (string, options: `ZIVID_ONE_S`, `ZIVID_ONE_M`, `ZIVID_ONE_L`, default: `ZIVID_ONE_M`)
- `load_zivid_two_camera.launch`: Loads the Zivid Two or Zivid Two Plus camera
Arguments:
- `type` (string, options: `ZIVID_TWO_M70`, `ZIVID_TWO_M100`, `ZIVID_TWO_PLUS_L110`, `ZIVID_TWO_PLUS_M60`, `ZIVID_TWO_PLUS_M130`, default: `ZIVID_TWO_M70`)
- `load_zivid_two_camera.launch`: Visualizes the Zivid Two or Zivid Two Plus camera in RViz
Arguments:
- `type` (string, options: `ZIVID_TWO_M70`, `ZIVID_TWO_M100`, `ZIVID_TWO_PLUS_L110`, `ZIVID_TWO_PLUS_M60`, `ZIVID_TWO_PLUS_M130`, default: `ZIVID_TWO_M70`)
172 changes: 172 additions & 0 deletions zivid_description/config/config.rviz
Original file line number Diff line number Diff line change
@@ -0,0 +1,172 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /TF1
Splitter Ratio: 0.5
Tree Height: 549
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
optical_frame:
Alpha: 1
Show Axes: false
Show Trail: false
projector_frame:
Alpha: 1
Show Axes: false
Show Trail: false
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
base_link:
Value: true
optical_frame:
Value: true
projector_frame:
Value: true
Marker Scale: 0.5
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
base_link:
optical_frame:
{}
projector_frame:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: base_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 0.8492640256881714
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.01670752838253975
Y: -0.021326012909412384
Z: -0.08468055725097656
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.8097971677780151
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 6.068594932556152
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1200
X: 164
Y: 0
7 changes: 7 additions & 0 deletions zivid_description/launch/load_zivid_one_plus_camera.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
<launch>
<!-- Specify the type of the Zivid two camera -->
<!-- Options: ZIVID_ONE_PLUS_S, ZIVID_ONE_PLUS_M, ZIVID_ONE_PLUS_L -->
<arg name="type" default="ZIVID_ONE_PLUS_M"/>

<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find zivid_description)/urdf/zivid_one_plus_camera.urdf.xacro' type:=$(arg type)"/>
</launch>
7 changes: 7 additions & 0 deletions zivid_description/launch/load_zivid_two_camera.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
<launch>
<!-- Specify the type of the Zivid two camera -->
<!-- Options: ZIVID_TWO_M70, ZIVID_TWO_M100, ZIVID_TWO_PLUS_L110, ZIVID_TWO_PLUS_M60, ZIVID_TWO_PLUS_M130 -->
<arg name="type" default="ZIVID_TWO_M70" />

<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find zivid_description)/urdf/zivid_two_camera.urdf.xacro' type:=$(arg type)"/>
</launch>
12 changes: 12 additions & 0 deletions zivid_description/launch/view_zivid_one_plus_camera.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
<launch>
<!-- Specify the type of the Zivid two camera -->
<!-- Options: ZIVID_ONE_PLUS_S, ZIVID_ONE_PLUS_M, ZIVID_ONE_PLUS_L -->
<arg name="type" default="ZIVID_ONE_PLUS_M" />

<include file="$(find zivid_description)/launch/load_zivid_one_plus_camera.launch">
<arg name="type" default="$(arg type)"/>
</include>

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find zivid_description)/config/config.rviz" required="true"/>
</launch>
12 changes: 12 additions & 0 deletions zivid_description/launch/view_zivid_two_camera.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
<launch>
<!-- Specify the type of the Zivid two camera -->
<!-- Options: ZIVID_TWO_M70, ZIVID_TWO_M100, ZIVID_TWO_PLUS_L110, ZIVID_TWO_PLUS_M60, ZIVID_TWO_PLUS_M130 -->
<arg name="type" default="ZIVID_TWO_M70" />

<include file="$(find zivid_description)/launch/load_zivid_two_camera.launch">
<arg name="type" default="$(arg type)"/>
</include>

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find zivid_description)/config/config.rviz" required="true"/>
</launch>
Binary file not shown.
Binary file added zivid_description/meshes/collision/zivid-two.stl
Binary file not shown.
Binary file not shown.
Binary file added zivid_description/meshes/visual/zivid-two.stl
Binary file not shown.
21 changes: 21 additions & 0 deletions zivid_description/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<package format="2">
<name>zivid_description</name>
<version>0.1.0</version>
<description>Description of visual and collision geometry of Zivid One+ and Zivid Two 3D Camera</description>

<maintainer email="[email protected]">Zivid</maintainer>
<author email="[email protected]">Dave Kroezen - SAM XL</author>

<license>BSD3</license>

<buildtool_depend>catkin</buildtool_depend>

<exec_depend>xacro</exec_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>

<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
56 changes: 56 additions & 0 deletions zivid_description/urdf/macros/zivid_one_plus_camera.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,56 @@
<?xml version="1.0"?>

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

<!-- Import Rviz colors -->
<xacro:include filename="$(find zivid_description)/urdf/zivid_materials.xacro" />

<!-- Zivid One+ Optical Frame Angles -->
<xacro:property name="angle_type_s" value="15.0" />
<xacro:property name="angle_type_m" value="8.5" />
<xacro:property name="angle_type_l" value="6.0" />


<xacro:macro name="zivid_one_plus_camera" params="prefix type">
<!-- Zivid Base Link -->
<link name="${prefix}base_link">
<!-- Visuals -->
<visual>
<origin xyz="-0.0030 -0.0758 0.0445" rpy="${0.5*pi} 0 ${0.5*pi}"/>
<geometry>
<mesh filename="package://zivid_description/meshes/visual/zivid-one-plus.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="zivid_gray"/>
</visual>

<!-- Collisions -->
<collision>
<origin xyz="-0.0030 -0.0758 0.0445" rpy="${0.5*pi} 0 ${0.5*pi}"/>
<geometry>
<mesh filename="package://zivid_description/meshes/collision/zivid-one-plus.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<!-- Zivid Optical Frame (Measurement Frame)-->
<link name="${prefix}optical_frame"/>

<!-- Set the angle of the optical frame based on the type -->
<xacro:if value="${type == 'ZIVID_ONE_PLUS_S'}" >
<xacro:property name="optical_frame_angle" value="${angle_type_s}" />
</xacro:if>
<xacro:if value="${type == 'ZIVID_ONE_PLUS_M'}" >
<xacro:property name="optical_frame_angle" value="${angle_type_m}" />
</xacro:if>
<xacro:if value="${type == 'ZIVID_ONE_PLUS_L'}" >
<xacro:property name="optical_frame_angle" value="${angle_type_l}" />
</xacro:if>

<!-- Zivid Optical (Measurement) Joint -->
<joint name="${prefix}optical_joint" type="fixed">
<origin xyz="0.065 0.062 0.0445" rpy="-${0.5*pi} 0 -${0.5*pi + optical_frame_angle/180*pi}"/>
<parent link="${prefix}base_link"/>
<child link="${prefix}optical_frame"/>
</joint>
</xacro:macro>
</robot>
39 changes: 39 additions & 0 deletions zivid_description/urdf/macros/zivid_two_camera.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
<?xml version="1.0"?>

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

<!-- Import Rviz colors -->
<xacro:include filename="$(find zivid_description)/urdf/zivid_materials.xacro" />

<xacro:macro name="zivid_two_camera" params="prefix type">
<!-- Zivid Base Link -->
<link name="${prefix}base_link">
<!-- Visuals -->
<visual>
<origin xyz="-0.0395 -0.0845 0.0" rpy="${0.5*pi} 0 ${0.5*pi}"/>
<geometry>
<mesh filename="package://zivid_description/meshes/visual/zivid-two.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="zivid_gray"/>
</visual>

<!-- Collisions -->
<collision>
<origin xyz="-0.0395 -0.0845 0.0" rpy="${0.5*pi} 0 ${0.5*pi}"/>
<geometry>
<mesh filename="package://zivid_description/meshes/collision/zivid-two.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>

<!-- Zivid Optical Frame (Measurement Frame)-->
<link name="${prefix}optical_frame"/>

<!-- Zivid Optical (Measurement) Joint -->
<joint name="${prefix}optical_joint" type="fixed">
<origin xyz="0.04702 0.05584 0.0295" rpy="-${0.5*pi} 0 -${0.5*pi}"/>
<parent link="${prefix}base_link"/>
<child link="${prefix}optical_frame"/>
</joint>
</xacro:macro>
</robot>
Loading