之前博客《ROS实验笔记之——基于Prometheus自主无人机开源项目的学习与仿真》已经介绍过Prometheus项目。

本博文继续学习其中的控制模块～

ROS无人机仿真之轨迹跟踪

ROS无人机仿真之航点跟踪

首先需要注意的是，要运行这个项目需要保证.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

Control model

px4_sender

给飞行控制器发送各种期望值（实际控制算法运行在飞控中）

px4_pos_estimator

获取无人机的姿态

* mavros位置估计程序

* 1. 订阅激光SLAM (cartorgrapher_ros节点) 发布的位置信息,从laser坐标系转换至NED坐标系

* 2. 订阅Mocap设备 (vrpn-client-ros节点) 发布的位置信息，从mocap坐标系转换至NED坐标系

* 3. 订阅飞控发布的位置、速度及欧拉角信息，作对比用

* 4. 存储飞行数据，实验分析及作图使用

* 5. 选择激光SLAM或者Mocap设备作为位置来源，发布位置及偏航角(xyz+yaw)给飞控

px4_pos_controller

位置环控制代码，订阅无人机当前位置及期望位置。并通过位置控制器进行解算得到期望姿态角。最终把期望姿态角发送到PX4飞控中，由PX4飞控进行姿态环控制。

而目前工程中已经实现了5种位置环控制器（/home/kwanwaipang/Prometheus/Modules/control/include/Position_Controller）

而提供的轨迹追踪测试在：/home/kwanwaipang/Prometheus/Modules/control/include/controller_test.h（目前提供了圆形、8字、阶跃）

基于Gazebo的轨迹追踪仿真

运行launch文件 roslaunch prometheus_gazebo sitl_pos_control.launch
在terminal_control终端中选择command input control,首先输入999进行解锁并切换至offboard模式，并起飞
输入4选择Move移动模式，再次输入4进入轨迹追踪子模式，并根据提示输入指令选择所需要测试的内容
在rviz或ground_station终端中可观察无人机的追踪情况
在rviz中，较粗较短的轴代表期望位姿，较长较细的为当前位姿，绿色的为期望轨迹，红色的为运行轨迹。
轨迹形状参数请查阅terminal_control.yaml
可通过修改参数state_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

原文链接：https://blog.csdn.net/gwplovekimi/article/details/116521669