Waypoint #31
Description
I have tried to modify the code so that the drone tries to reach a set of discrete points but the quadrotor gets stuck on the first waypoint when run in offboard mode. Here is the code I created using the one from the repository as a starting point.
author = "Jaeyoung Lim"
contact = "jalim@ethz.ch"
import rclpy
from rclpy.node import Node
from rclpy.clock import Clock
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy
from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleStatus, VehicleLocalPosition
import numpy as np
import time
class OffboardControl(Node):
def __init__(self):
super().__init__('setpoint')
qos_profile = QoSProfile(
reliability=QoSReliabilityPolicy.BEST_EFFORT,
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
history=QoSHistoryPolicy.KEEP_LAST,
depth=10
)
self.status_sub = self.create_subscription(
VehicleStatus,
'/fmu/out/vehicle_status',
self.vehicle_status_callback,
qos_profile)
self.subscription = self.create_subscription(
VehicleLocalPosition, '/fmu/out/vehicle_local_position', self.vehicle_local_position_callback, qos_profile
)
self.offboard_control_mode_publisher = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile)
self.trajectory_setpoint_publisher = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile)
self.waypoints = [
[20, 0, -5], [20, 20, -5], [0, 20, -5],
[0, 0, -5], [0, 0, 0] # Waypoints truncated for brevity
]
self.waypoint_index = 0
timer_period = 0.01
self.timer = self.create_timer(timer_period, self.send_commands)
self.nav_state = VehicleStatus.NAVIGATION_STATE_MAX
self.arming_state = VehicleStatus.ARMING_STATE_DISARMED
def vehicle_status_callback(self, msg):
# TODO: handle NED->ENU transformation
print("NAV_STATUS: ", msg.nav_state)
print(" - offboard status: ", VehicleStatus.NAVIGATION_STATE_OFFBOARD)
self.nav_state = msg.nav_state
self.arming_state = msg.arming_state
def vehicle_local_position_callback(self, msg):
x, y = msg.x, msg.y
self.get_logger().info(f'Received Position - X: {x}, Y: {y}')
if (self.waypoints[self.waypoint_index][0] - 0.3 < x < self.waypoints[self.waypoint_index][0] + 0.3 and
self.waypoints[self.waypoint_index][1] - 0.3 < y < self.waypoints[self.waypoint_index][1] + 0.3):
self.waypoint_index += 1
if self.waypoint_index >= len(self.waypoints):
self.get_logger().info("Reached final waypoint, exiting...")
rclpy.shutdown()
return
self.get_logger().info(f'Next waypoint: {self.waypoints[self.waypoint_index]}')
def send_commands(self):
offboard_msg = OffboardControlMode()
offboard_msg.timestamp = int(Clock().now().nanoseconds / 1000)
offboard_msg.position = True
offboard_msg.velocity = False
offboard_msg.acceleration = False
self.offboard_control_mode_publisher.publish(offboard_msg)
if (self.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD and self.arming_state == VehicleStatus.ARMING_STATE_ARMED):
trajectory_msg = TrajectorySetpoint()
trajectory_msg.position[0] = float(self.waypoints[self.waypoint_index][0])
trajectory_msg.position[1] = float(self.waypoints[self.waypoint_index][1])
trajectory_msg.position[2] = float(self.waypoints[self.waypoint_index][2])
self.trajectory_setpoint_publisher.publish(trajectory_msg)
def main(args=None):
rclpy.init(args=args)
node = OffboardControl()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if name == 'main':
main()