【rotors】Multi-rotor UAV simulation (2) - set the flight trajectory

【rotors】Multi-rotor UAV simulation (1) - Build a rotors simulation environment
【rotors】Multi-rotor UAV simulation (2) - set flight trajectory
【rotors】Multi-rotor UAV simulation (3) - SE3 Control
[rotors] multi-rotor UAV simulation (four) - parameter compensation and PID control
[rotors] multi-rotor UAV simulation (five) - multi-UAV simulation

The content of this post refers to the content of two bloggers: Reed
Liao

1 Introduction

In the previous section , we installed rotors and analyzed the launch file of the startup program. There are two important node programs: lee_position_controller_node 、 hovering_example, in this section we will look at hovering_examplethe content of the node program.

2. Source code analysis

hovering_exampleThe location of the node program file is ~/UAV_rotors/src/rotors_simulator/rotors_gazebo/src/hovering_example.cpp, the source code and its comments are as follows:

/*
 * 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;
}

It can be seen that hovering_examplethe node program is a simple demo for publishing trajectory information. Our users only need to create a release handle for the trajectory topic, and then publish the desired trajectory points. In UAV path planning, we first plan out the path points and then The path points are interpolated to produce a smooth and continuous track point, and then these track points are continuously published to realize the movement of the drone.

3. Write a node program that publishes a circular trajectory

By simply modifying hovering_examplethe content, we can write a simple node program that publishes a circular trajectory, and ~/UAV_rotors/src/rotors_simulator/rotors_gazebo/srccreate a file in the folder run_circle_locus.cppwith the following content:

//  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;
}

Edit ~/UAV_rotors/src/rotors_simulator/rotors_gazebo/CMakeLists.txt, add the following:

# 自己的文件
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})

Recompile the program once:

cd ~/UAV_rotors
catkin build

(Additionally, modifying the launch file does not require recompilation, but modifying the source file requires recompilation, and each compilation will take a long time. In order to save time, we can specify the specified function package for compilation, such as catkin build rotors_gazebo)

After the compilation is successful, we modify mav_hovering_example.launchthe file and change the node started in it hovering_exampleto the node program we wrote:

<!-- <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, the last step is to run the launch file:

roslaunch rotors_gazebo mav_hovering_example.launch

The resulting graph is as follows: