Directly passtrough commands from ROS2 without PX4 mixer

florian.pix · August 22, 2022, 8:11am

----------------- see answer below ----------------------------

florian.pix · August 22, 2022, 8:13am

If you follow these steps you should be able in the end to publish to a ROS2 topic to directly send commands over the CAN bus or set a specifc PWM on the MAIN and AUX outputs.

set SYS_CTRL_ALLOC 1
edit or create a rc. file in ROMFS/init.d
change parameters to your use case

Here’s an example for my use case where I had ESCs connected via CAN and some LEDs on PWM AUX. When you set FUNC1 to 101 that means the first (FUNC1) motor will be controlled by the topic actuator_motors i.e. actuator_motors.control[1] (101). See the parameter doc:

#!/bin/sh
#
# auv parameters.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#

set VEHICLE_TYPE auv

# MAV_TYPE_SUBMARINE 12
param set-default MAV_TYPE 12

param set-default SYS_CTRL_ALLOC 1

# This is the aux pass mixer that isn't used anymore as we directly publish to actuator_servos
set MIXER pass

param set-default PWM_MAIN_DIS1 900
param set-default PWM_MAIN_DIS2 900
param set-default PWM_MAIN_DIS3 900
param set-default PWM_MAIN_DIS4 900
param set-default PWM_MAIN_DIS5 900
param set-default PWM_MAIN_DIS6 900
param set-default PWM_MAIN_DIS7 900
param set-default PWM_MAIN_DIS8 900
param set-default PWM_MAIN_DISARM 900

param set-default PWM_MAIN_FAIL1 900
param set-default PWM_MAIN_FAIL2 900
param set-default PWM_MAIN_FAIL3 900
param set-default PWM_MAIN_FAIL4 900
param set-default PWM_MAIN_FAIL5 900
param set-default PWM_MAIN_FAIL6 900
param set-default PWM_MAIN_FAIL7 900
param set-default PWM_MAIN_FAIL8 900

param set-default PWM_MAIN_MAX 1900
param set-default PWM_MAIN_MAX1 1900
param set-default PWM_MAIN_MAX2 1900
param set-default PWM_MAIN_MAX3 1900
param set-default PWM_MAIN_MAX4 1900
param set-default PWM_MAIN_MAX5 1900
param set-default PWM_MAIN_MAX6 1900
param set-default PWM_MAIN_MAX7 1900
param set-default PWM_MAIN_MAX8 1900

param set-default PWM_MAIN_MIN 1100
param set-default PWM_MAIN_MIN1 1100
param set-default PWM_MAIN_MIN2 1100
param set-default PWM_MAIN_MIN3 1100
param set-default PWM_MAIN_MIN4 1100
param set-default PWM_MAIN_MIN5 1100
param set-default PWM_MAIN_MIN6 1100
param set-default PWM_MAIN_MIN7 1100
param set-default PWM_MAIN_MIN8 1100

# not the actual PWM pulse length but just a rate that is dependent on the ESC; our ESCs and most others use rate 50
param set-default PWM_MAIN_RATE 50

# we don't want to reverse the outputs so we set these to 0
param set-default PWM_MAIN_REV1 0
param set-default PWM_MAIN_REV2 0
param set-default PWM_MAIN_REV3 0
param set-default PWM_MAIN_REV4 0
param set-default PWM_MAIN_REV5 0
param set-default PWM_MAIN_REV6 0
param set-default PWM_MAIN_REV7 0
param set-default PWM_MAIN_REV8 0

# offset to normalized input (actuator_motors) could be useful in the future when we see that the motors are too different
param set-default PWM_MAIN_TRIM1 0.0000
param set-default PWM_MAIN_TRIM2 0.0000
param set-default PWM_MAIN_TRIM3 0.0000
param set-default PWM_MAIN_TRIM4 0.0000
param set-default PWM_MAIN_TRIM5 0.0000
param set-default PWM_MAIN_TRIM6 0.0000
param set-default PWM_MAIN_TRIM7 0.0000
param set-default PWM_MAIN_TRIM8 0.0000

param set-default PWM_MAIN_FUNC1 0
param set-default PWM_MAIN_FUNC2 0
param set-default PWM_MAIN_FUNC3 0
param set-default PWM_MAIN_FUNC4 0
param set-default PWM_MAIN_FUNC5 0
param set-default PWM_MAIN_FUNC6 0
param set-default PWM_MAIN_FUNC7 0
param set-default PWM_MAIN_FUNC8 0

# This is the aux pass mixer that isn't used anymore as we directly publish to actuator_servos
set MIXER_AUX pass

# LED start to shine at 1200
param set-default PWM_AUX_DISARM 900
param set-default PWM_AUX_DIS1 900
param set-default PWM_AUX_DIS2 900
param set-default PWM_AUX_DIS3 900
param set-default PWM_AUX_DIS4 900
param set-default PWM_AUX_DIS5 900
param set-default PWM_AUX_DIS6 900
param set-default PWM_AUX_DIS7 900
param set-default PWM_AUX_DIS8 900

param set-default PWM_AUX_FAIL1 900
param set-default PWM_AUX_FAIL2 900
param set-default PWM_AUX_FAIL3 900
param set-default PWM_AUX_FAIL4 900
param set-default PWM_AUX_FAIL5 900
param set-default PWM_AUX_FAIL6 900
param set-default PWM_AUX_FAIL7 900
param set-default PWM_AUX_FAIL8 900

param set-default PWM_AUX_MAX 1900
param set-default PWM_AUX_MAX1 1900
param set-default PWM_AUX_MAX2 1900
param set-default PWM_AUX_MAX3 1900
param set-default PWM_AUX_MAX4 1900
param set-default PWM_AUX_MAX5 1900
param set-default PWM_AUX_MAX6 1900
param set-default PWM_AUX_MAX7 1900
param set-default PWM_AUX_MAX8 1900

param set-default PWM_AUX_MIN 1100
param set-default PWM_AUX_MIN1 1100
param set-default PWM_AUX_MIN2 1100
param set-default PWM_AUX_MIN3 1100
param set-default PWM_AUX_MIN4 1100
param set-default PWM_AUX_MIN5 1100
param set-default PWM_AUX_MIN6 1100
param set-default PWM_AUX_MIN7 1100
param set-default PWM_AUX_MIN8 1100

# activate channels 1, 2, 3, 4, 5, 6, 7, 8
# if you want to deactivate some channels use 1234 for 1, 2, 3, 4 or 1458 for 1, 4, 5, 8
param set-default PWM_AUX_OUT 12345678
set PWM_AUX_OUT 12345678

# not the actual PWM pulse length but just a rate that is dependent on the ESC; our ESCs and most others use rate 50
param set-default PWM_AUX_RATE 50

# we don't want to reverse the outputs so we set these to 0
param set-default PWM_AUX_REV1 0
param set-default PWM_AUX_REV2 0
param set-default PWM_AUX_REV3 0
param set-default PWM_AUX_REV4 0
param set-default PWM_AUX_REV5 0
param set-default PWM_AUX_REV6 0
param set-default PWM_AUX_REV7 0
param set-default PWM_AUX_REV8 0

# offset to normalized input (actuator_servos) could be useful in the future when we see that the actuators are too different
param set-default PWM_AUX_TRIM1 0.0000
param set-default PWM_AUX_TRIM2 0.0000
param set-default PWM_AUX_TRIM3 0.0000
param set-default PWM_AUX_TRIM4 0.0000
param set-default PWM_AUX_TRIM5 0.0000
param set-default PWM_AUX_TRIM6 0.0000
param set-default PWM_AUX_TRIM7 0.0000
param set-default PWM_AUX_TRIM8 0.0000

param set-default PWM_AUX_FUNC1 201
param set-default PWM_AUX_FUNC2 202
param set-default PWM_AUX_FUNC3 203
param set-default PWM_AUX_FUNC4 204
param set-default PWM_AUX_FUNC5 205
param set-default PWM_AUX_FUNC6 206
param set-default PWM_AUX_FUNC7 207
param set-default PWM_AUX_FUNC8 208

# UAVCAN

param set-default UAVCAN_EC_DIS1 4096
param set-default UAVCAN_EC_DIS2 4096
param set-default UAVCAN_EC_DIS3 4096
param set-default UAVCAN_EC_DIS4 4096
param set-default UAVCAN_EC_DIS5 4096
param set-default UAVCAN_EC_DIS6 4096
param set-default UAVCAN_EC_DIS7 4096
param set-default UAVCAN_EC_DIS8 4096

param set-default UAVCAN_EC_FAIL1 4096
param set-default UAVCAN_EC_FAIL2 4096
param set-default UAVCAN_EC_FAIL3 4096
param set-default UAVCAN_EC_FAIL4 4096
param set-default UAVCAN_EC_FAIL5 4096
param set-default UAVCAN_EC_FAIL6 4096
param set-default UAVCAN_EC_FAIL7 4096
param set-default UAVCAN_EC_FAIL8 4096

param set-default UAVCAN_EC_FUNC1 101
param set-default UAVCAN_EC_FUNC2 102
param set-default UAVCAN_EC_FUNC3 103
param set-default UAVCAN_EC_FUNC4 104
param set-default UAVCAN_EC_FUNC5 105
param set-default UAVCAN_EC_FUNC6 106
param set-default UAVCAN_EC_FUNC7 107
param set-default UAVCAN_EC_FUNC8 108

param set-default UAVCAN_EC_MAX1 8191
param set-default UAVCAN_EC_MAX2 8191
param set-default UAVCAN_EC_MAX3 8191
param set-default UAVCAN_EC_MAX4 8191
param set-default UAVCAN_EC_MAX5 8191
param set-default UAVCAN_EC_MAX6 8191
param set-default UAVCAN_EC_MAX7 8191
param set-default UAVCAN_EC_MAX8 8191

param set-default UAVCAN_EC_MIN1 0
param set-default UAVCAN_EC_MIN2 0
param set-default UAVCAN_EC_MIN3 0
param set-default UAVCAN_EC_MIN4 0
param set-default UAVCAN_EC_MIN5 0
param set-default UAVCAN_EC_MIN6 0
param set-default UAVCAN_EC_MIN7 0
param set-default UAVCAN_EC_MIN8 0

# allow all motors to spin backwards as well as forwards
param set-default UAVCAN_EC_REV 0

param set-default UAVCAN_SV_FAIL1 -1
param set-default UAVCAN_SV_FAIL2 -1
param set-default UAVCAN_SV_FAIL3 -1
param set-default UAVCAN_SV_FAIL4 -1
param set-default UAVCAN_SV_FAIL5 -1
param set-default UAVCAN_SV_FAIL6 -1
param set-default UAVCAN_SV_FAIL7 -1
param set-default UAVCAN_SV_FAIL8 -1

param set-default UAVCAN_SV_FUNC1 0
param set-default UAVCAN_SV_FUNC2 0
param set-default UAVCAN_SV_FUNC3 0
param set-default UAVCAN_SV_FUNC4 0
param set-default UAVCAN_SV_FUNC5 0
param set-default UAVCAN_SV_FUNC6 0
param set-default UAVCAN_SV_FUNC7 0
param set-default UAVCAN_SV_FUNC8 0

param set-default UAVCAN_SV_REV 0

add the topics actuator_motors and actuator_servos to the microRTPS bridge:

4.1. add
` - msg: actuator_servos
receive: true

msg: actuator_motors
receive: true
to the end of
PX4-Autopilot/msg/tools/urtps_bridge_topics.yaml

4.2 add

  - msg: ActuatorServos
    receive: true
  - msg: ActuatorMotors
    receive: true

to the end of
px4_ros_com/templates/urtps_bridge_topics.yaml

4.3. cleanly make the px4 firmware
4.4. rebuild px4_roscom

start the bridge

5.1. if not already running on PX4 side: micrortps_client start
(if you are using UART like me, add other arguments if using UDP (e.g. in simulation))

5.2. on PC micrortps_agent
(sometimes when you disconnect the pixhawk it might change device; so sometime you need to add -d /dev/ttyACM1 or 2 or ...)

either directly publish to this px4_msg type from a ROS2 node or write an adapter that listens to ros type messages and publishes them as px4 type
Look at the example provided in px4_ros_com for debug_vect_advertiser.cpp
run the publisher; you can check if it is working by running listener actuator_motors/listener actuator_servos on the PX4 side. Keep in mind that you still need to arm to enable outputs so run commander arm to observe actual changes to uavcan status and pwm_out status

microRTPS
ROS2
PX4
pixhawk
passtrough
actuator_motors
actuator_servos
pwm_out
uavcan

maxinho · September 3, 2022, 4:27pm

Thx a lot for the detailed explanation.

maxinho · September 5, 2022, 5:25pm

I configured my vehicle as a modified mc:

set VEHICLE_TYPE mc
param set-default MAV_TYPE 2
set MIXER_AUX pass
set MIXER pass
param set-default PWM_AUX_FUNC1 101
param set-default PWM_AUX_FUNC2 102

I disabled the mc_rate_control, mc_pos_control, mc_att_control modules but control_allocator and flight_mode_manager get started.

I can send commands to actuator_motors topic from my offboard node and they get applied. Though, something in the system sends 0 commands to the topic in a regular interval resulting in stuttering of the motors, i.e. my command gets applied and then a 0 command from somewhere else gets applied.

Do you have any idea which module publishes this 0 command? Also: The whole system return to disarmed automatically after 10 seconds or so.

maxinho · September 6, 2022, 4:03pm

Okay, here is a quick write up of todays finding.

Result: I can control the motors of my custom airframe smoothly by sending commands from a ROS2 node on the companion computer. The commands are send directly to the actuator_motors message.

Disclaimer: I am a newbie in the PX4 stack and have somehow hacked my way through control_allocation, airframe setup, offboard control.

Create custom airframe
I have created a custom airframe ( or rather modified the 50000_generic_ground_vehicle).

50000_generic_ground_vehicle

#!/bin/sh
# @name Generic Ground Vehicle (Differential)
# @type Rover
# @class Rover
# @output Motor1 left
# @output Motor2 right
# @maintainer
# @board bitcraze_crazyflie exclude

. ${R}etc/init.d/rc.rover_defaults

param set-default BAT1_N_CELLS 6
param set-default CA_AIRFRAME 6 # Differential
param set-default PWM_AUX_FUNC2 101
param set-default PWM_AUX_FUNC3 102

When selecting the rover airframe in Qgroundcontrol, this will start rc.rover_apps. In rc.rover_apps, I disabled the “rover_pos_control start”. Thus, only ekf2, control_allocator, and land_detector rover are started.
As described above I added actuator_motors to synced topics
In my offboard publisher, I first publish a bunch of empty ActuatorMotors commands, then the arm command and the switch_to_offboard mode command. Then, I have a timer loop where I simply publish ActuatorMotors with my setpoints.

TLDR; I think the solution is to not start any modules, like mc_rate_control, mc_autotune_attitude_control, flight_mode_manager, mc_pos_control, etc. You can check whether those modules are running in the debug console.

I’d be glad to hear what made it work for you.