案例三

效果展示

开发流程

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
17
18
19
//订阅tf 转换广播
tf::TransformBroadcaster broadcaster;

double hz = 20;
ros::Rate rate(hz);
double t = 0;
while (ros::ok()) {

    tf::Transform transform;
    //设置位置
    transform.setOrigin(tf::Vector3(5.0*sin(t), 5.0*cos(t), 0));
    //设置姿态
    tf::Quaternion quaternion;
    quaternion.setRPY(0, 0, 0);
    transform.setRotation(quaternion);
    broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "turtle1back"));
    t+= (2*M_PI/hz) / 2;
        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 * 1.5;
msg.angular.z = angular * 9;
publisher.publish(msg);