移动分析

案例设计

通过指定目标点的X和Y值，让小乌龟到达指定点。

完整初始化代码

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
#include <iostream>
#include "ros/ros.h"

#include "QApplication"
#include "QtWidgets"

using namespace std;

void clickDone(QLineEdit *xEdit, QLineEdit *yEdit) {
    double distX = xEdit->text().toDouble();
    double distY = yEdit->text().toDouble();

    ROS_INFO_STREAM("distX: " << distX << "  distY:" << distY);
}

int main(int argc, char **argv) {
    string nodeName = "turtle_control";

    // 创建节点
    ros::init(argc, argv, nodeName, ros::init_options::AnonymousName);
    ros::NodeHandle node;

    // 创建Qt程序
    QApplication app(argc, argv);
    //窗体
    QWidget window;
    window.setWindowTitle("小乌龟控制器");
    window.resize(400, 0);
    //布局
    QFormLayout layout;
    window.setLayout(&layout);

    // 目标x坐标
    QLineEdit xEdit("10.54444");
    layout.addRow("x坐标", &xEdit);

    // 目标y坐标
    QLineEdit yEdit("5.54444");
    layout.addRow("y坐标", &yEdit);

    //当前姿态坐标信息
    QHBoxLayout poseLayout;
    layout.addRow(&poseLayout);

    QFormLayout xLayout;
    QLabel xLb;
    xLayout.addRow("x坐标", &xLb);
    poseLayout.addLayout(&xLayout);

    QFormLayout yLayout;
    QLabel yLb;
    yLayout.addRow("y坐标", &yLb);
    poseLayout.addLayout(&yLayout);

    QFormLayout thetaLayout;
    QLabel thetaLb;
    thetaLayout.addRow("角度", &thetaLb);
    poseLayout.addLayout(&thetaLayout);

    // 执行按钮
    QPushButton btnDone("执行");
    layout.addRow(&btnDone);

    // 事件设置
    btnDone.connect(&btnDone, &QPushButton::clicked, bind(&clickDone, &xEdit, &yEdit));

    window.show();

    return app.exec();
}

运动分析

小乌龟如何到达目标点?

数据获取

小乌龟当前的坐标和角度

可以通过订阅/turtle1/pose获得相关的信息

控制操作

通过设置小乌龟的线速度和角速度可以让小乌龟动起来

可以通过发布数据到/turtle1/cmd_vel控制小乌龟移动

示例代码

发布者与订阅者

1
2
3
4
5
6
7
8
//小乌龟控制地址
string velTopicName = "/turtle1/cmd_vel";
//小乌龟数据获得
string poseTopicName = "/turtle1/pose";
// 创建小乌龟移动发布者
ros::Publisher &&publisher = node.advertise<geometry_msgs::Twist>(velTopicName, 1000);
// 创建小乌龟位置的订阅者
const ros::Subscriber &subscriber = node.subscribe(poseTopicName, 1000, poseCallback);

获取小乌龟实时位置信息的回调

1
2
3
4
5
6
7
8
9
void poseCallback(const turtlesim::Pose::ConstPtr &msg) {
    ROS_INFO_STREAM("x: " << msg->x);
    ROS_INFO_STREAM("y: " << msg->y);
    ROS_INFO_STREAM("theta: " << msg->theta);
    ROS_INFO_STREAM("linear: " << msg->linear_velocity);
    ROS_INFO_STREAM("angular: " << msg->angular_velocity);
    ROS_INFO_STREAM("degrees: " << msg->theta * 180 / M_PI);
    ROS_INFO_STREAM("=========================================");
}

直线运动计算

通过最简单的示例，先解决指线运动。

1
2
距离 = 速度 * 时间
速度 = 距离 / 时间

我们已知的是当前小乌龟的坐标turtle(x, y) ，和目标点dist(x, y)，我们要去得到是小乌龟的速度。

首先我们需要计算出两点间的距离：

1
distance = sqrt(pow(srcX - distX) + pow(srcY - distY))

其次我们需要确定的是时间time，我们可以给定一个预期的值。

我们可以将计算的结果进行运行测试。

测试发现，小乌龟默认运行的时间是1s。没有提供给我们设置时间接口。

解决时间不可控的问题

方案一:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
ros::Rate rate(1);
for (int i = 0; i < 5; ++i) {
    //设置速度
    geometry_msgs::Twist twist;
    twist.linear.x = linearVel;
    twist.angular.z = 0;
    publisher.publish(twist);

    rate.sleep();
 }

方案二：

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
double runDistance = 0;
while (runDistance < distance) {

    //设置速度
    geometry_msgs::Twist twist;
    twist.linear.x = linearVel;
    twist.angular.z = 0;
    publisher.publish(twist);

    rate.sleep();

    runDistance += linearVel / hz;
}

方案三:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
while (calcDistance(srcX, srcY, distX, distY) > 0.1) {
      // 获取srcX，srcY
      srcX = pose->x;
      srcY = pose->y;

      //设置速度
      geometry_msgs::Twist twist;
      twist.linear.x = linearVel;
      twist.angular.z = 0;
      publisher.publish(twist);
      rate.sleep();
}

思考：是否存在完美的事物

  • 速度是否是绝对平均
  • 距离差值是否是绝对为0
  • 时间是否绝对为预期