From b4d839bdfca0e2714103c1d2fe3750f3a8f36832 Mon Sep 17 00:00:00 2001
From: pengzw <1430757558@qq.com>
Date: Sun, 20 Jan 2019 08:51:49 +0800
Subject: [PATCH] Adds multi-camera support

---
 README.md             | 9 ++++++++-
 jetson_csi_cam.launch | 6 ++++--
 2 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/README.md b/README.md
index ad3cf8d..b82391e 100644
--- a/README.md
+++ b/README.md
@@ -9,6 +9,7 @@ This ROS package makes it simple to use CSI cameras on the Nvidia Jetson TK1, TX
 * Control resolution and framerate.
 * Camera calibration support.
 * Works efficiently, allowing for high resolution and/or high fps video by taking advantage of gstreamer and the Nvidia multimedia API.
+* Multi camera support.
 
 ---
 
@@ -72,11 +73,16 @@ At this point everything should be ready to go.
 
 # Usage
 
-> **TL;DR:** To publish the camera stream to the ROS topic `/csi_cam/image_raw`, use this command in the terminal:
+> **TL;DR:** To publish the camera stream to the ROS topic `/csi_cam_0/image_raw`, use this command in the terminal:
 >
 > ```
 > roslaunch jetson_csi_cam jetson_csi_cam.launch width:=<image width> height:=<image height> fps:=<desired framerate>
 > ```
+> If you have another camera on your Jetson TX2, to publish the other camera stream to the ROS topic `/csi_cam_1/image_raw`, use this command in the terminal:
+>
+> ```
+> roslaunch jetson_csi_cam jetson_csi_cam.launch sensor_id:=1 width:=<image width> height:=<image height> fps:=<desired framerate>
+> ```
 > If you would like to learn more of the details, read ahead.
 
 ## Capturing Video
@@ -105,6 +111,7 @@ In other words, to set any of the arguments use the `<arg_name>:=<arg_value>` op
 
 #### Accepted arguments for `jetson_csi_cam.launch`
 
+* `sensor_id` -- The sensor id of each camera
 * `width` -- Image Width
 * `height` -- Image Height
 * `fps` -- Desired framerate. True framerate may not reach this if set too high.
diff --git a/jetson_csi_cam.launch b/jetson_csi_cam.launch
index 0e12f81..4230375 100644
--- a/jetson_csi_cam.launch
+++ b/jetson_csi_cam.launch
@@ -1,6 +1,7 @@
 <launch>
   <!-- Command Line Arguments -->
-  <arg name="cam_name" default="csi_cam" />                  <!-- The name of the camera (corrsponding to the camera info) -->
+  <arg name="sensor_id" default="0" />                       <!-- The sensor id of the camera -->
+  <arg name="cam_name" default="csi_cam_$(arg sensor_id)" /> <!-- The name of the camera (corrsponding to the camera info) -->
   <arg name="frame_id" default="/$(arg cam_name)_link" />    <!-- The TF frame ID. -->
   <arg name="sync_sink" default="true" />                    <!-- Synchronize the app sink. Setting this to false may resolve problems with sub-par framerates. -->
   <arg name="width" default="1920" />                        <!-- Image Width -->
@@ -8,12 +9,13 @@
   <arg name="fps" default="30" />                            <!-- Desired framerate. True framerate may not reach this if set too high. -->
 
   <!-- Make arguments available to parameter server -->
+  <param name="$(arg cam_name)/camera_id" type="int" value="$(arg sensor_id)" />
   <param name="$(arg cam_name)/image_width" type="int" value="$(arg width)" />
   <param name="$(arg cam_name)/image_height" type="int" value="$(arg height)" />
   <param name="$(arg cam_name)/target_fps" type="int" value="$(arg fps)" />
 
   <!-- Define the GSCAM pipeline -->
-  <env name="GSCAM_CONFIG" value="nvcamerasrc ! video/x-raw(memory:NVMM),
+  <env name="GSCAM_CONFIG" value="nvcamerasrc sensor-id=$(arg sensor_id) ! video/x-raw(memory:NVMM),
     width=(int)$(arg width), height=(int)$(arg height), format=(string)I420, framerate=(fraction)$(arg fps)/1 ! 
     nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />