基于rotors_simulator和ROS的无人机仿真

uav_tracking项目代码

安装 rotors_simulator

1. 新建工作空间并初始化（有工作空间的跳过此步骤）

mkdir -p ~/rotors_ws/src
cd ~/rotors_ws/src
catkin_init_workspace

2. 克隆源代码

cd ~/rotors_ws/src
git clone https://github.com/ethz-asl/rotors_simulator.git

3. 初始化wstool，并更新所需库

wstool init
wget https://raw.githubusercontent.com/ethz-asl/rotors_simulator/master/rotors_hil.rosinstall
wstool merge rotors_hil.rosinstall
wstool update

4. 编译

cd ~/rotors_ws/
catkin_make

5. 环境变量

source ./devel/setup.bash

也可以用下面的方式：

echo "source ~/rotors_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

6. 测试

roslaunch rotors_gazebo mav_hovering_example.launch mav_name:=firefly world_name:=basic

如果安装成功，就能启动gazebo，看到飞机

切换环境和飞机模型

可以在启动时指定参数切换飞机和场景，也可以修改launch文件。

下面以修改mav_hovering_example_with_vi_sensor.launch为例，mav_hovering_example_with_vi_sensor.launch文件位于~/rotors_ws/src/rotors_simulator/rotors_gazebo/launch/路径下：

mav_hovering_example_with_vi_sensor.launch文件：

修改launch文件中的mav_name切换飞机

1. 打开launch文件

   sudo gedit ~/rotors_ws/src/rotors_simulator/rotors_gazebo/launch/mav_hovering_example_with_vi_sensor.launch

2. 第三行参数firefly可以改为~/rotors_ws/src/rotors_simulator/rotors_description/urdf路径中的任意一个飞机名，如iris，ardrone等。

修改launch文件中的world_name切换场景

1. 打开launch文件

   sudo gedit ~/rotors_ws/src/rotors_simulator/rotors_gazebo/launch/mav_hovering_example_with_vi_sensor.launch

2. 第四行参数outdoor可以改为~/rotors_ws/src/rotors_simulator/rotors_gazebo/worlds/路径中的任意一个文件名，如outdoor，plane等。

~/rotors_ws/src/rotors_simulator/rotors_gazebo/worlds/路径下的文件如下图

测试

修改完之后运行

roslaunch rotors_gazebo mav_hovering_example_with_vi_sensor.launch

即可查看修改效果

ps. 如果修改的是其他launch文件，则运行对应的文件查看。

创建直接的 ROS package 来控制飞机

1. 新建 ROS package

cd ~/rotors_ws/src
catkin_create_pkg [PACKAGE_NAME] std_msgs rospy roscpp

如：新建跟踪 package: catkin_create_pkg uav_tracking std_msgs rospy roscpp

2. 修改 uav_tracking/CMakeLists.txt 添加依赖

修改前

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)

修改后

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs

  sensor_msgs
  geometry_msgs
  mav_msgs
  gazebo_msgs
)

• sensor_msgs,添加对ROS官方sensor_msgs消息的依赖,在包的程序中可以使用sensor_msgs/xxx类型的消息;
• geometry_msgs,添加对ROS官方geometry_msgs消息的依赖,在包的程序中可以使用geometry_msgs/xxx类型的消息;
• mav_msgs,添加对mav_msgs消息的依赖,在包的程序中可以使用mav_msgs/xxx类型的消息,该消息的定义在UAV\src\mav_comm包中定义;
• gazebo_msgs添加对ROS官方gazebo_msgs消息的依赖,在包的程序中可以使用gazebo_msgs/xxx类型的消息,主要用于从Gazebo中获取消息.

3. 修改 uav_tracking/package.xml

在 <exec_depend>std_msg</exec_depend> 后添加：

<depend>gazebo_msgs</depend>
<depend>gazebo_plugins</depend>
<depend>geometry_msgs</depend>
<depend>joy</depend>
<depend>mav_msgs</depend>
<depend>rotors_gazebo_plugins</depend>
<depend>sensor_msgs</depend>
<depend>xacro</depend>

添加后：

4. 新建测试 cpp 文件

在uav_tracking/src/中新建control_test.cpp文件并写入下面代码，用于控制飞机：

点击查看代码

#include <ros/ros.h>
#include <iostream>
#include <chrono>
#include <thread>
#include <geometry_msgs/PointStamped.h>
#include <std_srvs/Empty.h>
#include <Eigen/Core>

#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>

ros::Publisher trajectory_pub;
geometry_msgs::PointStamped current_position;

float linear_smoothing_navigation_step = 2;
bool flag_gps_initialized_OK = false;
bool flag_take_off_OK = false;
int flag_tasks_OK = 0;
Eigen::Vector3d home;

/*
Description: 
    updateUavPosition(const geometry_msgs::PointStamped& msg)
    gps数据更新的回调函数.

Parameters:
    msg 位置信息

Return:
    无
*/
void updateUavPosition(const geometry_msgs::PointStamped& msg)
{
    if (!flag_gps_initialized_OK)
    {
        flag_gps_initialized_OK= true;
        home[0] = msg.point.x;
        home[1] = msg.point.y;
        home[2] = msg.point.z;
    }
    current_position = msg;
    // std::cout<<"UAV current position is: "<<msg.point.x<< msg.point.y<< msg.point.z<<std::endl;
}

/*
Description: 
    getDistanceToTarget(const Eigen::Vector3d& target)
    获取当前位置到指定位置位置的距离.

Parameters:
    target 需要飞达的位置点

Return:
    double 当前位置到达目标点的位置
*/
double getDistanceToTarget(const Eigen::Vector3d& target)
{
    double temp = 0;
    temp += pow((target[0] - current_position.point.x), 2);
    temp += pow((target[1] - current_position.point.y), 2);
    temp += pow((target[2] - current_position.point.z), 2);
    temp = sqrt(temp);
    return temp;
}

/*
Description: 
    reachTargetPosition(const Eigen::Vector3d& target, float max_error)
    判定是否到达指定的目标点.

Parameters:
    target 需要飞达的位置点
    max_error 允许的位置误差阈值,当前位置和目标位置小于该阈值时,判定无人机到达目标点

Return:
    bool 到达目标点时返回 true
         未到达目标点时返回 false
*/
bool reachTargetPosition(const Eigen::Vector3d& target, float max_error)
{
    double temp = getDistanceToTarget(target);

    if (temp < max_error)
        return true;
    return false;
}

/*
Description: 
    linearSmoothingNavigationTask(const Eigen::Vector3d& target)
    控制无人机从当前位置飞向指定位置.

Parameters:
    target 需要飞达的位置点

Return:
    bool 起飞结束后返回 true
         起飞过程中返回 false
*/
bool linearSmoothingNavigationTask(const Eigen::Vector3d& target)
{
    trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
    trajectory_msg.header.stamp = ros::Time::now();

    if (reachTargetPosition(target,0.2))
        return true;

    double dist = getDistanceToTarget(target);
    Eigen::Vector3d next_step;

    if(dist<linear_smoothing_navigation_step)
    {
        next_step = target;
    }
    else
    {
        next_step[0] = current_position.point.x+(target[0]-current_position.point.x)/dist*linear_smoothing_navigation_step;
        next_step[1] = current_position.point.y+(target[1]-current_position.point.y)/dist*linear_smoothing_navigation_step;
        next_step[2] = current_position.point.z+(target[2]-current_position.point.z)/dist*linear_smoothing_navigation_step;
    }

    double desired_yaw = 0.0; 

    mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(next_step, desired_yaw, &trajectory_msg);
    trajectory_pub.publish(trajectory_msg);
    return false;
}

/*
Description: 
    takeOffTask(float height)
    起飞函数,调用后无人机从起始位置起飞指定高度.

Parameters:
    height 指定的起飞高度

Return:
    bool 起飞结束后返回 true
         起飞过程中返回 false
*/
bool takeOffTask(float height)
{
    trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
    trajectory_msg.header.stamp = ros::Time::now();

    static Eigen::Vector3d desired_position(current_position.point.x, current_position.point.y, height);
    double desired_yaw = 0.0;

    if (reachTargetPosition(desired_position,0.2))
        return true;

    mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position, desired_yaw, &trajectory_msg);
    trajectory_pub.publish(trajectory_msg);
    return false;
}

/*
Description: 
    gohome()
    反航函数,调用后无人机先沿着当前高度飞到反航点正上方,然后降落.

Parameters:
    无

Return:
    无
*/
void gohome()
{
    static Eigen::Vector3d temp(home[0], home[1], current_position.point.z);
    static bool flag_temp = false;

    if (!flag_temp)
    {
        flag_temp = linearSmoothingNavigationTask(temp);
    }
    else
    {
        linearSmoothingNavigationTask(home);
    }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "UAV_Controler");
    ros::NodeHandle nh;
    // Create a private node handle for accessing node parameters.
    ros::NodeHandle nh_private("~");

    std::string uav_name = "";  
    ros::param::get("~mav_name",uav_name);

    // 订阅话题
    // /odometry_sensor1/position   无人机位置信息(包含噪声)
    ros::Subscriber position_sub = nh.subscribe(std::string("/"+uav_name+"/odometry_sensor1/position").c_str(), 10, &updateUavPosition);

    trajectory_pub = nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);

    // 等待5s,让Gazebo可以成功启动.
    ros::Duration(5.0).sleep();

    // 创建控制Gazebo自动运行的服务,这里自动运行是指让Gazebo自动Play
    std_srvs::Empty srv;
    bool unpaused = ros::service::call("/gazebo/unpause_physics", srv); 

    // 尝试让Gazebo自动运行
    int i=0;
    while (i <= 10 && !unpaused) {
        ROS_INFO("Wait for 1 second before trying to unpause Gazebo again.");
        std::this_thread::sleep_for(std::chrono::seconds(1));
        unpaused = ros::service::call("/gazebo/unpause_physics", srv);
        ++i;
    }

    // 判断Gazebo有没有自动运行,没有成功Play则退出
    if (!unpaused) {
        ROS_FATAL("Could not wake up Gazebo.");
        return -1;
    } else {
        ROS_INFO("Unpaused the Gazebo simulation.");
    }

    std::vector<Eigen::Vector3d> path;
    path.push_back(Eigen::Vector3d(5.f,5.f,5.f));
    path.push_back(Eigen::Vector3d(-5.f,5.f,5.f));
    path.push_back(Eigen::Vector3d(-5.f,-5.f,5.f));
    path.push_back(Eigen::Vector3d(5.f,-5.f,5.f));
    path.push_back(Eigen::Vector3d(5.f,5.f,5.f));
    std::cout << path.size() << std::endl;

    ros::Rate loop_rate(10);
    while (ros::ok())
    { 
        if(flag_gps_initialized_OK && !flag_take_off_OK)
        {
            // ROS_INFO("UAV take off task is running...");
            flag_take_off_OK = takeOffTask(3);
        }
        else if(flag_take_off_OK && flag_tasks_OK<path.size())
        {
            if(flag_tasks_OK<path.size())
            {

                bool temp = linearSmoothingNavigationTask(path[flag_tasks_OK]);
                if (temp)
                    flag_tasks_OK ++;
            }
        }
        else if(flag_tasks_OK >= path.size())
        {
            gohome();
        }

        ros::spinOnce();
        loop_rate.sleep();
    }
}

  

5. 创建launch文件夹并新建control_test.launch文件

cd ~/rotors_ws/src/uav_tracking/
mkdir launch && cd launch
touch control_test.launch
gedit control_test.launch

在control_test.launch中写入：

<launch>
    <arg name="mav_name" default="firefly"/>
    <arg name="world_name" default="basic"/>
    <arg name="enable_logging" default="false" />
    <arg name="enable_ground_truth" default="true" />
    <arg name="log_file" default="$(arg mav_name)" />
    <arg name="debug" default="false"/>
    <arg name="gui" default="true"/>
    <arg name="paused" default="false"/>
    <!-- The following line causes gzmsg and gzerr messages to be printed to the console
        (even when Gazebo is started through roslaunch) -->
    <arg name="verbose" default="false"/>

    <!--Run Gazebo-->
    <env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models"/>
    <env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find rotors_gazebo)/worlds/$(arg world_name).world" />
        <arg name="debug" value="$(arg debug)" />
        <arg name="paused" value="$(arg paused)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="verbose" value="$(arg verbose)"/>
    </include>

    <!--Run UAV model and control node-->
    <group ns="$(arg mav_name)">
        <include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
        <arg name="mav_name" value="$(arg mav_name)" />
        <arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo" />
        <arg name="enable_logging" value="$(arg enable_logging)" />
        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
        <arg name="log_file" value="$(arg log_file)"/>
        </include>

        <node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
        <rosparam command="load" file="$(find rotors_gazebo)/resource/lee_controller_$(arg mav_name).yaml" />
        <rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg mav_name).yaml" />
        <remap from="odometry" to="odometry_sensor1/odometry" />
        </node>

        <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
        <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

        <!--name属性用来定义节点运行的名称。pkg定义节点所在的功能包名称，type定义节点的可执行文件名称，这两个属性等同于在终端中使用rosrun命令执行节点时的输入参数-->
        <node 
            name="control_test" pkg="uav_tracking" type="control_test" output="screen">
            <param name="mav_name" type="string" value="$(arg mav_name)"/>
         </node>  
    </group>
</launch>

6. 在 uav_tracking/CMakeLists.txt 添加编译

在uav_tracking/CMakeLists.txt文件最下方添加：

add_executable(control_test src/control_test.cpp)
target_link_libraries(control_test ${catkin_LIBRARIES})

7. 编译

cd ~/rotors_ws
catkin_make

8. 运行

重启终端：

source ~/rotors_ws/devel/setup.bash
roslaunch uav_tracking control_test.launch

如果启动成功，就可以看到程序自动启动仿真，无人机根据control_test.cpp的代码开始自己运动

tracking 包的使用

基于ROS的无人机跟踪：tracking package

ref

• rotors_simulator项目
• uav_tracking包
• rotors_simulator ROS wiki
• rotors无人机仿真
• 无人机ROS仿真包 rotors_simulator 编译教程
• 无人机ROS仿真包 rotors_simulator 使用教程