I am actually using a code I got from github which controls the drone through key binded velocity controls I tried changing it to a program which would make make the program follow waypoints as position in global LLA and I came to know that there’s no px4_msgs to do that I tried to publish it through mavros but its not having any effect on drone simulation.QGC mavlink inspector showing POSITION_TARGET_GLOBAL_INT at 0Hz
This is what printed in px4_sitl terminal
[0/1] cd /root/PX4-Autopilot/build/px4...topilot/build/px4_sitl_default/bin/px4
______ __ __ ___
| ___ \ \ \ / / / |
| |_/ / \ V / / /| |
| __/ / \ / /_| |
| | / /^\ \ \___ |
\_| \/ \/ |_/
px4 starting.
INFO [px4] startup script: /bin/sh etc/init.d-posix/rcS 0
INFO [init] found model autostart file as SYS_AUTOSTART=4001
INFO [param] selected parameter default file parameters.bson
INFO [param] importing from 'parameters.bson'
INFO [parameters] BSON document size 334 bytes, decoded 334 bytes (INT32:14, FLOAT:3)
INFO [param] selected parameter backup file parameters_backup.bson
INFO [dataman] data manager file './dataman' size is 7872608 bytes
INFO [init] Gazebo simulator
INFO [init] starting gazebo with world: /root/PX4-Autopilot/Tools/simulation/gz/worlds/default.sdf
WARN [init] PX4_GZ_MODEL_POSE not set, spawning at origin.
INFO [gz_bridge] world: default, model name: x500_0, simulation model: x500
INFO [gz_bridge] Requested Model Position: 0,0,0,0,0,0
INFO [gz_bridge] Model position z is less or equal 0.0, moving upwards
Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway.
Warning [Utils.cc:130] [/sdf/world[@name="default"]/spherical_coordinates/altitude:/root/PX4-Autopilot/Tools/simulation/gz/worlds/default.sdf:L153]: XML Element[altitude], child of element[spherical_coordinates], not defined in SDF. Copying[altitude] as children of [spherical_coordinates].
libEGL warning: egl: failed to create dri2 screen
libEGL warning: egl: failed to create dri2 screen
INFO [lockstep_scheduler] setting initial absolute time to 8000 us
WARN [vehicle_angular_velocity] no gyro selected, using sensor_gyro:0 1310988
INFO [commander] LED: open /dev/led0 failed (22)
INFO [tone_alarm] home set
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030
INFO [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280
INFO [logger] logger started (mode=all)
INFO [logger] Start file log (type: full)
INFO [logger] [logger] ./log/2024-03-02/06_03_54.ulg
INFO [logger] Opened full log file: ./log/2024-03-02/06_03_54.ulg
INFO [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO [px4] Startup script returned successfully
pxh> INFO [mavlink] partner IP: 127.0.0.1
INFO [tone_alarm] notify negative
INFO [commander] Ready for takeoff!
INFO [uxrce_dds_client] synchronized with time offset 1709359434408551us
INFO [uxrce_dds_client] successfully created rt/fmu/out/battery_status data writer, topic id: 19
INFO [uxrce_dds_client] successfully created rt/fmu/out/estimator_status_flags data writer, topic id: 86
INFO [uxrce_dds_client] successfully created rt/fmu/out/failsafe_flags data writer, topic id: 92
INFO [uxrce_dds_client] successfully created rt/fmu/out/manual_control_setpoint data writer, topic id: 139
INFO [uxrce_dds_client] successfully created rt/fmu/out/position_setpoint_triplet data writer, topic id: 170INFO [uxrce_dds_client] successfully created rt/fmu/out/sensor_combined data writer, topic id: 192
INFO [uxrce_dds_client] successfully created rt/fmu/out/timesync_status data writer, topic id: 215
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_attitude data writer, topic id: 231
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_control_mode data writer, topic id: 238
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_command_ack data writer, topic id: 235
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_global_position data writer, topic id: 239
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_gps_position data writer, topic id: 241
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_local_position data writer, topic id: 245
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_odometry data writer, topic id: 250
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_status data writer, topic id: 255
INFO [tone_alarm] notify negative
INFO [tone_alarm] notify negative
INFO [commander] Armed by external command
INFO [tone_alarm] arming warning
INFO [tone_alarm] notify negative
WARN [navigator] Already higher than takeoff altitude
WARN [navigator] Already higher than takeoff altitude
INFO [tone_alarm] notify negative
INFO [tone_alarm] notify negative
INFO [commander] Disarmed by auto preflight disarming
INFO [tone_alarm] notify neutral
INFO [tone_alarm] notify negative
INFO [commander] Armed by external command
INFO [tone_alarm] arming warning
INFO [navigator] Using default takeoff altitude: 2.5 m
INFO [logger] Start file log (type: full)
INFO [logger] [logger] ./log/2024-03-02/06_04_18.ulg
INFO [logger] closed logfile, bytes written: 5854127
INFO [logger] Opened full log file: ./log/2024-03-02/06_04_18.ulg
INFO [tone_alarm] notify negative
WARN [timesync] time jump detected. Resetting time synchroniser.
INFO [commander] Takeoff detected
INFO [tone_alarm] notify negative
INFO [uxrce_dds_client] successfully created rt/fmu/out/mode_completed data writer, topic id: 148
INFO [tone_alarm] notify negative
INFO [tone_alarm] notify negative
INFO [tone_alarm] notify negative
WARN [timesync] time jump detected. Resetting time synchroniser.
INFO [tone_alarm] notify negative
INFO [tone_alarm] notify negative
INFO [tone_alarm] notify negative
INFO [tone_alarm] notify negative
INFO [tone_alarm] notify negative
WARN [timesync] time jump detected. Resetti
THis is the code I am using
from mavros_msgs.msg import GlobalPositionTarget
import rclpy
from rclpy.node import Node
import numpy as np
from rclpy.clock import Clock
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy
from std_msgs.msg import Header
from geographic_msgs.msg import GeoPose
from px4_msgs.msg import OffboardControlMode
from px4_msgs.msg import TrajectorySetpoint
from px4_msgs.msg import VehicleStatus
from px4_msgs.msg import VehicleAttitude
from px4_msgs.msg import VehicleCommand
from px4_msgs.msg import VehicleGlobalPosition
from geometry_msgs.msg import Twist, Vector3
from math import pi
from std_msgs.msg import Bool
from geographic_msgs.msg import GeoPoseStamped
class OffboardControl(Node):
id=0
HOME=0
def __init__(self):
super().__init__('minimal_publisher')
qos_profile = QoSProfile(
reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1
)
#Create subscriptions
self.status_sub = self.create_subscription(
VehicleStatus,
'/fmu/out/vehicle_status',
self.vehicle_status_callback,
qos_profile)
self.gpsekf=self.create_subscription(
VehicleGlobalPosition,
'/fmu/out/vehicle_global_position',
self.gps_callback,
qos_profile)
self.offboard_gpos_sub = self.create_subscription(
VehicleGlobalPosition,
'/offboard_gpos_cmd',
self.offboard_gpos_callback,
qos_profile)
self.attitude_sub = self.create_subscription(
VehicleAttitude,
'/fmu/out/vehicle_attitude',
self.attitude_callback,
qos_profile)
self.my_bool_sub = self.create_subscription(
Bool,
'/arm_message',
self.arm_message_callback,
qos_profile)
#Create publishers
self.publisher_offboard_mode = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile)
self.publisher_velocity = self.create_publisher(Twist, '/fmu/in/setpoint_velocity/cmd_vel_unstamped', qos_profile)
self.publisher_trajectory = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile)
self.vehicle_command_publisher_ = self.create_publisher(VehicleCommand, "/fmu/in/vehicle_command", 10)
self.tpos_publisher=self.create_publisher(GeoPoseStamped, 'mavros/setpoint_position/global', 20)
#creates callback function for the arm timer
# period is arbitrary, just should be more than 2Hz
arm_timer_period = .1 # seconds
self.arm_timer_ = self.create_timer(arm_timer_period, self.arm_timer_callback)
cgps_timer_period=5
self.cgps_timer_=self.create_timer(cgps_timer_period,self.cgps_timer_callback)
# creates callback function for the command loop
# period is arbitrary, just should be more than 2Hz. Because live controls rely on this, a higher frequency is recommended
# commands in cmdloop_callback won't be executed if the vehicle is not in offboard mode
timer_period = 0.01 # seconds
self.timer = self.create_timer(timer_period, self.cmdloop_callback)
self.glomav = self.create_timer(timer_period, self.glo_callback)
self.nav_state = VehicleStatus.NAVIGATION_STATE_MAX
self.arm_state = VehicleStatus.ARMING_STATE_ARMED
self.velocity = Vector3()
self.lath=0.0
self.lonh=0.0
self.alth=0.0
self.clat=0.0
self.clon=0.0
self.calt=0.0
self.tlat=0.0
self.tlon=0.0
self.talt=0.0
self.yaw = 0.0 #yaw value we send as command
self.trueYaw = 0.0 #current yaw value of drone
self.offboardMode = False
self.flightCheck = False
self.myCnt = 0
self.arm_message = False
self.failsafe = False
#states with corresponding callback functions that run once when state switches
self.states = {
"IDLE": self.state_init,
"ARMING": self.state_arming,
"TAKEOFF": self.state_takeoff,
"LOITER": self.state_loiter,
"OFFBOARD": self.state_offboard
}
self.current_state = "IDLE"
self.last_state = self.current_state
def offboard_gpos_callback(self,msg):
self.tlat=msg.lat
self.tlon=msg.lon
self.talt=msg.alt
def gps_callback(self,msg):
if(self.HOME==0):
self.lath=self.clat=self.tlat=msg.lat
self.lonh=self.clon=self.tlon=msg.lon
self.alth=self.calt=self.talt=msg.alt
self.HOME=1
self.get_logger().info(f"HLat HLon HAlt: {self.lath} {self.lonh} {self.alth}")
else:
self.clat=msg.lat
self.clon=msg.lon
self.calt=msg.alt
def cgps_timer_callback(self):
self.get_logger().info(f"CLat CLon CAlt: {self.clat} {self.clon} {self.calt}")
def arm_message_callback(self, msg):
self.arm_message = msg.data
self.get_logger().info(f"Arm Message: {self.arm_message}")
#callback function that arms, takes off, and switches to offboard mode
#implements a finite state machine
def arm_timer_callback(self):
match self.current_state:
case "IDLE":
if(self.flightCheck and self.arm_message == True):
self.current_state = "ARMING"
self.get_logger().info(f"Arming")
case "ARMING":
if(not(self.flightCheck)):
self.current_state = "IDLE"
self.get_logger().info(f"Arming, Flight Check Failed")
elif(self.arm_state == VehicleStatus.ARMING_STATE_ARMED and self.myCnt > 10):
self.current_state = "TAKEOFF"
self.get_logger().info(f"Arming, Takeoff")
self.arm() #send arm command
case "TAKEOFF":
if(not(self.flightCheck)):
self.current_state = "IDLE"
self.get_logger().info(f"Takeoff, Flight Check Failed")
elif(self.nav_state == VehicleStatus.NAVIGATION_STATE_AUTO_TAKEOFF):
self.current_state = "LOITER"
self.get_logger().info(f"Takeoff, Loiter")
self.arm() #send arm command
self.take_off() #send takeoff command
# waits in this state while taking off, and the
# moment VehicleStatus switches to Loiter state it will switch to offboard
case "LOITER":
if(not(self.flightCheck)):
self.current_state = "IDLE"
self.get_logger().info(f"Loiter, Flight Check Failed")
elif(self.nav_state == VehicleStatus.NAVIGATION_STATE_AUTO_LOITER):
self.current_state = "OFFBOARD"
self.get_logger().info(f"Loiter, Offboard")
self.arm()
case "OFFBOARD":
if(not(self.flightCheck) or self.arm_state == VehicleStatus.ARMING_STATE_DISARMED or self.failsafe == True):
self.current_state = "IDLE"
self.get_logger().info(f"Offboard, Flight Check Failed")
for i in range(20){
}
self.state_offboard()
if(self.arm_state != VehicleStatus.ARMING_STATE_ARMED):
self.arm_message = False
if (self.last_state != self.current_state):
self.last_state = self.current_state
self.get_logger().info(self.current_state)
self.myCnt += 1
def state_init(self):
self.myCnt = 0
def state_arming(self):
self.myCnt = 0
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0)
self.get_logger().info("Arm command send")
def state_takeoff(self):
self.myCnt = 0
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_TAKEOFF, param1 = 1.0, param7=5.0) # param7 is altitude in meters
self.get_logger().info("Takeoff command send")
def state_loiter(self):
self.myCnt = 0
self.get_logger().info("Loiter Status")
def state_offboard(self):
self.myCnt = 0
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 1., 6.)
self.offboardMode = True
# Arms the vehicle
def arm(self):
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0)
self.get_logger().info("Arm command send")
# Takes off the vehicle to a user specified altitude (meters)
def take_off(self):
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_TAKEOFF, param1 = 1.0, param7=5.0) # param7 is altitude in meters
self.get_logger().info("Takeoff command send")
#publishes command to /fmu/in/vehicle_command
def publish_vehicle_command(self, command, param1=0.0, param2=0.0, param7=0.0):
msg = VehicleCommand()
msg.param1 = param1
msg.param2 = param2
msg.param7 = param7 # altitude value in takeoff command
msg.command = command # command ID
msg.target_system = 1 # system which should execute the command
msg.target_component = 1 # component which should execute the command, 0 for all components
msg.source_system = 1 # system sending the command
msg.source_component = 1 # component sending the command
msg.from_external = True
msg.timestamp = int(Clock().now().nanoseconds / 1000) # time in microseconds
self.vehicle_command_publisher_.publish(msg)
#receives and sets vehicle status values
def vehicle_status_callback(self, msg):
if (msg.nav_state != self.nav_state):
self.get_logger().info(f"NAV_STATUS: {msg.nav_state}")
if (msg.arming_state != self.arm_state):
self.get_logger().info(f"ARM STATUS: {msg.arming_state}")
if (msg.failsafe != self.failsafe):
self.get_logger().info(f"FAILSAFE: {msg.failsafe}")
if (msg.pre_flight_checks_pass != self.flightCheck):
self.get_logger().info(f"FlightCheck: {msg.pre_flight_checks_pass}")
self.nav_state = msg.nav_state
self.arm_state = msg.arming_state
self.failsafe = msg.failsafe
self.flightCheck = msg.pre_flight_checks_pass
#receives current trajectory values from drone and grabs the yaw value of the orientation
def attitude_callback(self, msg):
orientation_q = msg.q
#trueYaw is the drones current yaw value
self.trueYaw = -(np.arctan2(2.0*(orientation_q[3]*orientation_q[0] + orientation_q[1]*orientation_q[2]),
1.0 - 2.0*(orientation_q[0]*orientation_q[0] + orientation_q[1]*orientation_q[1])))
#publishes offboard control modes and velocity as trajectory setpoints
def cmdloop_callback(self):
if(self.offboardMode == True):
# Publish offboard control modes
self.id+=1
offboard_msg = OffboardControlMode()
offboard_msg.timestamp = int(Clock().now().nanoseconds / 1000)
offboard_msg.position = True
offboard_msg.velocity = False
offboard_msg.acceleration = False
self.publisher_offboard_mode.publish(offboard_msg)
def glo_callback(self):
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_WAYPOINT)
for i in range(20):
msg=GeoPoseStamped()
msg.header=Header()
msg.header.stamp=Clock().now().to_msg()
msg.header.frame_id="glotarget"
msg.pose=GeoPose()
msg.pose.position.latitude=self.tlat
msg.pose.position.longitude=self.tlon
msg.pose.position.altitude=self.talt
self.tpos_publisher.publish(msg)
def main(args=None):
rclpy.init(args=args)
offboard_control = OffboardControl()
rclpy.spin(offboard_control)
offboard_control.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
I saw somewhere that I need to change the nav_mode self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_WAYPOINT) I tried to do it with this.