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

无人机仿真（位置控制）

本次项目所用的仿真环境是开源的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
1
2
3
4

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

$ cd ..
$ catkin_make
$ source devel/setup.sh
1
2
3

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147

程序中首先定义了三个函数：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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

根据上面的数据格式的结果显示进行分析，其消息格式下主要涵盖了两个类型的消息：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>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

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

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

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

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