Hi, I’m trying to control plane movement by publishing AttitudeTarget data to /mavros/setpoint_raw/attitude topic. But I don’t see any visual effect in QGC.
I create Node like this:
#!/usr/bin/env python3
import time
from .guidance.guidance import Guidance
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from mavros.base import SENSOR_QOS
from mavros_msgs.msg import AttitudeTarget
from mavros_msgs.srv import SetMode
from geometry_msgs.msg import PoseStamped, Quaternion
QOS = SENSOR_QOS
class GuidanceNode(Node):
def __init__(self):
super().__init__('guidance')
self.guidance = Guidance()
qos_profile = QoSProfile(
reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=10
)
# Getting local position of UAV
self.pose_subscription = self.create_subscription(
PoseStamped,
"/mavros/local_position/pose",
self.__pose_callback,
qos_profile
)
# setting up roll, pitch, yaw angles to UAV
self.attitude_target_publisher = self.create_publisher(AttitudeTarget, "/mavros/setpoint_raw/attitude",
QOS)
def set_attitute_target(self, roll, pitch, yaw) -> Quaternion:
w, x, y, z = self.guidance.to_quaternion(roll, pitch, yaw)
msg = AttitudeTarget()
msg.header.stamp = self.get_clock().now().to_msg()
msg.type_mask = 0b10111000
msg.orientation = Quaternion(w=w, x=x, y=y, z=z)
msg.thrust = 0.5
self.attitude_target_publisher.publish(msg)
def set_guided_mode(self):
self.get_logger().info("Setting up mode to GUIDED...")
timeout = 1.0 # seconds
set_mode_client = self.create_client(SetMode, '/mavros/set_mode')
while not set_mode_client.wait_for_service(timeout):
self.get_logger().info('/mavros/set_mode Service not available, waiting again...')
set_mode_req = SetMode.Request()
set_mode_req.custom_mode = 'GUIDED'
set_mode_future = set_mode_client.call_async(set_mode_req)
rclpy.spin_until_future_complete(self, set_mode_future)
response = set_mode_future.result()
if response.mode_sent:
self.get_logger().info("Mode switched to GUIDED")
else:
self.get_logger().info("Mode switch failed")
def __pose_callback(self, msg: PoseStamped):
# Since MAVROS uses ENA (east-north-down) coordinate system
# and guidance algorithm uses NED (north-east-down) coordinate sytem
# before setting up target and UAV coord - there is a need to do conversion
# by next rules:
# swap the order of the first two coordinates: replace 'x' and 'y'
# provide 'z' coord with minus sign
self.guidance.set_uav_pos(msg.pose.position.y, msg.pose.position.x, -msg.pose.position.z)
def main(args=None):
print('Pugach Guidance node starting')
rclpy.init(args=args)
guidance_node = GuidanceNode()
# First, change mode to GUIDED
guidance_node.set_guided_mode()
while rclpy.ok():
guidance_node.set_attitute_target(0, 0, 0)
rclpy.spin_once(guidance_node)
print('Pugach Guidance node shutting down.')
guidance_node.destroy_node()
rclpy.shutdown()
if name == '__main__':
main()
I see that mode changed to GUIDED, so it means we have connection to QGC. But plane doesn’t react to attitude messages. Does anybody see where is the error here?