Object detection module completed

zed_components/src/include/visibility_control.h

@@ -1,17 +1,3 @@
-// Copyright 2016 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
 #ifndef ZED_COMPONENTS__VISIBILITY_CONTROL_H_
 #define ZED_COMPONENTS__VISIBILITY_CONTROL_H_
 

zed_components/src/zed_camera/include/zed_camera_component.hpp

@@ -28,8 +28,8 @@
 
 #include <sl/Camera.hpp>
 
-#include <zed_interfaces/msg/objects.hpp>
-#include <zed_interfaces/msg/object_stamped.hpp>
+#include <zed_interfaces/msg/objects_stamped.hpp>
+#include <zed_interfaces/msg/object.hpp>
 
 
 namespace stereolabs {
@@ -52,7 +52,7 @@ typedef std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::TransformStamped>>
 typedef std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odomPub;
 typedef std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Path>> pathPub;
 
-typedef std::shared_ptr<rclcpp::Publisher<zed_interfaces::msg::Objects>> objPub;
+typedef std::shared_ptr<rclcpp::Publisher<zed_interfaces::msg::ObjectsStamped>> objPub;
 
 typedef std::unique_ptr<sensor_msgs::msg::Image> imageMsgPtr;
 typedef std::shared_ptr<sensor_msgs::msg::CameraInfo> camInfoMsgPtr;
@@ -69,7 +69,7 @@ typedef std::shared_ptr<geometry_msgs::msg::TransformStamped> transfMsgPtr;
 typedef std::unique_ptr<nav_msgs::msg::Odometry> odomMsgPtr;
 typedef std::unique_ptr<nav_msgs::msg::Path> pathMsgPtr;
 
-typedef std::unique_ptr<zed_interfaces::msg::Objects> objDetMsgPtr;
+typedef std::unique_ptr<zed_interfaces::msg::ObjectsStamped> objDetMsgPtr;
 
 //typedef rclcpp::Service<stereolabs_zed_interfaces::srv::ResetOdometry>::SharedPtr resetOdomSrvPtr;
 //typedef rclcpp::Service<stereolabs_zed_interfaces::srv::RestartTracking>::SharedPtr restartTrkSrvPtr;

zed_components/src/zed_camera/src/zed_camera_component.cpp

@@ -1435,7 +1435,7 @@ bool ZedCamera::startCamera() {
         RCLCPP_INFO(get_logger(), "*** SVO OPENING ***");
 
         mInitParams.input.setFromSVOFile(mSvoFilepath.c_str());
-        mInitParams.svo_real_time_mode = true;
+        mInitParams.svo_real_time_mode = false;
         mSvoMode = true;
     } else {
         RCLCPP_INFO(get_logger(), "*** CAMERA OPENING ***");
@@ -1909,7 +1909,7 @@ bool ZedCamera::startObjDetect() {
     }
 
     if(!mPubObjDet) {
-        mPubObjDet = create_publisher<zed_interfaces::msg::Objects>(mObjectDetTopic, mObjDetQos);
+        mPubObjDet = create_publisher<zed_interfaces::msg::ObjectsStamped>(mObjectDetTopic, mObjDetQos);
         RCLCPP_INFO_STREAM(get_logger(), "Advertised on topic " << mPubObjDet->get_topic_name());
     }
 
@@ -2325,7 +2325,7 @@ void ZedCamera::threadFunc_zedGrab() {
             processOdometry();
             processPose();
 
-            if(mCamRealModel == sl::MODEL::ZED || !mPublishImuTF) {
+            if(mCamRealModel == sl::MODEL::ZED || !mPublishImuTF || mSvoMode) {
                 publishTFs(mFrameTimestamp);
             }
         }
@@ -2847,7 +2847,7 @@ void ZedCamera::callback_pubSensorsData() {
 
 
     // Publish TF at the same frequency of IMU data, so they are always synchronized
-    if(new_imu_data && mPublishImuTF) {
+    if(new_imu_data && mPublishImuTF && !mSvoMode) {
         publishTFs(ts_imu);
     }
 }
@@ -3403,18 +3403,15 @@ void ZedCamera::processDetectedObjects(rclcpp::Time t) {
 
     size_t objCount = objects.object_list.size();
 
-    objDetMsgPtr objMsg = std::make_unique<zed_interfaces::msg::Objects>();
+    objDetMsgPtr objMsg = std::make_unique<zed_interfaces::msg::ObjectsStamped>();
 
-    objMsg->objects.resize(objCount);
+    objMsg->header.stamp = t;
+    objMsg->header.frame_id = mLeftCamFrameId;
 
-    std_msgs::msg::Header header;
-    header.stamp = t;
-    header.frame_id = mLeftCamFrameId;
+    objMsg->objects.resize(objCount);
 
     size_t idx = 0;
     for (auto data : objects.object_list) {
-        objMsg->objects[idx].header = header;
-
         objMsg->objects[idx].label = sl::toString(data.label).c_str();
         objMsg->objects[idx].label_id = data.id;
         objMsg->objects[idx].confidence = data.confidence;

zed_interfaces/CMakeLists.txt

@@ -23,8 +23,8 @@ add_custom_target(all_${PROJECT_NAME}_files SOURCES ${extra_files})
 ###############################################################################
 
 set(MSG_FILES
-    "msg/Objects.msg"
-    "msg/ObjectStamped.msg"
+    "msg/Object.msg"
+    "msg/ObjectsStamped.msg"
     "msg/Keypoint2Di.msg"
     "msg/Keypoint2Df.msg"
     "msg/Keypoint3D.msg"

zed_interfaces/msg/ObjectStamped.msg → zed_interfaces/msg/Object.msg

@@ -1,6 +1,3 @@
-# Standard Header
-std_msgs/Header header
-
 # Object label
 string label
 

zed_interfaces/msg/ObjectsStamped.msg

@@ -0,0 +1,5 @@
+# Standard Header
+std_msgs/Header header
+
+# Array of `object_stamped` topics
+zed_interfaces/Object[] objects

zed_wrapper/config/common.yaml

@@ -7,7 +7,7 @@
 /**:
     ros__parameters:
         general:
-            svo_file:                   '' #'/home/walter/Desktop/HD2K_SN28946376_11-31-00_Walter_outdoor_loop_closing.svo'
+            svo_file:                   '' #'/media/walter/Stereolabs-SSD/SVO/Walter_outdoor/HD2K_SN28946376_11-31-00_Walter_outdoor_loop_closing.svo'
             svo_loop:                   false                               # Enable loop mode when using an SVO as input source
             debug_mode:                 false
             camera_timeout_sec:         5

zed_wrapper/config/zed2.yaml

@@ -15,24 +15,24 @@
 
         pos_tracking:
             imu_fusion:                 true            # enable/disable IMU fusion. When set to false, only the optical odometry will be used.
-            publish_imu_tf:             true           # enable/disable the static IMU TF broadcasting
+            publish_imu_tf:             true            # enable/disable the static IMU TF broadcasting
 
         sensors:
-            sensors_timestamp_sync:     false           # Synchronize Sensors messages timestamp with latest received frame
-            qos_history:        0                       # '0': KEEP_LAST - '1': KEEP_ALL
-            qos_depth:          10                      # Queue size if using KEEP_LAST
-            qos_reliability:    1                       # '0': BEST_EFFORT - '1': RELIABLE
-            qos_durability:     1                       # '0': TRANSIENT_LOCAL - '1': VOLATILE
+            sensors_timestamp_sync:     false            # Synchronize Sensors messages timestamp with latest received frame
+            qos_history:                0               # '0': KEEP_LAST - '1': KEEP_ALL
+            qos_depth:                  10              # Queue size if using KEEP_LAST
+            qos_reliability:            1               # '0': BEST_EFFORT - '1': RELIABLE
+            qos_durability:             1               # '0': TRANSIENT_LOCAL - '1': VOLATILE
 
         object_detection:
             od_enabled:                 false           # True to enable Object Detection [only ZED 2]
             confidence_threshold:       50              # Minimum value of the detection confidence of an object [0,100]
             object_tracking_enabled:    true            # Enable/disable the tracking of the detected objects
             people_detection:           true            # Enable/disable the detection of persons
             vehicle_detection:          false           # Enable/disable the detection of vehicles
-            qos_history:        0                       # '0': KEEP_LAST - '1': KEEP_ALL
-            qos_depth:          10                      # Queue size if using KEEP_LAST
-            qos_reliability:    1                       # '0': BEST_EFFORT - '1': RELIABLE
-            qos_durability:     1                       # '0': TRANSIENT_LOCAL - '1': VOLATILE
+            qos_history:                0               # '0': KEEP_LAST - '1': KEEP_ALL
+            qos_depth:                  10              # Queue size if using KEEP_LAST
+            qos_reliability:            1               # '0': BEST_EFFORT - '1': RELIABLE
+            qos_durability:             1               # '0': TRANSIENT_LOCAL - '1': VOLATILE