本教程建立在上一篇教程[ROS——无人机ROS仿真包 rotors_simulator 编译教程]的基础上,因此请先按照教程编译rotors_simulator 仿真包.本章教大家如何自己创建ROS包控制无人机运动. ~先上效果再讲解,兴趣来的更快些~     下面进入实战

1.创建自己的ROS包

1.1 创建ROS包

先进入工程目录的src文件夹下,然后创建control包  

cd UAV/src/
catkin_create_pkg control std_msgs rospy roscpp

   

1.2 添加rotors_simulator依赖

修改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
)

  其中 \bulletsensor_msgs,添加对ROS官方sensor_msgs消息的依赖,在包的程序中可以使用sensor_msgs/xxx类型的消息; \bulletgeometry_msgs,添加对ROS官方geometry_msgs消息的依赖,在包的程序中可以使用geometry_msgs/xxx类型的消息; \bulletmav_msgs,添加对mav_msgs消息的依赖,在包的程序中可以使用mav_msgs/xxx类型的消息,该消息的定义在UAV\src\mav_comm包中定义; \bulletgazebo_msgs添加对ROS官方gazebo_msgs消息的依赖,在包的程序中可以使用gazebo_msgs/xxx类型的消息,主要用于从Gazebo中获取消息.   修改package.xml文件,在其中添加:  

<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>

  添加后如下图所示:

1.3 创建.cpp文件

在UAV/src文件夹中创建control.cpp文件,用于编写控制代码,添加后的工程架构如下图所示     在CMakeLists.txt文件最下方添加.cpp文件的编译依赖信息:  

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

2. 控制代码

1）初始化ROS节点； 2）创建position_sub订阅获取无人机位置信息的话题 \bullet话题名称为/odometry_sensor1/position，可以通过rqt查询到其他话题名称及对应的消息数据类型； \bullet回调函数为updateUavPosition，位置信息类型为geometry_msgs::PointStamped； \bullet注意一个细节，当GPS初始化成功，接收到位置信息时才开始执行任务。 3）创建发布控制指令的publisher，命名为trajectory_pub \bullettrajectory_pub = nh.advertise(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10); \bullet在此控制无人机的移动借助于rotors_simulator包中的控制节点，因此publisher中的话题为rotors_simulator包中定义的话题； \bullet在此使用rotors_simulator包中的控制节点只能对无人机位置进行控制，也就是发送指定位置点到该话题即可。但是需要注意，如果目标位置和当前位置距离较远时，无人机会侧翻； \bullet为避免目标点较远时无人机侧翻，这里定义了linearSmoothingNavigationTask()函数来平滑轨迹，该函数中将目标点到当前位置的距离分段，并计算出下一步要到达的临时目标点，此时函数返回0。多次调用该函数后才可到达目标点，此时函数返回1。   我已经尽可能详细的将代码进行了注释,还有疑问的请留言.  

#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();
    }
}

3. launch配置

在工程目录下创建launch文件夹，并在其中添加uav.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" />

        <node 
            name="control" pkg="control" type="control" output="screen">
            <param name="mav_name" type="string" value="$(arg mav_name)"/>
         </node>  
    </group>
</launch>

4. 编译&运行

 

catkin_make
source devel/setup.bash
roslaunch uav.launch

  然后你就可以看到程序自动启动Gazebo，并且无人机可以开始运动咯~  

4. git源码

懒小象的github: https://github.com/MeMiracle/UAV