Skip to content

Add publisher for disparity #189

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.

Sign up for GitHub

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

Closed
fujimo-t wants to merge 1 commit into from
+54 −5
Closed

Add publisher for disparity #189

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
3 changes: 3 additions & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ find_package(catkin COMPONENTS
roscpp
rosconsole
sensor_msgs
stereo_msgs
dynamic_reconfigure
tf2_ros
pcl_conversions
Expand All @@ -54,6 +55,7 @@ checkPackage("image_transport" "")
checkPackage("roscpp" "")
checkPackage("rosconsole" "")
checkPackage("sensor_msgs" "")
checkPackage("stereo_msgs" "")
checkPackage("dynamic_reconfigure" "")
checkPackage("tf2_ros" "")
checkPackage("pcl_conversions" "")
Expand All @@ -69,6 +71,7 @@ catkin_package(
roscpp
rosconsole
sensor_msgs
stereo_msgs
opencv
image_transport
dynamic_reconfigure
Expand Down
1 change: 1 addition & 0 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ The zed_ros_wrapper is a catkin package. It depends on the following ROS package
- roscpp
- rosconsole
- sensor_msgs
- stereo_msgs
- opencv
- image_transport
- dynamic_reconfigure
Expand Down
4 changes: 3 additions & 1 deletion launch/zed_camera.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<arg name="base_frame" default="zed_center" />
<arg name="camera_frame" default="zed_left_camera" />
<arg name="depth_frame" default="zed_depth_camera" />
<arg name="disparity_frame" default="zed_depth_camera" />
<!-- Publish urdf zed -->
<arg name="publish_urdf" default="true" />

Expand All @@ -44,7 +45,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<param name="base_frame" value="$(arg base_frame)" />
<param name="camera_frame" value="$(arg camera_frame)" />
<param name="depth_frame" value="$(arg depth_frame)" />

<param name="disparity_frame" value="$(arg disparity_frame)" />

<!-- SVO file path -->
<param name="svo_filepath" value="$(arg svo_file)" />

Expand Down
2 changes: 2 additions & 0 deletions package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
<build_depend>roscpp</build_depend>
<build_depend>rosconsole</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>stereo_msgs</build_depend>
<build_depend>opencv</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
Expand All @@ -29,6 +30,7 @@
<run_depend>roscpp</run_depend>
<run_depend>rosconsole</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>stereo_msgs</run_depend>
<run_depend>opencv</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
Expand Down
49 changes: 45 additions & 4 deletions src/zed_wrapper_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include <sensor_msgs/distortion_models.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <stereo_msgs/DisparityImage.h>
#include <dynamic_reconfigure/server.h>
#include <zed_wrapper/ZedConfig.h>
#include <nav_msgs/Odometry.h>
Expand Down Expand Up @@ -100,6 +101,7 @@ namespace zed_wrapper {
image_transport::Publisher pub_right;
image_transport::Publisher pub_raw_right;
image_transport::Publisher pub_depth;
ros::Publisher pub_disparity;
ros::Publisher pub_cloud;
ros::Publisher pub_rgb_cam_info;
ros::Publisher pub_left_cam_info;
Expand All @@ -116,6 +118,7 @@ namespace zed_wrapper {
std::string right_frame_id;
std::string rgb_frame_id;
std::string depth_frame_id;
std::string disparity_frame_id;
std::string cloud_frame_id;
std::string odometry_frame_id;
std::string base_frame_id;
Expand Down Expand Up @@ -352,6 +355,26 @@ namespace zed_wrapper {
}
pub_depth.publish(imageToROSmsg(depth, encoding, depth_frame_id, t));
}

/* \brief Publish a cv::Mat disparity image with a ros Publisher
* \param disparity : the disparity image to publish
* \param pub_disparity : the publisher object to use
* \param disparity_frame_id : the id of the reference frame of the depth image
* \param t : the ros::Time to stamp the depth image
*/
void publishDisparity(cv::Mat disparity, ros::Publisher& pub_disparity, string disparity_frame_id, ros::Time t) {
sl::CameraInformation zedParam = zed.getCameraInformation();
sensor_msgs::ImagePtr disparity_image = imageToROSmsg(disparity, sensor_msgs::image_encodings::TYPE_32FC1, disparity_frame_id, t);

stereo_msgs::DisparityImage msg;
msg.image = *disparity_image;
msg.header = msg.image.header;
msg.f = zedParam.calibration_parameters.left_cam.fx;
msg.T = zedParam.calibration_parameters.T.x;
msg.min_disparity = msg.f * msg.T / zed.getDepthMaxRangeValue();
msg.max_disparity = msg.f * msg.T / zed.getDepthMinRangeValue();
pub_disparity.publish(msg);
}

/* \brief Publish a pointCloud with a ros Publisher
* \param width : the width of the point cloud
Expand Down Expand Up @@ -523,7 +546,7 @@ namespace zed_wrapper {
trackParams.area_file_path = odometry_DB.c_str();


sl::Mat leftZEDMat, rightZEDMat, depthZEDMat;
sl::Mat leftZEDMat, rightZEDMat, depthZEDMat, disparityZEDMat;
// Main loop
while (nh_ns.ok()) {
// Check for subscribers
Expand All @@ -534,9 +557,10 @@ namespace zed_wrapper {
int right_SubNumber = pub_right.getNumSubscribers();
int right_raw_SubNumber = pub_raw_right.getNumSubscribers();
int depth_SubNumber = pub_depth.getNumSubscribers();
int disparity_SubNumber = pub_disparity.getNumSubscribers();
int cloud_SubNumber = pub_cloud.getNumSubscribers();
int odom_SubNumber = pub_odom.getNumSubscribers();
bool runLoop = (rgb_SubNumber + rgb_raw_SubNumber + left_SubNumber + left_raw_SubNumber + right_SubNumber + right_raw_SubNumber + depth_SubNumber + cloud_SubNumber + odom_SubNumber) > 0;
bool runLoop = (rgb_SubNumber + rgb_raw_SubNumber + left_SubNumber + left_raw_SubNumber + right_SubNumber + right_raw_SubNumber + depth_SubNumber + disparity_SubNumber + cloud_SubNumber + odom_SubNumber) > 0;

runParams.enable_point_cloud = false;
if (cloud_SubNumber > 0)
Expand All @@ -554,7 +578,7 @@ namespace zed_wrapper {
zed.disableTracking();
tracking_activated = false;
}
computeDepth = (depth_SubNumber + cloud_SubNumber + odom_SubNumber) > 0; // Detect if one of the subscriber need to have the depth information
computeDepth = (depth_SubNumber + disparity_SubNumber + cloud_SubNumber + odom_SubNumber) > 0; // Detect if one of the subscriber need to have the depth information
ros::Time t = ros::Time::now(); // Get current time

grabbing = true;
Expand Down Expand Up @@ -663,6 +687,14 @@ namespace zed_wrapper {
publishDepth(toCVMat(depthZEDMat), pub_depth, depth_frame_id, t); // in meters
}

// Publish the disparity image if someone has subscribed to
if (disparity_SubNumber > 0) {
zed.retrieveMeasure(disparityZEDMat, sl::MEASURE_DISPARITY);
// Need to flip sign, but cause of this is not sure
cv::Mat disparity = toCVMat(disparityZEDMat) * -1.0;
publishDisparity(disparity, pub_disparity, disparity_frame_id, t);
}

// Publish the point cloud if someone has subscribed to
if (cloud_SubNumber > 0) {
// Run the point cloud conversion asynchronously to avoid slowing down all the program
Expand Down Expand Up @@ -753,7 +785,8 @@ namespace zed_wrapper {
nh_ns.param<std::string>("base_frame", base_frame_id, "base_frame");
nh_ns.param<std::string>("camera_frame", camera_frame_id, "camera_frame");
nh_ns.param<std::string>("depth_frame", depth_frame_id, "depth_frame");

nh_ns.param<std::string>("disparity_frame", disparity_frame_id, "dispairty_frame");

// Get parameters from launch file
nh_ns.getParam("resolution", resolution);
nh_ns.getParam("quality", quality);
Expand Down Expand Up @@ -814,6 +847,8 @@ namespace zed_wrapper {

string depth_cam_info_topic = "depth/camera_info";

string disparity_topic = "disparity/disparity_image";

string point_cloud_topic = "point_cloud/cloud_registered";
cloud_frame_id = camera_frame_id;

Expand All @@ -837,6 +872,8 @@ namespace zed_wrapper {
nh_ns.getParam("depth_topic", depth_topic);
nh_ns.getParam("depth_cam_info_topic", depth_cam_info_topic);

nh_ns.getParam("disparity_topic", disparity_topic);

nh_ns.getParam("point_cloud_topic", point_cloud_topic);

nh_ns.getParam("odometry_topic", odometry_topic);
Expand Down Expand Up @@ -918,6 +955,10 @@ namespace zed_wrapper {
pub_depth = it_zed.advertise(depth_topic, 1); //depth
NODELET_INFO_STREAM("Advertized on topic " << depth_topic);

// Disparity publisher
pub_disparity = nh.advertise<stereo_msgs::DisparityImage>(disparity_topic, 1);
NODELET_INFO_STREAM("Advertized on topic " << disparity_topic);

//PointCloud publisher
pub_cloud = nh.advertise<sensor_msgs::PointCloud2> (point_cloud_topic, 1);
NODELET_INFO_STREAM("Advertized on topic " << point_cloud_topic);
Expand Down