iT邦幫忙

2023 iThome 鐵人賽
DAY 3
0
自我挑戰組

ROS2 及 ROS Porting 自學筆記系列 第 24 篇

Day 24 ROS Transform - Part2

15th鐵人賽 ros2 ros2 porting ros ros transform
1699 瀏覽

  • 分享至 

  • xImage
    •  

今天的範例會延用昨天的創的packagelearning_tf2_cpp，然後把turtlesim和tf2的功能結合起來。雖然看起來很多，但其實都長得超像XD 就是幾個function的變形而已，不要害怕/images/emoticon/emoticon04.gif

Broadcaster

Broadcaster顧名思義就是廣播的意思，將frame之間的關係發布出去。下面的範例是將客製化訊息turtlesim::msg::Pose轉換成tf2。

  1. 首先下載turtle_tf2_broadcaster.cpp

    cd ~/ros2_ws/src/learning_tf2_cpp/src
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp

  2. 檢視turtle_tf2_broadcaster.cpp

    #include <functional>
#include <memory>
#include <sstream>
#include <string>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
#include "turtlesim/msg/pose.hpp"

class FramePublisher : public rclcpp::Node
{
public:
FramePublisher()
: Node("turtle_tf2_frame_publisher")
{
    // Declare and acquire `turtlename` parameter
    turtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");

    // Initialize the transform broadcaster
    tf_broadcaster_ =
    std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    // Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
    // callback function on each message
    std::ostringstream stream;
    stream << "/" << turtlename_.c_str() << "/pose";
    std::string topic_name = stream.str();

    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
    topic_name, 10,
    std::bind(&FramePublisher::handle_turtle_pose, this, std::placeholders::_1));
}

private:
void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg)
{
    geometry_msgs::msg::TransformStamped t;

    // Read message content and assign it to
    // corresponding tf variables
    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "world";
    t.child_frame_id = turtlename_.c_str();

    // Turtle only exists in 2D, thus we get x and y translation
    // coordinates from the message and set the z coordinate to 0
    t.transform.translation.x = msg->x;
    t.transform.translation.y = msg->y;
    t.transform.translation.z = 0.0;

    // For the same reason, turtle can only rotate around one axis
    // and this why we set rotation in x and y to 0 and obtain
    // rotation in z axis from the message
    tf2::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    // Send the transformation
    tf_broadcaster_->sendTransform(t);
}

rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
std::string turtlename_;
};

int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<FramePublisher>());
rclcpp::shutdown();
return 0;
}

    可以看到結構和昨天的static broadcaster很像，都有TransformBroadcaster和TransformStamped，只是TransformStamped的內容是從turtlesim的Pose中取得，然後再廣播出去。

    turtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");

    首先定義參數turtlename，預設參數為turtle，但也可以在執行時指定turtlename，例如：turtle1或turtle2。

    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
topic_name, 10,
std::bind(&FramePublisher::handle_turtle_pose, this, _1));

    這邊訂閱turtlesim的Pose，並且指定callback function為handle_turtle_pose。

    geometry_msgs::msg::TransformStamped t;

// Read message content and assign it to
// corresponding tf variables
t.header.stamp = this->get_clock()->now();
t.header.frame_id = "world";
t.child_frame_id = turtlename_.c_str();

// Turtle only exists in 2D, thus we get x and y translation
// coordinates from the message and set the z coordinate to 0
t.transform.translation.x = msg->x;
t.transform.translation.y = msg->y;
t.transform.translation.z = 0.0;

// For the same reason, turtle can only rotate around one axis
// and this why we set rotation in x and y to 0 and obtain
// rotation in z axis from the message
tf2::Quaternion q;
q.setRPY(0, 0, msg->theta);
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();

    可以看到這裡非常眼熟，只是將TransformStamped的內容改成turtlesim的Pose。

  3. 撰寫launch file 來簡化操作多個terminal流程

    from launch import LaunchDescription
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'}
            ]
        ),
    ])

    第一個Node用來啟動turtlesim，第二個Node用來啟動turtle_tf2_broadcaster，並且指定turtlename參數為turtle1。

  4. 編輯package.xml，新增launch file的dependencies

    <exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

  5. 編輯CMakeLists.txt，新增可執行檔和安裝可執行檔和launch file

    add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
    turtle_tf2_broadcaster
    geometry_msgs
    rclcpp
    tf2
    tf2_ros
    turtlesim
)
...
install(TARGETS
    turtle_tf2_broadcaster
    DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch
    DESTINATION share/${PROJECT_NAME})

  6. Build

    cd ~/ros2_ws
rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select learning_tf2_cpp
source ~/ros2_ws/install/setup.bash

  7. 啟動turtlesim和turtle_tf2_broadcaster

    ros2 launch learning_tf2_cpp turtle_tf2_demo.launch.py

  8. 開啟第二個terminal

    ros2 run tf2_ros tf2_echo world turtle1

    可以看到turtle1在world的frame座標

    At time 1625137663.912474878
- Translation: [5.276, 7.930, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137664.950813527
- Translation: [3.750, 6.563, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137665.906280726
- Translation: [2.320, 5.282, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137666.850775673
- Translation: [2.153, 5.133, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.365, 0.931]

Listener

這個範例跟上面反過來，將訊息從tf2中取出並發布Topic出去。這個範例還偷渡了前面學到的Service，用來產生新的烏龜。

  1. 下載turtle_tf2_listener.cpp

    cd ~/ros2_ws/src/learning_tf2_cpp/src
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_listener.cpp

  2. 檢視turtle_tf2_listener.cpp

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

#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"

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
    target_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1");

    tf_buffer_ =
    std::make_unique<tf2_ros::Buffer>(this->get_clock());
    tf_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 t;

        // Look up for the transformation between target_frame and turtle2 frames
        // and send velocity commands for turtle2 to reach target_frame
        try {
        t = tf_buffer_->lookupTransform(
            toFrameRel, fromFrameRel,
            tf2::TimePointZero);
        } catch (const 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(
        t.transform.translation.y,
        t.transform.translation.x);

        static const double scaleForwardSpeed = 0.5;
        msg.linear.x = scaleForwardSpeed * sqrt(
        pow(t.transform.translation.x, 2) +
        pow(t.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> tf_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;
}

    這裡新增transform_listener.h用來接收tf2的訊息

    tf_listener_ =
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

    創建TransformListener，接著就會機受來自tf2的所有座標轉換訊息。

    t = tf_buffer_->lookupTransform(
        toFrameRel, fromFrameRel,
        tf2::TimePointZero);

    lookupTransform用來query特定的transform，而他有三個參數：

    第一個參數是目標座標(Target Frame)，這裡是turtle2。

    第二個參數是來源座標(Source Frame)，這裡是turtle1。

    第三個參數是時間，這邊使用tf2::TimePointZero，代表最新的時間。

  3. 編輯CMakeLists.txt，新增可執行檔和安裝可執行檔

    add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
ament_target_dependencies(
    turtle_tf2_listener
    geometry_msgs
    rclcpp
    tf2
    tf2_ros
    turtlesim
)
...
install(TARGETS
    turtle_tf2_listener
    DESTINATION lib/${PROJECT_NAME})

  4. 創建launch file

    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='turtle1',
            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',
            name='listener',
            parameters=[
                {'target_frame': LaunchConfiguration('target_frame')}
            ]
        ),
    ])

    除了前面的兩個Node，這裡新增了一個Argument用來宣告target_frame。另外還有兩個Node，第三個Node是用來廣播turtle2的frame，第四個Node是用來接收tf2的訊息，並且發布出去。

  5. Build

    cd ~/ros2_ws
rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select learning_tf2_cpp
source ~/ros2_ws/install/setup.bash

  6. 執行launch file

    ros2 launch learning_tf2_cpp turtle_tf2_demo.launch.py

  7. 在第二個terminal中執行鍵盤控制

    ros2 run turtlesim turtle_teleop_key

    就可以用方向鍵操作烏龜，然後可以看到turtle2會跟著turtle1移動。

Frame

這個小節會介紹如何將fixed frame和dynamic frame透過C++ API加入系統。這邊基本上和broadcaster相似，不過會多介紹一些功能。

⚠️ tf2 tree 不允許有loop，也就是frame之間不允許有循環的關係。這點必須要小心。

Fixed frame broadcaster

  1. 下載fixed_frame_tf2_broadcaster.cpp:

    cd ~/ros2_ws/src/learning_tf2_cpp/src
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/fixed_frame_tf2_broadcaster.cpp

  2. 查看fixed_frame_tf2_broadcaster.cpp

    #include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

class FixedFrameBroadcaster : public rclcpp::Node
{
public:
FixedFrameBroadcaster()
: Node("fixed_frame_tf2_broadcaster")
{
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
    100ms, std::bind(&FixedFrameBroadcaster::broadcast_timer_callback, this));
}

private:
void broadcast_timer_callback()
{
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 0.0;
    t.transform.translation.y = 2.0;
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
}

rclcpp::TimerBase::SharedPtr timer_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<FixedFrameBroadcaster>());
rclcpp::shutdown();
return 0;
}

    geometry_msgs::msg::TransformStamped t;

t.header.stamp = this->get_clock()->now();
t.header.frame_id = "turtle1";
t.child_frame_id = "carrot1";
t.transform.translation.x = 0.0;
t.transform.translation.y = 2.0;
t.transform.translation.z = 0.0;

    這裡從turtle1為patern，新增一個新的child frame叫做carrot1，並且設定carrot1相對於turtle1的位置。這樣在turtle1在移動時，carrot1永遠會在turtle1的左邊2m的位置。

  3. 編輯CMakeLists.txt，加入可執行檔和安裝位置

    add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
    fixed_frame_tf2_broadcaster
    geometry_msgs
    rclcpp
    tf2_ros
)
...
install(TARGETS
    fixed_frame_tf2_broadcaster
    DESTINATION lib/${PROJECT_NAME})

  4. 撰寫launch file

    import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_cpp'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
        )

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_cpp',
            executable='fixed_frame_tf2_broadcaster',
            name='fixed_broadcaster',
        ),
    ])

    這裡使用IncludeLaunchDescription來引入前面範例的launch file，然後再新增一個Node來廣播carrot1的frame。

    其實這個node可以用昨天提到的static broadcaster來實現，不過這邊是為了示範tf2的功能。

  5. Build

    cd ~/ros2_ws
rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select learning_tf2_cpp
source ~/ros2_ws/install/setup.bash

  6. 執行launch file

    ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py

  7. 開啟第二個terminal，查看carrot1的frame

    ros2 run tf2_tools view_frames.py

    可以看到carrot1的frame是turtle1的child frame

  8. 我們可以讓turtle2改跟著carrot1走

    ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1

    或是更改turtle_tf2_fixed_frame_demo.launch.py

    def generate_launch_description():
demo_nodes = IncludeLaunchDescription(
    ...,
    launch_arguments={'target_frame': 'carrot1'}.items(),
    )

  9. 因為官方忘記用--symlink-install，所以需要重build XD

    cd ~/ros2_ws
colcon build --packages-select learning_tf2_cpp

    這樣烏龜二號就可以跟個蘿蔔一號走了～～～

Dynamic frame broadcaster

動態frame的廣播器，可以讓frame的位置隨著時間改變。這邊會延用前面的範例，讓carrot1的frame隨著時間改變。程式的架構本身不變，但是在位置上我們會加入時間的變數。

  1. 下載dynamic_frame_tf2_broadcaster.cpp 
    cd ~/ros2_ws/src/learning_tf2_cpp/src
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/dynamic_frame_tf2_broadcaster.cpp
  2. 檢視dynamic_frame_tf2_broadcaster 
    #include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

const double PI = 3.141592653589793238463;

class DynamicFrameBroadcaster : public rclcpp::Node
{
public:
DynamicFrameBroadcaster()
: Node("dynamic_frame_tf2_broadcaster")
{
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
    100ms, std::bind(&DynamicFrameBroadcaster::broadcast_timer_callback, this));
}

private:
void broadcast_timer_callback()
{
    rclcpp::Time now = this->get_clock()->now();
    double x = now.seconds() * PI;

    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = now;
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 10 * sin(x);
    t.transform.translation.y = 10 * cos(x);
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
}

rclcpp::TimerBase::SharedPtr timer_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<DynamicFrameBroadcaster>());
rclcpp::shutdown();
return 0;
}
    可以看到多了時間和三角函數，用來改變carrot1的相對位置 
    double x = now.seconds() * PI;
...
t.transform.translation.x = 10 * sin(x);
t.transform.translation.y = 10 * cos(x);
  3. 編輯CMakeLists.txt，加入可執行檔和安裝位置 
    add_executable(dynamic_frame_tf2_broadcaster src/dynamic_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
    dynamic_frame_tf2_broadcaster
    geometry_msgs
    rclcpp
    tf2_ros
)
...
install(TARGETS
    dynamic_frame_tf2_broadcaster
    DESTINATION lib/${PROJECT_NAME})
  4. 撰寫launch file 
    import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


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

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_cpp',
            executable='dynamic_frame_tf2_broadcaster',
            name='dynamic_broadcaster',
        ),
    ])
  5. Build 
    cd ~/ros2_ws
rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select learning_tf2_cpp
source ~/ros2_ws/install/setup.bash
  6. 執行launch file 
    ros2 launch learning_tf2_cpp turtle_tf2_dynamic_frame_demo.launch.py
    就可以看到carrot的相對位置一直跑來跑去。如果移動turtle1，carrot1的位置也會跟著改變。

Time

tf2本身除了座標資訊外，還會有時間的訊息，預設可以存10秒的歷史資料。這邊會介紹如何使用時間的資訊。

  1. 讓我們回去複習turtle_tf2_listener.cpp，裡面的lookupTransform()

    transformStamped = tf_buffer_->lookupTransform(
    toFrameRel,
    fromFrameRel,
    tf2::TimePointZero);

    最後一個參數tf2::TimePointZero等於0，意思是指定buffer內最後的transform。

  2. 現在我們不要buffer內最後的transform，而是指定現在的時間，可以用

    rclcpp::Time now = this->get_clock()->now();
transformStamped = tf_buffer_->lookupTransform(
    toFrameRel,
    fromFrameRel,
    now);

    build完後執行launch file，可以看到錯誤訊息

    [INFO] [1629873136.345688064] [listener]: Could not transform turtle1 to turtle2: Lookup would
require extrapolation into the future.  Requested time 1629873136.345539 but the latest data
is at time 1629873136.338804, when looking up transform from frame [turtle1] to frame [turtle2]

    原因有兩個。第一個是listener有自己的buffer用來存不同broadcasters的transforms。第二個是broadcaster送出transform會花掉一些時間。上敘的buffer+時間差造成lookupTransform()會找不到現在及時的transform。

  3. 解決方法是給他一些幾毫秒的緩衝時間。在ROS中會在lookupTransform前，使用waitforTransform。然而在ROS2這點被優化了，可以在第四個參數指定緩衝時間。

    transformStamped = tf_buffer_->lookupTransform(
    toFrameRel,
    fromFrameRel,
    now,
    50ms);

    這樣會block住直到有transform可以用，或是超過50ms。

  4. 檢查一下是否運作正常，如果跨機器的話這個時間可能要再延長。

    ros2 launch learning_tf2_cpp turtle_tf2_demo.launch.py

Time Travel

如果我們不想要turtle2馬上追著carrot1跑，而是追著carrot15秒鐘前的位置呢？loopupTransform都幫你想到了(osrf怎麼這麼貼心??)。

  1. 編輯turtle_tf2_listener.cpp

    rclcpp::Time when = this->get_clock()->now() - rclcpp::Duration(5, 0);
t = tf_buffer_->lookupTransform(
    toFrameRel,
    fromFrameRel,
    when,
    50ms);

    將now換成when，而when則是被我們操作過的時間，被抹去了5秒，跟迪亞波羅一樣？？？？(JOJO!!!)

  2. 這時候重build後啟動launch file

    ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py

    就可以看到turtle2會追著carrot15秒前的位置跑。

  3. (Optional)lookupTransform不只可以魔改source frame的時間，也可以魔改target frame的時間。最多可以有6個參數

    rclcpp::Time now = this->get_clock()->now();
rclcpp::Time when = now - rclcpp::Duration(5, 0);
t = tf_buffer_->lookupTransform(
    toFrameRel,
    now,
    fromFrameRel,
    when,
    "world",
    50ms);
    1. Target frameturtle2
    2. Transform的時間
    3. Source framecarrot1
    4. Source frame的時間估計
    5. Fixed frameworld
    6. Target Frame的等待容許時間

    這樣tf2會計算carrot1到world的transform。接著在worldframe，tf2時光旅行到5秒前，然後再計算world到turtle2的transform。這樣就可以得到turtle2在5秒前的位置了。

  4. 重新build後啟動launch file

    ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py

    就可以看到跟Step2一樣，turtle2會追著carrot15秒前的位置跑。

ROS vs. ROS2

這邊的差異只要是在ROS tf和ROS2 tf2的差異，因為ROS2的tf2相比ROS又有些不同，讓人很頭痛。

說明 ROS tf ROS2 tf2
frame之間的關係 rosrun tf tf_echo [src_frame] [target_frame] ros2 run tf2_ros tf2_echo [src_frame] [target_frame]
transform 時間buffer waitforTransform+lookupTransform lookupTransform Only
最後的buffer ros::Time(0) tf2::TimePointZero
現在的時間 ros::Time::now() this->get_clock()->now()
lookupTransform 回傳 void 需要pass transform by reference geometry_msgs::msg::TransformStamped

tf2其實將滿多tf的功能拆分成tf2和tf2_ros，散落在兩個packages中

Reference


上一篇
Day23 ROS2 Transform
下一篇
Day25 ROS2 Bag
系列文
ROS2 及 ROS Porting 自學筆記 共 30 篇
  1. 26
    Day26 ROS2 RQT工具
  2. 27
    Day27 ROS2 可視化工具
  3. 28
    Day28 ROS2 ROS1 Bridge
  4. 29
    Day29 ROS2 Simulation
  5. 30
    Day30 ROS2 總結
完整目錄
圖片
  熱門推薦
圖片
{{ item.channelVendor }} | {{ item.webinarstarted }} |
{{ formatDate(item.duration) }}
直播中

尚未有邦友留言

立即登入留言
iThome鐵人賽
參賽組數
902 組
團體組數
37 組
累計文章數
19757 篇
完賽人數
529 人
iT+看影片追技術 看影片追技術 看更多
熱門tag 看更多
15th鐵人賽 16th鐵人賽 13th鐵人賽 14th鐵人賽 17th鐵人賽 12th鐵人賽 11th鐵人賽 鐵人賽 2019鐵人賽 javascript 2018鐵人賽 python 2017鐵人賽 windows php c# linux windows server css react
熱門問題
熱門回答
熱門文章
IT邦幫忙