Code reformatting

 * moved nodelet code in "nodelet" folder
 * split function declarations and definitions moving declarations in zed_wrapper_nodelet.h
 * fixed warnings in debug messages

CMakeLists.txt

@@ -76,6 +76,15 @@ catkin_package(
     tf2_geometry_msgs
     pcl_conversions
 )
+
+###############################################################################
+# SOURCES
+
+set(NODE_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/zed_wrapper_node.cpp)
+set(NODELET_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/nodelet/src/zed_wrapper_nodelet.cpp)
+
+###############################################################################
+
 ###############################################################################
 # INCLUDES
 
@@ -86,6 +95,7 @@ include_directories(
 	${ZED_INCLUDE_DIRS}
 	${OpenCV_INCLUDE_DIRS}
 	${PCL_INCLUDE_DIRS}
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/nodelet/include
 )
 
 link_directories(${ZED_LIBRARY_DIR})
@@ -114,11 +124,11 @@ set(LINK_LIBRARIES
   ${OpenCV_LIBS}
   ${PCL_LIBRARIES})
 
-add_library(ZEDWrapper src/zed_wrapper_nodelet.cpp)
+add_library(ZEDWrapper ${NODELET_SRC})
 target_link_libraries(ZEDWrapper ${LINK_LIBRARIES})
 add_dependencies(ZEDWrapper ${PROJECT_NAME}_gencfg)
 
-add_executable(zed_wrapper_node src/zed_wrapper_node.cpp)
+add_executable(zed_wrapper_node ${NODE_SRC})
 target_link_libraries(zed_wrapper_node ZEDWrapper ${LINK_LIBRARIES})
 add_dependencies(zed_wrapper_node ${PROJECT_NAME}_gencfg)
 

src/nodelet/include/zed_wrapper_nodelet.h

@@ -0,0 +1,251 @@
+///////////////////////////////////////////////////////////////////////////
+//
+// Copyright (c) 2017, STEREOLABS.
+//
+// All rights reserved.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+///////////////////////////////////////////////////////////////////////////
+
+/****************************************************************************************************
+ ** This sample is a wrapper for the ZED library in order to use the ZED Camera with ROS.          **
+ ** A set of parameters can be specified in the launch file.                                       **
+ ****************************************************************************************************/
+
+#include <csignal>
+#include <cstdio>
+#include <math.h>
+#include <limits>
+#include <thread>
+#include <chrono>
+#include <memory>
+#include <sys/stat.h>
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/distortion_models.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/Imu.h>
+#include <image_transport/image_transport.h>
+#include <dynamic_reconfigure/server.h>
+#include <zed_wrapper/ZedConfig.h>
+#include <nav_msgs/Odometry.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include <boost/make_shared.hpp>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <sl/Camera.hpp>
+
+using namespace std;
+
+namespace zed_wrapper {
+
+    class ZEDWrapperNodelet : public nodelet::Nodelet {
+
+    public:
+        ZEDWrapperNodelet();
+        virtual ~ZEDWrapperNodelet();
+
+    private:
+        virtual void onInit();
+        void device_poll();        
+
+    protected:
+        /* \brief Convert an sl:Mat to a cv::Mat
+         * \param mat : the sl::Mat to convert
+         */
+        cv::Mat toCVMat(sl::Mat &mat);
+
+        cv::Mat convertRodrigues(sl::float3 r);
+
+        /* \brief Test if a file exist
+         * \param name : the path to the file
+         */
+        bool file_exist(const std::string& name);
+
+        /* \brief Image to ros message conversion
+         * \param img : the image to publish
+         * \param encodingType : the sensor_msgs::image_encodings encoding type
+         * \param frameId : the id of the reference frame of the image
+         * \param t : the ros::Time to stamp the image
+         */
+        sensor_msgs::ImagePtr imageToROSmsg(cv::Mat img, const std::string encodingType, std::string frameId, ros::Time t);
+
+        /* \brief Publish the pose of the camera with a ros Publisher
+         * \param base_transform : Transformation representing the camera pose from base frame
+         * \param pub_odom : the publisher object to use
+         * \param odom_frame_id : the id of the reference frame of the pose
+         * \param t : the ros::Time to stamp the image
+         */
+        void publishOdom(tf2::Transform base_transform, ros::Publisher &pub_odom, string odom_frame_id, ros::Time t);
+
+        /* \brief Publish the pose of the camera as a transformation
+         * \param base_transform : Transformation representing the camera pose from base frame
+         * \param trans_br : the TransformBroadcaster object to use
+         * \param odometry_transform_frame_id : the id of the transformation
+         * \param t : the ros::Time to stamp the image
+         */
+        void publishTrackedFrame(tf2::Transform base_transform, tf2_ros::TransformBroadcaster &trans_br, string odometry_transform_frame_id, ros::Time t);
+
+        /* \brief Publish a cv::Mat image with a ros Publisher
+         * \param img : the image to publish
+         * \param pub_img : the publisher object to use
+         * \param img_frame_id : the id of the reference frame of the image
+         * \param t : the ros::Time to stamp the image
+         */
+        void publishImage(cv::Mat img, image_transport::Publisher &pub_img, string img_frame_id, ros::Time t);
+
+        /* \brief Publish a cv::Mat depth image with a ros Publisher
+         * \param depth : the depth image to publish
+         * \param pub_depth : the publisher object to use
+         * \param depth_frame_id : the id of the reference frame of the depth image
+         * \param t : the ros::Time to stamp the depth image
+         */
+        void publishDepth(cv::Mat depth, image_transport::Publisher &pub_depth, string depth_frame_id, ros::Time t);
+
+        /* \brief Publish a pointCloud with a ros Publisher
+         * \param width : the width of the point cloud
+         * \param height : the height of the point cloud
+         * \param pub_cloud : the publisher object to use
+         */
+        void publishPointCloud(int width, int height, ros::Publisher &pub_cloud);
+
+        /* \brief Publish the informations of a camera with a ros Publisher
+         * \param cam_info_msg : the information message to publish
+         * \param pub_cam_info : the publisher object to use
+         * \param t : the ros::Time to stamp the message
+         */
+        void publishCamInfo(sensor_msgs::CameraInfoPtr cam_info_msg, ros::Publisher pub_cam_info, ros::Time t);
+
+        /* \brief Get the information of the ZED cameras and store them in an information message
+         * \param zed : the sl::zed::Camera* pointer to an instance
+         * \param left_cam_info_msg : the information message to fill with the left camera informations
+         * \param right_cam_info_msg : the information message to fill with the right camera informations
+         * \param left_frame_id : the id of the reference frame of the left camera
+         * \param right_frame_id : the id of the reference frame of the right camera
+         */
+        void fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr left_cam_info_msg, sensor_msgs::CameraInfoPtr right_cam_info_msg,
+                string left_frame_id, string right_frame_id, bool raw_param = false);        
+
+        /* \brief Callback to handle dynamic reconfigure events in ROS
+         */
+        void dynamicReconfCallback(zed_wrapper::ZedConfig &config, uint32_t level);
+
+        /* \brief Callback to publish IMU raw data with a ROS publisher.
+         * \param e : the ros::TimerEvent binded to the callback
+         */
+        void imuPubCallback(const ros::TimerEvent & e);
+
+    private:
+        ros::NodeHandle nh;
+        ros::NodeHandle nh_ns;
+        boost::shared_ptr<boost::thread> device_poll_thread;
+        image_transport::Publisher pub_rgb;
+        image_transport::Publisher pub_raw_rgb;
+        image_transport::Publisher pub_left;
+        image_transport::Publisher pub_raw_left;
+        image_transport::Publisher pub_right;
+        image_transport::Publisher pub_raw_right;
+        image_transport::Publisher pub_depth;
+        ros::Publisher pub_cloud;
+        ros::Publisher pub_rgb_cam_info;
+        ros::Publisher pub_left_cam_info;
+        ros::Publisher pub_right_cam_info;
+        ros::Publisher pub_rgb_cam_info_raw;
+        ros::Publisher pub_left_cam_info_raw;
+        ros::Publisher pub_right_cam_info_raw;
+        ros::Publisher pub_depth_cam_info;
+        ros::Publisher pub_odom;
+        ros::Publisher pub_imu;
+        ros::Timer  pub_imu_timer;
+
+        // tf
+        tf2_ros::TransformBroadcaster transform_odom_broadcaster;
+        std::string left_frame_id;
+        std::string right_frame_id;
+        std::string rgb_frame_id;
+        std::string depth_frame_id;
+        std::string cloud_frame_id;
+        std::string odometry_frame_id;
+        std::string base_frame_id;
+        std::string camera_frame_id;
+        std::string imu_frame_id;
+
+        // initialization Transform listener
+        boost::shared_ptr<tf2_ros::Buffer> tfBuffer;
+        boost::shared_ptr<tf2_ros::TransformListener> tf_listener;
+        bool publish_tf;
+
+        // Launch file parameters
+        int resolution;
+        int quality;
+        int sensing_mode;
+        int rate;
+        int gpu_id;
+        int zed_id;
+        int depth_stabilization;
+        std::string odometry_DB;
+        std::string svo_filepath;
+        double imu_pub_rate;
+
+        //Tracking variables
+        sl::Pose pose;
+        tf2::Transform base_transform;
+
+        // zed object
+        sl::InitParameters param;
+        sl::Camera zed;
+        unsigned int serial_number;
+        int user_cam_model; // Camera model set by ROS Param 
+
+        // flags
+        int confidence;
+        int exposure;
+        int gain;
+        bool autoExposure;
+        bool triggerAutoExposure;
+        bool computeDepth;
+        bool grabbing = false;
+        int openniDepthMode = 0; // 16 bit UC data in mm else 32F in m, for more info http://www.ros.org/reps/rep-0118.html
+
+        // Point cloud variables
+        sl::Mat cloud;
+        string point_cloud_frame_id = "";
+        ros::Time point_cloud_time;
+
+        // Dynamic reconfigure
+        boost::shared_ptr<dynamic_reconfigure::Server<zed_wrapper::ZedConfig>> server;
+
+    }; // class ZEDROSWrapperNodelet
+} // namespace
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(zed_wrapper::ZEDWrapperNodelet, nodelet::Nodelet);