Add dynamic confidence

CMakeLists.txt

@@ -39,6 +39,11 @@ find_package(catkin REQUIRED COMPONENTS
   rosconsole
   sensor_msgs
   cv_bridge
+  dynamic_reconfigure
+)
+
+generate_dynamic_reconfigure_options(
+  cfg/Zed.cfg
 )
 
 catkin_package(
@@ -49,6 +54,7 @@ catkin_package(
     opencv2
     cv_bridge
     image_transport
+    dynamic_reconfigure
 )
 
 ###############################################################################
@@ -73,6 +79,7 @@ link_directories(${OpenCV_LIBRARY_DIRS})
 
 add_definitions(-std=c++11)# -m64) #-Wall)
 
+
 add_executable(
   zed_wrapper_node
   src/zed_wrapper_node.cpp
@@ -85,5 +92,11 @@ target_link_libraries(
 	${CUDA_LIBRARIES} ${CUDA_nppi_LIBRARY} ${CUDA_npps_LIBRARY}
         ${OpenCV_LIBS}  
     )
+
+add_dependencies(zed_wrapper_node ${PROJECT_NAME}_gencfg)
 ###############################################################################
 
+#Add all files in subdirectories of the project in
+# a dummy_target so qtcreator have access to all files
+FILE(GLOB_RECURSE extra_files ${CMAKE_SOURCE_DIR}/*)
+add_custom_target(dummy_${PROJECT_NAME} SOURCES ${extra_files})

cfg/Zed.cfg

@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+PACKAGE = "zed_ros_wrapper"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("confidence", int_t, 0, "Confidence threshold, the lower the better", 80,  1, 100)
+
+exit(gen.generate(PACKAGE, "zed_ros_wrapper", "Zed"))

launch/zed_depth.launch

@@ -1,23 +1,25 @@
 <launch>
-
+
+  <include file="$(find zed_wrapper)/launch/zed_tf.launch" />
+
   <arg name="computeDepth" value="1"/>
   <arg name="svo_file" default=""/>
 
-  <node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" args="$(arg computeDepth) $(arg svo_file)" output="screen">
-
-    <param name="resolution"            value="3" />
-    <param name="quality"               value="1" />
-    <param name="sensing_mode"          value="1" />
-    <param name="frame_rate"            value="25" />
-  	
-	<!-- <param name="left_topic"            value="rgb/image_raw" /> -->
-	<!-- <param name="second_topic"          value="depth/image_raw" /> -->
-	<param name="left_topic"            value="rgb/image_rect" />
-	<param name="second_topic"          value="depth/image_rect" />
-	  <param name="left_cam_info_topic"   value="/camera/rgb/camera_info" />
-	  <param name="second_cam_info_topic" value="/camera/depth/camera_info" />
-	  <param name="left_frame_id"         value="/zed_rgb_optical_frame" />
-	  <param name="second_frame_id"       value="/zed_depth_optical_frame" />
-
-  </node>
+  <group ns="camera">
+    <node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" args="$(arg computeDepth) $(arg svo_file)" output="screen">
+
+      <param name="resolution"            value="2" />
+      <param name="quality"               value="2" />
+      <param name="sensing_mode"          value="1" />
+      <param name="frame_rate"            value="15" />
+
+      <param name="left_topic"            value="rgb/image_rect_color" />
+      <param name="second_topic"          value="depth_registered/image_raw" />
+      <param name="left_cam_info_topic"   value="rgb/camera_info" />
+      <param name="second_cam_info_topic" value="depth/camera_info" />
+      <param name="left_frame_id"         value="/zed_optical_frame" />
+      <param name="second_frame_id"       value="/zed_optical_frame" />
+
+    </node>
+  </group>
 </launch>

launch/zed_stereo.launch

@@ -1,21 +1,25 @@
 <launch>
-
+
+  <include file="$(find zed_wrapper)/launch/zed_tf.launch" />
+
   <arg name="computeDepth" default="0"/>
   <arg name="svo_file" default=""/>
-
-  <node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" args="$(arg computeDepth) $(arg svo_file)" output="screen">
-
-    <param name="resolution"            value="3" />
-    <param name="quality"               value="1" />
-    <param name="sensing_mode"          value="1" />
-    <param name="frame_rate"            value="25" />
-
-    <param name="left_topic"            value="/stereo/left_image" />
-	  <param name="second_topic"          value="/stereo/right_image" />
-	  <param name="left_cam_info_topic"   value="/stereo/left_camera_info" />
-	  <param name="second_cam_info_topic" value="/stereo/right_camera_info" />
-	  <param name="left_frame_id"         value="/zed_left_frame" />
-	  <param name="second_frame_id"       value="/zed_right_frame" />
-
-  </node>
+
+  <group ns="/camera">
+    <node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" args="$(arg computeDepth) $(arg svo_file)" output="screen">
+
+      <param name="resolution"            value="2" />
+      <param name="quality"               value="2" />
+      <param name="sensing_mode"          value="1" />
+      <param name="frame_rate"            value="15" />
+
+      <param name="left_topic"            value="left/image_raw" />
+      <param name="second_topic"          value="right/image_raw" />
+      <param name="left_cam_info_topic"   value="left/camera_info" />
+      <param name="second_cam_info_topic" value="right/camera_info" />
+      <param name="left_frame_id"         value="/zed_optical_frame" />
+      <param name="second_frame_id"       value="/zed_optical_frame" />
+
+    </node>
+  </group>
 </launch>

launch/zed_tf.launch

@@ -0,0 +1,13 @@
+<launch>
+  <arg name="camera" default="zed"/>
+  <arg name="tf_prefix" default="" />
+  <arg name="pi/2" value="1.5707963267948966" />
+  <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
+
+  <node pkg="tf" type="static_transform_publisher" name="$(arg camera)_base_link1"
+    args="0 0 0 0 0 0 1 camera_link $(arg tf_prefix)/$(arg camera)_frame 100" />
+
+  <node pkg="tf" type="static_transform_publisher" name="$(arg camera)_base_link2"
+    args="$(arg optical_rotate) $(arg tf_prefix)/$(arg camera)_frame $(arg tf_prefix)/$(arg camera)_optical_frame  100" />
+
+</launch>

package.xml

@@ -16,13 +16,15 @@
   <build_depend>opencv2</build_depend>
   <build_depend>cv_bridge</build_depend>
   <build_depend>image_transport</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
 
   <run_depend>roscpp</run_depend>
   <run_depend>rosconsole</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>opencv2</run_depend>
   <run_depend>cv_bridge</run_depend>
   <run_depend>image_transport</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
 
   <export>
   </export>

records/record_depth.sh

@@ -0,0 +1,2 @@
+#!/bin/bash
+rosbag record /camera/rgb/camera_info /camera/depth_registered/image_raw/compressedDepth /camera/rgb/image_rect_color/compressed /tf

records/record_stereo.sh

@@ -0,0 +1,2 @@
+#!/bin/bash
+rosbag record /camera/left/camera_info /camera/right/camera_info /camera/left/image_raw/compressed /camera/right/image_raw/compressed /tf

src/zed_wrapper_node.cpp

@@ -43,6 +43,9 @@
 #include <cv_bridge/cv_bridge.h>
 #include <sensor_msgs/image_encodings.h>
 #include <image_transport/image_transport.h>
+#include <dynamic_reconfigure/server.h>
+#include <zed_wrapper/ZedConfig.h>
+
 
 //opencv includes
 #include <opencv2/core/core.hpp>
@@ -57,6 +60,7 @@ using namespace sl::zed;
 using namespace std;
 
 int computeDepth;
+int confidence;
 
 // Function to publish left and depth/right images
 
@@ -158,6 +162,12 @@ void fillCamInfo(Camera* zed, sensor_msgs::CameraInfoPtr left_cam_info_msg, sens
     second_cam_info_msg->header.frame_id = second_frame_id;
 }
 
+void callback(zed_ros_wrapper::ZedConfig &config, uint32_t level) {
+  ROS_INFO("Reconfigure confidence : %d",
+            config.confidence);
+  confidence = config.confidence;
+}
+
 // Main function
 
 int main(int argc, char **argv) {
@@ -226,6 +236,13 @@ int main(int argc, char **argv) {
         return 1;
     }
 
+    dynamic_reconfigure::Server<zed_ros_wrapper::ZedConfig> server;
+    dynamic_reconfigure::Server<zed_ros_wrapper::ZedConfig>::CallbackType f;
+
+    f = boost::bind(&callback, _1, _2);
+    server.setCallback(f);
+
+    confidence = 80;
     int width = zed->getImageSize().width;
     int height = zed->getImageSize().height;
     ROS_DEBUG_STREAM("Image size : " << width << "x" << height);
@@ -258,7 +275,7 @@ int main(int argc, char **argv) {
     fillCamInfo(zed, left_cam_info_msg, second_cam_info_msg, left_frame_id, second_frame_id);
 
     ros::Rate loop_rate(rate);
-
+    ros::Time old_t = ros::Time::now();
     bool old_image = false;
 
     try {
@@ -270,17 +287,39 @@ int main(int argc, char **argv) {
                 // Get current time
                 ros::Time t = ros::Time::now();
 
-                if (computeDepth)
+                if (computeDepth) {
+                    int actual_confidence = zed->getConfidenceThreshold();
+                    if(actual_confidence != confidence)
+                        zed->setConfidenceThreshold(confidence);
                     old_image = zed->grab(static_cast<sl::zed::SENSING_MODE> (sensing_mode), true, true);
+                }
                 else
                     old_image = zed->grab(static_cast<sl::zed::SENSING_MODE> (sensing_mode), false, false);
 
                 if (old_image) {
-                    // Wait for a new image to proceed
+                    ROS_WARN("Wait for a new image to proceed");
                     std::this_thread::sleep_for(std::chrono::milliseconds(2));
+                    if ((t - old_t).toSec() > 5) {
+                      ROS_INFO("Reinit camera");
+                      ERRCODE err;
+                      if (computeDepth)
+                          err = zed->init(static_cast<sl::zed::MODE> (quality), -1, true);
+                      else
+                          err = zed->init(sl::zed::MODE::NONE, -1, true);
+
+                      // Quit if an error occurred
+                      if (err != SUCCESS) {
+                          //ERRCODE display
+                          ROS_ERROR_STREAM(errcode2str(err));
+                          delete zed;
+                          return 1;
+                      }
+                    }
                     continue;
                 }
 
+                old_t = ros::Time::now();
+
                 // Retrieve RGBA Left image
                 slMat2cvMat(zed->retrieveImage(sl::zed::SIDE::LEFT)).copyTo(leftImRGBA);
                 // Convert to RGB