GAAS学习（一）Offboard基本控制

开源项目GAAS：
https://github.com/generalized-intelligence/GAAS

一、确保环境安装配置完成

ROS
Firmware
Mavros
Gazebo
及相关依赖

修改环境变量bashrc

sudo gedit ~/.bashrc

添加如下内容

source ~/Firmware/Tools/setup_gazebo.bash ~/Firmware/ ~/Firmware/build/posix_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/Firmware
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/Firmware/Tools/sitl_gazebo

检测：
出现如下界面，即环境没有问题

roslaunch px4 mavros_posix_sitl.launch

在这里插入图片描述查看当前mavros状态

rostopic echo /mavros/state

其中 connected为 True，为连接成功
在这里插入图片描述

二、使用Gaas相关代码

git clone git@github.com:generalized-intelligence/GAAS.git

运行px4_mavros_run.py，起飞3m

python px4_mavros_run.py

主要部分代码如下：

    def start(self):
	#初始化节点
        rospy.init_node("offboard_node")
	#等待
        for i in range(10):
            if self.current_heading is not None:
                break
            else:
                print("Waiting for initialization.")
                time.sleep(0.5)
	#设置起飞高度、偏航
        self.cur_target_pose = self.construct_target(0, 0, self.takeoff_height, self.current_heading)
	
        #print ("self.cur_target_pose:", self.cur_target_pose, type(self.cur_target_pose))
	#不停发布，解锁，模式offboard
        for i in range(10):
            self.local_target_pub.publish(self.cur_target_pose)
            self.arm_state = self.arm()
            self.offboard_state = self.offboard()
            time.sleep(0.2)

	#起飞判断检测
        if self.takeoff_detection():
            print("Vehicle Took Off!")

        else:
            print("Vehicle Took Off Failed!")
            return

        '''
        main ROS thread
        '''
	#解锁状态、offboard状态、ros未杀死
        while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False):
	    #起飞到设定位置
            self.local_target_pub.publish(self.cur_target_pose)
	    #模式land并高度小于0.15,上锁
            if (self.state is "LAND") and (self.local_pose.pose.position.z < 0.15):
                if(self.disarm()):
                    self.state = "DISARMED"
            time.sleep(0.1)

在这里插入图片描述
运行commander.py
向右飞一米
逆时针旋转90度
降落

# -*- coding: utf-8 -*-


import rospy
from mavros_msgs.msg import GlobalPositionTarget, State
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from geometry_msgs.msg import PoseStamped, Twist
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import Float32, String
from pyquaternion import Quaternion
import time
import math


class Commander:
    def __init__(self):
        rospy.init_node("commander_node")
        rate = rospy.Rate(20)
        self.position_target_pub = rospy.Publisher('gi/set_pose/position', PoseStamped, queue_size=10)
        self.yaw_target_pub = rospy.Publisher('gi/set_pose/orientation', Float32, queue_size=10)
        self.custom_activity_pub = rospy.Publisher('gi/set_activity/type', String, queue_size=10)
	

	#俯仰滚转升降，按照bodyemu
    def move(self, x, y, z, BODY_OFFSET_ENU=True):
        self.position_target_pub.publish(self.set_pose(x, y, z, BODY_OFFSET_ENU))

	#偏航设置
    def turn(self, yaw_degree):
        self.yaw_target_pub.publish(yaw_degree)
	
    	
    	#降落模式
    def land(self):
        self.custom_activity_pub.publish(String("LAND"))


    	#悬停模式
    def hover(self):
        self.custom_activity_pub.publish(String("HOVER"))


    	#返回模式
    def return_home(self, height):
        self.position_target_pub.publish(self.set_pose(0, 0, height, False))

	#位置设置
    def set_pose(self, x=0, y=0, z=2, BODY_FLU = True):
        pose = PoseStamped()
        pose.header.stamp = rospy.Time.now()

        # ROS uses ENU internally, so we will stick to this convention
        if BODY_FLU:
            pose.header.frame_id = 'base_link'

        else:
            pose.header.frame_id = 'map'

        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.position.z = z

        return pose


if __name__ == "__main__":
    
    con = Commander()
    time.sleep(2)
    con.move(1, 0, 0)
    time.sleep(2)
    con.turn(90)
    time.sleep(2)
    con.land()