之前博客《ROS实验笔记之——基于Prometheus自主无人机开源项目的学习与仿真》已经介绍过Prometheus项目。
本博文继续学习其中的控制模块～
ROS无人机仿真之轨迹跟踪
ROS无人机仿真之航点跟踪
目录
仿真功能启动脚本
Control model
px4_sender
px4_pos_controller
基于Gazebo的轨迹追踪仿真
首先需要注意的是，要运行这个项目需要保证.bashrc文件里面的设置正确，如下所示：
一旦更新了代码后，需要运行下面命令进行编译
cd Prometheus
./compile_gazebo.sh好～接下来就具体看看代码。对代码的理解写在了注释里面～
最基本的仿真启动文件（/home/kwanwaipang/Prometheus/Simulator/gazebo_simulator/launch_basic/sitl.launch）
roslaunch prometheus_gazebo sitl.launch使用px4_sender对飞机进行控制测试（/home/kwanwaipang/Prometheus/Simulator/gazebo_simulator/launch_control/sitl_control.launch）
roslaunch prometheus_gazebo sitl_control.launch 使用px4_pos_controller对飞机进行控制测试
roslaunch prometheus_gazebo sitl_pos_control.launch航点追踪
roslaunch prometheus_gazebo sitl_waypoint_tracking.launch键盘控制命令：rosrun prometheus_control terminal_control
给飞行控制器发送各种期望值（实际控制算法运行在飞控中）
获取无人机的姿态
* mavros位置估计程序
* 1. 订阅激光SLAM (cartorgrapher_ros节点) 发布的位置信息,从laser坐标系转换至NED坐标系
* 2. 订阅Mocap设备 (vrpn-client-ros节点) 发布的位置信息，从mocap坐标系转换至NED坐标系
* 3. 订阅飞控发布的位置、速度及欧拉角信息，作对比用
* 4. 存储飞行数据，实验分析及作图使用
* 5. 选择激光SLAM或者Mocap设备作为位置来源，发布位置及偏航角(xyz+yaw)给飞控
位置环控制代码，订阅无人机当前位置及期望位置。并通过位置控制器进行解算得到期望姿态角。最终把期望姿态角发送到PX4飞控中，由PX4飞控进行姿态环控制。
而目前工程中已经实现了5种位置环控制器（/home/kwanwaipang/Prometheus/Modules/control/include/Position_Controller）
而提供的轨迹追踪测试在：/home/kwanwaipang/Prometheus/Modules/control/include/controller_test.h（目前提供了圆形、8字、阶跃）
roslaunch prometheus_gazebo sitl_pos_control.launchterminal_control终端中选择command input control,首先输入999进行解锁并切换至offboard模式 ，并起飞在rviz中，较粗较短的轴代表期望位姿，较长较细的为当前位姿，绿色的为期望轨迹，红色的为运行轨迹。
terminal_control.yamlstate_fromposehistory_window来修改轨迹的长短sitl_pos_control.launch文件如下所示：
<launch>
<!-- 启动PX4中的SITL功能 -->
<!-- 这里的环境变量将传递到rcS启动脚本中-->
<!-- 模型选择 -->
<!-- p450仿真模型 -->
<!-- 参看 ~/prometheus_px4/ROMFS/px4fmu_common/init.d-posix/1045_p450 中的修改内容 -->
<env name="PX4_SIM_MODEL" value="p450" />
<!-- 估计器参数选择 可选ekf2_vision和ekf2_gps-->
<!-- ekf2_gps 使用GPS作为定位来源， ekf2_vision 使用外部输入（gazebo真值、slam等）作为定位来源-->
<!-- 参看 ~/prometheus_px4/ROMFS/px4fmu_common/init.d-posix/rcS 中的修改内容 -->
<env name="PX4_ESTIMATOR" value="ekf2_vision" />
<!-- 仿真速度因子 1.0代表与真实时间同步，大于1加快仿真速度，小于1则减慢 （电脑性能较差，可选择减小该参数）-->
<env name="PX4_SIM_SPEED_FACTOR" value="1.0" />
<!-- PX4 configs -->
<arg name="interactive" default="true"/>
<!-- PX4 SITL -->
<arg unless="$(arg interactive)" name="px4_command_arg1" value="-d"/>
<arg if="$(arg interactive)" name="px4_command_arg1" value=""/>
<!-- 节点源文件路径: ~/Firmware_v110/platforms/posix/src/px4/common/main.cpp -->
<node name="sitl" pkg="px4" type="px4" output="screen"
args="$(find px4)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS $(arg px4_command_arg1)"/>
<!-- 启动Gazebo -->
<!-- Gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<arg name="respawn_gazebo" default="false"/>
<arg name="world" default="$(find prometheus_gazebo)/worlds/empty.world"/>
<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
<arg name="world_name" value="$(arg world)"/>
</include>
<!-- Spawn vehicle model -->
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_ros/scripts/spawn_model -->
<arg name="x" default="1.0"/>
<arg name="y" default="1.0"/>
<arg name="z" default="0.2"/>
<arg name="R" default="0"/>
<arg name="P" default="0"/>
<arg name="Y" default="0.0"/>
<arg name="sdf" default="$(find prometheus_gazebo)/amov_models/p450/p450.sdf"/>
<arg name="model" default="p450"/>
<node name="$(anon vehicle_spawn)" pkg="gazebo_ros" type="spawn_model" output="screen"
args="-sdf -file $(arg sdf) -model $(arg model) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)">
</node>
<!-- 启动MAVROS -->
<node pkg="mavros" type="mavros_node" name="mavros" output="screen">
<param name="fcu_url" value="udp://:14540@localhost:14557" />
<param name="gcs_url" value="" />
<param name="target_system_id" value="1" />
<param name="target_component_id" value="1" />
<rosparam command="load" file="$(find prometheus_gazebo)/config/mavros_config/px4_pluginlists.yaml" />
<rosparam command="load" file="$(find prometheus_gazebo)/config/mavros_config/px4_config.yaml" />
</node>
<!-- TF transform -->
<include file="$(find prometheus_gazebo)/launch_basic/tf_transform.launch">
<arg name="x" value="$(arg x)"/>
<arg name="y" value="$(arg y)"/>
<arg name="z" value="$(arg z)"/>
</include>
<!-- 启动Prometheus代码 -->
<!-- run the px4_pos_estimator.cpp -->
<arg name="input_source" default="2"/>
<node pkg="prometheus_control" type="px4_pos_estimator" name="px4_pos_estimator" output="screen">
<!-- 定位数据输入源 0 for vicon， 1 for 激光SLAM, 2 for gazebo ground truth, 3 for T265 -->
<param name="input_source" value="$(arg input_source)" />
<param name="offset_x" value="$(arg x)" />
<param name="offset_y" value="$(arg y)" />
<param name="offset_z" value="$(arg z)" />
</node>
<node pkg="prometheus_control" type="px4_pos_controller" name="px4_pos_controller" output="screen">
<rosparam command="load" file="$(find prometheus_gazebo)/config/prometheus_control_config/px4_pos_controller.yaml"/>
</node>
<!-- run the ground_station.cpp -->
<node pkg="prometheus_station" type="ground_station" name="ground_station" output="screen" launch-prefix="gnome-terminal --tab --">
</node>
<!-- run the ground_station_msg.cpp -->
<node pkg="prometheus_station" type="ground_station_msg" name="ground_station_msg" output="screen" launch-prefix="gnome-terminal --tab --">
</node>
<!-- run the terminal_control.cpp -->
<node pkg="prometheus_control" type="terminal_control" name="terminal_control" output="screen" launch-prefix="gnome-terminal --">
<rosparam command="load" file="$(find prometheus_gazebo)/config/prometheus_control_config/terminal_control.yaml" />
</node>
<!-- run the rviz -->
<arg name="visualization" default="true"/>
<group if="$(arg visualization)">
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find prometheus_gazebo)/config/rviz_config/rviz_controller_test.rviz" />
</group>
</launch>
仿真结果见开篇的视频～
.bashrc文件备份～
# ~/.bashrc: executed by bash(1) for non-login shells.
# see /usr/share/doc/bash/examples/startup-files (in the package bash-doc)
# for examples
# If not running interactively, don't do anything
case $- in
*i*) ;;
*) return;;
esac
# don't put duplicate lines or lines starting with space in the history.
# See bash(1) for more options
HISTCONTROL=ignoreboth
# append to the history file, don't overwrite it
shopt -s histappend
# for setting history length see HISTSIZE and HISTFILESIZE in bash(1)
HISTSIZE=1000
HISTFILESIZE=2000
# check the window size after each command and, if necessary,
# update the values of LINES and COLUMNS.
shopt -s checkwinsize
# If set, the pattern "**" used in a pathname expansion context will
# match all files and zero or more directories and subdirectories.
#shopt -s globstar
# make less more friendly for non-text input files, see lesspipe(1)
[ -x /usr/bin/lesspipe ] && eval "$(SHELL=/bin/sh lesspipe)"
# set variable identifying the chroot you work in (used in the prompt below)
if [ -z "${debian_chroot:-}" ] && [ -r /etc/debian_chroot ]; then
debian_chroot=$(cat /etc/debian_chroot)
fi
# set a fancy prompt (non-color, unless we know we "want" color)
case "$TERM" in
xterm-color|*-256color) color_prompt=yes;;
esac
# uncomment for a colored prompt, if the terminal has the capability; turned
# off by default to not distract the user: the focus in a terminal window
# should be on the output of commands, not on the prompt
#force_color_prompt=yes
if [ -n "$force_color_prompt" ]; then
if [ -x /usr/bin/tput ] && tput setaf 1 >&/dev/null; then
# We have color support; assume it's compliant with Ecma-48
# (ISO/IEC-6429). (Lack of such support is extremely rare, and such
# a case would tend to support setf rather than setaf.)
color_prompt=yes
else
color_prompt=
fi
fi
if [ "$color_prompt" = yes ]; then
PS1='${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ '
else
PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ '
fi
unset color_prompt force_color_prompt
# If this is an xterm set the title to user@host:dir
case "$TERM" in
xterm*|rxvt*)
PS1="\[\e]0;${debian_chroot:+($debian_chroot)}\u@\h: \w\a\]$PS1"
;;
*)
;;
esac
# enable color support of ls and also add handy aliases
if [ -x /usr/bin/dircolors ]; then
test -r ~/.dircolors && eval "$(dircolors -b ~/.dircolors)" || eval "$(dircolors -b)"
alias ls='ls --color=auto'
#alias dir='dir --color=auto'
#alias vdir='vdir --color=auto'
alias grep='grep --color=auto'
alias fgrep='fgrep --color=auto'
alias egrep='egrep --color=auto'
fi
# colored GCC warnings and errors
#export GCC_COLORS='error=01;31:warning=01;35:note=01;36:caret=01;32:locus=01:quote=01'
# some more ls aliases
alias ll='ls -alF'
alias la='ls -A'
alias l='ls -CF'
# Add an "alert" alias for long running commands. Use like so:
# sleep 10; alert
alias alert='notify-send --urgency=low -i "$([ $? = 0 ] && echo terminal || echo error)" "$(history|tail -n1|sed -e '\''s/^\s*[0-9]\+\s*//;s/[;&|]\s*alert$//'\'')"'
# Alias definitions.
# You may want to put all your additions into a separate file like
# ~/.bash_aliases, instead of adding them here directly.
# See /usr/share/doc/bash-doc/examples in the bash-doc package.
if [ -f ~/.bash_aliases ]; then
. ~/.bash_aliases
fi
# enable programmable completion features (you don't need to enable
# this, if it's already enabled in /etc/bash.bashrc and /etc/profile
# sources /etc/bash.bashrc).
if ! shopt -oq posix; then
if [ -f /usr/share/bash-completion/bash_completion ]; then
. /usr/share/bash-completion/bash_completion
elif [ -f /etc/bash_completion ]; then
. /etc/bash_completion
fi
fi
# Set ROS alias command_
alias cw='cd ~/catkin_ws'
alias cs='cd ~/catkin_ws/src'
alias cm='cd ~/catkin_ws && catkin_make -DCMAKE_BUILD_TYPE=Debug'
alias CM='cd ~/catkin_ws && catkin_make -DCMAKE_BUILD_TYPE=Debug'
alias rb1='ssh ubuntu@192.168.0.103'
alias rb2='ssh ubuntu@192.168.0.104'
alias rb3='ssh ubuntu@192.168.0.106'
alias ekf='roslaunch single_led EKF.launch'
alias save_map='rosrun map_server map_saver -f ~/map/ICDC_slam_map_vlp_00'
alias see_image='rosrun image_view image_view image:=/mvcam/image'
alias open='cd ~/demo && sh launch_top.sh'
alias slam='cd ~/demo/slam_demo && sh launch_slam.sh'
alias keyboard='roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch'
alias get_result='rosrun single_led getpositioning_3rb'
alias debug='cd ~/demo/debug && sh launch_test.sh'
# Set ROS Kinetic
export TURTLEBOT3_MODEL=burger
#export TURTLEBOT3_MODEL=waffle
source /opt/ros/melodic/setup.bash
source ~/catkin_ws/devel/setup.bash
source /home/kwanwaipang/Prometheus/devel/setup.bash
# Set ROS Network
###########my iphone
#export ROS_HOSTNAME=172.20.10.11
#export ROS_MASTER_URI=http://172.20.10.11:11311
#############liphy company
#export ROS_HOSTNAME=192.168.11.155
#export ROS_MASTER_URI=http://192.168.11.155:11311
###############wen lan yuan
#export ROS_HOSTNAME=192.168.0.108
#export ROS_MASTER_URI=http://192.168.0.108:11311
#####my home
#ROS_MASTER_URI，主节点IP
#export ROS_HOSTNAME=10.79.138.249
#export ROS_HOSTNAME=192.168.1.105
#export ROS_HOSTNAME=192.168.1.12
#export ROS_HOSTNAME=192.168.144.229
#export ROS_HOSTNAME=192.168.0.110
#export ROS_HOSTNAME=192.168.0.111
#export ROS_HOSTNAME=192.168.0.112
#source ~/.bashrc
#ROS_NAMESPACE=tb3_1 rosrun turtlebot3_teleop turtlebot3_teleop_key
#ROS_NAMESPACE=tb3_0 roslaunch turtlebot3_bringup turtlebot3_robot.launch
#export ROS_HOSTNAME=10.79.138.249
#export ROS_MASTER_URI=http://10.79.138.249:11311
#export ROS_HOSTNAME=192.168.0.112
#export ROS_MASTER_URI=http://192.168.0.112:11311
#export ROS_MASTER_URI=http://192.168.0.100:11311
#export ROS_MASTER_URI=http://192.168.0.112:11311
#export ROS_MASTER_URI=http://192.168.144.229:11311
export TURTLEBOT3_MODEL=burger
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/catkin_ws/src/ORB_SLAM2/Examples/ROS
source ~/Prometheus/devel/setup.bash
export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:~/Prometheus/devel/lib
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~/Prometheus/Simulator/gazebo_simulator/models
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~/Prometheus/Simulator/gazebo_simulator/amov_models
source ~/prometheus_px4/Tools/setup_gazebo.bash ~/prometheus_px4 ~/prometheus_px4/build/amovlab_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/prometheus_px4
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/prometheus_px4/Tools/sitl_gazebo