Program Listing for File TFPublisher.hpp

↰ Return to documentation for file (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/logger.hpp"
#include "rclcpp/parameter_client.hpp"
#include "tf2_ros/static_transform_broadcaster.h"

namespace rclcpp {
class Node;
}  // namespace rclcpp

namespace dai {
namespace ros {
class TFPublisher {
   public:
    explicit TFPublisher(std::shared_ptr<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,
                         const bool rsCompatibilityMode);
    std::string getURDF();
    geometry_msgs::msg::Vector3 transFromExtr(std::vector<float> translation);
    geometry_msgs::msg::Quaternion quatFromRotM(std::vector<std::vector<float>> extrMat);

   private:
    void convertModelName();
    std::string prepareXacroArgs();
    void publishDescription();
    void publishCamTransforms(nlohmann::json camData, std::shared_ptr<rclcpp::Node> node, const dai::CalibrationHandler& calHandler);
    void publishImuTransform(nlohmann::json json, std::shared_ptr<rclcpp::Node> node, const dai::CalibrationHandler& calHandler);
    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;
    bool rsCompatibilityMode;
    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"},
    };
    const std::unordered_map<dai::CameraBoardSocket, std::string> rsSocketNameMap = {
        {dai::CameraBoardSocket::AUTO, "color"},
        {dai::CameraBoardSocket::CAM_A, "color"},
        {dai::CameraBoardSocket::CAM_B, "infra2"},
        {dai::CameraBoardSocket::CAM_C, "infra1"},
    };
};
}  // namespace ros
}  // namespace dai