Static transform publishing

Added parameter for transform publication and mounting link frame name.
Publish mounting frame to optical frame as static transform, values taken from parameters.
Publish optical frame to cloud frame with values from extrinsics buffer.
Removed is_active check, as it is no longer needed with deactive() stopping the framebuffer explicitly.

CMakeLists.txt

@@ -11,6 +11,7 @@ endif()
 
 set(IFM3D_ROS2_DEPS
   builtin_interfaces
+  geometry_msgs
   lifecycle_msgs
   rclcpp
   rclcpp_components
@@ -20,6 +21,7 @@ set(IFM3D_ROS2_DEPS
   rosidl_default_generators
   sensor_msgs
   std_msgs
+  tf2_ros
   )
 
 find_package(ifm3d 1.2.4 CONFIG REQUIRED COMPONENTS

config/camera_default_parameters.yaml

@@ -16,7 +16,12 @@
     tf:
       cloud_link:
         frame_name: "camera_cloud_link"
+        publish_transform: true
+        transform: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+      mounting_link:
+        frame_name: "camera_mounting_link"
       optical_link:
         frame_name: "camera_optical_link"
+        publish_transform: true
     use_sim_time: false
     xmlrpc_port: 80

include/ifm3d_ros2/camera_node.hpp

@@ -14,13 +14,15 @@
 #include <thread>
 #include <vector>
 
+#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
-
 #include <rcl_interfaces/msg/set_parameters_result.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2/LinearMath/Quaternion.h>
 
 #include <ifm3d_ros2/visibility_control.h>
 
@@ -247,6 +249,21 @@ class IFM3D_ROS2_PUBLIC CameraNode : public rclcpp_lifecycle::LifecycleNode
    */
   void deactivate_publishers();
 
+  /**
+   * Publish the transform from the mounting link to the optical link as static tf
+   */
+  void publish_optical_link_transform();
+
+  /**
+   * @brief Publish the transform from the mounting link to the cloud link as static tf if it changed
+   *
+   * A change can either be new translational/rotational data from extrinsics or
+   * a name change of one of the frames.
+   *
+   * @param msg ExtrinsicsMsg from camera
+   */
+  void publish_cloud_link_transform_if_changed(const ExtrinsicsMsg& msg);
+
   ifm3d_ros2::buffer_id_utils::data_stream_type stream_type_from_port_info(const std::vector<ifm3d::PortInfo>& ports,
                                                                            const uint16_t pcic_port);
 
@@ -261,14 +278,22 @@ class IFM3D_ROS2_PUBLIC CameraNode : public rclcpp_lifecycle::LifecycleNode
   std::string ip_{};
   std::uint16_t pcic_port_{};
   std::string tf_cloud_link_frame_name_{};
+  bool tf_cloud_link_publish_transform_{};
+  std::string tf_mounting_link_frame_name_{};
   std::string tf_optical_link_frame_name_{};
+  bool tf_optical_link_publish_transform_{};
+  std::vector<double> tf_optical_link_transform_{};
   std::uint16_t xmlrpc_port_{};
 
   /// Subscription to parameter changes
   std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;
   /// Callbacks need to be stored to work properly; using a map with parameter name as key
   std::map<std::string, rclcpp::ParameterCallbackHandle::SharedPtr> registered_param_callbacks_;
 
+  // TF handling
+  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
+  geometry_msgs::msg::TransformStamped cloud_link_transform_;
+
   ifm3d_ros2::buffer_id_utils::data_stream_type data_stream_type_;
 
   DumpServer dump_srv_{};
@@ -284,8 +309,6 @@ class IFM3D_ROS2_PUBLIC CameraNode : public rclcpp_lifecycle::LifecycleNode
   std::map<ifm3d::buffer_id, PCLPublisher> pcl_publishers_;
   std::map<ifm3d::buffer_id, ExtrinsicsPublisher> extrinsics_publishers_;
 
-  std::atomic_bool is_active_{};
-
 };  // end: class CameraNode
 
 }  // namespace ifm3d_ros2

launch/camera.launch.py

@@ -93,24 +93,6 @@ def generate_launch_description():
         log_cmd=True,
     )
 
-    tf_node = Node(
-        executable="static_transform_publisher",
-        package="tf2_ros",
-        name=[LaunchConfiguration("camera_name"), "_internal_transform_publiser"],
-        namespace=LaunchConfiguration("camera_namespace"),
-        arguments=[
-            '0',
-            '0',
-            '0',
-            '0',
-            '0',
-            '0',
-            [LaunchConfiguration("camera_name"), "_optical_link"],
-            [LaunchConfiguration("camera_name"), "_link"],
-        ],
-        log_cmd=True,
-    )
-
     # Launching RViz2 conditionally, depending on the "visualition" argument
     rviz_node = Node(
         executable="rviz2",
@@ -159,7 +141,6 @@ def generate_launch_description():
     for declared_argument in declared_arguments:
         ld.add_entity(declared_argument)
     ld.add_action(camera_node)
-    ld.add_action(tf_node)
     ld.add_action(rviz_node)
     ld.add_action(configure_camera)
     ld.add_action(activate_camera)

package.xml

@@ -5,14 +5,15 @@
   <name>ifm3d_ros2</name>
   <version>1.0.1</version>
   <description>ifm ToF Camera ROS2 package</description>
-  <author email="Philipp.Senger@ifm.com">Philipp Senger</author>
   <maintainer email="support.robotics@ifm.com">ifm Robotics Support</maintainer>
   <license>Apache 2.0</license>
+  <author email="Philipp.Senger@ifm.com">Philipp Senger</author>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
   <build_depend>builtin_interfaces</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_depend>launch</build_depend>
   <build_depend>launch_ros</build_depend>
   <build_depend>lifecycle_msgs</build_depend>
@@ -26,6 +27,7 @@
   <build_depend>rosidl_default_generators</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>tf2_ros</build_depend>
 
   <exec_depend>ament_index_python</exec_depend>
   <exec_depend>builtin_interfaces</exec_depend>
@@ -42,6 +44,7 @@
   <exec_depend>rosidl_default_runtime</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
 
   <test_depend>launch_testing_ament_cmake</test_depend>
   <test_depend>python3-opencv</test_depend>