模拟实验

双帆船的帆机通过绳索与两个帆连接，帆的实际角度会受到风的影响，在0度和帆机输出到角度之间。在模拟过程中假定帆的实际角度就是帆机输出的角度。

1. 启动图形化界面

打开一个新终端，运行以下命令，启动第一个 ROS 节点。

roslaunch sailboat_launch start_tf_tree_onboat.launch

如果没有报错，Ctrl+Alt+T 打开新终端，运行以下命令，启动第二个节点。

roslaunch spare_function spare_function_simulation.launch

如果没有报错，Ctrl+Alt+T 打开新终端，进入 simulation 文件夹下。此目录需要根据自己安装的目录做修改，可能是这样：

cd ~/sailboat_ws/src/Sailboat-Ros/Sh/simulation

也可能是这样：

cd ~/sailboat_ws/src/2019-sailboat-ros/Sailboat-Ros/Sh/simulation

命令 cd 代表进入某目录，紧接着 ~/ 表示 “home/你的用户名” 这个目录。总之，目录需要自己根据实际状况修改。也可以使用鼠标和图形化界面手动打开 simulation 文件夹，然后鼠标右键选择打开新终端。

进入 simulation 文件夹后，运行文件夹中的 interface.sh 脚本文件。命令如下：

./interface.sh

等待几秒钟后，将会陆续启动 rviz 和 rqt 两个应用程序。左边的任务栏中可以看到 rviz 的文字图标和九个蓝点的 rqt 图标。

在 rviz 中点击相应的选项可以显示帆船的路径。

在 rqt 中勾选相应的复选框并展开对应的话题，可以观察帆船当前的输入和输出。

2. 运行 python 文件

最后需要启动自己为帆船写的程序，用于输出帆和舵的角度。

cd ~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/scripts
python example.py

同样，目录需要根据自己 python 文件的位置决定。上述命令中的 example.py 是示例代码，输出的帆和舵的角度始终为0度。自己编写帆船程序后需要使用 python 命令运行自己的 python 文件。如果在运行 python 文件时有报错，请根据提示检查自己写的 python 文件对应行存在的问题。

3. 调试

如果你的 python 文件没有报错，说明程序在语法上通过了编译，接下来需要检查在逻辑上是否存在问题，帆船是否可以按照预期行驶。这时需要借助 rviz 观察帆船的路径，通过 rqt 观察输出的数据。通过调整动态参数检查是否所有情况下都可以正常运行。

如何自定义 rqt 中显示的数据，以及如何调整动态参数（或者说，动态参数是什么），将在下一章详解。

python 文件与 rqt 的关系

python 文件的位置：

~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/scripts

1. 获取传感器数据

设计帆和舵的控制策略时，需要传感器数据。文件在开头导入了传感器消息文件 Sensor_msg。

from sailboat_message.msg import Sensor_msg

随后，程序在第三个函数中使用了传感器数据。

def sensorCallback(msg): #sailboat_message::Sensor_msg
    global sensor_submsg 
    sensor_submsg[0] = msg.Roll
    sensor_submsg[1] = msg.Yaw
    sensor_submsg[2] = msg.AWA

这个函数参数 msg 是从 ROS 订阅得到的，我们可以在主函数中看到函数的调用：

rospy.Subscriber("sensor", Sensor_msg, sensorCallback)

回到第三个函数，传感器数据文件位于

~/sailboat_ws/src/Sailboat-Ros/Message/sailboat_message/msg

打开这个文件，我们可以看到传感器数据有这些：

Header header
float64 ux
float64 vy
float64 wz
float64 gx
float64 gy
float64 gz
float64 Posx
float64 Posy
float64 PosZ
float64 Roll
float64 Pitch
float64 Yaw
float64 AWA
float64 AWS
float64 TWA
float64 TWS

在示例文件 example.py 中，sensorCallback 函数只使用了其中的三个数据 (Roll, Yaw, AWA)，如果我们需要其它传感器数据，就将需要的值在 sensorCallback 函数中赋值给全局变量数组 sensor_submsg，然后修改程序开头全局变量数组的长度。例如，我希望使用相对风速 AWS，应当这样做：

sensor_submsg = [0,0,0,0]

def sensorCallback(msg): #sailboat_message::Sensor_msg
    global sensor_submsg 
    sensor_submsg[0] = msg.Roll
    sensor_submsg[1] = msg.Yaw
    sensor_submsg[2] = msg.AWA
    sensor_submsg[3] = msg.AWS

这样，主函数需要使用 AWS 时，只需调用全局变量 sensor_submsg[3] 即可。

2. 将数据输出到 rqt

如果希望将一些自己定义的数据输出到 rqt 以方便调试，那么需要修改文件 spare_function_out.msg，文件目录如下

/sailboat_ws/src/Sailboat-Ros/Class/spare_function/msg

打开文件后，可以看到目前里面只有帆和舵的数据

float64 sail
float64 rudder

如果我们在主函数中自己定义了一个名称为 is_tacking 的布尔变量，来判断帆船当前处于顺风状态还是逆风，需要将 spare_function_out.msg 文件修改为

float64 sail
float64 rudder
bool is_tacking_publish

此外，还需要将自己写的 python 程序，例如 example.py 的主函数改为

try:
        while not rospy.is_shutdown():
            mach_np = [ra, sa, 0]
            out_np = [ra, sa, is_tacking]
            #para_np = [0,0,0,0,0,0,0]
            # 后面的内容此处省略

然后将主函数外部的第二个函数修改为

def getOutput(msg): #spare_function::spare_function_out
    out_pub = spare_function_out()
    out_pub.rudder = msg[0]
    out_pub.sail = msg[1]
    out_pub.is_tacking_publish = msg[2]
    return out_pub

然后在 rqt 中就可以看到输出了。上述示例中，可以将 is_tacking_publish 和 is_tacking 两个变量设置为相同的名称，也可以不同，这里是为了方便看清变量之间的关系。

例如，为了显示一下效果，这里将 is_tacking 定为 true

try:
        while not rospy.is_shutdown():
        	ra = 0.5
        	sa = 0.5
        	is_tacking = True
            mach_np = [ra, sa, 0]
            out_np = [ra, sa, is_tacking]
            #para_np = [0,0,0,0,0,0,0]
            # 后面的内容此处省略

因为按照上述操作修改了 msg 文件，所以需要将工作空间重新编译。
提示：只修改 python 类型的文件后不需要重新编译，修改其它文件则需要编译。

cd ~/sailboat_ws
catkin clean
提示是否要清理，输入 y ，回车确认
catkin build

重新编译完成后，按照第一章的步骤启动图形化界面、运行 python 文件。在 rqt 中勾选 spare_function_out 并展开，可以看到此时已经可以显示自己定义的参数 is_tacking_publish 了。

3. 使用动态参数

动态参数就是在程序运行过程中可以实时修改的参数。程序将会使用最新修改的动态参数进行计算。假如我写了一个让帆船保持首向的程序，我希望在帆船运行一段时间后修改目标首向，但又不想把 python 程序打开手动修改对应的变量。这时可以使用动态参数来简化这一操作。此外，动态参数还可以在实船实验时实时调整 PID 参数。
与动态参数相关的文件及目录如下：

文件 spare_function_para.msg 目录如下：
~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/msg
文件 spare_function.cfg 目录如下：
~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/cfg

如需增加或修改动态参数变量名，先在 spare_function_para.msg 中修改或添加，然后在 spare_function.cfg 中修改或添加。在 spare_function.cfg 中，若将目标首向 oyaw 的下限和上限修改为 -180 和 180，将默认值修改为 91，可以这样：

文件 spare_function.cfg

gen.add("PC_Ctrl",   bool_t,   0, "PC_Ctrl",  False)

gen.add("oyaw", double_t, 0, "oyaw",    91,  -180,   180)

gen.add("rudderP", double_t, 0, "rudderP",    0.5,  -10,   10)
gen.add("rudderI", double_t, 0, "rudderI",    0,    -10,   10)
gen.add("rudderD", double_t, 0, "rudderD",    0.2,  -10,   10)

gen.add("sailP", double_t, 0, "sailP",    -0.44,  -10,   10)
gen.add("sailI", double_t, 0, "sailI",    0,    -10,   10)
gen.add("sailD", double_t, 0, "sailD",    0,  -10,   10)

如果对动态参数的变量名进行了修改或增加，则还需要在 python 文件中修改 getOutParaPut 函数和 getConfigCallback 函数，文件开头的全局变量 para_cfg 也需要相应的修改。

使用动态参数时，只需要使用数组 para_cfg 即可。

同样的，如果对 msg 文件或 cfg 文件进行修改，则需要重新编译。

例如，除了修改动态参数的默认值和范围，我还希望在主程序中使用动态参数，并将目标航向输出到 rqt 里，则可以这样做：

python文件：添加输出

try:
        while not rospy.is_shutdown():
            ra = 0.5
            sa = 0.5
            is_tacking = True
	    	oyaw = para_cfg[1]
            mach_np = [ra, sa, 0]
            out_np = [ra, sa, is_tacking, oyaw]
            #para_np = [0,0,0,0,0,0,0]

python文件：添加输出

def getOutput(msg): #spare_function::spare_function_out
    out_pub = spare_function_out()
    out_pub.rudder = msg[0]
    out_pub.sail = msg[1]
    out_pub.is_tacking_publish = msg[2]
    out_pub.oyaw = msg[3]
    return out_pub

spare_function_out.msg

float64 sail
float64 rudder
bool is_tacking_publish
float64 oyaw

然后重新编译工作空间，启动图形化界面，运行 python 文件。
rqt 界面中，在上方的 Dynamic Reconfigure 模块中点击 refresh 刷新一下动态参数列表。然后点击 example_ 后面一串数字的那个选项，可以打开刚刚设置好的动态参数。点击 wind_simulation 则可以调整模拟风速和风向的动态参数。
在 Topic Monitor 中勾选 spare_function_out 和 spare_function_para，查看相应的数据。然后拖动动态参数的游标，可以看到下边的数据也会跟着改变。

实船实验

注意：如果要在实船实验中调整动态参数，则需要在实验前将自己电脑的的名字写入船上主板的某配置文件的开头，这样才可以获取修改动态参数的权限。详见疑问汇总“多机通信.md”。如果没有这个文档，请和助教联系获取。
实验前，需要将所有修改过的文件复制到船上主板中：请注意区分本机端口和远程登录的船上主板端口

登录 sjtu151 端口删除文件 spare_function.cfg
cd ~/sailboat-ws/src/Sailboat-Ros/Class/spare_function/cfg/
rm -f spare_function.cfg
在本机端口复制文件给sjtu151
scp ~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/cfg/spare_function.cfg sjtu151@192.168.1.51:/home/sjtu151/sailboat-ws/src/Sailboat-Ros/Class/spare_function/cfg/

登录sjtu151端口删除文件 spare_function_para.msg
cd ~/sailboat-ws/src/Sailboat-Ros/Class/spare_function/msg/
rm -f spare_function_para.msg
在本机端口复制文件给sjtu151
scp ~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/msg/spare_function_para.msg
sjtu151@192.168.1.51:/home/sjtu151/sailboat-ws/src/Sailboat-Ros/Class/spare_function/msg/

登录sjtu151端口删除文件 spare_function_out.msg
cd ~/sailboat-ws/src/Sailboat-Ros/Class/spare_function/msg/
rm -f spare_function_out.msg
在本机端口复制文件给sjtu151
scp ~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/msg/spare_function_out.msg sjtu151@192.168.1.51:/home/sjtu151/sailboat-ws/src/Sailboat-Ros/Class/spare_function/msg/

登录sjtu151端口删除 python 文件 
cd ~/sailboat-ws/src/Sailboat-Ros/Class/spare_function/scripts
rm -f python文件名
在本机端口复制文件给sjtu151
scp ~/sailboat-ws/src/Sailboat-Ros/Class/spare_function/scripts/example.py sjtu151@192.168.1.51:/home/sjtu151/sailboat_ws/src/Sailboat-Ros/Class/spare_function/scripts/

然后登录sjtu151端口重新编译
cd ~/sailboat_ws
catkin clean
catkin build

然后依次启动船上程序和岸上程序，然后记录数据即可。启动岸上程序之后，你的电脑将会跳出 rqt 和 rviz 的图形化界面，之后所有的操作与模拟时一致。