Adding device selection support (#9)

* added support of device selection * updated readme * updated readme for device feature * formating to respect original author's format * fitting requested changes (device -> device_id) and (StereoCamera constructor back in one line)
willdzeng · Apr 3, 2018 · 92b50b7 · 92b50b7
1 parent ef185c6
commit 92b50b7
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 23 deletions.
diff --git a/README.md b/README.md
@@ -2,7 +2,7 @@ zed_cpu_ros
 ===========
 A simple zed camera driver which only use CPU and only publish left and right raw images and its camera info.
 
-# Useage:
+# Usage:
 1. git the packge into your working space
 
     ```
@@ -13,9 +13,9 @@ A simple zed camera driver which only use CPU and only publish left and right ra
     ```
 2. Get your calibration files:
     You can get your calibration files from zed or do a calibration your self by using ROS camera calibration package.
-    
+
     (1). From zed:
-    
+
     Find your zed calibration files in
     ```
     cd /usr/local/zed/settings
@@ -38,7 +38,7 @@ A simple zed camera driver which only use CPU and only publish left and right ra
     ```
 
     (2). Do a calibration yourself:
-    
+
     This option is suggested. Reference: http://wiki.ros.org/camera_calibration
     ```
     roslaunch zed_cpu_ros camera_calibration.launch
@@ -53,18 +53,19 @@ A simple zed camera driver which only use CPU and only publish left and right ra
     ```
 ## Launch file parameters
 
- Parameter                    |           Description                                       |              Value          
-------------------------------|-------------------------------------------------------------|-------------------------           
- resolution                   | ZED Camera resolution                                       | '0': HD2K                   
- _                            | _                                                           | '1': HD1080                 
- _                            | _                                                           | '2': HD720                  
- _                            | _                                                           | '3': VGA                                    
- frame_rate                   | Rate at which images are published                          | int                                             
- left_frame_id                | Left Frame ID                                               | string        
- right_frame_id               | Right Frame ID                                              | string        
- load_zed_config              | Whether to use ZED calibration file                         | bool        
- config_file_location         | The location of ZED calibration file                        | string        
- show_image                   | Whether to use opencv show image                            | bool        
+ Parameter                    |           Description                                       |              Value
+------------------------------|-------------------------------------------------------------|-------------------------
+ device_id                   | device_id selection                                       |  int
+ resolution                   | ZED Camera resolution                                       | '0': HD2K
+ _                            | _                                                           | '1': HD1080
+ _                            | _                                                           | '2': HD720
+ _                            | _                                                           | '3': VGA
+ frame_rate                   | Rate at which images are published                          | int
+ left_frame_id                | Left Frame ID                                               | string
+ right_frame_id               | Right Frame ID                                              | string
+ load_zed_config              | Whether to use ZED calibration file                         | bool
+ config_file_location         | The location of ZED calibration file                        | string
+ show_image                   | Whether to use opencv show image                            | bool
 
 # TODO:
 
@@ -75,5 +76,5 @@ A simple zed camera driver which only use CPU and only publish left and right ra
 Patented articulated traction control ARTI technology for stair climbing and obstacle traversal without complex software or controls
 http://transcendrobotics.com/
 
-# Authour:
-Di Zeng 
+# Author:
+Di Zeng
diff --git a/launch/zed_cpu_ros.launch b/launch/zed_cpu_ros.launch
@@ -1,6 +1,7 @@
 <launch>
 <arg name="config_file_location" default="$(find zed_cpu_ros)/config/SN1267.conf"/>
 <arg name="camera_namespace" default="camera"/>
+<arg name="device_id" default="0"/>
 
 <node pkg="zed_cpu_ros" type="zed_cpu_ros" name="zed_cpu_ros_node" output="screen" ns="$(arg camera_namespace)" required="true">
 	<param name="resolution" value="2"/>
@@ -10,6 +11,7 @@
 	<param name="left_frame_id" value="left_frame"/>
 	<param name="right_frame_id" value="right_frame"/>
 	<param name="load_zed_config" value="true"/>
+	<param name="device_id" value="$(arg device_id)"/>
 </node>
 
 <node pkg="tf" type="static_transform_publisher" name="static_tf_1" args="0.25 0 0.4 0 0 0 1 base_link left_frame 30"/>

diff --git a/src/zed_cpu_ros.cpp b/src/zed_cpu_ros.cpp
@@ -24,16 +24,15 @@ class StereoCamera
 {
 
 public:
-
 	/**
 	 * @brief      { stereo camera driver }
 	 *
 	 * @param[in]  resolution  The resolution
 	 * @param[in]  frame_rate  The frame rate
 	 */
-	StereoCamera(int resolution, double frame_rate): frame_rate_(30.0) {
+	 StereoCamera(int device_id, int resolution, double frame_rate):frame_rate_(30.0) {
 
-		camera_ = new cv::VideoCapture(0);
+		camera_ = new cv::VideoCapture(device_id);
 		cv::Mat raw;
 		cv::Mat left_image;
 		cv::Mat right_image;
@@ -135,9 +134,10 @@ class ZedCameraROS {
 		private_nh.param("right_frame_id", right_frame_id_, std::string("right_camera"));
 		private_nh.param("show_image", show_image_, false);
 		private_nh.param("load_zed_config", load_zed_config_, true);
+		private_nh.param("device_id", device_id_, 0);
 
 		ROS_INFO("Try to initialize the camera");
-		StereoCamera zed(resolution_, frame_rate_);
+		StereoCamera zed(device_id_, resolution_, frame_rate_);
 		ROS_INFO("Initialized the camera");
 
 		// setup publisher stuff
@@ -381,7 +381,7 @@ class ZedCameraROS {
 	}
 
 private:
-	int resolution_;
+	int device_id_, resolution_;
 	double frame_rate_;
 	bool show_image_, load_zed_config_;
 	double width_, height_;