真实机器人实现多机跟随

一. 思路

根本思路使用tf教程中演示的两个turtlebotsim的案例来实现。

首先，构建完整的tf_tree
第一个机器人或者是遥控控制，或者是键盘控制
第二个机器人根据tf变换关系，将与第一个机器人之间的距离转换为速度指令，然后执行命令。

二. 实现

首先是将笔记本配置为主机，将两个机器人分别配置为从机，构建统一的ros通讯架构。

1. 1号机器人

1号机器人需要持续不断的往外发布里程计的信息。底盘需要往外一直不断的发布/odom的消息。

我们只需要将机器人的底盘启动起来即可。

维护tf_tree

首先将odom和world的tf连接起来。

查看机器人1号对应的odom数据结构：

其中最重要的是pose信息。我们需要订阅pose信息，然后将其发布到tf上。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

std::string turtle_name;

void poseCallback(const nav_msgs::OdometryConstPtr& msg){
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, 0.0) );
  tf::Quaternion q;
  q.setX(msg->pose.pose.orientation.x);
  q.setY(msg->pose.pose.orientation.y);
  q.setZ(msg->pose.pose.orientation.z);
  q.setW(msg->pose.pose.orientation.w);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv){
  ros::init(argc, argv, "robot1_broadcaster");
  if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
  turtle_name = argv[1];

  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/odom", 10, &poseCallback);

  ros::spin();
  return 0;
}

运行的时候注意将robot_name也写上。

2.2号机器人

2号机器人需要做两件事情：

1. 将自身的pose发布到同样一颗tf tree上，与1号机器人之间构建坐标变换。
2. 编写node节点，定于tf，然后根据/robot2 -> /robot1的坐标变换关系，转换为速度指令/cmd_vel发布给robot2.

三、总结

其中，如何将机器人之间的坐标变换关系转换到速度指令，是比较关键的。当前我们直接按照机器人之间的x,y的间隔距离，按照比例的关系就直接映射到速度阈值中。
距离为0的时候，速度也为0；距离大的时候，速度也大。