增加功能包报错

[githubzhouzz]

你好。我在原代码基础上增加了一个领航-跟随功能包，出现了报错：

[ WARN] [1714033954.065190431, 14.425000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame robot2 at time 0.000000 according to authority unknown_publisher

发布代码：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
#include <nav_msgs/Odometry.h>
// std::string turtle_name;
std::string robot_name;

void poseCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
// 创建tf的广播器
static tf::TransformBroadcaster br;
static tf::TransformBroadcaster br0;
static tf::TransformBroadcaster br1;

// 初始化tf数据
tf::Transform transform;
tf::Transform transform0;
tf::Transform transform1;

transform.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, 0.0) );
double roll, pitch, yaw;
tf::Quaternion q;
tf::Quaternion quat;
tf::quaternionMsgToTF(msg->pose.pose.orientation, quat);
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
q.setRPY(0.0, 0.0, yaw);
transform.setRotation(q);
// 广播world与海龟坐标系之间的tf数据
br.sendTransform(tf::StampedTransform(transform, ros::Time(0), "world", "robot1"));

transform0.setOrigin( tf::Vector3((msg->pose.pose.position.x)-0.5, (msg->pose.pose.position.y)+1.0, 0.0) );//初始化  相距0.6m,xunizuobiao x,yzhi
transform0.setRotation( tf::Quaternion(0, 0, 0, 1) );
br0.sendTransform(tf::StampedTransform(transform0, ros::Time(0), "world", "robot2"));

transform1.setOrigin( tf::Vector3((msg->pose.pose.position.x)-0.5, (msg->pose.pose.position.y)-1.0, 0.0) );//初始化  相距0.6m,xunizuobiao x,yzhi
transform1.setRotation( tf::Quaternion(0, 0, 0, 1) );
br1.sendTransform(tf::StampedTransform(transform1, ros::Time(0), "world", "robot3"));

}

int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_broadcaster1");

// 输入参数作为海龟的名字
if (argc != 2)
{
	ROS_ERROR("need turtle name as argument"); 
	return -1;
}
robot_name = argv[1];
// 订阅海龟的位姿话题
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(robot_name+"/base_pose_ground_truth", 10, &poseCallback);
//ros::Subscriber sub = node.subscribe(robot_name+"/odom", 10, &poseCallback);
// 循环等待回调函数
ros::spin();
return 0;

};

订阅代码：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
//#include "sensor_msgs/LaserScan.h"

void StepTargerVizableLoop(tf::TransformListener &listener1,tf::StampedTransform &transformf1)
{

while (1)
{
try {
	listener1.waitForTransform("/robot2", "/robot1", ros::Time(0), ros::Duration(60.0) );
    listener1.lookupTransform("/robot2", "/robot1",ros::Time(0), transformf1);
}
catch (tf::TransformException &ex) 
	{
		ROS_ERROR("%s",ex.what());
		ros::Duration(1.0).sleep();
		continue;
	}
}

}

int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_listener1");

// 创建节点句柄
ros::NodeHandle node;

// 获取turtle1与turtle2坐标系之间的tf数据
tf::StampedTransform transformf1;

// 创建发布robot2速度控制指令的发布者
ros::Publisher robot2_vel = node.advertise<geometry_msgs::Twist>("/robot2/cmd_vel", 10);

// 创建tf的监听器
tf::TransformListener listener1;

ros::Rate rate(10.0);
while (node.ok())
{
	StepTargerVizableLoop(listener1,transformf1);

	// 根据robot1与robot2坐标系之间的位置关系，发布turtle2的速度控制指令
	geometry_msgs::Twist vel_msg;
	vel_msg.angular.z = 4.0 * atan2(transformf1.getOrigin().y(),
			                        transformf1.getOrigin().x());
	vel_msg.linear.x = 0.5 * sqrt(pow(transformf1.getOrigin().x(), 2) +
			                      pow(transformf1.getOrigin().y(), 2));
	robot2_vel.publish(vel_msg);

	rate.sleep();
}
return 0;

};