【rotors】多旋翼无人机仿真（二）——设置飞行轨迹

【rotors】多旋翼无人机仿真（一）——搭建rotors仿真环境
【rotors】多旋翼无人机仿真（二）——设置飞行轨迹
【rotors】多旋翼无人机仿真（三）——SE3控制
【rotors】多旋翼无人机仿真（四）——参数补偿和PID控制
【rotors】多旋翼无人机仿真（五）——多无人机仿真

本贴内容参考两位博主的内容：
月照银海似蛟
Reed Liao

1、前言

在上一节中，我们安装了rotors，并分析了启动程序的launch文件，其中有两个节点程序比较重要：lee_position_controller_node 、 hovering_example，这一节我们来看一下hovering_example节点程序的内容。

2、源码解析

hovering_example节点程序文件位置在~/UAV_rotors/src/rotors_simulator/rotors_gazebo/src/hovering_example.cpp，源码及其注释如下：

/*
 * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <thread>
#include <chrono>

#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>

int main(int argc, char** argv) {
    
    
  ros::init(argc, argv, "hovering_example");
  ros::NodeHandle nh;
  // Create a private node handle for accessing node parameters.
  // nh_private的命名空间是相对路径，可参考https://blog.csdn.net/mynameisJW/article/details/115816641
  ros::NodeHandle nh_private("~");
  // 创建一个发布者，用于发送轨迹消息
  ros::Publisher trajectory_pub =
      nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(
          mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);
  ROS_INFO("Started hovering example.");

  std_srvs::Empty srv;
  bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);
  unsigned int i = 0;

  // Trying to unpause Gazebo for 10 seconds.
  // 等待Gazebo启动10秒
  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;
  }

  if (!unpaused) {
    
    
    ROS_FATAL("Could not wake up Gazebo.");
    return -1;
  } else {
    
    
    ROS_INFO("Unpaused the Gazebo simulation.");
  }

  // Wait for 5 seconds to let the Gazebo GUI show up.
  ros::Duration(5.0).sleep();


// 定义轨迹消息的变量
  trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
  trajectory_msg.header.stamp = ros::Time::now();

  // Default desired position and yaw.
  // 定义期望位置和偏航角
  Eigen::Vector3d desired_position(0.0, 0.0, 1.0);
  double desired_yaw = 0.0;

  // Overwrite defaults if set as node parameters.
  nh_private.param("x", desired_position.x(), desired_position.x());
  nh_private.param("y", desired_position.y(), desired_position.y());
  nh_private.param("z", desired_position.z(), desired_position.z());
  nh_private.param("yaw", desired_yaw, desired_yaw);

// 将desired_position desired_yaw转换数据类型为trajectory_msg
  mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(
      desired_position, desired_yaw, &trajectory_msg);

  ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f].",
           nh.getNamespace().c_str(), desired_position.x(),
           desired_position.y(), desired_position.z());
  // 发布轨迹消息
  trajectory_pub.publish(trajectory_msg);

  ros::spinOnce();
  ros::shutdown();

  return 0;
}

可见hovering_example节点程序是一个发布轨迹消息的简单demo，我们用户方只需要创建一个轨迹话题的发布句柄，然后publish期望的轨迹点即可，在无人机路径规划中，首先我们规划出路径点然后将路径点插值出一条光滑连续的轨迹点，之后不断publish这些轨迹点即可实现无人机的移动。

3、编写一个发布圆形轨迹的节点程序

通过简单的修改hovering_example内容，我们就可以写一个简单的发布圆形轨迹的节点程序，在~/UAV_rotors/src/rotors_simulator/rotors_gazebo/src文件夹中创建run_circle_locus.cpp文件，内容如下：

//  run_circle_locus.cpp
// 飞一个圆形轨迹

#include <thread>
#include <chrono>
#include <cmath>
#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>

int main(int argc, char** argv) {
    
    
    ros::init(argc, argv, "circle_locus_example");
    ros::NodeHandle nh;
    // Create a private node handle for accessing node parameters.
    ros::NodeHandle nh_private("~");
    ros::Publisher trajectory_pub =
        nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(
            mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);
    ROS_INFO("Started circle locus example.");

    std_srvs::Empty srv;
    bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);
    unsigned int i = 0;

    // Trying to unpause Gazebo for 10 seconds.
    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;
    }

    if (!unpaused) {
    
    
    ROS_FATAL("Could not wake up Gazebo.");
    return -1;
    } else {
    
    
    ROS_INFO("Unpaused the Gazebo simulation.");
    }

    // Wait for 5 seconds to let the Gazebo GUI show up.
    ros::Duration(5.0).sleep();

    trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
    trajectory_msg.header.stamp = ros::Time::now();
    double begin_t = ros::Time::now().toSec();
    double t = 0;
    double angle = 0;
    ros::Rate rate(10);
    // Default desired position and yaw.
    Eigen::Vector3d desired_position(0.0, 0.0, 1.0);
    double desired_yaw = 0.0;

    // Overwrite defaults if set as node parameters.
    nh_private.param("x", desired_position.x(), desired_position.x());
    nh_private.param("y", desired_position.y(), desired_position.y());
    nh_private.param("z", desired_position.z(), desired_position.z());
    nh_private.param("yaw", desired_yaw, desired_yaw);


    while (ros::ok())
    {
    
    
        t = ros::Time::now().toSec() - begin_t;
        angle = fmod(2 * M_PI / 10 * t , 2 * M_PI); // 10s飞一圈
        desired_position.x() = cos(angle); // 圆的半径是1
        desired_position.y() = sin(angle);
        desired_position.z() = 1;
        desired_yaw = fmod(angle + M_PI/2 , 2 * M_PI);
        trajectory_msg.header.stamp = ros::Time::now();
        mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(
        desired_position, desired_yaw, &trajectory_msg);
        trajectory_pub.publish(trajectory_msg);
        rate.sleep();
        ros::spinOnce();

    }
    
    ros::shutdown();

    return 0;
}

修改~/UAV_rotors/src/rotors_simulator/rotors_gazebo/CMakeLists.txt，添加以下内容：

# 自己的文件
add_executable(run_circle_locus src/run_circle_locus.cpp)
target_link_libraries(run_circle_locus ${catkin_LIBRARIES})
add_dependencies(run_circle_locus ${catkin_EXPORTED_TARGETS})

重新编译一次程序：

cd ~/UAV_rotors
catkin build

(补充说明一下，修改launch文件不需要重新编译，但是修改源文件需要重新编译，每次编译时间会比较久，为了节约时间，我们可以指定编译规定功能包，如catkin build rotors_gazebo)

编译成功后，我们修改一下mav_hovering_example.launch文件，把里面启动的hovering_example节点改为我们写的节点程序：

<!-- <node name="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen"/> -->
<node name="run_circle_locus" pkg="rotors_gazebo" type="run_circle_locus" output="screen"/>

ok，最后一步就是运行launch文件啦：

roslaunch rotors_gazebo mav_hovering_example.launch

结果图如下：