一. 思路
根本思路使用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号机器人需要做两件事情:
- 将自身的pose发布到同样一颗tf tree上,与1号机器人之间构建坐标变换。
- 编写node节点,定于tf,然后根据
/robot2 -> /robot1
的坐标变换关系,转换为速度指令/cmd_vel发布给robot2.
三、总结
其中,如何将机器人之间的坐标变换关系转换到速度指令,是比较关键的。当前我们直接按照机器人之间的x,y的间隔距离,按照比例的关系就直接映射到速度阈值中。
距离为0的时候,速度也为0;距离大的时候,速度也大。