Mavros: Maintenir la position de vol tout en donnant la commande de vitesse 2D au drone

Alimentez-vous les données Optitrack dans le Pixhawk via MAVROS ?

Je fournis des données Optitrack en tant que /mavros/mocap et j'ai également modifié les paramètres de Pixhawk de sorte qu'il utilise ces informations pour l'estimation de la position locale, le résultat est en fait assez bon.

Mais cela étant dit, j'essaie actuellement de travailler avec le simulateur Gazebo (sans compter mocap). Dans mon code, je crée à la fois des éditeurs setpoint_position et setpoint_velcity. J'ai mis "set_position.pose.position.z = 2" et "set_velocity.linear.x = 1" Mais le drone plane juste à 2 mètres, sans bouger nulle part.

Et ça me donne cette erreur :
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'}

J'ai trouvé le problème pour le message d'erreur. J'ai fait une erreur en essayant d'envoyer '/mavros/setpoint_velocity/cmd_vel'. Le type de message doit être TwistStamped, au lieu de Twist. Après avoir modifié mon set_velocity en 'set_velocity.twist.linear.x = 1', l'erreur disparaît.

Cependant, un nouveau problème apparaît. Le drone n'est pas stable du tout, il continue de trembler en avançant (direction +x). Je ne sais pas si mon approche est correcte. S'il vous plaît laissez-moi savoir s'il existe un moyen formel de le faire.

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

En fait, l'erreur indique quel est le problème. La commande de vélocité que vous envoyez est un message Twist , tandis que le nœud attend un message TwistStamped .

Cependant, un nouveau problème apparaît. Le drone n'est pas stable du tout, il continue de trembler en avançant (direction +x). Je ne sais pas si mon approche est correcte. S'il vous plaît laissez-moi savoir s'il existe un moyen formel de le faire.

Sans vérifier le code que vous utilisez, il est assez difficile de comprendre ce qui ne va pas.

@TSC21 Merci pour votre aide ! Malheureusement, je ne sais pas si je peux poster mon code source original. Je reviendrai sur ce problème dès que j'en aurai l'autorisation. Je m'excuse pour le dérangement.

Je publierai également la solution à ce problème particulier une fois que je l'aurai trouvée, car je pense que c'est une très bonne application/démo.

Je m'excuse pour le dérangement.

L'inconvénient n'est pas pour nous qui essayons de vous aider. Mais affirmer la fonctionnalité d'un morceau de code sans connaître le code est assez difficile.

@TSC21 Oui. Je comprends parfaitement.

D'ACCORD. Voici donc un script simple que je dois contrôler à la fois la vitesse et la position.

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()

Vous devez baser votre code sur celui-ci : https://dev.px4.io/en/ros/mavros_offboard.html.

Je vois que vous n'envoyez la consigne de position qu'une seule fois alors que vous devriez l'envoyer pendant un certain temps. Ensuite, quand il vérifie qu'il est dans le goal_pose , il devrait se déplacer dans la direction x avec cette vitesse. En outre, vous devez séparer la vérification du mode et de l'état d'armement en deux conditions différentes.

Je pense à une approche différente. Au lieu de compter sur '/mavros/setpoint_position/local' pour atteindre le maintien d'altitude, je pourrais aussi bien écrire mon propre contrôleur d'altitude PID qui prend la différence entre l'altitude souhaitée et mon altitude actuelle (set_z -measure_z) comme entrée, et contrôle de la vitesse de sortie dans la direction z. Par conséquent, tout ce dont j'ai besoin est de publier Vz pour maintenir l'altitude, cela peut nécessiter quelques réglages.

C'est quelque chose que j'ai déjà fait dans test_mavros avec control_toolbox . Vérifiez ce qui s'y trouve et vous n'aurez peut-être pas à le mettre en œuvre à partir de zéro.

Merci! Je vais y jeter un coup d'œil et je pense que c'est une excellente approche. Je pense que ce problème peut être clos maintenant.

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

J'ai modifié cela pour maintenir l'altitude tout en contrôlant la vitesse horizontale. Vous pouvez également essayer setpoint_raw, mais j'ai trouvé que cela ne fonctionnait pas.

@schmittlema , pouvez-vous me dire comment avez-vous modifié pour donner une vitesse constante avec maintien d'altitude?

Au dessous de. J'ai fait un 3 contrôleurs PID pour chaque axe. Ensuite, si la vitesse souhaitée est nulle, il maintient cette position d'axe. Z essaie toujours de maintenir l'altitude. Espérons que cela aide.
`"""
tester le contrôle de position hors-bord avec un simple script de décollage
"""

importer rospy
à partir de l'état d'importation mavros_msgs.msg
à partir de geometry_msgs.msg importer PoseStamped, Pose, Point, Quaternion, Vector3, Twist, TwistStamped
importer des mathématiques
importer numpy
à partir de l'en-tête d'importation std_msgs.msg
de PID importer PID

classe VelocityController :
cible = PoseStamped()
sortie = 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)`

Salut. Vous ne pouvez pas contrôler Vx, Vy et Z, uniquement les vitesses ou uniquement la position. Si le drone tremble c'est qu'il répond à la consigne de position puis à la vitesse.

@AlexisTM actuellement, vous pouvez :
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

Les masques de bits sont :
pour contrôler vx, vy, z, y -> 3043 ( 0xBE3 )
pour contrôler vx, vy, z -> 4067 ( 0xFE3 )
pour contrôler vx, vy, z, y_rate -> 2019 ( 0x7E3 )

En effet, mais il a mentionné l'utilisation de setpoint_position et setpoint_velocity . Il doit utiliser setpoint_raw et définir le type_mask.

De plus, je ne suis pas certain que le mode fonctionne car le offboard_mode est converti en vehicule_mode et le offboard_mode.ignore_alt_hold n'est pas du tout mentionné. Par conséquent, je ne l'ai jamais testé.

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