Mavros: Manter a posição de vôo enquanto dá ao drone o comando de velocidade 2D

Você está inserindo dados Optitrack no Pixhawk por meio do MAVROS?

Estou alimentando Optitrack Data como / mavros / mocap e também alterei os parâmetros do Pixhawk para que ele use essas informações para estimativa de posição local, o resultado é realmente muito bom.

Dito isso, atualmente estou apenas tentando trabalhar com o simulador Gazebo (não incluindo o mocap). No meu código, crio editores setpoint_position e setpoint_velcity. Eu defini "set_position.pose.position.z = 2" e "set_velocity.linear.x = 1" Mas o drone apenas paira a 2 metros, sem se mover para qualquer lugar.

E me dá este erro:
Could not process inbound connection: topic types do not match: [geometry_msgs/TwistStamped] vs. [geometry_msgs/Twist]{'topic': '/mavros/setpoint_velocity/cmd_vel', 'tcp_nodelay': '0', 'md5sum': '98d34b0043a2093cf9d9345ab6eef12e', 'type': 'geometry_msgs/TwistStamped', 'callerid': '/mavros'}

Encontrei o problema da mensagem de erro. Eu cometi um erro ao tentar enviar '/ mavros / setpoint_velocity / cmd_vel'. O tipo de mensagem deve ser TwistStamped, em vez de Twist. Depois de modificar meu set_velocity para 'set_velocity.twist.linear.x = 1', o erro desaparece.

No entanto, um novo problema aparece. O drone não está nada estável, ele continua tremendo enquanto se move para frente (direção + x). Não tenho certeza se minha abordagem está correta. Por favor, deixe-me saber se existe uma maneira formal de fazer isso.

Could not process inbound connection: topic types do not match: [geometry_msgs/TwistStamped] vs. [geometry_msgs/Twist]{'topic': '/mavros/setpoint_velocity/cmd_vel', 'tcp_nodelay': '0', 'md5sum': '98d34b0043a2093cf9d9345ab6eef12e', 'type': 'geometry_msgs/TwistStamped', 'callerid': '/mavros'}

Na verdade, o erro indica qual é o problema. O comando de velocidade que você está enviando é uma Twist msg, enquanto o nó está esperando uma TwistStamped msg.

No entanto, um novo problema aparece. O drone não está nada estável, ele continua tremendo enquanto se move para frente (direção + x). Não tenho certeza se minha abordagem está correta. Por favor, deixe-me saber se existe uma maneira formal de fazer isso.

Sem verificar o código que você está usando, é meio difícil entender o que está errado.

@ TSC21 Obrigado pela ajuda! Infelizmente, não tenho certeza se posso postar meu código-fonte original. Voltarei a este assunto assim que tiver permissão. Peço desculpas pela inconveniência.

Também postarei a solução para esse problema específico assim que encontrá-la, porque acho que é um aplicativo / demonstração muito bom.

Peço desculpas pela inconveniência.

O inconveniente não é para nós que estamos tentando ajudá-lo. Mas afirmar a funcionalidade de um trecho de código sem saber o código é um pouco difícil.

@ TSC21 Sim. Eu entendo completamente.

OK. Portanto, aqui está um script simples que preciso controlar a velocidade e a posição.

import rospy
import numpy as np
import math
import mavros_msgs

from geometry_msgs.msg import PoseStamped, TwistStamped
from mavros_msgs import srv
from mavros_msgs.msg import State

#=================Parameter Initializaiton========================
goal_pose = PoseStamped()
current_pose = PoseStamped()
set_velocity = TwistStamped()
current_state = State()


def altitude_hold():
    global goal_pose
    goal_pose.pose.position.z = 2

#==============Call Back Functions=====================
def pos_sub_callback(pose_sub_data):
    global current_pose
    current_pose = pose_sub_data

def state_callback(state_data):
    global current_state
    current_state = state_data

#============Intialize Node, Publishers and Subscribers=================
rospy.init_node('Vel_Control_Node', anonymous = True)
rate = rospy.Rate(10) #publish at 10 Hz
local_position_subscribe = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pos_sub_callback)
local_position_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size = 10)
setpoint_velocity_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',TwistStamped, queue_size = 10)
state_status_subscribe = rospy.Subscriber('/mavros/state',State,state_callback)
altitude_hold()


#============Define Velocity==============================================
set_velocity.twist.linear.x = 1 #moving 1m/s at x direction


while not rospy.is_shutdown():
    local_position_pub.publish(goal_pose)

    if current_state.mode != "OFFBOARD" or not current_state.armed:
        arm = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
        arm(True)
        set_mode = rospy.ServiceProxy('/mavros/set_mode',mavros_msgs.srv.SetMode)
        mode = set_mode(custom_mode = 'OFFBOARD')

        if mode.success:
            rospy.loginfo('Switched to OFFBOARD mode!')

    setpoint_velocity_pub.publish(set_velocity)


    rate.sleep()

Você deve basear seu código neste: https://dev.px4.io/en/ros/mavros_offboard.html.

Vejo que você envia o ponto de ajuste de posição apenas uma vez, ao passo que deveria enviá-lo por um tempo. Então, quando ele verificar que está na goal_pose , ele deve se mover na direção x com aquela velocidade. Além disso, você deve separar a verificação do modo e do estado de arme em duas condições diferentes.

Estou pensando em uma abordagem diferente. Em vez de depender de '/ mavros / setpoint_position / local' para obter a retenção de altitude, também posso escrever meu próprio controlador de altitude PID que leva a diferença da altitude desejada e minha altitude atual (set_z - medido_z) como minha entrada, e controle de velocidade de saída na direção z. Portanto, tudo que eu preciso é publicar Vz para manter a altitude, mas pode exigir alguns ajustes.

Isso é algo que já fiz em test_mavros com control_toolbox . Verifique o que está lá e talvez você não precise implementá-lo do zero.

Obrigado! Vou dar uma olhada nisso e acredito que é uma ótima abordagem. Acho que esse problema pode ser encerrado agora.

https://github.com/darknight-007/docking/blob/master/testVelocityControl.py

Eu modifiquei isso para obter a sustentação da altitude enquanto controlava a velocidade horizontal. Você também pode tentar setpoint_raw, mas descobri que não funcionou.

@schmittlema , você pode me dizer como você modificou para fornecer uma velocidade constante com a

Abaixo. Fiz 3 controladores PID para cada eixo. Então, se a velocidade desejada for zero, ele mantém a posição do eixo. Z está sempre tentando manter a altitude. Espero que ajude.
`" ""
testando o controle de posição fora de bordo com um script de decolagem simples
"" "

importar rospy
de mavros_msgs.msg estado de importação
de geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, Vector3, Twist, TwistStamped
importar matemática
importar numpy
from std_msgs.msg import Header
de PID importar PID

classe VelocityController:
alvo = PoseStamped ()
saída = TwistStamped ()

def __init__(self):
    self.X = PID()
    self.Y = PID()
    self.Z = PID()
    self.lastTime = rospy.get_time()
    self.target = None

def setTarget(self, target):
    self.target = target

def update(self, state,x_vel,y_vel,hold_state):
    if (self.target is None):
        rospy.logwarn("Target position for velocity controller is none.")
        return None
    # simplify variables a bit
    time = state.header.stamp.to_sec()
    position = state.pose.position
    orientation = state.pose.orientation
    # create output structure
    output = TwistStamped()
    output.header = state.header
    # output velocities
    linear = Vector3()
    angular = Vector3()
    if x_vel == 0:
        self.target.position.x = hold_state.pose.position.x
        linear.x = self.X.update(self.target.position.x, position.x, time)
    else:
        linear.x = x_vel
    if y_vel == 0:
        self.target.position.y = hold_state.pose.position.y
        linear.y = self.Y.update(self.target.position.y, position.y, time)
    else:
        linear.y = y_vel
    # Control in Z vel
    linear.z = self.Z.update(self.target.position.z, position.z, time)
    # Control yaw (no x, y angular)
    # TODO
    output.twist = Twist()
    output.twist.linear = linear
    return output

def stop(self):
    setTarget(self.current)
    update(self.current)`

Ei. Você não pode controlar Vx, Vy e Z, apenas velocidades ou apenas posição. Se o drone está tremendo é porque ele responde ao ponto de ajuste da posição e depois à velocidade.

@AlexisTM atualmente você pode:
https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_receiver.cpp#L902
https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_receiver.cpp#L1005 -L1008
https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/mc_pos_control_main.cpp#L1612

As bitmasks são:
para controlar vx, vy, z, y -> 3043 ( 0xBE3 )
para controlar vx, vy, z -> 4067 ( 0xFE3 )
para controlar vx, vy, z, y_rate -> 2019 ( 0x7E3 )

De fato, ele mencionou o uso de setpoint_position e setpoint_velocity . Ele tem que usar setpoint_raw e definir o type_mask.

Além disso, não tenho certeza se o modo está funcionando, pois offboard_mode é convertido em vehicule_mode e offboard_mode.ignore_alt_hold não é mencionado de forma alguma. Portanto, nunca testei.

https://github.com/PX4/Firmware/blob/ Budap4061ea54cd41289a9faf37c1cac5868c11773/src/modules/commander/commander.cpp#L4001 -L4039