Mavros mavcmd 213: unexpected command error with result 0

asked 2021-09-22 09:24:03 -0600

ahalic gravatar image

updated 2021-09-24 01:37:48 -0600

Hello,

I am trying to use mavcmd 213 from within a Python script. However, I have encountered an unexpected problem. When executing the script, I get the error CMD: Unexpected command 213, result 0.

After reading mavros documentation it seems to me result 0 is equal to the command being valid and being executed, which is even more confusing.

Additionally, when I manually enter the command via rosrun mavros mavcmd long 213 <heading> <speed> <absolute> 0 0 0 0 0 the command is successfully executed.

Moreover, despite the unexpected command error it seems that the command is actually executed (at the moment I have no access to my engine/rotors to check if they are actually moving), because if the command is not resent in a loop i get the FCU: target not received last 3secs, stopping.

Below I attached a snippet of my Python code that executes the command, maybe something is wrong here:

set_yaw_speed = rospy.ServiceProxy('mavros/cmd/command', CommandLong)
syw = set_yaw_speed(broadcast=1, command=213, confirmation=1, param1=self.course, param2=self.speed, param3=0, param4=0, param5=0, param6=0, param7=0)

I left the param3 to 0 intentionally, as its meant to be absolute course. The self.course and self.speed are floats that are attributes of an object, but I tried substituting them manually for a predefined variable, which changed nothing. I can provide bigger part of code if necessary.

I should probably add I am running ROS Lunar with Pixhawk 2.1. I am running Ubuntu ver 16.04 kernel 4.15.0-142-generic.

I would be much obliged for any help or tips or explanations. Thanks in advance. If any additional information is needed I will be glad to try to provide it.

Added full code as requested by osilva.

Some explanation: Desired course and desired speed are floats with 1 decimal precision. They are read from topic /AC_man which is a custom topic to which desired course and speed are published after being calculated by a standalone module written in C#. Mode is also changed by a standalone python script which listens for a string on topic /mode_command and executes mavcmd 176 (I can confirm it works, as Pixhawk mode gets changed).

Full code below:

#!/usr/bin/env python
# - *- coding: utf- 8 - *-
import rospy
from hh_1.msg import ac_man
from std_msgs.msg import Float64
from mavros_msgs.msg import VFR_HUD 
from mavros_msgs.srv import CommandLong
from hh_1.msg import mode_cmd

class AntiCollision():
    HDG_TOPIC = '/mavros/global_position/compass_hdg' #where to listen for current heading
    SPD_TOPIC = '/mavros/vfr_hud' #where to listen for current speed
    AC_TOPIC = '/AC_man' #where to listen for anticollision maneuver
    COURSE_HYST = 2.0 #course hysteresis for maneuver [deg]
    SPEED_HYST = 0.3 #speed hysteresis for maneuver [m/s]
    SERV_TIMEOUT = 10 #service timeout [s]

    def __init__(self):
        self.current_mode = 'AUTO'
        self.current_course = None
        self.current_speed = None
        self.desired_course = None
        self.desired_speed = None
        self.course_type = 0 #0 - absolute course, 1 - relative course

    def AC_callback(self, data):
        """Get vehicle desired (new) course and speed"""
        self.desired_course = float(data ...
(more)
edit retag flag offensive close merge delete

Comments

Yes please share more code. I’d like to see how you wait for the message for example. Thank you

osilva gravatar image osilva  ( 2021-09-23 18:17:06 -0600 )edit

Added full code with some explanation.

ahalic gravatar image ahalic  ( 2021-09-24 01:33:36 -0600 )edit