Ros2 control servo via xrce agent/client

Hello, i would love to be able to control and servo via ros 2

I have ros2 on a raspbery pi with a v6x connected via ethernet, i can see the topics and echos the data of each topics.

I need to control the servo by ros… How to do that ?

I see the topics /fmu/out/actuator_outputs that publish the pwm ok each servo and the topics /fmu/out/actuator_servos that publish servo position -1 to 1.

So how to publish a position of the servo by ros ?
Should i put px4 in off board control mode ?
Should i create /fmu/in/actuator_servos ?

thanks for your help !

Hi @Olivier_Olivier ,
Could this work for you? Payloads and Cameras | PX4 User Guide
You configure the proper output and then you send the appropriate command. The guide tells you how to do that with mavlink, but each mavlink command is associated with a VehicleCommand message that you can send from ROS 2, you don’t even have to add topics to the bridge.
Look into the mavlink message specification or into the PX4 source code to understand how to format the VehicleCommand message

hello and thanks you for reply !

It can work but my servo L and my servo R are affected to eleverons, (I chose the fliying wing airframe) , so with this ros can interact with px4 with a higher level, not just controlling servo, I need this servo can also be controlled by my rc transmitter. If I put “Offboard Actuator Set” and command them with ros, they can be used by px4 to control the flight.

My goal is to control a parachute with 2 servo but I want to pilot it also by RC. The problem with flying wing airframe is the two servo move together ( one goes up and the other down) for the roll , I just want the right servo move to turn right and left for left. I think I have to change the mixing parameter of this airframe ? But I can’t find the file and how to change this .
If I can do that, ros will can control the parachute with an higher level , for exemple , publish on attitude set point , or perhaps on rc override, or just go to a waypoint, and px4 will output the correct value for each servo.
My question is with how to move servos with ros via xrce , I have read I can do that with the topic /actuactor_servos, but I need to put a flag to stop the commander publishing value in this topic, and ros can publish this value.

When I do

ros2 topic echo /fmu/out/actuator_servos

I can see value of the position off servo -1 to 1 for each servo.

Can ros publish directly this value in place off px4, how to do that if its possible ?

Thanks

Hi @Olivier_Olivier , sorry for the late reply. At the moment there is no immediate way to use offboard mode to directly control motors (actuator_motors) and servos (actuator_servos): as you said you need to prevent other modules (mainly the control allocation module) for publishing on those topics.

Hello Benja and thanks for repply !

Ok I understant now ! But i have read this :

Blockquote ###bkueng commented Feb 14, 2023

I haven’t thought it through completely but along the lines:

So this is wrong ? For now no possibility to do this ? I guess is also impossible ROS control servo by PWM with actuator_outputs ?

I think, now i have made a correct servo mixing so now i try control my servo with vehicle_torque_setpoint.
0 to -1 the left sevo is actionned from 0 to 2 turn and 0 to 1 the right servo move 0 to 2 turns. It work well with my rc transmitter the stick do exactly the servo movement I want !

I have made a python node who publish in /fmu/in/vehicle_torque_setpoint.
I put the pixhawk in offboard mode and i send commands but servo don’t move :frowning:
Where i’m wrong ?
Should i publish directly on /fmu/out/vehicle_torque_setpoint ?

This node subscribe to a px4 a topic to have the px4 timestamp and use it to timestamp the command i send. Is it the good way to do this ?

Here is the output of my node :

timestamp: 1677770483288374
timestamp_sample: 1677770483288374
xyz:
- 0.20000000298023224
- 0.6000000238418579
- 0.0
---
timestamp: 1677770483386081
timestamp_sample: 1677770483386081
xyz:
- 0.20000000298023224
- 0.6000000238418579
- 0.0
---
timestamp: 1677770483488759
timestamp_sample: 1677770483488759
xyz:
- 0.0
- 0.0
- 0.0
---

It look like the original /fmu/out/vehicle_torque_setpoint when i move my rc stick! So i would love this node move my servo now !

Another question about the timestamp, sometimes i lost some px4 message and the command is published with the same timestamp for 2 or 3 second. This will affect the command?
Px4 will do the action if the timestamp is in the past ?

Also I have really strange problems, sometimes I lost totally the connexion between ROS and px4, the px4 topics disapears without doing anythink !?!
I don’t know why ?! I have to restart the pixhawk, and unplug- plug the ethernet cable form the v6x to the raspbery. The topics reappear after a very long time ! perhaps 2 or 3 minutes before i can see the px4 topics in ros ? Is it normal ? Is xrce agent/client problem ? Is there logs i can see to identify the problem ?
When i do my first test it was really fast, if i reboot my pixhawk, i can see topics in less than 20 s ! Now it take 3 minutes !!

Thaks

That’s exactly the point, the control allocation module must be disabled in order to not have multiple modules publishing on the same topics.

That great! First of all, /fmu/in/vehicle_torque_setpoint is the right topic. Now you need to make sure that PX4 is listening to it, adding /fmu/in/vehicle_torque_setpoint and /fmu/in/vehicle_thrust_setpoint to the list of topics subscribed by the microdds: https://docs.px4.io/main/en/middleware/xrce_dds.html#dds-topics-yaml

Once that is done, you can start going offboard! Offboard Mode | PX4 User Guide (main)

You can just use the ROS 2 time, the bridge will synchronize everything : ROS 2 Offboard Control Example | PX4 User Guide (main)

Are you using ETH? If so, try using the static IP setup: PX4 Ethernet Setup | PX4 User Guide (main) BOOTPROTO = static

Hello Benja,

Blockquote That great! First of all, /fmu/in/vehicle_torque_setpoint is the right topic. Now you need to make sure that PX4 is listening to it, adding /fmu/in/vehicle_torque_setpoint and /fmu/in/vehicle_thrust_setpoint to the list of topics subscribed by the microdds: XRCE-DDS (PX4-FastDDS Bridge) | PX4 User Guide

I have already done this : here is the result of ros2 topic list :

o.quirant@station-sol:~$ ros2 topic list
/fmu/in/actuator_outputs
/fmu/in/actuator_servos
/fmu/in/actuator_servos_trim
/fmu/in/led_control
/fmu/in/obstacle_distance
/fmu/in/offboard_control_mode
/fmu/in/onboard_computer_status
/fmu/in/sensor_optical_flow
/fmu/in/telemetry_status
/fmu/in/trajectory_setpoint
/fmu/in/vehicle_attitude_setpoint
/fmu/in/vehicle_command
/fmu/in/vehicle_global_position
/fmu/in/vehicle_local_position
/fmu/in/vehicle_mocap_odometry
/fmu/in/vehicle_rates_setpoint
/fmu/in/vehicle_trajectory_bezier
/fmu/in/vehicle_trajectory_waypoint
/fmu/in/vehicle_visual_odometry
/fmu/out/actuator_outputs
/fmu/out/actuator_servos
/fmu/out/actuator_servos_trim
/fmu/out/failsafe_flags
/fmu/out/failure_detector_status
/fmu/out/manual_control_setpoint
/fmu/out/sensor_accel
/fmu/out/sensor_baro
/fmu/out/sensor_combined
/fmu/out/sensor_gps
/fmu/out/sensor_gyro
/fmu/out/sensor_mag
/fmu/out/timesync_status
/fmu/out/vehicle_acceleration
/fmu/out/vehicle_air_data
/fmu/out/vehicle_angular_velocity
/fmu/out/vehicle_attitude
/fmu/out/vehicle_control_mode
/fmu/out/vehicle_gps_position
/fmu/out/vehicle_imu
/fmu/out/vehicle_land_detected
/fmu/out/vehicle_local_position
/fmu/out/vehicle_odometry
/fmu/out/vehicle_status
/fmu/out/vehicle_thrust_setpoint
/fmu/out/vehicle_torque_setpoint
/parameter_events
/rosout
o.quirant@station-sol:~$ 

when i launch a node who publish value on /fmu/in/vehicle_torque_setpoint, i can see the value mooving, here is the résult :

type or paste code heretimestamp: 1680613412828839
timestamp_sample: 1680613412828839
xyz:
- -0.20000000298023224
- 0.4000000059604645
- 0.0
---
timestamp: 1680613412828839
timestamp_sample: 1680613412828839
xyz:
- -0.20000000298023224
- 0.4000000059604645
- 0.0
---
timestamp: 1680613412828839
timestamp_sample: 1680613412828839
xyz:
- -0.20000000298023224
- 0.4000000059604645
- 0.0
---
timestamp: 1680613412828839
timestamp_sample: 1680613412828839
xyz:
- 0.0
- 0.30000001192092896
- 0.0
---

here is the output off /fmu/out/vehicle_control_mode, i can see offboard is true

timestamp: 1680613389062354
flag_armed: false
flag_multicopter_position_control_enabled: false
flag_control_manual_enabled: false
flag_control_auto_enabled: false
flag_control_offboard_enabled: true
flag_control_rates_enabled: false
flag_control_attitude_enabled: false
flag_control_acceleration_enabled: false
flag_control_velocity_enabled: false
flag_control_position_enabled: false
flag_control_altitude_enabled: false
flag_control_climb_rate_enabled: false
flag_control_termination_enabled: false
---

But when i try to see /fmu/out/vehicle_torque_setpoint, the topic is not published and is see nothing :frowning: perhaps is because i’m in offboard mode ? when i change to manual, i can see the topic /fmu/out/vehicle_torque_setpoint and i see the value is changing when i moove the rc stick!

Where i’m wrong ? i have my pixhawk in offboard mode, i publish correct value with correct timestamp on /fmu/in/vehicle_torque_setpoint, but servo don’t moove

Hi @Olivier_Olivier , no, I don’t see

/fmu/in/vehicle_thrust_setpoint
/fmu/in/vehicle_torque_setpoint

I see

/fmu/out/vehicle_thrust_setpoint
/fmu/out/vehicle_torque_setpoint

(out instead of in).
How did you adjusted your dds_topics.yaml?

/fmu/out/vehicle_torque_setpoint is the topic that is echoing the UORB topic vehicle_torque_setpoint from PX4 to ROS 2. But it is unidirectional! If you publish something there it will not be echoed back to PX4. To do so you need to create a subscriber in dds_topics.yaml

Sory it’s not the good topic list … i don’t know why but /fmu/out/vehicle_global_position disapear, so i try different yalm file and build the firmaware to make /fmu/out/vehicle_global_position come back , but impossible, so strange !!

here is my dds_topics.yalm

#####
#
# This file maps all the topics that are to be used on the micro DDS client.
#
#####
publications:

 

  - topic: /fmu/out/collision_constraints
    type: px4_msgs::msg::CollisionConstraints
    
  - topic: /fmu/out/actuator_outputs
    type: px4_msgs::msg::ActuatorOutputs

  - topic: /fmu/out/actuator_servos
    type: px4_msgs::msg::ActuatorServos
   
  - topic: /fmu/out/actuator_servos_trim
    type: px4_msgs::msg::ActuatorServosTrim
    
  - topic: /fmu/out/airspeed
    type: px4_msgs::msg::Airspeed
    
  - topic: /fmu/out/cellular_status
    type: px4_msgs::msg::CellularStatus
      
  - topic: /fmu/out/collision_report
    type: px4_msgs::msg::CollisionReport
      
  - topic: /fmu/out/differential_pressure
    type: px4_msgs::msg::DifferentialPressure
    
  - topic: /fmu/out/distance_sensor
    type: px4_msgs::msg::DistanceSensor
    
  - topic: /fmu/out/failure_detector_status
    type: px4_msgs::msg::FailureDetectorStatus
    
  - topic: /fmu/out/follow_target
    type: px4_msgs::msg::FollowTarget

  - topic: /fmu/out/follow_target_estimator
    type: px4_msgs::msg::FollowTargetEstimator
    
  - topic: /fmu/out/follow_target_status
    type: px4_msgs::msg::FollowTargetStatus
    
  - topic: /fmu/out/position_controller_status
    type: px4_msgs::msg::PositionControllerStatus
    
  - topic: /fmu/out/position_controller_landing_status
    type: px4_msgs::msg::PositionControllerLandingStatus
    
  - topic: /fmu/out/position_setpoint
    type: px4_msgs::msg::PositionSetpoint
  
  - topic: /fmu/out/sensor_accel
    type: px4_msgs::msg::SensorAccel
    
  - topic: /fmu/out/sensor_baro
    type: px4_msgs::msg::SensorBaro
    
  - topic: /fmu/out/sensor_gps
    type: px4_msgs::msg::SensorGps
    
  - topic: /fmu/out/sensor_gyro
    type: px4_msgs::msg::SensorGyro

  - topic: /fmu/out/sensor_hygrometer
    type: px4_msgs::msg::SensorHygrometer
  
  - topic: /fmu/out/wind
    type: px4_msgs::msg::Wind  
    
  - topic: /fmu/out/sensor_mag
    type: px4_msgs::msg::SensorMag
    
  - topic: /fmu/out/sensor_optical_flow
    type: px4_msgs::msg::SensorOpticalFlow
    
  - topic: /fmu/out/tecs_status
    type: px4_msgs::msg::TecsStatus
    
  - topic: /fmu/out/yaw_estimator_status
    type: px4_msgs::msg::YawEstimatorStatus
    
  - topic: /fmu/out/landing_target_pose
    type: px4_msgs::msg::LandingTargetPose
    
  - topic: /fmu/out/launch_detection_status
    type: px4_msgs::msg::LaunchDetectionStatus
    
  - topic: /fmu/out/manual_control_setpoint
    type: px4_msgs::msg::ManualControlSetpoint
    
  - topic: /fmu/out/manual_control_switches
    type: px4_msgs::msg::ManualControlSwitches
    
  - topic: /fmu/out/obstacle_distance
    type: px4_msgs::msg::ObstacleDistance
    
  - topic: /fmu/out/onboard_computer_status
    type: px4_msgs::msg::OnboardComputerStatus
    
  - topic: /fmu/out/vehicle_air_data
    type: px4_msgs::msg::VehicleAirData
    
  - topic: /fmu/out/vehicle_acceleration
    type: px4_msgs::msg::VehicleAcceleration
  
  - topic: /fmu/out/vehicle_imu
    type: px4_msgs::msg::VehicleImu
    
  - topic: /fmu/out/vehicle_land_detected
    type: px4_msgs::msg::VehicleLandDetected
    
  - topic: /fmu/out/vehicle_optical_flow_vel
    type: px4_msgs::msg::VehicleOpticalFlow

  - topic: /fmu/out/vehicle_optical_flow
    type: px4_msgs::msg::VehicleOpticalFlowVel
 
  - topic: /fmu/out/vehicle_roi
    type: px4_msgs::msg::VehicleRoi
    
 
  - topic: /fmu/out/failsafe_flags
    type: px4_msgs::msg::FailsafeFlags

  - topic: /fmu/out/sensor_combined
    type: px4_msgs::msg::SensorCombined

  - topic: /fmu/out/timesync_status
    type: px4_msgs::msg::TimesyncStatus

  - topic: /fmu/out/vehicle_angular_velocity
    type: px4_msgs::msg::VehicleAngularVelocity

  - topic: /fmu/out/vehicle_attitude
    type: px4_msgs::msg::vehicleAttitude

  - topic: /fmu/out/vehicle_control_mode
    type: px4_msgs::msg::VehicleControlMode

  - topic: /fmu/out/vehicle_gps_position
    type: px4_msgs::msg::SensorGps

  - topic: /fmu/in/vehicle_global_position
    type: px4_msgs::msg::VehicleGlobalPosition

  - topic: /fmu/out/vehicle_local_position
    type: px4_msgs::msg::VehicleLocalPosition

  - topic: /fmu/out/vehicle_odometry
    type: px4_msgs::msg::VehicleOdometry

  - topic: /fmu/out/vehicle_status
    type: px4_msgs::msg::VehicleStatus

  - topic: /fmu/out/vehicle_trajectory_waypoint_desired
    type: px4_msgs::msg::VehicleTrajectoryWaypoint

 



subscriptions:

  - topic: /fmu/in/offboard_control_mode
    type: px4_msgs::msg::OffboardControlMode

  - topic: /fmu/in/onboard_computer_status
    type: px4_msgs::msg::OnboardComputerStatus

  - topic: /fmu/in/obstacle_distance
    type: px4_msgs::msg::ObstacleDistance

  - topic: /fmu/in/sensor_optical_flow
    type: px4_msgs::msg::SensorOpticalFlow

  - topic: /fmu/in/telemetry_status
    type: px4_msgs::msg::TelemetryStatus

  - topic: /fmu/in/trajectory_setpoint
    type: px4_msgs::msg::TrajectorySetpoint

  - topic: /fmu/in/vehicle_attitude_setpoint
    type: px4_msgs::msg::VehicleAttitudeSetpoint

  - topic: /fmu/in/vehicle_mocap_odometry
    type: px4_msgs::msg::VehicleOdometry

  - topic: /fmu/in/vehicle_rates_setpoint
    type: px4_msgs::msg::VehicleRatesSetpoint

  - topic: /fmu/in/vehicle_visual_odometry
    type: px4_msgs::msg::VehicleOdometry

  - topic: /fmu/in/vehicle_command
    type: px4_msgs::msg::VehicleCommand

  - topic: /fmu/in/vehicle_trajectory_bezier
    type: px4_msgs::msg::VehicleTrajectoryBezier

  - topic: /fmu/in/vehicle_trajectory_waypoint
    type: px4_msgs::msg::VehicleTrajectoryWaypoint

  - topic: /fmu/in/actuator_outputs
    type: px4_msgs::msg::ActuatorOutputs

  - topic: /fmu/in/actuator_servos
    type: px4_msgs::msg::ActuatorServos
   
  - topic: /fmu/in/actuator_servos_trim
    type: px4_msgs::msg::ActuatorServosTrim
    
  - topic: /fmu/in/vehicle_global_position
    type: px4_msgs::msg::VehicleGlobalPosition
  
  - topic: /fmu/in/vehicle_local_position
    type: px4_msgs::msg::VehicleLocalPosition
    
  - topic: /fmu/in/led_control
    type: px4_msgs::msg::LedControl
    
  - topic: /fmu/in/vehicle_thrust_setpoint
    type: px4_msgs::msg::VehicleThrustSetpoint
  
  - topic: /fmu/in/vehicle_torque_setpoint
    type: px4_msgs::msg::VehicleTorqueSetpoint
    
    
    
    


        

the 2 last are the good ones…

and here is my topic list :

zit@rpi:~$ ros2 topic list
/fmu/in/actuator_outputs
/fmu/in/actuator_servos
/fmu/in/actuator_servos_trim
/fmu/in/led_control
/fmu/in/obstacle_distance
/fmu/in/offboard_control_mode
/fmu/in/onboard_computer_status
/fmu/in/sensor_optical_flow
/fmu/in/telemetry_status
/fmu/in/trajectory_setpoint
/fmu/in/vehicle_attitude_setpoint
/fmu/in/vehicle_command
/fmu/in/vehicle_global_position
/fmu/in/vehicle_local_position
/fmu/in/vehicle_mocap_odometry
/fmu/in/vehicle_rates_setpoint
/fmu/in/vehicle_thrust_setpoint
/fmu/in/vehicle_torque_setpoint
/fmu/in/vehicle_trajectory_bezier
/fmu/in/vehicle_trajectory_waypoint
/fmu/in/vehicle_visual_odometry
/fmu/out/actuator_outputs
/fmu/out/actuator_servos
/fmu/out/actuator_servos_trim
/fmu/out/failsafe_flags
/fmu/out/failure_detector_status
/fmu/out/manual_control_setpoint
/fmu/out/sensor_accel
/fmu/out/sensor_baro
/fmu/out/sensor_combined
/fmu/out/sensor_gps
/fmu/out/sensor_gyro
/fmu/out/sensor_mag
/fmu/out/timesync_status
/fmu/out/vehicle_acceleration
/fmu/out/vehicle_air_data
/fmu/out/vehicle_angular_velocity
/fmu/out/vehicle_attitude
/fmu/out/vehicle_control_mode
/fmu/out/vehicle_gps_position
/fmu/out/vehicle_imu
/fmu/out/vehicle_land_detected
/fmu/out/vehicle_local_position
/fmu/out/vehicle_odometry
/fmu/out/vehicle_status
/fmu/out/vehicle_thrust_setpoint
/fmu/out/vehicle_torque_setpoint
/parameter_events
/rosout

My ros node publish a sequence off value (-1 to 1) for roll and pitch on /fmu/in/vehicle_torque_setpoint, when i echo this topic i can see the changes ! But if i echo /fmu/out/vehicle_torque_setpoint the topic in not published, and i see nothing. I can see the topic /fmu/out/vehicle_torque_set point if i’m NOT on offboard mode.
So i’m in offboard mode, i publish on /fmu/in/vehicle_torque_setpoint, but when i echo /fmu/out/actuator_output or /fmu/out/actuator_servos nothing moove :frowning:

Also the /fmu/out/vehicle_global_position disapear since 3 days now and impossible to make it come back ( also impossible with stable, dev, or beta verion from qground control) it’s so strange !

Can you see the topic in PX4 with listener vehicle_torque_setpoint? Let’s narrow it down.

Hoping that you do can see it (which means that the bridge is working), the next step is: are you also publishing /fmu/in/vehicle_thrust_setpoint? They have to be published in pairs.

Do you really need all those topics bridged?

Hi, now it work !!

The problem was:
I was trying to run the ros node on my laptop on the same network than the raspbery , my laptop can see all the topics and echo them… so I was thinking i can publish also, but no … Why I don’t know !
When i run node directly on the raspbery it work perfecly ( whith your precious help !! thank you !! )
But now the topic /fmu/out/vehicle_torque_setpoint disapear ! So is it impossible to subscibe to a “out” topics, and when I pass px4 "offboard mode " publish on the same “in” topic ?

that’s weird, so it is a ros issue. Do you confirm that you can’t echo the topics published by the laptop on the raspberry? (publisher: laptop, ros2 topic echo: raspberry).

No, you should be able to send command in the in topic and subscribe to what is coming back on the out topic

that’s weird, so it is a ros issue. Do you confirm that you can’t echo the topics published by the laptop on the raspberry? (publisher: laptop, ros2 topic echo: raspberry).

I have done some test, here is the résult :

If i launch the python file who publish on /fmu/in/vehicle_torque_setpoint on the laptop, i can echo the topic /fmu/out/vehicle_torque_setpoint on the raspbery and see the value change but servos don’t moove !!

If i launch the node on the raspbery , i can echo /fmu/out/vehicle_torque_setpoint on the laptop see the value change and servos are mooving according to the command.

hello,Have you solved this problem。I want to use ROS2 to directly control the servos and motors, and at the same time, I hope the RC can also control these servo and motor. Do you have any solution? Thank you!
I want to use offboard motion to control actuators directly.I have set (OffboardControlMode().direct_actuator=True),can I use ActuatorServos to control all servos, or I can only control the servos of offboard_actuator_set?

you can controll all servo by ros , before i was using /fmu/in/vehicle_torque_setpoint and i was able to move servo with pitch and roll command, now it seem to don’t work anymore, I don’t know why.
But i think you can control your servos by /fmu/in/actuator_servos topic.

Thank you for you answer.yes,i want to control all servos by /fmu/in/actuator_servos topic,but at offboard mode,can i pulish this topic,i don’t know if this conflicts with control allocation.if it’s feasible,can i realize it by setting offboardmode().direct_actuator=true?or I need to do others?

You don’t need to select direct acutator

if you use actuator inside px4 for exemple , you have your actuators for a delta wigns , in actuator on qground control you select eleveron 1 and eleveron 2, as you servo 1 and servo 2.
You can control independentle by sending a value -1 to1 off each actuator, but if you publish on /fmu/in/vehicle_ torque_setpoint you can pushich pitch for exemple -1 to 1 and px4 will moove the 2 servo to pitch, if you publish on roll px4 moove the 2 servo at the opositite do de roll…
Direct actuator is for “outside px4” use, for exemple a gripper …

thank you for your reply.My problem is that I want to directly control servos and motors by my own ros2 node.Because I want to control wheel and aircfrat of my fixed wing.It seems Ican’t control others servos except roll、pitch、yaw servos by publishing /fmu/in/vehicle_ torque_setpoint topic.

So you can use /fmu/in/actuator_servo to control your others servo…
if the topic not present when you do ros2 topic list , you can add it by editing the yaml file with the message definition i the source then compille px4.

Yes,I don’t find the topic in topc list,waht’t is the name of the yaml file? Is it uorb_rtps_message_ids.yam. there is no uorb_rtps_message_ids.yam in PX4-Autopilot.Thank you very much