Merge pull request #45 from stereolabs/devel_foxy

XACRO and Launch parameters support
stereolabs · Jul 21, 2021 · b512dce · b512dce
2 parents 3468ba4 + 1099568
commit b512dce
Show file tree

Hide file tree

Showing 23 changed files with 6,112 additions and 4,925 deletions.
diff --git a/last_changes.md b/last_changes.md
@@ -1,6 +1,24 @@
 LATEST CHANGES
 ==============
 
+2020-07-21
+----------
+- Add xacro support for automatic URDF configuration
+- Reworked launch files to support xacro and launch parameters
+    - Use `ros2 launch zed_wrapper <launch_file> -s` to retrieve all the available parameters
+- Add `svo_path:=<full path to SVO file>` as input for all the launch files to start the node using an SVO as input without modifying 'common.yaml`
+
+2020-07-15
+----------
+- Improved diagnostic information adding elaboration time on all the main tasks
+- Improved diagnostic time and frequencies calculation
+- Added StopWatch to sl_tools
+
+2020-07-14
+----------
+- Enabled Diagnostic status publishing
+- Changed the default values of the QoS parameter reliability for all topics from BEST_EFFORT to RELIABLE to guarantee compatibility with all ROS2 tools
+
 2020-07-12
 ----------
 - Fixed tab error in `zedm.yaml`

diff --git a/zed_components/CMakeLists.txt b/zed_components/CMakeLists.txt
@@ -60,6 +60,8 @@ set(DEPENDENCIES
     stereo_msgs
     zed_interfaces
     std_srvs
+    diagnostic_msgs
+    diagnostic_updater
 )
 
 find_package(ament_cmake REQUIRED)
@@ -78,12 +80,8 @@ find_package(sensor_msgs REQUIRED)
 find_package(stereo_msgs REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(std_srvs REQUIRED)
-
-# Add a define to fix compile errors under "eloquent"
-if($ENV{ROS_DISTRO} STREQUAL "eloquent")
-    add_definitions(-DROS_ELOQUENT)
-endif()
-
+find_package(diagnostic_msgs REQUIRED)
+find_package(diagnostic_updater REQUIRED)
 
 ###############################################################################
 #Add all files in subdirectories of the project in

diff --git a/zed_components/package.xml b/zed_components/package.xml
@@ -29,6 +29,9 @@
     <build_depend>tf2_geometry_msgs</build_depend>
     <build_depend>image_transport</build_depend>
     <build_depend>std_srvs</build_depend>
+    <build_depend>diagnostic_msgs</build_depend>
+    <build_depend>diagnostic_updater</build_depend>
+
 
     <exec_depend>launch_ros</exec_depend>
     <exec_depend>zed_interfaces</exec_depend>
@@ -45,6 +48,8 @@
     <exec_depend>tf2_ros</exec_depend>
     <exec_depend>tf2_geometry_msgs</exec_depend>
     <exec_depend>image_transport</exec_depend>
+    <exec_depend>diagnostic_msgs</exec_depend>
+    <exec_depend>diagnostic_updater</exec_depend>
 
     <test_depend>ament_cmake_pytest</test_depend>
     <test_depend>ament_lint_auto</test_depend>

diff --git a/zed_components/src/tools/include/sl_tools.h b/zed_components/src/tools/include/sl_tools.h
@@ -1,18 +1,18 @@
-/******************************************************************************** 
+/********************************************************************************
  * MIT License
- * 
+ *
  * Copyright (c) 2020 Stereolabs
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
- * 
+ *
  * The above copyright notice and this permission notice shall be included in all
  * copies or substantial portions of the Software.
- * 
+ *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -45,117 +45,137 @@
 //
 ///////////////////////////////////////////////////////////////////////////
 
-#include <sensor_msgs/msg/image.hpp>
 #include <rclcpp/clock.hpp>
+#include <sensor_msgs/msg/image.hpp>
 #include <sl/Camera.hpp>
 #include <string>
-
-namespace sl_tools {
-
-    /*! \brief Check if a ZED camera is ready
-    * \param serial_number : the serial number of the camera to be checked
-    */
-    int checkCameraReady(unsigned int serial_number);
-
-    /*! \brief Get ZED camera properties
-    * \param serial_number : the serial number of the camera
-    */
-    sl::DeviceProperties getZEDFromSN(unsigned int serial_number);
-
-
-
-    std::vector<float> 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 Get Stereolabs SDK version
-     * \param major : major value for version
-     * \param minor : minor value for version
-     * \param sub_minor _ sub_minor value for version
-     */
-    std::string getSDKVersion(int& major, int& minor, int& sub_minor);
-
-    /*! \brief Convert StereoLabs timestamp to ROS timestamp
-     *  \param t : Stereolabs timestamp to be converted
-     *  \param t : ROS2 clock type
-     */
-    rclcpp::Time slTime2Ros(sl::Timestamp t, rcl_clock_type_t clock_type = RCL_ROS_TIME);
-
-    /*! \brief sl::Mat to ros message conversion
-     * \param img : the image to publish
-     * \param frameId : the id of the reference frame of the image
-     * \param t : rclcpp ros::Time to stamp the image
-     */
-    std::shared_ptr<sensor_msgs::msg::Image> imageToROSmsg(sl::Mat& img, std::string frameId, rclcpp::Time t);
-
-    /*! \brief sl::Mat to ros message conversion
-     * \param left : the left image to convert and stitch
-     * \param right : the right image to convert and stitch
-     * \param frameId : the id of the reference frame of the image
-     * \param t : rclcpp rclcpp::Time to stamp the image
-     */
-    std::shared_ptr<sensor_msgs::msg::Image> imagesToROSmsg(sl::Mat& left, sl::Mat& right, std::string frameId, rclcpp::Time t);
-
-    /*! \brief qos value to string
-     * \param qos the value to convert
-     */
-    std::string qos2str(rmw_qos_history_policy_t qos);
-
-    /*! \brief qos value to string
-     * \param qos the value to convert
-     */
-    std::string qos2str(rmw_qos_reliability_policy_t qos);
-
-    /*! \brief qos value to string
-     * \param qos the value to convert
-     */
-    std::string qos2str(rmw_qos_durability_policy_t qos);
-
-    /*!
-         * \brief The CSmartMean class is used to
-         * make a mobile window mean of a sequence of values
-         * and reject outliers.
-         * Tutorial:
-         * https://www.myzhar.com/blog/tutorials/tutorial-exponential-weighted-average-good-moving-windows-average/
-         */
-    class SmartMean {
-      public:
-        SmartMean(int winSize);
-
-        int getValCount() {
-            return mValCount;   ///< Return the number of values in the sequence
-        }
-
-        double getMean() {
-            return mMean;   ///< Return the updated mean
-        }
-
-        /*!
-         * \brief addValue
-         * Add a value to the sequence
-         * \param val value to be added
-         * \return mean value
-         */
-        double addValue(double val);
-
-      private:
-        int mWinSize; ///< The size of the window (number of values ti evaluate)
-        int mValCount; ///< The number of values in sequence
-
-        double mMeanCorr; ///< Used for bias correction
-        double mMean;     ///< The mean of the last \ref mWinSize values
-
-        double mGamma; ///< Weight value
-    };
-
-  /*! \brief check if ZED2 or ZED2i
-  * \param camModel the model to check
-  */
-  bool isZED2OrZED2i(sl::MODEL camModel);
-
-} // namespace
-
-#endif // SL_TOOLS_H
+#include <chrono>
+
+namespace sl_tools
+{
+/*! \brief Check if a ZED camera is ready
+ * \param serial_number : the serial number of the camera to be checked
+ */
+int checkCameraReady(unsigned int serial_number);
+
+/*! \brief Get ZED camera properties
+ * \param serial_number : the serial number of the camera
+ */
+sl::DeviceProperties getZEDFromSN(unsigned int serial_number);
+
+std::vector<float> 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 Get Stereolabs SDK version
+ * \param major : major value for version
+ * \param minor : minor value for version
+ * \param sub_minor _ sub_minor value for version
+ */
+std::string getSDKVersion(int& major, int& minor, int& sub_minor);
+
+/*! \brief Convert StereoLabs timestamp to ROS timestamp
+ *  \param t : Stereolabs timestamp to be converted
+ *  \param t : ROS2 clock type
+ */
+rclcpp::Time slTime2Ros(sl::Timestamp t, rcl_clock_type_t clock_type = RCL_ROS_TIME);
+
+/*! \brief check if ZED2 or ZED2i
+ * \param camModel the model to check
+ */
+bool isZED2OrZED2i(sl::MODEL camModel);
+
+/*! \brief sl::Mat to ros message conversion
+ * \param img : the image to publish
+ * \param frameId : the id of the reference frame of the image
+ * \param t : rclcpp ros::Time to stamp the image
+ */
+std::shared_ptr<sensor_msgs::msg::Image> imageToROSmsg(sl::Mat& img, std::string frameId, rclcpp::Time t);
+
+/*! \brief sl::Mat to ros message conversion
+ * \param left : the left image to convert and stitch
+ * \param right : the right image to convert and stitch
+ * \param frameId : the id of the reference frame of the image
+ * \param t : rclcpp rclcpp::Time to stamp the image
+ */
+std::shared_ptr<sensor_msgs::msg::Image> imagesToROSmsg(sl::Mat& left, sl::Mat& right, std::string frameId,
+                                                        rclcpp::Time t);
+
+/*! \brief qos value to string
+ * \param qos the value to convert
+ */
+std::string qos2str(rmw_qos_history_policy_t qos);
+
+/*! \brief qos value to string
+ * \param qos the value to convert
+ */
+std::string qos2str(rmw_qos_reliability_policy_t qos);
+
+/*! \brief qos value to string
+ * \param qos the value to convert
+ */
+std::string qos2str(rmw_qos_durability_policy_t qos);
+
+/*!
+ * \brief The CSmartMean class is used to
+ * make a mobile window mean of a sequence of values
+ * and reject outliers.
+ * Tutorial:
+ * https://www.myzhar.com/blog/tutorials/tutorial-exponential-weighted-average-good-moving-windows-average/
+ */
+class SmartMean
+{
+public:
+  SmartMean(int winSize);
+
+  int getValCount()
+  {
+    return mValCount;  ///< Return the number of values in the sequence
+  }
+
+  double getMean()
+  {
+    return mMean;  ///< Return the updated mean
+  }
+
+  /*!
+   * \brief addValue
+   * Add a value to the sequence
+   * \param val value to be added
+   * \return mean value
+   */
+  double addValue(double val);
+
+private:
+  int mWinSize;   ///< The size of the window (number of values ti evaluate)
+  int mValCount;  ///< The number of values in sequence
+
+  double mMeanCorr;  ///< Used for bias correction
+  double mMean;      ///< The mean of the last \ref mWinSize values
+
+  double mGamma;  ///< Weight value
+};
+
+/**
+ * @brief Stop Timer used to measure time intervals
+ *
+ */
+class StopWatch
+{
+public:
+  StopWatch();
+  ~StopWatch(){};
+
+  void tic();    //!< Set the beginning time
+  double toc();  //!< Returns the seconds elapsed from the last tic
+
+private:
+  std::chrono::steady_clock::time_point mStartTime;
+};
+
+}  // namespace sl_tools
+
+#endif  // SL_TOOLS_H
diff --git a/zed_components/src/tools/src/sl_tools.cpp b/zed_components/src/tools/src/sl_tools.cpp
@@ -419,4 +419,21 @@ bool isZED2OrZED2i(sl::MODEL camModel){
     return false;
 }
 
+StopWatch::StopWatch()
+{
+    tic(); // Start the timer at creation
+}
+
+void StopWatch::tic()
+{
+    mStartTime = std::chrono::steady_clock::now(); // Set the start time point
+}
+
+double StopWatch::toc()
+{
+    auto now = std::chrono::steady_clock::now();
+    double elapsed_usec = std::chrono::duration_cast<std::chrono::microseconds>(now - mStartTime).count();
+    return elapsed_usec/1e6;
+}
+
 } // namespace