Urdf loader humble (#347)

README.md

@@ -141,6 +141,31 @@ This runs your camera as a ROS2 Component and gives you the ability to customize
 Paramerers that begin with `r_` can be freely modified during runtime, for example with rqt. 
 Parameters that begin with `i_` are set when camera is initializing, to change them you have to call `stop` and `start` services. This can be used to hot swap NNs during runtime, changing resolutions, etc. Below you can see some examples:
 
+
+#### Publishing TFs from extrinsics
+
+By default, camera transforms are published from default URDF descriptions based on CAD models. This can be overriden by using TFPublisher class from `depthai_bridge`, which based on Device's camera calibration data republishes the description with updated information. To enable this behavior in `depthai_ros_driver`, you can use following parameters:
+- `camera.i_publish_tf_from_calibration` - setting this to true launches TFPublisher
+
+Then you can set following arguments:
+- `camera.i_tf_camera_name` - if not set, defaults to the node name
+- `camera.i_tf_camera_model` - if not set, it will be automatically detected. If the node is unable to detect STL file for the camera it is set to `OAK-D`. To explicitly set it in `camera.launch.py`, set `override_cam_model:=true`
+- `camera.i_tf_base_frame`
+- `camera.i_tf_parent_frame`
+- `camera.i_tf_cam_pos_x`
+- `camera.i_tf_cam_pos_y`
+- `camera.i_tf_cam_pos_z`
+- `camera.i_tf_cam_roll`
+- `camera.i_tf_cam_pitch`
+- `camera.i_tf_cam_yaw`
+
+When using `camera.launch.py`, you can set `pass_tf_args_as_params:=true` so that TF arguments are used to fill those parameters. For example `ros2 launch depthai_ros_driver camera.launch.py pass_tf_args_as_params:=true parent_frame:=map cam_pos_x:=1.0 imu_from_descr:=true`
+
+It is also possible to set custom URDF path (for now only absolute path works) and custom xacro arguments using `camera.i_tf_custom_urdf_path` and `camera.i_tf_custom_xacro_args`. Please note that robot_state_publisher must be running.
+
+**NOTE ON IMU EXTRINSICS**
+If your camera has uncalibrated IMU, a warning will be shown, and IMU will be published with zero rotation and translation. You can override this behavior by setting `camera.i_tf_imu_from_descr`: true. This will publish default IMU extrinsics from URDF based on camera model.
+
 #### Setting RGB parameters
 
 By default RGB camera outputs `ISP` frame. To set custom width and height of output image, you can set `i_isp_num` and `i_isp_den` which scale image dimensions (2 and 3 by default, so from 1920x1080 to 1280x720), note for RGBD alignment to work resulting width and height must be divisible by 16. 

depthai_bridge/CMakeLists.txt

@@ -37,6 +37,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(stereo_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(vision_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 set(dependencies
   camera_info_manager
@@ -48,6 +49,7 @@ set(dependencies
   stereo_msgs
   std_msgs
   vision_msgs
+  tf2_ros
 )
 
 include_directories(
@@ -62,6 +64,7 @@ file(GLOB LIB_SRC
 "src/ImgDetectionConverter.cpp"
 "src/SpatialDetectionConverter.cpp"
 "src/ImuConverter.cpp"
+"src/TFPublisher.cpp"
 "src/TrackedFeaturesConverter.cpp"
 )
 

depthai_bridge/include/depthai_bridge/TFPublisher.hpp

@@ -0,0 +1,87 @@
+#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);
+    /*
+     * @brief Obtain URDF description by running Xacro with provided arguments.
+     */
+    std::string getURDF();
+    geometry_msgs::msg::Quaternion quatFromRotM(nlohmann::json rotMatrix);
+    geometry_msgs::msg::Vector3 transFromExtr(nlohmann::json translation);
+
+   private:
+    /*
+     * @brief Converts model name to one of the available model families
+     */
+    void convertModelName();
+    /*
+     * @brief Prepare arguments for xacro command. If custom URDF location is not provided, check if model name is available in depthai_descriptions package.
+     */
+    std::string prepareXacroArgs();
+    /*
+     * @brief Get URDF description and set it as a parameter for robot_state_publisher
+     */
+    void publishDescription();
+    /*
+     * @brief Publish camera transforms ("standard" and optical) based on calibration data.
+     * Frame names are based on socket names and use following convention: [base_frame]_[socket_name]_camera_frame and
+     * [base_frame]_[socket_name]_camera_optical_frame
+     */
+    void publishCamTransforms(nlohmann::json camData, rclcpp::Node* node);
+    /*
+     * @brief Publish IMU transform based on calibration data.
+     * Frame name is based on IMU name and uses following convention: [base_frame]_imu_frame.
+     * If IMU extrinsics are not set, warning is printed out and imu frame is published with zero translation and rotation.
+     */
+    void publishImuTransform(nlohmann::json json, rclcpp::Node* node);
+    /*
+     * @brief Check if model STL file is available in depthai_descriptions package.
+     */
+    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;
+};
+}  // namespace ros
+}  // namespace dai

depthai_bridge/package.xml

@@ -25,6 +25,7 @@
   <depend>std_msgs</depend>
   <depend>stereo_msgs</depend>
   <depend>vision_msgs</depend>
+  <depend>tf2_ros</depend>
 
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>xacro</exec_depend>