案例二

效果展示

开发流程

1. 建立Broadcaster,将小乌龟1坐标发送给TF工具

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
void callback(const turtlesim::Pose::ConstPtr &message, tf::TransformBroadcaster broadcaster) {
    tf::Transform transform;
    //设置位置
    transform.setOrigin(tf::Vector3(message->x, message->y, 0));
    //设置姿态
    tf::Quaternion quaternion;
    quaternion.setRPY(0, 0, message->theta);
    transform.setRotation(quaternion);
    broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "turtle1"));
}

Tip

我们在小乌龟pose回调中,得到小乌龟相对于窗体的坐标信息(message)。

broadcaster在发送相对位置信息时,需要给定两个类型的数据:

  • 在相对环境中的 坐标(origin),包含x,y,z坐标
  • 在相对环境中的姿态 (rotation),包含x,y,z方向的转动情况

在发送数据时,要标明谁相对谁的位置。参照物我们认为是父坐标。

2. 建立Broadcaster,将小乌龟2坐标发送给TF工具

参考第1步骤

3. 构建一个相对于小乌龟1的坐标点,将点的坐标发送给TF工具

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
//订阅tf 转换广播
tf::TransformBroadcaster broadcaster;

ros::Rate rate(10);
while (ros::ok()) {
    tf::Transform transform;
    //设置位置
    transform.setOrigin(tf::Vector3(-1, 0, 0));
    //设置姿态
    tf::Quaternion quaternion;
    quaternion.setRPY(0, 0, 0);
    transform.setRotation(quaternion);
    broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "turtle1back"));

    rate.sleep();
}

4. 建立Listener,通过查看TF工具获得小乌龟1和小乌龟2间的相对坐标

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
tf::TransformListener listener;
ros::Rate rate(10);
while (ros::ok()) {
    tf::StampedTransform transform;
    try {
        listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
    } catch (exception e) {
        ROS_INFO_STREAM(e.what());
        rate.sleep();
        continue;
    }
    tf::Vector3 &origin = transform.getOrigin();
    double x = origin.x();
    double y = origin.y();

    const tf::Quaternion &quaternion = transform.getRotation();
    double theta = quaternion.z();
}

Tip

转换监听器listener可以从TF工具中,获得想要的两个物体间的相对坐标。

前一个参数是作为参照物存在的,坐标和姿态都是(0,0,0)

第二个参数是相对于第一个参数的参照物的坐标和姿态。

5. 运动规划

1
2
3
4
5
6
7
double distance = sqrt(pow(x, 2) + pow(y, 2));
double angular = atan2(y, x);

geometry_msgs::Twist msg;
msg.linear.x = distance * 0.5;
msg.angular.z = angular * 3;
publisher.publish(msg);