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}
)
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
3 changes: 3 additions & 0 deletions zivid_description/launch/load_zivid_camera.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
<launch>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find zivid_description)/urdf/zivid_camera.urdf.xacro'"/>
</launch>
6 changes: 6 additions & 0 deletions zivid_description/launch/view_zivid_camera.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
<launch>
<include file="$(find zivid_description)/launch/load_zivid_camera.launch" />

<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 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+ 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>
46 changes: 46 additions & 0 deletions zivid_description/urdf/macros/zivid_camera.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Properties -->
<material name="zivid_gray">
<color rgba="0.25 0.25 0.25 1"/>
</material>

<xacro:macro name="zivid_camera" params="prefix">
<!-- 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}"/>
dave992 marked this conversation as resolved.
Show resolved Hide resolved
<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"/>
runenordmo marked this conversation as resolved.
Show resolved Hide resolved
</geometry>
</collision>
</link>

<!-- Zivid Optical (Measurement) and Projector Frames -->
<link name="${prefix}optical_frame"/>
<link name="${prefix}projector_frame"/>

<!-- Zivid Optical (Measurement) and Projector Joints -->
<joint name="${prefix}optical_joint" type="fixed">
<origin xyz="0.065 0.062 0.0445" rpy="-${0.5*pi} 0 -${0.5*pi + 8.5/180*pi}"/>

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am not sure we need a optical joint that is not the same as the base_link?
The optical joint's frame should ideally just be the same frame as the points in the pointcloud is given in, which is a fixed point in the camera - I can figure out exactly how it's specified.

Then the only frame that is essential is that optical frame, and a hand-eye transform will be used to get the pointcloud a robot's frame.
I think might be useful to also have a rough estimate of the projector coordinate system relative to the optical frame, like you have added (discussed in 624a977#r563312884).

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The optical joint's frame should ideally just be the same frame as the points in the pointcloud is given in, which is a fixed point in the camera - I can figure out exactly how it's specified.

joint != frame.
A frame is a coordinate frame. A joint is a connection between frames defining the transformation between them.

The measurement frame (optical_frame) is indeed defined by the frame in which the camera outputs the captures. This may or may not coincide with another frame, but is definitely a distinct frame (even if it is just for semantics). The joint just connect the two links together.

Having a confirmation on the location of the optical_frame relative to the mounting hole (/base_link) would be very helpful indeed. Our usage (attached to a robot manipulator) does show that this location is correct or at least really close to the actual measurement frame. We often use this description "as is" without calibration for some quick captures.

Then the only frame that is essential is that optical frame

If looking at the camera in isolation yes, but my intent behind making this package is to actually connect it to other hardware. Then the base_link is essential as well, even if only by convention, expectations, and ease of use.

The base_link is located such that the geometry can easily be attached, it is the "starting point" of the geometry. In this case, I picked the center mounting hole as I saw this as a convenient location to which I can attach the camera for example a robot or end-effector. All description packages should start with a base_link.

and a hand-eye transform will be used to get the pointcloud a robot's frame

I would say that calibration is indeed needed for real-world applications, but not part of the scope of this package. Description packages are just there to give the ideal geometry and required frames of hardware. This can be used then for simulations or for a first best guess of your real-world counterpart.

Typically calibrations will result in a new frame, for example: calibrated_optical_frame, that is then separately attached to the description by the user.

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Let's keep the base_link link and optical joint. I see your point on that being useful for the simulation and a first best guess or starting point.

Typically calibrations will result in a new frame, for example: calibrated_optical_frame, that is then separately attached to the description by the user.

Yes, I agree, in a real-world application the hand-eye calibration will take over, to be able to know how the point cloud is related to the robot's base. And then the transformation between the base_link frame and the optical_frame is mostly useful for simulations and verifying that the robot-camera calibration is sound.

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Having a confirmation on the location of the optical_frame relative to the mounting hole (/base_link) would be very helpful indeed.

Yes, I will get this information

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ok, so the point cloud is given relative to a location that is the optical center given at a certain temperature+aperture location. So this will vary for each camera, even within the same model, for instance Zivid One+ M.

So I think we can communicate through the naming of the joints and frames that the transformation between the between the mounting hole and the camera's optical center at the given calibration point (certain temperature+aperture) is an approximation.
And the we can use the fixed approximate values provided in the datasheet.

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So this will vary for each camera, even within the same model, for instance Zivid One+ M.

Would the driver have a way of retrieving that information?

There's no requirement for the xacro:macro to contain that link.

If the driver could publish it (as a TF frame), that would work just as well.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So this will vary for each camera, even within the same model, for instance Zivid One+ M.

Would the driver have a way of retrieving that information?

Would also be interested in this, especially if it is moving between usages (e.g. due to temperature differences)

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

(forgive me if this has been discussed before / is generally known about Zivid devices)

Unless the pointcloud / depth images are automatically transformed by the driver to have their origins at a fixed point (so the driver / embedded firmware compensates for the offsets/variation due to temperature/other factors), not having the precise location of the optical frame significantly complicates using Zivid cameras for really precise/accurate work.

Extrinsic calibrations could likely compensate for that offset (they would incorporate it into whatever transform they determine between the camera itself and the mounting link), but IIUC from the comments by @runenordmo, that would essentially only be the extrinsic calibration for one particular 'state' of the sensor.

If the camera itself already compensates for this, a static link in the URDF/xacro:macro would seem to suffice. If not, the driver would ideally publish the transform itself -- perhaps not continuously, but at least the one associated with a particular capture. The rest of the system could then match it based on time from the header.stamp and the TF buffer.

<parent link="${prefix}base_link"/>
<child link="${prefix}optical_frame"/>
</joint>

<joint name="${prefix}projector_joint" type="fixed">
dave992 marked this conversation as resolved.
Show resolved Hide resolved
<origin xyz="-0.0030 -0.0758 0.0445" rpy="-${0.5*pi} 0 -${0.5*pi}"/>
<parent link="${prefix}base_link"/>
<child link="${prefix}projector_frame"/>
</joint>
</xacro:macro>
</robot>
5 changes: 5 additions & 0 deletions zivid_description/urdf/zivid_camera.urdf.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
<?xml version="1.0"?>
<robot name="zivid_camera_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find zivid_description)/urdf/macros/zivid_camera.xacro"/>
<xacro:zivid_camera prefix=""/>
</robot>