table of Contents
Simulation experiment
The sail machine of the twin sailboat is connected to the two sails by ropes, and the actual angle of the sail will be affected by the wind, which is between 0 degrees and the output angle of the sail machine. In the simulation process, it is assumed that the actual angle of the sail is the output angle of the sail machine.
1. Start the graphical interface
Open a new terminal and run the following command to start the first ROS node.
roslaunch sailboat_launch start_tf_tree_onboat.launch
If no error is reported, Ctrl+ Alt+ TOpen a new terminal and run the following command to start the second node.
roslaunch spare_function spare_function_simulation.launch
If no error is reported, Ctrl+ Alt+ TOpen a new terminal and enter the simulation folder. This directory needs to be modified according to the directory you installed, it may be like this:
cd ~/sailboat_ws/src/Sailboat-Ros/Sh/simulation
It could also be like this:
cd ~/sailboat_ws/src/2019-sailboat-ros/Sailboat-Ros/Sh/simulation
The command cd means to enter a directory, followed by ~/ means the directory "home/your username". In short, the catalog needs to be modified according to the actual situation. You can also use the mouse and graphical interface to manually open the simulation folder, and then right-click to select Open New Terminal.
After entering the simulation folder, run the interface.sh script file in the folder. The command is as follows:
./interface.sh
After waiting a few seconds, two applications, rviz and rqt, will be launched one after another. You can see the text icon of rviz and the rqt icon with nine blue dots in the task bar on the left.
Click the corresponding option in rviz to display the path of the sailboat.
Check the corresponding checkboxes in rqt and expand the corresponding topics to observe the current input and output of the sailboat.
2. Run the python file
Finally, I need to start the program I wrote for the sailboat to output the angle of the sail and rudder.
cd ~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/scripts
python example.py
Similarly, the directory needs to be determined according to the location of your python file. The example.py in the above command is the sample code, and the angle of the sail and rudder is always 0 degrees. After writing the sailing program by yourself, you need to use the python command to run your own python file. If there is an error when running the python file, please check the corresponding line of the python file you wrote for the problem.
3. Debug
If your python file does not report an error, it means that the program has been grammatically compiled. Next, you need to check whether there is a logical problem and whether the sailboat can drive as expected. At this time, you need to use rviz to observe the path of the sailboat, and to observe the output data through rqt. Check whether it can operate normally under all conditions by adjusting dynamic parameters.
How to customize the data displayed in rqt and how to adjust dynamic parameters (or what are dynamic parameters) will be explained in detail in the next chapter.
The relationship between python files and rqt
The location of the python file:
~/sailboat_ws/src/Sailboat-Ros/Class/spare_function/scripts
1. Get sensor data
When designing a control strategy for sails and rudders, sensor data is needed. The file imports the sensor message file Sensor_msg at the beginning.
from sailboat_message.msg import Sensor_msg
Subsequently, the program uses sensor data in the third function.
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
This function parameter msg is obtained from ROS subscription, we can see the function call in the main function:
rospy.Subscriber("sensor", Sensor_msg, sensorCallback)
Back to the third function, the sensor data file is located
~/sailboat_ws/src/Sailboat-Ros/Message/sailboat_message/msg
Open this file, we can see that the sensor data has these:
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
In the example file example.py, the sensorCallback function uses only three of the data (Roll, Yaw, AWA). If we need other sensor data, we assign the required value to the global variable array sensor_submsg in the sensorCallback function, and then Modify the length of the global variable array at the beginning of the program. For example, if I want to use relative wind speed AWS, I should do this:
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
In this way, when the main function needs to use AWS, it only needs to call the global variable sensor_submsg[3].
2. Output data to rqt
If you want to output some self-defined data to rqt to facilitate debugging, you need to modify the file spare_function_out.msg, the file directory is as follows
/sailboat_ws/src/Sailboat-Ros/Class/spare_function/msg
After opening the file, you can see that currently there are only sail and rudder data
float64 sail
float64 rudder
If we define a boolean variable named is_tacking in the main function to determine whether the sailboat is currently downwind or upwind, we need to modify the spare_function_out.msg file to
float64 sail
float64 rudder
bool is_tacking_publish
In addition, you also need to change your own python program, such as the main function of example.py to
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]
# 后面的内容此处省略
Then modify the second function outside the main function to
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
Then you can see the output in rqt. In the above example, the two variables of is_tacking_publish and is_tacking can be set to the same name, or they can be different. This is to make it easier to see the relationship between the variables.
For example, to show the effect, here is_tacking is set to 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]
# 后面的内容此处省略
Because the msg file is modified according to the above operation, the workspace needs to be recompiled.
Tip: You do not need to recompile after only modifying the python type file, and you need to compile if you modify other files .
cd ~/sailboat_ws
catkin clean
提示是否要清理，输入 y ，回车确认
catkin build
After the recompilation is complete, follow the steps in Chapter 1 to start the graphical interface and run the python file . Check spare_function_out in rqt and expand it, you can see that the parameter is_tacking_publish defined by yourself can already be displayed at this time.
3. Use dynamic parameters
Dynamic parameters are parameters that can be modified in real time during the running of the program. The program will use the latest modified dynamic parameters for calculation. Suppose I write a program that keeps the sailing boat heading. I want to modify the target heading after the sailing boat runs for a period of time, but I don't want to open the python program to manually modify the corresponding variables. You can use dynamic parameters to simplify this operation. In addition, the dynamic parameters can also be adjusted in real time during the actual ship experiment.
The files and directories related to dynamic parameters are as follows:
文件 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
If you need to add or modify the dynamic parameter variable name, first modify or add in spare_function_para.msg, and then modify or add in spare_function.cfg. In spare_function.cfg, if the lower limit and upper limit of the target heading oyaw are modified to -180 and 180, and the default value is modified to 91, you can do this:
文件 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)
If the variable name of the dynamic parameter is modified or added, the getOutParaPut function and getConfigCallback function need to be modified in the python file, and the global variable para_cfg at the beginning of the file also needs to be modified accordingly.
When using dynamic parameters, you only need to use the array para_cfg.
Similarly, if you modify the msg file or cfg file, you need to recompile.
For example, in addition to modifying the default value and range of the dynamic parameters, I also want to use the dynamic parameters in the main program and output the target course to rqt, then I can do this:
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
Then recompile the workspace, start the graphical interface, and run the python file.
In the rqt interface, click refresh in the Dynamic Reconfigure module above to refresh the dynamic parameter list. Then click on the option of a string of numbers after example_ to open the dynamic parameters just set. Click wind_simulation to adjust the dynamic parameters of the simulated wind speed and direction.
Check spare_function_out and spare_function_para in Topic Monitor to view the corresponding data. Then drag the cursor of the dynamic parameter, you can see that the data below will also change accordingly.
Real ship experiment
Note: If you want to adjust the dynamic parameters in the actual ship experiment, you need to write the name of your computer to the beginning of a configuration file on the ship's motherboard before the experiment, so that you can get the permission to modify the dynamic parameters. For details, please refer to the question summary "Multi-machine communication.md". If you do not have this document, please contact the teaching assistant to obtain it.
Before the experiment, you need to copy all the modified files to the main board on board: please pay attention to distinguish between the local port and the remote login port on the main board
登录 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
Then start the program on board and program on shore in turn, and then record the data. After starting the shore program, your computer will jump out of the graphical interface of rqt and rviz, and all subsequent operations are the same as during simulation.