Program Listing for File TFPublisher.hpp

↰ Return to documentation for file (/tmp/ws/src/depthai-ros/depthai_bridge/include/depthai_bridge/TFPublisher.hpp)

#pragma once
#include "depthai-shared/common/CameraFeatures.hpp"
#include "depthai/device/CalibrationHandler.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "nlohmann/json.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/parameter_client.hpp"
#include "tf2_ros/static_transform_broadcaster.h"

namespace dai {
namespace ros {
class TFPublisher {
   public:
    explicit TFPublisher(rclcpp::Node* node,
                         const dai::CalibrationHandler& calHandler,
                         const std::vector<dai::CameraFeatures>& camFeatures,
                         const std::string& camName,
                         const std::string& camModel,
                         const std::string& baseFrame,
                         const std::string& parentFrame,
                         const std::string& camPosX,
                         const std::string& camPosY,
                         const std::string& camPosZ,
                         const std::string& camRoll,
                         const std::string& camPitch,
                         const std::string& camYaw,
                         const std::string& imuFromDescr,
                         const std::string& customURDFLocation,
                         const std::string& customXacroArgs);
    std::string getURDF();
    geometry_msgs::msg::Quaternion quatFromRotM(nlohmann::json rotMatrix);
    geometry_msgs::msg::Vector3 transFromExtr(nlohmann::json translation);

   private:
    void convertModelName();
    std::string prepareXacroArgs();
    void publishDescription();
    void publishCamTransforms(nlohmann::json camData, rclcpp::Node* node);
    void publishImuTransform(nlohmann::json json, rclcpp::Node* node);
    bool modelNameAvailable();
    std::string getCamSocketName(int socketNum);
    std::unique_ptr<rclcpp::AsyncParametersClient> _paramClient;
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> _tfPub;
    std::string _camName;
    std::string _camModel;
    std::string _baseFrame;
    std::string _parentFrame;
    std::string _camPosX;
    std::string _camPosY;
    std::string _camPosZ;
    std::string _camRoll;
    std::string _camPitch;
    std::string _camYaw;
    std::string _imuFromDescr;
    std::string _customURDFLocation;
    std::string _customXacroArgs;
    std::vector<dai::CameraFeatures> _camFeatures;
    rclcpp::Logger _logger;
    const std::unordered_map<dai::CameraBoardSocket, std::string> _socketNameMap = {
        {dai::CameraBoardSocket::AUTO, "rgb"},
        {dai::CameraBoardSocket::CAM_A, "rgb"},
        {dai::CameraBoardSocket::CAM_B, "left"},
        {dai::CameraBoardSocket::CAM_C, "right"},
        {dai::CameraBoardSocket::CAM_D, "left_back"},
        {dai::CameraBoardSocket::CAM_E, "right_back"},
    };
};
}  // namespace ros
}  // namespace dai