Waypoint for indoor flying

[crazy-robotics]

---

Issue details

I am using pixhawk 4 with companion jetson orin nano and livox mid 360 for indoor flying, SLAM, navigation and obstacle avoidance without gps for indoor flying. I am able to fly the drone in offboard mod indoor on publishing on /mavros/setpoint_position/local which is working well. now i am working on waypoint following like the drone make a square, follow a predefined trajectory etc i check the following topic of mavros supporting waypoints which are

/mavros/geofence/waypoints
/mavros/mission/waypoints
/mavros/rallypoint/waypoints

if you look into the msg type of all or /mavros/mission/waypoints

uint16 current_seq
mavros_msgs/Waypoint[] waypoints
  uint8 FRAME_GLOBAL=0
  uint8 FRAME_LOCAL_NED=1
  uint8 FRAME_MISSION=2
  uint8 FRAME_GLOBAL_REL_ALT=3
  uint8 FRAME_LOCAL_ENU=4
  uint8 FRAME_GLOBAL_INT=5
  uint8 FRAME_GLOBAL_RELATIVE_ALT_INT=6
  uint8 FRAME_LOCAL_OFFSET_NED=7
  uint8 FRAME_BODY_NED=8
  uint8 FRAME_BODY_OFFSET_NED=9
  uint8 FRAME_GLOBAL_TERRAIN_ALT=10
  uint8 FRAME_GLOBAL_TERRAIN_ALT_INT=11
  uint8 FRAME_BODY_FRD=12
  uint8 FRAME_RESERVED_13=13
  uint8 FRAME_RESERVED_14=14
  uint8 FRAME_RESERVED_15=15
  uint8 FRAME_RESERVED_16=16
  uint8 FRAME_RESERVED_17=17
  uint8 FRAME_RESERVED_18=18
  uint8 FRAME_RESERVED_19=19
  uint8 FRAME_LOCAL_FRD=20
  uint8 FRAME_LOCAL_FLU=21
  uint8 frame
  uint16 command
  bool is_current
  bool autocontinue
  float32 param1
  float32 param2
  float32 param3
  float32 param4
  float64 x_lat
  float64 y_long
  float64 z_alt

float64 x_lat float64 y_long which i dont have. how i make my drone Waypoint following

MAVROS version and platform

Mavros: 1.20.0 (install by sudo apt install ros-noetic-mavros*)
ROS: noetic
Ubuntu: 20.04

Autopilot type and version

PX4

Version: 1.5

[thanhkaist]

@crazy-robotics It's impressive that you've successfully set up SLAM with the Livox Mid 360 working seamlessly! I'm currently working on a similar setup, though I'm not quite at your level yet. While my question may not directly answer yours, I hope we can collaborate and figure this out together.

Could you kindly share the detailed steps to make the EKF effectively utilize SLAM data?

Specifically, could you provide information on:

Which topics need to be published?
What parameters need to be configured?
How to properly set the origin?

From what I understand, SLAM typically outputs relative positions (e.g., from /odom to /base_link). Is this information sufficient for the EKF, or are additional inputs required?

Your insights would be incredibly helpful, and I'd love to stay in touch to exchange ideas and tackle challenges together until we complete this task.

[crazy-robotics]

Sure, DM

[thanhkaist]

@crazy-robotics Could I have your email/Facebook id to DM you? I think I reached your point and am finding a way to set waypoints.

[crazy-robotics]

sure contact me at [email protected]

[owenonline]

@crazy-robotics I am attempting to use a mavros with ROS 2 Foxy on Ubuntu 20.04 on a jetson xavier nx with a Pixhawk 6c running PX4 to navigate a rover indoors. I've tried publishing on /mavros/setpoint_position/local, /mavros/setpoint_velocity/cmd_vel_unstamped, and /mavros/setpoint_raw/local continuously and then attempting to arm in offboard mode, and while I am able to switch to offboard mode (regardless of whether any of those topics is being published to), I cannot get the vehicle to arm. In QGC, I just see the error

"No offboard signal: The offboard component is not sending setpoints or the required estimate (e.g. position) is missing."

Since you seem to have solved this problem, are you able to share your solution? The arming code I'm currently using is below:

import rclpy
from geometry_msgs.msg import Twist, PoseStamped
from nav_msgs.msg import Odometry
from sensor_msgs.msg import NavSatFix, Imu
from mavros_msgs.msg import PositionTarget
from std_msgs.msg import Float64, Bool
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
from rclpy.node import Node
from rclpy.qos import QoSPresetProfiles
from mavros_msgs.srv import CommandBool, SetMode, CommandLong
import threading
from time import time, sleep


MISSIONSTART = 0
ARMING = 1
MOVING = 2
DISARMING = 3
MISSIONCOMPLETE = 4

class FCUIntermediary(Node):
	velo_msg: Twist = None
	position_msg: PoseStamped = None

	accel_msg: PositionTarget = None

	def __init__(self):
		super().__init__('fcu_intermediary')

		# declare parameters
		self.declare_parameter('broadcast_interval', 0.1)
		broadcast_interval = self.get_parameter('broadcast_interval').value

		# pubsub setup
		self.velo_subscriber = self.create_subscription(Twist, '/f1tenth_racer/cmd_vel', self.cmd_vel_callback, QoSPresetProfiles.SYSTEM_DEFAULT.value, callback_group=MutuallyExclusiveCallbackGroup())
		self.killswitch_subscriber = self.create_subscription(Bool, '/f1tenth_racer/killswitch', self.killswitch_callback, QoSPresetProfiles.SYSTEM_DEFAULT.value, callback_group=MutuallyExclusiveCallbackGroup())
		# self.velo_publisher = self.create_publisher(Twist, '/mavros/setpoint_velocity/cmd_vel_unstamped', 20)
		# self.position_publisher = self.create_publisher(PoseStamped, '/mavros/setpoint_position/local', 20)

		self.setpoint_publisher = self.create_publisher(PositionTarget, '/mavros/setpoint_raw/local', 20)

        # arming and disarming services
		print('setting up motion clients')
		self.set_mode_client = self.create_client(SetMode, '/mavros/set_mode')
		while not self.set_mode_client.wait_for_service(timeout_sec=1.0):
			print('set mode service not available, waiting again...')
			
		self.arming_client = self.create_client(CommandBool, '/mavros/cmd/arming')
		while not self.arming_client.wait_for_service(timeout_sec=1.0):
			print('arming service not available, waiting again...')

		self.killswitch_client = self.create_client(CommandLong, '/mavros/cmd/command')
		while not self.killswitch_client.wait_for_service(timeout_sec=1.0):
			print('killswitch service not available, waiting again...')

		# initialize message to 0 so that if MOVING stage is reached before nav2 begins publishing, there is something to publish
		# initial_message = Twist()
		# initial_message.linear.x = 0.0
		# initial_message.linear.y = 0.0
		# initial_message.linear.z = 0.0

		# initial_message.angular.x = 0.0
		# initial_message.angular.y = 0.0
		# initial_message.angular.z = 0.0

		# self.velo_msg = initial_message

		# initial_position_msg = PoseStamped()
		# initial_position_msg.pose.position.x = 0.0
		# initial_position_msg.pose.position.y = 0.0
		# initial_position_msg.pose.position.z = 0.0

		initial_setpoint_msg = PositionTarget()
		initial_setpoint_msg.coordinate_frame = 1
		initial_setpoint_msg.position.x = 0.0
		initial_setpoint_msg.position.y = 0.0
		initial_setpoint_msg.position.z = 2.0
		initial_setpoint_msg.velocity.x = 0.0
		initial_setpoint_msg.velocity.y = 0.0
		initial_setpoint_msg.velocity.z = 0.0

		self.setpoint_msg = initial_setpoint_msg

		self.mission_status = MISSIONSTART

		# Start main loop
		self.mission_timer = self.create_timer(broadcast_interval, self.mission_timer_callback)

	def cmd_vel_callback(self, msg):
		"""Callback for the cmd_vel topic."""
		self.velo_msg = msg

	def killswitch_callback(self, msg):
		"""Callback for the killswitch topic."""
		if msg.data:
			self.disarm()

	def mission_timer_callback(self):
		"""Main loop for vehicle control. Handles the arming, moving, and disarming of the rover."""

		# start the chain of events that arms the rover
		if self.mission_status == MISSIONSTART:
			print("switching to offboard mode")
			set_mode_req = SetMode.Request()
			set_mode_req.base_mode = 0
			set_mode_req.custom_mode = "OFFBOARD"
			set_mode_future = self.set_mode_client.call_async(set_mode_req)
			set_mode_future.add_done_callback(self.init_callback)
			self.mission_status = ARMING
			return
		
		if self.mission_status == ARMING:
			# px4 requires a stream of setpoint messages to be sent to the rover in order to arm
			
			# self.velo_publisher.publish(self.velo_msg)
			# self.position_publisher.publish(self.position_msg)
			self.setpoint_publisher.publish(self.setpoint_msg)
			return
		
		if self.mission_status == MOVING:
			self.get_logger().debug(f"Published velocity message: {self.velo_msg}")
			# self.velo_publisher.publish(self.velo_msg)
			# self.position_publisher.publish(self.position_msg)
			self.setpoint_publisher.publish(self.setpoint_msg)
			return
		
		if self.mission_status == DISARMING:
			msg = Twist()
			msg.linear.x = 0.0
			msg.linear.y = 0.0
			msg.linear.z = 0.0

			msg.angular.x = 0.0
			msg.angular.y = 0.0
			msg.angular.z = 0.0
			
			# self.velo_publisher.publish(msg) # continue publishing stop messages until disarmed
			self.setpoint_publisher.publish(self.setpoint_msg)
			return
	
		if self.mission_status == MISSIONCOMPLETE:
			print("MISSION COMPLETE")
			self.get_logger().info("Car is shut down.")
			self.destroy_node()
			self.shutdown_flag = True
		
	def init_callback(self, future):
		print("...in offboard mode, arming")
		sleep(4) # wait for the stream of messages to be long enough to allow arming
		arm_req = CommandBool.Request()
		arm_req.value = True
		arm_future = self.arming_client.call_async(arm_req)
		arm_future.add_done_callback(self.arm_callback)

	def arm_callback(self, future):
		print("...armed, moving")
		self.start_time = time()
		self.mission_status = MOVING

	def disarm_callback(self, future):
		print("...disarmed")
		self.mission_status = MISSIONCOMPLETE

	def disarm(self):
		"""Disarm the vehicle"""

		disarm_req = CommandBool.Request()
		disarm_req.value = False
		disarm_future = self.arming_client.call_async(disarm_req)
		disarm_future.add_done_callback(self.disarm_callback)
		self.mission_status = DISARMING

def main(args=None):
    rclpy.init(args=args)
    fcu_intermediary = FCUIntermediary()
    
    # Add a flag to check for shutdown
    fcu_intermediary.shutdown_flag = False
    
    # Spin until shutdown is requested
    while rclpy.ok() and not fcu_intermediary.shutdown_flag:
        rclpy.spin_once(fcu_intermediary)
        
    # Continuously attempt to shut down the node until it succeeds
    while True:
        try:
            rclpy.shutdown()
            break
        finally:
            sleep(1)

if __name__ == '__main__':
    main()