< >
Home » ROS2与tf2入门教程 » ROS2与tf2入门教程-tf2和时间(C++)

ROS2与tf2入门教程-tf2和时间(C++)

说明：

  • 介绍如何在lookup_transform函数使用timeout等待转换在 tf2 树上可用

步骤：

  • 新建turtle_tf2_listener_timeout.py
cd ~/tf2_ws/src/learning_tf2_cpp/src
cp turtle_tf2_listener.cpp turtle_tf2_listener_timeout.cpp
vim turtle_tf2_listener_timeout.cpp
  • 修改lookupTransform所在代码后为
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/exceptions.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <turtlesim/srv/spawn.hpp>

#include <chrono>
#include <memory>
#include <string>

using std::placeholders::_1;
using namespace std::chrono_literals;

class FrameListener : public rclcpp::Node
{
public:
  FrameListener()
  : Node("turtle_tf2_frame_listener"),
    turtle_spawning_service_ready_(false),
    turtle_spawned_(false)
  {
    // Declare and acquire `target_frame` parameter
    this->declare_parameter<std::string>("target_frame", "turtle1");
    this->get_parameter("target_frame", target_frame_);

    tf_buffer_ =
      std::make_unique<tf2_ros::Buffer>(this->get_clock());
    transform_listener_ =
      std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

    // Create a client to spawn a turtle
    spawner_ =
      this->create_client<turtlesim::srv::Spawn>("spawn");

    // Create turtle2 velocity publisher
    publisher_ =
      this->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 1);

    // Call on_timer function every second
    timer_ = this->create_wall_timer(
      1s, std::bind(&FrameListener::on_timer, this));
  }

private:
  void on_timer()
  {
    // Store frame names in variables that will be used to
    // compute transformations
    std::string fromFrameRel = target_frame_.c_str();
    std::string toFrameRel = "turtle2";

    if (turtle_spawning_service_ready_) {
      if (turtle_spawned_) {
        geometry_msgs::msg::TransformStamped transformStamped;

        // Look up for the transformation between target_frame and turtle2 frames
        // and send velocity commands for turtle2 to reach target_frame
        try {
          rclcpp::Time now = this->get_clock()->now();
          transformStamped = tf_buffer_->lookupTransform(
            toFrameRel,
            fromFrameRel,
            now,
            50ms);
        } catch (tf2::TransformException & ex) {
          RCLCPP_INFO(
            this->get_logger(), "Could not transform %s to %s: %s",
            toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());
          return;
        }

        geometry_msgs::msg::Twist msg;

        static const double scaleRotationRate = 1.0;
        msg.angular.z = scaleRotationRate * atan2(
          transformStamped.transform.translation.y,
          transformStamped.transform.translation.x);

        static const double scaleForwardSpeed = 0.5;
        msg.linear.x = scaleForwardSpeed * sqrt(
          pow(transformStamped.transform.translation.x, 2) +
          pow(transformStamped.transform.translation.y, 2));

        publisher_->publish(msg);
      } else {
        RCLCPP_INFO(this->get_logger(), "Successfully spawned");
        turtle_spawned_ = true;
      }
    } else {
      // Check if the service is ready
      if (spawner_->service_is_ready()) {
        // Initialize request with turtle name and coordinates
        // Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
        auto request = std::make_shared<turtlesim::srv::Spawn::Request>();
        request->x = 4.0;
        request->y = 2.0;
        request->theta = 0.0;
        request->name = "turtle2";

        // Call request
        using ServiceResponseFuture =
          rclcpp::Client<turtlesim::srv::Spawn>::SharedFuture;
        auto response_received_callback = [this](ServiceResponseFuture future) {
            auto result = future.get();
            if (strcmp(result->name.c_str(), "turtle2") == 0) {
              turtle_spawning_service_ready_ = true;
            } else {
              RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch");
            }
          };
        auto result = spawner_->async_send_request(request, response_received_callback);
      } else {
        RCLCPP_INFO(this->get_logger(), "Service is not ready");
      }
    }
  }
  // Boolean values to store the information
  // if the service for spawning turtle is available
  bool turtle_spawning_service_ready_;
  // if the turtle was successfully spawned
  bool turtle_spawned_;
  rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawner_{nullptr};
  rclcpp::TimerBase::SharedPtr timer_{nullptr};
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_{nullptr};
  std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
  std::string target_frame_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FrameListener>());
  rclcpp::shutdown();
  return 0;
}
  • lookupTransform的第三个参数设置tf2::TimePointZero表示最新的变换信息
  • 设置为rclcpp::Time now = this->get_clock()->now();　设置now表示当前时间的变换信息
  • 但是当前时间可能没有可用的变换信息，就会出现异常。可以设置第四个参数，增加等待时间。
  • 修改CMakeLists.txt文件，再原来的内容基础上添加turtle_tf2_listener_timeout.cpp的内容
add_executable(turtle_tf2_listener_timeout src/turtle_tf2_listener_timeout.cpp)
ament_target_dependencies(
  turtle_tf2_listener_timeout
  geometry_msgs
  rclcpp
  tf2
  tf2_ros
  turtlesim
)
install(TARGETS
  turtle_tf2_listener_timeout
  DESTINATION lib/${PROJECT_NAME})
  • 创建一个启动文件
cd ~/tf2_ws/src/learning_tf2_cpp/launch
cp turtle_tf2_demo.launch.py turtle_tf2_demo_timeout.launch.py
vim turtle_tf2_demo_timeout.launch.py
  • 内容如下
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
  return LaunchDescription([
    Node(
      package='turtlesim',
      executable='turtlesim_node',
      name='sim'
    ),
    
    Node(
      package='learning_tf2_cpp',
      executable='turtle_tf2_broadcaster',
      name='broadcaster1',
      parameters=[
          {'turtlename': 'turtle1'}
      ]
    ),

    DeclareLaunchArgument(
        'target_frame', default_value='carrot1',
        description='Target frame name.'
    ),
    Node(
        package='learning_tf2_cpp',
        executable='turtle_tf2_broadcaster',
        name='broadcaster2',
        parameters=[
            {'turtlename': 'turtle2'}
        ]
    ),
    Node(
        package='learning_tf2_cpp',
        executable='turtle_tf2_listener_timeout',
        name='listener',
        parameters=[
            {'target_frame': LaunchConfiguration('target_frame')}
        ]
    ),
  ])
  • 创建turtle_tf2_fixed_frame_demo_timeout.launch.py
cd ~/tf2_ws/src/learning_tf2_cpp/launch
cp turtle_tf2_fixed_frame_demo.launch.py turtle_tf2_fixed_frame_demo_timeout.launch.py
vim turtle_tf2_fixed_frame_demo_timeout.launch.py
  • 内容如下
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
  target_frame = LaunchConfiguration('target_frame')

  declare_target_frame_cmd = DeclareLaunchArgument(
    'target_frame',
    default_value='carrot1',
    description='target_frame')

  demo_nodes = IncludeLaunchDescription(
    PythonLaunchDescriptionSource([os.path.join(
      get_package_share_directory('learning_tf2_cpp'), 'launch'),
      '/turtle_tf2_demo_timeout.launch.py']),
    launch_arguments={'target_frame': target_frame}.items()
  )

  return LaunchDescription([
    declare_target_frame_cmd,
    demo_nodes,
    Node(
          package='learning_tf2_cpp',
          executable='fixed_frame_tf2_broadcaster',
          name='fixed_broadcaster',
    ),
  ])
  • 构建
colcon build --symlink-install --packages-select learning_tf2_cpp
  • 加载工作空间
. ~/tf2_ws/install/local_setup.bash

测试：

  • 启动文件turtle_tf2_demo_timeout.launch.py
. ~/tf2_ws/install/local_setup.bash
ros2 launch learning_tf2_cpp turtle_tf2_demo_timeout.launch.py
  • 因为还没发布carrot1坐标系，会导致变换不可用，触发异常　
  • 错误类似
[turtle_tf2_listener_timeout-4]          at line 156 in /tmp/binarydeb/ros-galactic-tf2-0.17.2/src/buffer_core.cpp
[turtle_tf2_listener_timeout-4] Warning: Invalid frame ID "carrot1" passed to canTransform argument source_frame - frame does not exist
  • 启动turtle_tf2_fixed_frame_demo_timeout.launch.py
. ~/tf2_ws/install/local_setup.bash
ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_timeout.launch.py
  • 如果在5秒carrot1到turtle1的变换不可用，会触发异常　
  • 打印变换信息
ros2 run tf2_ros tf2_echo carrot1 turtle1

参考:

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros2与tf2入门教程