前言

无人机仿真主要分为两类：硬件在环仿真（HITL）和软件在环仿真（SITL全称Software in the loop）。

无人机软件在环仿真是指完全用计算机来模拟出无人机飞行时的状态，而硬件在环仿真是指计算机连接飞控板来测试飞控软件是否可以流畅运行。一般来说硬件在环仿真若没有加上真实的转台进行测试的话，其与软件在环仿真没有很大的区别。

在无须解决在研发过程中的硬件问题带来的麻烦，并且可以直观的调试代码，搭建一套无人机仿真系统，对于研发来说好处是数不剩数的。

上一篇（链接：《无人机 gazebo 仿真---rotors_simulator 功能包介绍》）讲解了 无人机仿真功能包 rotors_simulator 的 编译，并通过 如下指令，将飞机飞了起来 ，

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

指令 可以让无人机悬停在一个位置
在这里插入图片描述

本篇博客则来看具体是怎么做到的

mav_hovering_example.launch

那么则需要看这个文件 mav_hovering_example.launch
抽取主要内容

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

主要是在gazebo中加载
mav_generic_odometry_sensor.gazebo这个模型
打开该文件
下面来看下该模型

mav_generic_odometry_sensor.gazebo

<xacro:include filename="$(find rotors_description)/urdf/$(arg mav_name)_base.xacro" />

无人机的模型

  <xacro:odometry_plugin_macro
    namespace="${namespace}"
    odometry_sensor_suffix="1"
    parent_link="${namespace}/base_link"
    pose_topic="odometry_sensor1/pose"
    pose_with_covariance_topic="odometry_sensor1/pose_with_covariance"
    position_topic="odometry_sensor1/position"
    transform_topic="odometry_sensor1/transform"
    odometry_topic="odometry_sensor1/odometry"
    parent_frame_id="world"
    child_frame_id="${namespace}/odometry_sensor1"
    mass_odometry_sensor="0.00001"
    measurement_divisor="1"
    measurement_delay="0"
    unknown_delay="0.0"
    noise_normal_position="0 0 0"
    noise_normal_quaternion="0 0 0"
    noise_normal_linear_velocity="0 0 0"
    noise_normal_angular_velocity="0 0 0"
    noise_uniform_position="0 0 0"
    noise_uniform_quaternion="0 0 0"
    noise_uniform_linear_velocity="0 0 0"
    noise_uniform_angular_velocity="0 0 0"
    enable_odometry_map="false"
    odometry_map=""
    image_scale="">
    <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" /> <!-- [kg m^2] [kg m^2] [kg m^2] [kg m^2] [kg m^2] [kg m^2] -->
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
  </xacro:odometry_plugin_macro>

装了一个里程计在无人机上。 链接的joint是在插件里写好的。 父link是上面无人机的base_link，子link是里程计模块。

里程计可以发布的topic有

    pose_topic="odometry_sensor1/pose"
    pose_with_covariance_topic="odometry_sensor1/pose_with_covariance"
    position_topic="odometry_sensor1/position"
    transform_topic="odometry_sensor1/transform"
    odometry_topic="odometry_sensor1/odometry"

通过 rostopic list 也可以看到这几个topic

对应的功能分别是：

  • 位姿（位置+姿态）消息格式是：geometry_msgs/Pose
  • 位姿的协方差 消息格式是：geometry_msgs/PoseWithCovarianceStamped
  • TF 消息格式是：geometry_msgs/TransformStamped
  • 里程计 消息格式是：nav_msgs/Odometry

看完加载的模型，继续看mav_hovering_example.launch文件，下面就该启动节点了

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

启动了 lee_position_controller_node一个位置控制器的节点

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

启动了一个 hovering_example

hovering_example.cpp

来简单看下hovering_example.cpp文件里的代码

int main(int argc, char** argv) {
  ros::init(argc, argv, "hovering_example");
  ros::NodeHandle nh;
  ros::NodeHandle nh_private("~");

ros的基本操作

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

定义要发布的topic

本节点要发布一个 轨迹的 topic
其中 mav_msgs::default_topics::COMMAND_TRAJECTORY 对应的消息名称就是command/trajectory

trajectory_msgs::MultiDOFJointTrajectory
这个的消息格式是

std_msgs/Header header
string[] joint_names
trajectory_msgs/MultiDOFJointTrajectoryPoint[] points

这个消息对于无人机来说可以存几个航点
其中的 points 是 的格式是：

geometry_msgs/Transform[] transforms
geometry_msgs/Twist[] velocities
geometry_msgs/Twist[] accelerations
duration time_from_start

弄清了这个消息，继续来看代码

  std_srvs::Empty srv;
  bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);
  unsigned 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;
  }
  if (!unpaused) {
    ROS_FATAL("Could not wake up Gazebo.");
    return -1;
  } else {
    ROS_INFO("Unpaused the Gazebo simulation.");
  }

取消gazebo 的暂停

  trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
  trajectory_msg.header.stamp = ros::Time::now();

声明要发布的轨迹的变量

  Eigen::Vector3d desired_position(0.0, 0.0, 1.0);
  double desired_yaw = 0.0;

期望的位置 是 0，0，1。 期望的航向角度是0度

  mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(
      desired_position, desired_yaw, &trajectory_msg);

将 期望位置和航向，转成MultiDofJointTrajectory 的 消息格式

trajectory_pub.publish(trajectory_msg);

发布该消息

hovering_example.cpp 的代码就解读完了

下面来看 launch 文件中的另一个 节点 的代码 lee_position_controller_node.cpp

lee_position_controller_node.cpp

int main(int argc, char** argv) {
  ros::init(argc, argv, "lee_position_controller_node");

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

ros的基本操作

rotors_control::LeePositionControllerNode lee_position_controller_node(nh, private_nh);

实例化了LeePositionControllerNode类

来看其构造函数

LeePositionControllerNode::LeePositionControllerNode(
  const ros::NodeHandle& nh, const ros::NodeHandle& private_nh)
  :nh_(nh),
   private_nh_(private_nh){

InitializeParams();

首先进行参数初始化
其中初始化的就是 位置、速度、角度、角速度 的增益

然后开始订阅消息

  cmd_pose_sub_ = nh_.subscribe(
      mav_msgs::default_topics::COMMAND_POSE, 1,
      &LeePositionControllerNode::CommandPoseCallback, this);

订阅位姿的命令消息

  cmd_multi_dof_joint_trajectory_sub_ = nh_.subscribe(
      mav_msgs::default_topics::COMMAND_TRAJECTORY, 1,
      &LeePositionControllerNode::MultiDofJointTrajectoryCallback, this);

订阅轨迹的命令消息

  odometry_sub_ = nh_.subscribe(mav_msgs::default_topics::ODOMETRY, 1,
                               &LeePositionControllerNode::OdometryCallback, this);

订阅里程计消息

那么首先看上个hovering_example.cpp 发的轨迹的回调函数

void LeePositionControllerNode::MultiDofJointTrajectoryCallback(
    const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& msg) {

  command_timer_.stop();
  commands_.clear();
  command_waiting_times_.clear();

清除掉其他的控制指令

const size_t n_commands = msg->points.size();

获取点的个数

  if(n_commands < 1){
    ROS_WARN_STREAM("Got MultiDOFJointTrajectory message, but message has no points.");
    return;
  }

点的个数小于1 ，说明没有航点，则返回

  mav_msgs::EigenTrajectoryPoint eigen_reference;
  mav_msgs::eigenTrajectoryPointFromMsg(msg->points.front(), &eigen_reference);
  commands_.push_front(eigen_reference);

先把第一个航点取出来

  for (size_t i = 1; i < n_commands; ++i) {
    const trajectory_msgs::MultiDOFJointTrajectoryPoint& reference_before = msg->points[i-1];
    const trajectory_msgs::MultiDOFJointTrajectoryPoint& current_reference = msg->points[i];

    mav_msgs::eigenTrajectoryPointFromMsg(current_reference, &eigen_reference);

    commands_.push_back(eigen_reference);
    command_waiting_times_.push_back(current_reference.time_from_start - reference_before.time_from_start);
  }

如果航点的个数大于1，将其它航点取出来

lee_position_controller_.SetTrajectoryPoint(commands_.front());

将第一个航点的命令传入了 lee_position_controller_这个类里面
多航点的就是不断的pop_front，就可以把后续的都传入了

其它的订阅命令就不看了，来看下里程计的订阅

void LeePositionControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) {

  EigenOdometry odometry;
  eigenOdometryFromMsg(odometry_msg, &odometry);

将里程计的信息 转成了 eigenOdometry的格式

lee_position_controller_.SetOdometry(odometry);

将里程计的数值，传入了lee_position_controller_这个类中

  Eigen::VectorXd ref_rotor_velocities;
  lee_position_controller_.CalculateRotorVelocities(&ref_rotor_velocities);

通过 lee_position_controller_ 的 计算电机转速的函数，获得 电机的转速

  mav_msgs::ActuatorsPtr actuator_msg(new mav_msgs::Actuators);
  actuator_msg->angular_velocities.clear();
  for (int i = 0; i < ref_rotor_velocities.size(); i++)
    actuator_msg->angular_velocities.push_back(ref_rotor_velocities[i]);
  actuator_msg->header.stamp = odometry_msg->header.stamp;
  motor_velocity_reference_pub_.publish(actuator_msg);

发布电机的转速

那么lee_position_controller_这个类是如何 通过 获得 里程计数据和航点指令，计算出的电机转速呢？

这则需要继续去分析 lee_position_controller_ 这个类的功能。 估计里面是一个位置控制器、速度控制器、姿态控制器。最后经过电机混控计算出电机转速。