Gazebo地图设计及无人机通信

地图构建

设计过程

软件介绍

为简化gazebo地图设计流程，将其设计过程封装为windows插件形式，可直接将获得的地图直接导入linux下gazebo中进行使用。
其原理为生成高度地图，及图片存在色差，越暗的地方代表的高度越高。

图 1 功能展示

绘制地图

使用高度地图，其高度由白色向黑色高度增加，因此可以直接下载卫星雷达黑白地图，或使用绘图软件绘制图片。

图 2 卫星地图获得的高度地图

图 3 画板绘制的高度地图

添加物体

使用地图制作软件添加物体及无人机，下图为卫星地图转换过程：

图 4 添加物体
下图为自绘制地图转换过程

图 5 自绘制地图添加物体
现支持添加pine和oak两类树种，可添加车及无人机，其中无人机平台可分别搭载多种传感器，其类型如下图所示

图 6 无人机支持添加的类型
除此外软件自定义添加物体由如下图所示

图 7 自定义物品添加

导入gazebo库

将软件生成的文件导入库中，软件生成地图的模型文件，地图的gazebo world文件及无人机launch文件，分别添加即可完成。
完成地图构建，启动gazebo
使用命令启动gazebo，可获得相应的模型

图 8 使用卫星地图获得的地图模型

图 9 无人机所处位置细节
使用自绘制图片完成地图构建

图 10 自绘制图片地图

图 11 无人机细节
当然，也可以使用随机彩色图片进行地图的绘制，只要其存在色差，及可反应地图的变化情况。

图 12 随机图片地图构建

无人机控制

控制模式

使用速度控制模式，无人机程序使用速度pid逼近其预期速度，能够达到比较好的仿真效果。
设置速度控制PID为0.1，0.1，0.1，可根据实际情况进行调整
使用ROS通信将无人机状态实时输入控制无人机的位置状态。
通过python rosbag库进行通信，为对无人机前后左右，上下的速度控制，以及对偏航角度的控制。

图 13 单机控制效果

图 14 多机控制效果

无人机传感器类型

二维激光雷达

图 15 装备二维激光雷达无人机探测空间障碍
可通过rviz观测空间探测情况，并使用相应的程序建图。

图 16 双目传感器获取图片
可通过双目获取深度图，及相关处理进行vio视觉导航

图 17 下视相机获取下视图
这里简单展示使用无人机egoplan进行路径规划过程

图 18 使用深度相机完成三维路径规划

无人机控制代码python

    def pid_vel(self):
        wu_x=self.target_motion.twist.linear.x-self.current_motion.twist.linear.x
        wu_y=self.target_motion.twist.linear.y-self.current_motion.twist.linear.y
        wu_z=self.target_motion.twist.linear.z-self.current_motion.twist.linear.z
        wu_ya=self.target_motion.twist.angular.z-self.current_motion.twist.angular.z
        self.i_x=self.i_x+wu_x
        self.i_y=self.i_y+wu_y
        self.i_z=self.i_z+wu_z
        
        self.current_motion.twist.linear.x=self.current_motion.twist.linear.x+wu_x*0.1+self.i_x*0.1+(wu_x-self.pre_x)*0.1
        self.current_motion.twist.linear.y=self.current_motion.twist.linear.y+wu_y*0.1+self.i_y*0.1+(wu_y-self.pre_y)*0.1
    
        self.current_motion.twist.linear.z=self.current_motion.twist.linear.z+wu_z*0.1+self.i_z*0.1+(wu_z-self.pre_z)*0.1
        self.current_motion.twist.angular.z=self.current_motion.twist.angular.z+wu_ya*0.1
        self.pre_x=wu_x
        self.pre_y=wu_y
        self.pre_z=wu_z
        
        if abs(self.target_motion.twist.linear.x-self.current_motion.twist.linear.x)<0.005 and abs(wu_x-self.pre_x)<0.001:
            self.current_motion.twist.linear.x=self.target_motion.twist.linear.x
        if abs(self.target_motion.twist.linear.y-self.current_motion.twist.linear.y)<0.005 and abs(wu_y-self.pre_y)<0.001:
            self.current_motion.twist.linear.y=self.target_motion.twist.linear.y
        if abs(self.target_motion.twist.linear.z-self.current_motion.twist.linear.z)<0.005 and abs(wu_z-self.pre_z)<0.001:
            self.current_motion.twist.linear.z=self.target_motion.twist.linear.z
        if abs(self.target_motion.twist.angular.z-self.current_motion.twist.angular.z)<0.0005:
            self.current_motion.twist.angular.z=self.target_motion.twist.angular.z
        
    def start(self):
        '''
        main ROS thread
        '''
        while not rospy.is_shutdown():
            try:
                self.pid_vel()
                '''
                if self.current_motion.twist.linear.x==0 and self.current_motion.twist.linear.y==0 and self.current_motion.twist.linear.z==0 and self.current_motion.twist.angular.z==0 and self.target_motion.twist.linear.x==0 and self.target_motion.twist.linear.y==0 and self.target_motion.twist.linear.z==0 and self.target_motion.twist.angular.z==0:
                    continue
                '''
                self.current_position.pose.position.x=self.current_position.pose.position.x+self.current_motion.twist.linear.x/30*math.cos(self.current_position.pose.orientation.z)-self.current_motion.twist.linear.y/30*math.sin(self.current_position.pose.orientation.z)
                self.current_position.pose.position.y=self.current_position.pose.position.y+self.current_motion.twist.linear.y/30*math.cos(self.current_position.pose.orientation.z)+self.current_motion.twist.linear.x/30*math.sin(self.current_position.pose.orientation.z)
                self.current_position.pose.position.z=self.current_position.pose.position.z+self.current_motion.twist.linear.z/30
                #print(,math.sin(self.current_position.pose.orientation.z))
                self.current_position.pose.orientation.y=(self.current_motion.twist.linear.x*math.cos(self.current_position.pose.orientation.z)-self.current_motion.twist.linear.y*math.sin(self.current_position.pose.orientation.z))/12
                self.current_position.pose.orientation.x=-(self.current_motion.twist.linear.y*math.cos(self.current_position.pose.orientation.z)+self.current_motion.twist.linear.x*math.sin(self.current_position.pose.orientation.z))/12
                self.current_position.pose.orientation.z=self.current_motion.twist.angular.z
                
                self.target_motion_pub.publish(self.current_position)
                print(self.current_position.pose.orientation.z)
            except:
                print(112)
            rate.sleep()