[ROS项目]无人机仿真（位置控制）

无人机仿真（位置控制）

本次项目所用的仿真环境是开源的Rotors功能包，初次了解无人机的控制，也是摸索小白，希望可以起到抛砖引玉的作用。

1. 安装Rotors软件功能包

首先需要建立ROS软件包，并下载对应的Rotors的软件功能包：

$ mkdir -p /ROS/rotors_ws/src
$ cd ROS/rotors_ws/src
$ catkin_init_workspace
$ git clone https://github.com/xmy0916/rotors

之后编译git下来的代码即可：

$ cd ..
$ catkin_make
$ source devel/setup.sh

2. 运行Rotors的Firefly机器人的launch文件

Firefly机器人是一个六轴的机器人，带有三个摄像头，从rviz的topic中可以看到有深度、RGB双目摄像头。出于个人喜好，可以对launch文件中的world进行修改，选择不同的运行物理环境，我喜欢沙都没有。所以，用了basic.world来对无人机进行仿真。订阅后的摄像头图像会发现看到了上面螺旋桨的旋转，所以我在mav_with_vi_sensor.gazebo文件中对摄像头的位置进行了微调，为以后的建图以及图像识别的容错率提升打好基础。

在调整好摄像头的角度后，遇到了其他插件无法转换至bask_link上的问题，需要将坐标系换成world坐标系，来完成转换。

3. 控制Firefly

首先在控制交互上我选用的是通过扫描键盘按下的键值，然后进行对应的控制。所以识别被按下键盘值成为首要解决。

#include <fstream>
#include <iostream>

#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <ros/ros.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <stdio.h>
#include <termios.h>
static struct termios initial_settings, new_settings;
static int peek_character = -1;
void init_keyboard(void);
void close_keyboard(void);
int kbhit(void);
int readch(void); 
void init_keyboard()
{
    tcgetattr(0,&initial_settings);
    new_settings = initial_settings;
    new_settings.c_lflag |= ICANON;
    new_settings.c_lflag |= ECHO;
    new_settings.c_lflag |= ISIG;
    new_settings.c_cc[VMIN] = 1;
    new_settings.c_cc[VTIME] = 0;
    tcsetattr(0, TCSANOW, &new_settings);
}
 
void close_keyboard()
{
    tcsetattr(0, TCSANOW, &initial_settings);
}
 
int kbhit()
{
    unsigned char ch;
    int nread;
 
    if (peek_character != -1) return 1;
    new_settings.c_cc[VMIN]=0;
    tcsetattr(0, TCSANOW, &new_settings);
    nread = read(0,&ch,1);
    new_settings.c_cc[VMIN]=1;
    tcsetattr(0, TCSANOW, &new_settings);
    if(nread == 1) 
    {
        peek_character = ch;
        return 1;
    }
    return 0;
}
 
int readch()
{
    char ch;
 
    if(peek_character != -1) 
    {
        ch = peek_character;
        peek_character = -1;
        return ch;
    }
    read(0,&ch,1);
    return ch;
}

int main(int argc, char** argv) {
  init_keyboard();
  ros::init(argc, argv, "waypoint_publisher");
  ros::NodeHandle nh("//firefly");
  ros::Publisher trajectory_pub = nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);

  ROS_INFO("Started waypoint_publisher.");

  const float DEG_2_RAD = M_PI / 180.0;
  float pos_x = 0,pos_y = 0,pos_z = 1;
  float yaw_deg = 0;
  int isChange = 1;
  while(ros::ok())
  {
    if(kbhit())
    {
      int key = readch();
      switch(key)
      {
	case 56:
		pos_z += 0.5;
		isChange = 1;
		printf("%d\n",key);
		break;
	case 50://2
		pos_z -= 0.5;
		isChange = 1;
		printf("%d\n",key);
		break;
	case 97://a
		pos_y += 0.5;
		isChange = 1;
		printf("%d\n",key);
		break;
	case 100://d
		pos_y -= 0.5;
		isChange = 1;
		printf("%d\n",key);
		break;
	case 119://w
		pos_x += 0.5;
		isChange = 1;
		printf("%d\n",key);
		break;
	case 120://x
		pos_x -= 0.5;
		isChange = 1;
		printf("%d\n",key);
		break;
	default:
		printf("%d\n",key);
		break;
      }
    }
    if(isChange == 1)
    {
      trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
      trajectory_msg.header.stamp = ros::Time::now();

      Eigen::Vector3d desired_position(pos_x, pos_y,pos_z);

      double desired_yaw = yaw_deg * DEG_2_RAD;

      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);   
      isChange = 0;
    }
    ros::spinOnce();
  }
  ros::shutdown();
  return 0;
}

程序中首先定义了三个函数：void init_keyboard(void)、void close_keyboard(void)和int kbhit(void),第一个是用来对键盘的初始化、第二个函数是tcsetattr是用于设置终端参数的函数，而第三个函数的存在原因是，Windows系统下可以使用_kbhit()函数来获取键盘事件，来判断是否有按键被按下，而Unix/Linux中，没有提供kbhit()函数，所以我们需要自己来实现kbhit()程序。

在键盘控制中我们设定了6个键，分别对应前进、后退、向左、向右、上升和下降，没有加入旋转选项。在分析发布数据格式时候进行讨论。对控制话题发布的格式是ros::Publisher trajectory_pub = nh.advertise <trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);，所以根据该代码可以发现话题是：COMMAND_TRAJECTORY且数据格式是trajectory_msgs::MultiDOFJointRrajectory通过rosmsg show来查询该数据格式的具体内容

$ rosmsg show trajectory_msgs/MultiDOFJointTrajectory
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string[] joint_names
trajectory_msgs/MultiDOFJointTrajectoryPoint[] points
  geometry_msgs/Transform[] transforms
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w
  geometry_msgs/Twist[] velocities
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z
  geometry_msgs/Twist[] accelerations
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z
  duration time_from_start

根据上面的数据格式的结果显示进行分析，其消息格式下主要涵盖了两个类型的消息：string[] joint_names和trajectory_msgs/MultiDOFJointTrajectoryPoint[] points而且后者又涵盖了三个子消息类型：geometry_msgs/Transform[] transforms、geometry_msgs/Twist[] velocities和geometry_msgs/Twist[] accelerations主要对无人机进行控制。

在对位置进行控制的时候，首先对x,y,z三个坐标进行数据获取，之后定义好yaw角度，也就是俯仰角（对无人机不进行角度的控制，只是单纯的位置控制）。

到这里可能会有一个疑问，说我发布三个坐标值的数据，那个无人机怎么就可以对坐标进行跟踪，或者移动带该坐标点了？

我这里想说明的是，在运行控制无人机的结点之前，需要在Gazebo中建立无人机模型以及控制方案，在之前机械臂控制中就需要ros_control的控制方案的选择，那么接下来对运行无人机模型的launch文件进行描述。

<launch>
  <arg name="mav_name" default="firefly"/>
  <arg name="world_name" default="outdoor"/>
  <arg name="enable_logging" default="false" />
  <arg name="enable_ground_truth" default="true" />
  <arg name="log_file" default="$(arg mav_name)" />

  <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="true"/> -->
    <arg name="paused" value="true"/>
    <!-- <arg name="gui" value="false"/> -->
    <!-- <arg name="verbose" value="true"/> -->
  </include>

  <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_with_vi_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="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen"/>
    <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="rviz" pkg="rviz" type="rviz" required="true"
          args="-d $(find rotors_gazebo)/$(arg mav_name).rviz"/>
  </group>
</launch>

前面一些对变量的定义和目标文件位置的定义进行跳过，第一个运行的文件是gazebo_ros软件包下运行设定好的world文件的launch可执行launch文件，目的是将环境带入一些现实生活中的物理模型。接着第二个执行文件就是通过spawn_mav.launch对无人机模型进行导入。接着，也就是最重要的一部，lee_position_controller_node可执行文件就是对无人机的6个电机进行速度的控制，完成位置控制。接下来的三个结点就是发布一些自身无人机的数据，而rviz就是将无人机模型及其自身的摄像头进行订阅和显示。

最后放上一个演示效果图和演示视频，某人说放上EXO照片会长粉也不知道考不靠谱。

可是看到这高糊的图片，注定与爱豆EXO无缘了。

最后就放上正常的控制仿真视频的图片了：