FastRTPS ROS2 Foxy Vehicle Visual Odometry Advertiser

ROS / MAVROS / MicroDDS
1

Hi all!

After a while I feel kind of lonely in the journey of trying to fly a drone using a MOCAP system. with only using fastRTPS and ROS2 Foxy. Currently I can publish and subscribe to the real PX4 over UART as demonstrated in this: https://github.com/PX4/px4_ros_com/issues/38 which I got it to work.

I can listen to sensor_combined and vehicle_odometry. Also I can advertise the debug_vect and confirm that it receives by using the listener of uorb topics on the px4 through the Mavlink shell.

Currently I am trying to send a simple pose to the px4 using vehicle_visual_odometry (VehicleVisualOdometry_PubSupTopic) since vehicle_mocap_odometry (VehicleMocapOdometry_PubSubTopic) is not working with ekf2 according to documentation here: https://dev.px4.io/master/en/ros/external_position_estimation.html -> " EKF2 only subscribes to vehicle_visual_odometry topics and can hence only process the first two messages (a MoCap system must generate these messages to work with EKF2).".

Also I confirmed vehicle_visual_odometry is being received properly by the px4 using uorb topic listener.

When I evaluate vehicle_local_position nothing is happening with the given input. My EKF_AID_MASK= 24 (https://docs.px4.io/master/en/advanced_config/tuning_the_ecl_ekf.html)

My questions:

  1. How to sync the timestamps properly?
  2. How do I make the px4 blindly use the MOCAP/OptiTrack system, since its very accurate (ofcourse EKF’ing with IMU-data would be optimal)

Please help, I feel this topic under PX4 software hasn’t been used a lot and I am trying to make it work with very little support. Since ROS2 will be a big part in the future of PX4 I think its most important that we can help each other to solve this problem for the best of both great developers!

Best regards,
Edwin van Emmerik

1 Like
2

When I evaluate vehicle_local_position nothing is happening with the given input.

Can you please elaborate on this?

How to sync the timestamps properly?

What problems are you facing?

3

I evaluate vehicle_local_position using listener vehicle_local_position in the nsh>. When I do so while using my vehicle_visual_odometry-advertiser. It seems as if the EKF2 is not incorporating the received topic (vehicle_visual_odometry) in its position estimation:

nsh> listener vehicle_local_position

TOPIC: vehicle_local_position
 vehicle_local_position_s
	timestamp: 8629105506  (0.006233 seconds ago)
	timestamp_sample: 8629104607  (899 us before timestamp)
	ref_timestamp: 0
	ref_lat: 0.000000
	ref_lon: 0.000000
	x: 0.0000
	y: 0.0000
	z: -0.0001
	delta_xy: [7.2473, 7.7104]
	delta_z: -4.3229
	vx: 0.0098
	vy: 0.0006
	vz: 0.0002
	z_deriv: 0.0008
	delta_vxy: [0.7831, 0.8399]
	delta_vz: 0.0103
	ax: 0.0138
	ay: -0.0036
	az: 0.0129
	heading: 2.5372
	delta_heading: -0.0003
	ref_alt: 0.0000
	dist_bottom: 0.1001
	eph: 0.3119
	epv: 0.0170
	evh: 0.1773
	evv: 0.0340
	vxy_max: inf
	vz_max: inf
	hagl_min: inf
	hagl_max: inf
	xy_valid: False
	z_valid: True
	v_xy_valid: False
	v_z_valid: True
	xy_reset_counter: 4
	z_reset_counter: 1
	vxy_reset_counter: 3
	vz_reset_counter: 1
	heading_reset_counter: 2
	xy_global: False
	z_global: False
	dist_bottom_valid: True

I am facing the problem that my timestamp is correct however my timestep sample is far off, the output I am receiving is this:
nsh> listener vehicle_visual_odometry

TOPIC: vehicle_visual_odometry
 vehicle_odometry_s
	timestamp: 8687979394  (0.074752 seconds ago)
	timestamp_sample: 9251740194  (18446744073145790816 us before timestamp)
	x: 0.0010
	y: 0.0000
	z: 0.0000
	q: [1.0000, 0.0000, 0.0000, 0.0000]  (Roll: 0.0 deg, Pitch: -0.0 deg, Yaw: 0.0 deg)
	q_offset: [0.0000, 0.0000, 0.0000, 0.0000]  (Roll: 0.0 deg, Pitch: -0.0 deg, Yaw: 0.0 deg)
	pose_covariance: [0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
	vx: 0.0000
	vy: 0.0000
	vz: 0.0000
	rollspeed: 0.0000
	pitchspeed: 0.0000
	yawspeed: 0.0000
	velocity_covariance: [0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
	local_frame: 0
	velocity_frame: 0
4

What you are seeing on the timestamp is that the timestamp after sync is resulting in a timestamp higher than the timestamp on the FMU. Which might result of some skew or data drops on your link. How do you have the bridge setup? By serial? What baud rates? Have you tried to remove unneeded topics from the bridge?

5

Lets start with first things first: To what value need the timestamp_sample be set to? Currently I obtain it by listening to “timestamp” value of the Timesync_PubSubTopic I am subscribed to. Meaning. I set both timestamp and timestamp_sample the same (since setting it to timestamp_sample=1 didn’t give me proper results either).

Bridge setup:
A: RPI is running mavlink shell. I start micrortps client by using:
nsh> micrortps_client start -t UART -d /dev/ttyS2 -b 460800

B: On RPI in another terminal I run in my ~/px4_ros_com_ros2 (workspace):
$ micrortps_agent -t UART -d /dev/ttyUSB0 -b 460800

C: On PC I run my own script:
$ ros2 launch px4_ros_com vehicle_mocap_odometry_advertiser.launch.py
which contains:

#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include <px4_msgs/msg/debug_vect.hpp>
#include <px4_msgs/msg/vehicle_mocap_odometry.hpp>
#include <px4_msgs/msg/vehicle_visual_odometry.hpp>
#include <px4_msgs/msg/timesync.hpp>

using namespace std::chrono_literals;

class VehicleMocapOdometryAdvertiser : public rclcpp::Node
{
public:
	VehicleMocapOdometryAdvertiser() : Node("vehicle_mocap_odometry_advertiser") {
#ifdef ROS_DEFAULT_API
		publisher_ = this->create_publisher<px4_msgs::msg::VehicleVisualOdometry>("VehicleVisualOdometry_PubSubTopic", 10);
#else
		publisher_ = this->create_publisher<px4_msgs::msg::VehicleVisualOdometry>("VehicleVisualOdometry_PubSubTopic");
#endif
		auto timer_callback =
		[this]()->void {
			auto vehicle_mocap_odometry = px4_msgs::msg::VehicleVisualOdometry();
			vehicle_mocap_odometry.timestamp = timesync_.timestamp; //std::chrono::time_point_cast<std::chrono::microseconds>(std::chrono::steady_clock::now()).time_since_epoch().count();
			vehicle_mocap_odometry.timestamp_sample = timesync_.timestamp;

			vehicle_mocap_odometry.local_frame = 0;
			vehicle_mocap_odometry.x = 5.0;
			vehicle_mocap_odometry.y = 0.0;
			vehicle_mocap_odometry.z = -1.0;
			vehicle_mocap_odometry.q = {1.0, 0.0, 0.0, 0.0};
			
			this->publisher_->publish(vehicle_mocap_odometry);
			std::cout << "============================="   << std::endl;
			std::cout << "TimeSync timestamp: " << timesync_.timestamp << std::endl;
		};
		
		subscription_ = this->create_subscription<px4_msgs::msg::Timesync>(
			"/Timesync_PubSubTopic", 10,
			[this](const px4_msgs::msg::Timesync::UniquePtr msg) {
			timesync_.timestamp = msg->timestamp;
		});
		std::cout << "============================="   << std::endl;
		std::cout << "TimeSync timestamp: " << timesync_.timestamp << std::endl;
		timer_ = this->create_wall_timer(30ms, timer_callback);
	}

private:
	rclcpp::TimerBase::SharedPtr timer_;
	rclcpp::Publisher<px4_msgs::msg::VehicleVisualOdometry>::SharedPtr publisher_;
	rclcpp::Subscription<px4_msgs::msg::Timesync>::SharedPtr subscription_;
	px4_msgs::msg::Timesync timesync_;
};

int main(int argc, char *argv[])
{
	std::cout << "Starting vehicle_mocap_odometry advertiser node..." << std::endl;
	setvbuf(stdout, NULL, _IONBF, BUFSIZ);
	rclcpp::init(argc, argv);
	rclcpp::spin(std::make_shared<VehicleMocapOdometryAdvertiser>());

	rclcpp::shutdown();
	return 0;
}
6

Try to increase the baud rate. Also activate the hardware flow control if possible. Also remove the topics send/rcv you don’t need in uorb_rtps_message_ids.yaml.

7

I increased the baud rate to 921600 and activated HW flow control. Not yet removed the topics I am not using (I suppose you mean on the RPI at the workspace directory of the micrortps_agent and not at the firmware / micrortps_client side). However I still have two questions:

  1. Should the MOCAP system subscribe to the PX4 Timesync topic or publish to it?
  2. Ideally, to what value should the timestamp_sample variable be set to?

EDIT:
I subscribed to the VehicleOdometry message. Simply copied the timestamp and timestamp_sample to a private variable of the class that publishes the VehicleVisualOdometry. It is working now, however with a work around. Would be happy to hear your opinion about it since I hope it will just be a temporary fix.
NOTE: When the rpi ubuntu server 20.04 is not used for a certain amount of time it hibernates and this causes a mismatch in timestamp/sample (even when I copy the timestamp/sample from the vehicle_odometry message). Therefore when it hibernates I have to reboot the rpi. Looking for a work-around where the rpi ubuntu server doesn’t hibernate anymore.

Thank you for your help so far. I am enjoying the work you deliver for PX4.

1 Like
8

I can subscribe to VehicleOdometry and it indeed publish data to my ros2 subscriber.

However, my pixhawk publish nothing related to “vehicle_local_positon” to my ros2 subscriber. It published nothing even checking it by using “ros2 topic echo /fmu/vehicle_local_position”.

Both “VehicleOdometry” and “VehicleLocaPosition” are working properly in the nsh terminal using “listener vehicle_local_position” and “listener vehicle_odometry”.

Did your ros2 subscribe to the “vehicle_local_position” topic properly? I think we met the same problem.