Expose TF2 listener CB

[SteveMacenski]

This exposes the TF2 Listener subscriber callback so that a user can override it to perform a slightly altered operation. This is in efforts to a new BaseFootprintPublisher I'm adding to nav2_util which looks at incoming TF messages and checks if they're updating the transform of base_link. If so, then I want to automatically publish a base_footprint transform with the Z, Pitch, Roll removed to be flat on the surface 2D.

Initially, I had made a TF2MsgCallback which the user could register on TF2 Listener construction to be called within the subscription callback, but I ran into issues since there are so many default constructor arguments, selecting the order of where to place it to (1) not disrupt users and (2) not have to specify a massive number of QoS settings or options.

  TransformListener(
    tf2::BufferCore & buffer,
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
    bool spin_thread = true,
    const rclcpp::QoS & qos = DynamicListenerQoS(),
    const rclcpp::QoS & static_qos = StaticListenerQoS(),
    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options =
    detail::get_default_transform_listener_sub_options<AllocatorT>(),
    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options =
    detail::get_default_transform_listener_static_sub_options<AllocatorT>())

So instead, I opted for making the subscription itself overridable so I can call TransformListener::subscription_callback(msg, is_static); internally and then do my thing with that data, e.g.

  void subscription_callback(tf2_msgs::msg::TFMessage::ConstSharedPtr msg, bool is_static) override
  {
    TransformListener::subscription_callback(msg, is_static);

    if (is_static) {
      return;
    }

    for (unsigned int i = 0; i != msg->transforms.size(); i++) {
      auto & t = msg->transforms[i];
      if (t.child_frame_id == base_link_frame_) {
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = t.header.stamp;
        transform.header.frame_id = base_link_frame_;
        transform.child_frame_id = base_footprint_frame_;

        // Project to Z-zero
        transform.transform.translation = t.transform.translation;
        transform.transform.translation.z = 0.0;

        // Remove Roll and Pitch
        tf::Quaternion q;
        q.setRPY(0, 0, tf2::getYaw(t.transform.orientation));
        q.normalize();
        transform.transform.rotation.x = q.x();
        transform.transform.rotation.y = q.y();
        transform.transform.rotation.z = q.z();
        transform.transform.rotation.w = q.w();

        tf_broadcaster_->sendTransform(transform);
        return;
      }
    }
  }

Thus, this seems like the best trade off to be (1) not disruptive to other users and (2) get what I need to get done on an event-basis instead of polling or manually listening into the TF topic. In general, it seems useful to be able to override this method for custom logic or introspection.

[SteveMacenski]

^ You can see this in action in the Nav2 draft PR linked

[ahcorde]

• Linux
• Linux-aarch64
• Windows

[SteveMacenski]

13:30:18 === src/ros2/yaml_cpp_vendor (git) ===
13:30:18 Cloning into '.'...
13:30:18 <== '. ../venv/bin/activate && "/home/jenkins-agent/workspace/ci_linux/venv/bin/python" "/home/jenkins-agent/workspace/ci_linux/venv/bin/vcs" import "src" --force --retry 5 --input 00-ros2.repos' exited with return code '1'
13:30:19 Build step 'Execute shell' marked build as failure

Wow, the build farm is having alot of timeout issues in internet connection, huh? That seems odd that this comes up so much

[SteveMacenski]

@ahcorde can we retrigger?

[ahcorde]

• Linux
• Linux-aarch64
• Windows

[SteveMacenski]

Passing now!

[SteveMacenski]

Thanks!!