静态变换在机器人系统中非常有用,一般用来描述机器人基坐标系与相对其静止的其他坐标系之间的关系,比如雷达坐标系、相机坐标系等。本篇我们就来看一下,如何通过C++编程,实现静态坐标系变换的广播。
一、创建功能包
ros2 pkg create --build-type ament_cmake learning_tf2_cpp

二、编写静态广播器节点
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <memory>
using std::placeholders::_1;
class StaticFramePublisher : public rclcpp::Node
{
public:
explicit StaticFramePublisher(char * transformation[])
: Node("static_turtle_tf2_broadcaster")
{
tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
// Publish static transforms once at startup
this->make_transforms(transformation);
}
private:
void make_transforms(char * transformation[])
{
rclcpp::Time now;
geometry_msgs::msg::TransformStamped t;
t.header.stamp = now;
t.header.frame_id = "world";
t.child_frame_id = transformation[1];
t.transform.translation.x = atof(transformation[2]);
t.transform.translation.y = atof(transformation[3]);
t.transform.translation.z = atof(transformation[4]);
tf2::Quaternion q;
q.setRPY(
atof(transformation[5]),
atof(transformation[6]),
atof(transformation[7]));
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();
tf_publisher_->sendTransform(t);
}
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_publisher_;
};
int main(int argc, char * argv[])
{
auto logger = rclcpp::get_logger("logger");
// Obtain parameters from command line arguments
if (argc != 8) {
RCLCPP_INFO(
logger, "Invalid number of parameters\nusage: "
"ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster "
"child_frame_name x y z roll pitch yaw");
return 1;
}
// As the parent frame of the transform is `world`, it is
// necessary to check that the frame name passed is different
if (strcmp(argv[1], "world") == 0) {
RCLCPP_INFO(logger, "Your static turtle name cannot be 'world'");
return 1;
}
// Pass parameters and initialize node
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<StaticFramePublisher>(argv));
rclcpp::shutdown();
return 0;
}
2.1 代码解析
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>
tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
this->make_transforms(transformation);
rclcpp::Time now;
geometry_msgs::msg::TransformStamped t;
t.header.stamp = now;
t.header.frame_id = "world";
t.child_frame_id = transformation[1];
t.transform.translation.x = atof(transformation[2]);
t.transform.translation.y = atof(transformation[3]);
t.transform.translation.z = atof(transformation[4]);
tf2::Quaternion q;
q.setRPY(
atof(transformation[5]),
atof(transformation[6]),
atof(transformation[7]));
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();
tf_publisher_->sendTransform(t);
2.2 修改package.xml文件
<description>Examples of static transform broadcaster using rclcpp</description>
<maintainer email="huchunxu@guyuehome.com">Hu Chunxu</maintainer>
<license>Apache License 2.0</license>
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>turtlesim</depend>
2.3 修改CMakeLists.txt文件
find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(turtlesim REQUIRED)
add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
static_turtle_tf2_broadcaster
geometry_msgs
rclcpp
tf2
tf2_ros
turtlesim
)
install(TARGETS
static_turtle_tf2_broadcaster
DESTINATION lib/${PROJECT_NAME})
三、编译和运行
rosdep install -i --from-path src --rosdistro galactic -y
colcon build --packages-select learning_tf2_cpp

. install/setup.bash
ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0
ros2 topic echo /tf_static

四、静态坐标系变换更合适的广播方式
# 使用欧拉角描述姿态
ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id
ros2 run tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='tf2_ros',
executable='static_transform_publisher',
arguments = ['0', '0', '1', '0', '0', '0', 'world', 'mystaticturtle']
),
])
评论(0)
您还未登录,请登录后发表或查看评论