diff --git a/zed_components/CMakeLists.txt b/zed_components/CMakeLists.txt index 069efac4..1d8ce37d 100644 --- a/zed_components/CMakeLists.txt +++ b/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) ############################################################################### diff --git a/zed_components/src/zed_camera/include/zed_camera_component.hpp b/zed_components/src/zed_camera/include/zed_camera_component.hpp index 076e337d..3b6ca1d3 100644 --- a/zed_components/src/zed_camera/include/zed_camera_component.hpp +++ b/zed_components/src/zed_camera/include/zed_camera_component.hpp @@ -26,6 +26,9 @@ #include #include +#include +#include + #include #include @@ -71,7 +74,7 @@ typedef std::unique_ptr pathMsgPtr; typedef std::unique_ptr objDetMsgPtr; -//typedef rclcpp::Service::SharedPtr resetOdomSrvPtr; +typedef rclcpp::Service::SharedPtr resetOdomSrvPtr; //typedef rclcpp::Service::SharedPtr restartTrkSrvPtr; //typedef rclcpp::Service::SharedPtr setPoseSrvPtr; //typedef rclcpp::Service::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 parameters); + + void callback_resetOdometry(const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr 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 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 mObjDetPeriodMean_msec; std::unique_ptr 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 diff --git a/zed_components/src/zed_camera/src/zed_camera_component.cpp b/zed_components/src/zed_camera/src/zed_camera_component.cpp index aebfae6f..f398a3fe 100644 --- a/zed_components/src/zed_camera/src/zed_camera_component.cpp +++ b/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(srv_name, + std::bind(&ZedCamera::callback_resetOdometry, + this, _1, _2, _3)); + RCLCPP_INFO(get_logger(), " * '%s'", mResetOdomSrv->get_service_name()); +} + template 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 request_header, + const std::shared_ptr req, + std::shared_ptr 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"