diff --git a/README.md b/README.md index d335d61..22893fb 100644 --- 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 \ No newline at end of file +# Author: +Di Zeng diff --git a/launch/zed_cpu_ros.launch b/launch/zed_cpu_ros.launch index 83c5c22..613c12b 100644 --- a/launch/zed_cpu_ros.launch +++ b/launch/zed_cpu_ros.launch @@ -1,6 +1,7 @@ + @@ -10,6 +11,7 @@ + diff --git a/src/zed_cpu_ros.cpp b/src/zed_cpu_ros.cpp index c94c979..ba95025 100644 --- 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_;