Reset odometry service

zed_components/CMakeLists.txt

@@ -59,6 +59,7 @@ set(DEPENDENCIES
     sensor_msgs
     stereo_msgs
     zed_interfaces
+    std_srvs
 )
 
 find_package(ament_cmake REQUIRED)
@@ -76,6 +77,7 @@ find_package(nav_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(stereo_msgs REQUIRED)
 find_package(image_transport REQUIRED)
+find_package(std_srvs REQUIRED)
 
 
 ###############################################################################

zed_components/src/zed_camera/include/zed_camera_component.hpp

@@ -26,6 +26,9 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <nav_msgs/msg/path.hpp>
 
+#include <std_srvs/srv/trigger.hpp>
+#include <std_srvs/srv/set_bool.hpp>
+
 #include <sl/Camera.hpp>
 
 #include <zed_interfaces/msg/objects_stamped.hpp>
@@ -71,7 +74,7 @@ typedef std::unique_ptr<nav_msgs::msg::Path> pathMsgPtr;
 
 typedef std::unique_ptr<zed_interfaces::msg::ObjectsStamped> objDetMsgPtr;
 
-//typedef rclcpp::Service<stereolabs_zed_interfaces::srv::ResetOdometry>::SharedPtr resetOdomSrvPtr;
+typedef rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr resetOdomSrvPtr;
 //typedef rclcpp::Service<stereolabs_zed_interfaces::srv::RestartTracking>::SharedPtr restartTrkSrvPtr;
 //typedef rclcpp::Service<stereolabs_zed_interfaces::srv::SetPose>::SharedPtr setPoseSrvPtr;
 //typedef rclcpp::Service<stereolabs_zed_interfaces::srv::StartSvoRecording>::SharedPtr startSvoRecSrvPtr;
@@ -89,6 +92,7 @@ class ZedCamera : public rclcpp::Node
 protected:
     // ----> Initialization functions
     void initParameters();
+    void initServices();
 
     void getGeneralParams();
     void getVideoParams();
@@ -117,6 +121,10 @@ class ZedCamera : public rclcpp::Node
     void callback_pubSensorsData();
     void callback_pubFusedPc();
     rcl_interfaces::msg::SetParametersResult callback_paramChange(std::vector<rclcpp::Parameter> parameters);
+
+    void callback_resetOdometry(const std::shared_ptr<rmw_request_id_t> request_header,
+                                const std::shared_ptr<std_srvs::srv::Trigger_Request> req,
+                                std::shared_ptr<std_srvs::srv::Trigger_Response> res);
     // <---- Callbacks
 
     // ----> Thread functions
@@ -235,7 +243,7 @@ class ZedCamera : public rclcpp::Node
     bool mMappingEnabled = false;
     float mMappingRes = 0.05f;
     float mMappingRangeMax = 10.0f;
-    bool mObjDetEnabled = true;
+    bool mObjDetEnabled = false;
     bool mObjDetTracking = true;
     float mObjDetConfidence = 40.0f;
     std::vector<sl::OBJECT_CLASS> mObjDetFilter;
@@ -342,7 +350,7 @@ class ZedCamera : public rclcpp::Node
     // <---- TF Transforms Flags
 
     // ----> Messages (ONLY THOSE NOT CHANGING WHILE NODE RUNS)
-    // Camera info
+    // Camera infos
     camInfoMsgPtr mRgbCamInfoMsg;
     camInfoMsgPtr mLeftCamInfoMsg;
     camInfoMsgPtr mRightCamInfoMsg;
@@ -351,6 +359,7 @@ class ZedCamera : public rclcpp::Node
     camInfoMsgPtr mRightCamInfoRawMsg;
     camInfoMsgPtr mDepthCamInfoMsg;
 
+    // Transforms
     transfMsgPtr mCameraImuTransfMgs;
     // <---- Messages
 
@@ -452,14 +461,20 @@ class ZedCamera : public rclcpp::Node
     std::unique_ptr<sl_tools::SmartMean> mObjDetPeriodMean_msec;
     std::unique_ptr<sl_tools::SmartMean> mPubFusedCloudPeriodMean_sec;
 
-    // Last frame time
+    // ----> Timestamps
     rclcpp::Time mPrevFrameTimestamp;
     rclcpp::Time mFrameTimestamp;
+    // <---- Timestamps
 
-    // Point cloud variables
+    // ----> Point cloud variables
     sl::Mat mCloud;
     sl::FusedPointCloud mFusedPC;
     rclcpp::Time mPointCloudTime;
+    // <---- Point cloud variables
+
+    // ----> Services
+    resetOdomSrvPtr mResetOdomSrv;
+    // <---- Services
 };
 
 } // namespace stereolabs

zed_components/src/zed_camera/src/zed_camera_component.cpp

@@ -62,6 +62,9 @@ ZedCamera::ZedCamera(const rclcpp::NodeOptions &options)
     initParameters();
     // <---- Parameters initialization
 
+    // Init services
+    initServices();
+
     // Start camera
     startCamera();
 }
@@ -109,6 +112,33 @@ ZedCamera::~ZedCamera() {
     // <---- Verify that the grab thread is not active
 }
 
+void ZedCamera::initServices() {
+    RCLCPP_INFO(get_logger(), "*** SERVICES ***");
+
+    std::string srv_name;
+
+    /*std::string srv_name;
+    std::string srv_prefix = get_namespace();
+
+    if (srv_prefix.length() > 1) {
+        srv_prefix += "/";
+    }
+
+    if ('/' != srv_prefix.at(0)) {
+        srv_prefix = '/'  + srv_prefix;
+    }
+
+    srv_prefix += get_name();*/
+    std::string srv_prefix = "~/";
+
+    // ResetOdometry
+    srv_name = srv_prefix + "reset_odometry";
+    mResetOdomSrv = create_service<std_srvs::srv::Trigger>(srv_name,
+                                                           std::bind(&ZedCamera::callback_resetOdometry,
+                                                                     this, _1, _2, _3));
+    RCLCPP_INFO(get_logger(), " * '%s'", mResetOdomSrv->get_service_name());
+}
+
 template<typename T>
 void ZedCamera::getParam(std::string paramName, T defValue, T& outVal, std::string log_info )
 {
@@ -3866,6 +3896,18 @@ void ZedCamera::callback_pubFusedPc() {
     mPubFusedCloud->publish(std::move(pointcloudFusedMsg));
 }
 
+void ZedCamera::callback_resetOdometry(const std::shared_ptr<rmw_request_id_t> request_header,
+                                       const std::shared_ptr<std_srvs::srv::Trigger_Request> req,
+                                       std::shared_ptr<std_srvs::srv::Trigger_Response> res) {
+    (void)request_header;
+    (void)req;
+
+    RCLCPP_INFO(get_logger(), "Reset Odometry service called");
+    mResetOdom = true;
+    res->message = "Odometry reset OK";
+    res->success = true;
+}
+
 } // namespace stereolabs
 
 #include "rclcpp_components/register_node_macro.hpp"