Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding device selection support #9

Merged
merged 5 commits into from
Apr 3, 2018
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 19 additions & 18 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

```
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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:

Expand All @@ -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
2 changes: 2 additions & 0 deletions launch/zed_cpu_ros.launch
Original file line number Diff line number Diff line change
@@ -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"/>
Expand All @@ -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"/>
Expand Down
10 changes: 5 additions & 5 deletions src/zed_cpu_ros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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_;
Expand Down