do not set timestamp for publications

px4_ros2_cpp/src/components/health_and_arming_checks.cpp

@@ -43,7 +43,7 @@ HealthAndArmingChecks::HealthAndArmingChecks(
 
         _mode_requirements.fillArmingCheckReply(reply);
 
-        reply.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+        reply.timestamp = 0; // Let PX4 set the timestamp
         _arming_check_reply_pub->publish(reply);
         _check_triggered = true;
 

px4_ros2_cpp/src/components/message_compatibility_check.cpp

@@ -269,7 +269,7 @@ bool messageCompatibilityCheck(
       reinterpret_cast<char *>(request.topic_name.data()),
       message_to_check.topic_name.c_str(), request.topic_name.size() - 1);
     request.topic_name.back() = '\0';
-    request.timestamp = node.get_clock()->now().nanoseconds() / 1000;
+    request.timestamp = 0; // Let PX4 set the timestamp
     px4_msgs::msg::MessageFormatResponse response;
     switch (requestMessageFormat(
         node, request, message_format_response_sub, message_format_request_pub,

px4_ros2_cpp/src/components/mode.cpp

@@ -184,7 +184,7 @@ void ModeBase::completed(Result result)
   px4_msgs::msg::ModeCompleted mode_completed{};
   mode_completed.nav_state = static_cast<uint8_t>(id());
   mode_completed.result = static_cast<uint8_t>(result);
-  mode_completed.timestamp = node().get_clock()->now().nanoseconds() / 1000;
+  mode_completed.timestamp = 0; // Let PX4 set the timestamp
   _mode_completed_pub->publish(mode_completed);
   _completed = true;
 }
@@ -281,7 +281,7 @@ void ModeBase::activateSetpointType(SetpointBase & setpoint)
   px4_msgs::msg::VehicleControlMode control_mode{};
   control_mode.source_id = static_cast<uint8_t>(id());
   setpoint.getConfiguration().fillControlMode(control_mode);
-  control_mode.timestamp = node().get_clock()->now().nanoseconds() / 1000;
+  control_mode.timestamp = 0; // Let PX4 set the timestamp
   _config_control_setpoints_pub->publish(control_mode);
 }
 

px4_ros2_cpp/src/components/mode_executor.cpp

@@ -123,7 +123,7 @@ Result ModeExecutorBase::sendCommandSync(
   cmd.param6 = param6;
   cmd.param7 = param7;
   cmd.source_component = px4_msgs::msg::VehicleCommand::COMPONENT_MODE_EXECUTOR_START + id();
-  cmd.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+  cmd.timestamp = 0; // Let PX4 set the timestamp
 
 
   rclcpp::WaitSet wait_set;

px4_ros2_cpp/src/components/overrides.cpp

@@ -33,7 +33,7 @@ void ConfigOverrides::deferFailsafes(bool enabled, int timeout_s)
 void ConfigOverrides::update()
 {
   if (_is_setup) {
-    _current_overrides.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+    _current_overrides.timestamp = 0; // Let PX4 set the timestamp
     _config_overrides_pub->publish(_current_overrides);
 
   } else {

px4_ros2_cpp/src/components/registration.cpp

@@ -83,7 +83,7 @@ bool Registration::doRegister(const RegistrationSettings & settings)
   bool got_reply = false;
 
   for (int retries = 0; retries < 5 && !got_reply; ++retries) {
-    request.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+    request.timestamp = 0; // Let PX4 set the timestamp
     _register_ext_component_request_pub->publish(request);
 
     // wait for publisher, it might take a while initially...
@@ -163,7 +163,7 @@ void Registration::doUnregister()
 {
   if (_registered) {
     RCLCPP_DEBUG(_node.get_logger(), "Unregistering");
-    _unregister_ext_component.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+    _unregister_ext_component.timestamp = 0; // Let PX4 set the timestamp
     _unregister_ext_component_pub->publish(_unregister_ext_component);
     _registered = false;
   }

px4_ros2_cpp/src/control/peripheral_actuators.cpp

@@ -36,7 +36,7 @@ void PeripheralActuatorControls::set(const Eigen::Matrix<float, kNumActuators, 1
     cmd.param5 = values(4);
     cmd.param6 = values(5);
     cmd.param7 = 0; // index
-    cmd.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+    cmd.timestamp = 0; // Let PX4 set the timestamp
     _vehicle_command_pub->publish(cmd);
   }
 }

px4_ros2_cpp/src/control/setpoint_types/direct_actuators.cpp

@@ -28,7 +28,7 @@ void DirectActuatorsSetpointType::updateMotors(
   for (int i = 0; i < kMaxNumMotors; ++i) {
     sp_motors.control[i] = motor_commands(i);
   }
-  sp_motors.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+  sp_motors.timestamp = 0; // Let PX4 set the timestamp
   _actuator_motors_pub->publish(sp_motors);
 }
 
@@ -42,7 +42,7 @@ void DirectActuatorsSetpointType::updateServos(
   for (int i = 0; i < kMaxNumServos; ++i) {
     sp_servos.control[i] = servo_commands(i);
   }
-  sp_servos.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+  sp_servos.timestamp = 0; // Let PX4 set the timestamp
   _actuator_servos_pub->publish(sp_servos);
 }
 

px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp

@@ -34,7 +34,7 @@ void AttitudeSetpointType::update(
   sp.thrust_body[1] = thrust_setpoint_frd(1);
   sp.thrust_body[2] = thrust_setpoint_frd(2);
   sp.yaw_sp_move_rate = yaw_sp_move_rate_rad_s;
-  sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+  sp.timestamp = 0; // Let PX4 set the timestamp
   _vehicle_attitude_setpoint_pub->publish(sp);
 }
 
@@ -48,7 +48,7 @@ void AttitudeSetpointType::update(
   onUpdate();
 
   px4_msgs::msg::VehicleAttitudeSetpoint sp{};
-  sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+  sp.timestamp = 0; // Let PX4 set the timestamp
 
   sp.yaw_sp_move_rate = yaw_sp_move_rate_rad_s;
 

px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp

@@ -30,7 +30,7 @@ void RatesSetpointType::update(
   sp.thrust_body[0] = thrust_setpoint_frd(0);
   sp.thrust_body[1] = thrust_setpoint_frd(1);
   sp.thrust_body[2] = thrust_setpoint_frd(2);
-  sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+  sp.timestamp = 0; // Let PX4 set the timestamp
   _vehicle_rates_setpoint_pub->publish(sp);
 }
 

px4_ros2_cpp/src/control/setpoint_types/experimental/trajectory.cpp

@@ -25,7 +25,7 @@ void TrajectorySetpointType::update(
   onUpdate();
 
   px4_msgs::msg::TrajectorySetpoint sp{};
-  sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+  sp.timestamp = 0; // Let PX4 set the timestamp
 
   sp.position[0] = sp.position[1] = sp.position[2] = NAN;
   sp.velocity[0] = velocity_ned_m_s.x();
@@ -47,7 +47,7 @@ void TrajectorySetpointType::updatePosition(
   onUpdate();
 
   px4_msgs::msg::TrajectorySetpoint sp{};
-  sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+  sp.timestamp = 0; // Let PX4 set the timestamp
 
   sp.position[0] = position_ned_m.x();
   sp.position[1] = position_ned_m.y();

px4_ros2_cpp/src/control/setpoint_types/goto.cpp

@@ -47,7 +47,7 @@ void GotoSetpointType::update(
   sp.flag_set_max_vertical_speed = max_vertical_speed.has_value();
   sp.flag_set_max_heading_rate = max_heading_rate.has_value();
 
-  sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+  sp.timestamp = 0; // Let PX4 set the timestamp
   _goto_setpoint_pub->publish(sp);
 }
 

px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp

@@ -63,7 +63,7 @@ void GlobalPositionMeasurementInterface::update(
   aux_global_position.epv = sqrt(global_position_measurement.vertical_variance.value_or(NAN));
 
   // Publish
-  aux_global_position.timestamp = _node.get_clock()->now().nanoseconds() * 1e-3;
+  aux_global_position.timestamp = 0; // Let PX4 set the timestamp
   _aux_global_position_pub->publish(aux_global_position);
 }
 

px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp

@@ -101,7 +101,7 @@ void LocalPositionMeasurementInterface::update(
   aux_local_position.angular_velocity = {NAN, NAN, NAN};
 
   // Publish
-  aux_local_position.timestamp = _node.get_clock()->now().nanoseconds() * 1e-3;
+  aux_local_position.timestamp = 0; // Let PX4 set the timestamp
   _aux_local_position_pub->publish(aux_local_position);
 }
 

px4_ros2_cpp/test/integration/util.cpp

@@ -71,7 +71,7 @@ void VehicleState::sendCommand(
   cmd.param5 = param5;
   cmd.param6 = param6;
   cmd.param7 = param7;
-  cmd.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+  cmd.timestamp = 0; // Let PX4 set the timestamp
   _vehicle_command_pub->publish(cmd);
 }