ROS发布静态坐标变化

消息类型

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/Transform transform
  geometry_msgs/Vector3 translation
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion rotation
    float64 x
    float64 y
    float64 z
    float64 w

例程(方法一)

#include <ros/ros.h>
#include "tf2_ros/static_transform_broadcaster.h"
#include"geometry_msgs/TransformStamped.h" //静态坐标消息类型
#include "tf2/LinearMath/Quaternion.h" //欧拉角-四元数转换
#include <bits/stdc++.h> //c++标准库

int main(int argc, char  *argv[]){
    ros::init(argc,argv,"static_tf"); //初始化节点
    ros::NodeHandle nh; //创建句柄
    tf2_ros::StaticTransformBroadcaster pub ; //创建发布者


    int id = 101; //初始tag的编号
    double x = 0.3, y = 0.3; //初始tag的坐标
    for(int i = 1; i <= 9; i ++){
        for(int j = 1; j <= 13; j ++){
            geometry_msgs::TransformStamped ts; //创建geometry_msgs命名空间下TransformStamped类型的变量,名为ts
            ts.header.seq = 100; // consecutively increasing ID 
            ts.header.stamp = ros::Time::now(); //时间戳
            ts.header.frame_id = "map"; //父坐标系名称(数据类型string)
            ts.child_frame_id = "tag_" + std::to_string(id); //子坐标系名称(数据类型string)
            ts.transform.translation.x = x; //将计算出的坐标封装在ts中
            ts.transform.translation.y = y;
            ts.transform.translation.z = 0;

            tf2::Quaternion qtn; //创建四元数变量
            qtn.setRPY(1.57,0,0); //将欧拉角转化为四元数(因为在TransformStamped类型中,用四元数表示位姿)
            ts.transform.rotation.x = qtn.getX(); //将计算出的欧拉角封装在ts变量中
            ts.transform.rotation.y = qtn.getY();
            ts.transform.rotation.z = qtn.getZ();
            ts.transform.rotation.w = qtn.getW();
        
            pub.sendTransform(ts); //发送静态坐标信息

            id ++;
            x += 0.6;
        }
        y += 0.6;
        x = 0.3;
    }

    ros::spin(); //阻塞程序,防止节点运行完自动关闭.
    return 0;
}

例程(方法二)

通过调用tf2_ros中的工具,通过launch文件发布静态变换

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

static_transform_publisher x y z qx qy qz qw frame_id child_frame_id

<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1" />
</launch>

通过简单程序批量生成以上格式的字符串即可。

效果