【ローター】マルチローターUAVシミュレーション(2) - 飛行軌道を設定する

【ローター】マルチローター UAV シミュレーション (1) - ローター シミュレーション環境の構築
【ローター】マルチローター UAV シミュレーション (2) - 飛行軌道の設定
【ローター】マルチローター UAV シミュレーション (3) - SE3 制御
【ローター】マルチ-ローター UAV シミュレーション (4 個) - パラメーター補正と PID 制御
[ローター] マルチローター UAV シミュレーション (5 個) - マルチ UAV シミュレーション

この投稿の内容は、2 人のブロガー: Reed
Liaoの内容を参照しています。

1 はじめに

前のセクションでは、ローターをインストールし、スタートアップ プログラムの起動ファイルを分析しましたが、次の 2 つの重要なノード プログラムがあります。このセクションでは、ノード プログラムの内容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ノード プログラムは、軌道情報を公開するための単純なデモであることがわかります。ユーザーは、軌道トピックのリリース ハンドルを作成し、目的の軌道点を公開するだけで済みます。UAV の経路計画では、最初に経路を計画します。パス ポイントが補間されて滑らかで連続的なトラック ポイントが生成され、これらのトラック ポイントが連続的に公開されてドローンの動きを実現します。

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

(さらに、起動ファイルの変更には再コンパイルは必要ありませんが、ソース ファイルの変更には再コンパイルが必要で、それぞれのコンパイルに時間がかかります。時間を節約するために、コンパイルに指定した関数パッケージを指定できます。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、最後のステップは起動ファイルを実行することです。

roslaunch rotors_gazebo mav_hovering_example.launch

結果のグラフは次のとおりです。