!

Note: This tutorial assumes that you have completed the previous tutorials: First Steps, Parameters, Workspace Calibration.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Using the Ensenso Urdf Files

Description: This tutorial introduces and shows how to use launch files, that generate the corresponding urdf files of the Ensenso cameras.

Tutorial Level: BEGINNER

Introduction

With xacros you can define flexible urdf models for your robot. We offer some xacro macros, which build up the corresponding model of the camera. Usually you build up a urdf file to check for collisions with either the model itself, or external models. Our xacro/urdf files are only used for visual purpose. If you want to use them as collision models, you either have to choose a big margin and make sure the collision model is in the right place before running your models in reality!

Currently we supply several xacro-files, that build up the urdf file when launching the launch-file. The following models are supported:

  • N35-Model.
  • X-Series Model (xFA and normal) with mono cameras attached to them.

The X-Series are distinguished between two different baselines:

  • 200 Millimetres
  • 400 Millimetres

There are also parameters you can define, that are:

  • The objectives' radius (only X-Series Xacros)
  • The objectives' length (only X-Series Xacros)
  • The margin of the collision box to the visual model.

Our xacros need a reference frame in order to built up the the urdf model. The reference frame itself is the frame where the camera is fastened. Depending on the Model this is:

  • N35: Back of the Camera, in the middle of the 4 screws.
  • X-Series: Bottom of the camera in the middle of the model. Usually this is used for camera mounts.

Example

In the example launch file the camera has been workspace calibrated beforehand. If you don't know how to calibrate a workspace, please refer to the Workspace Calibration tutorial. Also, we use a X200_xFA Series Model.

If your camera is workspace calibrated, you can just run the launch-file example with

roslaunch ensenso_camera ensenso_X_with_xacro.launch type:=X200_xFA camera:=<123>

where you have to exchange the <123> with the serial of you camera. In order to see other currently supported models, you can exchange the type with:

  • X200
  • X400
  • X400_xFA

If you used the X200_xFA model, you should see something similar below in Rviz.

Screenshot of RViz showing the X200_xFA Model URDF

Because the N-Series differ in the reference frame of the xacros, there is a separate example launch-file. For N-Series you can run it with (exchange the <123> with the serial of you camera)

roslaunch ensenso_camera ensenso_N_with_xacro.launch camera:=<123>

Example in Detail

Now we go through the example launch file of the N-Series in detail.

Complete launch file

First of the complete launch file:

   1 <launch>
   2   <!-- In this example the camera has been workspace calibrated a priori -->
   3   <!-- Camera Parameters -->
   4   <arg name="camera" doc="Camera Serial"/>
   5   <arg name="target_frame" value="Workspace" />
   6 
   7   <!-- Xacro Parameters -->
   8   <!-- From that attachment frame the xacro builds the camera frames -->
   9   <arg name="xacro_reference_frame" default="xacro_ref" />
  10   <!-- Margin for the the collision model of the camera -->
  11   <arg name="collision_margin" default="0.04" />
  12 
  13   <!-- a static transform from the camera_optical_frame (left lense) to the xacro reference frame (camera mount) -->
  14   <!-- Fits for N Series -->
  15   <node pkg="tf2_ros" type="static_transform_publisher" name="camera_to_model_broadcaster"
  16     args="0.05 0.0 -0.05 0.0 0.0 0.0 1.0 $(arg camera)_optical_frame $(arg xacro_reference_frame)" />
  17 
  18   <!-- Use the xacro and surpass parameters -->
  19   <param name="ensenso_description"
  20     command="xacro --inorder '$(find ensenso_description)/ensenso_N_Series.xacro'
  21       corresponding_frame_name:=$(arg xacro_reference_frame)
  22       camera:=$(arg camera)
  23       margin:=$(arg collision_margin)" />
  24 
  25   <!-- publish the transforms between the defined frames in the xacro above -->
  26   <node pkg="robot_state_publisher" type="robot_state_publisher" name="camera_state_publisher" >
  27     <remap from="robot_description" to="ensenso_description" />
  28   </node>
  29 
  30   <!-- Start the Ensenso Camera as Nodelet -->
  31   <node pkg="nodelet" type="nodelet" name="manager_"  args="manager" output="screen" />
  32   <node pkg="nodelet" type="nodelet" name="Ensenso_$(arg camera)" args="load ensenso_camera/nodelet /manager_" output="screen">
  33     <param name="serial" type="string" value="$(arg camera)" />
  34     <param name="target_frame" type="string" value="$(arg target_frame)" />
  35   </node>
  36 </launch>

Launch file explained

The first two parameters are the name of the target_frame and camera serial number.

   4   <arg name="camera" doc="Camera Serial"/>
   5   <arg name="target_frame" value="Workspace" />

After these parameters, the xacro parameters for the N-Series are defined. One is the xacro_reference_frame. This is needed for the xacro in order to build its own joints and links. The collision_margin is the margin between the visual model of the camera and its collision box.

   9   <arg name="xacro_reference_frame" default="xacro_ref" />
  10   <!-- Margin for the the collision model of the camera -->
  11   <arg name="collision_margin" default="0.04" />

The next thing we need is a static transform publisher, that defines, where the camera mount frame (xacro_reference_frame) is. Therefore we use the camera's optical frame, which the camera publishes as camera_frame and define the mounting frame relative to the optical frame.

  13   <!-- a static transform from the camera_optical_frame (left lense) to the xacro reference frame (camera mount) -->
  14   <!-- Fits for N Series -->
  15   <node pkg="tf2_ros" type="static_transform_publisher" name="camera_to_model_broadcaster"
  16     args="0.05 0.0 -0.05 0.0 0.0 0.0 1.0 $(arg camera)_optical_frame $(arg xacro_reference_frame)" />

In the next lines we load the ensenso description onto the parameter server with the command args. Also, we surpass the parameters given above to the xacro macros, which will built up our urdf files.

  18   <!-- Use the xacro and surpass parameters -->
  19   <param name="ensenso_description"
  20     command="xacro --inorder '$(find ensenso_description)/ensenso_N_Series.xacro'
  21       corresponding_frame_name:=$(arg xacro_reference_frame)
  22       camera:=$(arg camera)
  23       margin:=$(arg collision_margin)" />

In order to get the joints of the generated urdf published, we need a robot state publisher. It will publish the joints and links defined in the xacros, as TF transforms and frames.

  25   <!-- publish the transforms between the defined frames in the xacro above -->
  26   <node pkg="robot_state_publisher" type="robot_state_publisher" name="camera_state_publisher" >
  27     <remap from="robot_description" to="ensenso_description" />
  28   </node>

In the last lines of the launch file, we start the ensenso camera as nodelet. It will publish its camera_frame (optical camera frame), needed by the generated urdf.

  30   <!-- Start the Ensenso Camera as Nodelet -->
  31   <node pkg="nodelet" type="nodelet" name="manager_"  args="manager" output="screen" />
  32   <node pkg="nodelet" type="nodelet" name="Ensenso_$(arg camera)" args="load ensenso_camera/nodelet /manager_" output="screen">
  33     <param name="serial" type="string" value="$(arg camera)" />
  34     <param name="target_frame" type="string" value="$(arg target_frame)" />
  35   </node>

Wiki: ensenso_driver/Tutorials/EnsensoUrdf (last edited 2019-12-10 08:28:09 by YasinGuenduez)