!

Note: This tutorial assumes that you have completed the previous tutorials: Running a Camera as Nodelet, Camera Frames with multiple Cameras.
(!) 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.

Running several Ensenso Nodelets

Description: This tutorial shows how to run several cameras as nodelets within the same nodelet manager.

Tutorial Level: BEGINNER

Next Tutorial: Textured Point Cloud

The Intend of using the same Nodelet Manager

If the cameras run as nodelets in the same manager, they can be opened within one process, opening up additional functionality with the EnsensoSDK. For example, an RGB mono and a stereo camera can be opened in a joint process to create a coloured point cloud, which is described in the next tutorial.

The following tutorial will show you briefly how you can start several nodelets together with one nodelet manager.

The complete launch file

The following launch file starts a mono camera nodelet and a stereo camera nodelet within the same manager. Details are described below.

   1 <launch>
   2   <arg name="serial_stereo" doc="Serial of the Ensenso stereo camera" />
   3   <arg name="serial_mono" doc="Serial of the mono mamera" />
   4   <arg name="stereo_ns" doc="Namespace for the stereo camera" />
   5   <arg name="mono_ns" doc="Namespace for the mono camera" />
   6   <arg name="target_frame_name" value="Workspace" doc="The root of the tf tree"/>
   7 
   8   <node pkg="nodelet" type="nodelet" name="manager_"  args="manager" output="screen" />
   9   <!-- Camera nodes running as nodelets -->
  10   <node pkg="nodelet" type="nodelet" name="Ensenso_$(arg serial_stereo)" args="load ensenso_camera/nodelet /manager_" output="screen" ns="$(arg stereo_ns)">
  11     <param name="serial" type="string" value="$(arg serial_stereo)" />
  12     <param name="target_frame" type="string" value="$(arg target_frame_name)" />
  13   </node>
  14   <node pkg="nodelet" type="nodelet" name="Ensenso_$(arg serial_mono)" args="load ensenso_camera/nodelet_mono /manager_" output="screen" ns="$(arg mono_ns)">
  15     <param name="serial" type="string" value="$(arg serial_mono)" />
  16     <param name="link_frame" value="$(arg serial_stereo)_optical_frame" />
  17   </node>
  18 </launch>

Step by Step

As in the previous tutorial the first few lines take arguments. The serial of the stereo camera is defined in the argument serial_stereo, whereas the mono camera's serial is defined with the serial_mono argument.

   2   <arg name="serial_stereo" doc="Serial of the Ensenso stereo camera" />
   3   <arg name="serial_mono" doc="Serial of the mono mamera" />

The crucial arguments are the namespaces of the cameras. Because the topics and actions are defined with a default NodeHandle, the topics would overwrite themselves in the global namespace /. Therefore each nodelet has to be given its own namespace in the following lines:

   4   <arg name="stereo_ns" doc="Namespace for the stereo camera" />
   5   <arg name="mono_ns" doc="Namespace for the mono camera" />

In the next lines the node manager, named manager_ is defined. Its name has to be used in the loading argument of all nodelets you want to have handled by this nodelet manager.

   8   <node pkg="nodelet" type="nodelet" name="manager_"  args="manager" output="screen" />

The next bracket starts the nodelet for stereo cameras, defined by the ensenso_camera/nodelet argument taken by the nodelet. Crucial however is the manager's name, which has to be the same for the two nodelets.

  10   <node pkg="nodelet" type="nodelet" name="Ensenso_$(arg serial_stereo)" args="load ensenso_camera/nodelet /manager_" output="screen" ns="$(arg stereo_ns)">
  11     <param name="serial" type="string" value="$(arg serial_stereo)" />
  12     <param name="target_frame" type="string" value="$(arg target_frame_name)" />
  13   </node>

The last lines define the nodelet for mono cameras, defined by the ensenso_camera/nodelet_mono argument.

  14   <node pkg="nodelet" type="nodelet" name="Ensenso_$(arg serial_mono)" args="load ensenso_camera/nodelet_mono /manager_" output="screen" ns="$(arg mono_ns)">
  15     <param name="serial" type="string" value="$(arg serial_mono)" />
  16     <param name="link_frame" value="$(arg serial_stereo)_optical_frame" />
  17   </node>

All parameters are mapped to their node counterparts respectively. For the nodelet, they are defined in the ensenso_camera_node and for the nodelet_mono class they are defined by the ensenso_camera_mono_node.

Running the Nodelets

This launch file is already defined in the ensenso_camera package. But you have to define the serial numbers of the cameras and their nodelets namespaces first. Afterwards just launch it with:

$ roslaunch ensenso_camera mono_stereo_nodelets.launch

Wiki: ensenso_driver/Tutorials/SeveralNodelets (last edited 2019-08-07 14:12:25 by YasinGuenduez)