Hi!
Could I get some help getting my actuator_servo and actuator_motor publishings connected to my SITL? I’m also looking to properly transition to fixed-wing, if I can get help there too.
Current command to do fixed-wing, as I understand it:
# Create and publish VehicleCommand message to change mode
cmd = VehicleCommand()
cmd.command = 84 # Command to change flight mode (see https://docs.px4.io/main/en/msg_docs/VehicleCommand.html, line 84)
cmd.param1 = 1.0 # Parameter to indicate the desired mode (check specific parameter for your system)
cmd.target_system = 1
cmd.source_system = 1
cmd.target_component = 0
cmd.source_component = 0
self.vehicle_command_pub.publish(cmd)
self.get_logger().info("Published VehicleCommand message to change mode to fixed-wing")
Background:
- I want to test automating actuator and motor outputs so I can wrap my simulation code with ROS 2 and have it auto-fly in real life.
- In the test code and setup provided below, I take the provided offboard_control.cpp example (converted to python), and throw in a timer so that after 20 seconds, a VTOL transition should occur that allows for the sample outputs to servo and motor to happen (and theoretically get some crazy plane crash
).
The problem:
- At 200 seconds, the plane just hovers back down and lands. No actuator work was done.
- The “mode” that the plane turns into (as reported by QGroundControl) becomes a weird number, which leads me to believe I am not properly transitioning to fixed-wing mode.
- None of the servos or motors seem to behave any differently.
Here is an image of the actuator setup for the SITL in the QGroundControl:
Here is the public repo:
https://github.com/FredJones4/vtol_ctrl_ros2
This repo here should be a public repository and easily viewable. The intent with sharing the repo is:
- Allow you to see the complete configuration
- Prevent the bombardment of code on the comments section (for the sake of future readers after I push more to this code, I will still add the notes down below).
- Allow access to the terminal output of xrce_dds agent, the ros node, and the gazebo simulation.
I don’t have anything beyond some basic example code, so I feel fine sharing this. Please let me know if I am breaking community guidelines in doing so; if I am, I will remove it.
Gazebo simulation log: vtol_ctrl_ros2/GZ_notes.md at main · FredJones4/vtol_ctrl_ros2 · GitHub
Ros 2 Node (called ros2_px4_interface, just like the package): vtol_ctrl_ros2/ros2NodeNotes.md at main · FredJones4/vtol_ctrl_ros2 · GitHub
DDS client log: vtol_ctrl_ros2/someDDSClientNotes.md at main · FredJones4/vtol_ctrl_ros2 · GitHub
Note about the DDS log: It was running for a long, long time, so I just grabbed a small sample. Putting it side by side with the ros2 notes and comparing numbers will help.
Here is the simulation before starting the node:
GZ Notes:
make px4_sitl gz_standard_vtol
[0/1] cd /home/cmhales/PX4-Autopilot/build/px4_sitl_default/src/modules/simulation/gz_bridg...v PX4_SIM_MODEL=gz_standard_vtol /home/cmhales/PX4-Autopilot/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=4004
INFO [param] selected parameter default file parameters.bson
INFO [param] importing from 'parameters.bson'
INFO [parameters] BSON document size 404 bytes, decoded 404 bytes (INT32:14, FLOAT:6)
INFO [param] selected parameter backup file parameters_backup.bson
ERROR [param] Parameter FW_L1_PERIOD not found.
INFO [dataman] data manager file './dataman' size is 7866640 bytes
INFO [init] starting gazebo with world: /home/cmhales/PX4-Autopilot/Tools/simulation/gz/worlds/default.sdf
WARN [init] PX4_GZ_MODEL_NAME or PX4_GZ_MODEL not set using PX4_SIM_MODEL.
INFO [gz_bridge] world: default, model name: standard_vtol_0, simulation model: standard_vtol
Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway.
INFO [lockstep_scheduler] setting initial absolute time to 8000 us
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] partner IP: 127.0.0.1
INFO [tone_alarm] notify negative
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-08-13/22_58_42.ulg
INFO [logger] Opened full log file: ./log/2024-08-13/22_58_42.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 [uxrce_dds_client] synchronized with time offset 1723589921582647us
INFO [uxrce_dds_client] successfully created rt/fmu/out/failsafe_flags data writer, topic id: 80
INFO [uxrce_dds_client] successfully created rt/fmu/out/position_setpoint_triplet data writer, topic id: 149
INFO [uxrce_dds_client] successfully created rt/fmu/out/sensor_combined data writer, topic id: 168
INFO [uxrce_dds_client] successfully created rt/fmu/out/timesync_status data writer, topic id: 191
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_control_mode data writer, topic id: 212
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_gps_position data writer, topic id: 215
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_local_position data writer, topic id: 219
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_odometry data writer, topic id: 224
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_status data writer, topic id: 229
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_attitude data writer, topic id: 206
INFO [uxrce_dds_client] successfully created rt/fmu/out/vehicle_global_position data writer, topic id: 213
INFO [commander] Ready for takeoff!
WARN [health_and_arming_checks] Preflight Fail: Yaw estimate error
INFO [commander] Armed by internal command
INFO [tone_alarm] arming warning
INFO [commander] Takeoff detected
INFO [commander] Landing detected
INFO [commander] Disarmed by landing
INFO [tone_alarm] notify neutral
INFO [logger] closed logfile, bytes written: 108044716
ERROR [timesync] Time jump detected. Resetting time synchroniser.
ROS log:
ros2 run ros2_px4_interface ros2_px4_interface
counter: 0
[INFO] [1723590336.302824611] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590336.307544902] [offboard_control]: Published TrajectorySetpoint message
counter: 1
[INFO] [1723590336.309821622] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590336.310468801] [offboard_control]: Published TrajectorySetpoint message
counter: 2
[INFO] [1723590336.372162840] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590336.374561394] [offboard_control]: Published TrajectorySetpoint message
counter: 3
[INFO] [1723590336.472817584] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590336.474186525] [offboard_control]: Published TrajectorySetpoint message
counter: 4
[INFO] [1723590336.572186229] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590336.573539101] [offboard_control]: Published TrajectorySetpoint message
counter: 5
[INFO] [1723590336.672504461] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590336.673615627] [offboard_control]: Published TrajectorySetpoint message
counter: 6
[INFO] [1723590336.772026352] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590336.773755887] [offboard_control]: Published TrajectorySetpoint message
counter: 7
[INFO] [1723590336.871478192] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590336.872946451] [offboard_control]: Published TrajectorySetpoint message
counter: 8
[INFO] [1723590336.972183433] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590336.973700089] [offboard_control]: Published TrajectorySetpoint message
counter: 9
[INFO] [1723590337.074135961] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590337.077226372] [offboard_control]: Published TrajectorySetpoint message
counter: 10
[INFO] [1723590337.174743971] [offboard_control]: Published VehicleCommand message with command 176
[INFO] [1723590337.177140665] [offboard_control]: Published VehicleCommand message with command 400
[INFO] [1723590337.178245610] [offboard_control]: Arm command sent
[INFO] [1723590337.178918569] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590337.179361609] [offboard_control]: Published TrajectorySetpoint message
counter: 11
[INFO] [1723590337.602357388] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590337.603934319] [offboard_control]: Published TrajectorySetpoint message
counter: 12
[INFO] [1723590337.607501544] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590337.610578616] [offboard_control]: Published TrajectorySetpoint message
counter: 13
[INFO] [1723590337.672282122] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590337.674714359] [offboard_control]: Published TrajectorySetpoint message
counter: 14
[INFO] [1723590337.771540585] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590337.772016404] [offboard_control]: Published TrajectorySetpoint message
counter: 15
[INFO] [1723590337.874468041] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590337.879214617] [offboard_control]: Published TrajectorySetpoint message
counter: 16
...
[INFO] [1723590356.173869009] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590356.176792804] [offboard_control]: Published TrajectorySetpoint message
counter: 199
[INFO] [1723590356.274240300] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590356.278435388] [offboard_control]: Published TrajectorySetpoint message
counter: 200
[INFO] [1723590356.373755878] [offboard_control]: Preparing to change mode from VTOL to fixed-wing
counter: 201
[INFO] [1723590356.474259158] [offboard_control]: Published OffboardControlMode message
counter: 202
[INFO] [1723590356.572148112] [offboard_control]: Published OffboardControlMode message
counter: 203
[INFO] [1723590356.672642835] [offboard_control]: Published OffboardControlMode message
counter: 204
...
[INFO] [1723590361.174287142] [offboard_control]: Published OffboardControlMode message
counter: 249
[INFO] [1723590361.272467288] [offboard_control]: Published OffboardControlMode message
counter: 250
[INFO] [1723590361.373395313] [offboard_control]: Published OffboardControlMode message
[INFO] [1723590361.376263131] [offboard_control]: Changing mode to fixed-wing
[INFO] [1723590361.377772126] [offboard_control]: Published VehicleCommand message to change mode to fixed-wing
DDS log: (not included for sheer length)
The python code that creates the node:
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand
from px4_msgs.msg import (ActuatorMotors, ActuatorServos, VehicleCommand, OffboardControlMode)
from std_msgs.msg import Header
import time
import math
HZ_RATE = 100.0
class ROS2PX4Interface(Node):
def __init__(self):
super().__init__('offboard_control')
self.offboard_control_mode_publisher = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', 10)
self.trajectory_setpoint_publisher = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', 10)
self.vehicle_command_publisher = self.create_publisher(VehicleCommand, '/fmu/in/vehicle_command', 10)
self.offboard_setpoint_counter = 0
qos_profile = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE,
depth=10
)
# Create publishers with the specified QoS profiles
self.actuator_motors_pub = self.create_publisher(
ActuatorMotors,
'/fmu/in/actuator_motors',
qos_profile
)
self.vehicle_command_pub = self.create_publisher(
VehicleCommand,
'/fmu/in/vehicle_command',
qos_profile
)
self.actuator_servos_pub = self.create_publisher(
ActuatorServos,
'/fmu/in/actuator_servos',
qos_profile
)
self.offboard_control_mode_pub = self.create_publisher(
OffboardControlMode,
'/fmu/in/offboard_control_mode',
qos_profile
)
self.timer = self.create_timer(0.1, self.timer_callback) # 100 ms timer, or 10 Hz clock
def change_mode(self):
current_time = self.get_clock().now()
elapsed_time = (current_time - self.timer_start).nanoseconds/ 1e9
if elapsed_time >= self.timer_period:
self.get_logger().info("Changing mode to fixed-wing")
# Create and publish VehicleCommand message to change mode
cmd = VehicleCommand()
cmd.command = 84 # Command to change flight mode (check specific value for your system)
cmd.param1 = 1.0 # Parameter to indicate the desired mode (check specific parameter for your system)
cmd.target_system = 1
cmd.source_system = 1
cmd.target_component = 0
cmd.source_component = 0
self.vehicle_command_pub.publish(cmd)
self.get_logger().info("Published VehicleCommand message to change mode to fixed-wing")
# Stop the timer after mode change
self.timer2.destroy()
def timer_callback(self):
print("counter: ", self.offboard_setpoint_counter)
if self.offboard_setpoint_counter == 10:
# Change to Offboard mode after 10 setpoints
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 1.0, 6.0)
# Arm the vehicle
self.arm()
if self.offboard_setpoint_counter < 200: # 20 seconds
# Publish offboard control mode and trajectory setpoint
self.publish_offboard_control_mode()
self.publish_trajectory_setpoint()
if self.offboard_setpoint_counter > 10:
self.offboard_setpoint_counter += 1
elif self.offboard_setpoint_counter == 200:
self.timer_start = self.get_clock().now()
# Initialize the mode change (for example, change to fixed-wing after 5 seconds)
self.get_logger().info("Preparing to change mode from VTOL to fixed-wing")
self.timer_period = 5.0 # Change mode after 5 seconds
self.timer2 = self.create_timer(1.0, self.change_mode)
self.offboard_setpoint_counter += 1
pass
elif self.offboard_setpoint_counter < 400: #TEMP
self.publish_offboard_control_mode()
self.offboard_setpoint_counter += 1
else:
self.publish_offboard_control_mode()
self.publish_actuator_items()
# Increment the counter and stop after reaching 11
if self.offboard_setpoint_counter < 11:
self.offboard_setpoint_counter += 1
def arm(self):
"""Send a command to arm the vehicle."""
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0)
self.get_logger().info('Arm command sent')
def disarm(self):
"""Send a command to disarm the vehicle."""
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0)
self.get_logger().info('Disarm command sent')
def publish_offboard_control_mode(self):
"""Publish the offboard control mode."""
msg = OffboardControlMode()
msg.position = True
msg.velocity = False
msg.acceleration = False
msg.attitude = False
msg.body_rate = False
msg.timestamp = int(time.time() * 1e6) # microseconds
self.offboard_control_mode_publisher.publish(msg)
self.get_logger().info('Published OffboardControlMode message')
def publish_trajectory_setpoint(self):
"""Publish a trajectory setpoint."""
msg = TrajectorySetpoint()
msg.position = [0.0, 0.0, -20.0]
msg.yaw = -3.14 # [-PI:PI]
msg.timestamp = int(time.time() * 1e6) # microseconds
self.trajectory_setpoint_publisher.publish(msg)
self.get_logger().info('Published TrajectorySetpoint message')
def publish_vehicle_command(self, command, param1=0.0, param2=0.0):
"""Publish vehicle command."""
msg = VehicleCommand()
msg.param1 = param1
msg.param2 = param2
msg.command = command
msg.target_system = 1
msg.target_component = 1
msg.source_system = 1
msg.source_component = 1
msg.from_external = True
msg.timestamp = int(time.time() * 1e6) # microseconds
self.vehicle_command_publisher.publish(msg)
self.get_logger().info(f'Published VehicleCommand message with command {command}')
def publish_actuator_motors(self, motors_msg):
self.actuator_motors_pub.publish(motors_msg)
self.get_logger().info('Published ActuatorMotors message')
def publish_actuator_servos(self, servos_msg):
self.actuator_servos_pub.publish(servos_msg)
self.get_logger().info('Published ActuatorServos message')
def publish_actuator_items(self):
motors_msg = ActuatorMotors() # Populate with relevant data
motors_msg.control = [0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
# # command_msg = VehicleCommand() # Populate with relevant data
servos_msg = ActuatorServos() # Populate with relevant data
servos_msg.control = [1.0, 1.0, 1.0, 0.0, 0.0, 0.0,
0.0, 0.0]
# control_mode_msg = OffboardControlMode() # Populate with relevant data
# self.publish_actuator_motors(motors_msg)
# node.publish_vehicle_command(command_msg)
self.publish_actuator_servos(servos_msg)
# node.publish_offboard_control_mode(control_mode_msg)
def main(args=None):
rclpy.init(args=args)
node = ROS2PX4Interface()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
The STIL was made by the following CLI commands:
cd PX4-Autopilot
make px4_sitl gz_standard_vtol