From cea185268e133ad22418fb9990f9c19df6e30681 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 19 Oct 2022 19:36:47 -0400 Subject: [PATCH] msg ROS2 compatibility, microdds_client improvements (timesync, reduced code size, added topics, etc), fastrtps purge - update all msgs to be directly compatible with ROS2 - microdds_client improvements - timesync - reduced code size - add to most default builds if we can afford it - lots of other little changes - purge fastrtps (I tried to save this multiple times, but kept hitting roadblocks) --- .ci/Jenkinsfile-compile | 7 - .github/workflows/checks.yml | 1 - .github/workflows/metadata.yml | 34 +- .gitmodules | 7 +- .vscode/cmake-variants.yaml | 6 +- Documentation/Doxyfile.in | 1 - Jenkinsfile | 157 +--- Makefile | 21 +- README.md | 2 +- .../px4fmu_common/init.d-posix/CMakeLists.txt | 1 - ROMFS/px4fmu_common/init.d-posix/px4-rc.rtps | 4 - ROMFS/px4fmu_common/init.d-posix/rcS | 7 +- Tools/astyle/files_to_check_code_style.sh | 3 - Tools/build_micrortps_agent.sh | 15 - {msg/tools => Tools/msg}/__init__.py | 0 {msg/tools => Tools/msg}/generate_msg_docs.py | 4 +- Tools/msg/px_generate_uorb_topic_files.py | 253 ++++++ .../msg}/px_generate_uorb_topic_helper.py | 8 +- {msg => Tools/msg}/templates/ucdr/msg.h.em | 72 +- {msg => Tools/msg}/templates/uorb/msg.cpp.em | 16 +- {msg => Tools/msg}/templates/uorb/msg.h.em | 24 +- .../msg}/templates/uorb/uORBTopics.cpp.em | 19 +- .../msg}/templates/uorb/uORBTopics.hpp.em | 17 +- Tools/setup/arch.sh | 4 +- Tools/setup/ubuntu.sh | 4 +- Tools/update_px4_ros2_bridge.sh | 135 --- boards/beaglebone/blue/default.px4board | 1 + boards/cubepilot/cubeorange/default.px4board | 3 +- .../diatone/mamba-f405-mk2/default.px4board | 1 - boards/emlid/navio2/default.px4board | 1 + boards/holybro/kakutef7/default.px4board | 1 + boards/holybro/kakuteh7/default.px4board | 1 + boards/matek/h743-mini/default.px4board | 1 + boards/matek/h743-slim/default.px4board | 1 + boards/matek/h743/default.px4board | 1 + boards/modalai/fc-v1/default.px4board | 1 + boards/modalai/fc-v1/rtps.px4board | 1 - boards/modalai/fc-v2/default.px4board | 3 +- boards/mro/ctrl-zero-classic/default.px4board | 1 + boards/mro/ctrl-zero-h7-oem/default.px4board | 1 + boards/mro/ctrl-zero-h7-oem/rtps.px4board | 1 - boards/mro/ctrl-zero-h7/default.px4board | 1 + boards/mro/ctrl-zero-h7/rtps.px4board | 1 - boards/mro/pixracerpro/default.px4board | 1 + boards/mro/pixracerpro/rtps.px4board | 1 - boards/nxp/fmuk66-e/default.px4board | 5 +- boards/nxp/fmuk66-e/rtps.px4board | 1 - boards/nxp/fmuk66-v3/default.px4board | 1 + boards/nxp/fmuk66-v3/rtps.px4board | 1 - boards/nxp/fmurt1062-v1/default.px4board | 1 + boards/omnibus/f4sd/default.px4board | 1 + boards/px4/fmu-v4/rtps.px4board | 1 - boards/px4/fmu-v5/cryptotest.px4board | 4 +- boards/px4/fmu-v5/default.px4board | 1 + boards/px4/fmu-v5/protected.px4board | 11 +- boards/px4/fmu-v5/rtps.px4board | 3 - boards/px4/fmu-v5/stackcheck.px4board | 14 +- boards/px4/fmu-v5/uavcanv0periph.px4board | 5 +- boards/px4/fmu-v5x/default.px4board | 10 +- boards/px4/fmu-v5x/rtps.px4board | 15 - boards/px4/fmu-v5x/test.px4board | 6 +- boards/px4/fmu-v6c/default.px4board | 1 + boards/px4/fmu-v6x/default.px4board | 8 +- boards/px4/sitl/default.px4board | 1 + boards/px4/sitl/rtps.px4board | 1 - .../smartap-airlink/default.px4board | 1 + boards/spracing/h7extreme/default.px4board | 1 + msg/{action_request.msg => ActionRequest.msg} | 0 msg/{actuator_armed.msg => ActuatorArmed.msg} | 0 ...ator_controls.msg => ActuatorControls.msg} | 2 +- ..._status.msg => ActuatorControlsStatus.msg} | 2 +- ...actuator_motors.msg => ActuatorMotors.msg} | 0 ...tuator_outputs.msg => ActuatorOutputs.msg} | 0 ...actuator_servos.msg => ActuatorServos.msg} | 0 ...servos_trim.msg => ActuatorServosTrim.msg} | 0 msg/{actuator_test.msg => ActuatorTest.msg} | 0 msg/{adc_report.msg => AdcReport.msg} | 0 msg/{airspeed.msg => Airspeed.msg} | 0 ...ed_validated.msg => AirspeedValidated.msg} | 0 msg/{airspeed_wind.msg => AirspeedWind.msg} | 0 ....msg => AutotuneAttitudeControlStatus.msg} | 0 msg/{battery_status.msg => BatteryStatus.msg} | 0 msg/{button_event.msg => ButtonEvent.msg} | 2 +- msg/CMakeLists.txt | 471 +++++----- msg/{camera_capture.msg => CameraCapture.msg} | 0 msg/{camera_status.msg => CameraStatus.msg} | 0 msg/{camera_trigger.msg => CameraTrigger.msg} | 2 - ...cellular_status.msg => CellularStatus.msg} | 0 ...nstraints.msg => CollisionConstraints.msg} | 0 ...llision_report.msg => CollisionReport.msg} | 0 ..._status.msg => ControlAllocatorStatus.msg} | 0 msg/{cpuload.msg => Cpuload.msg} | 0 msg/{debug_array.msg => DebugArray.msg} | 0 ...{debug_key_value.msg => DebugKeyValue.msg} | 0 msg/{debug_value.msg => DebugValue.msg} | 0 msg/{debug_vect.msg => DebugVect.msg} | 0 ..._pressure.msg => DifferentialPressure.msg} | 0 ...distance_sensor.msg => DistanceSensor.msg} | 0 ...ekf2_timestamps.msg => Ekf2Timestamps.msg} | 0 msg/{esc_report.msg => EscReport.msg} | 0 msg/{esc_status.msg => EscStatus.msg} | 2 +- ...source_1d.msg => EstimatorAidSource1d.msg} | 1 - ...source_2d.msg => EstimatorAidSource2d.msg} | 1 - ...source_3d.msg => EstimatorAidSource3d.msg} | 1 - msg/{estimator_bias.msg => EstimatorBias.msg} | 1 - ...vent_flags.msg => EstimatorEventFlags.msg} | 0 ..._gps_status.msg => EstimatorGpsStatus.msg} | 0 ...novations.msg => EstimatorInnovations.msg} | 0 ...status.msg => EstimatorSelectorStatus.msg} | 0 ...ensor_bias.msg => EstimatorSensorBias.msg} | 0 ...timator_states.msg => EstimatorStates.msg} | 0 ...timator_status.msg => EstimatorStatus.msg} | 0 ...tus_flags.msg => EstimatorStatusFlags.msg} | 0 msg/{event.msg => Event.msg} | 0 msg/{failsafe_flags.msg => FailsafeFlags.msg} | 0 ...r_status.msg => FailureDetectorStatus.msg} | 0 msg/{follow_target.msg => FollowTarget.msg} | 0 ...stimator.msg => FollowTargetEstimator.msg} | 0 ...rget_status.msg => FollowTargetStatus.msg} | 0 ...nerator_status.msg => GeneratorStatus.msg} | 0 ...geofence_result.msg => GeofenceResult.msg} | 0 ...tus.msg => GimbalDeviceAttitudeStatus.msg} | 0 ...mation.msg => GimbalDeviceInformation.msg} | 0 ...titude.msg => GimbalDeviceSetAttitude.msg} | 0 ...ation.msg => GimbalManagerInformation.msg} | 0 ...itude.msg => GimbalManagerSetAttitude.msg} | 0 ....msg => GimbalManagerSetManualControl.msg} | 0 ...ger_status.msg => GimbalManagerStatus.msg} | 0 msg/{gps_dump.msg => GpsDump.msg} | 0 ...{gps_inject_data.msg => GpsInjectData.msg} | 0 msg/{gripper.msg => Gripper.msg} | 0 msg/{health_report.msg => HealthReport.msg} | 1 - msg/{heater_status.msg => HeaterStatus.msg} | 0 msg/{home_position.msg => HomePosition.msg} | 0 ...t_estimate.msg => HoverThrustEstimate.msg} | 0 msg/{input_rc.msg => InputRc.msg} | 0 ...msg => InternalCombustionEngineStatus.msg} | 0 ...iumsbd_status.msg => IridiumsbdStatus.msg} | 0 msg/{irlock_report.msg => IrlockReport.msg} | 0 msg/{landing_gear.msg => LandingGear.msg} | 0 ...tions.msg => LandingTargetInnovations.msg} | 0 ..._target_pose.msg => LandingTargetPose.msg} | 0 msg/{led_control.msg => LedControl.msg} | 0 msg/{log_message.msg => LogMessage.msg} | 0 msg/{logger_status.msg => LoggerStatus.msg} | 0 ...{mag_worker_data.msg => MagWorkerData.msg} | 0 ...imate.msg => MagnetometerBiasEstimate.msg} | 0 ...setpoint.msg => ManualControlSetpoint.msg} | 0 ...switches.msg => ManualControlSwitches.msg} | 0 msg/{mavlink_log.msg => MavlinkLog.msg} | 0 msg/{mavlink_tunnel.msg => MavlinkTunnel.msg} | 0 msg/{mission.msg => Mission.msg} | 0 msg/{mission_result.msg => MissionResult.msg} | 0 ...t_orientation.msg => MountOrientation.msg} | 0 ...sion_item.msg => NavigatorMissionItem.msg} | 0 msg/{npfg_status.msg => NpfgStatus.msg} | 0 ...acle_distance.msg => ObstacleDistance.msg} | 0 ...ntrol_mode.msg => OffboardControlMode.msg} | 0 ...r_status.msg => OnboardComputerStatus.msg} | 0 msg/{orb_test.msg => OrbTest.msg} | 0 msg/{orb_test_large.msg => OrbTestLarge.msg} | 0 ...{orb_test_medium.msg => OrbTestMedium.msg} | 0 msg/{orbit_status.msg => OrbitStatus.msg} | 0 ...rameter_update.msg => ParameterUpdate.msg} | 0 msg/{ping.msg => Ping.msg} | 0 ...sg => PositionControllerLandingStatus.msg} | 0 ...tatus.msg => PositionControllerStatus.msg} | 0 ...tion_setpoint.msg => PositionSetpoint.msg} | 0 ...riplet.msg => PositionSetpointTriplet.msg} | 6 +- ..._button_state.msg => PowerButtonState.msg} | 0 msg/{power_monitor.msg => PowerMonitor.msg} | 0 msg/{pps_capture.msg => PpsCapture.msg} | 0 msg/{pwm_input.msg => PwmInput.msg} | 0 msg/{px4io_status.msg => Px4ioStatus.msg} | 0 msg/{radio_status.msg => RadioStatus.msg} | 0 ...ate_ctrl_status.msg => RateCtrlStatus.msg} | 0 msg/{rc_channels.msg => RcChannels.msg} | 0 ...c_parameter_map.msg => RcParameterMap.msg} | 0 msg/{rpm.msg => Rpm.msg} | 0 ..._time_estimate.msg => RtlTimeEstimate.msg} | 0 msg/{satellite_info.msg => SatelliteInfo.msg} | 0 msg/{sensor_accel.msg => SensorAccel.msg} | 0 ...sor_accel_fifo.msg => SensorAccelFifo.msg} | 0 msg/{sensor_baro.msg => SensorBaro.msg} | 0 ...sensor_combined.msg => SensorCombined.msg} | 0 ...or_correction.msg => SensorCorrection.msg} | 0 ...ss_relative.msg => SensorGnssRelative.msg} | 0 msg/{sensor_gps.msg => SensorGps.msg} | 0 msg/{sensor_gyro.msg => SensorGyro.msg} | 0 ...{sensor_gyro_fft.msg => SensorGyroFft.msg} | 0 ...ensor_gyro_fifo.msg => SensorGyroFifo.msg} | 0 ...or_hygrometer.msg => SensorHygrometer.msg} | 0 msg/{sensor_mag.msg => SensorMag.msg} | 0 ...optical_flow.msg => SensorOpticalFlow.msg} | 0 ...eflight_mag.msg => SensorPreflightMag.msg} | 0 ...nsor_selection.msg => SensorSelection.msg} | 0 msg/{sensors_status.msg => SensorsStatus.msg} | 2 +- ...rs_status_imu.msg => SensorsStatusImu.msg} | 0 msg/{system_power.msg => SystemPower.msg} | 0 msg/{takeoff_status.msg => TakeoffStatus.msg} | 0 ...{task_stack_info.msg => TaskStackInfo.msg} | 0 msg/{tecs_status.msg => TecsStatus.msg} | 0 ...lemetry_status.msg => TelemetryStatus.msg} | 0 ...timesync_status.msg => TimesyncStatus.msg} | 7 +- ...ectory_bezier.msg => TrajectoryBezier.msg} | 0 ...ry_setpoint.msg => TrajectorySetpoint.msg} | 0 ...ry_waypoint.msg => TrajectoryWaypoint.msg} | 0 ...onder_report.msg => TransponderReport.msg} | 0 msg/{tune_control.msg => TuneControl.msg} | 0 ...request.msg => UavcanParameterRequest.msg} | 0 ...ter_value.msg => UavcanParameterValue.msg} | 0 msg/{ulog_stream.msg => UlogStream.msg} | 0 ...{ulog_stream_ack.msg => UlogStreamAck.msg} | 0 msg/{uwb_distance.msg => UwbDistance.msg} | 0 msg/{uwb_grid.msg => UwbGrid.msg} | 0 ...celeration.msg => VehicleAcceleration.msg} | 0 ...ehicle_air_data.msg => VehicleAirData.msg} | 0 ...ion.msg => VehicleAngularAcceleration.msg} | 0 ...=> VehicleAngularAccelerationSetpoint.msg} | 0 ...elocity.msg => VehicleAngularVelocity.msg} | 0 ...hicle_attitude.msg => VehicleAttitude.msg} | 2 +- ...tpoint.msg => VehicleAttitudeSetpoint.msg} | 0 ...vehicle_command.msg => VehicleCommand.msg} | 0 ..._command_ack.msg => VehicleCommandAck.msg} | 0 ...constraints.msg => VehicleConstraints.msg} | 0 ...ontrol_mode.msg => VehicleControlMode.msg} | 0 ...position.msg => VehicleGlobalPosition.msg} | 0 msg/{vehicle_imu.msg => VehicleImu.msg} | 0 ...le_imu_status.msg => VehicleImuStatus.msg} | 0 ...d_detected.msg => VehicleLandDetected.msg} | 0 ..._position.msg => VehicleLocalPosition.msg} | 0 ...t.msg => VehicleLocalPositionSetpoint.msg} | 0 ...gnetometer.msg => VehicleMagnetometer.msg} | 0 ...hicle_odometry.msg => VehicleOdometry.msg} | 0 ...ptical_flow.msg => VehicleOpticalFlow.msg} | 0 ...flow_vel.msg => VehicleOpticalFlowVel.msg} | 0 ..._setpoint.msg => VehicleRatesSetpoint.msg} | 0 msg/{vehicle_roi.msg => VehicleRoi.msg} | 0 msg/{vehicle_status.msg => VehicleStatus.msg} | 0 ...setpoint.msg => VehicleThrustSetpoint.msg} | 0 ...setpoint.msg => VehicleTorqueSetpoint.msg} | 0 ...bezier.msg => VehicleTrajectoryBezier.msg} | 2 +- ...oint.msg => VehicleTrajectoryWaypoint.msg} | 2 +- ...hicle_status.msg => VtolVehicleStatus.msg} | 0 msg/{wheel_encoders.msg => WheelEncoders.msg} | 0 msg/{wind.msg => Wind.msg} | 0 ...ator_status.msg => YawEstimatorStatus.msg} | 0 msg/templates/module.yaml | 6 - msg/templates/px4/ros/msg.h.em | 94 -- msg/templates/px4/uorb/msg.h.em | 99 -- msg/templates/uorb_microcdr/dds_topics.h.em | 225 ----- .../uorb_microcdr/microRTPS_client.cpp.em | 310 ------- msg/templates/uorb_microcdr/msg.cpp.em | 156 ---- msg/templates/uorb_microcdr/msg.h.em | 69 -- msg/templates/uorb_microcdr/uORBTopics.cpp.em | 42 - msg/templates/uorb_microcdr/uORBTopics.hpp.em | 44 - msg/templates/urtps/Publisher.cpp.em | 219 ----- msg/templates/urtps/Publisher.h.em | 120 --- msg/templates/urtps/RtpsTopics.cpp.em | 187 ---- msg/templates/urtps/RtpsTopics.h.em | 220 ----- msg/templates/urtps/Subscriber.cpp.em | 284 ------ msg/templates/urtps/Subscriber.h.em | 140 --- msg/templates/urtps/microRTPS_agent.cpp.em | 408 --------- .../urtps/microRTPS_agent_CMakeLists.txt.em | 63 -- msg/templates/urtps/microRTPS_timesync.cpp.em | 295 ------ msg/templates/urtps/microRTPS_timesync.h.em | 303 ------- msg/templates/urtps/microRTPS_transport.cpp | 841 ----------------- msg/templates/urtps/microRTPS_transport.h | 164 ---- msg/templates/urtps/msg.idl.em | 154 ---- msg/timesync.msg | 4 - msg/tools/generate_microRTPS_bridge.py | 465 ---------- msg/tools/px_generate_uorb_topic_files.py | 561 ------------ msg/tools/uorb_rtps_classifier.py | 186 ---- msg/tools/uorb_to_ros_msgs.py | 127 --- msg/tools/uorb_to_ros_urtps_topics.py | 160 ---- msg/tools/urtps_bridge_topics.yaml | 94 -- platforms/common/uORB/Subscription.hpp | 2 + src/drivers/drv_hrt.h | 12 +- src/drivers/protocol_splitter/Kconfig | 5 - .../protocol_splitter/protocol_splitter.cpp | 845 ------------------ src/lib/CMakeLists.txt | 1 + .../timesync}/CMakeLists.txt | 15 +- src/lib/timesync/Timesync.cpp | 168 ++++ src/lib/timesync/Timesync.hpp | 144 +++ .../airship_att_control_main.cpp | 2 +- src/modules/commander/failsafe/CMakeLists.txt | 2 +- .../failsafe/parse_flags_from_msg.py | 4 +- src/modules/ekf2/EKF/airspeed_fusion.cpp | 4 +- src/modules/ekf2/EKF/ekf.h | 76 +- src/modules/ekf2/EKF/mag_fusion.cpp | 4 +- src/modules/ekf2/EKF/optflow_fusion.cpp | 2 +- src/modules/ekf2/EKF/sideslip_fusion.cpp | 4 +- src/modules/ekf2/EKF/vel_pos_fusion.cpp | 16 +- .../EKF/zero_innovation_heading_update.cpp | 6 +- src/modules/ekf2/EKF2.hpp | 36 +- src/modules/mavlink/CMakeLists.txt | 1 + src/modules/mavlink/mavlink_timesync.cpp | 132 +-- src/modules/mavlink/mavlink_timesync.h | 92 +- src/modules/microdds_client/CMakeLists.txt | 190 ++-- .../microdds_client/Micro-XRCE-DDS-Client | 2 +- src/modules/microdds_client/dds_topics.h.em | 127 +++ src/modules/microdds_client/dds_topics.yaml | 86 ++ .../microdds_client/generate_dds_topics.py | 143 +++ .../microdds_client/microdds_client.cpp | 268 ++++-- src/modules/microdds_client/microdds_client.h | 17 +- src/modules/microdds_client/module.yaml | 56 ++ src/modules/microdds_client/utilities.hpp | 138 +++ src/modules/micrortps_bridge/CMakeLists.txt | 193 ---- src/modules/micrortps_bridge/Kconfig | 12 - src/modules/micrortps_bridge/README.md | 1 - src/modules/micrortps_bridge/micro-CDR | 1 - .../micrortps_client/CMakeLists.txt | 120 --- .../micrortps_client/microRTPS_client.h | 99 -- .../microRTPS_client_main.cpp | 330 ------- .../micrortps_client/module.yaml | 38 - .../res/basic_example_flow.png | Bin 182298 -> 0 bytes .../sitl_targets_gazebo.cmake | 4 +- test/mavsdk_tests/mavsdk_test_runner.py | 4 +- 318 files changed, 1948 insertions(+), 8939 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d-posix/px4-rc.rtps delete mode 100755 Tools/build_micrortps_agent.sh rename {msg/tools => Tools/msg}/__init__.py (100%) rename {msg/tools => Tools/msg}/generate_msg_docs.py (98%) create mode 100755 Tools/msg/px_generate_uorb_topic_files.py rename {msg/tools => Tools/msg}/px_generate_uorb_topic_helper.py (96%) rename {msg => Tools/msg}/templates/ucdr/msg.h.em (54%) rename {msg => Tools/msg}/templates/uorb/msg.cpp.em (85%) rename {msg => Tools/msg}/templates/uorb/msg.h.em (90%) rename {msg => Tools/msg}/templates/uorb/uORBTopics.cpp.em (84%) rename {msg => Tools/msg}/templates/uorb/uORBTopics.hpp.em (85%) delete mode 100755 Tools/update_px4_ros2_bridge.sh delete mode 100644 boards/modalai/fc-v1/rtps.px4board delete mode 100644 boards/mro/ctrl-zero-h7-oem/rtps.px4board delete mode 100644 boards/mro/ctrl-zero-h7/rtps.px4board delete mode 100644 boards/mro/pixracerpro/rtps.px4board delete mode 100644 boards/nxp/fmuk66-e/rtps.px4board delete mode 100644 boards/nxp/fmuk66-v3/rtps.px4board delete mode 100644 boards/px4/fmu-v4/rtps.px4board delete mode 100644 boards/px4/fmu-v5/rtps.px4board delete mode 100644 boards/px4/fmu-v5x/rtps.px4board delete mode 100644 boards/px4/sitl/rtps.px4board rename msg/{action_request.msg => ActionRequest.msg} (100%) rename msg/{actuator_armed.msg => ActuatorArmed.msg} (100%) rename msg/{actuator_controls.msg => ActuatorControls.msg} (88%) rename msg/{actuator_controls_status.msg => ActuatorControlsStatus.msg} (66%) rename msg/{actuator_motors.msg => ActuatorMotors.msg} (100%) rename msg/{actuator_outputs.msg => ActuatorOutputs.msg} (100%) rename msg/{actuator_servos.msg => ActuatorServos.msg} (100%) rename msg/{actuator_servos_trim.msg => ActuatorServosTrim.msg} (100%) rename msg/{actuator_test.msg => ActuatorTest.msg} (100%) rename msg/{adc_report.msg => AdcReport.msg} (100%) rename msg/{airspeed.msg => Airspeed.msg} (100%) rename msg/{airspeed_validated.msg => AirspeedValidated.msg} (100%) rename msg/{airspeed_wind.msg => AirspeedWind.msg} (100%) rename msg/{autotune_attitude_control_status.msg => AutotuneAttitudeControlStatus.msg} (100%) rename msg/{battery_status.msg => BatteryStatus.msg} (100%) rename msg/{button_event.msg => ButtonEvent.msg} (85%) rename msg/{camera_capture.msg => CameraCapture.msg} (100%) rename msg/{camera_status.msg => CameraStatus.msg} (100%) rename msg/{camera_trigger.msg => CameraTrigger.msg} (89%) rename msg/{cellular_status.msg => CellularStatus.msg} (100%) rename msg/{collision_constraints.msg => CollisionConstraints.msg} (100%) rename msg/{collision_report.msg => CollisionReport.msg} (100%) rename msg/{control_allocator_status.msg => ControlAllocatorStatus.msg} (100%) rename msg/{cpuload.msg => Cpuload.msg} (100%) rename msg/{debug_array.msg => DebugArray.msg} (100%) rename msg/{debug_key_value.msg => DebugKeyValue.msg} (100%) rename msg/{debug_value.msg => DebugValue.msg} (100%) rename msg/{debug_vect.msg => DebugVect.msg} (100%) rename msg/{differential_pressure.msg => DifferentialPressure.msg} (100%) rename msg/{distance_sensor.msg => DistanceSensor.msg} (100%) rename msg/{ekf2_timestamps.msg => Ekf2Timestamps.msg} (100%) rename msg/{esc_report.msg => EscReport.msg} (100%) rename msg/{esc_status.msg => EscStatus.msg} (98%) rename msg/{estimator_aid_source_1d.msg => EstimatorAidSource1d.msg} (96%) rename msg/{estimator_aid_source_2d.msg => EstimatorAidSource2d.msg} (95%) rename msg/{estimator_aid_source_3d.msg => EstimatorAidSource3d.msg} (95%) rename msg/{estimator_bias.msg => EstimatorBias.msg} (96%) rename msg/{estimator_event_flags.msg => EstimatorEventFlags.msg} (100%) rename msg/{estimator_gps_status.msg => EstimatorGpsStatus.msg} (100%) rename msg/{estimator_innovations.msg => EstimatorInnovations.msg} (100%) rename msg/{estimator_selector_status.msg => EstimatorSelectorStatus.msg} (100%) rename msg/{estimator_sensor_bias.msg => EstimatorSensorBias.msg} (100%) rename msg/{estimator_states.msg => EstimatorStates.msg} (100%) rename msg/{estimator_status.msg => EstimatorStatus.msg} (100%) rename msg/{estimator_status_flags.msg => EstimatorStatusFlags.msg} (100%) rename msg/{event.msg => Event.msg} (100%) rename msg/{failsafe_flags.msg => FailsafeFlags.msg} (100%) rename msg/{failure_detector_status.msg => FailureDetectorStatus.msg} (100%) rename msg/{follow_target.msg => FollowTarget.msg} (100%) rename msg/{follow_target_estimator.msg => FollowTargetEstimator.msg} (100%) rename msg/{follow_target_status.msg => FollowTargetStatus.msg} (100%) rename msg/{generator_status.msg => GeneratorStatus.msg} (100%) rename msg/{geofence_result.msg => GeofenceResult.msg} (100%) rename msg/{gimbal_device_attitude_status.msg => GimbalDeviceAttitudeStatus.msg} (100%) rename msg/{gimbal_device_information.msg => GimbalDeviceInformation.msg} (100%) rename msg/{gimbal_device_set_attitude.msg => GimbalDeviceSetAttitude.msg} (100%) rename msg/{gimbal_manager_information.msg => GimbalManagerInformation.msg} (100%) rename msg/{gimbal_manager_set_attitude.msg => GimbalManagerSetAttitude.msg} (100%) rename msg/{gimbal_manager_set_manual_control.msg => GimbalManagerSetManualControl.msg} (100%) rename msg/{gimbal_manager_status.msg => GimbalManagerStatus.msg} (100%) rename msg/{gps_dump.msg => GpsDump.msg} (100%) rename msg/{gps_inject_data.msg => GpsInjectData.msg} (100%) rename msg/{gripper.msg => Gripper.msg} (100%) rename msg/{health_report.msg => HealthReport.msg} (99%) rename msg/{heater_status.msg => HeaterStatus.msg} (100%) rename msg/{home_position.msg => HomePosition.msg} (100%) rename msg/{hover_thrust_estimate.msg => HoverThrustEstimate.msg} (100%) rename msg/{input_rc.msg => InputRc.msg} (100%) rename msg/{internal_combustion_engine_status.msg => InternalCombustionEngineStatus.msg} (100%) rename msg/{iridiumsbd_status.msg => IridiumsbdStatus.msg} (100%) rename msg/{irlock_report.msg => IrlockReport.msg} (100%) rename msg/{landing_gear.msg => LandingGear.msg} (100%) rename msg/{landing_target_innovations.msg => LandingTargetInnovations.msg} (100%) rename msg/{landing_target_pose.msg => LandingTargetPose.msg} (100%) rename msg/{led_control.msg => LedControl.msg} (100%) rename msg/{log_message.msg => LogMessage.msg} (100%) rename msg/{logger_status.msg => LoggerStatus.msg} (100%) rename msg/{mag_worker_data.msg => MagWorkerData.msg} (100%) rename msg/{magnetometer_bias_estimate.msg => MagnetometerBiasEstimate.msg} (100%) rename msg/{manual_control_setpoint.msg => ManualControlSetpoint.msg} (100%) rename msg/{manual_control_switches.msg => ManualControlSwitches.msg} (100%) rename msg/{mavlink_log.msg => MavlinkLog.msg} (100%) rename msg/{mavlink_tunnel.msg => MavlinkTunnel.msg} (100%) rename msg/{mission.msg => Mission.msg} (100%) rename msg/{mission_result.msg => MissionResult.msg} (100%) rename msg/{mount_orientation.msg => MountOrientation.msg} (100%) rename msg/{navigator_mission_item.msg => NavigatorMissionItem.msg} (100%) rename msg/{npfg_status.msg => NpfgStatus.msg} (100%) rename msg/{obstacle_distance.msg => ObstacleDistance.msg} (100%) rename msg/{offboard_control_mode.msg => OffboardControlMode.msg} (100%) rename msg/{onboard_computer_status.msg => OnboardComputerStatus.msg} (100%) rename msg/{orb_test.msg => OrbTest.msg} (100%) rename msg/{orb_test_large.msg => OrbTestLarge.msg} (100%) rename msg/{orb_test_medium.msg => OrbTestMedium.msg} (100%) rename msg/{orbit_status.msg => OrbitStatus.msg} (100%) rename msg/{parameter_update.msg => ParameterUpdate.msg} (100%) rename msg/{ping.msg => Ping.msg} (100%) rename msg/{position_controller_landing_status.msg => PositionControllerLandingStatus.msg} (100%) rename msg/{position_controller_status.msg => PositionControllerStatus.msg} (100%) rename msg/{position_setpoint.msg => PositionSetpoint.msg} (100%) rename msg/{position_setpoint_triplet.msg => PositionSetpointTriplet.msg} (67%) rename msg/{power_button_state.msg => PowerButtonState.msg} (100%) rename msg/{power_monitor.msg => PowerMonitor.msg} (100%) rename msg/{pps_capture.msg => PpsCapture.msg} (100%) rename msg/{pwm_input.msg => PwmInput.msg} (100%) rename msg/{px4io_status.msg => Px4ioStatus.msg} (100%) rename msg/{radio_status.msg => RadioStatus.msg} (100%) rename msg/{rate_ctrl_status.msg => RateCtrlStatus.msg} (100%) rename msg/{rc_channels.msg => RcChannels.msg} (100%) rename msg/{rc_parameter_map.msg => RcParameterMap.msg} (100%) rename msg/{rpm.msg => Rpm.msg} (100%) rename msg/{rtl_time_estimate.msg => RtlTimeEstimate.msg} (100%) rename msg/{satellite_info.msg => SatelliteInfo.msg} (100%) rename msg/{sensor_accel.msg => SensorAccel.msg} (100%) rename msg/{sensor_accel_fifo.msg => SensorAccelFifo.msg} (100%) rename msg/{sensor_baro.msg => SensorBaro.msg} (100%) rename msg/{sensor_combined.msg => SensorCombined.msg} (100%) rename msg/{sensor_correction.msg => SensorCorrection.msg} (100%) rename msg/{sensor_gnss_relative.msg => SensorGnssRelative.msg} (100%) rename msg/{sensor_gps.msg => SensorGps.msg} (100%) rename msg/{sensor_gyro.msg => SensorGyro.msg} (100%) rename msg/{sensor_gyro_fft.msg => SensorGyroFft.msg} (100%) rename msg/{sensor_gyro_fifo.msg => SensorGyroFifo.msg} (100%) rename msg/{sensor_hygrometer.msg => SensorHygrometer.msg} (100%) mode change 100755 => 100644 rename msg/{sensor_mag.msg => SensorMag.msg} (100%) rename msg/{sensor_optical_flow.msg => SensorOpticalFlow.msg} (100%) rename msg/{sensor_preflight_mag.msg => SensorPreflightMag.msg} (100%) rename msg/{sensor_selection.msg => SensorSelection.msg} (100%) rename msg/{sensors_status.msg => SensorsStatus.msg} (87%) rename msg/{sensors_status_imu.msg => SensorsStatusImu.msg} (100%) rename msg/{system_power.msg => SystemPower.msg} (100%) rename msg/{takeoff_status.msg => TakeoffStatus.msg} (100%) rename msg/{task_stack_info.msg => TaskStackInfo.msg} (100%) rename msg/{tecs_status.msg => TecsStatus.msg} (100%) rename msg/{telemetry_status.msg => TelemetryStatus.msg} (100%) rename msg/{timesync_status.msg => TimesyncStatus.msg} (72%) rename msg/{trajectory_bezier.msg => TrajectoryBezier.msg} (100%) rename msg/{trajectory_setpoint.msg => TrajectorySetpoint.msg} (100%) rename msg/{trajectory_waypoint.msg => TrajectoryWaypoint.msg} (100%) rename msg/{transponder_report.msg => TransponderReport.msg} (100%) rename msg/{tune_control.msg => TuneControl.msg} (100%) rename msg/{uavcan_parameter_request.msg => UavcanParameterRequest.msg} (100%) rename msg/{uavcan_parameter_value.msg => UavcanParameterValue.msg} (100%) rename msg/{ulog_stream.msg => UlogStream.msg} (100%) rename msg/{ulog_stream_ack.msg => UlogStreamAck.msg} (100%) rename msg/{uwb_distance.msg => UwbDistance.msg} (100%) rename msg/{uwb_grid.msg => UwbGrid.msg} (100%) rename msg/{vehicle_acceleration.msg => VehicleAcceleration.msg} (100%) rename msg/{vehicle_air_data.msg => VehicleAirData.msg} (100%) rename msg/{vehicle_angular_acceleration.msg => VehicleAngularAcceleration.msg} (100%) rename msg/{vehicle_angular_acceleration_setpoint.msg => VehicleAngularAccelerationSetpoint.msg} (100%) rename msg/{vehicle_angular_velocity.msg => VehicleAngularVelocity.msg} (100%) rename msg/{vehicle_attitude.msg => VehicleAttitude.msg} (86%) rename msg/{vehicle_attitude_setpoint.msg => VehicleAttitudeSetpoint.msg} (100%) rename msg/{vehicle_command.msg => VehicleCommand.msg} (100%) rename msg/{vehicle_command_ack.msg => VehicleCommandAck.msg} (100%) rename msg/{vehicle_constraints.msg => VehicleConstraints.msg} (100%) rename msg/{vehicle_control_mode.msg => VehicleControlMode.msg} (100%) rename msg/{vehicle_global_position.msg => VehicleGlobalPosition.msg} (100%) rename msg/{vehicle_imu.msg => VehicleImu.msg} (100%) rename msg/{vehicle_imu_status.msg => VehicleImuStatus.msg} (100%) rename msg/{vehicle_land_detected.msg => VehicleLandDetected.msg} (100%) rename msg/{vehicle_local_position.msg => VehicleLocalPosition.msg} (100%) rename msg/{vehicle_local_position_setpoint.msg => VehicleLocalPositionSetpoint.msg} (100%) rename msg/{vehicle_magnetometer.msg => VehicleMagnetometer.msg} (100%) rename msg/{vehicle_odometry.msg => VehicleOdometry.msg} (100%) rename msg/{vehicle_optical_flow.msg => VehicleOpticalFlow.msg} (100%) rename msg/{vehicle_optical_flow_vel.msg => VehicleOpticalFlowVel.msg} (100%) rename msg/{vehicle_rates_setpoint.msg => VehicleRatesSetpoint.msg} (100%) rename msg/{vehicle_roi.msg => VehicleRoi.msg} (100%) rename msg/{vehicle_status.msg => VehicleStatus.msg} (100%) rename msg/{vehicle_thrust_setpoint.msg => VehicleThrustSetpoint.msg} (100%) rename msg/{vehicle_torque_setpoint.msg => VehicleTorqueSetpoint.msg} (100%) rename msg/{vehicle_trajectory_bezier.msg => VehicleTrajectoryBezier.msg} (93%) rename msg/{vehicle_trajectory_waypoint.msg => VehicleTrajectoryWaypoint.msg} (95%) rename msg/{vtol_vehicle_status.msg => VtolVehicleStatus.msg} (100%) rename msg/{wheel_encoders.msg => WheelEncoders.msg} (100%) rename msg/{wind.msg => Wind.msg} (100%) rename msg/{yaw_estimator_status.msg => YawEstimatorStatus.msg} (100%) delete mode 100644 msg/templates/module.yaml delete mode 100644 msg/templates/px4/ros/msg.h.em delete mode 100644 msg/templates/px4/uorb/msg.h.em delete mode 100644 msg/templates/uorb_microcdr/dds_topics.h.em delete mode 100644 msg/templates/uorb_microcdr/microRTPS_client.cpp.em delete mode 100644 msg/templates/uorb_microcdr/msg.cpp.em delete mode 100644 msg/templates/uorb_microcdr/msg.h.em delete mode 100644 msg/templates/uorb_microcdr/uORBTopics.cpp.em delete mode 100644 msg/templates/uorb_microcdr/uORBTopics.hpp.em delete mode 100644 msg/templates/urtps/Publisher.cpp.em delete mode 100644 msg/templates/urtps/Publisher.h.em delete mode 100644 msg/templates/urtps/RtpsTopics.cpp.em delete mode 100644 msg/templates/urtps/RtpsTopics.h.em delete mode 100644 msg/templates/urtps/Subscriber.cpp.em delete mode 100644 msg/templates/urtps/Subscriber.h.em delete mode 100644 msg/templates/urtps/microRTPS_agent.cpp.em delete mode 100644 msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em delete mode 100644 msg/templates/urtps/microRTPS_timesync.cpp.em delete mode 100644 msg/templates/urtps/microRTPS_timesync.h.em delete mode 100644 msg/templates/urtps/microRTPS_transport.cpp delete mode 100644 msg/templates/urtps/microRTPS_transport.h delete mode 100644 msg/templates/urtps/msg.idl.em delete mode 100644 msg/timesync.msg delete mode 100644 msg/tools/generate_microRTPS_bridge.py delete mode 100755 msg/tools/px_generate_uorb_topic_files.py delete mode 100644 msg/tools/uorb_rtps_classifier.py delete mode 100755 msg/tools/uorb_to_ros_msgs.py delete mode 100755 msg/tools/uorb_to_ros_urtps_topics.py delete mode 100644 msg/tools/urtps_bridge_topics.yaml delete mode 100644 src/drivers/protocol_splitter/Kconfig delete mode 100644 src/drivers/protocol_splitter/protocol_splitter.cpp rename src/{drivers/protocol_splitter => lib/timesync}/CMakeLists.txt (90%) create mode 100644 src/lib/timesync/Timesync.cpp create mode 100644 src/lib/timesync/Timesync.hpp create mode 100644 src/modules/microdds_client/dds_topics.h.em create mode 100644 src/modules/microdds_client/dds_topics.yaml create mode 100644 src/modules/microdds_client/generate_dds_topics.py create mode 100644 src/modules/microdds_client/module.yaml create mode 100644 src/modules/microdds_client/utilities.hpp delete mode 100644 src/modules/micrortps_bridge/CMakeLists.txt delete mode 100644 src/modules/micrortps_bridge/Kconfig delete mode 100644 src/modules/micrortps_bridge/README.md delete mode 160000 src/modules/micrortps_bridge/micro-CDR delete mode 100644 src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt delete mode 100644 src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h delete mode 100644 src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp delete mode 100644 src/modules/micrortps_bridge/micrortps_client/module.yaml delete mode 100644 src/modules/micrortps_bridge/res/basic_example_flow.png diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index c4ad1ac591..ba9b8a1523 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -69,24 +69,18 @@ pipeline { "matek_h743-slim_default", "matek_h743_default", "modalai_fc-v1_default", - "modalai_fc-v1_rtps", "modalai_fc-v2_default", "modalai_voxl2-io_default", "mro_ctrl-zero-f7_default", "mro_ctrl-zero-f7-oem_default", "mro_ctrl-zero-h7-oem_default", - "mro_ctrl-zero-h7-oem_rtps", "mro_ctrl-zero-h7_default", - "mro_ctrl-zero-h7_rtps", "mro_pixracerpro_default", - "mro_pixracerpro_rtps", "mro_x21-777_default", "mro_x21_default", "nxp_fmuk66-e_default", - "nxp_fmuk66-e_rtps", "nxp_fmuk66-e_socketcan", "nxp_fmuk66-v3_default", - "nxp_fmuk66-v3_rtps", "nxp_fmuk66-v3_socketcan", "nxp_fmurt1062-v1_default", "nxp_ucans32k146_canbootloader", @@ -104,7 +98,6 @@ pipeline { "px4_fmu-v5_debug", "px4_fmu-v5_default", "px4_fmu-v5_lto", - "px4_fmu-v5_rtps", "px4_fmu-v5_stackcheck", "px4_fmu-v5_uavcanv0periph", "px4_fmu-v5x_default", diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml index 9c72c966fa..ef15d32171 100644 --- a/.github/workflows/checks.yml +++ b/.github/workflows/checks.yml @@ -23,7 +23,6 @@ jobs: "shellcheck_all", "NO_NINJA_BUILD=1 px4_fmu-v5_default", "NO_NINJA_BUILD=1 px4_sitl_default", - "BUILD_MICRORTPS_AGENT=1 px4_sitl_rtps", "airframe_metadata", "module_documentation", "parameters_metadata", diff --git a/.github/workflows/metadata.yml b/.github/workflows/metadata.yml index 4372a34da1..10799ba38f 100644 --- a/.github/workflows/metadata.yml +++ b/.github/workflows/metadata.yml @@ -116,7 +116,7 @@ jobs: ls -ls * # TODO: deploy graph_px4_sitl.json to S3 px4-travis:Firmware/master/ - micrortps_agent: + ROS2_msgs: runs-on: ubuntu-latest container: px4io/px4-dev-base-focal:2021-09-08 steps: @@ -124,34 +124,8 @@ jobs: with: token: ${{ secrets.ACCESS_TOKEN }} - - name: microRTPS agent - run: | - make px4_sitl_rtps - git clone https://github.com/PX4/micrortps_agent.git - cp -R build/px4_sitl_rtps/src/modules/micrortps_bridge/micrortps_agent/* micrortps_agent - - ROS_msgs: - runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 - steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} - - - name: PX4 ROS msgs + - name: PX4 ROS2 msgs run: | git clone https://github.com/PX4/px4_msgs.git - python3 msg/tools/uorb_to_ros_msgs.py msg/ px4_msgs/msg/ - - ROS2_bridge: - runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 - steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} - - - name: PX4 ROS2 bridge - run: | - git clone https://github.com/PX4/px4_ros_com.git - ./msg/tools/uorb_to_ros_urtps_topics.py -i msg/tools/urtps_bridge_topics.yaml -o px4_ros_com/templates/urtps_bridge_topics.yaml + rm px4_msgs/msg/*.msg + cp msg/*.msg px4_msgs/msg/ diff --git a/.gitmodules b/.gitmodules index a1b3d788af..4b9627a94c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -18,10 +18,6 @@ path = src/drivers/gps/devices url = https://github.com/PX4/PX4-GPSDrivers.git branch = main -[submodule "src/modules/micrortps_bridge/micro-CDR"] - path = src/modules/micrortps_bridge/micro-CDR - url = https://github.com/PX4/Micro-CDR.git - branch = master [submodule "platforms/nuttx/NuttX/nuttx"] path = platforms/nuttx/NuttX/nuttx url = https://github.com/PX4/NuttX.git @@ -64,4 +60,5 @@ branch = px4 [submodule "src/modules/microdds_client/Micro-XRCE-DDS-Client"] path = src/modules/microdds_client/Micro-XRCE-DDS-Client - url = https://github.com/eProsima/Micro-XRCE-DDS-Client.git + url = https://github.com/PX4/Micro-XRCE-DDS-Client.git + branch = px4 diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index d47e962fd6..1f93a5ce3e 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -6,11 +6,11 @@ CONFIG: buildType: RelWithDebInfo settings: CONFIG: px4_sitl_default - px4_sitl_rtps: - short: px4_sitl_rtps + px4_sitl_nolockstep: + short: px4_sitl_nolockstep buildType: RelWithDebInfo settings: - CONFIG: px4_sitl_rtps + CONFIG: px4_sitl_nolockstep px4_sitl_asan: short: px4_sitl (AddressSanitizer) buildType: AddressSanitizer diff --git a/Documentation/Doxyfile.in b/Documentation/Doxyfile.in index f6d12430c7..0874d152cf 100644 --- a/Documentation/Doxyfile.in +++ b/Documentation/Doxyfile.in @@ -829,7 +829,6 @@ RECURSIVE = YES # run. EXCLUDE = @CMAKE_SOURCE_DIR@/src/modules/uavcan/libuavcan \ - @CMAKE_SOURCE_DIR@/src/modules/micrortps_bridge/micro-CDR \ @CMAKE_SOURCE_DIR@/src/examples \ @CMAKE_SOURCE_DIR@/src/templates diff --git a/Jenkinsfile b/Jenkinsfile index b25af16dff..30deefc365 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -13,76 +13,6 @@ pipeline { } } parallel { - // stage('Catkin build on ROS workspace') { - // agent { - // docker { - // image 'px4io/px4-dev-ros-melodic:2021-08-18' - // args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' - // } - // } - // steps { - // sh 'ls -l' - // sh '''#!/bin/bash -l - // echo $0; - // mkdir -p catkin_ws/src; - // cd catkin_ws; - // git -C ${WORKSPACE}/catkin_ws/src/Firmware submodule update --init --recursive --force Tools/simulation/gazebo/sitl_gazebo - // git clone --recursive ${WORKSPACE}/catkin_ws/src/Firmware/Tools/simulation/gazebo/sitl_gazebo src/mavlink_sitl_gazebo; - // git -C ${WORKSPACE}/catkin_ws/src/Firmware fetch --tags; - // source /opt/ros/melodic/setup.bash; - // export PYTHONPATH=/opt/ros/$ROS_DISTRO/lib/python2.7/dist-packages:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages; - // catkin init; - // catkin build -j$(nproc) -l$(nproc); - // ''' - // // test if the binary was correctly installed and runs using 'mavros_posix_sitl.launch' - // sh '''#!/bin/bash -l - // echo $0; - // source catkin_ws/devel/setup.bash; - // rostest px4 pub_test.launch; - // ''' - // } - // post { - // always { - // sh 'rm -rf catkin_ws' - // } - // failure { - // archiveArtifacts(allowEmptyArchive: false, artifacts: '.ros/**/*.xml, .ros/**/*.log') - // } - // } - // options { - // checkoutToSubdirectory('catkin_ws/src/Firmware') - // } - // } - - stage('Colcon build on ROS2 workspace') { - agent { - docker { - image 'px4io/px4-dev-ros2-foxy:2021-08-18' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' - } - } - steps { - sh 'ls -l' - sh '''#!/bin/bash -l - echo $0; - unset ROS_DISTRO; - mkdir -p colcon_ws/src; - cd colcon_ws; - git -C ${WORKSPACE}/colcon_ws/src/Firmware submodule update --init --recursive --force Tools/simulation/gazebo/sitl_gazebo; - git -C ${WORKSPACE}/colcon_ws/src/Firmware fetch --tags; - source /opt/ros/foxy/setup.sh; - colcon build --event-handlers console_direct+ --symlink-install; - ''' - } - post { - always { - sh 'rm -rf colcon_ws' - } - } - options { - checkoutToSubdirectory('colcon_ws/src/Firmware') - } - } stage('Airframe') { agent { @@ -149,7 +79,7 @@ pipeline { docker { image 'px4io/px4-dev-base-focal:2021-08-18' } } steps { - sh 'mkdir -p build/msg_docs; ./msg/tools/generate_msg_docs.py -d build/msg_docs' + sh 'mkdir -p build/msg_docs; ./Tools/msg/generate_msg_docs.py -d build/msg_docs' dir('build') { archiveArtifacts(artifacts: 'msg_docs/*.md') stash includes: 'msg_docs/*.md', name: 'msg_documentation' @@ -288,37 +218,6 @@ pipeline { } } - stage('microRTPS agent') { - agent { - docker { image 'px4io/px4-dev-base-focal:2021-08-18' } - } - steps { - sh('export') - sh('git fetch --all --tags') - sh('make distclean; git clean -ff -x -d .') - sh('make px4_sitl_rtps') - withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) { - sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/micrortps_agent.git -b ${BRANCH_NAME}") - sh("rm -rf micrortps_agent/src micrortps_agent/idl") - sh('cp -R build/px4_sitl_rtps/src/modules/micrortps_bridge/micrortps_agent/* micrortps_agent') - sh('cd micrortps_agent; git status; git add src; git commit -a -m "Update microRTPS agent source code `date`" || true') - sh('cd micrortps_agent; git push origin ${BRANCH_NAME} || true') - sh('cd micrortps_agent; git status; git add idl; git commit -a -m "Update IDL definitions `date`" || true') - sh('cd micrortps_agent; git push origin ${BRANCH_NAME} || true') - sh('cd micrortps_agent; git status; git add CMakeLists.txt; git commit -a -m "Update CMakeLists.txt `date`" || true') - sh('cd micrortps_agent; git push origin ${BRANCH_NAME} || true') - sh('rm -rf micrortps_agent') - } - } - when { - anyOf { - branch 'main' - branch 'master' // should be removed, but in case there is something going on... - branch 'pr-jenkins' // for testing - } - } - } - stage('PX4 ROS msgs') { agent { docker { image 'px4io/px4-dev-base-focal:2021-08-18' } @@ -329,66 +228,16 @@ pipeline { withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) { sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_msgs.git") // 'main' branch - sh('./msg/tools/uorb_to_ros_msgs.py msg/ px4_msgs/msg/') + sh('rm -f px4_msgs/msg/*.msg') + sh('cp msg/*.msg px4_msgs/msg/') sh('cd px4_msgs; git status; git add .; git commit -a -m "Update message definitions `date`" || true') sh('cd px4_msgs; git push origin main || true') - // 'ros1' branch - sh('cd px4_msgs; git checkout ros1') - sh('./msg/tools/uorb_to_ros_msgs.py msg/ px4_msgs/msg/') - sh('cd px4_msgs; git status; git add .; git commit -a -m "Update message definitions `date`" || true') - sh('cd px4_msgs; git push origin ros1 || true') sh('rm -rf px4_msgs') } } when { anyOf { branch 'main' - branch 'master' // should be removed, but in case there is something going on... - branch 'pr-jenkins' // for testing - } - } - } - - stage('PX4 ROS2 bridge') { - agent { - docker { image 'px4io/px4-dev-base-focal:2021-08-18' } - } - steps { - sh('export') - sh('make distclean; git clean -ff -x -d .') - withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) { - sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_ros_com.git -b ${BRANCH_NAME}") - // deploy uORB RTPS ID map - sh('./msg/tools/uorb_to_ros_urtps_topics.py -i msg/tools/urtps_bridge_topics.yaml -o px4_ros_com/templates/urtps_bridge_topics.yaml') - sh('cd px4_ros_com; git status; git add .; git commit -a -m "Update uORB RTPS ID map `date`" || true') - sh('cd px4_ros_com; git push origin ${BRANCH_NAME} || true') - // deploy uORB RTPS required tools - sh('cp msg/tools/uorb_rtps_classifier.py px4_ros_com/scripts/uorb_rtps_classifier.py') - sh('cp msg/tools/generate_microRTPS_bridge.py px4_ros_com/scripts/generate_microRTPS_bridge.py') - sh('cp msg/tools/px_generate_uorb_topic_files.py px4_ros_com/scripts/px_generate_uorb_topic_files.py') - sh('cp msg/tools/px_generate_uorb_topic_helper.py px4_ros_com/scripts/px_generate_uorb_topic_helper.py') - // deploy templates - sh('cp msg/templates/urtps/microRTPS_agent.cpp.em px4_ros_com/templates/microRTPS_agent.cpp.em') - sh('cp msg/templates/urtps/microRTPS_timesync.cpp.em px4_ros_com/templates/microRTPS_timesync.cpp.em') - sh('cp msg/templates/urtps/microRTPS_timesync.h.em px4_ros_com/templates/microRTPS_timesync.h.em') - sh('cp msg/templates/urtps/microRTPS_transport.cpp px4_ros_com/templates/microRTPS_transport.cpp') - sh('cp msg/templates/urtps/microRTPS_transport.h px4_ros_com/templates/microRTPS_transport.h') - sh('cp msg/templates/urtps/Publisher.cpp.em px4_ros_com/templates/Publisher.cpp.em') - sh('cp msg/templates/urtps/Publisher.h.em px4_ros_com/templates/Publisher.h.em') - sh('cp msg/templates/urtps/Subscriber.cpp.em px4_ros_com/templates/Subscriber.cpp.em') - sh('cp msg/templates/urtps/Subscriber.h.em px4_ros_com/templates/Subscriber.h.em') - sh('cp msg/templates/urtps/RtpsTopics.cpp.em px4_ros_com/templates/RtpsTopics.cpp.em') - sh('cp msg/templates/urtps/RtpsTopics.h.em px4_ros_com/templates/RtpsTopics.h.em') - sh('cd px4_ros_com; git status; git add .; git commit -a -m "Update uORB RTPS script tools `date`" || true') - sh('cd px4_ros_com; git push origin ${BRANCH_NAME} || true') - sh('rm -rf px4_msgs') - } - } - when { - anyOf { - branch 'main' - branch 'master' // should be removed, but in case there is something going on... - branch 'pr-jenkins' // for testing } } } diff --git a/Makefile b/Makefile index 34aa1c1273..c61ea751d0 100644 --- a/Makefile +++ b/Makefile @@ -172,11 +172,6 @@ ifdef PYTHON_EXECUTABLE override CMAKE_ARGS += -DPYTHON_EXECUTABLE=${PYTHON_EXECUTABLE} endif -# Check if the microRTPS agent is to be built -ifdef BUILD_MICRORTPS_AGENT - override CMAKE_ARGS += -DBUILD_MICRORTPS_AGENT=ON -endif - # Functions # -------------------------------------------------------------------- # describe how to build a cmake config @@ -253,7 +248,7 @@ endef # Other targets # -------------------------------------------------------------------- -.PHONY: qgc_firmware px4fmu_firmware misc_qgc_extra_firmware check_rtps +.PHONY: qgc_firmware px4fmu_firmware misc_qgc_extra_firmware # QGroundControl flashable NuttX firmware qgc_firmware: px4fmu_firmware misc_qgc_extra_firmware @@ -278,15 +273,7 @@ misc_qgc_extra_firmware: \ check_airmind_mindpx-v2_default \ sizes -# builds with RTPS -check_rtps: \ - check_px4_fmu-v3_rtps \ - check_px4_fmu-v4_rtps \ - check_px4_fmu-v4pro_rtps \ - check_px4_sitl_rtps \ - sizes - -.PHONY: sizes check quick_check check_rtps uorb_graphs +.PHONY: sizes check quick_check uorb_graphs sizes: @-find build -name *.elf -type f | xargs size 2> /dev/null || : @@ -495,7 +482,9 @@ shellcheck_all: validate_module_configs: @find "$(SRC_DIR)"/src/modules "$(SRC_DIR)"/src/drivers "$(SRC_DIR)"/src/lib -name *.yaml -type f \ - -not -path "$(SRC_DIR)/src/lib/mixer_module/*" -not -path "$(SRC_DIR)/src/lib/crypto/libtommath/*" -print0 | \ + -not -path "$(SRC_DIR)/src/lib/mixer_module/*" \ + -not -path "$(SRC_DIR)/src/modules/microdds_client/dds_topics.yaml" \ + -not -path "$(SRC_DIR)/src/lib/crypto/libtommath/*" -print0 | \ xargs -0 "$(SRC_DIR)"/Tools/validate_yaml.py --schema-file "$(SRC_DIR)"/validation/module_schema.yaml # Cleanup diff --git a/README.md b/README.md index a7daa142c4..edc263b4e7 100644 --- a/README.md +++ b/README.md @@ -75,7 +75,7 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/con * [Paul Riseborough](https://github.com/priseborough) * Vision based navigation and Obstacle Avoidance * [Markus Achtelik](https://github.com/markusachtelik) - * RTPS/ROS2 Interface + * DDS/ROS2 Interface * [Nuno Marques](https://github.com/TSC21) See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github). diff --git a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt index 419ed0ed18..1ba7ccde27 100644 --- a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt @@ -36,7 +36,6 @@ add_subdirectory(airframes) px4_add_romfs_files( px4-rc.mavlink px4-rc.params - px4-rc.rtps px4-rc.simulator rc.replay rcS diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.rtps b/ROMFS/px4fmu_common/init.d-posix/px4-rc.rtps deleted file mode 100644 index d3cfa4f817..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.rtps +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/sh -# shellcheck disable=SC2154 - -micrortps_client start -t UDP -r $((2019+2*px4_instance)) -s $((2020+2*px4_instance)) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 09ae36ce4f..4e6acb9c56 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -259,11 +259,8 @@ fi navigator start -# Try to start the micrortps_client with UDP transport if module exists -if px4-micrortps_client status > /dev/null 2>&1 -then - . px4-rc.rtps -fi +# Try to start the microdds_client with UDP transport if module exists +microdds_client start -t udp -p 8888 if param greater -s MNT_MODE_IN -1 then diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 33a98569ae..6681bf84ac 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -8,7 +8,6 @@ if [ $# -gt 0 ]; then fi exec find boards msg src platforms test \ - -path msg/templates/urtps -prune -o \ -path platforms/nuttx/NuttX -prune -o \ -path platforms/qurt/dspal -prune -o \ -path src/drivers/uavcan/libuavcan -prune -o \ @@ -21,8 +20,6 @@ exec find boards msg src platforms test \ -path src/modules/ekf2/EKF -prune -o \ -path src/modules/gyro_fft/CMSIS_5 -prune -o \ -path src/modules/mavlink/mavlink -prune -o \ - -path src/modules/micrortps_bridge/micro-CDR -prune -o \ - -path src/modules/micrortps_bridge/microRTPS_client -prune -o \ -path test/mavsdk_tests/catch2 -prune -o \ -path src/lib/crypto/monocypher -prune -o \ -path src/lib/crypto/libtomcrypt -prune -o \ diff --git a/Tools/build_micrortps_agent.sh b/Tools/build_micrortps_agent.sh deleted file mode 100755 index 6d45eaeb41..0000000000 --- a/Tools/build_micrortps_agent.sh +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env bash -set -e - -SCRIPT_DIR=$0 -if [[ ${SCRIPT_DIR:0:1} != '/' ]]; then - SCRIPT_DIR=$(dirname $(realpath -s "$PWD/$0")) -fi - -PX4_DIR=$(cd "$(dirname $(dirname $SCRIPT_DIR))" && pwd) - -if [ -d $PX4_DIR/build/*_rtps ]; then - cd $PX4_DIR/build/*_rtps/src/modules/micrortps_bridge/micrortps_agent/ - cmake -Bbuild - cmake --build build -j$(nproc --all) -fi diff --git a/msg/tools/__init__.py b/Tools/msg/__init__.py similarity index 100% rename from msg/tools/__init__.py rename to Tools/msg/__init__.py diff --git a/msg/tools/generate_msg_docs.py b/Tools/msg/generate_msg_docs.py similarity index 98% rename from msg/tools/generate_msg_docs.py rename to Tools/msg/generate_msg_docs.py index 8dd961968b..cd80971586 100755 --- a/msg/tools/generate_msg_docs.py +++ b/Tools/msg/generate_msg_docs.py @@ -66,7 +66,7 @@ if __name__ == "__main__": with open(msg_filename, 'r') as source_file: msg_contents = source_file.read() - #Format markdown using msg name, comment, url, contents. + #Format markdown using msg name, comment, url, contents. markdown_output="""# %s (UORB message) %s @@ -90,7 +90,7 @@ if __name__ == "__main__": readme_text="""# uORB Message Reference :::note -This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/msg/tools/generate_msg_docs.py) from the source code. +This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/Tools/msg/generate_msg_docs.py) from the source code. ::: This topic lists the UORB messages available in PX4 (some of which may be may be shared by the [PX4-ROS 2 Bridge](../ros/ros2_comm.md)). diff --git a/Tools/msg/px_generate_uorb_topic_files.py b/Tools/msg/px_generate_uorb_topic_files.py new file mode 100755 index 0000000000..4a26cbf10a --- /dev/null +++ b/Tools/msg/px_generate_uorb_topic_files.py @@ -0,0 +1,253 @@ +#!/usr/bin/env python3 +############################################################################# +# +# Copyright (C) 2013-2022 PX4 Pro Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################# + +""" +px_generate_uorb_topic_files.py +Generates c/cpp header/source files for uorb topics from .msg +message files +""" + +import os +import argparse +import re +import sys + +try: + import em +except ImportError as e: + print("Failed to import em: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user empy") + print("") + sys.exit(1) + +try: + import genmsg.template_tools +except ImportError as e: + print("Failed to import genmsg: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user pyros-genmsg") + print("") + sys.exit(1) + + +__author__ = "Sergey Belash, Thomas Gubler, Beat Kueng" +__copyright__ = "Copyright (C) 2013-2022 PX4 Development Team." +__license__ = "BSD" +__email__ = "thomasgubler@gmail.com" + + +TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em'] +TOPICS_LIST_TEMPLATE_FILE = ['uORBTopics.hpp.em', 'uORBTopics.cpp.em'] +OUTPUT_FILE_EXT = ['.h', '.cpp'] +INCL_DEFAULT = ['std_msgs:./msg/std_msgs'] +PACKAGE = 'px4' +TOPICS_TOKEN = '# TOPICS ' + + +def get_topics(filename): + """ + Get TOPICS names from a "# TOPICS" line + """ + ofile = open(filename, 'r') + text = ofile.read() + result = [] + for each_line in text.split('\n'): + if each_line.startswith(TOPICS_TOKEN): + topic_names_str = each_line.strip() + topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "") + topic_names_list = topic_names_str.split(" ") + for topic in topic_names_list: + # topic name PascalCase (file name) to snake_case (topic name) + topic_name = re.sub(r'(?= 0): type_name = type_name[sl_pos + 1:] + type_name = re.sub(r'(?'%(name)) + name_snake = re.sub(r'(?'%(name_snake)) }@ static inline constexpr int ucdr_topic_size_@(topic)() @@ -87,11 +124,8 @@ static inline constexpr int ucdr_topic_size_@(topic)() return @(struct_size); } -bool ucdr_serialize_@(topic)(const @(uorb_struct)& topic, ucdrBuffer& buf) +bool ucdr_serialize_@(topic)(const @(uorb_struct)& topic, ucdrBuffer& buf, int64_t time_offset = 0) { - if (ucdr_buffer_remaining(&buf) < @(struct_size)) { - return false; - } @{ for field_type, field_name, field_size, padding in fields: if padding > 0: @@ -99,7 +133,18 @@ for field_type, field_name, field_size, padding in fields: print('\tbuf.offset += {:}; // padding'.format(padding)) print('\tstatic_assert(sizeof(topic.{0}) == {1}, "size mismatch");'.format(field_name, field_size)) - print('\tmemcpy(buf.iterator, &topic.{0}, sizeof(topic.{0}));'.format(field_name)) + + if field_type == 'uint64' and field_name == 'timestamp': + print('\tconst uint64_t timestamp_adjusted = topic.timestamp + time_offset;') + print('\tmemcpy(buf.iterator, ×tamp_adjusted, sizeof(topic.{0}));'.format(field_name)) + + elif field_type == 'uint64' and field_name == 'timestamp_sample': + print('\tconst uint64_t timestamp_sample_adjusted = topic.timestamp_sample + time_offset;') + print('\tmemcpy(buf.iterator, ×tamp_sample_adjusted, sizeof(topic.{0}));'.format(field_name)) + + else: + print('\tmemcpy(buf.iterator, &topic.{0}, sizeof(topic.{0}));'.format(field_name)) + print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name)) print('\tbuf.offset += sizeof(topic.{:});'.format(field_name)) @@ -107,11 +152,8 @@ for field_type, field_name, field_size, padding in fields: return true; } -bool ucdr_deserialize_@(topic)(ucdrBuffer& buf, @(uorb_struct)& topic) +bool ucdr_deserialize_@(topic)(ucdrBuffer& buf, @(uorb_struct)& topic, int64_t time_offset = 0) { - if (ucdr_buffer_remaining(&buf) < @(struct_size)) { - return false; - } @{ for field_type, field_name, field_size, padding in fields: if padding > 0: @@ -120,6 +162,10 @@ for field_type, field_name, field_size, padding in fields: print('\tstatic_assert(sizeof(topic.{0}) == {1}, "size mismatch");'.format(field_name, field_size)) print('\tmemcpy(&topic.{0}, buf.iterator, sizeof(topic.{0}));'.format(field_name)) + + if field_type == 'uint64' and (field_name == 'timestamp' or field_name == 'timestamp_sample'): + print('\ttopic.{0} -= time_offset;'.format(field_name)) + print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name)) print('\tbuf.offset += sizeof(topic.{:});'.format(field_name)) diff --git a/msg/templates/uorb/msg.cpp.em b/Tools/msg/templates/uorb/msg.cpp.em similarity index 85% rename from msg/templates/uorb/msg.cpp.em rename to Tools/msg/templates/uorb/msg.cpp.em index 03776549f5..e613d9511f 100644 --- a/msg/templates/uorb/msg.cpp.em +++ b/Tools/msg/templates/uorb/msg.cpp.em @@ -13,12 +13,11 @@ @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file @# - search_path (dict) search paths for genmsg -@# - topics (List of String) multi-topic names -@# - constrained_flash set to true if flash is constrained +@# - topics (List of String) topic names @############################################### /**************************************************************************** * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,8 +55,7 @@ import genmsg.msgs from px_generate_uorb_topic_helper import * # this is in Tools/ -uorb_struct = '%s_s'%spec.short_name -topic_name = spec.short_name +uorb_struct = '%s_s'%name_snake_case sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) @@ -67,7 +65,7 @@ topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field #include #include #include -#include +#include #include #include #include @@ -76,10 +74,10 @@ topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field @# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed" @# This is used for the logger -constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );"; +constexpr char __orb_@(name_snake_case)_fields[] = "@( ";".join(topic_fields) );"; -@[for multi_topic in topics]@ -ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(topic_name)_fields, static_cast(ORB_ID::@multi_topic)); +@[for topic in topics]@ +ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(name_snake_case)_fields, static_cast(ORB_ID::@topic)); @[end for] void print_message(const orb_metadata *meta, const @uorb_struct& message) diff --git a/msg/templates/uorb/msg.h.em b/Tools/msg/templates/uorb/msg.h.em similarity index 90% rename from msg/templates/uorb/msg.h.em rename to Tools/msg/templates/uorb/msg.h.em index 9854e1eaef..69aa1347a1 100644 --- a/msg/templates/uorb/msg.h.em +++ b/Tools/msg/templates/uorb/msg.h.em @@ -13,11 +13,11 @@ @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file @# - search_path (dict) search paths for genmsg -@# - topics (List of String) multi-topic names +@# - topics (List of String) topic names @############################################### /**************************************************************************** * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -52,12 +52,12 @@ @{ import genmsg.msgs +import re from px_generate_uorb_topic_helper import * # this is in Tools/ -uorb_struct = '%s_s'%spec.short_name -uorb_struct_upper = spec.short_name.upper() -topic_name = spec.short_name +uorb_struct = '%s_s'%name_snake_case +uorb_struct_upper = name_snake_case.upper() }@ #pragma once @@ -77,6 +77,8 @@ for field in spec.parsed_fields(): if (not field.is_header): (package, name) = genmsg.names.package_resource_name(field.base_type) package = package or spec.package # convert '' to package + + name = re.sub(r'(?'%(name)) }@ @@ -127,9 +129,17 @@ for constant in spec.constants: #endif }; +#ifdef __cplusplus +namespace px4 { + namespace msg { + using @(spec.short_name) = @(uorb_struct); + } // namespace msg +} // namespace px4 +#endif + /* register this as object request broker structure */ -@[for multi_topic in topics]@ -ORB_DECLARE(@multi_topic); +@[for topic in topics]@ +ORB_DECLARE(@topic); @[end for] #ifdef __cplusplus diff --git a/msg/templates/uorb/uORBTopics.cpp.em b/Tools/msg/templates/uorb/uORBTopics.cpp.em similarity index 84% rename from msg/templates/uorb/uORBTopics.cpp.em rename to Tools/msg/templates/uorb/uORBTopics.cpp.em index c71082e354..c8b1c88397 100644 --- a/msg/templates/uorb/uORBTopics.cpp.em +++ b/Tools/msg/templates/uorb/uORBTopics.cpp.em @@ -8,11 +8,11 @@ @# @# Context: @# - msgs (List) list of all msg files -@# - topics (List) list of all topic names +@# - multi_topics (List) list of all multi-topic names @############################################### /**************************************************************************** * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -46,19 +46,22 @@ #include #include @{ -msg_names = [mn.replace(".msg", "") for mn in msgs] +msg_names = list(set([mn.replace(".msg", "") for mn in msgs])) # set() filters duplicates +msg_names.sort() msgs_count = len(msg_names) -topics_all = topics -topics_all.sort() -topics_count_all = len(topics_all) + +topic_names = list(set(topics)) # set() filters duplicates +topic_names.sort() +topics_count = len(topics) + }@ @[for msg_name in msg_names]@ #include @[end for] const constexpr struct orb_metadata *const uorb_topics_list[ORB_TOPICS_COUNT] = { -@[for idx, topic_name in enumerate(topics_all, 1)]@ - ORB_ID(@(topic_name))@[if idx != topics_count_all], @[end if] +@[for idx, topic_name in enumerate(topic_names, 1)]@ + ORB_ID(@(topic_name))@[if idx != topic_names], @[end if] @[end for] }; diff --git a/msg/templates/uorb/uORBTopics.hpp.em b/Tools/msg/templates/uorb/uORBTopics.hpp.em similarity index 85% rename from msg/templates/uorb/uORBTopics.hpp.em rename to Tools/msg/templates/uorb/uORBTopics.hpp.em index 528b95acee..aea7015cc7 100644 --- a/msg/templates/uorb/uORBTopics.hpp.em +++ b/Tools/msg/templates/uorb/uORBTopics.hpp.em @@ -7,12 +7,11 @@ @# Start of Template @# @# Context: -@# - msgs (List) list of all msg files @# - topics (List) list of all topic names @############################################### /**************************************************************************** * - * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2021-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,11 +43,9 @@ ****************************************************************************/ @{ -msg_names = [mn.replace(".msg", "") for mn in msgs] -msgs_count = len(msg_names) -topics_all = topics -topics_all.sort() -topics_count_all = len(topics_all) +topics_count = len(topics) +topic_names_all = list(set(topics)) # set() filters duplicates +topic_names_all.sort() }@ #pragma once @@ -57,7 +54,7 @@ topics_count_all = len(topics_all) #include -static constexpr size_t ORB_TOPICS_COUNT{@(topics_count_all)}; +static constexpr size_t ORB_TOPICS_COUNT{@(topics_count)}; static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; } /* @@ -66,8 +63,8 @@ static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; } extern const struct orb_metadata *const *orb_get_topics() __EXPORT; enum class ORB_ID : uint8_t { -@[for idx, msg_name in enumerate(topics_all)]@ - @(msg_name) = @(idx), +@[for idx, topic_name in enumerate(topic_names_all)]@ + @(topic_name) = @(idx), @[end for] INVALID }; diff --git a/Tools/setup/arch.sh b/Tools/setup/arch.sh index 83bc225d49..f27d7e756f 100755 --- a/Tools/setup/arch.sh +++ b/Tools/setup/arch.sh @@ -9,8 +9,6 @@ ## - jMAVSim simulator (omit with arg: --no-sim-tools) ## - Gazebo simulator (not by default, use --gazebo) ## -## Not Installs: -## - FastRTPS and FastCDR INSTALL_NUTTX="true" INSTALL_SIM="true" @@ -122,7 +120,7 @@ if [[ $INSTALL_SIM == "true" ]]; then echo echo "Installing PX4 simulation dependencies" - # java (jmavsim or fastrtps) + # java (jmavsim) sudo pacman -S --noconfirm --needed \ ant \ jdk-openjdk \ diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index 6d1439bb6c..b17539de6f 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -10,8 +10,6 @@ set -e ## - NuttX toolchain (omit with arg: --no-nuttx) ## - jMAVSim and Gazebo9 simulator (omit with arg: --no-sim-tools) ## -## Not Installs: -## - FastRTPS and FastCDR INSTALL_NUTTX="true" INSTALL_SIM="true" @@ -216,7 +214,7 @@ if [[ $INSTALL_SIM == "true" ]]; then else java_version=14 fi - # Java (jmavsim or fastrtps) + # Java (jmavsim) sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ ant \ openjdk-$java_version-jre \ diff --git a/Tools/update_px4_ros2_bridge.sh b/Tools/update_px4_ros2_bridge.sh deleted file mode 100755 index 58ebca5b7c..0000000000 --- a/Tools/update_px4_ros2_bridge.sh +++ /dev/null @@ -1,135 +0,0 @@ -#!/usr/bin/env bash -set -e - -agent_template_files_updated=0 -code_generator_files_updated=0 - -# parse help argument -if [[ $1 == "-h" ]] || [[ $1 == "--help" ]]; then - echo -e "Usage: update_px4_ros2_bridge.bash [options...] \t This script allows to update px4_ros_com and px4_msgs in a specified directory." >&2 - echo - echo -e "\t--ws_dir \t\t Location of the ament/colcon workspace. Default: $HOME/colcon_ws." - echo -e "\t--px4_ros_com \t\t Updates px4_ros_com microRTPS agent code generation and templates." - echo -e "\t--px4_msgs \t\t Updates px4_msgs messages definition files." - echo -e "\t--all \t\t Updates both px4_ros_com and px4_msgs." - echo - exit 0 -fi - -# parse the arguments -while [ $# -gt 0 ]; do - if [[ $1 == *"--"* ]]; then - v="${1/--/}" - if [ ! -z $2 ]; then - declare $v="$2" - else - declare $v=1 - fi - fi - shift -done - -# get script directory -SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" - -# get PX4-Autopilot directory -PX4_DIR=$(cd "$(dirname "$SCRIPT_DIR")" && pwd) - -function compare_and_update () { - cmp -s "$1" "$2" - if [ $? -eq 1 ]; then - cp "$1" "$2" - return 0 - fi - - return 1; -} - -# update microRTPS agent code generators / helpers -function update_agent_code { - declare -a templates=( \ - "microRTPS_agent.cpp.em" \ - "microRTPS_timesync.cpp.em" \ - "microRTPS_timesync.h.em" \ - "microRTPS_transport.cpp" \ - "microRTPS_transport.h" \ - "Publisher.cpp.em" \ - "Publisher.h.em" \ - "Subscriber.cpp.em" \ - "Subscriber.h.em" \ - "RtpsTopics.cpp.em" \ - "RtpsTopics.h.em" \ - ) - - for file in ${templates[@]}; do - compare_and_update "$PX4_DIR/msg/templates/urtps/$file" "$ws_dir/src/px4_ros_com/templates/$file" \ - && echo -e "--\t\t- '$ws_dir/src/px4_ros_com/templates/$file' updated" && ((agent_template_files_updated+=1)) - done - if [ $agent_template_files_updated -eq 0 ]; then - echo -e "--\t\t- No template files updated" - elif [ $agent_template_files_updated -eq 1 ]; then - echo -e "--\t\t - 1 template file updated" - else - echo -e "--\t\t - $agent_template_files_updated template files updated" - fi -} - -# update microRTPS agent code templates -function update_agent_templates { - declare -a code_generators=( \ - "uorb_rtps_classifier.py" \ - "generate_microRTPS_bridge.py" \ - "px_generate_uorb_topic_files.py" \ - ) - for file in ${code_generators[@]}; do - compare_and_update "$PX4_DIR/msg/tools/$file $ws_dir/src/px4_ros_com/scripts/$file" \ - && echo -e "--\t\t- '$ws_dir/src/px4_ros_com/scripts/$file' updated" && ((code_generator_files_updated+=1)) - done - if [ $code_generator_files_updated -eq 0 ]; then - echo -e "--\t\t- No code generators / helpers files updated" - elif [ $code_generator_files_updated -eq 1 ]; then - echo -e "--\t\t - 1 code generator / helper file updated" - else - echo -e "--\t\t - $code_generator_files_updated code generator / helper files updated" - fi -} - -# update px4_ros_com files -function update_px4_ros_com { - python3 $PX4_DIR/msg/tools/uorb_to_ros_urtps_topics.py -i $PX4_DIR/msg/tools/urtps_bridge_topics.yaml -o $ws_dir/src/px4_ros_com/templates/urtps_bridge_topics.yaml - echo -e "--------------- \033[1mmicroRTPS agent code generation and templates update\033[0m ----------------" - echo "-------------------------------------------------------------------------------------------------------" - update_agent_code - update_agent_templates - return 0 -} - -# function to update px4_msgs -function update_px4_msgs { - find "$ws_dir/src/px4_msgs/msg/" -maxdepth 1 -type f -delete - python3 $PX4_DIR/msg/tools/uorb_to_ros_msgs.py $PX4_DIR/msg/ $ws_dir/src/px4_msgs/msg/ -} - -# decisor -echo "-------------------------------------------------------------------------------------------------------" -if [ -d "${ws_dir}" ]; then - ws_dir=$(cd "$ws_dir" && pwd) - if [ ! -z ${all} ]; then - update_px4_ros_com - echo "-------------------------------------------------------------------------------------------------------" - update_px4_msgs - elif [ ! -z ${px4_ros_com} ]; then - update_px4_ros_com - echo "-------------------------------------------------------------------------------------------------------" - elif [ ! -z ${px4_msgs} ]; then - update_px4_msgs - fi - echo -e "-------------------------------- \033[0;32mUpdate successful!\033[0m ---------------------------------" - echo "-------------------------------------------------------------------------------------------------------" - exit 0 -else - echo -e "-- \033[0;31mWorkspace directory doesn't exist...\033[0m" - echo -e "---------------------------------- \033[0;31mUpdate failed!\033[0m -----------------------------------" - echo "-------------------------------------------------------------------------------------------------------" - exit $ERRCODE -fi diff --git a/boards/beaglebone/blue/default.px4board b/boards/beaglebone/blue/default.px4board index e28a13c13b..edb01b3b89 100644 --- a/boards/beaglebone/blue/default.px4board +++ b/boards/beaglebone/blue/default.px4board @@ -48,6 +48,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index fe85f68e6a..1f12767b7c 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -34,7 +34,6 @@ CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y CONFIG_DRIVERS_UAVCAN=y CONFIG_MODULES_AIRSPEED_SELECTOR=y -CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y @@ -53,7 +52,6 @@ CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y -CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y @@ -63,6 +61,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/diatone/mamba-f405-mk2/default.px4board b/boards/diatone/mamba-f405-mk2/default.px4board index 7fcd4a1a81..6655e8e388 100644 --- a/boards/diatone/mamba-f405-mk2/default.px4board +++ b/boards/diatone/mamba-f405-mk2/default.px4board @@ -15,7 +15,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y CONFIG_DRIVERS_MAGNETOMETER_AKM_AK8963=y CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y -CONFIG_DRIVERS_OSD=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y diff --git a/boards/emlid/navio2/default.px4board b/boards/emlid/navio2/default.px4board index 48faec33f6..0eee7a73c3 100644 --- a/boards/emlid/navio2/default.px4board +++ b/boards/emlid/navio2/default.px4board @@ -50,6 +50,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 455d95d091..c4d16ff985 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -29,6 +29,7 @@ CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 629c1a557f..9e53c4a38d 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -56,6 +56,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/matek/h743-mini/default.px4board b/boards/matek/h743-mini/default.px4board index a9069bb741..e654a21f97 100644 --- a/boards/matek/h743-mini/default.px4board +++ b/boards/matek/h743-mini/default.px4board @@ -53,6 +53,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/matek/h743-slim/default.px4board b/boards/matek/h743-slim/default.px4board index bcff78646b..06c0cd20c3 100644 --- a/boards/matek/h743-slim/default.px4board +++ b/boards/matek/h743-slim/default.px4board @@ -52,6 +52,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/matek/h743/default.px4board b/boards/matek/h743/default.px4board index 87395dd23d..0e3b91b463 100644 --- a/boards/matek/h743/default.px4board +++ b/boards/matek/h743/default.px4board @@ -53,6 +53,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index dbf4338931..8c3e771211 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -66,6 +66,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/modalai/fc-v1/rtps.px4board b/boards/modalai/fc-v1/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/modalai/fc-v1/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index a49105cf96..104d5e9947 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -36,7 +36,6 @@ CONFIG_DRIVERS_UAVCAN=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 CONFIG_MODULES_AIRSPEED_SELECTOR=y -CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y @@ -54,7 +53,6 @@ CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y -CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y @@ -64,6 +62,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/mro/ctrl-zero-classic/default.px4board b/boards/mro/ctrl-zero-classic/default.px4board index d592c791f4..f9bcd91e0a 100644 --- a/boards/mro/ctrl-zero-classic/default.px4board +++ b/boards/mro/ctrl-zero-classic/default.px4board @@ -62,6 +62,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/mro/ctrl-zero-h7-oem/default.px4board b/boards/mro/ctrl-zero-h7-oem/default.px4board index 5c64e1af3c..9fa47c2e4b 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.px4board +++ b/boards/mro/ctrl-zero-h7-oem/default.px4board @@ -62,6 +62,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/mro/ctrl-zero-h7-oem/rtps.px4board b/boards/mro/ctrl-zero-h7-oem/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/mro/ctrl-zero-h7-oem/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/mro/ctrl-zero-h7/default.px4board b/boards/mro/ctrl-zero-h7/default.px4board index 5fc4ad7dd8..11f35c83b1 100644 --- a/boards/mro/ctrl-zero-h7/default.px4board +++ b/boards/mro/ctrl-zero-h7/default.px4board @@ -63,6 +63,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/mro/ctrl-zero-h7/rtps.px4board b/boards/mro/ctrl-zero-h7/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/mro/ctrl-zero-h7/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/mro/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board index f3c6137964..72b170ffcb 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/default.px4board @@ -62,6 +62,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/mro/pixracerpro/rtps.px4board b/boards/mro/pixracerpro/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/mro/pixracerpro/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board index 3772be3f2b..f0d2e8fe15 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/default.px4board @@ -41,6 +41,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y @@ -49,6 +50,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y @@ -64,7 +66,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y -CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y @@ -75,7 +77,6 @@ CONFIG_MODULES_SIMULATION_SENSOR_GPS_SIM=y CONFIG_MODULES_SIMULATION_SENSOR_MAG_SIM=y CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y -CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_I2CDETECT=y diff --git a/boards/nxp/fmuk66-e/rtps.px4board b/boards/nxp/fmuk66-e/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/nxp/fmuk66-e/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board index 63032fb003..40ef605364 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -68,6 +68,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/nxp/fmuk66-v3/rtps.px4board b/boards/nxp/fmuk66-v3/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/nxp/fmuk66-v3/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/nxp/fmurt1062-v1/default.px4board b/boards/nxp/fmurt1062-v1/default.px4board index cea42ef564..bdc42a1cc6 100644 --- a/boards/nxp/fmurt1062-v1/default.px4board +++ b/boards/nxp/fmurt1062-v1/default.px4board @@ -45,6 +45,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board index 3c5dacf2f4..3de33fb7eb 100644 --- a/boards/omnibus/f4sd/default.px4board +++ b/boards/omnibus/f4sd/default.px4board @@ -29,6 +29,7 @@ CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/px4/fmu-v4/rtps.px4board b/boards/px4/fmu-v4/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/px4/fmu-v4/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/px4/fmu-v5/cryptotest.px4board b/boards/px4/fmu-v5/cryptotest.px4board index 274e5b45ca..c023a5cb75 100644 --- a/boards/px4/fmu-v5/cryptotest.px4board +++ b/boards/px4/fmu-v5/cryptotest.px4board @@ -1,5 +1,7 @@ +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_BOARD_CRYPTO=y -CONFIG_DRIVERS_SW_CRYPTO=y CONFIG_DRIVERS_STUB_KEYSTORE=y +CONFIG_DRIVERS_SW_CRYPTO=y CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub" diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index 0ac4351440..7d3514675c 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -73,6 +73,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/px4/fmu-v5/protected.px4board b/boards/px4/fmu-v5/protected.px4board index d51a7314d5..284cb67dbc 100644 --- a/boards/px4/fmu-v5/protected.px4board +++ b/boards/px4/fmu-v5/protected.px4board @@ -4,9 +4,13 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_COMMON_OPTICAL_FLOW=n CONFIG_COMMON_TELEMETRY=n -CONFIG_DRIVERS_OSD=n +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_PWM_INPUT=n CONFIG_DRIVERS_ROBOCLAW=n -CONFIG_DRIVERS_RPM=n CONFIG_DRIVERS_UAVCAN=n CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_AIRSPEED_SELECTOR=n @@ -17,8 +21,8 @@ CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_FW_POS_CONTROL_L1=n CONFIG_MODULES_GYRO_FFT=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_MODULES_ROVER_POS_CONTROL=n -CONFIG_MODULES_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=n CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n @@ -30,7 +34,6 @@ CONFIG_SYSTEMCMDS_DUMPFILE=n CONFIG_SYSTEMCMDS_GPIO=n CONFIG_SYSTEMCMDS_I2CDETECT=n CONFIG_SYSTEMCMDS_LED_CONTROL=n -CONFIG_SYSTEMCMDS_MOTOR_TEST=n CONFIG_SYSTEMCMDS_MTD=n CONFIG_SYSTEMCMDS_NSHTERM=n CONFIG_SYSTEMCMDS_REFLECT=n diff --git a/boards/px4/fmu-v5/rtps.px4board b/boards/px4/fmu-v5/rtps.px4board deleted file mode 100644 index e77de930f2..0000000000 --- a/boards/px4/fmu-v5/rtps.px4board +++ /dev/null @@ -1,3 +0,0 @@ -CONFIG_DRIVERS_HEATER=n -CONFIG_DRIVERS_OSD=n -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board index 213d5767a8..e6cc948fde 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -1,43 +1,43 @@ # CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set +CONFIG_COMMON_BAROMETERS=n +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n +CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_COMMON_HYGROMETERS=n CONFIG_COMMON_TELEMETRY=n +CONFIG_DRIVERS_ADC_ADS1115=n CONFIG_DRIVERS_BATT_SMBUS=n CONFIG_DRIVERS_CAMERA_CAPTURE=n CONFIG_DRIVERS_CAMERA_TRIGGER=n -CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n -CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_DRIVERS_HEATER=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=n CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n CONFIG_DRIVERS_IRLOCK=n -CONFIG_DRIVERS_OSD=n CONFIG_DRIVERS_PCA9685=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n CONFIG_DRIVERS_POWER_MONITOR_INA226=n CONFIG_DRIVERS_PWM_INPUT=n -CONFIG_DRIVERS_PWM_OUT_SIM=n CONFIG_DRIVERS_ROBOCLAW=n -CONFIG_DRIVERS_RPM=n CONFIG_DRIVERS_SMART_BATTERY_BATMON=n CONFIG_DRIVERS_UAVCAN=n CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n CONFIG_MODULES_CAMERA_FEEDBACK=n CONFIG_MODULES_ESC_BATTERY=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_GIMBAL=n CONFIG_MODULES_GYRO_CALIBRATION=n CONFIG_MODULES_GYRO_FFT=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_ROVER_POS_CONTROL=n -CONFIG_MODULES_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=n CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n -CONFIG_SYSTEMCMDS_MOTOR_TEST=n CONFIG_SYSTEMCMDS_REFLECT=n CONFIG_SYSTEMCMDS_SERIAL_TEST=n CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_BAROMETER_MS5611=y diff --git a/boards/px4/fmu-v5/uavcanv0periph.px4board b/boards/px4/fmu-v5/uavcanv0periph.px4board index 59d08a9b56..9578fb422a 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.px4board +++ b/boards/px4/fmu-v5/uavcanv0periph.px4board @@ -4,8 +4,8 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_HYGROMETERS=n CONFIG_COMMON_TELEMETRY=n CONFIG_DRIVERS_ADC_ADS1115=n -CONFIG_DRIVERS_HEATER=n CONFIG_DRIVERS_GPS=n +CONFIG_DRIVERS_HEATER=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n CONFIG_DRIVERS_IRLOCK=n @@ -14,15 +14,14 @@ CONFIG_DRIVERS_PCA9685=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n CONFIG_DRIVERS_PWM_INPUT=n CONFIG_DRIVERS_ROBOCLAW=n -CONFIG_DRIVERS_RPM=n CONFIG_DRIVERS_SMART_BATTERY_BATMON=n CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n CONFIG_MODULES_ESC_BATTERY=n CONFIG_MODULES_GYRO_FFT=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_MICRODDS_CLIENT=n CONFIG_MODULES_ROVER_POS_CONTROL=n -CONFIG_MODULES_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=n CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index f0ff95bb6d..09b04fbcd6 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -9,7 +9,8 @@ CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" CONFIG_BOARD_SERIAL_PPB="/dev/ttyS3" CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y -CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y @@ -30,12 +31,9 @@ CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y CONFIG_COMMON_OSD=y -CONFIG_DRIVERS_PCA9685=y -CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y -CONFIG_DRIVERS_PWM_INPUT=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_COMMON_RC=y @@ -76,6 +74,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y @@ -88,7 +87,6 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_DMESG=y -CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_GPIO=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_I2CDETECT=y @@ -103,7 +101,6 @@ CONFIG_SYSTEMCMDS_REBOOT=y CONFIG_SYSTEMCMDS_REFLECT=y CONFIG_SYSTEMCMDS_SD_BENCH=y CONFIG_SYSTEMCMDS_SD_STRESS=y -CONFIG_SYSTEMCMDS_SERIAL_TEST=y CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y @@ -112,4 +109,3 @@ CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y -CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/px4/fmu-v5x/rtps.px4board b/boards/px4/fmu-v5x/rtps.px4board deleted file mode 100644 index 272e47aa57..0000000000 --- a/boards/px4/fmu-v5x/rtps.px4board +++ /dev/null @@ -1,15 +0,0 @@ -CONFIG_COMMON_BAROMETERS=n -CONFIG_COMMON_TELEMETRY=n -CONFIG_DRIVERS_OSD=n -CONFIG_EXAMPLES_FAKE_GPS=n -CONFIG_SYSTEMCMDS_ACTUATOR_TEST=n -CONFIG_SYSTEMCMDS_DUMPFILE=n -CONFIG_SYSTEMCMDS_GPIO=n -CONFIG_SYSTEMCMDS_MOTOR_TEST=n -CONFIG_SYSTEMCMDS_REFLECT=n -CONFIG_SYSTEMCMDS_SD_BENCH=n -CONFIG_SYSTEMCMDS_SD_STRESS=n -CONFIG_SYSTEMCMDS_SERIAL_TEST=n -CONFIG_DRIVERS_BAROMETER_BMP388=y -CONFIG_DRIVERS_BAROMETER_MS5611=y -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/px4/fmu-v5x/test.px4board b/boards/px4/fmu-v5x/test.px4board index 5279a489dc..1c71c5afbc 100644 --- a/boards/px4/fmu-v5x/test.px4board +++ b/boards/px4/fmu-v5x/test.px4board @@ -1,13 +1,11 @@ CONFIG_DRIVERS_ADC_ADS1115=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IRLOCK=n -CONFIG_DRIVERS_PCA9685=n -CONFIG_DRIVERS_PCA9685_PWM_OUT=n -CONFIG_DRIVERS_RPM=n -CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_GYRO_FFT=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_BOARD_TESTING=y CONFIG_DRIVERS_TEST_PPM=y diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index 9f550cf3c0..ff3292e15d 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -57,6 +57,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 992238ede2..ef2ff976cb 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -8,8 +8,8 @@ CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y -CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y +CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y @@ -24,7 +24,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y -CONFIG_DRIVERS_MAGNETOMETER_RM3100=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y @@ -53,7 +52,6 @@ CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y -CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y @@ -66,11 +64,10 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y -CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y -CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_VTOL_ATT_CONTROL=y @@ -87,7 +84,6 @@ CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y -CONFIG_SYSTEMCMDS_SD_BENCH=y CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 32bc968c23..1dcc9d36ca 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -35,6 +35,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y diff --git a/boards/px4/sitl/rtps.px4board b/boards/px4/sitl/rtps.px4board deleted file mode 100644 index 81e62c7825..0000000000 --- a/boards/px4/sitl/rtps.px4board +++ /dev/null @@ -1 +0,0 @@ -CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/sky-drones/smartap-airlink/default.px4board b/boards/sky-drones/smartap-airlink/default.px4board index 943191213e..6d4ae08ccf 100644 --- a/boards/sky-drones/smartap-airlink/default.px4board +++ b/boards/sky-drones/smartap-airlink/default.px4board @@ -69,6 +69,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/spracing/h7extreme/default.px4board b/boards/spracing/h7extreme/default.px4board index 75ef687ce5..f44aeac552 100644 --- a/boards/spracing/h7extreme/default.px4board +++ b/boards/spracing/h7extreme/default.px4board @@ -37,6 +37,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/msg/action_request.msg b/msg/ActionRequest.msg similarity index 100% rename from msg/action_request.msg rename to msg/ActionRequest.msg diff --git a/msg/actuator_armed.msg b/msg/ActuatorArmed.msg similarity index 100% rename from msg/actuator_armed.msg rename to msg/ActuatorArmed.msg diff --git a/msg/actuator_controls.msg b/msg/ActuatorControls.msg similarity index 88% rename from msg/actuator_controls.msg rename to msg/ActuatorControls.msg index 010e72790d..5a183a2cdc 100644 --- a/msg/actuator_controls.msg +++ b/msg/ActuatorControls.msg @@ -20,5 +20,5 @@ uint8 GROUP_INDEX_GIMBAL = 2 uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[9] control -# TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 +# TOPICS actuator_controls_0 actuator_controls_1 actuator_controls_2 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc diff --git a/msg/actuator_controls_status.msg b/msg/ActuatorControlsStatus.msg similarity index 66% rename from msg/actuator_controls_status.msg rename to msg/ActuatorControlsStatus.msg index 4e3bf83450..287fac8a4d 100644 --- a/msg/actuator_controls_status.msg +++ b/msg/ActuatorControlsStatus.msg @@ -7,4 +7,4 @@ uint8 INDEX_THROTTLE = 3 float32[4] control_power -# TOPICS actuator_controls_status actuator_controls_status_0 actuator_controls_status_1 +# TOPICS actuator_controls_status_0 actuator_controls_status_1 diff --git a/msg/actuator_motors.msg b/msg/ActuatorMotors.msg similarity index 100% rename from msg/actuator_motors.msg rename to msg/ActuatorMotors.msg diff --git a/msg/actuator_outputs.msg b/msg/ActuatorOutputs.msg similarity index 100% rename from msg/actuator_outputs.msg rename to msg/ActuatorOutputs.msg diff --git a/msg/actuator_servos.msg b/msg/ActuatorServos.msg similarity index 100% rename from msg/actuator_servos.msg rename to msg/ActuatorServos.msg diff --git a/msg/actuator_servos_trim.msg b/msg/ActuatorServosTrim.msg similarity index 100% rename from msg/actuator_servos_trim.msg rename to msg/ActuatorServosTrim.msg diff --git a/msg/actuator_test.msg b/msg/ActuatorTest.msg similarity index 100% rename from msg/actuator_test.msg rename to msg/ActuatorTest.msg diff --git a/msg/adc_report.msg b/msg/AdcReport.msg similarity index 100% rename from msg/adc_report.msg rename to msg/AdcReport.msg diff --git a/msg/airspeed.msg b/msg/Airspeed.msg similarity index 100% rename from msg/airspeed.msg rename to msg/Airspeed.msg diff --git a/msg/airspeed_validated.msg b/msg/AirspeedValidated.msg similarity index 100% rename from msg/airspeed_validated.msg rename to msg/AirspeedValidated.msg diff --git a/msg/airspeed_wind.msg b/msg/AirspeedWind.msg similarity index 100% rename from msg/airspeed_wind.msg rename to msg/AirspeedWind.msg diff --git a/msg/autotune_attitude_control_status.msg b/msg/AutotuneAttitudeControlStatus.msg similarity index 100% rename from msg/autotune_attitude_control_status.msg rename to msg/AutotuneAttitudeControlStatus.msg diff --git a/msg/battery_status.msg b/msg/BatteryStatus.msg similarity index 100% rename from msg/battery_status.msg rename to msg/BatteryStatus.msg diff --git a/msg/button_event.msg b/msg/ButtonEvent.msg similarity index 85% rename from msg/button_event.msg rename to msg/ButtonEvent.msg index bc745b9017..bbca356aab 100644 --- a/msg/button_event.msg +++ b/msg/ButtonEvent.msg @@ -3,4 +3,4 @@ bool triggered # Set to true if the event is triggered # TOPICS button_event safety_button -uint8 ORB_QUEUE_LENGTH = 2 \ No newline at end of file +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index d459488399..756f2f454c 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# Copyright (c) 2016-2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,223 +37,188 @@ cmake_policy(SET CMP0057 NEW) include(px4_list_make_absolute) set(msg_files - action_request.msg - actuator_armed.msg - actuator_controls.msg - actuator_controls_status.msg - actuator_motors.msg - actuator_outputs.msg - actuator_servos.msg - actuator_servos_trim.msg - actuator_test.msg - adc_report.msg - airspeed.msg - airspeed_validated.msg - airspeed_wind.msg - autotune_attitude_control_status.msg - battery_status.msg - button_event.msg - camera_capture.msg - camera_status.msg - camera_trigger.msg - cellular_status.msg - collision_constraints.msg - collision_report.msg - control_allocator_status.msg - cpuload.msg - differential_pressure.msg - distance_sensor.msg - ekf2_timestamps.msg - esc_report.msg - esc_status.msg - estimator_aid_source_1d.msg - estimator_aid_source_2d.msg - estimator_aid_source_3d.msg - estimator_bias.msg - estimator_event_flags.msg - estimator_gps_status.msg - estimator_innovations.msg - estimator_selector_status.msg - estimator_sensor_bias.msg - estimator_states.msg - estimator_status.msg - estimator_status_flags.msg - event.msg - failure_detector_status.msg - failsafe_flags.msg - follow_target.msg - follow_target_estimator.msg - follow_target_status.msg - generator_status.msg - geofence_result.msg - gimbal_device_attitude_status.msg - gimbal_device_information.msg - gimbal_device_set_attitude.msg - gimbal_manager_information.msg - gimbal_manager_set_attitude.msg - gimbal_manager_set_manual_control.msg - gimbal_manager_status.msg - gps_dump.msg - gps_inject_data.msg - health_report.msg - gripper.msg - heater_status.msg - home_position.msg - hover_thrust_estimate.msg - input_rc.msg - internal_combustion_engine_status.msg - iridiumsbd_status.msg - irlock_report.msg - landing_gear.msg - landing_target_innovations.msg - landing_target_pose.msg - led_control.msg - log_message.msg - logger_status.msg - mag_worker_data.msg - magnetometer_bias_estimate.msg - manual_control_setpoint.msg - manual_control_switches.msg - mavlink_log.msg - mavlink_tunnel.msg - mission.msg - mission_result.msg - mount_orientation.msg - navigator_mission_item.msg - npfg_status.msg - obstacle_distance.msg - offboard_control_mode.msg - onboard_computer_status.msg - orbit_status.msg - parameter_update.msg - ping.msg - position_controller_landing_status.msg - position_controller_status.msg - position_setpoint.msg - position_setpoint_triplet.msg - power_button_state.msg - power_monitor.msg - pps_capture.msg - pwm_input.msg - px4io_status.msg - radio_status.msg - rate_ctrl_status.msg - rc_channels.msg - rc_parameter_map.msg - rpm.msg - rtl_time_estimate.msg - satellite_info.msg - sensor_accel.msg - sensor_accel_fifo.msg - sensor_baro.msg - sensor_combined.msg - sensor_correction.msg - sensor_gnss_relative.msg - sensor_gps.msg - sensor_gyro.msg - sensor_gyro_fft.msg - sensor_gyro_fifo.msg - sensor_hygrometer.msg - sensor_mag.msg - sensor_optical_flow.msg - sensor_preflight_mag.msg - sensor_selection.msg - sensors_status.msg - sensors_status_imu.msg - system_power.msg - takeoff_status.msg - task_stack_info.msg - tecs_status.msg - telemetry_status.msg - timesync.msg - timesync_status.msg - trajectory_bezier.msg - trajectory_setpoint.msg - trajectory_waypoint.msg - transponder_report.msg - tune_control.msg - uavcan_parameter_request.msg - uavcan_parameter_value.msg - ulog_stream.msg - ulog_stream_ack.msg - uwb_distance.msg - uwb_grid.msg - vehicle_acceleration.msg - vehicle_air_data.msg - vehicle_angular_acceleration.msg - vehicle_angular_acceleration_setpoint.msg - vehicle_angular_velocity.msg - vehicle_attitude.msg - vehicle_attitude_setpoint.msg - vehicle_command.msg - vehicle_command_ack.msg - vehicle_constraints.msg - vehicle_control_mode.msg - vehicle_global_position.msg - vehicle_imu.msg - vehicle_imu_status.msg - vehicle_land_detected.msg - vehicle_local_position.msg - vehicle_local_position_setpoint.msg - vehicle_magnetometer.msg - vehicle_odometry.msg - vehicle_optical_flow.msg - vehicle_optical_flow_vel.msg - vehicle_rates_setpoint.msg - vehicle_roi.msg - vehicle_status.msg - vehicle_thrust_setpoint.msg - vehicle_torque_setpoint.msg - vehicle_trajectory_bezier.msg - vehicle_trajectory_waypoint.msg - vtol_vehicle_status.msg - wheel_encoders.msg - wind.msg - yaw_estimator_status.msg + ActionRequest.msg + ActuatorArmed.msg + ActuatorControls.msg + ActuatorControlsStatus.msg + ActuatorMotors.msg + ActuatorOutputs.msg + ActuatorServos.msg + ActuatorServosTrim.msg + ActuatorTest.msg + AdcReport.msg + Airspeed.msg + AirspeedValidated.msg + AirspeedWind.msg + AutotuneAttitudeControlStatus.msg + BatteryStatus.msg + ButtonEvent.msg + CameraCapture.msg + CameraStatus.msg + CameraTrigger.msg + CellularStatus.msg + CollisionConstraints.msg + CollisionReport.msg + ControlAllocatorStatus.msg + Cpuload.msg + DebugArray.msg + DebugKeyValue.msg + DebugValue.msg + DebugVect.msg + DifferentialPressure.msg + DistanceSensor.msg + Ekf2Timestamps.msg + EscReport.msg + EscStatus.msg + EstimatorAidSource1d.msg + EstimatorAidSource2d.msg + EstimatorAidSource3d.msg + EstimatorBias.msg + EstimatorEventFlags.msg + EstimatorGpsStatus.msg + EstimatorInnovations.msg + EstimatorSelectorStatus.msg + EstimatorSensorBias.msg + EstimatorStates.msg + EstimatorStatus.msg + EstimatorStatusFlags.msg + Event.msg + FailsafeFlags.msg + FailureDetectorStatus.msg + FollowTarget.msg + FollowTargetEstimator.msg + FollowTargetStatus.msg + GeneratorStatus.msg + GeofenceResult.msg + GimbalDeviceAttitudeStatus.msg + GimbalDeviceInformation.msg + GimbalDeviceSetAttitude.msg + GimbalManagerInformation.msg + GimbalManagerSetAttitude.msg + GimbalManagerSetManualControl.msg + GimbalManagerStatus.msg + GpsDump.msg + GpsInjectData.msg + Gripper.msg + HealthReport.msg + HeaterStatus.msg + HomePosition.msg + HoverThrustEstimate.msg + InputRc.msg + InternalCombustionEngineStatus.msg + IridiumsbdStatus.msg + IrlockReport.msg + LandingGear.msg + LandingTargetInnovations.msg + LandingTargetPose.msg + LedControl.msg + LoggerStatus.msg + LogMessage.msg + MagnetometerBiasEstimate.msg + MagWorkerData.msg + ManualControlSetpoint.msg + ManualControlSwitches.msg + MavlinkLog.msg + MavlinkTunnel.msg + Mission.msg + MissionResult.msg + MountOrientation.msg + NavigatorMissionItem.msg + NpfgStatus.msg + ObstacleDistance.msg + OffboardControlMode.msg + OnboardComputerStatus.msg + OrbitStatus.msg + OrbTest.msg + OrbTestLarge.msg + OrbTestMedium.msg + ParameterUpdate.msg + Ping.msg + PositionControllerLandingStatus.msg + PositionControllerStatus.msg + PositionSetpoint.msg + PositionSetpointTriplet.msg + PowerButtonState.msg + PowerMonitor.msg + PpsCapture.msg + PwmInput.msg + Px4ioStatus.msg + RadioStatus.msg + RateCtrlStatus.msg + RcChannels.msg + RcParameterMap.msg + Rpm.msg + RtlTimeEstimate.msg + SatelliteInfo.msg + SensorAccel.msg + SensorAccelFifo.msg + SensorBaro.msg + SensorCombined.msg + SensorCorrection.msg + SensorGnssRelative.msg + SensorGps.msg + SensorGyro.msg + SensorGyroFft.msg + SensorGyroFifo.msg + SensorHygrometer.msg + SensorMag.msg + SensorOpticalFlow.msg + SensorPreflightMag.msg + SensorSelection.msg + SensorsStatus.msg + SensorsStatusImu.msg + SystemPower.msg + TakeoffStatus.msg + TaskStackInfo.msg + TecsStatus.msg + TelemetryStatus.msg + TimesyncStatus.msg + TrajectoryBezier.msg + TrajectorySetpoint.msg + TrajectoryWaypoint.msg + TransponderReport.msg + TuneControl.msg + UavcanParameterRequest.msg + UavcanParameterValue.msg + UlogStream.msg + UlogStreamAck.msg + UwbDistance.msg + UwbGrid.msg + VehicleAcceleration.msg + VehicleAirData.msg + VehicleAngularAcceleration.msg + VehicleAngularAccelerationSetpoint.msg + VehicleAngularVelocity.msg + VehicleAttitude.msg + VehicleAttitudeSetpoint.msg + VehicleCommand.msg + VehicleCommandAck.msg + VehicleConstraints.msg + VehicleControlMode.msg + VehicleGlobalPosition.msg + VehicleImu.msg + VehicleImuStatus.msg + VehicleLandDetected.msg + VehicleLocalPosition.msg + VehicleLocalPositionSetpoint.msg + VehicleMagnetometer.msg + VehicleOdometry.msg + VehicleOpticalFlow.msg + VehicleOpticalFlowVel.msg + VehicleRatesSetpoint.msg + VehicleRoi.msg + VehicleStatus.msg + VehicleThrustSetpoint.msg + VehicleTorqueSetpoint.msg + VehicleTrajectoryBezier.msg + VehicleTrajectoryWaypoint.msg + VtolVehicleStatus.msg + WheelEncoders.msg + Wind.msg + YawEstimatorStatus.msg ) - -if(NOT px4_constrained_flash_build) - list(APPEND msg_files - debug_array.msg - debug_key_value.msg - debug_value.msg - debug_vect.msg - ) -endif() - -if(PX4_TESTING) - list(APPEND msg_files - orb_test.msg - orb_test_large.msg - orb_test_medium.msg - ) -endif() - list(SORT msg_files) -set(deprecated_msgs - ekf2_innovations.msg # 2019-11-22, Updated estimator interface and logging; replaced by 'estimator_innovations'. - ) - -foreach(msg IN LISTS deprecated_msgs) - if(msg IN_LIST msg_files) - get_filename_component(msg_we ${msg} NAME_WE) - list(APPEND invalid_msgs ${msg_we}) - endif() -endforeach() -if(invalid_msgs) - list(LENGTH invalid_msgs invalid_msgs_size) - if(${invalid_msgs_size} GREATER 1) - foreach(msg IN LISTS invalid_msgs) - string(CONCAT invalid_msgs_cs ${invalid_msgs_cs} "'${msg}', ") - endforeach() - STRING(REGEX REPLACE ", +$" "" invalid_msgs_cs ${invalid_msgs_cs}) - message(FATAL_ERROR "${invalid_msgs_cs} are listed as deprecated. Please use different names for the messages.") - else() - message(FATAL_ERROR "'${invalid_msgs}' is listed as deprecated. Please use a different name for the message.") - endif() -endif() - px4_list_make_absolute(msg_files ${CMAKE_CURRENT_SOURCE_DIR} ${msg_files}) if(NOT EXTERNAL_MODULES_LOCATION STREQUAL "") @@ -271,63 +236,64 @@ endif() # headers set(msg_out_path ${PX4_BINARY_DIR}/uORB/topics) set(ucdr_out_path ${PX4_BINARY_DIR}/uORB/ucdr) -set(msg_source_out_path ${CMAKE_CURRENT_BINARY_DIR}/topics_sources) +set(msg_source_out_path ${CMAKE_CURRENT_BINARY_DIR}/topics_sources) -set(uorb_headers ${msg_out_path}/uORBTopics.hpp) -set(uorb_sources ${msg_source_out_path}/uORBTopics.cpp) +set(uorb_headers) +set(uorb_sources) set(uorb_ucdr_headers) foreach(msg_file ${msg_files}) get_filename_component(msg ${msg_file} NAME_WE) + + # Pascal case to snake case (MsgFile -> msg_file) + string(REGEX REPLACE "(.)([A-Z][a-z]+)" "\\1_\\2" msg "${msg}") + string(REGEX REPLACE "([a-z0-9])([A-Z])" "\\1_\\2" msg "${msg}") + string(TOLOWER "${msg}" msg) + list(APPEND uorb_headers ${msg_out_path}/${msg}.h) list(APPEND uorb_sources ${msg_source_out_path}/${msg}.cpp) list(APPEND uorb_ucdr_headers ${ucdr_out_path}/${msg}.h) endforeach() -if (px4_constrained_flash_build) - set(added_arguments --constrained-flash) -endif() - # set parent scope msg_files for other modules to consume (eg topic_listener) set(msg_files ${msg_files} PARENT_SCOPE) # Generate uORB headers -add_custom_command(OUTPUT ${uorb_headers} - COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py +add_custom_command( + OUTPUT + ${uorb_headers} + ${msg_out_path}/uORBTopics.hpp + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --headers -f ${msg_files} -i ${CMAKE_CURRENT_SOURCE_DIR} -o ${msg_out_path} - -e templates/uorb - -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/headers - -q - ${added_arguments} + -e ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb DEPENDS ${msg_files} - templates/uorb/msg.h.em - templates/uorb/uORBTopics.hpp.em - tools/px_generate_uorb_topic_files.py - tools/px_generate_uorb_topic_helper.py + ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb/msg.h.em + ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb/uORBTopics.hpp.em + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_helper.py COMMENT "Generating uORB topic headers" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} VERBATIM ) add_custom_target(uorb_headers DEPENDS ${uorb_headers}) -add_custom_command(OUTPUT ${uorb_ucdr_headers} - COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py +# Generate microcdr headers +add_custom_command( + OUTPUT ${uorb_ucdr_headers} + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --headers -f ${msg_files} -i ${CMAKE_CURRENT_SOURCE_DIR} -o ${ucdr_out_path} - -e templates/ucdr - -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/ucdr_headers - -q - ${added_arguments} + -e ${PX4_SOURCE_DIR}/Tools/msg/templates/ucdr DEPENDS ${msg_files} - templates/ucdr/msg.h.em - tools/px_generate_uorb_topic_files.py - tools/px_generate_uorb_topic_helper.py + ${PX4_SOURCE_DIR}/Tools/msg/templates/ucdr/msg.h.em + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_helper.py COMMENT "Generating uORB topic ucdr headers" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} VERBATIM @@ -335,26 +301,27 @@ add_custom_command(OUTPUT ${uorb_ucdr_headers} add_custom_target(uorb_ucdr_headers DEPENDS ${uorb_ucdr_headers}) # Generate uORB sources -add_custom_command(OUTPUT ${uorb_sources} - COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py +add_custom_command( + OUTPUT + ${uorb_sources} + ${msg_source_out_path}/uORBTopics.cpp + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --sources -f ${msg_files} -i ${CMAKE_CURRENT_SOURCE_DIR} -o ${msg_source_out_path} - -e templates/uorb - -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/sources - -q - ${added_arguments} + -e ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb DEPENDS ${msg_files} - templates/uorb/msg.cpp.em - templates/uorb/uORBTopics.cpp.em - tools/px_generate_uorb_topic_files.py - tools/px_generate_uorb_topic_helper.py + ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb/msg.cpp.em + ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb/uORBTopics.cpp.em + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_helper.py COMMENT "Generating uORB topic sources" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} VERBATIM ) -add_library(uorb_msgs ${uorb_sources}) +add_library(uorb_msgs ${uorb_headers} ${msg_out_path}/uORBTopics.hpp ${uorb_sources} ${msg_source_out_path}/uORBTopics.cpp) +target_link_libraries(uorb_msgs PRIVATE m) add_dependencies(uorb_msgs prebuild_targets uorb_headers) diff --git a/msg/camera_capture.msg b/msg/CameraCapture.msg similarity index 100% rename from msg/camera_capture.msg rename to msg/CameraCapture.msg diff --git a/msg/camera_status.msg b/msg/CameraStatus.msg similarity index 100% rename from msg/camera_status.msg rename to msg/CameraStatus.msg diff --git a/msg/camera_trigger.msg b/msg/CameraTrigger.msg similarity index 89% rename from msg/camera_trigger.msg rename to msg/CameraTrigger.msg index 0da1dd109d..abfdac6dda 100644 --- a/msg/camera_trigger.msg +++ b/msg/CameraTrigger.msg @@ -5,5 +5,3 @@ uint32 seq # Image sequence number bool feedback # Trigger feedback from camera uint32 ORB_QUEUE_LENGTH = 2 - -# TOPICS camera_trigger \ No newline at end of file diff --git a/msg/cellular_status.msg b/msg/CellularStatus.msg similarity index 100% rename from msg/cellular_status.msg rename to msg/CellularStatus.msg diff --git a/msg/collision_constraints.msg b/msg/CollisionConstraints.msg similarity index 100% rename from msg/collision_constraints.msg rename to msg/CollisionConstraints.msg diff --git a/msg/collision_report.msg b/msg/CollisionReport.msg similarity index 100% rename from msg/collision_report.msg rename to msg/CollisionReport.msg diff --git a/msg/control_allocator_status.msg b/msg/ControlAllocatorStatus.msg similarity index 100% rename from msg/control_allocator_status.msg rename to msg/ControlAllocatorStatus.msg diff --git a/msg/cpuload.msg b/msg/Cpuload.msg similarity index 100% rename from msg/cpuload.msg rename to msg/Cpuload.msg diff --git a/msg/debug_array.msg b/msg/DebugArray.msg similarity index 100% rename from msg/debug_array.msg rename to msg/DebugArray.msg diff --git a/msg/debug_key_value.msg b/msg/DebugKeyValue.msg similarity index 100% rename from msg/debug_key_value.msg rename to msg/DebugKeyValue.msg diff --git a/msg/debug_value.msg b/msg/DebugValue.msg similarity index 100% rename from msg/debug_value.msg rename to msg/DebugValue.msg diff --git a/msg/debug_vect.msg b/msg/DebugVect.msg similarity index 100% rename from msg/debug_vect.msg rename to msg/DebugVect.msg diff --git a/msg/differential_pressure.msg b/msg/DifferentialPressure.msg similarity index 100% rename from msg/differential_pressure.msg rename to msg/DifferentialPressure.msg diff --git a/msg/distance_sensor.msg b/msg/DistanceSensor.msg similarity index 100% rename from msg/distance_sensor.msg rename to msg/DistanceSensor.msg diff --git a/msg/ekf2_timestamps.msg b/msg/Ekf2Timestamps.msg similarity index 100% rename from msg/ekf2_timestamps.msg rename to msg/Ekf2Timestamps.msg diff --git a/msg/esc_report.msg b/msg/EscReport.msg similarity index 100% rename from msg/esc_report.msg rename to msg/EscReport.msg diff --git a/msg/esc_status.msg b/msg/EscStatus.msg similarity index 98% rename from msg/esc_status.msg rename to msg/EscStatus.msg index f786f6d351..e5e220ce0d 100644 --- a/msg/esc_status.msg +++ b/msg/EscStatus.msg @@ -25,4 +25,4 @@ uint8 esc_online_flags # Bitmask indicating which ESC is online/offline uint8 esc_armed_flags # Bitmask indicating which ESC is armed. For ESC's where the arming state is not known (returned by the ESC), the arming bits should always be set. -esc_report[8] esc +EscReport[8] esc diff --git a/msg/estimator_aid_source_1d.msg b/msg/EstimatorAidSource1d.msg similarity index 96% rename from msg/estimator_aid_source_1d.msg rename to msg/EstimatorAidSource1d.msg index be544f2102..46faddafac 100644 --- a/msg/estimator_aid_source_1d.msg +++ b/msg/EstimatorAidSource1d.msg @@ -18,7 +18,6 @@ bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused -# TOPICS estimator_aid_source_1d # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip # TOPICS estimator_aid_src_fake_hgt diff --git a/msg/estimator_aid_source_2d.msg b/msg/EstimatorAidSource2d.msg similarity index 95% rename from msg/estimator_aid_source_2d.msg rename to msg/EstimatorAidSource2d.msg index ada6be79d9..a70c9acaf5 100644 --- a/msg/estimator_aid_source_2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -18,6 +18,5 @@ bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused -# TOPICS estimator_aid_source_2d # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos # TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow diff --git a/msg/estimator_aid_source_3d.msg b/msg/EstimatorAidSource3d.msg similarity index 95% rename from msg/estimator_aid_source_3d.msg rename to msg/EstimatorAidSource3d.msg index e5c082eeec..bb53ae9823 100644 --- a/msg/estimator_aid_source_3d.msg +++ b/msg/EstimatorAidSource3d.msg @@ -18,7 +18,6 @@ bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused -# TOPICS estimator_aid_source_3d # TOPICS estimator_aid_src_ev_vel # TOPICS estimator_aid_src_gnss_vel # TOPICS estimator_aid_src_mag diff --git a/msg/estimator_bias.msg b/msg/EstimatorBias.msg similarity index 96% rename from msg/estimator_bias.msg rename to msg/EstimatorBias.msg index 988a6ce557..3dbadb94e1 100644 --- a/msg/estimator_bias.msg +++ b/msg/EstimatorBias.msg @@ -9,5 +9,4 @@ float32 innov # innovation of the last measurement fusion (m) float32 innov_var # innovation variance of the last measurement fusion (m^2) float32 innov_test_ratio # normalized innovation squared test ratio -# TOPICS estimator_bias # TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias estimator_ev_hgt_bias diff --git a/msg/estimator_event_flags.msg b/msg/EstimatorEventFlags.msg similarity index 100% rename from msg/estimator_event_flags.msg rename to msg/EstimatorEventFlags.msg diff --git a/msg/estimator_gps_status.msg b/msg/EstimatorGpsStatus.msg similarity index 100% rename from msg/estimator_gps_status.msg rename to msg/EstimatorGpsStatus.msg diff --git a/msg/estimator_innovations.msg b/msg/EstimatorInnovations.msg similarity index 100% rename from msg/estimator_innovations.msg rename to msg/EstimatorInnovations.msg diff --git a/msg/estimator_selector_status.msg b/msg/EstimatorSelectorStatus.msg similarity index 100% rename from msg/estimator_selector_status.msg rename to msg/EstimatorSelectorStatus.msg diff --git a/msg/estimator_sensor_bias.msg b/msg/EstimatorSensorBias.msg similarity index 100% rename from msg/estimator_sensor_bias.msg rename to msg/EstimatorSensorBias.msg diff --git a/msg/estimator_states.msg b/msg/EstimatorStates.msg similarity index 100% rename from msg/estimator_states.msg rename to msg/EstimatorStates.msg diff --git a/msg/estimator_status.msg b/msg/EstimatorStatus.msg similarity index 100% rename from msg/estimator_status.msg rename to msg/EstimatorStatus.msg diff --git a/msg/estimator_status_flags.msg b/msg/EstimatorStatusFlags.msg similarity index 100% rename from msg/estimator_status_flags.msg rename to msg/EstimatorStatusFlags.msg diff --git a/msg/event.msg b/msg/Event.msg similarity index 100% rename from msg/event.msg rename to msg/Event.msg diff --git a/msg/failsafe_flags.msg b/msg/FailsafeFlags.msg similarity index 100% rename from msg/failsafe_flags.msg rename to msg/FailsafeFlags.msg diff --git a/msg/failure_detector_status.msg b/msg/FailureDetectorStatus.msg similarity index 100% rename from msg/failure_detector_status.msg rename to msg/FailureDetectorStatus.msg diff --git a/msg/follow_target.msg b/msg/FollowTarget.msg similarity index 100% rename from msg/follow_target.msg rename to msg/FollowTarget.msg diff --git a/msg/follow_target_estimator.msg b/msg/FollowTargetEstimator.msg similarity index 100% rename from msg/follow_target_estimator.msg rename to msg/FollowTargetEstimator.msg diff --git a/msg/follow_target_status.msg b/msg/FollowTargetStatus.msg similarity index 100% rename from msg/follow_target_status.msg rename to msg/FollowTargetStatus.msg diff --git a/msg/generator_status.msg b/msg/GeneratorStatus.msg similarity index 100% rename from msg/generator_status.msg rename to msg/GeneratorStatus.msg diff --git a/msg/geofence_result.msg b/msg/GeofenceResult.msg similarity index 100% rename from msg/geofence_result.msg rename to msg/GeofenceResult.msg diff --git a/msg/gimbal_device_attitude_status.msg b/msg/GimbalDeviceAttitudeStatus.msg similarity index 100% rename from msg/gimbal_device_attitude_status.msg rename to msg/GimbalDeviceAttitudeStatus.msg diff --git a/msg/gimbal_device_information.msg b/msg/GimbalDeviceInformation.msg similarity index 100% rename from msg/gimbal_device_information.msg rename to msg/GimbalDeviceInformation.msg diff --git a/msg/gimbal_device_set_attitude.msg b/msg/GimbalDeviceSetAttitude.msg similarity index 100% rename from msg/gimbal_device_set_attitude.msg rename to msg/GimbalDeviceSetAttitude.msg diff --git a/msg/gimbal_manager_information.msg b/msg/GimbalManagerInformation.msg similarity index 100% rename from msg/gimbal_manager_information.msg rename to msg/GimbalManagerInformation.msg diff --git a/msg/gimbal_manager_set_attitude.msg b/msg/GimbalManagerSetAttitude.msg similarity index 100% rename from msg/gimbal_manager_set_attitude.msg rename to msg/GimbalManagerSetAttitude.msg diff --git a/msg/gimbal_manager_set_manual_control.msg b/msg/GimbalManagerSetManualControl.msg similarity index 100% rename from msg/gimbal_manager_set_manual_control.msg rename to msg/GimbalManagerSetManualControl.msg diff --git a/msg/gimbal_manager_status.msg b/msg/GimbalManagerStatus.msg similarity index 100% rename from msg/gimbal_manager_status.msg rename to msg/GimbalManagerStatus.msg diff --git a/msg/gps_dump.msg b/msg/GpsDump.msg similarity index 100% rename from msg/gps_dump.msg rename to msg/GpsDump.msg diff --git a/msg/gps_inject_data.msg b/msg/GpsInjectData.msg similarity index 100% rename from msg/gps_inject_data.msg rename to msg/GpsInjectData.msg diff --git a/msg/gripper.msg b/msg/Gripper.msg similarity index 100% rename from msg/gripper.msg rename to msg/Gripper.msg diff --git a/msg/health_report.msg b/msg/HealthReport.msg similarity index 99% rename from msg/health_report.msg rename to msg/HealthReport.msg index 5d38e6db2a..189518052b 100644 --- a/msg/health_report.msg +++ b/msg/HealthReport.msg @@ -10,4 +10,3 @@ uint64 health_error_flags uint64 arming_check_warning_flags uint64 arming_check_error_flags - diff --git a/msg/heater_status.msg b/msg/HeaterStatus.msg similarity index 100% rename from msg/heater_status.msg rename to msg/HeaterStatus.msg diff --git a/msg/home_position.msg b/msg/HomePosition.msg similarity index 100% rename from msg/home_position.msg rename to msg/HomePosition.msg diff --git a/msg/hover_thrust_estimate.msg b/msg/HoverThrustEstimate.msg similarity index 100% rename from msg/hover_thrust_estimate.msg rename to msg/HoverThrustEstimate.msg diff --git a/msg/input_rc.msg b/msg/InputRc.msg similarity index 100% rename from msg/input_rc.msg rename to msg/InputRc.msg diff --git a/msg/internal_combustion_engine_status.msg b/msg/InternalCombustionEngineStatus.msg similarity index 100% rename from msg/internal_combustion_engine_status.msg rename to msg/InternalCombustionEngineStatus.msg diff --git a/msg/iridiumsbd_status.msg b/msg/IridiumsbdStatus.msg similarity index 100% rename from msg/iridiumsbd_status.msg rename to msg/IridiumsbdStatus.msg diff --git a/msg/irlock_report.msg b/msg/IrlockReport.msg similarity index 100% rename from msg/irlock_report.msg rename to msg/IrlockReport.msg diff --git a/msg/landing_gear.msg b/msg/LandingGear.msg similarity index 100% rename from msg/landing_gear.msg rename to msg/LandingGear.msg diff --git a/msg/landing_target_innovations.msg b/msg/LandingTargetInnovations.msg similarity index 100% rename from msg/landing_target_innovations.msg rename to msg/LandingTargetInnovations.msg diff --git a/msg/landing_target_pose.msg b/msg/LandingTargetPose.msg similarity index 100% rename from msg/landing_target_pose.msg rename to msg/LandingTargetPose.msg diff --git a/msg/led_control.msg b/msg/LedControl.msg similarity index 100% rename from msg/led_control.msg rename to msg/LedControl.msg diff --git a/msg/log_message.msg b/msg/LogMessage.msg similarity index 100% rename from msg/log_message.msg rename to msg/LogMessage.msg diff --git a/msg/logger_status.msg b/msg/LoggerStatus.msg similarity index 100% rename from msg/logger_status.msg rename to msg/LoggerStatus.msg diff --git a/msg/mag_worker_data.msg b/msg/MagWorkerData.msg similarity index 100% rename from msg/mag_worker_data.msg rename to msg/MagWorkerData.msg diff --git a/msg/magnetometer_bias_estimate.msg b/msg/MagnetometerBiasEstimate.msg similarity index 100% rename from msg/magnetometer_bias_estimate.msg rename to msg/MagnetometerBiasEstimate.msg diff --git a/msg/manual_control_setpoint.msg b/msg/ManualControlSetpoint.msg similarity index 100% rename from msg/manual_control_setpoint.msg rename to msg/ManualControlSetpoint.msg diff --git a/msg/manual_control_switches.msg b/msg/ManualControlSwitches.msg similarity index 100% rename from msg/manual_control_switches.msg rename to msg/ManualControlSwitches.msg diff --git a/msg/mavlink_log.msg b/msg/MavlinkLog.msg similarity index 100% rename from msg/mavlink_log.msg rename to msg/MavlinkLog.msg diff --git a/msg/mavlink_tunnel.msg b/msg/MavlinkTunnel.msg similarity index 100% rename from msg/mavlink_tunnel.msg rename to msg/MavlinkTunnel.msg diff --git a/msg/mission.msg b/msg/Mission.msg similarity index 100% rename from msg/mission.msg rename to msg/Mission.msg diff --git a/msg/mission_result.msg b/msg/MissionResult.msg similarity index 100% rename from msg/mission_result.msg rename to msg/MissionResult.msg diff --git a/msg/mount_orientation.msg b/msg/MountOrientation.msg similarity index 100% rename from msg/mount_orientation.msg rename to msg/MountOrientation.msg diff --git a/msg/navigator_mission_item.msg b/msg/NavigatorMissionItem.msg similarity index 100% rename from msg/navigator_mission_item.msg rename to msg/NavigatorMissionItem.msg diff --git a/msg/npfg_status.msg b/msg/NpfgStatus.msg similarity index 100% rename from msg/npfg_status.msg rename to msg/NpfgStatus.msg diff --git a/msg/obstacle_distance.msg b/msg/ObstacleDistance.msg similarity index 100% rename from msg/obstacle_distance.msg rename to msg/ObstacleDistance.msg diff --git a/msg/offboard_control_mode.msg b/msg/OffboardControlMode.msg similarity index 100% rename from msg/offboard_control_mode.msg rename to msg/OffboardControlMode.msg diff --git a/msg/onboard_computer_status.msg b/msg/OnboardComputerStatus.msg similarity index 100% rename from msg/onboard_computer_status.msg rename to msg/OnboardComputerStatus.msg diff --git a/msg/orb_test.msg b/msg/OrbTest.msg similarity index 100% rename from msg/orb_test.msg rename to msg/OrbTest.msg diff --git a/msg/orb_test_large.msg b/msg/OrbTestLarge.msg similarity index 100% rename from msg/orb_test_large.msg rename to msg/OrbTestLarge.msg diff --git a/msg/orb_test_medium.msg b/msg/OrbTestMedium.msg similarity index 100% rename from msg/orb_test_medium.msg rename to msg/OrbTestMedium.msg diff --git a/msg/orbit_status.msg b/msg/OrbitStatus.msg similarity index 100% rename from msg/orbit_status.msg rename to msg/OrbitStatus.msg diff --git a/msg/parameter_update.msg b/msg/ParameterUpdate.msg similarity index 100% rename from msg/parameter_update.msg rename to msg/ParameterUpdate.msg diff --git a/msg/ping.msg b/msg/Ping.msg similarity index 100% rename from msg/ping.msg rename to msg/Ping.msg diff --git a/msg/position_controller_landing_status.msg b/msg/PositionControllerLandingStatus.msg similarity index 100% rename from msg/position_controller_landing_status.msg rename to msg/PositionControllerLandingStatus.msg diff --git a/msg/position_controller_status.msg b/msg/PositionControllerStatus.msg similarity index 100% rename from msg/position_controller_status.msg rename to msg/PositionControllerStatus.msg diff --git a/msg/position_setpoint.msg b/msg/PositionSetpoint.msg similarity index 100% rename from msg/position_setpoint.msg rename to msg/PositionSetpoint.msg diff --git a/msg/position_setpoint_triplet.msg b/msg/PositionSetpointTriplet.msg similarity index 67% rename from msg/position_setpoint_triplet.msg rename to msg/PositionSetpointTriplet.msg index 7c9a2ca78b..6f9ac4d2a2 100644 --- a/msg/position_setpoint_triplet.msg +++ b/msg/PositionSetpointTriplet.msg @@ -3,6 +3,6 @@ uint64 timestamp # time since system start (microseconds) -px4/position_setpoint previous -px4/position_setpoint current -px4/position_setpoint next +PositionSetpoint previous +PositionSetpoint current +PositionSetpoint next diff --git a/msg/power_button_state.msg b/msg/PowerButtonState.msg similarity index 100% rename from msg/power_button_state.msg rename to msg/PowerButtonState.msg diff --git a/msg/power_monitor.msg b/msg/PowerMonitor.msg similarity index 100% rename from msg/power_monitor.msg rename to msg/PowerMonitor.msg diff --git a/msg/pps_capture.msg b/msg/PpsCapture.msg similarity index 100% rename from msg/pps_capture.msg rename to msg/PpsCapture.msg diff --git a/msg/pwm_input.msg b/msg/PwmInput.msg similarity index 100% rename from msg/pwm_input.msg rename to msg/PwmInput.msg diff --git a/msg/px4io_status.msg b/msg/Px4ioStatus.msg similarity index 100% rename from msg/px4io_status.msg rename to msg/Px4ioStatus.msg diff --git a/msg/radio_status.msg b/msg/RadioStatus.msg similarity index 100% rename from msg/radio_status.msg rename to msg/RadioStatus.msg diff --git a/msg/rate_ctrl_status.msg b/msg/RateCtrlStatus.msg similarity index 100% rename from msg/rate_ctrl_status.msg rename to msg/RateCtrlStatus.msg diff --git a/msg/rc_channels.msg b/msg/RcChannels.msg similarity index 100% rename from msg/rc_channels.msg rename to msg/RcChannels.msg diff --git a/msg/rc_parameter_map.msg b/msg/RcParameterMap.msg similarity index 100% rename from msg/rc_parameter_map.msg rename to msg/RcParameterMap.msg diff --git a/msg/rpm.msg b/msg/Rpm.msg similarity index 100% rename from msg/rpm.msg rename to msg/Rpm.msg diff --git a/msg/rtl_time_estimate.msg b/msg/RtlTimeEstimate.msg similarity index 100% rename from msg/rtl_time_estimate.msg rename to msg/RtlTimeEstimate.msg diff --git a/msg/satellite_info.msg b/msg/SatelliteInfo.msg similarity index 100% rename from msg/satellite_info.msg rename to msg/SatelliteInfo.msg diff --git a/msg/sensor_accel.msg b/msg/SensorAccel.msg similarity index 100% rename from msg/sensor_accel.msg rename to msg/SensorAccel.msg diff --git a/msg/sensor_accel_fifo.msg b/msg/SensorAccelFifo.msg similarity index 100% rename from msg/sensor_accel_fifo.msg rename to msg/SensorAccelFifo.msg diff --git a/msg/sensor_baro.msg b/msg/SensorBaro.msg similarity index 100% rename from msg/sensor_baro.msg rename to msg/SensorBaro.msg diff --git a/msg/sensor_combined.msg b/msg/SensorCombined.msg similarity index 100% rename from msg/sensor_combined.msg rename to msg/SensorCombined.msg diff --git a/msg/sensor_correction.msg b/msg/SensorCorrection.msg similarity index 100% rename from msg/sensor_correction.msg rename to msg/SensorCorrection.msg diff --git a/msg/sensor_gnss_relative.msg b/msg/SensorGnssRelative.msg similarity index 100% rename from msg/sensor_gnss_relative.msg rename to msg/SensorGnssRelative.msg diff --git a/msg/sensor_gps.msg b/msg/SensorGps.msg similarity index 100% rename from msg/sensor_gps.msg rename to msg/SensorGps.msg diff --git a/msg/sensor_gyro.msg b/msg/SensorGyro.msg similarity index 100% rename from msg/sensor_gyro.msg rename to msg/SensorGyro.msg diff --git a/msg/sensor_gyro_fft.msg b/msg/SensorGyroFft.msg similarity index 100% rename from msg/sensor_gyro_fft.msg rename to msg/SensorGyroFft.msg diff --git a/msg/sensor_gyro_fifo.msg b/msg/SensorGyroFifo.msg similarity index 100% rename from msg/sensor_gyro_fifo.msg rename to msg/SensorGyroFifo.msg diff --git a/msg/sensor_hygrometer.msg b/msg/SensorHygrometer.msg old mode 100755 new mode 100644 similarity index 100% rename from msg/sensor_hygrometer.msg rename to msg/SensorHygrometer.msg diff --git a/msg/sensor_mag.msg b/msg/SensorMag.msg similarity index 100% rename from msg/sensor_mag.msg rename to msg/SensorMag.msg diff --git a/msg/sensor_optical_flow.msg b/msg/SensorOpticalFlow.msg similarity index 100% rename from msg/sensor_optical_flow.msg rename to msg/SensorOpticalFlow.msg diff --git a/msg/sensor_preflight_mag.msg b/msg/SensorPreflightMag.msg similarity index 100% rename from msg/sensor_preflight_mag.msg rename to msg/SensorPreflightMag.msg diff --git a/msg/sensor_selection.msg b/msg/SensorSelection.msg similarity index 100% rename from msg/sensor_selection.msg rename to msg/SensorSelection.msg diff --git a/msg/sensors_status.msg b/msg/SensorsStatus.msg similarity index 87% rename from msg/sensors_status.msg rename to msg/SensorsStatus.msg index 28c2797812..c16bf1c6a2 100644 --- a/msg/sensors_status.msg +++ b/msg/SensorsStatus.msg @@ -12,4 +12,4 @@ uint8[4] priority bool[4] enabled bool[4] external -# TOPICS sensors_status sensors_status_baro sensors_status_mag +# TOPICS sensors_status_baro sensors_status_mag diff --git a/msg/sensors_status_imu.msg b/msg/SensorsStatusImu.msg similarity index 100% rename from msg/sensors_status_imu.msg rename to msg/SensorsStatusImu.msg diff --git a/msg/system_power.msg b/msg/SystemPower.msg similarity index 100% rename from msg/system_power.msg rename to msg/SystemPower.msg diff --git a/msg/takeoff_status.msg b/msg/TakeoffStatus.msg similarity index 100% rename from msg/takeoff_status.msg rename to msg/TakeoffStatus.msg diff --git a/msg/task_stack_info.msg b/msg/TaskStackInfo.msg similarity index 100% rename from msg/task_stack_info.msg rename to msg/TaskStackInfo.msg diff --git a/msg/tecs_status.msg b/msg/TecsStatus.msg similarity index 100% rename from msg/tecs_status.msg rename to msg/TecsStatus.msg diff --git a/msg/telemetry_status.msg b/msg/TelemetryStatus.msg similarity index 100% rename from msg/telemetry_status.msg rename to msg/TelemetryStatus.msg diff --git a/msg/timesync_status.msg b/msg/TimesyncStatus.msg similarity index 72% rename from msg/timesync_status.msg rename to msg/TimesyncStatus.msg index 9d557352dc..71e84e85ab 100644 --- a/msg/timesync_status.msg +++ b/msg/TimesyncStatus.msg @@ -1,8 +1,9 @@ uint64 timestamp # time since system start (microseconds) -uint8 SOURCE_PROTOCOL_MAVLINK = 0 -uint8 SOURCE_PROTOCOL_RTPS = 1 -uint8 source_protocol # timesync source. Source can be MAVLink or the microRTPS bridge +uint8 SOURCE_PROTOCOL_UNKNOWN = 0 +uint8 SOURCE_PROTOCOL_MAVLINK = 1 +uint8 SOURCE_PROTOCOL_DDS = 2 +uint8 source_protocol # timesync source uint64 remote_timestamp # remote system timestamp (microseconds) int64 observed_offset # raw time offset directly observed from this timesync packet (microseconds) diff --git a/msg/trajectory_bezier.msg b/msg/TrajectoryBezier.msg similarity index 100% rename from msg/trajectory_bezier.msg rename to msg/TrajectoryBezier.msg diff --git a/msg/trajectory_setpoint.msg b/msg/TrajectorySetpoint.msg similarity index 100% rename from msg/trajectory_setpoint.msg rename to msg/TrajectorySetpoint.msg diff --git a/msg/trajectory_waypoint.msg b/msg/TrajectoryWaypoint.msg similarity index 100% rename from msg/trajectory_waypoint.msg rename to msg/TrajectoryWaypoint.msg diff --git a/msg/transponder_report.msg b/msg/TransponderReport.msg similarity index 100% rename from msg/transponder_report.msg rename to msg/TransponderReport.msg diff --git a/msg/tune_control.msg b/msg/TuneControl.msg similarity index 100% rename from msg/tune_control.msg rename to msg/TuneControl.msg diff --git a/msg/uavcan_parameter_request.msg b/msg/UavcanParameterRequest.msg similarity index 100% rename from msg/uavcan_parameter_request.msg rename to msg/UavcanParameterRequest.msg diff --git a/msg/uavcan_parameter_value.msg b/msg/UavcanParameterValue.msg similarity index 100% rename from msg/uavcan_parameter_value.msg rename to msg/UavcanParameterValue.msg diff --git a/msg/ulog_stream.msg b/msg/UlogStream.msg similarity index 100% rename from msg/ulog_stream.msg rename to msg/UlogStream.msg diff --git a/msg/ulog_stream_ack.msg b/msg/UlogStreamAck.msg similarity index 100% rename from msg/ulog_stream_ack.msg rename to msg/UlogStreamAck.msg diff --git a/msg/uwb_distance.msg b/msg/UwbDistance.msg similarity index 100% rename from msg/uwb_distance.msg rename to msg/UwbDistance.msg diff --git a/msg/uwb_grid.msg b/msg/UwbGrid.msg similarity index 100% rename from msg/uwb_grid.msg rename to msg/UwbGrid.msg diff --git a/msg/vehicle_acceleration.msg b/msg/VehicleAcceleration.msg similarity index 100% rename from msg/vehicle_acceleration.msg rename to msg/VehicleAcceleration.msg diff --git a/msg/vehicle_air_data.msg b/msg/VehicleAirData.msg similarity index 100% rename from msg/vehicle_air_data.msg rename to msg/VehicleAirData.msg diff --git a/msg/vehicle_angular_acceleration.msg b/msg/VehicleAngularAcceleration.msg similarity index 100% rename from msg/vehicle_angular_acceleration.msg rename to msg/VehicleAngularAcceleration.msg diff --git a/msg/vehicle_angular_acceleration_setpoint.msg b/msg/VehicleAngularAccelerationSetpoint.msg similarity index 100% rename from msg/vehicle_angular_acceleration_setpoint.msg rename to msg/VehicleAngularAccelerationSetpoint.msg diff --git a/msg/vehicle_angular_velocity.msg b/msg/VehicleAngularVelocity.msg similarity index 100% rename from msg/vehicle_angular_velocity.msg rename to msg/VehicleAngularVelocity.msg diff --git a/msg/vehicle_attitude.msg b/msg/VehicleAttitude.msg similarity index 86% rename from msg/vehicle_attitude.msg rename to msg/VehicleAttitude.msg index f704d71933..6eeb20a21e 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/VehicleAttitude.msg @@ -8,5 +8,5 @@ float32[4] q # Quaternion rotation from the FRD body frame to float32[4] delta_q_reset # Amount by which quaternion has changed during last reset uint8 quat_reset_counter # Quaternion reset counter -# TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude +# TOPICS vehicle_attitude vehicle_attitude_groundtruth # TOPICS estimator_attitude diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/VehicleAttitudeSetpoint.msg similarity index 100% rename from msg/vehicle_attitude_setpoint.msg rename to msg/VehicleAttitudeSetpoint.msg diff --git a/msg/vehicle_command.msg b/msg/VehicleCommand.msg similarity index 100% rename from msg/vehicle_command.msg rename to msg/VehicleCommand.msg diff --git a/msg/vehicle_command_ack.msg b/msg/VehicleCommandAck.msg similarity index 100% rename from msg/vehicle_command_ack.msg rename to msg/VehicleCommandAck.msg diff --git a/msg/vehicle_constraints.msg b/msg/VehicleConstraints.msg similarity index 100% rename from msg/vehicle_constraints.msg rename to msg/VehicleConstraints.msg diff --git a/msg/vehicle_control_mode.msg b/msg/VehicleControlMode.msg similarity index 100% rename from msg/vehicle_control_mode.msg rename to msg/VehicleControlMode.msg diff --git a/msg/vehicle_global_position.msg b/msg/VehicleGlobalPosition.msg similarity index 100% rename from msg/vehicle_global_position.msg rename to msg/VehicleGlobalPosition.msg diff --git a/msg/vehicle_imu.msg b/msg/VehicleImu.msg similarity index 100% rename from msg/vehicle_imu.msg rename to msg/VehicleImu.msg diff --git a/msg/vehicle_imu_status.msg b/msg/VehicleImuStatus.msg similarity index 100% rename from msg/vehicle_imu_status.msg rename to msg/VehicleImuStatus.msg diff --git a/msg/vehicle_land_detected.msg b/msg/VehicleLandDetected.msg similarity index 100% rename from msg/vehicle_land_detected.msg rename to msg/VehicleLandDetected.msg diff --git a/msg/vehicle_local_position.msg b/msg/VehicleLocalPosition.msg similarity index 100% rename from msg/vehicle_local_position.msg rename to msg/VehicleLocalPosition.msg diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/VehicleLocalPositionSetpoint.msg similarity index 100% rename from msg/vehicle_local_position_setpoint.msg rename to msg/VehicleLocalPositionSetpoint.msg diff --git a/msg/vehicle_magnetometer.msg b/msg/VehicleMagnetometer.msg similarity index 100% rename from msg/vehicle_magnetometer.msg rename to msg/VehicleMagnetometer.msg diff --git a/msg/vehicle_odometry.msg b/msg/VehicleOdometry.msg similarity index 100% rename from msg/vehicle_odometry.msg rename to msg/VehicleOdometry.msg diff --git a/msg/vehicle_optical_flow.msg b/msg/VehicleOpticalFlow.msg similarity index 100% rename from msg/vehicle_optical_flow.msg rename to msg/VehicleOpticalFlow.msg diff --git a/msg/vehicle_optical_flow_vel.msg b/msg/VehicleOpticalFlowVel.msg similarity index 100% rename from msg/vehicle_optical_flow_vel.msg rename to msg/VehicleOpticalFlowVel.msg diff --git a/msg/vehicle_rates_setpoint.msg b/msg/VehicleRatesSetpoint.msg similarity index 100% rename from msg/vehicle_rates_setpoint.msg rename to msg/VehicleRatesSetpoint.msg diff --git a/msg/vehicle_roi.msg b/msg/VehicleRoi.msg similarity index 100% rename from msg/vehicle_roi.msg rename to msg/VehicleRoi.msg diff --git a/msg/vehicle_status.msg b/msg/VehicleStatus.msg similarity index 100% rename from msg/vehicle_status.msg rename to msg/VehicleStatus.msg diff --git a/msg/vehicle_thrust_setpoint.msg b/msg/VehicleThrustSetpoint.msg similarity index 100% rename from msg/vehicle_thrust_setpoint.msg rename to msg/VehicleThrustSetpoint.msg diff --git a/msg/vehicle_torque_setpoint.msg b/msg/VehicleTorqueSetpoint.msg similarity index 100% rename from msg/vehicle_torque_setpoint.msg rename to msg/VehicleTorqueSetpoint.msg diff --git a/msg/vehicle_trajectory_bezier.msg b/msg/VehicleTrajectoryBezier.msg similarity index 93% rename from msg/vehicle_trajectory_bezier.msg rename to msg/VehicleTrajectoryBezier.msg index 421ff1a015..d4bf99b469 100644 --- a/msg/vehicle_trajectory_bezier.msg +++ b/msg/VehicleTrajectoryBezier.msg @@ -12,7 +12,7 @@ uint8 POINT_4 = 4 uint8 NUMBER_POINTS = 5 -trajectory_bezier[5] control_points +TrajectoryBezier[5] control_points uint8 bezier_order # TOPICS vehicle_trajectory_bezier diff --git a/msg/vehicle_trajectory_waypoint.msg b/msg/VehicleTrajectoryWaypoint.msg similarity index 95% rename from msg/vehicle_trajectory_waypoint.msg rename to msg/VehicleTrajectoryWaypoint.msg index 182f548d23..6bff1cec84 100644 --- a/msg/vehicle_trajectory_waypoint.msg +++ b/msg/VehicleTrajectoryWaypoint.msg @@ -16,6 +16,6 @@ uint8 POINT_4 = 4 uint8 NUMBER_POINTS = 5 -trajectory_waypoint[5] waypoints +TrajectoryWaypoint[5] waypoints # TOPICS vehicle_trajectory_waypoint vehicle_trajectory_waypoint_desired diff --git a/msg/vtol_vehicle_status.msg b/msg/VtolVehicleStatus.msg similarity index 100% rename from msg/vtol_vehicle_status.msg rename to msg/VtolVehicleStatus.msg diff --git a/msg/wheel_encoders.msg b/msg/WheelEncoders.msg similarity index 100% rename from msg/wheel_encoders.msg rename to msg/WheelEncoders.msg diff --git a/msg/wind.msg b/msg/Wind.msg similarity index 100% rename from msg/wind.msg rename to msg/Wind.msg diff --git a/msg/yaw_estimator_status.msg b/msg/YawEstimatorStatus.msg similarity index 100% rename from msg/yaw_estimator_status.msg rename to msg/YawEstimatorStatus.msg diff --git a/msg/templates/module.yaml b/msg/templates/module.yaml deleted file mode 100644 index 099976c11e..0000000000 --- a/msg/templates/module.yaml +++ /dev/null @@ -1,6 +0,0 @@ -module_name: Sagtech MXS -serial_config: - - command: mxs start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} - port_config_param: - name: TRANS_MXS_CFG - group: Transponders diff --git a/msg/templates/px4/ros/msg.h.em b/msg/templates/px4/ros/msg.h.em deleted file mode 100644 index e102a087d4..0000000000 --- a/msg/templates/px4/ros/msg.h.em +++ /dev/null @@ -1,94 +0,0 @@ -@############################################### -@# -@# PX4 ROS compatible message source code -@# generation for C++ -@# -@# This file generates the multiplatform wrapper -@# -@# EmPy template for generating .h files -@# Based on the original template for ROS -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - file_name_in (String) Source file -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file @file_name_in */ - -@{ -import genmsg.msgs - - -cpp_class = 'px4_%s'%spec.short_name -native_type = spec.short_name -topic_name = spec.short_name -}@ - -#pragma once - -@############################## -@# Generic Includes -@############################## -#include "px4/@(topic_name).h" -#include "platforms/px4_message.h" - -@############################## -@# Class -@############################## - -namespace px4 -{ - -class @(cpp_class) : - public PX4Message<@(native_type)> -{ -public: - @(cpp_class)() : - PX4Message<@(native_type)>() - {} - - @(cpp_class)(@(native_type) msg) : - PX4Message<@(native_type)>(msg) - {} - - ~@(cpp_class)() {} - - static PX4TopicHandle handle() {return "@(topic_name)";} -}; - -}; diff --git a/msg/templates/px4/uorb/msg.h.em b/msg/templates/px4/uorb/msg.h.em deleted file mode 100644 index deed11912d..0000000000 --- a/msg/templates/px4/uorb/msg.h.em +++ /dev/null @@ -1,99 +0,0 @@ -@############################################### -@# -@# PX4 ROS compatible message source code -@# generation for C++ -@# -@# This file generates the multiplatform wrapper -@# -@# EmPy template for generating .h files -@# Based on the original template for ROS -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - file_name_in (String) Source file -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file @file_name_in */ - -@{ -import genmsg.msgs - - -native_type = '%s_s'%spec.short_name -topic_name = spec.short_name -}@ - -#pragma once - -@############################## -@# Generic Includes -@############################## -#include -#include "platforms/px4_message.h" - -@############################## -@# Class -@############################## - -namespace px4 -{ -@[for multi_topic in topics]@ -@{ -cpp_class = 'px4_%s'%multi_topic -}@ - - -class __EXPORT @(cpp_class) : - public PX4Message<@(native_type)> -{ -public: - @(cpp_class)() : - PX4Message<@(native_type)>() - {} - - @(cpp_class)(@(native_type) msg) : - PX4Message<@(native_type)>(msg) - {} - - ~@(cpp_class)() {} - - static PX4TopicHandle handle() {return ORB_ID(@(multi_topic));} -}; -@[end for] - -}; diff --git a/msg/templates/uorb_microcdr/dds_topics.h.em b/msg/templates/uorb_microcdr/dds_topics.h.em deleted file mode 100644 index 8d7b5e7a67..0000000000 --- a/msg/templates/uorb_microcdr/dds_topics.h.em +++ /dev/null @@ -1,225 +0,0 @@ -@############################################### -@# -@# EmPy template -@# -@############################################### -@# Generates publications and subscriptions for XRCE -@# -@# Context: -@# - msgs (List) list of all RTPS messages -@# - topics (List) list of all topic names -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import os - -import genmsg.msgs - -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -topic_names = [s.short_name for s in spec] -send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -send_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -}@ - - -#include -#include - -#include -#include -@[for idx, topic in enumerate(send_topics)]@ -#include -@[end for]@ -@[for idx, topic in enumerate(recv_topics)]@ -#include -@[end for]@ - -// Subscribers for messages to send -struct SendTopicsSubs { -@[ for idx, topic in enumerate(send_topics)]@ - uORB::Subscription @(topic)_sub{ORB_ID(@(topic))}; - uxrObjectId @(topic)_data_writer; -@[ end for]@ - - uxrSession* session; - - uint32_t num_payload_sent{}; - - bool init(uxrSession* session_, uxrStreamId stream_id, uxrObjectId participant_id); - void update(uxrStreamId stream_id); -}; - -bool SendTopicsSubs::init(uxrSession* session_, uxrStreamId stream_id, uxrObjectId participant_id) -{ - session = session_; - -@[ for idx, topic in enumerate(send_topics)]@ -@{ -topic_pascal = topic.replace("_", " ").title().replace(" ", "") -}@ - { - - uxrObjectId topic_id = uxr_object_id(@(idx)+1, UXR_TOPIC_ID); - const char* topic_xml = "" - "" - "rt/fmu/out/@(topic_pascal)" - "px4_msgs::msg::dds_::@(topic_pascal)_" - "" - ""; - uint16_t topic_req = uxr_buffer_create_topic_xml(session, stream_id, topic_id, participant_id, topic_xml, - UXR_REPLACE); - - uxrObjectId publisher_id = uxr_object_id(@(idx)+1, UXR_PUBLISHER_ID); - const char* publisher_xml = ""; - uint16_t publisher_req = uxr_buffer_create_publisher_xml(session, stream_id, publisher_id, participant_id, - publisher_xml, UXR_REPLACE); - - uxrObjectId datawriter_id = uxr_object_id(@(idx)+1, UXR_DATAWRITER_ID); - @(topic)_data_writer = datawriter_id; - const char* datawriter_xml = "" - "" - "" - "NO_KEY" - "rt/fmu/out/@(topic_pascal)" - "px4_msgs::msg::dds_::@(topic_pascal)_" - "" - "" - ""; - uint16_t datawriter_req = uxr_buffer_create_datawriter_xml(session, stream_id, datawriter_id, publisher_id, - datawriter_xml, UXR_REPLACE); - - // Send create entities message and wait its status - uint8_t status[3]; - uint16_t requests[3] = { - topic_req, publisher_req, datawriter_req - }; - if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) { - PX4_ERR("create entities failed: %s, topic: %i publisher: %i datawriter: %i", "@(topic_pascal)", status[0], status[1], status[2]); - return false; - } - } - -@[ end for]@ - - return true; -} - -void SendTopicsSubs::update(uxrStreamId stream_id) -{ -@[ for idx, topic in enumerate(send_topics)]@ - { - @(send_base_types[idx])_s data; - - if (@(topic)_sub.update(&data)) { - ucdrBuffer ub{}; - uint32_t topic_size = ucdr_topic_size_@(send_base_types[idx])(); - uxr_prepare_output_stream(session, stream_id, @(topic)_data_writer, &ub, topic_size); - ucdr_serialize_@(send_base_types[idx])(data, ub); - // TODO: fill up the MTU and then flush, which reduces the packet overhead - uxr_flash_output_streams(session); - num_payload_sent += topic_size; - } - } -@[ end for]@ - -} - -static void on_topic_update(uxrSession* session, uxrObjectId object_id, - uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t - length, void* args); - -// Publishers for received messages -struct RcvTopicsPubs { -@[ for idx, topic in enumerate(recv_topics)]@ - uORB::Publication <@(receive_base_types[idx])_s> @(topic)_pub{ORB_ID(@(topic))}; -@[ end for]@ - - uxrSession* session; - - uint32_t num_payload_received{}; - - bool init(uxrSession* session_, uxrStreamId stream_id, uxrStreamId input_stream, uxrObjectId participant_id); -}; - -bool RcvTopicsPubs::init(uxrSession* session_, uxrStreamId stream_id, uxrStreamId input_stream, uxrObjectId participant_id) -{ - session = session_; - uxr_set_topic_callback(session, on_topic_update, this); - - -@[ for idx, topic in enumerate(recv_topics)]@ -@{ -topic_pascal = topic.replace("_", " ").title().replace(" ", "") -}@ - { - - uxrObjectId subscriber_id = uxr_object_id(@(idx)+1, UXR_SUBSCRIBER_ID); - const char* subscriber_xml = ""; - uint16_t subscriber_req = uxr_buffer_create_subscriber_xml(session, stream_id, subscriber_id, participant_id, subscriber_xml, UXR_REPLACE); - - uxrObjectId topic_id = uxr_object_id(1000+@(idx), UXR_TOPIC_ID); - const char* topic_xml = "" - "" - "rt/fmu/in/@(topic_pascal)" - "px4_msgs::msg::dds_::@(topic_pascal)_" - "" - ""; - uint16_t topic_req = uxr_buffer_create_topic_xml(session, stream_id, topic_id, participant_id, topic_xml, UXR_REPLACE); - - uxrObjectId datareader_id = uxr_object_id(@(idx)+1, UXR_DATAREADER_ID); - const char* datareader_xml = "" - "" - "" - "NO_KEY" - "rt/fmu/in/@(topic_pascal)" - "px4_msgs::msg::dds_::@(topic_pascal)_" - "" - "" - ""; - uint16_t datareader_req = uxr_buffer_create_datareader_xml(session, stream_id, datareader_id, subscriber_id, datareader_xml, UXR_REPLACE); - - uint8_t status[3]; - uint16_t requests[3] = {topic_req, subscriber_req, datareader_req }; - if(!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) - { - PX4_ERR("create entities failed: %s %i %i %i", "@(topic)", status[0], status[1], status[2]); - return false; - } - - uxrDeliveryControl delivery_control = {0}; - delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED; - uxr_buffer_request_data(session, stream_id, datareader_id, input_stream, &delivery_control); - - } - -@[ end for]@ - - return true; -} - -void on_topic_update(uxrSession* session, uxrObjectId object_id, - uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t - length, void* args) -{ - RcvTopicsPubs* pubs = (RcvTopicsPubs*)args; - pubs->num_payload_received += length; - - switch (object_id.id) { -@[ for idx, topic in enumerate(recv_topics)]@ - case @(idx)+1: { - @(receive_base_types[idx])_s topic; - if (ucdr_deserialize_@(receive_base_types[idx])(*ub, topic)) { - pubs->@(topic)_pub.publish(topic); - } - } - break; -@[ end for]@ - - default: - PX4_ERR("unknown object id: %i", object_id.id); - break; - } -} diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em deleted file mode 100644 index ba49bfbcb8..0000000000 --- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em +++ /dev/null @@ -1,310 +0,0 @@ -@############################################### -@# -@# EmPy template for generating microRTPS_client.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - msgs (List) list of all RTPS messages -@# - topics (List) list of all topic names -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import os - -import genmsg.msgs - -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -topic_names = [s.short_name for s in spec] -send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -send_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -}@ -/**************************************************************************** - * - * Copyright (c) 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -@[for topic in list(set(topic_names))]@ -#include -#include -@[end for]@ - -using namespace time_literals; - -@[if recv_topics]@ -// Publishers for received messages -struct RcvTopicsPubs { -@[ for idx, topic in enumerate(recv_topics)]@ - uORB::Publication <@(receive_base_types[idx])_s> @(topic)_pub{ORB_ID(@(topic))}; -@[ end for]@ -}; -@[end if]@ - -@[if send_topics]@ -// Subscribers for messages to send -struct SendTopicsSubs { -@[ for idx, topic in enumerate(send_topics)]@ - uORB::Subscription @(topic)_sub{ORB_ID(@(topic))}; -@[ end for]@ -}; - -struct SendThreadArgs { - const uint32_t &datarate; - uint64_t &total_sent; - uint64_t &sent_last_sec; - uint64_t &sent; - int &sent_loop; - SendThreadArgs(const uint32_t &datarate_, uint64_t &total_sent_, - uint64_t &sent_last_sec_, uint64_t &sent_, int &sent_loop_) - : datarate(datarate_), - total_sent(total_sent_), - sent_last_sec(sent_last_sec_), - sent(sent_), - sent_loop(sent_loop_) {} -}; - -void *send(void *args) -{ - char data_buffer[BUFFER_SIZE]{}; - int read{0}; - uint32_t length{0}; - size_t header_length{0}; - uint8_t last_msg_seq{0}; - uint8_t last_remote_msg_seq{0}; - - struct SendThreadArgs *data = reinterpret_cast(args); - SendTopicsSubs *subs = new SendTopicsSubs(); - - float bandwidth_mult{0}; - float tx_interval{1.f}; - uint64_t tx_last_sec_read{0}; - hrt_abstime last_stats_update{0}; - - // ucdrBuffer to serialize using the user defined buffer - ucdrBuffer writer; - header_length = transport_node->get_header_length(); - ucdr_init_buffer(&writer, reinterpret_cast(&data_buffer[header_length]), BUFFER_SIZE - header_length); - - while (!_should_exit_task) { -@[ for idx, topic in enumerate(send_topics)]@ - { - @(send_base_types[idx])_s @(topic)_data; - - if (subs->@(topic)_sub.update(&@(topic)_data)) - { -@[ if topic == 'Timesync' or topic == 'timesync']@ - if (@(topic)_data.seq != last_remote_msg_seq && @(topic)_data.tc1 == 0) { - last_remote_msg_seq = @(topic)_data.seq; - - @(topic)_data.timestamp = hrt_absolute_time(); - @(topic)_data.seq = last_msg_seq; - @(topic)_data.tc1 = hrt_absolute_time() * 1000ULL; - @(topic)_data.ts1 = @(topic)_data.ts1; - - last_msg_seq++; -@[ end if]@ - // copy raw data into local buffer. Payload is shifted by header length to make room for header - serialize_@(send_base_types[idx])(&writer, &@(topic)_data, &data_buffer[header_length], &length); - - if (0 < (read = transport_node->write(static_cast(@(msgs[0].index(topic) + 1)), data_buffer, length))) { - data->total_sent += read; - tx_last_sec_read += read; - ++data->sent; - } - -@[ if topic == 'Timesync' or topic == 'timesync']@ - } - -@[ end if]@ - } - } -@[ end for]@ - - if (hrt_absolute_time() - last_stats_update >= 1_s) { - data->sent_last_sec = tx_last_sec_read; - if (data->datarate > 0) { - bandwidth_mult = static_cast(data->datarate) / static_cast(tx_last_sec_read); - // Apply a low-pass filter to determine the new TX interval - tx_interval += 0.5f * (tx_interval / bandwidth_mult - tx_interval); - // Clamp the interval between 1 and 1000 ms - tx_interval = math::constrain(tx_interval, MIN_TX_INTERVAL_US, MAX_TX_INTERVAL_US); - } - tx_last_sec_read = 0; - last_stats_update = hrt_absolute_time(); - } - - px4_usleep(tx_interval); - - ++data->sent_loop; - } - - delete(data); - delete(subs); - - return nullptr; -} - -static int launch_send_thread(pthread_t &sender_thread, struct SendThreadArgs &args) -{ - pthread_attr_t sender_thread_attr; - pthread_attr_init(&sender_thread_attr); - pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(2250)); - struct sched_param param; - (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_DEFAULT; - (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); - int rc = pthread_create(&sender_thread, &sender_thread_attr, &send, (void *)&args); - if (rc != 0) { - errno = rc; - PX4_ERR("Could not create send thread (%d)", errno); - return -1; - } - rc = pthread_setname_np(sender_thread, "urtpsclient_snd"); - if (pthread_setname_np(sender_thread, "urtpsclient_snd")) { - errno = rc; - PX4_ERR("Could not set pthread name for the send thread (%d)", errno); - } - pthread_attr_destroy(&sender_thread_attr); - - return 0; -} -@[end if]@ - -void micrortps_start_topics(const uint32_t &datarate, struct timespec &begin, uint64_t &total_rcvd, - uint64_t &total_sent, uint64_t &sent_last_sec, - uint64_t &rcvd_last_sec, uint64_t &received, uint64_t &sent, int &rcvd_loop, int &sent_loop) -{ - px4_clock_gettime(CLOCK_REALTIME, &begin); - _should_exit_task = false; - -@[if recv_topics]@ - char data_buffer[BUFFER_SIZE]{}; - int read{0}; - uint8_t topic_ID{255}; - - uint64_t rx_last_sec_read{0}; - hrt_abstime last_stats_update{0}; - - RcvTopicsPubs *pubs = new RcvTopicsPubs(); - - // Set the main task name to 'urtpsclient_rcv' in case there is - // data to receive - px4_prctl(PR_SET_NAME, "urtpsclient_rcv", px4_getpid()); - - // ucdrBuffer to deserialize using the user defined buffer - ucdrBuffer reader; - ucdr_init_buffer(&reader, reinterpret_cast(data_buffer), BUFFER_SIZE); -@[end if]@ - -@[if send_topics]@ - // var struct to be updated on the thread - SendThreadArgs *sender_thread_args = new SendThreadArgs(datarate, total_sent, sent_last_sec, sent, sent_loop); - - // create a thread for sending data - pthread_t sender_thread; - launch_send_thread(sender_thread, (*sender_thread_args)); -@[end if]@ - - while (!_should_exit_task) { -@[if recv_topics]@ - - read = transport_node->read(); - if (read > 0) { - total_rcvd += read; - rx_last_sec_read += read; - } - - while (transport_node->parse(&topic_ID, data_buffer, BUFFER_SIZE)) { - uint64_t read_time = hrt_absolute_time(); - - switch (topic_ID) { -@[ for idx, topic in enumerate(recv_topics)]@ - case @(msgs[0].index(topic) + 1): { - @(receive_base_types[idx])_s @(topic)_data; - deserialize_@(receive_base_types[idx])(&reader, &@(topic)_data, data_buffer); - - if (@(topic)_data.timestamp > read_time) { - // don't allow timestamps from the future - @(topic)_data.timestamp = read_time; - } - - pubs->@(topic)_pub.publish(@(topic)_data); - ++received; - } - break; -@[ end for]@ - default: - PX4_WARN("Unexpected topic ID '%hhu' to getMsg. Please make sure the client is capable of parsing the message associated to the topic ID '%hhu'", - topic_ID, topic_ID); - break; - } - } -@[end if]@ - - if (hrt_absolute_time() - last_stats_update >= 1_s) { - rcvd_last_sec = rx_last_sec_read; - rx_last_sec_read = 0; - last_stats_update = hrt_absolute_time(); - } - - // loop forever if informed loop number is negative - if (_options.loops >= 0 && rcvd_loop >= _options.loops) { break; } - - px4_usleep(_options.sleep_us); - ++rcvd_loop; - } - -@[if send_topics]@ - _should_exit_task = true; - pthread_join(sender_thread, nullptr); -@[end if]@ -@[if recv_topics]@ - delete(pubs); -@[end if]@ -} diff --git a/msg/templates/uorb_microcdr/msg.cpp.em b/msg/templates/uorb_microcdr/msg.cpp.em deleted file mode 100644 index 458f455f49..0000000000 --- a/msg/templates/uorb_microcdr/msg.cpp.em +++ /dev/null @@ -1,156 +0,0 @@ -@############################################### -@# -@# PX4 ROS compatible message source code -@# generation for C++ -@# -@# EmPy template for generating .h files -@# Based on the original template for ROS -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - file_name_in (String) Source file -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - search_path (dict) search paths for genmsg -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file @file_name_in */ - -@{ -import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ - -uorb_struct = '%s_s'%spec.short_name -topic_name = spec.short_name -}@ - -#include -#include -#include -#include - -@################################################# -@# Searching for serialize function per each field -@################################################# -@{ - -def print_info(field): - print("type: ", field.type, "name: ", field.name, "base_type: ", \ - field.base_type, "field.is_array:", ('0', '1')[field.is_array], " array_len: ", field.array_len, \ - "is_builtin:", ('0', '1')[field.is_builtin], "is_header:", ('0', '1')[field.is_header]) - -def print_level_info(fields): - for field in fields: - print_info(field) - if (not field.is_builtin): - print("\n") - children_fields = get_children_fields(field.base_type, search_path) - print_level_info(children_fields) - print("\n") - -def walk_through_parsed_fields(): - print_level_info(spec.parsed_fields()) - -def get_serialization_type_name(type_name): - if type_name in type_serialize_map: - return type_serialize_map[type_name] - else: - raise Exception("Type {0} not supported, add to type_serialize_map!".format(type_name)) - -def add_serialize_functions(fields, scope_name): - for field in fields: - if (not field.is_header): - if (field.is_builtin): - if (not field.is_array): - print(" ucdr_serialize_" + str(get_serialization_type_name(field.type)) + "(writer, input->" + scope_name+str(field.name) + ");") - else: - print(" ucdr_serialize_array_" + str(get_serialization_type_name(field.base_type)) + "(writer, input->" + scope_name+str(field.name) + ", " + str(field.array_len) + ");") - else: - name = field.name - children_fields = get_children_fields(field.base_type, search_path) - if (scope_name): name = scope_name + name - if (not field.is_array): - add_serialize_functions(children_fields, name + '.') - else: - for i in range(field.array_len): - add_serialize_functions(children_fields, name + ('[%d].' %i)) - -def add_deserialize_functions(fields, scope_name): - for field in fields: - if (not field.is_header): - if (field.is_builtin): - if (not field.is_array): - print(" ucdr_deserialize_" + str(get_serialization_type_name(field.type)) + "(reader, &output->" + scope_name+str(field.name) + ");") - else: - print(" ucdr_deserialize_array_" + str(get_serialization_type_name(field.base_type)) + "(reader, output->" + scope_name+str(field.name) + ", " + str(field.array_len) + ");") - else: - name = field.name - children_fields = get_children_fields(field.base_type, search_path) - if (scope_name): name = scope_name + name - if (not field.is_array): - add_deserialize_functions(children_fields, name + '.') - else: - for i in range(field.array_len): - add_deserialize_functions(children_fields, name + ('[%d].' %i)) - -def add_code_to_serialize(): - add_serialize_functions(spec.parsed_fields(), "") - -def add_code_to_deserialize(): - add_deserialize_functions(spec.parsed_fields(), "") -}@ - -void serialize_@(topic_name)(ucdrBuffer *writer, const struct @(uorb_struct) *input, char *output, uint32_t *length) -{ - if (nullptr == writer || nullptr == input || nullptr == output || nullptr == length) - return; - - ucdr_reset_buffer(writer); - -@add_code_to_serialize() - (*length) = ucdr_buffer_length(writer); -} - -void deserialize_@(topic_name)(ucdrBuffer *reader, struct @(uorb_struct) *output, const char *input) -{ - if (nullptr == reader || nullptr == output || nullptr == input) - return; - - ucdr_reset_buffer(reader); - -@add_code_to_deserialize() -} diff --git a/msg/templates/uorb_microcdr/msg.h.em b/msg/templates/uorb_microcdr/msg.h.em deleted file mode 100644 index 76b793ab70..0000000000 --- a/msg/templates/uorb_microcdr/msg.h.em +++ /dev/null @@ -1,69 +0,0 @@ -@############################################### -@# -@# PX4 ROS compatible message source code -@# generation for C++ -@# -@# EmPy template for generating .h files -@# Based on the original template for ROS -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - file_name_in (String) Source file -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file @file_name_in */ - -@{ -import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ - -uorb_struct = '%s_s'%spec.short_name -topic_name = spec.short_name -}@ - -#pragma once - -@############################## -@# Generic Includes -@############################## -#include -#include - -void serialize_@(topic_name)(ucdrBuffer *writer, const struct @(uorb_struct) *input, char *output, uint32_t *length); -void deserialize_@(topic_name)(ucdrBuffer *reader, struct @(uorb_struct) *output, const char *input); diff --git a/msg/templates/uorb_microcdr/uORBTopics.cpp.em b/msg/templates/uorb_microcdr/uORBTopics.cpp.em deleted file mode 100644 index 88dc5255d8..0000000000 --- a/msg/templates/uorb_microcdr/uORBTopics.cpp.em +++ /dev/null @@ -1,42 +0,0 @@ -@############################################### -@# -@# EmPy template for generating uORBTopics.cpp file -@# for logging purposes -@# -@############################################### -@# Start of Template -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include diff --git a/msg/templates/uorb_microcdr/uORBTopics.hpp.em b/msg/templates/uorb_microcdr/uORBTopics.hpp.em deleted file mode 100644 index 6eebd31544..0000000000 --- a/msg/templates/uorb_microcdr/uORBTopics.hpp.em +++ /dev/null @@ -1,44 +0,0 @@ -@############################################### -@# -@# EmPy template for generating uORBTopics.cpp file -@# for logging purposes -@# -@############################################### -@# Start of Template -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include diff --git a/msg/templates/urtps/Publisher.cpp.em b/msg/templates/urtps/Publisher.cpp.em deleted file mode 100644 index 86687f06a1..0000000000 --- a/msg/templates/urtps/Publisher.cpp.em +++ /dev/null @@ -1,219 +0,0 @@ -@############################################### -@# -@# EmPy template for generating _uRTPS_UART.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - fastrtps_version (str) FastRTPS version installed on the system -@# - ros2_distro (str) ROS2 distro name -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import genmsg.msgs -from packaging import version -import re - -topic = alias if alias else spec.short_name - -try: - ros2_distro = ros2_distro.decode("utf-8") -except AttributeError: - pass - -topic_name = topic - -# For ROS, use the topic pattern convention defined in -# http://wiki.ros.org/ROS/Patterns/Conventions -if ros2_distro: - topic_name_split = re.sub( r"([A-Z])", r" \1", topic).split() - topic_name = topic_name_split[0] - for w in topic_name_split[1:]: - topic_name += "_" + w - topic_name = topic_name.lower() -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/*! - * @@file @(topic)_Publisher.cpp - * This file contains the implementation of the publisher functions. - * - * This file was adapted from the fastrtpsgen tool. - */ - -#include "@(topic)_Publisher.h" - -#include -#include -#include -#include -#include -#include -@[if version.parse(fastrtps_version) >= version.parse('2.0')]@ -#include - -using SharedMemTransportDescriptor = eprosima::fastdds::rtps::SharedMemTransportDescriptor; -@[end if]@ - - -@(topic)_Publisher::@(topic)_Publisher() - : mp_participant(nullptr), - mp_publisher(nullptr) -{ } - -@(topic)_Publisher::~@(topic)_Publisher() -{ - Domain::removeParticipant(mp_participant); -} - -bool @(topic)_Publisher::init(const std::string &ns, std::string topic_name) -{ - // Create RTPSParticipant - ParticipantAttributes PParam; -@[if version.parse(fastrtps_version) < version.parse('2.0')]@ - PParam.rtps.builtin.domainId = 0; -@[else]@ - PParam.domainId = 0; -@[end if]@ -@[if version.parse(fastrtps_version) <= version.parse('1.8.4')]@ - PParam.rtps.builtin.leaseDuration = c_TimeInfinite; -@[else]@ - PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite; -@[end if]@ -@[if ros2_distro]@ - // ROS2 default memory management policy - PParam.rtps.builtin.writerHistoryMemoryPolicy = PREALLOCATED_WITH_REALLOC_MEMORY_MODE; -@[end if]@ - std::string nodeName = ns; - nodeName.append("@(topic)_publisher"); - PParam.rtps.setName(nodeName.c_str()); - -@[if ros2_distro]@ - // Check if ROS_LOCALHOST_ONLY is set. This means that one wants to use only - // the localhost network for data sharing. If FastRTPS/DDS >= 2.0 and - // RMW_IMPLEMENTATION is FastDDS then the Shared Memory transport is used - const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY"); - const char* rmw_implementation = std::getenv("RMW_IMPLEMENTATION"); - const char* ros_distro = std::getenv("ROS_DISTRO"); - if (localhost_only && strcmp(localhost_only, "1") == 0 - && ((rmw_implementation && ((strcmp(rmw_implementation, "rmw_fastrtps_cpp") == 0) - || (strcmp(rmw_implementation, "rmw_fastrtps_dynamic_cpp") == 0))) - || (!rmw_implementation && ros_distro && strcmp(ros_distro, "foxy") == 0))) { - // Create a custom network UDPv4 transport descriptor - // to whitelist the localhost - auto localhostUdpTransport = std::make_shared(); - localhostUdpTransport->interfaceWhiteList.emplace_back("127.0.0.1"); - - // Disable the built-in Transport Layer - PParam.rtps.useBuiltinTransports = false; - - // Add the descriptor as a custom user transport - PParam.rtps.userTransports.push_back(localhostUdpTransport); - -@[ if version.parse(fastrtps_version) >= version.parse('2.0')]@ - // Add shared memory transport when available - auto shmTransport = std::make_shared(); - PParam.rtps.userTransports.push_back(shmTransport); -@[ end if]@ - } -@[end if]@ - - mp_participant = Domain::createParticipant(PParam); - - if (mp_participant == nullptr) { - return false; - } - - // Register the type - Domain::registerType(mp_participant, static_cast(&@(topic)DataType)); - - // Create Publisher - PublisherAttributes Wparam; - Wparam.topic.topicKind = NO_KEY; - Wparam.topic.topicDataType = @(topic)DataType.getName(); -@[if ros2_distro]@ -@[ if ros2_distro == "ardent"]@ - Wparam.qos.m_partition.push_back("rt"); - std::string topicName = ns; -@[ else]@ - std::string topicName = "rt/"; - topicName.append(ns); -@[ end if]@ - // ROS2 default publish mode QoS policy - Wparam.qos.m_publishMode.kind = ASYNCHRONOUS_PUBLISH_MODE; -@[else]@ - std::string topicName = ns; -@[end if]@ - topic_name.empty() ? topicName.append("fmu/@(topic_name)/out") : topicName.append(topic_name); - Wparam.topic.topicName = topicName; - mp_publisher = Domain::createPublisher(mp_participant, Wparam, static_cast(&m_listener)); - - if (mp_publisher == nullptr) { - return false; - } - - return true; -} - -void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher *pub, MatchingInfo &info) -{ - // The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain - // are the same for all its subcomponents (publishers, subscribers) - bool is_different_endpoint = false; - - for (size_t i = 0; i < 6; i++) { - if (pub->getGuid().guidPrefix.value[i] != info.remoteEndpointGuid.guidPrefix.value[i]) { - is_different_endpoint = true; - break; - } - } - - // If the matching happens for the same entity, do not make a match - if (is_different_endpoint) { - if (info.status == MATCHED_MATCHING) { - n_matched++; - std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) publisher matched\033[0m" << std::endl; - - } else { - n_matched--; - std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) publisher unmatched\033[0m" << std::endl; - } - } -} - -void @(topic)_Publisher::publish(@(topic)_msg_t *st) -{ - mp_publisher->write(st); -} diff --git a/msg/templates/urtps/Publisher.h.em b/msg/templates/urtps/Publisher.h.em deleted file mode 100644 index 6851497d2a..0000000000 --- a/msg/templates/urtps/Publisher.h.em +++ /dev/null @@ -1,120 +0,0 @@ -@############################################### -@# -@# EmPy template for generating _uRTPS_UART.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - ros2_distro (str) ROS2 distro name -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import genmsg.msgs -from packaging import version -import re - -topic = alias if alias else spec.short_name -try: - ros2_distro = ros2_distro.decode("utf-8") -except AttributeError: - pass -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/*! - * @@file @(topic)_Publisher.h - * This header file contains the declaration of the publisher functions. - * - * This file was adapted from the fastrtpsgen tool. - */ - - -#ifndef _@(topic)__PUBLISHER_H_ -#define _@(topic)__PUBLISHER_H_ - -#include -#include - -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -#include "@(topic)_PubSubTypes.h" -@[else]@ -#include "@(topic)PubSubTypes.h" -@[end if]@ - -using namespace eprosima::fastrtps; -using namespace eprosima::fastrtps::rtps; - -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -@[ if ros2_distro]@ -using @(topic)_msg_t = @(package)::msg::dds_::@(topic)_; -using @(topic)_msg_datatype = @(package)::msg::dds_::@(topic)_PubSubType; -@[ else]@ -using @(topic)_msg_t = @(topic)_; -using @(topic)_msg_datatype = @(topic)_PubSubType; -@[ end if]@ -@[else]@ -@[ if ros2_distro]@ -using @(topic)_msg_t = @(package)::msg::@(topic); -using @(topic)_msg_datatype = @(package)::msg::@(topic)PubSubType; -@[ else]@ -using @(topic)_msg_t = @(topic); -using @(topic)_msg_datatype = @(topic)PubSubType; -@[ end if]@ -@[end if]@ - -class @(topic)_Publisher -{ -public: - @(topic)_Publisher(); - virtual ~@(topic)_Publisher(); - bool init(const std::string &ns, std::string topic_name = ""); - void run(); - void publish(@(topic)_msg_t *st); -private: - Participant *mp_participant; - Publisher *mp_publisher; - - class PubListener : public PublisherListener - { - public: - PubListener() : n_matched(0) {}; - ~PubListener() {}; - void onPublicationMatched(Publisher *pub, MatchingInfo &info); - int n_matched; - } m_listener; - @(topic)_msg_datatype @(topic)DataType; -}; - -#endif // _@(topic)__PUBLISHER_H_ diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em deleted file mode 100644 index be1d8d2045..0000000000 --- a/msg/templates/urtps/RtpsTopics.cpp.em +++ /dev/null @@ -1,187 +0,0 @@ -@############################################### -@# -@# EmPy template for generating RtpsTopics.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import genmsg.msgs -import os -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "RtpsTopics.h" - -bool RtpsTopics::init(std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex, - std::queue *t_send_queue, const std::string &ns) -{ -@[if recv_topics]@ - // Initialise subscribers - std::cout << "\033[0;36m--- Subscribers ---\033[0m" << std::endl; -@[for topic in recv_topics]@ - - if (_@(topic)_sub.init(@(msgs[0].index(topic) + 1), t_send_queue_cv, t_send_queue_mutex, t_send_queue, ns)) { - std::cout << "- @(topic) subscriber started" << std::endl; - - } else { - std::cerr << "Failed starting @(topic) subscriber" << std::endl; - return false; - } - -@[end for]@ - std::cout << "\033[0;36m-----------------------\033[0m" << std::endl << std::endl; -@[end if]@ -@[if send_topics]@ - - // Initialise publishers - std::cout << "\033[0;36m---- Publishers ----\033[0m" << std::endl; -@[for topic in send_topics]@ - -@[ if topic == 'Timesync' or topic == 'timesync']@ - if (_@(topic)_pub.init(ns)) { - if (_@(topic)_fmu_in_pub.init(ns, std::string("fmu/timesync/in"))) { - _timesync->start(&_@(topic)_fmu_in_pub); - std::cout << "- @(topic) publishers started" << std::endl; - } -@[ elif topic == 'TimesyncStatus' or topic == 'timesync_status']@ - if (_@(topic)_pub.init(ns, std::string("timesync_status"))) { - _timesync->init_status_pub(&_@(topic)_pub); - std::cout << "- @(topic) publisher started" << std::endl; -@[ else]@ - if (_@(topic)_pub.init(ns)) { - std::cout << "- @(topic) publisher started" << std::endl; -@[ end if]@ - - } else { - std::cerr << "ERROR starting @(topic) publisher" << std::endl; - return false; - } - -@[end for]@ - std::cout << "\033[0;36m-----------------------\033[0m" << std::endl; -@[end if]@ - return true; -} - -@[if send_topics]@ -template -void RtpsTopics::sync_timestamp_of_incoming_data(T &msg) { - uint64_t timestamp = getMsgTimestamp(&msg); - uint64_t timestamp_sample = getMsgTimestampSample(&msg); - _timesync->subtractOffset(timestamp); - setMsgTimestamp(&msg, timestamp); - _timesync->subtractOffset(timestamp_sample); - setMsgTimestampSample(&msg, timestamp_sample); -} - -void RtpsTopics::publish(const uint8_t topic_ID, char data_buffer[], size_t len) -{ - switch (topic_ID) { -@[for topic in send_topics]@ - - case @(msgs[0].index(topic) + 1): { // @(topic) publisher - @(topic)_msg_t st; - eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len); - eprosima::fastcdr::Cdr cdr_des(cdrbuffer); - st.deserialize(cdr_des); -@[ if topic == 'Timesync' or topic == 'timesync']@ - _timesync->processTimesyncMsg(&st, &_@(topic)_pub); -@[ end if]@ - - // apply timestamp offset - sync_timestamp_of_incoming_data(st); - - _@(topic)_pub.publish(&st); - } - break; -@[end for]@ - - default: - printf("\033[1;33m[ micrortps_agent ]\tUnexpected topic ID '%hhu' to publish. Please make sure the agent is capable of parsing the message associated to the topic ID '%hhu'\033[0m\n", - topic_ID, topic_ID); - break; - } -} -@[end if]@ -@[if recv_topics]@ -template -void RtpsTopics::sync_timestamp_of_outgoing_data(T &msg) { - uint64_t timestamp = getMsgTimestamp(&msg); - uint64_t timestamp_sample = getMsgTimestampSample(&msg); - _timesync->addOffset(timestamp); - setMsgTimestamp(&msg, timestamp); - _timesync->addOffset(timestamp_sample); - setMsgTimestampSample(&msg, timestamp_sample); -} - -bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr) -{ - bool ret = false; - - switch (topic_ID) { -@[for topic in recv_topics]@ - - case @(msgs[0].index(topic) + 1): // @(topic) subscriber - if (_@(topic)_sub.hasMsg()) { - @(topic)_msg_t msg = _@(topic)_sub.getMsg(); - - // apply timestamp offset - sync_timestamp_of_outgoing_data(msg); - - msg.serialize(scdr); - ret = true; - - _@(topic)_sub.unlockMsg(); - } - - break; -@[end for]@ - - default: - printf("\033[1;33m[ micrortps_agent ]\tUnexpected topic ID '%hhu' to getMsg. Please make sure the agent is capable of parsing the message associated to the topic ID '%hhu'\033[0m\n", - topic_ID, topic_ID); - break; - } - - return ret; -} -@[end if]@ diff --git a/msg/templates/urtps/RtpsTopics.h.em b/msg/templates/urtps/RtpsTopics.h.em deleted file mode 100644 index b82c7b20c8..0000000000 --- a/msg/templates/urtps/RtpsTopics.h.em +++ /dev/null @@ -1,220 +0,0 @@ -@############################################### -@# -@# EmPy template for generating RtpsTopics.h file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - fastrtps_version (List[str]) FastRTPS version installed on the system -@# - package (List[str]) messages package name. Defaulted to 'px4' -@# - ros2_distro (List[str]) ROS2 distro name -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import genmsg.msgs -import os -from packaging import version -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -package = package[0] -fastrtps_version = fastrtps_version[0] -try: - ros2_distro = ros2_distro[0].decode("utf-8") -except AttributeError: - ros2_distro = ros2_distro[0] -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include - -#include "microRTPS_timesync.h" - -@[for topic in send_topics]@ -#include "@(topic)_Publisher.h" -@[end for]@ -@[for topic in recv_topics]@ -#include "@(topic)_Subscriber.h" -@[end for]@ - - -@[for topic in (recv_topics + send_topics)]@ -@[ if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -@[ if ros2_distro]@ -using @(topic)_msg_t = @(package)::msg::dds_::@(topic)_; -@[ else]@ -using @(topic)_msg_t = @(topic)_; -@[ end if]@ -@[ else]@ -@[ if ros2_distro]@ -using @(topic)_msg_t = @(package)::msg::@(topic); -@[ else]@ -using @(topic)_msg_t = @(topic); -@[ end if]@ -@[ end if]@ -@[end for]@ - -class RtpsTopics -{ -public: - bool init(std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex, std::queue *t_send_queue, - const std::string &ns); - void set_timesync(const std::shared_ptr ×ync) { _timesync = timesync; }; -@[if send_topics]@ - template - void sync_timestamp_of_incoming_data(T &msg); - void publish(const uint8_t topic_ID, char data_buffer[], size_t len); -@[end if]@ -@[if recv_topics]@ - template - void sync_timestamp_of_outgoing_data(T &msg); - bool getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr); -@[end if]@ - -private: -@[if send_topics]@ - /** Publishers **/ -@[for topic in send_topics]@ -@[ if topic == 'Timesync' or topic == 'timesync']@ - @(topic)_Publisher _@(topic)_pub; - @(topic)_Publisher _@(topic)_fmu_in_pub; -@[ else]@ - @(topic)_Publisher _@(topic)_pub; -@[ end if]@ -@[end for]@ -@[end if]@ - -@[if recv_topics]@ - /** Subscribers **/ -@[for topic in recv_topics]@ - @(topic)_Subscriber _@(topic)_sub; -@[end for]@ -@[end if]@ - - // SFINAE - template struct hasTimestampSample{ - private: - template().timestamp_sample(int64_t()))> - static std::true_type detect(int); - template - static std::false_type detect(...); - public: - static constexpr bool value = decltype(detect(0))::value; - }; - - template - inline typename std::enable_if < !hasTimestampSample::value, uint64_t >::type - getMsgTimestampSample_impl(const T *) { return 0; } - - /** Msg metada Getters **/ -@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - template - inline uint64_t getMsgTimestamp(const T *msg) { return msg->timestamp_(); } - - template - inline typename std::enable_if::value, uint64_t>::type - getMsgTimestampSample_impl(const T *msg) { return msg->timestamp_sample_(); } - - template - inline uint8_t getMsgSysID(const T *msg) { return msg->sys_id_(); } - - template - inline uint8_t getMsgSeq(const T *msg) { return msg->seq_(); } -@[elif ros2_distro]@ - template - inline uint64_t getMsgTimestamp(const T *msg) { return msg->timestamp(); } - - template - inline typename std::enable_if::value, uint64_t>::type - getMsgTimestampSample_impl(const T *msg) { return msg->timestamp_sample(); } - - template - inline uint8_t getMsgSysID(const T *msg) { return msg->sys_id(); } - - template - inline uint8_t getMsgSeq(const T *msg) { return msg->seq(); } -@[end if]@ - - template - inline uint64_t getMsgTimestampSample(const T *msg) { return getMsgTimestampSample_impl(msg); } - - template - inline typename std::enable_if ::value, void>::type - setMsgTimestampSample_impl(T *, const uint64_t &) {} - - /** Msg metadata Setters **/ -@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - template - inline void setMsgTimestamp(T *msg, const uint64_t ×tamp) { msg->timestamp_() = timestamp; } - - template - inline typename std::enable_if::value, void>::type - setMsgTimestampSample_impl(T *msg, const uint64_t ×tamp_sample) { msg->timestamp_sample_() = timestamp_sample; } - - template - inline void setMsgSysID(T *msg, const uint8_t &sys_id) { msg->sys_id_() = sys_id; } - - template - inline void setMsgSeq(T *msg, const uint8_t &seq) { msg->seq_() = seq; } -@[elif ros2_distro]@ - template - inline void setMsgTimestamp(T *msg, const uint64_t ×tamp) { msg->timestamp() = timestamp; } - - template - inline typename std::enable_if::value, void>::type - setMsgTimestampSample_impl(T *msg, const uint64_t ×tamp_sample) { msg->timestamp_sample() = timestamp_sample; } - - template - inline void setMsgSysID(T *msg, const uint8_t &sys_id) { msg->sys_id() = sys_id; } - - template - inline void setMsgSeq(T *msg, const uint8_t &seq) { msg->seq() = seq; } -@[end if]@ - - template - inline void setMsgTimestampSample(T *msg, const uint64_t ×tamp_sample) { setMsgTimestampSample_impl(msg, timestamp_sample); } - - /** - * @@brief Timesync object ptr. - * This object is used to compuyte and apply the time offsets to the - * messages timestamps. - */ - std::shared_ptr _timesync; -}; diff --git a/msg/templates/urtps/Subscriber.cpp.em b/msg/templates/urtps/Subscriber.cpp.em deleted file mode 100644 index ab0c5f2d82..0000000000 --- a/msg/templates/urtps/Subscriber.cpp.em +++ /dev/null @@ -1,284 +0,0 @@ -@############################################### -@# -@# EmPy template for generating _uRTPS_UART.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - fastrtps_version (str) FastRTPS version installed on the system -@# - ros2_distro (str) ROS2 distro name -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import genmsg.msgs -from packaging import version -import re - -topic = alias if alias else spec.short_name -try: - ros2_distro = ros2_distro.decode("utf-8") -except AttributeError: - pass - -topic_name = topic - -# For ROS, use the topic pattern convention defined in -# http://wiki.ros.org/ROS/Patterns/Conventions -if ros2_distro: - topic_name_split = re.sub( r"([A-Z])", r" \1", topic).split() - topic_name = topic_name_split[0] - for w in topic_name_split[1:]: - topic_name += "_" + w - topic_name = topic_name.lower() -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/*! - * @@file @(topic)_Subscriber.cpp - * This file contains the implementation of the subscriber functions. - * - * This file was adapted from the fastrtpsgen tool. - */ - -#include "@(topic)_Subscriber.h" - -#include -#include -#include -#include -#include -#include -@[if version.parse(fastrtps_version) >= version.parse('2.0')]@ -#include - -using SharedMemTransportDescriptor = eprosima::fastdds::rtps::SharedMemTransportDescriptor; -@[end if]@ - - -@(topic)_Subscriber::@(topic)_Subscriber() - : mp_participant(nullptr), - mp_subscriber(nullptr) -{ } - -@(topic)_Subscriber::~@(topic)_Subscriber() -{ - Domain::removeParticipant(mp_participant); -} - -bool @(topic)_Subscriber::init(uint8_t topic_ID, std::condition_variable *t_send_queue_cv, - std::mutex *t_send_queue_mutex, std::queue *t_send_queue, const std::string &ns, - std::string topic_name) -{ - m_listener.topic_ID = topic_ID; - m_listener.t_send_queue_cv = t_send_queue_cv; - m_listener.t_send_queue_mutex = t_send_queue_mutex; - m_listener.t_send_queue = t_send_queue; - - // Create RTPSParticipant - ParticipantAttributes PParam; -@[if version.parse(fastrtps_version) < version.parse('2.0')]@ - PParam.rtps.builtin.domainId = 0; -@[else]@ - PParam.domainId = 0; -@[end if]@ -@[if version.parse(fastrtps_version) <= version.parse('1.8.4')]@ - PParam.rtps.builtin.leaseDuration = c_TimeInfinite; -@[else]@ - PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite; -@[end if]@ -@[if ros2_distro]@ - // ROS2 default memory management policy - PParam.rtps.builtin.writerHistoryMemoryPolicy = PREALLOCATED_WITH_REALLOC_MEMORY_MODE; -@[end if]@ - std::string nodeName = ns; - nodeName.append("@(topic)_subscriber"); - PParam.rtps.setName(nodeName.c_str()); - -@[if ros2_distro]@ - // Check if ROS_LOCALHOST_ONLY is set. This means that one wants to use only - // the localhost network for data sharing. If FastRTPS/DDS >= 2.0 and - // RMW_IMPLEMENTATION is FastDDS then the Shared Memory transport is used - const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY"); - const char* rmw_implementation = std::getenv("RMW_IMPLEMENTATION"); - const char* ros_distro = std::getenv("ROS_DISTRO"); - if (localhost_only && strcmp(localhost_only, "1") == 0 - && ((rmw_implementation && ((strcmp(rmw_implementation, "rmw_fastrtps_cpp") == 0) - || (strcmp(rmw_implementation, "rmw_fastrtps_dynamic_cpp") == 0))) - || (!rmw_implementation && ros_distro && strcmp(ros_distro, "foxy") == 0))) { - // Create a custom network UDPv4 transport descriptor - // to whitelist the localhost - auto localhostUdpTransport = std::make_shared(); - localhostUdpTransport->interfaceWhiteList.emplace_back("127.0.0.1"); - - // Disable the built-in Transport Layer - PParam.rtps.useBuiltinTransports = false; - - // Add the descriptor as a custom user transport - PParam.rtps.userTransports.push_back(localhostUdpTransport); - -@[ if version.parse(fastrtps_version) >= version.parse('2.0')]@ - // Add shared memory transport when available - auto shmTransport = std::make_shared(); - PParam.rtps.userTransports.push_back(shmTransport); -@[ end if]@ - } -@[end if]@ - - mp_participant = Domain::createParticipant(PParam); - - if (mp_participant == nullptr) { - return false; - } - - // Register the type - Domain::registerType(mp_participant, static_cast(&@(topic)DataType)); - - // Create Subscriber - SubscriberAttributes Rparam; - Rparam.topic.topicKind = NO_KEY; - Rparam.topic.topicDataType = @(topic)DataType.getName(); -@[if ros2_distro]@ -@[ if ros2_distro == "ardent"]@ - Rparam.qos.m_partition.push_back("rt"); - std::string topicName = ns; -@[ else]@ - std::string topicName = "rt/"; - topicName.append(ns); -@[ end if]@ -@[else]@ - std::string topicName = ns; -@[end if]@ - topic_name.empty() ? topicName.append("fmu/@(topic_name)/in") : topicName.append(topic_name); - Rparam.topic.topicName = topicName; - mp_subscriber = Domain::createSubscriber(mp_participant, Rparam, static_cast(&m_listener)); - - if (mp_subscriber == nullptr) { - return false; - } - - return true; -} - -void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber *sub, MatchingInfo &info) -{ -@# Since the time sync runs on the bridge itself, it is required that there is a -@# match between two topics of the same entity -@[if topic != 'Timesync' and topic != 'timesync' and topic != 'TimesyncStatus' and topic != 'timesync_status']@ - // The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain - // are the same for all its subcomponents (publishers, subscribers) - bool is_different_endpoint = false; - - for (size_t i = 0; i < 6; i++) { - if (sub->getGuid().guidPrefix.value[i] != info.remoteEndpointGuid.guidPrefix.value[i]) { - is_different_endpoint = true; - break; - } - } - - // If the matching happens for the same entity, do not make a match - if (is_different_endpoint) { - if (info.status == MATCHED_MATCHING) { - n_matched++; - std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber matched\033[0m" << std::endl; - - } else { - n_matched--; - std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber unmatched\033[0m" << std::endl; - } - } - -@[else]@ - (void)sub; - - if (info.status == MATCHED_MATCHING) { - n_matched++; - - } else { - n_matched--; - } -@[end if]@ -} - -void @(topic)_Subscriber::SubListener::onNewDataMessage(Subscriber *sub) -{ - if (n_matched > 0) { - std::unique_lock has_msg_lock(has_msg_mutex); - - if (has_msg.load() == true) { // Check if msg has been fetched - has_msg_cv.wait(has_msg_lock); // Wait till msg has been fetched - } - - has_msg_lock.unlock(); - - // Take data - if (sub->takeNextData(&msg, &m_info)) { - if (m_info.sampleKind == ALIVE) { - std::unique_lock lk(*t_send_queue_mutex); - - ++n_msg; - has_msg = true; - - t_send_queue->push(topic_ID); - lk.unlock(); - t_send_queue_cv->notify_one(); - - } - } - } -} - -bool @(topic)_Subscriber::hasMsg() -{ - if (m_listener.n_matched > 0) { - return m_listener.has_msg.load(); - } - - return false; -} - -@(topic)_msg_t @(topic)_Subscriber::getMsg() -{ - return m_listener.msg; -} - -void @(topic)_Subscriber::unlockMsg() -{ - if (m_listener.n_matched > 0) { - std::unique_lock has_msg_lock(m_listener.has_msg_mutex); - m_listener.has_msg = false; - has_msg_lock.unlock(); - m_listener.has_msg_cv.notify_one(); - } -} diff --git a/msg/templates/urtps/Subscriber.h.em b/msg/templates/urtps/Subscriber.h.em deleted file mode 100644 index 703112bef3..0000000000 --- a/msg/templates/urtps/Subscriber.h.em +++ /dev/null @@ -1,140 +0,0 @@ -@############################################### -@# -@# EmPy template for generating _uRTPS_UART.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - ros2_distro (str) ROS2 distro name -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@############################################### -@{ -import genmsg.msgs -from packaging import version -import re - -topic = alias if alias else spec.short_name -try: - ros2_distro = ros2_distro.decode("utf-8") -except AttributeError: - pass -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/*! - * @@file @(topic)_Subscriber.h - * This header file contains the declaration of the subscriber functions. - * - * This file was adapted from the fastrtpsgen tool. - */ - - -#ifndef _@(topic)__SUBSCRIBER_H_ -#define _@(topic)__SUBSCRIBER_H_ - -#include -#include -#include -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -#include "@(topic)_PubSubTypes.h" -@[else]@ -#include "@(topic)PubSubTypes.h" -@[end if]@ - -#include -#include -#include - -using namespace eprosima::fastrtps; -using namespace eprosima::fastrtps::rtps; - -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -@[ if ros2_distro]@ -using @(topic)_msg_t = @(package)::msg::dds_::@(topic)_; -using @(topic)_msg_datatype = @(package)::msg::dds_::@(topic)_PubSubType; -@[ else]@ -using @(topic)_msg_t = @(topic)_; -using @(topic)_msg_datatype = @(topic)_PubSubType; -@[ end if]@ -@[else]@ -@[ if ros2_distro]@ -using @(topic)_msg_t = @(package)::msg::@(topic); -using @(topic)_msg_datatype = @(package)::msg::@(topic)PubSubType; -@[ else]@ -using @(topic)_msg_t = @(topic); -using @(topic)_msg_datatype = @(topic)PubSubType; -@[ end if]@ -@[end if]@ - -class @(topic)_Subscriber -{ -public: - @(topic)_Subscriber(); - virtual ~@(topic)_Subscriber(); - bool init(uint8_t topic_ID, std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex, - std::queue *t_send_queue, const std::string &ns, std::string topic_name = ""); - void run(); - bool hasMsg(); - @(topic)_msg_t getMsg(); - void unlockMsg(); - -private: - Participant *mp_participant; - Subscriber *mp_subscriber; - - class SubListener : public SubscriberListener - { - public: - SubListener() : n_matched(0), n_msg(0), has_msg(false) {}; - ~SubListener() {}; - void onSubscriptionMatched(Subscriber *sub, MatchingInfo &info); - void onNewDataMessage(Subscriber *sub); - SampleInfo_t m_info; - int n_matched; - int n_msg; - @(topic)_msg_t msg; - std::atomic_bool has_msg; - uint8_t topic_ID; - std::condition_variable *t_send_queue_cv; - std::mutex *t_send_queue_mutex; - std::queue *t_send_queue; - std::condition_variable has_msg_cv; - std::mutex has_msg_mutex; - - } m_listener; - @(topic)_msg_datatype @(topic)DataType; -}; - -#endif // _@(topic)__SUBSCRIBER_H_ diff --git a/msg/templates/urtps/microRTPS_agent.cpp.em b/msg/templates/urtps/microRTPS_agent.cpp.em deleted file mode 100644 index 224bf3dcd7..0000000000 --- a/msg/templates/urtps/microRTPS_agent.cpp.em +++ /dev/null @@ -1,408 +0,0 @@ -@############################################### -@# -@# EmPy template for generating microRTPS_agent.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - ros2_distro (List[str]) ROS2 distro name -@############################################### -@{ -import genmsg.msgs -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -try: - ros2_distro = ros2_distro[0].decode("utf-8") -except AttributeError: - ros2_distro = ros2_distro[0] - -send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] -recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -}@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "microRTPS_transport.h" -#include "microRTPS_timesync.h" -#include "RtpsTopics.h" - -@[if ros2_distro]@ -#include -@[end if]@ - -// Default values -#define SLEEP_US 1 -#define MAX_SLEEP_US 1000000 -#define BAUDRATE 460800 -#define MAX_DATA_RATE 10000000 -#define DEVICE "/dev/ttyACM0" -#define POLL_MS 1 -#define MAX_POLL_MS 1000 -#define DEFAULT_RECV_PORT 2020 -#define DEFAULT_SEND_PORT 2019 -#define DEFAULT_IP "127.0.0.1" - -using namespace eprosima; -using namespace eprosima::fastrtps; - -volatile sig_atomic_t running = 1; -std::unique_ptr transport_node; -std::unique_ptr topics; -uint32_t total_sent = 0, sent = 0; - -struct options { - enum class eTransports { - UART, - UDP - }; - eTransports transport = options::eTransports::UART; - char device[64] = DEVICE; - int sleep_us = SLEEP_US; - uint32_t baudrate = BAUDRATE; - int poll_ms = POLL_MS; - uint16_t recv_port = DEFAULT_RECV_PORT; - uint16_t send_port = DEFAULT_SEND_PORT; - char ip[16] = DEFAULT_IP; - bool sw_flow_control = false; - bool hw_flow_control = false; - bool verbose_debug = false; - std::string ns = ""; -} _options; - -static void usage(const char *name) -{ - printf("usage: %s [options]\n\n" - " -b UART device baudrate. Defaults to 460800\n" - " -d UART device. Defaults to /dev/ttyACM0\n" - " -f Activates UART link SW flow control\n" - " -g Activates UART link HW flow control\n" - " -i Target remote IP address for UDP. Defaults to 127.0.0.1\n" - " -n Topics namespace. Identifies the vehicle in a multi-agent network\n" - " -o UART polling timeout in milliseconds. Defaults to 1ms\n" - " -r UDP port for receiving (local). Defaults to 2020\n" - " -s UDP port for sending (remote). Defaults to 2019\n" - " -t [UART|UDP] Defaults to UART\n" - " -v Add more verbosity\n" - " -w Iteration time for data publishing to the DDS world, in microseconds.\n" - " Defaults to 1us\n" - " (ROS2 only) Allows to pass arguments to the timesync ROS2 node.\n" - " Currently used for setting the usage of simulation time by the node using\n" - " '--ros-args -p use_sim_time:=true'\n", - name); -} - -static int parse_options(int argc, char **argv) -{ - static const struct option options[] = { - {"baudrate", required_argument, NULL, 'b'}, - {"device", required_argument, NULL, 'd'}, - {"sw-flow-control", no_argument, NULL, 'f'}, - {"hw-flow-control", no_argument, NULL, 'g'}, - {"ip-address", required_argument, NULL, 'i'}, - {"namespace", required_argument, NULL, 'n'}, - {"poll-ms", required_argument, NULL, 'o'}, - {"reception-port", required_argument, NULL, 'r'}, - {"sending-port", required_argument, NULL, 's'}, - {"transport", required_argument, NULL, 't'}, - {"increase-verbosity", no_argument, NULL, 'v'}, - {"sleep-time-us", required_argument, NULL, 'w'}, - {"ros-args", required_argument, NULL, 0}, - {"help", no_argument, NULL, 'h'}, - {NULL, 0, NULL, 0}}; - - int ch; - - while ((ch = getopt_long(argc, argv, "t:d:w:b:o:r:s:i:fghvn:", options, nullptr)) >= 0) { - switch (ch) { - case 't': _options.transport = strcmp(optarg, "UDP") == 0 ? - options::eTransports::UDP - : options::eTransports::UART; break; - - case 'd': if (nullptr != optarg) strcpy(_options.device, optarg); break; - - case 'w': _options.sleep_us = strtoul(optarg, nullptr, 10); break; - - case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break; - - case 'o': _options.poll_ms = strtol(optarg, nullptr, 10); break; - - case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break; - - case 's': _options.send_port = strtoul(optarg, nullptr, 10); break; - - case 'i': if (nullptr != optarg) strcpy(_options.ip, optarg); break; - - case 'f': _options.sw_flow_control = true; break; - - case 'g': _options.hw_flow_control = true; break; - - case 'h': usage(argv[0]); exit(0); break; - - case 'v': _options.verbose_debug = true; break; - - case 'n': if (nullptr != optarg) _options.ns = std::string(optarg) + "/"; break; - - default: -@[if ros2_distro]@ - break; -@[else]@ - usage(argv[0]); - return -1; -@[end if]@ - } - } - - if (_options.poll_ms < POLL_MS) { - _options.poll_ms = POLL_MS; - printf("\033[1;33m[ micrortps_agent ]\tPoll timeout too low. Using %d ms instead\033[0m\n", POLL_MS); - } else if (_options.poll_ms > MAX_POLL_MS) { - _options.poll_ms = MAX_POLL_MS; - printf("\033[1;33m[ micrortps_agent ]\tPoll timeout too high. Using %d ms instead\033[0m\n", MAX_POLL_MS); - } - - if (_options.sleep_us > MAX_SLEEP_US) { - _options.sleep_us = MAX_SLEEP_US; - printf("\033[1;33m[ micrortps_agent ]\tPublishing iteration cycle too slow. Using %d us instead\033[0m\n", MAX_SLEEP_US); - } - - if (_options.hw_flow_control && _options.sw_flow_control) { - printf("\033[0;31m[ micrortps_agent ]\tHW and SW flow control set. Please set only one or another\033[0m\n"); - return -1; - } - - return 0; -} - -@[if recv_topics]@ -std::atomic exit_sender_thread(false); -std::condition_variable t_send_queue_cv; -std::mutex t_send_queue_mutex; -std::queue t_send_queue; - -void t_send(void *) -{ - char data_buffer[BUFFER_SIZE] = {}; - uint32_t length = 0; - uint8_t topic_ID = 255; - - while (running && !exit_sender_thread) { - std::unique_lock lk(t_send_queue_mutex); - - while (t_send_queue.empty() && !exit_sender_thread) { - t_send_queue_cv.wait(lk); - } - - topic_ID = t_send_queue.front(); - t_send_queue.pop(); - lk.unlock(); - - size_t header_length = transport_node->get_header_length(); - /* make room for the header to fill in later */ - eprosima::fastcdr::FastBuffer cdrbuffer(&data_buffer[header_length], sizeof(data_buffer) - header_length); - eprosima::fastcdr::Cdr scdr(cdrbuffer); - - if (!exit_sender_thread) { - if (topics->getMsg(topic_ID, scdr)) { - length = scdr.getSerializedDataLength(); - - if (0 < (length = transport_node->write(topic_ID, data_buffer, length))) { - total_sent += length; - ++sent; - } - } - } - } -} -@[end if]@ - -void signal_handler(int signum) -{ - printf("\n\033[1;33m[ micrortps_agent ]\tInterrupt signal (%d) received.\033[0m\n", signum); - running = 0; - transport_node->close(); -} - -int main(int argc, char **argv) -{ - printf("\033[0;37m--- MicroRTPS Agent ---\033[0m\n"); - - if (-1 == parse_options(argc, argv)) { - printf("\033[1;33m[ micrortps_agent ]\tEXITING...\033[0m\n"); - return -1; - } - - topics = std::make_unique(); - -@[if ros2_distro]@ - // Initialize communications via the rmw implementation and set up a global signal handler. - rclcpp::init(argc, argv, rclcpp::InitOptions()); -@[end if]@ - - // register signal SIGINT and signal handler - signal(SIGINT, signal_handler); - - printf("[ micrortps_agent ]\tStarting link...\n"); - - const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY"); - - if (localhost_only && strcmp(localhost_only, "1") == 0) { - printf("[ micrortps_agent ]\tUsing only the localhost network...\n"); - } - - /** - * Set the system ID to Mission Computer, in order to identify the agent side - * - * Note: theoretically a multi-agent system is possible, but this would require - * adjustments in the way the timesync is done (would have to create a timesync - * instance per agent). Keeping it contained for a 1:1 link for now is reasonable. - */ - const uint8_t sys_id = static_cast(MicroRtps::System::MISSION_COMPUTER); - - switch (_options.transport) { - case options::eTransports::UART: { - transport_node = std::make_unique(_options.device, _options.baudrate, _options.poll_ms, - _options.sw_flow_control, _options.hw_flow_control, sys_id, _options.verbose_debug); - printf("[ micrortps_agent ]\tUART transport: device: %s; baudrate: %d; poll: %dms; flow_control: %s\n", - _options.device, _options.baudrate, _options.poll_ms, - _options.sw_flow_control ? "SW enabled" : (_options.hw_flow_control ? "HW enabled" : "No")); - } - break; - - case options::eTransports::UDP: { - transport_node = std::make_unique(_options.ip, _options.recv_port, _options.send_port, - sys_id, _options.verbose_debug); - printf("[ micrortps_agent ]\tUDP transport: ip address: %s; recv port: %u; send port: %u\n", - _options.ip, _options.recv_port, _options.send_port); - } - break; - - default: - printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n"); - return -1; - } - - if (0 > transport_node->init()) { - printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n"); - return -1; - } - - sleep(1); - -@[if send_topics]@ - char data_buffer[BUFFER_SIZE] = {}; - int received = 0, loop = 0; - int length = 0, total_read = 0; - bool receiving = false; - uint8_t topic_ID = 255; - std::chrono::time_point start, end; -@[end if]@ - - // Init timesync - topics->set_timesync(std::make_shared(_options.verbose_debug)); - -@[if recv_topics]@ - topics->init(&t_send_queue_cv, &t_send_queue_mutex, &t_send_queue, _options.ns); -@[end if]@ - - running = true; -@[if recv_topics]@ - std::thread sender_thread(t_send, nullptr); -@[end if]@ - - while (running) { -@[if send_topics]@ - ++loop; - if (!receiving) { start = std::chrono::steady_clock::now(); } - - // Publish messages received from UART - length = transport_node->read(); - if (length > 0) { - total_read += length; - } - - while (transport_node->parse(&topic_ID, data_buffer, BUFFER_SIZE)) { - topics->publish(topic_ID, data_buffer, sizeof(data_buffer)); - ++received; - receiving = true; - end = std::chrono::steady_clock::now(); - } -@[else]@ - usleep(_options.sleep_us); -@[end if]@ - } - -@[if recv_topics]@ - exit_sender_thread = true; - t_send_queue_cv.notify_one(); - sender_thread.join(); - - std::chrono::duration elapsed_secs = end - start; - if (received > 0) { - printf("[ micrortps_agent ]\tRECEIVED: %d messages - %d bytes; %d LOOPS - %.03f seconds - %.02fKB/s\n", - received, total_read, loop, elapsed_secs.count(), static_cast(total_read) / (1000 * elapsed_secs.count())); - } -@[end if]@ -@[if recv_topics]@ - if (sent > 0) { - printf("[ micrortps_agent ]\tSENT: %lu messages - %lu bytes\n", static_cast(sent), - static_cast(total_sent)); - } -@[end if]@ - -@[if ros2_distro]@ - rclcpp::shutdown(); -@[end if]@ - transport_node.reset(); - topics.reset(); - - return 0; -} diff --git a/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em b/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em deleted file mode 100644 index f75d13f5d1..0000000000 --- a/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em +++ /dev/null @@ -1,63 +0,0 @@ -################################################################################ -# -# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). -# Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# 3. Neither the name of the copyright holder nor the names of its contributors -# may be used to endorse or promote products derived from this software without -# specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -################################################################################ - -cmake_minimum_required(VERSION 2.8.12) -project(micrortps_agent) - -# Find requirements -find_package(fastrtps REQUIRED) -find_package(fastcdr REQUIRED) - -# Set C++14 -include(CheckCXXCompilerFlag) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG OR - CMAKE_CXX_COMPILER_ID MATCHES "Clang") - check_cxx_compiler_flag(--std=c++14 SUPPORTS_CXX14) - if(SUPPORTS_CXX14) - add_compile_options(--std=c++14) - else() - message(FATAL_ERROR "Compiler doesn't support C++14") - endif() -endif() - -file(GLOB MICRORTPS_AGENT_SOURCES src/*.cpp src/*.h) -add_executable(micrortps_agent ${MICRORTPS_AGENT_SOURCES}) -target_link_libraries(micrortps_agent fastrtps fastcdr) - -# Install to '/usr/local/bin' if `make install` is used -install( - TARGETS micrortps_agent - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib -) diff --git a/msg/templates/urtps/microRTPS_timesync.cpp.em b/msg/templates/urtps/microRTPS_timesync.cpp.em deleted file mode 100644 index a441339fb1..0000000000 --- a/msg/templates/urtps/microRTPS_timesync.cpp.em +++ /dev/null @@ -1,295 +0,0 @@ -@############################################### -@# -@# EmPy template for generating microRTPS_timesync.cpp file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - package (List[str]) messages package name. Defaulted to 'px4' -@# - ros2_distro (List[str]) ROS2 distro name -@############################################### -@{ -import genmsg.msgs -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -package = package[0] -fastrtps_version = fastrtps_version[0] -try: - ros2_distro = ros2_distro[0].decode("utf-8") -except AttributeError: - ros2_distro = ros2_distro[0] -}@ -/**************************************************************************** - * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/*! - * @@file microRTPS_timesync.cpp - * @@brief source code for time sync implementation - */ - -#include -#include -#include - -#include "microRTPS_timesync.h" - -TimeSync::TimeSync(bool debug) - : _offset_ns(-1), -@[if ros2_distro]@ - _timesync_node(std::make_shared("timesync_node")), -@[end if]@ - _skew_ns_per_sync(0.0), - _num_samples(0), - _request_reset_counter(0), - _last_msg_seq(0), - _last_remote_msg_seq(0), - _debug(debug) -{ } - -TimeSync::~TimeSync() { stop(); } - -void TimeSync::start(TimesyncPublisher *pub) -{ - stop(); - -@[if ros2_distro]@ - auto spin_node = [this]() { - rclcpp::spin(_timesync_node); - }; -@[end if]@ - - auto run = [this, pub]() { - while (!_request_stop) { - timesync_msg_t msg = newTimesyncMsg(); - - pub->publish(&msg); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - }; - _request_stop = false; -@[if ros2_distro]@ - _timesync_node_thread.reset(new std::thread(spin_node)); -@[end if]@ - _send_timesync_thread.reset(new std::thread(run)); -} - -void TimeSync::init_status_pub(TimesyncStatusPublisher *status_pub) -{ - auto run = [this, status_pub]() { - while (!_request_stop) { - timesync_status_msg_t status_msg = newTimesyncStatusMsg(); - - status_pub->publish(&status_msg); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - }; - _request_stop = false; - _send_timesync_status_thread.reset(new std::thread(run)); -} - -void TimeSync::stop() -{ - _request_stop = true; - -@[if ros2_distro]@ - if (_timesync_node_thread && _timesync_node_thread->joinable()) { _timesync_node_thread->join(); } -@[end if]@ - if (_send_timesync_thread && _send_timesync_thread->joinable()) { _send_timesync_thread->join(); } - if (_send_timesync_status_thread && _send_timesync_status_thread->joinable()) { _send_timesync_status_thread->join(); } -} - -void TimeSync::reset() -{ - _num_samples = 0; - _request_reset_counter = 0; -} - -@[if ros2_distro]@ -uint64_t TimeSync::getROSTimeNSec() const -{ - return _timesync_node->now().nanoseconds(); -} - -uint64_t TimeSync::getROSTimeUSec() const -{ - return RCL_NS_TO_US(getROSTimeNSec()); -} -@[else]@ -uint64_t TimeSync::getSteadyTimeNSec() const -{ - auto time = std::chrono::steady_clock::now(); - return std::chrono::time_point_cast(time).time_since_epoch().count(); -} - -uint64_t TimeSync::getSteadyTimeUSec() const -{ - auto time = std::chrono::steady_clock::now(); - return std::chrono::time_point_cast(time).time_since_epoch().count(); -} -@[end if]@ - -bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns) -{ - _rtti = local_t3_ns - local_t1_ns; - _remote_time_stamp = remote_t2_ns; - - // assume rtti is evenly split both directions - int64_t remote_t3_ns = remote_t2_ns + _rtti.load() / 2ll; - - int64_t measurement_offset = remote_t3_ns - local_t3_ns; - - if (_request_reset_counter > REQUEST_RESET_COUNTER_THRESHOLD) { - reset(); - - if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync clock changed, resetting\033[0m" << std::endl; } - } - - if (_num_samples == 0) { - updateOffset(measurement_offset); - _skew_ns_per_sync = 0; - } - - if (_num_samples >= WINDOW_SIZE) { - if (std::abs(measurement_offset - _offset_ns.load()) > TRIGGER_RESET_THRESHOLD_NS) { - _request_reset_counter++; - - if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync offset outlier, discarding\033[0m" << std::endl; } - - return false; - - } else { - _request_reset_counter = 0; - } - } - - // ignore if rtti > 50ms - if (_rtti.load() > 50ll * 1000ll * 1000ll) { - if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << _rtti.load() / (1000ll * 1000ll) << "ms\033[0m" << std::endl; } - - return false; - } - - double alpha = ALPHA_FINAL; - double beta = BETA_FINAL; - - if (_num_samples < WINDOW_SIZE) { - double schedule = (double)_num_samples / WINDOW_SIZE; - double s = 1. - exp(.5 * (1. - 1. / (1. - schedule))); - alpha = (1. - s) * ALPHA_INITIAL + s * ALPHA_FINAL; - beta = (1. - s) * BETA_INITIAL + s * BETA_FINAL; - } - - _offset_prev = _offset_ns.load(); - updateOffset(static_cast((_skew_ns_per_sync + _offset_ns.load()) * (1. - alpha) + - measurement_offset * alpha)); - _skew_ns_per_sync = - static_cast(beta * (_offset_ns.load() - _offset_prev.load()) + (1. - beta) * _skew_ns_per_sync); - - _num_samples++; - - return true; -} - -void TimeSync::processTimesyncMsg(timesync_msg_t *msg, TimesyncPublisher *pub) -{ - if (getMsgSeq(msg) != _last_remote_msg_seq) { - _last_remote_msg_seq = getMsgSeq(msg); - - if (getMsgTC1(msg) > 0) { -@[if ros2_distro]@ - if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getROSTimeNSec())) { -@[else]@ - if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getSteadyTimeNSec())) { -@[end if]@ - if (_debug) { std::cerr << "\033[1;33m[ micrortps__timesync ]\tOffset not updated\033[0m" << std::endl; } - } - - } else if (getMsgTC1(msg) == 0) { -@[if ros2_distro]@ - setMsgTimestamp(msg, getROSTimeUSec()); -@[else]@ - setMsgTimestamp(msg, getSteadyTimeUSec()); -@[end if]@ - setMsgSeq(msg, getMsgSeq(msg) + 1); -@[if ros2_distro]@ - setMsgTC1(msg, getROSTimeNSec()); -@[else]@ - setMsgTC1(msg, getSteadyTimeNSec()); -@[end if]@ - - pub->publish(msg); - } - } -} - -timesync_msg_t TimeSync::newTimesyncMsg() -{ - timesync_msg_t msg{}; - -@[if ros2_distro]@ - setMsgTimestamp(&msg, getROSTimeUSec()); -@[else]@ - setMsgTimestamp(&msg, getSteadyTimeUSec()); -@[end if]@ - setMsgSeq(&msg, _last_msg_seq); - setMsgTC1(&msg, 0); -@[if ros2_distro]@ - setMsgTS1(&msg, getROSTimeNSec()); -@[else]@ - setMsgTS1(&msg, getSteadyTimeNSec()); -@[end if]@ - - _last_msg_seq++; - - return msg; -} - -timesync_status_msg_t TimeSync::newTimesyncStatusMsg() -{ - timesync_status_msg_t msg{}; - -@[if ros2_distro]@ - setMsgTimestamp(&msg, getROSTimeUSec()); -@[else]@ - setMsgTimestamp(&msg, getSteadyTimeUSec()); -@[end if]@ - setMsgSourceProtocol(&msg, 1); // SOURCE_PROTOCOL_RTPS - setMsgRemoteTimeStamp(&msg, _remote_time_stamp.load() / 1000ULL); - setMsgObservedOffset(&msg, _offset_prev.load()); - setMsgEstimatedOffset(&msg, _offset_ns.load()); - setMsgRoundTripTime(&msg, _rtti.load() / 1000ll); - - return msg; -} diff --git a/msg/templates/urtps/microRTPS_timesync.h.em b/msg/templates/urtps/microRTPS_timesync.h.em deleted file mode 100644 index ffe4541f18..0000000000 --- a/msg/templates/urtps/microRTPS_timesync.h.em +++ /dev/null @@ -1,303 +0,0 @@ -@############################################### -@# -@# EmPy template for generating microRTPS_timesync.h file -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - package (List[str]) messages package name. Defaulted to 'px4' -@# - ros2_distro (List[str]) ROS2 distro name -@############################################### -@{ -import genmsg.msgs -from packaging import version -from px_generate_uorb_topic_files import MsgScope # this is in Tools/ - -package = package[0] -fastrtps_version = fastrtps_version[0] -try: - ros2_distro = ros2_distro[0].decode("utf-8") -except AttributeError: - ros2_distro = ros2_distro[0] -}@ -/**************************************************************************** - * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/*! - * @@file microRTPS_timesync.h - * @@brief Adds time sync for the microRTPS bridge - * @@author Nuno Marques - * @@author Julian Kent - */ - -#pragma once - -#include -#include -#include - -@[if ros2_distro]@ -#include "Timesync_Publisher.h" -#include "TimesyncStatus_Publisher.h" - -#include -#include -#include -@[else]@ -#include "timesync_Publisher.h" -#include "timesync_status_Publisher.h" -@[end if]@ - -static constexpr double ALPHA_INITIAL = 0.05; -static constexpr double ALPHA_FINAL = 0.003; -static constexpr double BETA_INITIAL = 0.05; -static constexpr double BETA_FINAL = 0.003; -static constexpr int WINDOW_SIZE = 500; -static constexpr int64_t UNKNOWN = 0; -static constexpr int64_t TRIGGER_RESET_THRESHOLD_NS = 100ll * 1000ll * 1000ll; -static constexpr int REQUEST_RESET_COUNTER_THRESHOLD = 5; - -@# Sets the timesync DDS type according to the FastRTPS and ROS2 version -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -@[ if ros2_distro]@ -using timesync_msg_t = @(package)::msg::dds_::Timesync_; -using timesync_status_msg_t = @(package)::msg::dds_::TimesyncStatus_; -@[ else]@ -using timesync_msg_t = timesync_; -using timesync_status_msg_t = timesync_status_; -@[ end if]@ -@[else]@ -@[ if ros2_distro]@ -using timesync_msg_t = @(package)::msg::Timesync; -using timesync_status_msg_t = @(package)::msg::TimesyncStatus; -@[ else]@ -using timesync_msg_t = timesync; -using timesync_status_msg_t = timesync_status; -@[ end if]@ -@[end if]@ -@# Sets the timesync publisher entity depending on using ROS2 or not -@[if ros2_distro]@ -using TimesyncPublisher = Timesync_Publisher; -using TimesyncStatusPublisher = TimesyncStatus_Publisher; -@[else]@ -using TimesyncPublisher = timesync_Publisher; -using TimesyncStatusPublisher = timesync_status_Publisher; -@[end if]@ - -class TimeSync -{ -public: - TimeSync(bool debug); - virtual ~TimeSync(); - - /** - * @@brief Starts the timesync publishing thread - * @@param[in] pub The timesync publisher entity to use - */ - void start(TimesyncPublisher *pub); - - /** - * @@brief Init and run the timesync status publisher thread - * @@param[in] pub The timesync status publisher entity to use - */ - void init_status_pub(TimesyncStatusPublisher *status_pub); - - /** - * @@brief Resets the filter - */ - void reset(); - - /** - * @@brief Stops the timesync publishing thread - */ - void stop(); - -@[if ros2_distro]@ - /** - * @@brief Get ROS time in nanoseconds. This will match the system time, which - * corresponds to the system-wide real time since epoch. If use_sim_time - * is set, the simulation time is grabbed by the node and used instead - * More info about ROS2 clock and time in: - * https://design.ros2.org/articles/clock_and_time.html - * @@return ROS time in nanoseconds - */ - uint64_t getROSTimeNSec() const; - - /** - * @@brief Get ROS time in microseconds. Fetches the time from getROSTimeNSec() - * and converts it to microseconds - * @@return ROS time in microseconds - */ - uint64_t getROSTimeUSec() const; -@[else]@ - /** - * @@brief Get clock monotonic time (raw) in nanoseconds - * @@return Steady CLOCK_MONOTONIC time in nanoseconds - */ - uint64_t getSteadyTimeNSec() const; - - /** - * @@brief Get clock monotonic time (raw) in microseconds - * @@return Steady CLOCK_MONOTONIC time in microseconds - */ - uint64_t getSteadyTimeUSec() const; -@[end if]@ - - /** - * @@brief Adds a time offset measurement to be filtered - * @@param[in] local_t1_ns The agent CLOCK_MONOTONIC_RAW time in nanoseconds when the message was sent - * @@param[in] remote_t2_ns The (client) remote CLOCK_MONOTONIC time in nanoseconds - * @@param[in] local_t3_ns The agent current CLOCK_MONOTONIC time in nanoseconds - * @@return true or false depending if the time offset was updated - */ - bool addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns); - - /** - * @@brief Processes DDS timesync message - * @@param[in,out] msg The timestamp msg to be processed - */ - void processTimesyncMsg(timesync_msg_t *msg, TimesyncPublisher *pub); - - /** - * @@brief Creates a new timesync DDS message to be sent from the agent to the client - * @@return A new timesync message with the origin in the agent and with the agent timestamp - */ - timesync_msg_t newTimesyncMsg(); - - /** - * @@brief Creates a new timesync status DDS message to be sent from the agent to the client - * @@return A new timesync status message with the origin in the agent and with the agent timestamp - */ - timesync_status_msg_t newTimesyncStatusMsg(); - - /** - * @@brief Get the time sync offset in nanoseconds - * @@return The offset in nanoseconds - */ - inline int64_t getOffset() { return _offset_ns.load(); } - - /** - * @@brief Sums the time sync offset to the timestamp - * @@param[in,out] timestamp The timestamp to add the offset to - */ - inline void addOffset(uint64_t ×tamp) { timestamp = (timestamp * 1000LL + _offset_ns.load()) / 1000ULL; } - - /** - * @@brief Substracts the time sync offset to the timestamp - * @@param[in,out] timestamp The timestamp to subtract the offset of - */ - inline void subtractOffset(uint64_t ×tamp) { timestamp = (timestamp * 1000LL - _offset_ns.load()) / 1000ULL; } - -private: - std::atomic _offset_ns; - std::atomic _offset_prev; - std::atomic _remote_time_stamp; - std::atomic _rtti; - -@[if ros2_distro]@ - /** - * @@brief A ROS2 node to fetch the ROS time to be used for timesync - */ - std::shared_ptr _timesync_node; -@[end if]@ - - int64_t _skew_ns_per_sync; - int64_t _num_samples; - - int32_t _request_reset_counter; - uint8_t _last_msg_seq; - uint8_t _last_remote_msg_seq; - - bool _debug; - - std::unique_ptr _send_timesync_thread; - std::unique_ptr _send_timesync_status_thread; -@[if ros2_distro]@ - std::unique_ptr _timesync_node_thread; -@[end if]@ - std::atomic _request_stop{false}; - - /** - * @@brief Updates the offset of the time sync filter - * @@param[in] offset The value of the offset to update to - */ - inline void updateOffset(const uint64_t &offset) { _offset_ns.store(offset, std::memory_order_relaxed); } - - /** Timesync msg Getters **/ -@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - inline uint64_t getMsgTimestamp(const timesync_msg_t *msg) { return msg->timestamp_(); } - inline uint8_t getMsgSeq(const timesync_msg_t *msg) { return msg->seq_(); } - inline int64_t getMsgTC1(const timesync_msg_t *msg) { return msg->tc1_(); } - inline int64_t getMsgTS1(const timesync_msg_t *msg) { return msg->ts1_(); } -@[elif ros2_distro]@ - inline uint64_t getMsgTimestamp(const timesync_msg_t *msg) { return msg->timestamp(); } - inline uint8_t getMsgSeq(const timesync_msg_t *msg) { return msg->seq(); } - inline int64_t getMsgTC1(const timesync_msg_t *msg) { return msg->tc1(); } - inline int64_t getMsgTS1(const timesync_msg_t *msg) { return msg->ts1(); } - @[end if]@ - - /** Common timestamp setter **/ -@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - template - inline void setMsgTimestamp(T *msg, const uint64_t ×tamp) { msg->timestamp_() = timestamp; } -@[elif ros2_distro]@ - template - inline void setMsgTimestamp(T *msg, const uint64_t ×tamp) { msg->timestamp() = timestamp; } -@[end if]@ - - /** Timesync msg Setters **/ -@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - inline void setMsgSeq(timesync_msg_t *msg, const uint8_t &seq) { msg->seq_() = seq; } - inline void setMsgTC1(timesync_msg_t *msg, const int64_t &tc1) { msg->tc1_() = tc1; } - inline void setMsgTS1(timesync_msg_t *msg, const int64_t &ts1) { msg->ts1_() = ts1; } -@[elif ros2_distro]@ - inline void setMsgSeq(timesync_msg_t *msg, const uint8_t &seq) { msg->seq() = seq; } - inline void setMsgTC1(timesync_msg_t *msg, const int64_t &tc1) { msg->tc1() = tc1; } - inline void setMsgTS1(timesync_msg_t *msg, const int64_t &ts1) { msg->ts1() = ts1; } -@[end if]@ - - /** Timesync Status msg Setters **/ -@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_protocol_() = source_protocol; } - inline void setMsgRemoteTimeStamp(timesync_status_msg_t *msg, const uint64_t &remote_timestamp) { msg->remote_timestamp_() = remote_timestamp; } - inline void setMsgObservedOffset(timesync_status_msg_t *msg, const int64_t &observed_offset) { msg->observed_offset_() = observed_offset; } - inline void setMsgEstimatedOffset(timesync_status_msg_t *msg, const int64_t &estimated_offset) { msg->estimated_offset_() = estimated_offset; } - inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time_() = round_trip_time; } -@[elif ros2_distro]@ - inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_protocol() = source_protocol; } - inline void setMsgRemoteTimeStamp(timesync_status_msg_t *msg, const uint64_t &remote_timestamp) { msg->remote_timestamp() = remote_timestamp; } - inline void setMsgObservedOffset(timesync_status_msg_t *msg, const int64_t &observed_offset) { msg->observed_offset() = observed_offset; } - inline void setMsgEstimatedOffset(timesync_status_msg_t *msg, const int64_t &estimated_offset) { msg->estimated_offset() = estimated_offset; } - inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time() = round_trip_time; } -@[end if]@ -}; diff --git a/msg/templates/urtps/microRTPS_transport.cpp b/msg/templates/urtps/microRTPS_transport.cpp deleted file mode 100644 index 51bddadbac..0000000000 --- a/msg/templates/urtps/microRTPS_transport.cpp +++ /dev/null @@ -1,841 +0,0 @@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include -#if __has_include("px4_platform_common/log.h") && __has_include("px4_platform_common/time.h") -#include -#include -#endif - -#if defined(__linux__) || defined(__PX4_LINUX) -#include -#endif /* __linux__ */ - -#include "microRTPS_transport.h" - - -/** CRC table for the CRC-16. The poly is 0x8005 (x^16 + x^15 + x^2 + 1) */ -uint16_t const crc16_table[256] = { - 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, - 0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440, - 0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40, - 0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841, - 0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40, - 0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41, - 0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641, - 0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040, - 0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240, - 0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441, - 0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41, - 0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840, - 0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41, - 0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40, - 0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640, - 0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041, - 0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240, - 0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441, - 0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41, - 0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840, - 0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41, - 0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40, - 0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640, - 0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041, - 0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241, - 0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440, - 0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40, - 0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841, - 0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40, - 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, - 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, - 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 -}; - -Transport_node::Transport_node(const uint8_t sys_id, const bool debug): - _rx_buff_pos(0), - _debug(debug), - _sys_id(sys_id) -{ -} - -Transport_node::~Transport_node() -{ -} - -uint16_t Transport_node::crc16_byte(uint16_t crc, const uint8_t data) -{ - return (crc >> 8) ^ crc16_table[(crc ^ data) & 0xff]; -} - -uint16_t Transport_node::crc16(uint8_t const *buffer, size_t len) -{ - uint16_t crc = 0; - - while (len--) { - crc = crc16_byte(crc, *buffer++); - } - - return crc; -} - -ssize_t Transport_node::read() -{ - ssize_t len = node_read((void *)(_rx_buffer + _rx_buff_pos), sizeof(_rx_buffer) - _rx_buff_pos); - - if (len < 0) { - int errsv = errno; - - if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv) { -#ifndef PX4_DEBUG - - if (_debug) { printf("\033[0;31m[ micrortps_transport ]\tRead fail %d\033[0m\n", errsv); } - -#else - - if (_debug) { PX4_DEBUG("Read fail %d", errsv); } - -#endif /* PX4_DEBUG */ - } - - return len; - } - - _rx_buff_pos += len; - - return len; -} - -#ifndef PX4_DEBUG - -void Transport_node::print_buffer_debug() -{ - for (uint32_t i = 0; i < BUFFER_SIZE; ++i) { - if (i >= _rx_buff_pos) { - printf("."); - continue; - } - - printf("%" PRIi8, _rx_buffer[i]); - - if (_rx_buffer[i] == '>') { - printf("(X)"); - } - - printf(" "); - } - - printf("\n_rx_buff_pos: %" PRIu32 "\n", _rx_buff_pos); -} - -#endif /* PX4_DEBUG */ - -bool Transport_node::parse( - uint8_t *topic_id, char out_buffer[], size_t buffer_len) -{ - if (nullptr == out_buffer || nullptr == topic_id || !fds_OK()) { - return false; - } - - *topic_id = 255; - - static constexpr size_t header_size = sizeof(struct Header); - - // No need to look for a message if not even a header fits into the buffer. - if (_rx_buff_pos < header_size) { - return false; - } - - // Try to find the start of a message using the magic start sequence of '>>>'. - int32_t msg_start_pos = -1; - - for (uint32_t i = 0; i <= _rx_buff_pos; ++i) { - if ('>' == _rx_buffer[i] && memcmp(_rx_buffer + i, ">>>", 3) == 0) { - msg_start_pos = i; - break; - } - } - - auto DropDataBeforeIndex = [&](uint32_t index) { - if (index == 0) { - return; - } - - if (index > _rx_buff_pos) { - index = _rx_buff_pos; - -#ifndef PX4_DEBUG - - if (_debug) { printf("\033[0;31m[ micrortps_transport ]\t clamped index from %" PRIu32 " to %" PRIu32 "\033[0m\n", index, _rx_buff_pos); } - -#else - - if (_debug) { PX4_ERR("ERROR: clamped index from %" PRIu32 " to %" PRIu32 "\n", index, _rx_buff_pos); } - -#endif /* PX4_DEBUG */ - } - - memmove(_rx_buffer, _rx_buffer + index, _rx_buff_pos - index); - _rx_buff_pos -= index; - }; - - // No start sequence has been found in the buffer. We can drop all data up to - // 2 characters before the end (to account for a non-complete start sequence). - if (msg_start_pos == -1) { - // Note, _rx_buff_pos should always be larger than 2 as we enforce a min. - // buffer content corresponding to a header length some lines above. - int32_t drop_before_idx = _rx_buff_pos - 2; - DropDataBeforeIndex(drop_before_idx); - return false; - } - - // A start sequence has been found. We can drop everything before this index - // in the buffer. This could happen at the startup when syncing to the - // stream or with noise on the data stream. - DropDataBeforeIndex(msg_start_pos); - - // We have found a start sequence. Let's check if the header is in the buffer. - // [>,>,>,topic_id,sys_id,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart, - // ... ,payloadEnd] - if (header_size > _rx_buff_pos) { - // Header not yet in the buffer. - return false; - } - - struct Header *header = (struct Header *)&_rx_buffer[0]; - - uint32_t payload_len = - ((uint32_t)header->payload_len_h << 8) | header->payload_len_l; - - // The message won't fit the output or processing buffer. This could happen - // with a corrupted header or by an actual message that is too large for our - // buffers. Let's drop it. - uint32_t message_length = header_size + payload_len; - - if (message_length > buffer_len || (message_length + 3) > BUFFER_SIZE) { - DropDataBeforeIndex(3 + header_size); - return false; - } - - // Let's check if all payload data according to the message length in the - // header is already in the buffer. - if ((header_size + payload_len) > _rx_buff_pos) { - // The buffer does not yet contain the full message. We need to wait for - // more data. - return false; - } - - // The received message comes from this system. Discard it. - // This might happen when: - // 1. The same UDP port is being used to send a rcv packets or - // 2. The same topic on the agent is being used for outgoing and incoming - // data - if (header->sys_id == _sys_id) { - DropDataBeforeIndex(3); - return false; - } - - // Check the CRC of the message. - uint16_t read_crc = ((uint16_t)header->crc_h << 8) | header->crc_l; - uint16_t calc_crc = crc16((uint8_t *)_rx_buffer + header_size, payload_len); - - if (read_crc != calc_crc) { -#ifndef PX4_DEBUG - - if (_debug) { printf("\033[0;31m[ micrortps_transport ]\tBad CRC %" PRIu16 " != %" PRIu16 "\t\t(↓ %lu)\033[0m\n", read_crc, calc_crc, (unsigned long)(header_size + payload_len)); } - -#else - - if (_debug) { PX4_DEBUG("Bad CRC %u != %u\t\t(↓ %lu)", read_crc, calc_crc, (unsigned long)(header_size + payload_len)); } - -#endif /* PX4_DEBUG */ - - DropDataBeforeIndex(3); - return false; - } - - // Copy message to output buffer and drop it from the buffer. - memmove(out_buffer, _rx_buffer + header_size, payload_len); - *topic_id = header->topic_id; - - DropDataBeforeIndex(header_size + payload_len); - return true; -} - -size_t Transport_node::get_header_length() -{ - return sizeof(struct Header); -} - -ssize_t Transport_node::write(const uint8_t topic_id, char buffer[], size_t length) -{ - if (!fds_OK()) { - return -1; - } - - static struct Header header = {{'>', '>', '>'}, 0u, 0u, 0u, 0u, 0u, 0u, 0u}; - - // [>,>,>,topic_id,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end] - uint16_t crc = crc16((uint8_t *)&buffer[sizeof(header)], length); - - header.topic_id = topic_id; - header.sys_id = _sys_id; - header.seq = _seq_number++; - header.payload_len_h = (length >> 8) & 0xff; - header.payload_len_l = length & 0xff; - header.crc_h = (crc >> 8) & 0xff; - header.crc_l = crc & 0xff; - - /* Headroom for header is created in client */ - /* Fill in the header in the same payload buffer to call a single node_write */ - memcpy(buffer, &header, sizeof(header)); - ssize_t len = node_write(buffer, length + sizeof(header)); - - if (len != ssize_t(length + sizeof(header))) { - return len; - } - - return len + sizeof(header); -} - -UART_node::UART_node(const char *uart_name, const uint32_t baudrate, - const uint32_t poll_ms, const bool hw_flow_control, - const bool sw_flow_control, const uint8_t sys_id, - const bool debug): - Transport_node(sys_id, debug), - _uart_fd(-1), - _baudrate(baudrate), - _poll_ms(poll_ms), - _hw_flow_control(hw_flow_control), - _sw_flow_control(sw_flow_control) -{ - - if (nullptr != uart_name) { - strcpy(_uart_name, uart_name); - } -} - -UART_node::~UART_node() -{ - close(); -} - -int UART_node::init() -{ - // Open a serial port - _uart_fd = open(_uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); - - if (_uart_fd < 0) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: Failed to open device: %s (%d)\033[0m\n", _uart_name, errno); -#else - PX4_ERR("UART transport: Failed to open device: %s (%d)", _uart_name, errno); -#endif /* PX4_ERR */ - return -errno; - } - - // If using shared UART, no need to set it up - if (_baudrate == 0) { - _poll_fd[0].fd = _uart_fd; - _poll_fd[0].events = POLLIN; - return _uart_fd; - } - - // Try to set baud rate - struct termios uart_config; - int termios_state; - - // Back up the original uart configuration to restore it after exit - if ((termios_state = tcgetattr(_uart_fd, &uart_config)) < 0) { - int errno_bkp = errno; -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR GET CONF %s: %d (%d)\n\033[0m", _uart_name, termios_state, - errno); -#else - PX4_ERR("UART transport: ERR GET CONF %s: %d (%d)", _uart_name, termios_state, errno); -#endif /* PX4_ERR */ - close(); - return -errno_bkp; - } - -#if defined(__linux__) || defined(__PX4_LINUX) - uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); - uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); - - uart_config.c_lflag &= ~(ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE | ECHONL | ICANON | IEXTEN | ISIG); - - // never send SIGTTOU - uart_config.c_lflag &= ~(TOSTOP); - - // ignore modem control lines - uart_config.c_cflag |= CLOCAL; - - // 8 bits - uart_config.c_cflag |= CS8; -#else /* __linux__ */ - - // Clear ONLCR flag (which appends a CR for every LF) - uart_config.c_oflag &= ~ONLCR; -#endif - - // Flow control - if (_hw_flow_control) { - // HW flow control - uart_config.c_cflag |= CRTSCTS; - uart_config.c_iflag &= ~(IXON | IXOFF | IXANY); - } else if (_sw_flow_control) { - // SW flow control - uart_config.c_cflag &= ~CRTSCTS; - uart_config.c_lflag |= (IXON | IXOFF | IXANY); - } else { - uart_config.c_cflag &= ~CRTSCTS; - uart_config.c_iflag &= ~(IXON | IXOFF | IXANY); - } - - // Set baud rate - speed_t speed; - - if (!baudrate_to_speed(_baudrate, &speed)) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET BAUD %s: Unsupported _baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\033[0m\n", - _uart_name, _baudrate); -#else - PX4_ERR("UART transport: ERR SET BAUD %s: Unsupported baudrate: %" PRIu32 "\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\n", - _uart_name, _baudrate); -#endif /* PX4_ERR */ - close(); - return -EINVAL; - } - - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - int errno_bkp = errno; -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET BAUD %s: %d (%d)\033[0m\n", _uart_name, termios_state, - errno); -#else - PX4_ERR("ERR SET BAUD %s: %d (%d)", _uart_name, termios_state, errno); -#endif /* PX4_ERR */ - close(); - return -errno_bkp; - } - - if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { - int errno_bkp = errno; -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET CONF %s (%d)\033[0m\n", _uart_name, errno); -#else - PX4_ERR("UART transport: ERR SET CONF %s (%d)", _uart_name, errno); -#endif /* PX4_ERR */ - close(); - return -errno_bkp; - } - -#if defined(__linux__) || defined(__PX4_LINUX) - // For Linux, set high speed polling at the chip level. Since this routine relies on a USB latency - // change at the chip level it may fail on certain chip sets if their driver does not support this - // configuration request - { - struct serial_struct serial_ctl; - - if (ioctl(_uart_fd, TIOCGSERIAL, &serial_ctl) < 0) { - printf("\033[0;31m[ micrortps_transport ]\tError while trying to read serial port configuration: %d\033[0m\n", errno); - - if (ioctl(_uart_fd, TCFLSH, TCIOFLUSH) == -1) { - int errno_bkp = errno; - printf("\033[0;31m[ protocol__splitter ]\tCould not flush terminal\033[0m\n"); - close(); - return -errno_bkp; - } - } - - serial_ctl.flags |= ASYNC_LOW_LATENCY; - - if (ioctl(_uart_fd, TIOCSSERIAL, &serial_ctl) < 0) { - int errno_bkp = errno; - printf("\033[0;31m[ micrortps_transport ]\tError while trying to write serial port latency: %d\033[0m\n", errno); - close(); - return -errno_bkp; - } - } -#endif /* __linux__ */ - - char aux[64]; - bool flush = false; - - while (0 < ::read(_uart_fd, (void *)&aux, 64)) { - flush = true; -#ifndef px4_usleep - usleep(1000); -#else - /* With PX4 px4_usleep() should be used. */ - px4_usleep(1000); -#endif /* px4_usleep */ - } - - if (flush) { -#ifndef PX4_DEBUG - - if (_debug) { printf("[ micrortps_transport ]\tUART transport: Flush\n"); } - -#else - - if (_debug) { PX4_DEBUG("UART transport: Flush"); } - -#endif /* PX4_DEBUG */ - - } else { -#ifndef PX4_DEBUG - - if (_debug) { printf("[ micrortps_transport ]\tUART transport: No flush\n"); } - -#else - - if (_debug) { PX4_DEBUG("UART transport: No flush"); } - -#endif /* PX4_INFO */ - } - - _poll_fd[0].fd = _uart_fd; - _poll_fd[0].events = POLLIN; - - return _uart_fd; -} - -bool UART_node::fds_OK() -{ - return (-1 != _uart_fd); -} - -uint8_t UART_node::close() -{ - if (-1 != _uart_fd) { -#ifndef PX4_WARN - printf("\033[1;33m[ micrortps_transport ]\tClosed UART.\n\033[0m"); -#else - PX4_WARN("Closed UART."); -#endif /* PX4_WARN */ - ::close(_uart_fd); - _uart_fd = -1; - memset(&_poll_fd, 0, sizeof(_poll_fd)); - } - - return 0; -} - -ssize_t UART_node::node_read(void *buffer, size_t len) -{ - if (nullptr == buffer || !fds_OK()) { - return -1; - } - - ssize_t ret = 0; - int r = poll(_poll_fd, 1, _poll_ms); - - if (r == 1 && (_poll_fd[0].revents & POLLIN)) { - ret = ::read(_uart_fd, buffer, len); - } - - return ret; -} - -ssize_t UART_node::node_write(void *buffer, size_t len) -{ - if (nullptr == buffer || !fds_OK()) { - return -1; - } - - return ::write(_uart_fd, buffer, len); -} - -bool UART_node::baudrate_to_speed(uint32_t bauds, speed_t *speed) -{ -#ifndef B460800 -#define B460800 460800 -#endif - -#ifndef B500000 -#define B500000 500000 -#endif - -#ifndef B921600 -#define B921600 921600 -#endif - -#ifndef B1000000 -#define B1000000 1000000 -#endif - -#ifndef B1500000 -#define B1500000 1500000 -#endif - -#ifndef B2000000 -#define B2000000 2000000 -#endif - - switch (bauds) { - case 0: *speed = B0; break; - - case 50: *speed = B50; break; - - case 75: *speed = B75; break; - - case 110: *speed = B110; break; - - case 134: *speed = B134; break; - - case 150: *speed = B150; break; - - case 200: *speed = B200; break; - - case 300: *speed = B300; break; - - case 600: *speed = B600; break; - - case 1200: *speed = B1200; break; - - case 1800: *speed = B1800; break; - - case 2400: *speed = B2400; break; - - case 4800: *speed = B4800; break; - - case 9600: *speed = B9600; break; - - case 19200: *speed = B19200; break; - - case 38400: *speed = B38400; break; - - case 57600: *speed = B57600; break; - - case 115200: *speed = B115200; break; - - case 230400: *speed = B230400; break; - - case 460800: *speed = B460800; break; - - case 500000: *speed = B500000; break; - - case 921600: *speed = B921600; break; - - case 1000000: *speed = B1000000; break; - - case 1500000: *speed = B1500000; break; - - case 2000000: *speed = B2000000; break; - -#ifdef B3000000 - case 3000000: *speed = B3000000; break; - -#endif -#ifdef B3500000 - case 3500000: *speed = B3500000; break; - -#endif -#ifdef B4000000 - case 4000000: *speed = B4000000; break; - -#endif - default: - return false; - } - - return true; -} - -UDP_node::UDP_node(const char *udp_ip, uint16_t udp_port_recv, - uint16_t udp_port_send, const uint8_t sys_id, const bool debug): - Transport_node(sys_id, debug), - _sender_fd(-1), - _receiver_fd(-1), - _udp_port_recv(udp_port_recv), - _udp_port_send(udp_port_send) -{ - if (nullptr != udp_ip) { - strcpy(_udp_ip, udp_ip); - } -} - -UDP_node::~UDP_node() -{ - close(); -} - -int UDP_node::init() -{ - if (0 > init_receiver(_udp_port_recv) || 0 > init_sender(_udp_port_send)) { - return -1; - } - - return 0; -} - -bool UDP_node::fds_OK() -{ - return (-1 != _sender_fd && -1 != _receiver_fd); -} - -int UDP_node::init_receiver(uint16_t udp_port) -{ -#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - // udp socket data - memset((char *)&_receiver_inaddr, 0, sizeof(_receiver_inaddr)); - _receiver_inaddr.sin_family = AF_INET; - _receiver_inaddr.sin_port = htons(udp_port); - _receiver_inaddr.sin_addr.s_addr = htonl(INADDR_ANY); - - if ((_receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Create socket failed\033[0m\n"); -#else - PX4_ERR("UDP transport: Create socket failed"); -#endif /* PX4_ERR */ - return -1; - } - -#ifndef PX4_INFO - printf("[ micrortps_transport ]\tUDP transport: Trying to connect...\n"); -#else - PX4_INFO("UDP transport: Trying to connect..."); -#endif /* PX4_INFO */ - - if (bind(_receiver_fd, (struct sockaddr *)&_receiver_inaddr, sizeof(_receiver_inaddr)) < 0) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Bind failed\033[0m\n"); -#else - PX4_ERR("UDP transport: Bind failed"); -#endif /* PX4_ERR */ - return -1; - } - -#ifndef PX4_INFO - printf("[ micrortps_transport ]\tUDP transport: Connected to server!\n\n"); -#else - PX4_INFO("UDP transport: Connected to server!"); -#endif /* PX4_INFO */ -#endif /* __PX4_NUTTX */ - return 0; -} - -int UDP_node::init_sender(uint16_t udp_port) -{ -#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - - if ((_sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Create socket failed\033[0m\n"); -#else - PX4_ERR("UDP transport: Create socket failed"); -#endif /* PX4_ERR */ - return -1; - } - - memset((char *) &_sender_outaddr, 0, sizeof(_sender_outaddr)); - _sender_outaddr.sin_family = AF_INET; - _sender_outaddr.sin_port = htons(udp_port); - - if (inet_aton(_udp_ip, &_sender_outaddr.sin_addr) == 0) { -#ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUDP transport: inet_aton() failed\033[0m\n"); -#else - PX4_ERR("UDP transport: inet_aton() failed"); -#endif /* PX4_ERR */ - return -1; - } - -#endif /* __PX4_NUTTX */ - - return 0; -} - -uint8_t UDP_node::close() -{ -#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - - if (_sender_fd != -1) { -#ifndef PX4_WARN - printf("\033[1;33m[ micrortps_transport ]\tUDP transport: Closed sender socket!\033[0m\n"); -#else - PX4_WARN("UDP transport: Closed sender socket!"); -#endif /* PX4_WARN */ - shutdown(_sender_fd, SHUT_RDWR); - ::close(_sender_fd); - _sender_fd = -1; - } - - if (_receiver_fd != -1) { -#ifndef PX4_WARN - printf("\033[1;33m[ micrortps_transport ]\tUDP transport: Closed receiver socket!\033[0m\n"); -#else - PX4_WARN("UDP transport: Closed receiver socket!"); -#endif /* PX4_WARN */ - shutdown(_receiver_fd, SHUT_RDWR); - ::close(_receiver_fd); - _receiver_fd = -1; - } - -#endif /* __PX4_NUTTX */ - return 0; -} - -ssize_t UDP_node::node_read(void *buffer, size_t len) -{ - if (nullptr == buffer || !fds_OK()) { - return -1; - } - - ssize_t ret = 0; -#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - // Blocking call - static socklen_t addrlen = sizeof(_receiver_outaddr); - ret = recvfrom(_receiver_fd, buffer, len, 0, (struct sockaddr *)&_receiver_outaddr, &addrlen); -#endif /* !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) */ - return ret; -} - -ssize_t UDP_node::node_write(void *buffer, size_t len) -{ - if (nullptr == buffer || !fds_OK()) { - return -1; - } - - ssize_t ret = 0; -#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - ret = sendto(_sender_fd, buffer, len, 0, (struct sockaddr *)&_sender_outaddr, sizeof(_sender_outaddr)); -#endif /* !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) */ - return ret; -} diff --git a/msg/templates/urtps/microRTPS_transport.h b/msg/templates/urtps/microRTPS_transport.h deleted file mode 100644 index a9c3567e6d..0000000000 --- a/msg/templates/urtps/microRTPS_transport.h +++ /dev/null @@ -1,164 +0,0 @@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include - -#define BUFFER_SIZE 1024 -#define DEFAULT_UART "/dev/ttyACM0" - -namespace MicroRtps { - enum class System { - FMU, - MISSION_COMPUTER - }; -} - -class Transport_node -{ -public: - Transport_node(const uint8_t sys_id, const bool debug); - virtual ~Transport_node(); - - virtual int init() {return 0;} - virtual uint8_t close() {return 0;} - - ssize_t read(); - bool parse(uint8_t *topic_id, char out_buffer[], size_t buffer_len); - - /** - * write a buffer - * @param topic_id - * @param buffer buffer to write: it must leave get_header_length() bytes free at the beginning. This will be - * filled with the header. length does not include get_header_length(). So buffer looks like this: - * ------------------------------------------------- - * | header (leave free) | payload data | - * | get_header_length() bytes | length bytes | - * ------------------------------------------------- - * @param length buffer length excluding header length - * @return length on success, <0 on error - */ - ssize_t write(const uint8_t topic_id, char buffer[], size_t length); - - /** Get the Length of struct Header to make headroom for the size of struct Header along with payload */ - size_t get_header_length(); - -private: -#ifndef PX4_DEBUG - void print_buffer_debug(); -#endif /* PX4_DEBUG */ - - struct __attribute__((packed)) Header { - char marker[3]; - uint8_t topic_id; - uint8_t sys_id; - uint8_t seq; - uint8_t payload_len_h; - uint8_t payload_len_l; - uint8_t crc_h; - uint8_t crc_l; - }; - -protected: - virtual ssize_t node_read(void *buffer, size_t len) = 0; - virtual ssize_t node_write(void *buffer, size_t len) = 0; - virtual bool fds_OK() = 0; - uint16_t crc16_byte(uint16_t crc, const uint8_t data); - uint16_t crc16(uint8_t const *buffer, size_t len); - - uint32_t _rx_buff_pos; - char _rx_buffer[BUFFER_SIZE]{}; - - bool _debug; - - uint8_t _sys_id; - uint8_t _seq_number{0}; -}; - -class UART_node: public Transport_node -{ -public: - UART_node(const char *uart_name, const uint32_t baudrate, - const uint32_t poll_ms, const bool hw_flow_control, - const bool sw_flow_control, const uint8_t sys_id, - const bool debug); - virtual ~UART_node(); - - int init(); - uint8_t close(); - -protected: - ssize_t node_read(void *buffer, size_t len); - ssize_t node_write(void *buffer, size_t len); - bool fds_OK(); - bool baudrate_to_speed(uint32_t bauds, speed_t *speed); - - int _uart_fd; - char _uart_name[64]{}; - uint32_t _baudrate; - uint32_t _poll_ms; - bool _hw_flow_control{false}; - bool _sw_flow_control{false}; - struct pollfd _poll_fd[1]{}; -}; - -class UDP_node: public Transport_node -{ -public: - UDP_node(const char *udp_ip, uint16_t udp_port_recv, uint16_t udp_port_send, - const uint8_t sys_id, const bool debug); - virtual ~UDP_node(); - - int init(); - uint8_t close(); - -protected: - int init_receiver(uint16_t udp_port); - int init_sender(uint16_t udp_port); - ssize_t node_read(void *buffer, size_t len); - ssize_t node_write(void *buffer, size_t len); - bool fds_OK(); - - int _sender_fd; - int _receiver_fd; - char _udp_ip[16]{}; - uint16_t _udp_port_recv; - uint16_t _udp_port_send; - struct sockaddr_in _sender_outaddr; - struct sockaddr_in _receiver_inaddr; - struct sockaddr_in _receiver_outaddr; -}; diff --git a/msg/templates/urtps/msg.idl.em b/msg/templates/urtps/msg.idl.em deleted file mode 100644 index 1fa20e3fe6..0000000000 --- a/msg/templates/urtps/msg.idl.em +++ /dev/null @@ -1,154 +0,0 @@ -@############################################### -@# -@# ROS message to IDL converter -@# -@# EmPy template for generating .idl files -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@################################################################################ -@# -@# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). -@# Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. -@# -@# Redistribution and use in source and binary forms, with or without -@# modification, are permitted provided that the following conditions are met: -@# -@# 1. Redistributions of source code must retain the above copyright notice, this -@# list of conditions and the following disclaimer. -@# -@# 2. Redistributions in binary form must reproduce the above copyright notice, -@# this list of conditions and the following disclaimer in the documentation -@# and/or other materials provided with the distribution. -@# -@# 3. Neither the name of the copyright holder nor the names of its contributors -@# may be used to endorse or promote products derived from this software without -@# specific prior written permission. -@# -@# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -@# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -@# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -@# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -@# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -@# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -@# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -@# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -@# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -@# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -@# POSSIBILITY OF SUCH DAMAGE. -@# -@################################################################################ -@{ -import genmsg.msgs -from packaging import version -from px_generate_uorb_topic_helper import * # this is in Tools/ - -builtin_types = set() -array_types = set() - -topic = alias if alias else spec.short_name -}@ -@################################################# -@# Searching for serialize function per each field -@################################################# -@{ - -def get_include_directives(spec): - include_directives = set() - for field in (spec.parsed_fields()): - if genmsg.msgs.is_valid_constant_type(genmsg.msgs.bare_msg_type(field.type)): - continue - builtin_type = str(field.base_type).replace('px4/', '') - if version.parse(fastrtps_version) <= version.parse('1.7.2'): - include_directive = '#include "%s_.idl"' % builtin_type - else: - include_directive = '#include "%s.idl"' % builtin_type - builtin_types.add(builtin_type) - include_directives.add(include_directive) - return sorted(include_directives) - - -def get_idl_type_name(field_type): - if field_type in type_idl_map: - return type_idl_map[field_type] - else: - (package, name) = genmsg.names.package_resource_name(field_type) - return name - - -def add_msg_field(field): - if (not field.is_header): - if field.is_array: - if version.parse(fastrtps_version) <= version.parse('1.7.2'): - print(' {0}__{1}_array_{2} {3}_;'.format(topic, str(get_idl_type_name(field.base_type)).replace(" ", "_"), str(field.array_len), field.name)) - else: - print(' {0}__{1}_array_{2} {3};'.format(topic, str(get_idl_type_name(field.base_type)).replace(" ", "_"), str(field.array_len), field.name)) - else: - if version.parse(fastrtps_version) <= version.parse('1.7.2'): - base_type = get_idl_type_name(field.base_type) + "_" if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type) - else: - base_type = get_idl_type_name(field.base_type) if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type) - print(' {0} {1}_;'.format(base_type, field.name)) - -def add_msg_fields(): - for field in spec.parsed_fields(): - add_msg_field(field) - - -def add_array_typedefs(): - for field in spec.parsed_fields(): - if not field.is_header and field.is_array: - if version.parse(fastrtps_version) <= version.parse('1.7.2'): - base_type = get_idl_type_name(field.base_type) + "_" if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type) - else: - base_type = get_idl_type_name(field.base_type) if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type) - array_type = 'typedef {0} {1}__{2}_array_{3}[{4}];'.format(base_type, topic, get_idl_type_name(field.base_type).replace(" ", "_"), field.array_len, field.array_len) - if array_type not in array_types: - array_types.add(array_type) - for atype in array_types: - print(atype) - - -def add_msg_constants(): - sorted_constants = sorted(spec.constants, - key=sizeof_field_type, reverse=True) - for constant in sorted_constants: - print('const {0} {1}__{2} = {3};'.format(get_idl_type_name(constant.type), topic, constant.name, constant.val)) - -} -#ifndef __@(topic)__idl__ -#define __@(topic)__idl__ - -@############################# -@# Include dependency messages -@############################# -@[for line in get_include_directives(spec)]@ -@(line)@ -@[end for]@ - - -@# Constants -@add_msg_constants() -@# Array types -@add_array_typedefs() -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -struct @(topic)_ -@[else]@ -struct @(topic) -@[end if]@ -{ -@add_msg_fields() -@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ -}; // struct @(topic)_ - -#pragma keylist @(topic)_ -@[else]@ -}; // struct @(topic) - -#pragma keylist @(topic) -@[end if]@ - -#endif // __@(topic)__idl__ diff --git a/msg/timesync.msg b/msg/timesync.msg deleted file mode 100644 index 67f415c744..0000000000 --- a/msg/timesync.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint8 seq # timesync msg sequence -int64 tc1 # time sync timestamp 1 -int64 ts1 # time sync timestamp 2 diff --git a/msg/tools/generate_microRTPS_bridge.py b/msg/tools/generate_microRTPS_bridge.py deleted file mode 100644 index a1a0b31519..0000000000 --- a/msg/tools/generate_microRTPS_bridge.py +++ /dev/null @@ -1,465 +0,0 @@ -#!/usr/bin/env python3 -################################################################################ -# -# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). -# Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# 3. Neither the name of the copyright holder nor the names of its contributors -# may be used to endorse or promote products derived from this software without -# specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -################################################################################ - -# This script can generate the client and agent code based on a set of topics -# to sent and set to receive. It uses fastrtpsgen to generate the code from the -# IDL for the topic messages. The PX4 msg definitions are used to create the IDL -# used by fastrtpsgen using templates. - -import sys -import os -import argparse -import shutil -import px_generate_uorb_topic_files -from uorb_rtps_classifier import Classifier -import subprocess -import glob -import errno -import re - -try: - from six.moves import input -except ImportError as e: - print("Failed to import six: " + e) - print("") - print("You may need to install it using:") - print(" pip3 install --user six") - print("") - sys.exit(1) - -try: - from packaging import version -except ImportError as e: - print("Failed to import packaging: " + str(e)) - print("") - print("You may need to install it using:") - print(" pip3 install --user packaging") - print("") - sys.exit(1) - - -default_client_out = "src/modules/micrortps_bridge/micrortps_client" -default_agent_out = "src/modules/micrortps_bridge/micrortps_agent" -default_uorb_templates_dir = "templates/uorb_microcdr" -default_urtps_templates_dir = "templates/urtps" -default_urtps_topics_file = "tools/urtps_bridge_topics.yaml" -default_package_name = px_generate_uorb_topic_files.PACKAGE - -parser = argparse.ArgumentParser() - -parser.add_argument("-a", "--agent", dest='agent', action="store_true", - help="Flag for generate the agent, by default is true if -c is not specified") -parser.add_argument("-c", "--client", dest='client', action="store_true", - help="Flag for generate the client, by default is true if -a is not specified") -parser.add_argument("-i", "--generate-idl", dest='gen_idl', - action="store_true", help="Flag for generate idl files for each msg") -parser.add_argument("-j", "--idl-dir", dest='idl_dir', - type=str, help="IDL files dir", default='') -parser.add_argument("-m", "--mkdir-build", dest='mkdir_build', - action="store_true", help="Flag to create 'build' dir") -parser.add_argument("-l", "--generate-cmakelists", dest='cmakelists', - action="store_true", help="Flag to generate a CMakeLists.txt file for the micro-RTPS agent") -parser.add_argument("-t", "--topic-msg-dir", dest='msgdir', type=str, - help="Topics message, by default using relative path 'msg/'", default="msg") -parser.add_argument("-b", "--uorb-templates-dir", dest='uorb_templates', type=str, - help="uORB templates, by default using relative path to msgdir 'templates/uorb_microcdr'", default=default_uorb_templates_dir) -parser.add_argument("-q", "--urtps-templates-dir", dest='urtps_templates', type=str, - help="uRTPS templates, by default using relative path to msgdir 'templates/urtps'", default=default_urtps_templates_dir) -parser.add_argument("-y", "--rtps-ids-file", dest='yaml_file', type=str, - help="Setup uRTPS bridge topics file path, by default using relative path to msgdir 'tools/urtps_bridge_topics.yaml'", default=default_urtps_topics_file) -parser.add_argument("-p", "--package", dest='package', type=str, - help="Msg package naming, by default px4", default=default_package_name) -parser.add_argument("-o", "--agent-outdir", dest='agentdir', type=str, - help="Agent output dir, by default using relative path 'src/modules/micrortps_bridge/micrortps_agent'", default=default_agent_out) -parser.add_argument("-u", "--client-outdir", dest='clientdir', type=str, - help="Client output dir, by default using relative path 'src/modules/micrortps_bridge/micrortps_client'", default=default_client_out) -parser.add_argument("-f", "--fastrtpsgen-dir", dest='fastrtpsgen', type=str, nargs='?', - help="fastrtpsgen installation dir, only needed if fastrtpsgen is not in PATH, by default empty", default="") -parser.add_argument("-g", "--fastrtpsgen-include", dest='fastrtpsgen_include', type=str, - help="directory(ies) to add to preprocessor include paths of fastrtpsgen, by default empty", default="") -parser.add_argument("-r", "--ros2-distro", dest='ros2_distro', type=str, nargs='?', - help="ROS2 distro, only required if generating the agent for usage with ROS2 nodes, by default empty", default="") -parser.add_argument("--delete-tree", dest='del_tree', - action="store_true", help="Delete dir tree output dir(s)") - - -if len(sys.argv) <= 1: - parser.print_usage() - exit(-1) - -# Parse arguments -args = parser.parse_args() -agent = args.agent -client = args.client -cmakelists = args.cmakelists -del_tree = args.del_tree -gen_idl = args.gen_idl -mkdir_build = args.mkdir_build -package = args.package - -# Msg files path -msg_dir = os.path.abspath(args.msgdir) -px_generate_uorb_topic_files.append_to_include_path( - {msg_dir}, px_generate_uorb_topic_files.INCL_DEFAULT, package) - -# Agent files output path -agent_out_dir = os.path.abspath(args.agentdir) - -# Client files output path -client_out_dir = os.path.abspath(args.clientdir) - -# IDL files path -idl_dir = args.idl_dir -if idl_dir != '': - idl_dir = os.path.abspath(args.idl_dir) -else: - idl_dir = os.path.join(agent_out_dir, "idl") - -if args.fastrtpsgen is None or args.fastrtpsgen == '': - # Assume fastrtpsgen is in PATH - fastrtpsgen_path = 'fastrtpsgen' - for dirname in os.environ['PATH'].split(':'): - candidate = os.path.join(dirname, 'fastrtpsgen') - if os.path.isfile(candidate): - fastrtpsgen_path = candidate -else: - # Path to fastrtpsgen is explicitly specified - if os.path.isdir(args.fastrtpsgen): - fastrtpsgen_path = os.path.join( - os.path.abspath(args.fastrtpsgen), 'fastrtpsgen') - else: - fastrtpsgen_path = args.fastrtpsgen - -fastrtpsgen_include = args.fastrtpsgen_include -if fastrtpsgen_include is not None and fastrtpsgen_include != '': - fastrtpsgen_include = "-I " + \ - os.path.abspath( - args.fastrtpsgen_include) + " " - -# get FastRTPSGen version -# .. note:: since Fast-RTPS 1.8.0 release, FastRTPSGen is a separated repository -# and not included in the Fast-RTPS project. -# The starting version since this separation is 1.0.0, which follows its own -# versioning -fastrtpsgen_version = version.Version("1.0.0") -if(os.path.exists(fastrtpsgen_path)): - try: - fastrtpsgen_version_out = subprocess.check_output( - [fastrtpsgen_path, "-version"]).decode("utf-8").strip()[-5:] - except OSError: - raise - - try: - fastrtpsgen_version = version.parse(fastrtpsgen_version_out) - except version.InvalidVersion: - raise Exception( - "'fastrtpsgen -version' returned None or an invalid version") -else: - raise Exception( - "FastRTPSGen not found. Specify the location of fastrtpsgen with the -f flag") - - -# get ROS 2 version, if exists -ros2_distro = '' -ros_version = os.environ.get('ROS_VERSION') -if ros_version == '2': - if args.ros2_distro != '': - ros2_distro = args.ros2_distro - else: - ros2_distro = os.environ.get('ROS_DISTRO') - -# get FastRTPS version -fastrtps_version = '' -if not ros2_distro: - # grab the version installed system wise - fastrtps_version = subprocess.check_output( - "ldconfig -v 2>/dev/null | grep libfastrtps", shell=True).decode("utf-8").strip().split('so.')[-1] -else: - try: - # grab the version of the ros--fastrtps package - fastrtps_version = re.search(r'Version:\s*([\dd.]+)', subprocess.check_output( - "dpkg -s ros-" + ros2_distro + "-fastrtps 2>/dev/null | grep -i version", shell=True).decode("utf-8").strip()).group(1) - except subprocess.CalledProcessError: - # if ROS2 was installed from sources the command above fails, get the system-wide version instead - try: - fastrtps_version = subprocess.check_output( - "ldconfig -v 2>/dev/null | grep libfastrtps", shell=True).decode("utf-8").strip().split('so.')[-1] - except subprocess.CalledProcessError: - # if no system-wide version is found, look for the one provided by the source-installed ROS2 - fastrtps_path = re.search(r'(?!\:)[^\:]*fastrtps\/lib(?=\:)', os.environ.get('LD_LIBRARY_PATH')).group(0) - fastrtps_version = subprocess.check_output( - "ls " + fastrtps_path, shell=True).decode("utf-8").strip().split('so.')[-1] - - -# If nothing specified it's generated both -if agent == False and client == False: - agent = True - client = True - -if del_tree: - if agent: - _continue = str(input("\nFiles in " + agent_out_dir + - " will be erased, continue?[Y/n]\n")) - if _continue == "N" or _continue == "n": - print("Aborting execution...") - exit(-1) - else: - if agent and os.path.isdir(agent_out_dir): - shutil.rmtree(agent_out_dir) - - if client: - _continue = str(input( - "\nFiles in " + client_out_dir + " will be erased, continue?[Y/n]\n")) - if _continue.strip() in ("N", "n"): - print("Aborting execution...") - exit(-1) - else: - if client and os.path.isdir(client_out_dir): - shutil.rmtree(client_out_dir) - -if agent and os.path.isdir(os.path.join(agent_out_dir, "idl")): - shutil.rmtree(os.path.join(agent_out_dir, "idl")) - -# uORB templates path -uorb_templates_dir = (args.uorb_templates if os.path.isabs(args.uorb_templates) - else os.path.join(msg_dir, args.uorb_templates)) - -# uRTPS templates path -urtps_templates_dir = (args.urtps_templates if os.path.isabs(args.urtps_templates) - else os.path.join(msg_dir, args.urtps_templates)) - -# parse yaml file into a map of ids and messages to send and receive -classifier = (Classifier(os.path.abspath(args.yaml_file), msg_dir) if os.path.isabs(args.yaml_file) - else Classifier(os.path.join(msg_dir, args.yaml_file), msg_dir)) - - -uRTPS_CLIENT_TEMPL_FILE = 'microRTPS_client.cpp.em' -uRTPS_AGENT_TOPICS_H_TEMPL_FILE = 'RtpsTopics.h.em' -uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE = 'RtpsTopics.cpp.em' -uRTPS_AGENT_TEMPL_FILE = 'microRTPS_agent.cpp.em' -uRTPS_TIMESYNC_CPP_TEMPL_FILE = 'microRTPS_timesync.cpp.em' -uRTPS_TIMESYNC_H_TEMPL_FILE = 'microRTPS_timesync.h.em' -uRTPS_AGENT_CMAKELISTS_TEMPL_FILE = 'microRTPS_agent_CMakeLists.txt.em' -uRTPS_PUBLISHER_SRC_TEMPL_FILE = 'Publisher.cpp.em' -uRTPS_PUBLISHER_H_TEMPL_FILE = 'Publisher.h.em' -uRTPS_SUBSCRIBER_SRC_TEMPL_FILE = 'Subscriber.cpp.em' -uRTPS_SUBSCRIBER_H_TEMPL_FILE = 'Subscriber.h.em' - - -def generate_agent(out_dir): - global fastrtps_version - - if classifier.msgs_to_send: - for msg_file in classifier.msgs_to_send: - if gen_idl: - if out_dir != agent_out_dir: - px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", os.path.join(out_dir, "/idl"), urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - else: - px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", idl_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_SRC_TEMPL_FILE) - px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_H_TEMPL_FILE) - - if classifier.alias_msgs_to_send: - for msg_file in classifier.alias_msgs_to_send: - msg_alias = msg_file[0] - msg_name = msg_file[1] - if gen_idl: - if out_dir != agent_out_dir: - px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, os.path.join(out_dir, "/idl"), urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - else: - px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, idl_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_SRC_TEMPL_FILE) - px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_H_TEMPL_FILE) - - if classifier.msgs_to_receive: - for msg_file in classifier.msgs_to_receive: - if gen_idl: - if out_dir != agent_out_dir: - px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", os.path.join(out_dir, "/idl"), urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - else: - px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", idl_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_SRC_TEMPL_FILE) - px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_H_TEMPL_FILE) - - if classifier.alias_msgs_to_receive: - for msg_file in classifier.alias_msgs_to_receive: - msg_alias = msg_file[0] - msg_name = msg_file[1] - if gen_idl: - if out_dir != agent_out_dir: - px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, os.path.join(out_dir, "/idl"), urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - else: - px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, idl_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) - px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_SRC_TEMPL_FILE) - px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_H_TEMPL_FILE) - - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_AGENT_TEMPL_FILE) - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_TIMESYNC_CPP_TEMPL_FILE) - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_TIMESYNC_H_TEMPL_FILE) - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_AGENT_TOPICS_H_TEMPL_FILE) - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE) - if cmakelists: - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, os.path.dirname(out_dir), - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_AGENT_CMAKELISTS_TEMPL_FILE) - - # Final steps to install agent - mkdir_p(os.path.join(out_dir, "fastrtpsgen")) - prev_cwd_path = os.getcwd() - os.chdir(os.path.join(out_dir, "fastrtpsgen")) - if not glob.glob(os.path.join(idl_dir, "*.idl")): - raise Exception("No IDL files found in %s" % idl_dir) - - # If it is generating the bridge code for interfacing with ROS2, then set - # the '-typeros2' option in fastrtpsgen. - # .. note:: This is only available in FastRTPSGen 1.0.4 and above - gen_ros2_typename = "" - if ros2_distro and ros2_distro in ['dashing', 'eloquent', 'foxy', 'galactic', 'humble', 'rolling'] and fastrtpsgen_version >= version.Version("1.0.4"): - gen_ros2_typename = "-typeros2 " - - idl_files = [] - for idl_file in glob.glob(os.path.join(idl_dir, "*.idl")): - # Only run the generator for the topics that are actually marked to be - # used by the bridge - if os.path.splitext(os.path.basename(idl_file))[0] in classifier.msg_list: - idl_files.append(idl_file) - - try: - ret = subprocess.check_call(fastrtpsgen_path + " -d " + out_dir + - "/fastrtpsgen -example x64Linux2.6gcc " + gen_ros2_typename + fastrtpsgen_include + " ".join(str(idl_file) for idl_file in idl_files), shell=True) - except OSError: - raise - - rm_wildcard(os.path.join(out_dir, "fastrtpsgen/*PubSubMain*")) - rm_wildcard(os.path.join(out_dir, "fastrtpsgen/makefile*")) - rm_wildcard(os.path.join(out_dir, "fastrtpsgen/*Publisher*")) - rm_wildcard(os.path.join(out_dir, "fastrtpsgen/*Subscriber*")) - for f in glob.glob(os.path.join(out_dir, "fastrtpsgen/*.cxx")): - os.rename(f, f.replace(".cxx", ".cpp")) - cp_wildcard(os.path.join(out_dir, "fastrtpsgen/*"), out_dir) - if os.path.isdir(os.path.join(out_dir, "fastrtpsgen")): - shutil.rmtree(os.path.join(out_dir, "fastrtpsgen")) - cp_wildcard(os.path.join(urtps_templates_dir, - "microRTPS_transport.*"), agent_out_dir) - if cmakelists: - os.rename(os.path.join(os.path.dirname(out_dir), "microRTPS_agent_CMakeLists.txt"), - os.path.join(os.path.dirname(out_dir), "CMakeLists.txt")) - if (mkdir_build): - mkdir_p(os.path.join(os.path.dirname(out_dir), "build")) - os.chdir(prev_cwd_path) - return 0 - - -def rm_wildcard(pattern): - for f in glob.glob(pattern): - os.remove(f) - - -def cp_wildcard(pattern, destdir): - for f in glob.glob(pattern): - shutil.copy(f, destdir) - - -def mkdir_p(dirpath): - try: - os.makedirs(dirpath) - except OSError as e: - if e.errno == errno.EEXIST and os.path.isdir(dirpath): - pass - else: - raise - - -def generate_client(out_dir): - global fastrtps_version - - # Rename work in the default path - if default_client_out != out_dir: - def_file = os.path.join(default_client_out, "microRTPS_client.cpp") - if os.path.isfile(def_file): - os.rename(def_file, def_file.replace(".cpp", ".cpp_")) - def_file = os.path.join(default_client_out, "microRTPS_transport.cpp") - if os.path.isfile(def_file): - os.rename(def_file, def_file.replace(".cpp", ".cpp_")) - def_file = os.path.join(default_client_out, "microRTPS_transport.h") - if os.path.isfile(def_file): - os.rename(def_file, def_file.replace(".h", ".h_")) - - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, - out_dir, uorb_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_CLIENT_TEMPL_FILE) - - px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, - out_dir, - uorb_templates_dir, - package, - px_generate_uorb_topic_files.INCL_DEFAULT, - classifier.msg_list, - fastrtps_version, - ros2_distro, - 'dds_topics.h.em') - - # Final steps to install client - cp_wildcard(os.path.join(urtps_templates_dir, - "microRTPS_transport.*"), out_dir) - - return 0 - - -if agent: - generate_agent(agent_out_dir) - print(("\nAgent created in: " + agent_out_dir)) - -if client: - generate_client(client_out_dir) - print(("\nClient created in: " + client_out_dir)) diff --git a/msg/tools/px_generate_uorb_topic_files.py b/msg/tools/px_generate_uorb_topic_files.py deleted file mode 100755 index 2a038f381c..0000000000 --- a/msg/tools/px_generate_uorb_topic_files.py +++ /dev/null @@ -1,561 +0,0 @@ -#!/usr/bin/env python3 -############################################################################# -# -# Copyright (C) 2013-2018 PX4 Pro Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################# - -""" -px_generate_uorb_topic_files.py -Generates c/cpp header/source files for uorb topics from .msg (ROS syntax) -message files -""" - -import os -import shutil -import filecmp -import argparse -import sys - -try: - import em -except ImportError as e: - print("Failed to import em: " + str(e)) - print("") - print("You may need to install it using:") - print(" pip3 install --user empy") - print("") - sys.exit(1) - -try: - import genmsg.template_tools -except ImportError as e: - print("Failed to import genmsg: " + str(e)) - print("") - print("You may need to install it using:") - print(" pip3 install --user pyros-genmsg") - print("") - sys.exit(1) - -try: - from packaging import version -except ImportError as e: - print("Failed to import packaging: " + str(e)) - print("") - print("You may need to install it using:") - print(" pip3 install --user packaging") - print("") - sys.exit(1) - - -__author__ = "Sergey Belash, Thomas Gubler, Beat Kueng" -__copyright__ = "Copyright (C) 2013-2016 PX4 Development Team." -__license__ = "BSD" -__email__ = "thomasgubler@gmail.com" - - -TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em'] -TOPICS_LIST_TEMPLATE_FILE = ['uORBTopics.hpp.em', 'uORBTopics.cpp.em'] -OUTPUT_FILE_EXT = ['.h', '.cpp'] -INCL_DEFAULT = ['std_msgs:./msg/std_msgs'] -PACKAGE = 'px4' -TOPICS_TOKEN = '# TOPICS ' -IDL_TEMPLATE_FILE = 'msg.idl.em' - -CONSTRAINED_FLASH = False - - -class MsgScope: - NONE = 0 - SEND = 1 - RECEIVE = 2 - - -def get_topics(filename, msg_name): - """ - Get TOPICS names from a "# TOPICS" line. If there are no multi topics defined, - set topic name same as the message name, since the user doesn't expect any new - custom topic names. - """ - ofile = open(filename, 'r') - text = ofile.read() - result = [] - for each_line in text.split('\n'): - if each_line.startswith(TOPICS_TOKEN): - topic_names_str = each_line.strip() - topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "") - result.extend(topic_names_str.split(" ")) - ofile.close() - - if len(result) == 0: - result.append(msg_name) - - return result - - -def get_msgs_list(msgdir): - """ - Makes list of msg files in the given directory - """ - return [fn for fn in os.listdir(msgdir) if fn.endswith(".msg")] - - -def generate_output_from_file(format_idx, filename, outputdir, package, templatedir, includepath): - """ - Converts a single .msg file to an uorb header/source file - """ - msg_context = genmsg.msg_loader.MsgContext.create_default() - full_type_name = genmsg.gentools.compute_full_type_name( - package, os.path.basename(filename)) - spec = genmsg.msg_loader.load_msg_from_file( - msg_context, filename, full_type_name) - field_name_and_type = {} - for field in spec.parsed_fields(): - field_name_and_type.update({field.name: field.type}) - # assert if the timestamp field exists - try: - assert 'timestamp' in field_name_and_type - except AssertionError: - print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\tNo 'timestamp' field found in " + - spec.short_name + " msg definition!") - exit(1) - # assert if the timestamp field is of type uint64 - try: - assert field_name_and_type.get('timestamp') == 'uint64' - except AssertionError: - print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name + - " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!") - exit(1) - - # Get topics used for the message - topics = get_topics(filename, spec.short_name) - - if includepath: - search_path = genmsg.command_line.includepath_to_dict(includepath) - else: - search_path = {} - genmsg.msg_loader.load_depends(msg_context, spec, search_path) - md5sum = genmsg.gentools.compute_md5(msg_context, spec) - em_globals = { - "file_name_in": filename, - "md5sum": md5sum, - "search_path": search_path, - "msg_context": msg_context, - "spec": spec, - "topics": topics, - "constrained_flash": CONSTRAINED_FLASH - } - - # Make sure output directory exists: - if not os.path.isdir(outputdir): - os.makedirs(outputdir) - - template_file = os.path.join(templatedir, TEMPLATE_FILE[format_idx]) - output_file = os.path.join(outputdir, spec.short_name + - OUTPUT_FILE_EXT[format_idx]) - - return generate_by_template(output_file, template_file, em_globals) - - -def generate_idl_file(filename_msg, msg_dir, alias, outputdir, templatedir, package, includepath, fastrtps_version, ros2_distro, msgs): - """ - Generates an .idl from .msg file - """ - msg = os.path.join(msg_dir, filename_msg + ".msg") - - if (alias != ""): - em_globals = get_em_globals( - msg, alias, package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.NONE) - spec_short_name = alias - else: - em_globals = get_em_globals( - msg, "", package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.NONE) - spec_short_name = em_globals["spec"].short_name - - # Make sure output directory exists: - if not os.path.isdir(outputdir): - os.makedirs(outputdir) - - template_file = os.path.join(templatedir, IDL_TEMPLATE_FILE) - if version.parse(fastrtps_version) <= version.parse('1.7.2'): - output_file = os.path.join(outputdir, IDL_TEMPLATE_FILE.replace( - "msg.idl.em", str(spec_short_name + "_.idl"))) - else: - output_file = os.path.join(outputdir, IDL_TEMPLATE_FILE.replace( - "msg.idl.em", str(spec_short_name + ".idl"))) - - return generate_by_template(output_file, template_file, em_globals) - - -def generate_uRTPS_general(filename_send_msgs, filename_alias_send_msgs, filename_receive_msgs, filename_alias_receive_msgs, - msg_dir, outputdir, templatedir, package, includepath, msgs, fastrtps_version, ros2_distro, template_name): - """ - Generates source file by msg content - """ - send_msgs = list(os.path.join(msg_dir, msg + ".msg") - for msg in filename_send_msgs) - receive_msgs = list(os.path.join(msg_dir, msg + ".msg") - for msg in filename_receive_msgs) - - alias_send_msgs = list([os.path.join( - msg_dir, msg[1] + ".msg"), msg[0]] for msg in filename_alias_send_msgs) - - alias_receive_msgs = list([os.path.join( - msg_dir, msg[1] + ".msg"), msg[0]] for msg in filename_alias_receive_msgs) - - em_globals_list = [] - if send_msgs: - em_globals_list.extend([get_em_globals( - f, "", package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.SEND) for f in send_msgs]) - - if alias_send_msgs: - em_globals_list.extend([get_em_globals( - f[0], f[1], package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.SEND) for f in alias_send_msgs]) - - if receive_msgs: - em_globals_list.extend([get_em_globals( - f, "", package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.RECEIVE) for f in receive_msgs]) - - if alias_receive_msgs: - em_globals_list.extend([get_em_globals( - f[0], f[1], package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.RECEIVE) for f in alias_receive_msgs]) - - merged_em_globals = merge_em_globals_list(em_globals_list) - - # Make sure output directory exists: - if not os.path.isdir(outputdir): - os.makedirs(outputdir) - - template_file = os.path.join(templatedir, template_name) - output_file = os.path.join( - outputdir, template_name.replace(".em", "")) - - return generate_by_template(output_file, template_file, merged_em_globals) - - -def generate_topic_file(filename_msg, msg_dir, alias, outputdir, templatedir, package, includepath, msgs, fastrtps_version, ros2_distro, template_name): - """ - Generates a sources and headers from .msg file - """ - msg = os.path.join(msg_dir, filename_msg + ".msg") - - if (alias): - em_globals = get_em_globals( - msg, alias, package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.NONE) - spec_short_name = alias - else: - em_globals = get_em_globals( - msg, "", package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.NONE) - spec_short_name = em_globals["spec"].short_name - - # Make sure output directory exists: - if not os.path.isdir(outputdir): - os.makedirs(outputdir) - - template_file = os.path.join(templatedir, template_name) - output_file = os.path.join( - outputdir, spec_short_name + "_" + template_name.replace(".em", "")) - - return generate_by_template(output_file, template_file, em_globals) - - -def get_em_globals(filename_msg, alias, package, includepath, msgs, fastrtps_version, ros2_distro, scope): - """ - Generates em globals dictionary - """ - msg_context = genmsg.msg_loader.MsgContext.create_default() - full_type_name = genmsg.gentools.compute_full_type_name( - package, os.path.basename(filename_msg)) - spec = genmsg.msg_loader.load_msg_from_file( - msg_context, filename_msg, full_type_name) - - # Get topics used for the message - topics = get_topics(filename_msg, spec.short_name) - - if includepath: - search_path = genmsg.command_line.includepath_to_dict(includepath) - else: - search_path = {} - genmsg.msg_loader.load_depends(msg_context, spec, search_path) - md5sum = genmsg.gentools.compute_md5(msg_context, spec) - em_globals = { - "file_name_in": filename_msg, - "md5sum": md5sum, - "search_path": search_path, - "msg_context": msg_context, - "spec": spec, - "topics": topics, - "msgs": msgs, - "scope": scope, - "package": package, - "alias": alias, - "fastrtps_version": fastrtps_version, - "ros2_distro": ros2_distro - } - - return em_globals - - -def merge_em_globals_list(em_globals_list): - """ - Merges a list of em_globals to a single dictionary where each attribute is a list - """ - if len(em_globals_list) < 1: - return {} - - merged_em_globals = {} - for name in em_globals_list[0]: - merged_em_globals[name] = [em_globals[name] - for em_globals in em_globals_list] - - return merged_em_globals - - -def generate_by_template(output_file, template_file, em_globals): - """ - Invokes empy intepreter to geneate output_file by the - given template_file and predefined em_globals dict - """ - # check if folder exists: - folder_name = os.path.dirname(output_file) - if not os.path.exists(folder_name): - os.makedirs(folder_name) - - ofile = open(output_file, 'w') - # todo, reuse interpreter - interpreter = em.Interpreter(output=ofile, globals=em_globals, options={ - em.RAW_OPT: True, em.BUFFERED_OPT: True}) - try: - interpreter.file(open(template_file)) - except OSError as e: - ofile.close() - os.remove(output_file) - raise - interpreter.shutdown() - ofile.close() - return True - - -def convert_dir(format_idx, inputdir, outputdir, package, templatedir): - """ - Converts all .msg files in inputdir to uORB header/source files - """ - - # Find the most recent modification time in input dir - maxinputtime = 0 - for f in os.listdir(inputdir): - fni = os.path.join(inputdir, f) - if os.path.isfile(fni): - it = os.path.getmtime(fni) - if it > maxinputtime: - maxinputtime = it - - # Find the most recent modification time in output dir - maxouttime = 0 - if os.path.isdir(outputdir): - for f in os.listdir(outputdir): - fni = os.path.join(outputdir, f) - if os.path.isfile(fni): - it = os.path.getmtime(fni) - if it > maxouttime: - maxouttime = it - - # Do not generate if nothing changed on the input - if (maxinputtime != 0 and maxouttime != 0 and maxinputtime < maxouttime): - return False - - includepath = INCL_DEFAULT + [':'.join([package, inputdir])] - for f in os.listdir(inputdir): - # Ignore hidden files - if f.startswith("."): - continue - - fn = os.path.join(inputdir, f) - # Only look at actual files - if not os.path.isfile(fn): - continue - - if fn[-4:].lower() != '.msg': - continue - - generate_output_from_file( - format_idx, fn, outputdir, package, templatedir, includepath) - return True - - -def copy_changed(inputdir, outputdir, prefix='', quiet=False): - """ - Copies files from inputdir to outputdir if they don't exist in - outputdir or if their content changed - """ - - # Make sure output directory exists: - if not os.path.isdir(outputdir): - os.makedirs(outputdir) - - for input_file in os.listdir(inputdir): - fni = os.path.join(inputdir, input_file) - if os.path.isfile(fni): - # Check if input_file exists in outpoutdir, copy the file if not - fno = os.path.join(outputdir, prefix + input_file) - if not os.path.isfile(fno): - shutil.copy(fni, fno) - if not quiet: - print("{0}: new header file".format(fno)) - continue - - if os.path.getmtime(fni) > os.path.getmtime(fno): - # The file exists in inputdir and outputdir - # only copy if contents do not match - if not filecmp.cmp(fni, fno): - shutil.copy(fni, fno) - if not quiet: - print("{0}: updated".format(input_file)) - continue - - if not quiet: - print("{0}: unchanged".format(input_file)) - - -def convert_dir_save(format_idx, inputdir, outputdir, package, templatedir, temporarydir, prefix, quiet=False): - """ - Converts all .msg files in inputdir to uORB header files - Unchanged existing files are not overwritten. - """ - # Create new headers in temporary output directory - convert_dir(format_idx, inputdir, temporarydir, package, templatedir) - if generate_idx == 1: - generate_topics_list_file(inputdir, temporarydir, TOPICS_LIST_TEMPLATE_FILE[1], templatedir) - # Copy changed headers from temporary dir to output dir - copy_changed(temporarydir, outputdir, prefix, quiet) - - -def generate_topics_list_file(msgdir, outputdir, template_filename, templatedir): - # generate cpp file with topics list - msgs = get_msgs_list(msgdir) - topics = [] - for msg in msgs: - msg_filename = os.path.join(msgdir, msg) - topics.extend(get_topics(msg_filename, msg)) - tl_globals = {"msgs": msgs, "topics": topics} - tl_template_file = os.path.join(templatedir, template_filename) - tl_out_file = os.path.join(outputdir, template_filename.replace(".em", "")) - generate_by_template(tl_out_file, tl_template_file, tl_globals) - - -def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir): - # Get message file names ending with .msg only - msg_filenames = [p for p in files if os.path.basename(p).endswith(".msg")] - - # Get topics used in messages - topics = [] - for msg_filename in msg_filenames: - msg_name = os.path.basename(msg_filename).replace('.msg', '') - topics.extend(get_topics(msg_filename, msg_name)) - - # Get only the message file name for "msgs" component - msg_basenames = [os.path.basename(p) for p in msg_filenames] - - # Set the Template dictionary settings - tl_globals = {"msgs": msg_basenames, "topics": topics} - tl_template_file = os.path.join(templatedir, template_filename) - tl_out_file = os.path.join(outputdir, template_filename.replace(".em", "")) - generate_by_template(tl_out_file, tl_template_file, tl_globals) - - -def append_to_include_path(path_to_append, curr_include, package): - for p in path_to_append: - curr_include.append('%s:%s' % (package, p)) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser( - description='Convert msg files to uorb headers/sources') - parser.add_argument('--headers', help='Generate header files', - action='store_true') - parser.add_argument('--sources', help='Generate source files', - action='store_true') - parser.add_argument('-d', dest='dir', help='directory with msg files') - parser.add_argument('-f', dest='file', - help="files to convert (use only without -d)", - nargs="+") - parser.add_argument('-i', dest="include_paths", - help='Additional Include Paths', nargs="*", - default=None) - parser.add_argument('-e', dest='templatedir', - help='directory with template files',) - parser.add_argument('-k', dest='package', default=PACKAGE, - help='package name') - parser.add_argument('-o', dest='outputdir', - help='output directory for header files') - parser.add_argument('-t', dest='temporarydir', - help='temporary directory') - parser.add_argument('-p', dest='prefix', default='', - help='string added as prefix to the output file ' - ' name when converting directories') - parser.add_argument('-q', dest='quiet', default=False, action='store_true', - help='string added as prefix to the output file ' - ' name when converting directories') - parser.add_argument('--constrained-flash', dest='constrained_flash', default=False, action='store_true', - help='set to save flash space') - args = parser.parse_args() - - if args.include_paths: - append_to_include_path(args.include_paths, INCL_DEFAULT, args.package) - - CONSTRAINED_FLASH = args.constrained_flash - - if args.headers: - generate_idx = 0 - elif args.sources: - generate_idx = 1 - else: - print('Error: either --headers or --sources must be specified') - exit(-1) - if args.file is not None: - for f in args.file: - generate_output_from_file( - generate_idx, f, args.temporarydir, args.package, args.templatedir, INCL_DEFAULT) - - # Generate topics list header and source file - if os.path.isfile(os.path.join(args.templatedir, TOPICS_LIST_TEMPLATE_FILE[generate_idx])): - generate_topics_list_file_from_files(args.file, args.outputdir, TOPICS_LIST_TEMPLATE_FILE[generate_idx], args.templatedir) - - copy_changed(args.temporarydir, args.outputdir, args.prefix, args.quiet) - elif args.dir is not None: - convert_dir_save( - generate_idx, - args.dir, - args.outputdir, - args.package, - args.templatedir, - args.temporarydir, - args.prefix, - args.quiet) diff --git a/msg/tools/uorb_rtps_classifier.py b/msg/tools/uorb_rtps_classifier.py deleted file mode 100644 index 13a79ffa32..0000000000 --- a/msg/tools/uorb_rtps_classifier.py +++ /dev/null @@ -1,186 +0,0 @@ -#!/usr/bin/env python3 -################################################################################ -# -# Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# 3. Neither the name of the copyright holder nor the names of its contributors -# may be used to endorse or promote products derived from this software without -# specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -################################################################################ - -import argparse -import difflib -import errno -import os -from typing import Dict, List, Tuple -import yaml - - -class Classifier(): - """Class to classify RTPS msgs as to send, receive and set their IDs.""" - - def __init__(self, yaml_file, msg_folder) -> None: - self.msg_folder = msg_folder - self.msg_map = self.parse_yaml_msgs_file(yaml_file) - - # Check if base types are defined correctly - self.check_base_type() - - # Get messages to send and to receive - self.msgs_to_send: Dict[str, int] = dict() - self.msgs_to_receive: Dict[str, int] = dict() - self.alias_msgs_to_send: List[Tuple[str, str]] = [] - self.alias_msgs_to_receive: List[Tuple[str, str]] = [] - self.msg_list: List[str] = [] - - # Create message map - self.setup_msg_map() - - self.msg_files_send = self.set_msg_files_send() - self.msg_files_receive = self.set_msg_files_receive() - - def setup_msg_map(self) -> None: - """Setup dictionary with an ID map for the messages.""" - for topic in self.msg_map['rtps']: - if 'send' in list(topic.keys()): - if 'base' in list(topic.keys()): - self.alias_msgs_to_send.append( - (topic['msg'], topic['base'])) - else: - self.msgs_to_send.update({topic['msg']: 0}) - if 'receive' in list(topic.keys()): - if 'base' in list(topic.keys()): - self.alias_msgs_to_receive.append( - (topic['msg'], topic['base'])) - else: - self.msgs_to_receive.update({topic['msg']: 0}) - self.msg_list.append(topic['msg']) - - def set_msg_files_send(self) -> list: - """ - Append the path to the files which messages are marked to - be sent. - """ - return [os.path.join(self.msg_folder, msg + ".msg") - for msg in list(self.msgs_to_send.keys())] - - def set_msg_files_receive(self) -> list: - """ - Append the path to the files which messages are marked to - be received. - """ - return [os.path.join(self.msg_folder, msg + ".msg") - for msg in list(self.msgs_to_receive.keys())] - - def check_base_type(self) -> None: - """Check if alias message has correct base type.""" - registered_alias_msgs = list( - topic['base'] for topic in self.msg_map['rtps'] if 'base' in list(topic.keys())) - - base_types = [] - for topic in self.msg_map['rtps']: - if 'base' not in list(topic.keys()): - base_types.append(topic['msg']) - - incorrect_base_types = list( - set(registered_alias_msgs) - set(base_types)) - - base_types_suggestion = {} - for incorrect in incorrect_base_types: - base_types_suggestion.update({incorrect: difflib.get_close_matches( - incorrect, base_types, n=1, cutoff=0.6)}) - - if len(base_types_suggestion) > 0: - raise AssertionError( - ('\n' + '\n'.join('\t- The multi-topic message base type \'{}\' does not exist.{}'.format(k, (' Did you mean \'' + v[0] + '\'?' if v else '')) for k, v in list(base_types_suggestion.items())))) - - @staticmethod - def parse_yaml_msgs_file(yaml_file) -> dict: - """Parses a yaml file into a dict.""" - try: - with open(yaml_file, 'r') as file: - return yaml.safe_load(file) - except OSError as err: - if err.errno == errno.ENOENT: - raise IOError(errno.ENOENT, os.strerror( - errno.ENOENT), yaml_file) - raise - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - - parser.add_argument("-s", "--send", dest='send', - action="store_true", help="Get topics to be sent") - parser.add_argument("-a", "--alias", dest='alias', - action="store_true", help="Get alias topics") - parser.add_argument("-r", "--receive", dest='receive', - action="store_true", help="Get topics to be received") - parser.add_argument("-i", "--ignore", dest='ignore', - action="store_true", help="Get topics to be ignored") - parser.add_argument("-p", "--path", dest='path', - action="store_true", help="Get msgs and its paths") - parser.add_argument("-m", "--topic-msg-dir", dest='msgdir', type=str, - help="Topics message dir, by default msg/", default="msg") - parser.add_argument("-y", "--rtps-ids-file", dest='yaml_file', type=str, - help="RTPS msg IDs definition file absolute path, by default use relative path to msg, tools/urtps_bridge_topics.yaml", - default='tools/urtps_bridge_topics.yaml') - - # Parse arguments - args = parser.parse_args() - - msg_dir = args.msgdir - if args.msgdir == 'msg': - msg_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) - else: - msg_dir = os.path.abspath(args.msgdir) - classifier = (Classifier(os.path.abspath(args.yaml_file), msg_dir) if os.path.isabs(args.yaml_file) - else Classifier(os.path.join(msg_dir, args.yaml_file), msg_dir)) - - if args.send: - if args.path: - print(('send files: ' + ', '.join(str(msg_file) - for msg_file in classifier.msg_files_send) + '\n')) - else: - if args.alias: - print((', '.join(str(msg) - for msg in sorted(classifier.msgs_to_send)) + (' alias ' + ', '.join(msg[0] - for msg in classifier.alias_msgs_to_send) if len(classifier.alias_msgs_to_send) > 0 else '') + '\n')) - else: - print((', '.join(str(msg) - for msg in sorted(classifier.msgs_to_send)))) - if args.receive: - if args.path: - print(('receive files: ' + ', '.join(str(msg_file) - for msg_file in classifier.msg_files_receive) + '\n')) - else: - if args.alias: - print((', '.join(str(msg) - for msg in sorted(classifier.msgs_to_receive)) + (' alias ' + ', '.join(msg[0] - for msg in classifier.alias_msgs_to_receive) if len(classifier.alias_msgs_to_receive) > 0 else '') + '\n')) - else: - print((', '.join(str(msg) - for msg in sorted(classifier.msgs_to_receive)))) diff --git a/msg/tools/uorb_to_ros_msgs.py b/msg/tools/uorb_to_ros_msgs.py deleted file mode 100755 index a0d2aa9fb3..0000000000 --- a/msg/tools/uorb_to_ros_msgs.py +++ /dev/null @@ -1,127 +0,0 @@ -#!/usr/bin/env python3 -""" -Script to parse uORB message format to ROS msg format -Adapted from https://github.com/eProsima/px4_to_ros/blob/master/px4_to_ros2_PoC/px4_msgs/scripts/copy_and_rename.py -""" - -import os -import re -import sys -from shutil import copyfile - -__author__ = 'PX4 Development Team' -__copyright__ = \ - ''' - ' - ' Copyright (c) 2018 PX4 Development Team. All rights reserved. - ' - ' Redistribution and use in source and binary forms, or without - ' modification, permitted provided that the following conditions - ' are met: - ' - ' 1. Redistributions of source code must retain the above copyright - ' notice, list of conditions and the following disclaimer. - ' 2. Redistributions in binary form must reproduce the above copyright - ' notice, list of conditions and the following disclaimer in - ' the documentation and/or other materials provided with the - ' distribution. - ' 3. Neither the name PX4 nor the names of its contributors may be - ' used to endorse or promote products derived from self software - ' without specific prior written permission. - ' - ' THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - ' "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, NOT - ' LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - ' FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - ' COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - ' INCIDENTAL, SPECIAL, EXEMPLARY, CONSEQUENTIAL DAMAGES (INCLUDING, - ' BUT NOT LIMITED TO, OF SUBSTITUTE GOODS OR SERVICES; LOSS - ' OF USE, DATA, PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - ' AND ON ANY THEORY OF LIABILITY, IN CONTRACT, STRICT - ' LIABILITY, TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - ' ANY WAY OUT OF THE USE OF THIS SOFTWARE, IF ADVISED OF THE - ' POSSIBILITY OF SUCH DAMAGE. - ' - ''' -__credits__ = ['Nuno Marques '] -__license__ = 'BSD-3-Clause' -__version__ = '0.1.0' -__maintainer__ = 'Nuno Marques' -__email__ = 'nuno.marques@dronesolution.io' -__status__ = 'Development' - -input_dir = sys.argv[1] -output_dir = sys.argv[2] - - -def main(): - print("----------------------- \033[1mmicroRTPS bridge uORB to ROS messages\033[0m -----------------------") - print("-------------------------------------------------------------------------------------------------------") - - if not os.path.exists(os.path.abspath(output_dir)): - os.mkdir(os.path.abspath(output_dir)) - else: - ros_msg_dir = os.path.abspath(output_dir) - msg_files = os.listdir(ros_msg_dir) - for msg in msg_files: - if msg.endswith(".msg"): - os.remove(os.path.join(ros_msg_dir, msg)) - - msg_list = list() - - for filename in os.listdir(input_dir): - if '.msg' in filename: - msg_list.append(filename.rstrip('.msg')) - input_file = input_dir + filename - - output_file = output_dir + \ - filename.partition(".")[0].title().replace('_', '') + ".msg" - copyfile(input_file, output_file) - - for filename in os.listdir(output_dir): - if '.msg' in filename: - input_file = output_dir + filename - - fileUpdated = False - - with open(input_file, 'r') as f: - lines = f.readlines() - newlines = [] - alias_msgs = [] - alias_msg_files = [] - - for line in lines: - for msg_type in msg_list: - if ('px4/' + msg_type + ' ') in line: - fileUpdated = True - line = line.replace(('px4/' + msg_type), - msg_type.partition(".")[0].title().replace('_', '')) - - if re.findall('^' + msg_type + '[\s\[]', line.partition('#')[0]): - fileUpdated = True - line = line.replace(msg_type, - msg_type.partition(".")[0].title().replace('_', '')) - if '# TOPICS' in line: - fileUpdated = True - alias_msgs += line.split() - alias_msgs.remove('#') - alias_msgs.remove('TOPICS') - line = line.replace(line, '') - newlines.append(line) - - for msg_file in alias_msgs: - with open(output_dir + msg_file.partition(".")[0].title().replace('_', '') + ".msg", 'w+') as f: - for line in newlines: - f.write(line) - - if fileUpdated: - with open(input_file, 'w+') as f: - for line in newlines: - f.write(line) - - print("--\t\t- Generated {} ROS message files in '{}'".format(len(os.listdir(output_dir)), os.path.abspath(output_dir))) - print("-------------------------------------------------------------------------------------------------------") - - -if __name__ == '__main__': - main() diff --git a/msg/tools/uorb_to_ros_urtps_topics.py b/msg/tools/uorb_to_ros_urtps_topics.py deleted file mode 100755 index 8ff5f0e59d..0000000000 --- a/msg/tools/uorb_to_ros_urtps_topics.py +++ /dev/null @@ -1,160 +0,0 @@ -#!/usr/bin/env python3 -""" -Script to read an yaml file containing the microRTPS topics and update the naming convention to PascalCase -""" - -import argparse -import errno -from pathlib import Path -import os -import six -import yaml - -__author__ = 'PX4 Development Team' -__copyright__ = \ - ''' - ' - ' Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. - ' - ' Redistribution and use in source and binary forms, or without - ' modification, permitted provided that the following conditions - ' are met: - ' - ' 1. Redistributions of source code must retain the above copyright - ' notice, list of conditions and the following disclaimer. - ' 2. Redistributions in binary form must reproduce the above copyright - ' notice, list of conditions and the following disclaimer in - ' the documentation and/or other materials provided with the - ' distribution. - ' 3. Neither the name PX4 nor the names of its contributors may be - ' used to endorse or promote products derived from self software - ' without specific prior written permission. - ' - ' THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - ' "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, NOT - ' LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - ' FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - ' COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - ' INCIDENTAL, SPECIAL, EXEMPLARY, CONSEQUENTIAL DAMAGES (INCLUDING, - ' BUT NOT LIMITED TO, OF SUBSTITUTE GOODS OR SERVICES; LOSS - ' OF USE, DATA, PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - ' AND ON ANY THEORY OF LIABILITY, IN CONTRACT, STRICT - ' LIABILITY, TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - ' ANY WAY OUT OF THE USE OF THIS SOFTWARE, IF ADVISED OF THE - ' POSSIBILITY OF SUCH DAMAGE. - ' - ''' -__credits__ = ['Nuno Marques '] -__license__ = 'BSD-3-Clause' -__version__ = '0.1.0' -__maintainer__ = 'Nuno Marques' -__email__ = 'nuno.marques@dronesolution.io' -__status__ = 'Development' - -verbose = False - - -class IndenterDumper(yaml.Dumper): - """ Custom dumper for yaml files that apply the correct indentation """ - - def increase_indent(self, flow=False, indentless=False): - return super(IndenterDumper, self).increase_indent(flow, False) - - -def load_yaml_file(file): - """ - Open yaml file and parse the data into a list of dict - - :param file: the yaml file to load - :returns: the list of dictionaries that represent the topics to send and receive - :raises IOError: raises and error when the file is not found - """ - try: - with open(file, 'r') as f: - if verbose: - print(("--\t\t- '%s' file loaded" % file)) - return yaml.safe_load(f) - except OSError as e: - if e.errno == errno.ENOENT: - raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), file) - else: - raise - - -def update_dict(list): - """ - Update the message naming on the dictionary to fit the PascalCase convention - - :param file: the list of dicts to be updated - """ - if verbose: - num_of_msgs = 0 - for i, dictionary in enumerate(list["rtps"]): - list["rtps"][i] = {k: v.title().replace('_', '') if isinstance( - v, six.string_types) else v for k, v in six.iteritems(dictionary)} - if verbose: - num_of_msgs += 1 - if verbose: - print(("--\t\t- %d ROS message file names updated" % num_of_msgs)) - - -def update_yaml_file(list, in_file, out_file): - """ - Open the yaml file to dump the new list of dict toself. - - :param list: the list of updated dicts - :param file: the yaml file to load and write the new data - :raises IOError: raises and error when the file is not found - """ - try: - with open(out_file, 'w') as f: - f.write("# AUTOGENERATED-FILE! DO NOT MODIFY IT DIRECTLY.\n#" - " Edit instead the same file under PX4-Autopilot/msg/tools and" - " use the \n# PX4-Autopilot/msg/tools/uorb_to_ros_urtps_topics.py" - " to regenerate this file.\n") - yaml.dump(list, f, Dumper=IndenterDumper, default_flow_style=False) - if verbose: - if in_file == out_file: - print(("--\t\t- '%s' updated" % in_file)) - else: - print(("--\t\t- '%s' created" % out_file)) - - except OSError as err: - if err.errno == errno.ENOENT: - raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), out_file) - raise - - -def main(): - parser = argparse.ArgumentParser( - description='Read an yaml file containing the microRTPS topics and update the naming convention to PascalCase') - optional = parser._action_groups.pop() - required = parser.add_argument_group('Required') - required.add_argument("-i", "--input-file", dest="input_file", - help="Yaml file to read", metavar="INFILE") - optional.add_argument("-o", "--output-file", dest="output_file", - help="Yaml file to dump. If not set, it is the same as the input", - metavar="OUTFILE", default="") - optional.add_argument("-q", "--quiet", action="store_false", dest="verbose", - default=True, help="Don't print status messages to stdout") - - args = parser.parse_args() - global verbose - verbose = args.verbose - in_file = Path(args.input_file) - out_file = Path(args.output_file) if ( - Path(args.output_file) != in_file and Path(args.output_file) != "") else in_file - - if verbose: - print("---------------------- \033[1mmicroRTPS bridge yaml PX4 to ROS topics\033[0m ----------------------") - print("-------------------------------------------------------------------------------------------------------") - - list = load_yaml_file(in_file) - update_dict(list) - update_yaml_file(list, in_file, out_file) - if verbose: - print("-------------------------------------------------------------------------------------------------------") - - -if __name__ == "__main__": - main() diff --git a/msg/tools/urtps_bridge_topics.yaml b/msg/tools/urtps_bridge_topics.yaml deleted file mode 100644 index 747a86c8e3..0000000000 --- a/msg/tools/urtps_bridge_topics.yaml +++ /dev/null @@ -1,94 +0,0 @@ -##### -# -# This file maps all the topics that are to be used on the microRTPS bridge. -# When one wants to add a new topic to the bridge, it should add it to this file -# and mark it to be sent or received from the link. -# For alias/multi-topic messages (i.e. the ones found on the '#TOPICS' of the -# uORB messages), these can be also added, requiring an extra entry ('base') to -# define the base message. -# -# IMPORTANT NOTICE: The IDs of the messages sent on the bridge get generated -# according to the order of the messages in this file. To keep consistency and -# backwards compatibility, it is recommended that any new message that one wants -# to be streamed in the bridge gets added to the end of the list. Any changes -# in the middle of the list (additions, removals, replacements) will change also -# the current message IDS, which might result with incompatibilities with -# previous PX4 versions (where the list with this format got introduced or -# subsisted). -# -# Any updates to this file should be mirrored in both sides of the bridge (i.e., -# PX4 and px4_ros_com), when using it with ROS2. That can be easily done using -# the 'msg/tools/uorb_to_ros_urtps_topics.py' script to regenerate this same -# file under 'px4_ros_com/templates/''. The same is not applicable/required if -# using this bridge with "raw" RTPS/DDS applications, since the microRTPS agent -# to be used and stored in 'build//src/modules/micrortps_bridge/'' -# gets generated using this same list. -# -##### -rtps: - # topic ID 1 - - msg: debug_array - receive: true - # topic ID 2 - - msg: debug_key_value - receive: true - # topic ID 3 - - msg: debug_value - receive: true - # ... - - msg: debug_vect - receive: true - - msg: offboard_control_mode - receive: true - - msg: sensor_optical_flow - receive: true - - msg: position_setpoint - receive: true - - msg: position_setpoint_triplet - receive: true - - msg: telemetry_status - receive: true - - msg: timesync - receive: true - send: true - - msg: trajectory_waypoint - send: true - - msg: vehicle_command - receive: true - - msg: vehicle_control_mode - send: true - - msg: vehicle_local_position_setpoint - receive: true - - msg: vehicle_attitude_setpoint - receive: true - - msg: vehicle_rates_setpoint - receive: true - - msg: trajectory_setpoint - receive: true - - msg: vehicle_odometry - send: true - - msg: vehicle_mocap_odometry # multi-topic / alias of vehicle_odometry - base: vehicle_odometry - receive: true - - msg: vehicle_visual_odometry # multi-topic / alias of vehicle_odometry - base: vehicle_odometry - receive: true - - msg: vehicle_status - send: true - - msg: vehicle_trajectory_waypoint - receive: true - - msg: vehicle_trajectory_waypoint_desired # multi-topic / alias of vehicle_trajectory_waypoint - base: vehicle_trajectory_waypoint - send: true - - msg: collision_constraints - send: true - - msg: onboard_computer_status - receive: true - - msg: trajectory_bezier - receive: true - - msg: vehicle_trajectory_bezier - receive: true - - msg: timesync_status - send: true - - msg: sensor_combined - send: true diff --git a/platforms/common/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp index 3b56d65e6d..71c8809ad2 100644 --- a/platforms/common/uORB/Subscription.hpp +++ b/platforms/common/uORB/Subscription.hpp @@ -181,6 +181,8 @@ public: unsigned get_last_generation() const { return _last_generation; } orb_id_t get_topic() const { return get_orb_meta(_orb_id); } + ORB_ID orb_id() const { return _orb_id; } + protected: friend class SubscriptionCallback; diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index c0fa973251..bf701cbbd1 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -139,7 +139,7 @@ static inline hrt_abstime ts_to_abstime(const struct timespec *ts) hrt_abstime result; result = (hrt_abstime)(ts->tv_sec) * 1000000; - result += ts->tv_nsec / 1000; + result += (hrt_abstime)(ts->tv_nsec / 1000); return result; } @@ -149,9 +149,9 @@ static inline hrt_abstime ts_to_abstime(const struct timespec *ts) */ static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime) { - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; + ts->tv_sec = (time_t)abstime / 1000000; + abstime -= (hrt_abstime)(ts->tv_sec * 1000000); + ts->tv_nsec = (typeof(ts->tv_nsec))(abstime * 1000); } /** @@ -249,8 +249,8 @@ __EXPORT extern void px4_lockstep_wait_for_components(void); #else static inline int px4_lockstep_register_component(void) { return 0; } -static inline void px4_lockstep_unregister_component(int component) { } -static inline void px4_lockstep_progress(int component) { } +static inline void px4_lockstep_unregister_component(int component) { (void)component; } +static inline void px4_lockstep_progress(int component) {(void)component; } static inline void px4_lockstep_wait_for_components(void) { } #endif /* defined(ENABLE_LOCKSTEP_SCHEDULER) */ diff --git a/src/drivers/protocol_splitter/Kconfig b/src/drivers/protocol_splitter/Kconfig deleted file mode 100644 index f251603986..0000000000 --- a/src/drivers/protocol_splitter/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig DRIVERS_PROTOCOL_SPLITTER - bool "protocol_splitter" - default n - ---help--- - Enable support for protocol_splitter \ No newline at end of file diff --git a/src/drivers/protocol_splitter/protocol_splitter.cpp b/src/drivers/protocol_splitter/protocol_splitter.cpp deleted file mode 100644 index dacf0bb4c5..0000000000 --- a/src/drivers/protocol_splitter/protocol_splitter.cpp +++ /dev/null @@ -1,845 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file protocol_splitter.cpp - * - * NuttX Driver to multiplex MAVLink and RTPS on a single serial port. - * Makes sure the two protocols can be read & written simultaneously by two - * processes. - * - * It will create two devices: - * /dev/mavlink - * /dev/rtps - */ - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - - -class Mavlink2Dev; -class RtpsDev; -class ReadBuffer; - -extern "C" __EXPORT int protocol_splitter_main(int argc, char *argv[]); - -/* - MessageType is in MSB of header[1] - | - v - Mavlink 0000 0000b - Rtps 1000 0000b -*/ -enum class MessageType : uint8_t {Mavlink = 0x00, Rtps = 0x01}; - -constexpr char Sp2HeaderMagic = 'S'; -constexpr int Sp2HeaderSize = 4; - -/* - Header Structure: - - bits: 1 2 3 4 5 6 7 8 - header[0] - | Magic | - header[1] - |T| LenH | - header[2] - | LenL | - header[3] - | Checksum | -*/ -union __attribute__((packed)) Sp2Header { - uint8_t bytes[4]; - struct { - char magic; // 'S' - uint8_t len_h: 7, // Length MSB - type: 1; // 0=MAVLINK, 1=RTPS - uint8_t len_l; // Length LSB - uint8_t checksum; // XOR of the three bytes above - } fields; -}; - -struct StaticData { - Mavlink2Dev *mavlink2; - RtpsDev *rtps; - sem_t r_lock; - sem_t w_lock; - char device_name[16]; - ReadBuffer *read_buffer; -}; - -namespace -{ -static StaticData *objects = nullptr; -} - -// Perf counters -perf_counter_t bytes_received_count; -perf_counter_t header_bytes_received_count; -perf_counter_t bytes_lost_count; -perf_counter_t mavlink_messages_parsed_count; -perf_counter_t mavlink_bytes_parsed_count; -perf_counter_t rtps_messages_parsed_count; -perf_counter_t rtps_bytes_parsed_count; -perf_counter_t buffer_drops; - -class ReadBuffer -{ -public: - int read(int fd); - void copy(void *dest, size_t pos, size_t n); - void remove(size_t pos, size_t n); - - void print_stats(); - void update_lost_stats(); - - uint8_t buffer[1024] = {}; - size_t buf_size = 0; - - static const size_t BUFFER_THRESHOLD = sizeof(buffer) * 0.8; - - // We keep track of the first Mavlink and Rtps packet in the buffer. - // If start and end are equal there is no packet. - size_t start_mavlink = 0; - size_t end_mavlink = 0; - size_t start_rtps = 0; - size_t end_rtps = 0; - // Just for stats. - size_t mavlink_parsed = 0; - size_t rtps_parsed = 0; - size_t bytes_received = 0; - size_t bytes_lost = 0; - size_t header_bytes_received = 0; -}; - -int ReadBuffer::read(int fd) -{ - if (buf_size > BUFFER_THRESHOLD) { - // Drop the buffer if it's too full. This is not expected to happen, but it might, if one of the readers - // is too slow. In that case it's best to make space for new data, otherwise the faster reader might - // busy-loop w/o getting new data. - perf_count(buffer_drops); - - PX4_DEBUG("Buffer full, dropping: %zu %zu %zu %zu", start_mavlink, end_mavlink, start_rtps, end_rtps); - - // Drop only as much as we have to - size_t num_remove = math::max(start_mavlink, start_rtps); - - if (num_remove == 0) { - num_remove = buf_size; - } - - remove(0, num_remove); - start_mavlink -= math::min(num_remove, start_mavlink); - end_mavlink -= math::min(num_remove, end_mavlink); - start_rtps -= math::min(num_remove, start_rtps); - end_rtps -= math::min(num_remove, end_rtps); - } - - int bytes_available = 0; - int err = ::ioctl(fd, FIONREAD, (unsigned long)&bytes_available); - ssize_t r = 0; - - if (err != 0 || bytes_available > 0) { - r = ::read(fd, buffer + buf_size, sizeof(buffer) - buf_size); - } - - if (r <= 0) { - return r; - } - - buf_size += r; - bytes_received += r; - - // Update the lost/unused bytes count - update_lost_stats(); - - perf_set_count(bytes_received_count, bytes_received); - - return r; -} - -void ReadBuffer::copy(void *dest, size_t pos, size_t n) -{ - ASSERT(pos < buf_size); - ASSERT(pos + n <= buf_size); - - if (dest) { - memcpy(dest, buffer + pos, n); - } -} - -void ReadBuffer::remove(size_t pos, size_t n) -{ - ASSERT(pos < buf_size); - ASSERT(pos + n <= buf_size); - - memmove(buffer + pos, buffer + (pos + n), buf_size - pos - n); - buf_size -= n; -} - -void ReadBuffer::update_lost_stats() -{ - bytes_lost = bytes_received - mavlink_parsed - rtps_parsed - header_bytes_received; - - if (end_mavlink > start_mavlink) { - bytes_lost -= end_mavlink - start_mavlink; - } - - if (end_rtps > start_rtps) { - bytes_lost -= end_rtps - start_rtps; - } - - perf_set_count(bytes_lost_count, bytes_lost); -} - -void ReadBuffer::print_stats() -{ - PX4_INFO_RAW("\tReceived:\n"); - PX4_INFO_RAW("\tTotal: %9zu bytes\n", - bytes_received); - PX4_INFO_RAW("\tHeaders: %9zu bytes (%5.1f %%)\n", - header_bytes_received, - static_cast(static_cast(header_bytes_received) - / static_cast(bytes_received) - * 100.f)); - PX4_INFO_RAW("\tMAVLink: %9zu bytes (%5.1f %%)\n", - mavlink_parsed, - static_cast(static_cast(mavlink_parsed) - / static_cast(bytes_received - header_bytes_received) - * 100.f)); - PX4_INFO_RAW("\tRTPS: %9zu bytes (%5.1f %%)\n", - rtps_parsed, - static_cast(static_cast(rtps_parsed) - / static_cast(bytes_received - header_bytes_received) - * 100.f)); - - PX4_INFO_RAW("\tUnused: %9zu bytes (%5.1f %%)\n", bytes_lost, - static_cast(static_cast(bytes_lost) - / static_cast(bytes_received) - * 100.f)); -} - - -class DevCommon : public cdev::CDev -{ -public: - DevCommon(const char *device_path, ReadBuffer *read_buffer); - virtual ~DevCommon(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - virtual int open(file *filp); - virtual int close(file *filp); - - enum Operation {Read, Write}; - -protected: - - virtual pollevent_t poll_state(struct file *filp); - - int try_to_copy_data(char *buffer, size_t buflen, MessageType message_type); - void scan_for_packets(); - void cleanup(); - - void lock(enum Operation op) - { - sem_t *this_lock = op == Read ? &objects->r_lock : &objects->w_lock; - - while (sem_wait(this_lock) != 0) { - /* The only case that an error should occur here is if - * the wait was awakened by a signal. - */ - ASSERT(get_errno() == EINTR); - } - } - - void unlock(enum Operation op) - { - sem_t *this_lock = op == Read ? &objects->r_lock : &objects->w_lock; - sem_post(this_lock); - } - - int _fd = -1; - - Sp2Header _header; - - ReadBuffer *_read_buffer; -}; - -DevCommon::DevCommon(const char *device_path, ReadBuffer *read_buffer) - : CDev(device_path) - , _read_buffer(read_buffer) -{ -} - -DevCommon::~DevCommon() -{ - if (_fd >= 0) { - // discard all pending data, as close() might block otherwise on NuttX with flow control enabled - tcflush(_fd, TCIOFLUSH); - ::close(_fd); - } -} - -int DevCommon::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - if (cmd == FIONSPACE) { - int ret = ::ioctl(_fd, FIONSPACE, arg); - - if (ret == 0) { - int *buf_free = (int *)arg; - *buf_free -= sizeof(_header); - - if (*buf_free < 0) { - *buf_free = 0; - } - } - - return ret; - } - - return ::ioctl(_fd, cmd, arg); -} - -int DevCommon::open(file *filp) -{ - _fd = ::open(objects->device_name, O_RDWR | O_NOCTTY); - - if (_fd < 0) { - PX4_ERR("open failed: %s", strerror(errno)); - return -1; - } - - CDev::open(filp); - - - return 0; -} - -int DevCommon::close(file *filp) -{ - //int ret = ::close(_fd); // FIXME: calling this results in a dead-lock, because DevCommon::close() - // is called from within another close(), and NuttX seems to hold a semaphore at this point - _fd = -1; - CDev::close(filp); - return 0; -} - -pollevent_t DevCommon::poll_state(struct file *filp) -{ - pollfd fds[1]; - fds[0].fd = _fd; - fds[0].events = POLLIN; - - /* Here we should just check the poll state (which is called before an actual poll waiting). - * Instead we poll on the fd with some timeout, and then pretend that there is data. - * This will let the calling poll return immediately (there's still no busy loop since - * we do actually poll here). - * We do this because there is no simple way with the given interface to poll on - * the _fd in here or by overriding some other method. - */ - - ::poll(fds, sizeof(fds) / sizeof(fds[0]), 10); - - return POLLIN; -} - -int DevCommon::try_to_copy_data(char *buffer, size_t buflen, MessageType message_type) -{ - if (buflen == 0) { - return 0; - } - - switch (message_type) { - case MessageType::Mavlink: - if (_read_buffer->start_mavlink < _read_buffer->end_mavlink) { - // We have Mavlink data ready to send. - const size_t len_available = _read_buffer->end_mavlink - _read_buffer->start_mavlink; - // We can only send what fits in the callers buffer. - const size_t len_to_copy = math::min(len_available, buflen); - - // Copy it to the callers buffer and remove it from our buffer. - _read_buffer->copy(buffer, _read_buffer->start_mavlink, len_to_copy); - - // Shift the markers accordingly. - _read_buffer->start_mavlink += len_to_copy; - - // Keep track for stats. - _read_buffer->mavlink_parsed += len_to_copy; - - // Update the lost/unused bytes count - _read_buffer->update_lost_stats(); - - // Update the number of MAVLink bytes parsed - perf_set_count(mavlink_bytes_parsed_count, _read_buffer->mavlink_parsed); - - // Update the number of MAVLink messages parsed - perf_count(mavlink_messages_parsed_count); - - return len_to_copy; - - } else { - return 0; - } - - case MessageType::Rtps: - if (_read_buffer->start_rtps < _read_buffer->end_rtps) { - // We have Rtps data ready to send - const size_t len_available = _read_buffer->end_rtps - _read_buffer->start_rtps; - // We can only send what fits in the callers buffer. - const size_t len_to_copy = math::min(len_available, buflen); - - // Copy it to the callers buffer and remove it from our buffer. - _read_buffer->copy(buffer, _read_buffer->start_rtps, len_to_copy); - - // Shift the markers accordingly. - _read_buffer->start_rtps += len_to_copy; - - // Keep track for stats. - _read_buffer->rtps_parsed += len_to_copy; - - // Update the lost/unused bytes count - _read_buffer->update_lost_stats(); - - // Update the number of RTPS bytes parsed - perf_set_count(rtps_bytes_parsed_count, _read_buffer->rtps_parsed); - - // Update the number of RTPS messages parsed - perf_count(rtps_messages_parsed_count); - - return len_to_copy; - - } else { - return 0; - } - - break; - - - default: - return 0; - } -} - -void DevCommon::scan_for_packets() -{ - if (_read_buffer->buf_size < Sp2HeaderSize) { - // We have not even one header in the buffer, no need to scan yet. - return; - } - - bool mavlink_available = (_read_buffer->start_mavlink < _read_buffer->end_mavlink); - bool rtps_available = (_read_buffer->start_rtps < _read_buffer->end_rtps); - - if (mavlink_available && rtps_available) { - // We still have data for both, no need to scan yet. - return; - } - - const size_t begin = math::min(_read_buffer->end_mavlink, _read_buffer->end_rtps); - - for (size_t i = begin; i < _read_buffer->buf_size - Sp2HeaderSize; /* ++i */) { - - const Sp2Header *header = reinterpret_cast(&_read_buffer->buffer[i]); - - if (header->fields.magic != Sp2HeaderMagic) { - // Not the magic byte that we're looking for. - ++i; - continue; - } - - const uint8_t checksum = (_read_buffer->buffer[i] ^ _read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2]); - - if (header->fields.checksum != checksum) { - // Checksum failed. - ++i; - continue; - } - - if (header->fields.type != static_cast(MessageType::Mavlink) && - header->fields.type != static_cast(MessageType::Rtps)) { - // Ignore unknown protocols - ++i; - continue; - } - - const size_t payload_len = ((uint16_t)header->fields.len_h << 8) | header->fields.len_l; - - if (payload_len > sizeof(_read_buffer->buffer)) { - // This can happen if by accident data matches the header including checksum. - // Given we skip most data using the last payload_len, we should not see this too often, - // unless the link is very lossy and we often have to re-sync. - PX4_DEBUG("payload size %zu > buffer size %zu: %d, protocol: %s", - payload_len, sizeof(_read_buffer->buffer), - (header->fields.type == static_cast(MessageType::Mavlink)) ? "Mavlink" : "Rtps"); - ++i; - continue; - } - - if (i + Sp2HeaderSize + payload_len > _read_buffer->buf_size) { - // We don't have a enough data in the buffer yet, try again later. - break; - } - - if (header->fields.type == static_cast(MessageType::Mavlink) && !mavlink_available) { - _read_buffer->start_mavlink = i + Sp2HeaderSize; - _read_buffer->end_mavlink = _read_buffer->start_mavlink + payload_len; - mavlink_available = true; - - // Overwrite header magic byte, so we don't parse them again. - _read_buffer->buffer[i] = 0; - _read_buffer->header_bytes_received += Sp2HeaderSize; - - } else if (header->fields.type == static_cast(MessageType::Rtps) && !rtps_available) { - _read_buffer->start_rtps = i + Sp2HeaderSize; - _read_buffer->end_rtps = _read_buffer->start_rtps + payload_len; - rtps_available = true; - - // Overwrite header magic byte, so we don't parse them again. - _read_buffer->buffer[i] = 0; - _read_buffer->header_bytes_received += Sp2HeaderSize; - } - - if (mavlink_available && rtps_available) { - // Both have at least one message ready, we can stop now. - break; - } - - // Update the lost/unused bytes count - _read_buffer->update_lost_stats(); - - perf_set_count(header_bytes_received_count, _read_buffer->header_bytes_received); - - i += Sp2HeaderSize + payload_len; - - } -} - - -void DevCommon::cleanup() -{ - const bool mavlink_available = (_read_buffer->start_mavlink < _read_buffer->end_mavlink); - const bool rtps_available = (_read_buffer->start_rtps < _read_buffer->end_rtps); - - // Clean up garbage bytes and accumulated headers - - size_t garbage_end = 0; - - if (!mavlink_available && !rtps_available) { - garbage_end = math::max(_read_buffer->start_mavlink, _read_buffer->start_rtps); - - } else { - garbage_end = math::min(_read_buffer->start_mavlink, _read_buffer->start_rtps); - } - - if (garbage_end > 0) { - _read_buffer->remove(0, garbage_end); - - _read_buffer->start_mavlink -= math::min(garbage_end, _read_buffer->start_mavlink); - _read_buffer->end_mavlink -= math::min(garbage_end, _read_buffer->end_mavlink); - _read_buffer->start_rtps -= math::min(garbage_end, _read_buffer->start_rtps); - _read_buffer->end_rtps -= math::min(garbage_end, _read_buffer->end_rtps); - } -} - - -class Mavlink2Dev : public DevCommon -{ -public: - Mavlink2Dev(ReadBuffer *read_buffer); - virtual ~Mavlink2Dev() {} - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); -}; - -Mavlink2Dev::Mavlink2Dev(ReadBuffer *read_buffer) - : DevCommon("/dev/mavlink", read_buffer) -{ - _header.fields.magic = Sp2HeaderMagic; - _header.fields.len_h = 0; - _header.fields.len_l = 0; - _header.fields.checksum = 0; - _header.fields.type = static_cast(MessageType::Mavlink); -} - -ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen) -{ - lock(Read); - - // The cleanup needs to be right after a scan, so we don't clean up - // something that we haven't found yet. - scan_for_packets(); - cleanup(); - - // If we have already a packet ready in the current buffer, we don't have - // to read and can grab data straightaway. - int ret = try_to_copy_data(buffer, buflen, MessageType::Mavlink); - - if (ret > 0) { - unlock(Read); - return ret; - } - - // Otherwise, we have to do a read. - ret = _read_buffer->read(_fd); - - if (ret <= 0) { - unlock(Read); - return ret; - } - - // Now we need to check again if there is data available. - scan_for_packets(); - - // And try to copy it out. - ret = try_to_copy_data(buffer, buflen, MessageType::Mavlink); - - unlock(Read); - return ret; -} - -ssize_t Mavlink2Dev::write(struct file *filp, const char *buffer, size_t buflen) -{ - _header.fields.len_h = (buflen >> 8) & 0x7f; - _header.fields.len_l = buflen & 0xff; - _header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2]; - lock(Write); - int buf_free = 0; - ::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free); - ssize_t ret = -1; - - if ((size_t)buf_free >= sizeof(_header) + buflen) { - ret = ::write(_fd, _header.bytes, sizeof(_header)); - - if (ret == sizeof(_header)) { - ret = ::write(_fd, buffer, buflen); - } - } - - unlock(Write); - - return ret; -} - -class RtpsDev : public DevCommon -{ -public: - RtpsDev(ReadBuffer *read_buffer); - virtual ~RtpsDev() {} - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); - -protected: - static const uint8_t HEADER_SIZE = 10; -}; - -RtpsDev::RtpsDev(ReadBuffer *read_buffer) - : DevCommon("/dev/rtps", read_buffer) -{ - _header.fields.magic = Sp2HeaderMagic; - _header.fields.len_h = 0; - _header.fields.len_l = 0; - _header.fields.checksum = 0; - _header.fields.type = static_cast(MessageType::Rtps); -} - -ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen) -{ - lock(Read); - - scan_for_packets(); - cleanup(); - - // If we have already a packet ready in the current buffer, we don't have to read. - int ret = try_to_copy_data(buffer, buflen, MessageType::Rtps); - - if (ret > 0) { - unlock(Read); - return ret; - } - - // Otherwise, we have to do a read. - ret = _read_buffer->read(_fd); - - if (ret <= 0) { - unlock(Read); - return ret; - } - - scan_for_packets(); - - // And check again. - ret = try_to_copy_data(buffer, buflen, MessageType::Rtps); - - unlock(Read); - return ret; -} - -ssize_t RtpsDev::write(struct file *filp, const char *buffer, size_t buflen) -{ - _header.fields.len_h = (buflen >> 8) & 0x7f; - _header.fields.len_l = buflen & 0xff; - _header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2]; - lock(Write); - int buf_free = 0; - ssize_t ret = -1; - ::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free); - - if ((size_t)buf_free >= sizeof(_header) + buflen) { - ret = ::write(_fd, _header.bytes, sizeof(_header)); - - if (ret == sizeof(_header)) { - ret = ::write(_fd, buffer, buflen); - } - } - - unlock(Write); - - return ret; -} - -int protocol_splitter_main(int argc, char *argv[]) -{ - if (argc < 2) { - goto out; - } - - /* - * Start/load the driver. - */ - if (!strcmp(argv[1], "start")) { - if (objects) { - PX4_WARN("already running"); - return 1; - } - - if (argc != 3) { - goto out; - } - - objects = new StaticData(); - - if (!objects) { - PX4_ERR("alloc failed"); - return -1; - } - - bytes_received_count = perf_alloc(PC_COUNT, "protocol_splitter: bytes received"); - bytes_lost_count = perf_alloc(PC_COUNT, "protocol_splitter: bytes unused/lost"); - header_bytes_received_count = perf_alloc(PC_COUNT, "protocol_splitter: header bytes received"); - mavlink_messages_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: MAVLink msgs parsed"); - mavlink_bytes_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: MAVLink msgs bytes parsed"); - rtps_messages_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: RTPS msgs parsed"); - rtps_bytes_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: RTPS msgs bytes parsed"); - buffer_drops = perf_alloc(PC_COUNT, "protocol_splitter: buffer drops"); - - strncpy(objects->device_name, argv[2], sizeof(objects->device_name)); - sem_init(&objects->r_lock, 1, 1); - sem_init(&objects->w_lock, 1, 1); - objects->read_buffer = new ReadBuffer(); - objects->mavlink2 = new Mavlink2Dev(objects->read_buffer); - objects->rtps = new RtpsDev(objects->read_buffer); - - if (!objects->mavlink2 || !objects->rtps) { - delete objects->mavlink2; - delete objects->rtps; - delete objects->read_buffer; - sem_destroy(&objects->r_lock); - sem_destroy(&objects->w_lock); - delete objects; - objects = nullptr; - PX4_ERR("alloc failed"); - return -1; - - } else { - objects->mavlink2->init(); - objects->rtps->init(); - } - } - - if (!strcmp(argv[1], "stop")) { - if (objects) { - delete objects->mavlink2; - delete objects->rtps; - delete objects->read_buffer; - sem_destroy(&objects->r_lock); - sem_destroy(&objects->w_lock); - delete objects; - objects = nullptr; - - perf_free(bytes_received_count); - perf_free(header_bytes_received_count); - perf_free(bytes_lost_count); - perf_free(mavlink_messages_parsed_count); - perf_free(mavlink_bytes_parsed_count); - perf_free(rtps_messages_parsed_count); - perf_free(rtps_bytes_parsed_count); - perf_free(buffer_drops); - } - } - - /* - * Print driver status. - */ - if (!strcmp(argv[1], "status")) { - if (objects) { - PX4_INFO("running"); - - if (sem_wait(&objects->r_lock) != 0) { - return -1; - } - - objects->read_buffer->print_stats(); - sem_post(&objects->r_lock); - - } else { - PX4_INFO("not running"); - } - } - - return 0; - -out: - PX4_ERR("unrecognized command, try 'start ', 'stop', 'status'"); - return 1; -} diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 1601c4d4e6..14b4dcf9f6 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -65,6 +65,7 @@ add_subdirectory(systemlib) add_subdirectory(system_identification) add_subdirectory(tecs) add_subdirectory(terrain_estimation) +add_subdirectory(timesync) add_subdirectory(tunes) add_subdirectory(version) add_subdirectory(weather_vane) diff --git a/src/drivers/protocol_splitter/CMakeLists.txt b/src/lib/timesync/CMakeLists.txt similarity index 90% rename from src/drivers/protocol_splitter/CMakeLists.txt rename to src/lib/timesync/CMakeLists.txt index 8e09f1b106..dc150e0e79 100644 --- a/src/drivers/protocol_splitter/CMakeLists.txt +++ b/src/lib/timesync/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -30,10 +30,9 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ -px4_add_module( - MODULE drivers__protocol_splitter - MAIN protocol_splitter - SRCS - protocol_splitter.cpp - DEPENDS - ) + +px4_add_library(timesync + Timesync.cpp + Timesync.hpp +) +target_link_libraries(timesync) diff --git a/src/lib/timesync/Timesync.cpp b/src/lib/timesync/Timesync.cpp new file mode 100644 index 0000000000..84961b128a --- /dev/null +++ b/src/lib/timesync/Timesync.cpp @@ -0,0 +1,168 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file timesync.cpp + * timesync implementation. + * + * @author Mohammed Kabir + */ + +#include "Timesync.hpp" + +#include + +void Timesync::update(const uint64_t now_us, const int64_t remote_timestamp_ns, const int64_t originate_timestamp_ns) +{ + // Message originating from this system, compute time offset from it + if (remote_timestamp_ns > 0) { + // Calculate time offset between this system and the remote system, assuming RTT for + // the timesync packet is roughly equal both ways. + int64_t offset_us = (int64_t)((originate_timestamp_ns / 1000ULL) + now_us - (remote_timestamp_ns / 1000ULL) * 2) / 2 ; + + // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system + uint64_t rtt_us = now_us - (originate_timestamp_ns / 1000ULL); + + // Calculate the difference of this sample from the current estimate + uint64_t deviation = llabs((int64_t)_time_offset - offset_us); + + if (rtt_us < MAX_RTT_SAMPLE) { // Only use samples with low RTT + + if (sync_converged() && (deviation > MAX_DEVIATION_SAMPLE)) { + + // Increment the counter if we have a good estimate and are getting samples far from the estimate + _high_deviation_count++; + + // We reset the filter if we received 5 consecutive samples which violate our present estimate. + // This is most likely due to a time jump on the offboard system. + if (_high_deviation_count > MAX_CONSECUTIVE_HIGH_DEVIATION) { + PX4_ERR("Time jump detected. Resetting time synchroniser."); + // Reset the filter + reset_filter(); + } + + } else { + + // Filter gain scheduling + if (!sync_converged()) { + // Interpolate with a sigmoid function + double progress = (double)_sequence / (double)CONVERGENCE_WINDOW; + double p = 1.0 - exp(0.5 * (1.0 - 1.0 / (1.0 - progress))); + _filter_alpha = p * ALPHA_GAIN_FINAL + (1.0 - p) * ALPHA_GAIN_INITIAL; + _filter_beta = p * BETA_GAIN_FINAL + (1.0 - p) * BETA_GAIN_INITIAL; + + } else { + _filter_alpha = ALPHA_GAIN_FINAL; + _filter_beta = BETA_GAIN_FINAL; + } + + // Perform filter update + add_sample(offset_us); + + // Increment sequence counter after filter update + _sequence++; + + // Reset high deviation count after filter update + _high_deviation_count = 0; + + // Reset high RTT count after filter update + _high_rtt_count = 0; + } + + } else { + // Increment counter if round trip time is too high for accurate timesync + _high_rtt_count++; + + if (_high_rtt_count > MAX_CONSECUTIVE_HIGH_RTT) { + PX4_WARN("RTT too high for timesync: %llu ms", rtt_us / 1000ULL); + // Reset counter to rate-limit warnings + _high_rtt_count = 0; + } + + } + + // Publish status message + timesync_status_s tsync_status{}; + tsync_status.source_protocol = _source; + tsync_status.remote_timestamp = remote_timestamp_ns / 1000ULL; + tsync_status.observed_offset = offset_us; + tsync_status.estimated_offset = (int64_t)_time_offset; + tsync_status.round_trip_time = rtt_us; + tsync_status.timestamp = hrt_absolute_time(); + + _timesync_status_pub.publish(tsync_status); + } +} + +uint64_t Timesync::sync_stamp(uint64_t usec) +{ + // Only return synchronised stamp if we have converged to a good value + if (sync_converged()) { + return usec + (int64_t)_time_offset; + + } else { + return hrt_absolute_time(); + } +} + +void Timesync::add_sample(int64_t offset_us) +{ + // Online exponential smoothing filter. The derivative of the estimate is also + // estimated in order to produce an estimate without steady state lag: + // https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing + double time_offset_prev = _time_offset; + + if (_sequence == 0) { + // First offset sample + _time_offset = offset_us; + + } else { + // Update the clock offset estimate + _time_offset = _filter_alpha * offset_us + (1.0 - _filter_alpha) * (_time_offset + _time_skew); + + // Update the clock skew estimate + _time_skew = _filter_beta * (_time_offset - time_offset_prev) + (1.0 - _filter_beta) * _time_skew; + } +} + +void Timesync::reset_filter() +{ + // Do a full reset of all statistics and parameters + _sequence = 0; + _time_offset = 0.0; + _time_skew = 0.0; + _filter_alpha = ALPHA_GAIN_INITIAL; + _filter_beta = BETA_GAIN_INITIAL; + _high_deviation_count = 0; + _high_rtt_count = 0; +} diff --git a/src/lib/timesync/Timesync.hpp b/src/lib/timesync/Timesync.hpp new file mode 100644 index 0000000000..b17766b09a --- /dev/null +++ b/src/lib/timesync/Timesync.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Timesync.hpp + * time synchroniser definition. + * + * @author Mohammed Kabir + */ + +#pragma once + +#include + +#include +#include + +#include +#include + +using namespace time_literals; + +static constexpr time_t PX4_EPOCH_SECS = 1234567890ULL; + +// Filter gains +// +// Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead +// to a smoother estimate, but track time drift more slowly, introducing a bias +// in the estimate. Larger values will cause low-amplitude oscillations. +// +// Beta : Used to smooth the clock skew estimate. Smaller values will lead to a +// tighter estimation of the skew (derivative), but will negatively affect how fast the +// filter reacts to clock skewing (e.g cause by temperature changes to the oscillator). +// Larger values will cause large-amplitude oscillations. +static constexpr double ALPHA_GAIN_INITIAL = 0.05; +static constexpr double BETA_GAIN_INITIAL = 0.05; +static constexpr double ALPHA_GAIN_FINAL = 0.003; +static constexpr double BETA_GAIN_FINAL = 0.003; + +// Filter gain scheduling +// +// The filter interpolates between the INITIAL and FINAL gains while the number of +// exhanged timesync packets is less than CONVERGENCE_WINDOW. A lower value will +// allow the timesync to converge faster, but with potentially less accurate initial +// offset and skew estimates. +static constexpr uint32_t CONVERGENCE_WINDOW = 500; + +// Outlier rejection and filter reset +// +// Samples with round-trip time higher than MAX_RTT_SAMPLE are not used to update the filter. +// More than MAX_CONSECUTIVE_HIGH_RTT number of such events in a row will throw a warning +// but not reset the filter. +// Samples whose calculated clock offset is more than MAX_DEVIATION_SAMPLE off from the current +// estimate are not used to update the filter. More than MAX_CONSECUTIVE_HIGH_DEVIATION number +// of such events in a row will reset the filter. This usually happens only due to a time jump +// on the remote system. +// TODO : automatically determine these using ping statistics? +static constexpr uint64_t MAX_RTT_SAMPLE = 10_ms; +static constexpr uint64_t MAX_DEVIATION_SAMPLE = 100_ms; +static constexpr uint32_t MAX_CONSECUTIVE_HIGH_RTT = 5; +static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION = 5; + +class Timesync +{ +public: + Timesync(uint8_t source = timesync_status_s::SOURCE_PROTOCOL_UNKNOWN) : _source(source) {}; + ~Timesync() = default; + + void update(const uint64_t now_us, const int64_t remote_timestamp_ns, int64_t originate_timestamp_ns); + + /** + * Convert remote timestamp to local hrt time (usec) + * Use synchronised time if available, monotonic boot time otherwise + */ + uint64_t sync_stamp(uint64_t usec); + + int64_t offset() const { return (int64_t)_time_offset; } + +private: + + /** + * Online exponential filter to smooth time offset + */ + void add_sample(int64_t offset_us); + + /** + * Return true if the timesync algorithm converged to a good estimate, + * return false otherwise + */ + bool sync_converged() const { return _sequence >= CONVERGENCE_WINDOW; } + + /** + * Reset the exponential filter and its states + */ + void reset_filter(); + + uORB::PublicationMulti _timesync_status_pub{ORB_ID(timesync_status)}; + + uint32_t _sequence{0}; + + // Timesync statistics + double _time_offset{0}; + double _time_skew{0}; + + // Filter parameters + double _filter_alpha{ALPHA_GAIN_INITIAL}; + double _filter_beta{BETA_GAIN_INITIAL}; + + // Outlier rejection and filter reset + uint32_t _high_deviation_count{0}; + uint32_t _high_rtt_count{0}; + + uint8_t _source{}; +}; diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index e1b7d9972f..e76cdf582c 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -190,7 +190,7 @@ int AirshipAttitudeControl::print_status() perf_print_counter(_loop_perf); - print_message(ORB_ID(actuator_controls), _actuator_controls); + print_message(ORB_ID(actuator_controls_0), _actuator_controls); return 0; } diff --git a/src/modules/commander/failsafe/CMakeLists.txt b/src/modules/commander/failsafe/CMakeLists.txt index 23363265b1..0d7b3bd366 100644 --- a/src/modules/commander/failsafe/CMakeLists.txt +++ b/src/modules/commander/failsafe/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ # Extract information from failsafe msg file -set(failsafe_flags_msg_file ${PX4_SOURCE_DIR}/msg/failsafe_flags.msg) +set(failsafe_flags_msg_file ${PX4_SOURCE_DIR}/msg/FailsafeFlags.msg) set(generated_uorb_struct_field_mapping_header ${PX4_BINARY_DIR}/generated_uorb_struct_field_mapping.h) set(html_template_file ${CMAKE_CURRENT_SOURCE_DIR}/emscripten_template.html) set(html_output_file ${PX4_BINARY_DIR}/failsafe_html_template.html) diff --git a/src/modules/commander/failsafe/parse_flags_from_msg.py b/src/modules/commander/failsafe/parse_flags_from_msg.py index c7cace101e..6b1ddf24b9 100755 --- a/src/modules/commander/failsafe/parse_flags_from_msg.py +++ b/src/modules/commander/failsafe/parse_flags_from_msg.py @@ -12,7 +12,7 @@ import re if __name__ == "__main__": parser = argparse.ArgumentParser(description='Extract fields from .msg files') - parser.add_argument('msg_file', help='failsafe_flags.msg file') + parser.add_argument('msg_file', help='FailsafeFlags.msg file') parser.add_argument('header_file', help='generated_uorb_struct_field_mapping.h') parser.add_argument('html_template', help='HTML template input file') parser.add_argument('html_output', help='HTML output file') @@ -81,7 +81,7 @@ if __name__ == "__main__": macro_lines = '' for group in groups: for field_type, field_name, comment in group.fields: - macro_lines += ' .property("{0}", &{1}_s::{0}) \\\n'.format(field_name, msg_name) + macro_lines += ' .property("{0}", &px4::msg::{1}::{0}) \\\n'.format(field_name, msg_name) cpp_emscription_macro = '#define UORB_STRUCT_FIELD_MAPPING \\\n{}\n'.format(macro_lines) diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 0e89a350ae..261ee9a338 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -49,7 +49,7 @@ #include -void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const +void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &airspeed) const { // reset flags resetEstimatorAidStatus(airspeed); @@ -75,7 +75,7 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so setEstimatorAidStatusTestRatio(airspeed, innov_gate); } -void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed) +void Ekf::fuseAirspeed(estimator_aid_source1d_s &airspeed) { if (airspeed.innovation_rejected) { return; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 14ab85e479..93a8bec694 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -49,9 +49,9 @@ #include "bias_estimator.hpp" #include "height_bias_estimator.hpp" -#include -#include -#include +#include +#include +#include enum class Likelihood { LOW, MEDIUM, HIGH }; @@ -551,30 +551,30 @@ private: uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec) Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive - estimator_aid_source_1d_s _aid_src_baro_hgt{}; - estimator_aid_source_1d_s _aid_src_rng_hgt{}; - estimator_aid_source_1d_s _aid_src_airspeed{}; - estimator_aid_source_1d_s _aid_src_sideslip{}; + estimator_aid_source1d_s _aid_src_baro_hgt{}; + estimator_aid_source1d_s _aid_src_rng_hgt{}; + estimator_aid_source1d_s _aid_src_airspeed{}; + estimator_aid_source1d_s _aid_src_sideslip{}; - estimator_aid_source_2d_s _aid_src_fake_pos{}; - estimator_aid_source_1d_s _aid_src_fake_hgt{}; + estimator_aid_source2d_s _aid_src_fake_pos{}; + estimator_aid_source1d_s _aid_src_fake_hgt{}; - estimator_aid_source_1d_s _aid_src_ev_hgt{}; - estimator_aid_source_2d_s _aid_src_ev_pos{}; - estimator_aid_source_3d_s _aid_src_ev_vel{}; - estimator_aid_source_1d_s _aid_src_ev_yaw{}; + estimator_aid_source1d_s _aid_src_ev_hgt{}; + estimator_aid_source2d_s _aid_src_ev_pos{}; + estimator_aid_source3d_s _aid_src_ev_vel{}; + estimator_aid_source1d_s _aid_src_ev_yaw{}; - estimator_aid_source_1d_s _aid_src_gnss_hgt{}; - estimator_aid_source_2d_s _aid_src_gnss_pos{}; - estimator_aid_source_3d_s _aid_src_gnss_vel{}; - estimator_aid_source_1d_s _aid_src_gnss_yaw{}; + estimator_aid_source1d_s _aid_src_gnss_hgt{}; + estimator_aid_source2d_s _aid_src_gnss_pos{}; + estimator_aid_source3d_s _aid_src_gnss_vel{}; + estimator_aid_source1d_s _aid_src_gnss_yaw{}; - estimator_aid_source_1d_s _aid_src_mag_heading{}; - estimator_aid_source_3d_s _aid_src_mag{}; + estimator_aid_source1d_s _aid_src_mag_heading{}; + estimator_aid_source3d_s _aid_src_mag{}; - estimator_aid_source_2d_s _aid_src_aux_vel{}; + estimator_aid_source2d_s _aid_src_aux_vel{}; - estimator_aid_source_2d_s _aid_src_optical_flow{}; + estimator_aid_source2d_s _aid_src_optical_flow{}; // output predictor states Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad) @@ -662,12 +662,12 @@ private: void predictCovariance(); // ekf sequential fusion of magnetometer measurements - bool fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states = true); + bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); // update quaternion states and covariances using an innovation, observation variance and Jacobian vector // innovation : prediction - measurement // variance : observaton variance - bool fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s &aid_src_status); + bool fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s &aid_src_status); // fuse the yaw angle obtained from a dual antenna GPS unit void fuseGpsYaw(const gpsSample &gps_sample); @@ -683,12 +683,12 @@ private: // apply sensible limits to the declination and length of the NE mag field states estimates void limitDeclination(); - void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const; - void fuseAirspeed(estimator_aid_source_1d_s &airspeed); + void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &airspeed) const; + void fuseAirspeed(estimator_aid_source1d_s &airspeed); // fuse synthetic zero sideslip measurement - void updateSideslip(estimator_aid_source_1d_s &_aid_src_sideslip) const; - void fuseSideslip(estimator_aid_source_1d_s &_aid_src_sideslip); + void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const; + void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip); // fuse body frame drag specific forces for multi-rotor wind estimation void fuseDrag(const dragSample &drag_sample); @@ -718,24 +718,24 @@ private: void resetVerticalVelocityToZero(); // fuse optical flow line of sight rate measurements - void updateOptFlow(estimator_aid_source_2d_s &aid_src); + void updateOptFlow(estimator_aid_source2d_s &aid_src); void fuseOptFlow(); // horizontal and vertical position aid source - void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source_2d_s &aid_src) const; - void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source_1d_s &aid_src) const; + void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; + void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const; // 2d & 3d velocity aid source - void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source_2d_s &aid_src) const; - void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source_3d_s &aid_src) const; + void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; + void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const; // horizontal and vertical position fusion - void fuseHorizontalPosition(estimator_aid_source_2d_s &pos_aid_src); - void fuseVerticalPosition(estimator_aid_source_1d_s &hgt_aid_src); + void fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src); + void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src); // 2d & 3d velocity fusion - void fuseVelocity(estimator_aid_source_2d_s &vel_aid_src); - void fuseVelocity(estimator_aid_source_3d_s &vel_aid_src); + void fuseVelocity(estimator_aid_source2d_s &vel_aid_src); + void fuseVelocity(estimator_aid_source3d_s &vel_aid_src); void updateGpsYaw(const gpsSample &gps_sample); @@ -1085,7 +1085,7 @@ private: void resetGpsDriftCheckFilters(); - void resetEstimatorAidStatus(estimator_aid_source_1d_s &status) const + void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const { // only bother resetting if timestamp_sample is set if (status.timestamp_sample != 0) { @@ -1130,7 +1130,7 @@ private: } } - void setEstimatorAidStatusTestRatio(estimator_aid_source_1d_s &status, float innovation_gate) const + void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const { if (PX4_ISFINITE(status.innovation) && PX4_ISFINITE(status.innovation_variance) && (status.innovation_variance > 0.f)) { status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance); diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index dbc7440466..7bf93699f8 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -50,7 +50,7 @@ #include -bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, bool update_all_states) +bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states) { // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f)); @@ -223,7 +223,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b } // update quaternion states and covariances using the yaw innovation and yaw observation variance -bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s& aid_src_status) +bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status) { aid_src_status.innovation = innovation; diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index fc3b210175..26a199c18f 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -47,7 +47,7 @@ #include #include "utils.hpp" -void Ekf::updateOptFlow(estimator_aid_source_2d_s &aid_src) +void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) { resetEstimatorAidStatus(aid_src); aid_src.timestamp_sample = _flow_sample_delayed.time_us; diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 8a6b281a77..fae55e0c4a 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -47,7 +47,7 @@ #include -void Ekf::updateSideslip(estimator_aid_source_1d_s &sideslip) const +void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const { // reset flags resetEstimatorAidStatus(sideslip); @@ -71,7 +71,7 @@ void Ekf::updateSideslip(estimator_aid_source_1d_s &sideslip) const setEstimatorAidStatusTestRatio(sideslip, innov_gate); } -void Ekf::fuseSideslip(estimator_aid_source_1d_s &sideslip) +void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) { if (sideslip.innovation_rejected) { return; diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index e3959326f9..e7cf2db341 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -44,7 +44,7 @@ #include "ekf.h" void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, - const float innov_gate, estimator_aid_source_2d_s &aid_src) const + const float innov_gate, estimator_aid_source2d_s &aid_src) const { resetEstimatorAidStatus(aid_src); @@ -62,7 +62,7 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &ob } void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, - const float innov_gate, estimator_aid_source_3d_s &aid_src) const + const float innov_gate, estimator_aid_source3d_s &aid_src) const { resetEstimatorAidStatus(aid_src); @@ -88,7 +88,7 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob } void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, - const float innov_gate, estimator_aid_source_1d_s &aid_src) const + const float innov_gate, estimator_aid_source1d_s &aid_src) const { resetEstimatorAidStatus(aid_src); @@ -112,7 +112,7 @@ void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const floa } void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, - const float innov_gate, estimator_aid_source_2d_s &aid_src) const + const float innov_gate, estimator_aid_source2d_s &aid_src) const { resetEstimatorAidStatus(aid_src); @@ -129,7 +129,7 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve aid_src.timestamp_sample = time_us; } -void Ekf::fuseVelocity(estimator_aid_source_2d_s &aid_src) +void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src) { if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { // vx, vy @@ -145,7 +145,7 @@ void Ekf::fuseVelocity(estimator_aid_source_2d_s &aid_src) } } -void Ekf::fuseVelocity(estimator_aid_source_3d_s &aid_src) +void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) { if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { // vx, vy, vz @@ -162,7 +162,7 @@ void Ekf::fuseVelocity(estimator_aid_source_3d_s &aid_src) } } -void Ekf::fuseHorizontalPosition(estimator_aid_source_2d_s &aid_src) +void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) { // x & y if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { @@ -178,7 +178,7 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source_2d_s &aid_src) } } -void Ekf::fuseVerticalPosition(estimator_aid_source_1d_s &aid_src) +void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) { // z if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { diff --git a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp index 140875b9d6..2beea63e9f 100644 --- a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp @@ -47,7 +47,7 @@ void Ekf::controlZeroInnovationHeadingUpdate() // fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low float innovation = 0.f; float obs_var = _control_status.flags.vehicle_at_rest ? 0.001f : 0.1f; - estimator_aid_source_1d_s unused; + estimator_aid_source1d_s unused; fuseYaw(innovation, obs_var, unused); _last_static_yaw = NAN; @@ -62,7 +62,7 @@ void Ekf::controlZeroInnovationHeadingUpdate() if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { float innovation = wrap_pi(euler_yaw - _last_static_yaw); float obs_var = 0.01f; - estimator_aid_source_1d_s unused; + estimator_aid_source1d_s unused; fuseYaw(innovation, obs_var, unused); } @@ -78,7 +78,7 @@ void Ekf::controlZeroInnovationHeadingUpdate() if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { float innovation = 0.f; float obs_var = 0.01f; - estimator_aid_source_1d_s unused; + estimator_aid_source1d_s unused; fuseYaw(innovation, obs_var, unused); } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index f582d7fb2f..652e3f6b6f 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -353,30 +353,30 @@ private: uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; - uORB::PublicationMulti _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)}; - uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)}; - uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; - uORB::PublicationMulti _estimator_aid_src_sideslip_pub{ORB_ID(estimator_aid_src_sideslip)}; + uORB::PublicationMulti _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)}; + uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)}; + uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; + uORB::PublicationMulti _estimator_aid_src_sideslip_pub{ORB_ID(estimator_aid_src_sideslip)}; - uORB::PublicationMulti _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)}; - uORB::PublicationMulti _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)}; + uORB::PublicationMulti _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)}; + uORB::PublicationMulti _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)}; - uORB::PublicationMulti _estimator_aid_src_ev_hgt_pub{ORB_ID(estimator_aid_src_ev_hgt)}; - uORB::PublicationMulti _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)}; - uORB::PublicationMulti _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)}; - uORB::PublicationMulti _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)}; + uORB::PublicationMulti _estimator_aid_src_ev_hgt_pub{ORB_ID(estimator_aid_src_ev_hgt)}; + uORB::PublicationMulti _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)}; + uORB::PublicationMulti _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)}; + uORB::PublicationMulti _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)}; - uORB::PublicationMulti _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)}; - uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; - uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; - uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)}; + uORB::PublicationMulti _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)}; + uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; + uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; + uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)}; - uORB::PublicationMulti _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)}; - uORB::PublicationMulti _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)}; + uORB::PublicationMulti _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)}; + uORB::PublicationMulti _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)}; - uORB::PublicationMulti _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)}; + uORB::PublicationMulti _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)}; - uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; + uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; // publications with topic dependent on multi-mode uORB::PublicationMulti _attitude_pub; diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index b6163ee203..5d02efce66 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -100,6 +100,7 @@ px4_add_module( sensor_calibration geo mavlink_c + timesync tunes version UNITY_BUILD diff --git a/src/modules/mavlink/mavlink_timesync.cpp b/src/modules/mavlink/mavlink_timesync.cpp index ee0d2e3272..7bbe8f8dfa 100644 --- a/src/modules/mavlink/mavlink_timesync.cpp +++ b/src/modules/mavlink/mavlink_timesync.cpp @@ -72,82 +72,7 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg) } else if (tsync.tc1 > 0) { // Message originating from this system, compute time offset from it - // Calculate time offset between this system and the remote system, assuming RTT for - // the timesync packet is roughly equal both ways. - int64_t offset_us = (int64_t)((tsync.ts1 / 1000ULL) + now - (tsync.tc1 / 1000ULL) * 2) / 2 ; - - // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system - uint64_t rtt_us = now - (tsync.ts1 / 1000ULL); - - // Calculate the difference of this sample from the current estimate - uint64_t deviation = llabs((int64_t)_time_offset - offset_us); - - if (rtt_us < MAX_RTT_SAMPLE) { // Only use samples with low RTT - - if (sync_converged() && (deviation > MAX_DEVIATION_SAMPLE)) { - - // Increment the counter if we have a good estimate and are getting samples far from the estimate - _high_deviation_count++; - - // We reset the filter if we received 5 consecutive samples which violate our present estimate. - // This is most likely due to a time jump on the offboard system. - if (_high_deviation_count > MAX_CONSECUTIVE_HIGH_DEVIATION) { - PX4_ERR("[timesync] Time jump detected. Resetting time synchroniser."); - // Reset the filter - reset_filter(); - } - - } else { - - // Filter gain scheduling - if (!sync_converged()) { - // Interpolate with a sigmoid function - double progress = (double)_sequence / (double)CONVERGENCE_WINDOW; - double p = 1.0 - exp(0.5 * (1.0 - 1.0 / (1.0 - progress))); - _filter_alpha = p * ALPHA_GAIN_FINAL + (1.0 - p) * ALPHA_GAIN_INITIAL; - _filter_beta = p * BETA_GAIN_FINAL + (1.0 - p) * BETA_GAIN_INITIAL; - - } else { - _filter_alpha = ALPHA_GAIN_FINAL; - _filter_beta = BETA_GAIN_FINAL; - } - - // Perform filter update - add_sample(offset_us); - - // Increment sequence counter after filter update - _sequence++; - - // Reset high deviation count after filter update - _high_deviation_count = 0; - - // Reset high RTT count after filter update - _high_rtt_count = 0; - } - - } else { - // Increment counter if round trip time is too high for accurate timesync - _high_rtt_count++; - - if (_high_rtt_count > MAX_CONSECUTIVE_HIGH_RTT) { - PX4_WARN("[timesync] RTT too high for timesync: %llu ms (sender: %i)", rtt_us / 1000ULL, msg->compid); - // Reset counter to rate-limit warnings - _high_rtt_count = 0; - } - - } - - // Publish status message - timesync_status_s tsync_status{}; - - tsync_status.timestamp = hrt_absolute_time(); - tsync_status.source_protocol = timesync_status_s::SOURCE_PROTOCOL_MAVLINK; - tsync_status.remote_timestamp = tsync.tc1 / 1000ULL; - tsync_status.observed_offset = offset_us; - tsync_status.estimated_offset = (int64_t)_time_offset; - tsync_status.round_trip_time = rtt_us; - - _timesync_status_pub.publish(tsync_status); + _timesync.update(now, tsync.tc1, tsync.ts1); } break; @@ -181,58 +106,3 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg) break; } } - -uint64_t -MavlinkTimesync::sync_stamp(uint64_t usec) -{ - // Only return synchronised stamp if we have converged to a good value - if (sync_converged()) { - return usec + (int64_t)_time_offset; - - } else { - return hrt_absolute_time(); - } -} - -bool -MavlinkTimesync::sync_converged() -{ - return _sequence >= CONVERGENCE_WINDOW; -} - -void -MavlinkTimesync::add_sample(int64_t offset_us) -{ - /* Online exponential smoothing filter. The derivative of the estimate is also - * estimated in order to produce an estimate without steady state lag: - * https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing - */ - - double time_offset_prev = _time_offset; - - if (_sequence == 0) { // First offset sample - _time_offset = offset_us; - - } else { - // Update the clock offset estimate - _time_offset = _filter_alpha * offset_us + (1.0 - _filter_alpha) * (_time_offset + _time_skew); - - // Update the clock skew estimate - _time_skew = _filter_beta * (_time_offset - time_offset_prev) + (1.0 - _filter_beta) * _time_skew; - } - -} - -void -MavlinkTimesync::reset_filter() -{ - // Do a full reset of all statistics and parameters - _sequence = 0; - _time_offset = 0.0; - _time_skew = 0.0; - _filter_alpha = ALPHA_GAIN_INITIAL; - _filter_beta = BETA_GAIN_INITIAL; - _high_deviation_count = 0; - _high_rtt_count = 0; - -} diff --git a/src/modules/mavlink/mavlink_timesync.h b/src/modules/mavlink/mavlink_timesync.h index 3204696f42..a3e8295ae0 100644 --- a/src/modules/mavlink/mavlink_timesync.h +++ b/src/modules/mavlink/mavlink_timesync.h @@ -42,55 +42,7 @@ #include "mavlink_bridge_header.h" -#include -#include - -#include - -#include -#include - -using namespace time_literals; - -static constexpr time_t PX4_EPOCH_SECS = 1234567890ULL; - -// Filter gains -// -// Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead -// to a smoother estimate, but track time drift more slowly, introducing a bias -// in the estimate. Larger values will cause low-amplitude oscillations. -// -// Beta : Used to smooth the clock skew estimate. Smaller values will lead to a -// tighter estimation of the skew (derivative), but will negatively affect how fast the -// filter reacts to clock skewing (e.g cause by temperature changes to the oscillator). -// Larger values will cause large-amplitude oscillations. -static constexpr double ALPHA_GAIN_INITIAL = 0.05; -static constexpr double BETA_GAIN_INITIAL = 0.05; -static constexpr double ALPHA_GAIN_FINAL = 0.003; -static constexpr double BETA_GAIN_FINAL = 0.003; - -// Filter gain scheduling -// -// The filter interpolates between the INITIAL and FINAL gains while the number of -// exhanged timesync packets is less than CONVERGENCE_WINDOW. A lower value will -// allow the timesync to converge faster, but with potentially less accurate initial -// offset and skew estimates. -static constexpr uint32_t CONVERGENCE_WINDOW = 500; - -// Outlier rejection and filter reset -// -// Samples with round-trip time higher than MAX_RTT_SAMPLE are not used to update the filter. -// More than MAX_CONSECUTIVE_HIGH_RTT number of such events in a row will throw a warning -// but not reset the filter. -// Samples whose calculated clock offset is more than MAX_DEVIATION_SAMPLE off from the current -// estimate are not used to update the filter. More than MAX_CONSECUTIVE_HIGH_DEVIATION number -// of such events in a row will reset the filter. This usually happens only due to a time jump -// on the remote system. -// TODO : automatically determine these using ping statistics? -static constexpr uint64_t MAX_RTT_SAMPLE = 10_ms; -static constexpr uint64_t MAX_DEVIATION_SAMPLE = 100_ms; -static constexpr uint32_t MAX_CONSECUTIVE_HIGH_RTT = 5; -static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION = 5; +#include class Mavlink; @@ -106,47 +58,9 @@ public: * Convert remote timestamp to local hrt time (usec) * Use synchronised time if available, monotonic boot time otherwise */ - uint64_t sync_stamp(uint64_t usec); + uint64_t sync_stamp(uint64_t usec) { return _timesync.sync_stamp(usec); } private: - - /* do not allow top copying this class */ - MavlinkTimesync(MavlinkTimesync &); - MavlinkTimesync &operator = (const MavlinkTimesync &); - -protected: - - /** - * Online exponential filter to smooth time offset - */ - void add_sample(int64_t offset_us); - - /** - * Return true if the timesync algorithm converged to a good estimate, - * return false otherwise - */ - bool sync_converged(); - - /** - * Reset the exponential filter and its states - */ - void reset_filter(); - - uORB::PublicationMulti _timesync_status_pub{ORB_ID(timesync_status)}; - - uint32_t _sequence{0}; - - // Timesync statistics - double _time_offset{0}; - double _time_skew{0}; - - // Filter parameters - double _filter_alpha{ALPHA_GAIN_INITIAL}; - double _filter_beta{BETA_GAIN_INITIAL}; - - // Outlier rejection and filter reset - uint32_t _high_deviation_count{0}; - uint32_t _high_rtt_count{0}; - Mavlink *const _mavlink; + Timesync _timesync{}; }; diff --git a/src/modules/microdds_client/CMakeLists.txt b/src/modules/microdds_client/CMakeLists.txt index a3bad1a701..8860267f79 100644 --- a/src/modules/microdds_client/CMakeLists.txt +++ b/src/modules/microdds_client/CMakeLists.txt @@ -31,89 +31,125 @@ # ############################################################################ -px4_add_git_submodule(TARGET git_micro_xrce_dds_client PATH "${CMAKE_CURRENT_LIST_DIR}/Micro-XRCE-DDS-Client") +if(${CMAKE_VERSION} VERSION_LESS "3.11") + message(WARNING "skipping microdds_client, Micro-XRCE-DDS-Client needs to be fixed to work with CMAKE_VERSION ${CMAKE_VERSION}") -# If a toolchain file is given, explicitly define its path when building the Micro-XRCE-DDS-CLient -if(CMAKE_TOOLCHAIN_FILE MATCHES "cmake$") - set(microxrceddsclient_toolchain ${CMAKE_TOOLCHAIN_FILE}) -elseif(CMAKE_TOOLCHAIN_FILE) - set(microxrceddsclient_toolchain ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/${CMAKE_TOOLCHAIN_FILE}.cmake) -endif() +else() + px4_add_git_submodule(TARGET git_micro_xrce_dds_client PATH "${CMAKE_CURRENT_LIST_DIR}/Micro-XRCE-DDS-Client") -# Include NuttX headers -get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) -set(c_flags_with_includes "${CMAKE_C_FLAGS}") -set(cxx_flags_with_includes "${CMAKE_CXX_FLAGS}") -foreach(dir ${include_dirs}) - set(c_flags_with_includes "${c_flags_with_includes} -I${dir}") - set(cxx_flags_with_includes "${cxx_flags_with_includes} -I${dir}") -endforeach() + # If a toolchain file is given, explicitly define its path when building the Micro-XRCE-DDS-CLient + if(CMAKE_TOOLCHAIN_FILE MATCHES "cmake$") + set(microxrceddsclient_toolchain ${CMAKE_TOOLCHAIN_FILE}) + elseif(CMAKE_TOOLCHAIN_FILE) + set(microxrceddsclient_toolchain ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/${CMAKE_TOOLCHAIN_FILE}.cmake) + endif() -set(lib_dir ${CMAKE_INSTALL_LIBDIR}) -if("${lib_dir}" STREQUAL "") - set(lib_dir "lib") -endif() + # Include NuttX headers + get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) + set(c_flags_with_includes "${CMAKE_C_FLAGS}") + set(cxx_flags_with_includes "${CMAKE_CXX_FLAGS}") + foreach(dir ${include_dirs}) + set(c_flags_with_includes "${c_flags_with_includes} -I${dir}") + set(cxx_flags_with_includes "${cxx_flags_with_includes} -I${dir}") + endforeach() -set(microxrceddsclient_src_dir ${CMAKE_CURRENT_SOURCE_DIR}/Micro-XRCE-DDS-Client) -set(microxrceddsclient_build_dir ${CMAKE_CURRENT_BINARY_DIR}) + set(lib_dir ${CMAKE_INSTALL_LIBDIR}) + if("${lib_dir}" STREQUAL "") + set(lib_dir "lib") + endif() -include(ExternalProject) + set(microxrceddsclient_src_dir ${CMAKE_CURRENT_SOURCE_DIR}/Micro-XRCE-DDS-Client) + set(microxrceddsclient_build_dir ${CMAKE_CURRENT_BINARY_DIR}) -ExternalProject_Add( - libmicroxrceddsclient_project - PREFIX ${microxrceddsclient_build_dir} - SOURCE_DIR ${microxrceddsclient_src_dir} - INSTALL_DIR ${CMAKE_CURRENT_BINARY_DIR} - CMAKE_CACHE_ARGS - -DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER} - -DCMAKE_C_COMPILER:FILEPATH=${CMAKE_C_COMPILER} - -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE} - -DCMAKE_C_FLAGS:STRING=${c_flags_with_includes} - -DCMAKE_CXX_FLAGS:STRING=${cxx_flags_with_includes} - -DCMAKE_EXE_LINKER_FLAGS:STRING=${CMAKE_EXE_LINKER_FLAGS} - -DUCLIENT_PIC:BOOL=OFF - -DUCLIENT_PROFILE_TCP:BOOL=OFF - -DUCLIENT_PROFILE_UDP:BOOL=ON - -DUCLIENT_PROFILE_SERIAL:BOOL=ON - -DUCLIENT_PROFILE_DISCOVERY:BOOL=OFF - -DUCLIENT_PROFILE_CUSTOM_TRANSPORT:BOOL=ON - -DUCLIENT_PLATFORM_POSIX:BOOL=ON - -DUCLIENT_BUILD_MICROCDR:BOOL=ON - -DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_CURRENT_BINARY_DIR} - -DCMAKE_PREFIX_PATH:PATH=${CMAKE_CURRENT_BINARY_DIR} - -DCMAKE_TOOLCHAIN_FILE:STRING=${microxrceddsclient_toolchain} - BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a - BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a - DEPENDS prebuild_targets git_micro_xrce_dds_client - ) + include(ExternalProject) -add_library(libmicroxrceddsclient STATIC IMPORTED GLOBAL) -set_target_properties(libmicroxrceddsclient - PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a -) - -add_library(libmicrocdr STATIC IMPORTED GLOBAL) -set_target_properties(libmicrocdr - PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a -) - -add_library(microxrceddsclient INTERFACE) -add_dependencies(microxrceddsclient libmicroxrceddsclient) -add_dependencies(microxrceddsclient libmicrocdr) -add_dependencies(microxrceddsclient libmicroxrceddsclient_project) -target_include_directories(microxrceddsclient INTERFACE ${microxrceddsclient_build_dir}/include) - -px4_add_module( - MODULE modules__microdds_client - MAIN microdds_client - STACK_MAIN 8000 - SRCS - microdds_client.cpp - DEPENDS - microxrceddsclient - libmicroxrceddsclient - libmicrocdr - uorb_ucdr_headers + ExternalProject_Add( + libmicroxrceddsclient_project + PREFIX ${microxrceddsclient_build_dir} + SOURCE_DIR ${microxrceddsclient_src_dir} + INSTALL_DIR ${CMAKE_CURRENT_BINARY_DIR} + CMAKE_CACHE_ARGS + -DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER} + -DCMAKE_C_COMPILER:FILEPATH=${CMAKE_C_COMPILER} + -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE} + -DCMAKE_C_FLAGS:STRING=${c_flags_with_includes} + -DCMAKE_CXX_FLAGS:STRING=${cxx_flags_with_includes} + -DCMAKE_EXE_LINKER_FLAGS:STRING=${CMAKE_EXE_LINKER_FLAGS} + -DUCLIENT_PIC:BOOL=OFF + -DUCLIENT_PROFILE_TCP:BOOL=OFF + -DUCLIENT_PROFILE_UDP:BOOL=ON + -DUCLIENT_PROFILE_SERIAL:BOOL=ON + -DUCLIENT_PROFILE_DISCOVERY:BOOL=OFF + -DUCLIENT_PROFILE_CUSTOM_TRANSPORT:BOOL=OFF + -DUCLIENT_PROFILE_MULTITHREAD:BOOL=OFF + -DUCLIENT_PROFILE_SHARED_MEMORY:BOOL=OFF + -DUCLIENT_PLATFORM_POSIX:BOOL=ON + -DUCLIENT_BUILD_MICROCDR:BOOL=ON + -DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_CURRENT_BINARY_DIR} + -DCMAKE_PREFIX_PATH:PATH=${CMAKE_CURRENT_BINARY_DIR} + -DCMAKE_TOOLCHAIN_FILE:STRING=${microxrceddsclient_toolchain} + BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a + BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a + DEPENDS prebuild_targets git_micro_xrce_dds_client + LOG_CONFIGURE true + LOG_BUILD true + LOG_INSTALL true + LOG_OUTPUT_ON_FAILURE true ) -add_dependencies(modules__microdds_client topic_bridge_files) + add_library(libmicroxrceddsclient STATIC IMPORTED GLOBAL) + set_target_properties(libmicroxrceddsclient + PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a + ) + + add_library(libmicrocdr STATIC IMPORTED GLOBAL) + set_target_properties(libmicrocdr + PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a + ) + + add_library(microxrceddsclient INTERFACE) + add_dependencies(microxrceddsclient libmicroxrceddsclient) + add_dependencies(microxrceddsclient libmicrocdr) + add_dependencies(microxrceddsclient libmicroxrceddsclient_project) + target_include_directories(microxrceddsclient INTERFACE ${microxrceddsclient_build_dir}/include) + + + + add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h + COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py + --topic-msg-dir ${PX4_SOURCE_DIR}/msg + --client-outdir ${CMAKE_CURRENT_BINARY_DIR} + --dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml + --template_file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em + DEPENDS + ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py + ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml + ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em + COMMENT "Generating XRCE-DDS topic bridge" + ) + add_custom_target(topic_bridge_files DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h) + + px4_add_module( + MODULE modules__microdds_client + MAIN microdds_client + STACK_MAIN 9000 + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR} + COMPILE_FLAGS + #-O0 + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h + microdds_client.cpp + microdds_client.h + DEPENDS + microxrceddsclient + libmicroxrceddsclient + libmicrocdr + timesync + topic_bridge_files + uorb_ucdr_headers + MODULE_CONFIG + module.yaml + ) +endif() diff --git a/src/modules/microdds_client/Micro-XRCE-DDS-Client b/src/modules/microdds_client/Micro-XRCE-DDS-Client index b5187a9f39..4248559f3b 160000 --- a/src/modules/microdds_client/Micro-XRCE-DDS-Client +++ b/src/modules/microdds_client/Micro-XRCE-DDS-Client @@ -1 +1 @@ -Subproject commit b5187a9f399e34cda7d2cdce8823295f83d9f3cc +Subproject commit 4248559f3b111155c783e524e461ccc83e768103 diff --git a/src/modules/microdds_client/dds_topics.h.em b/src/modules/microdds_client/dds_topics.h.em new file mode 100644 index 0000000000..bde8b8d8cb --- /dev/null +++ b/src/modules/microdds_client/dds_topics.h.em @@ -0,0 +1,127 @@ +@############################################### +@# +@# EmPy template +@# +@############################################### +@# Generates publications and subscriptions for XRCE +@# +@# Context: +@# - msgs (List) list of all RTPS messages +@# - topics (List) list of all topic names +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@############################################### +@{ +import os + + +}@ + +#include + +#include +#include + +#include +#include +@[for include in type_includes]@ +#include +@[end for]@ + +// Subscribers for messages to send +struct SendTopicsSubs { +@[ for pub in publications]@ + uORB::Subscription @(pub['topic_simple'])_sub{ORB_ID(@(pub['topic_simple']))}; + uxrObjectId @(pub['topic_simple'])_data_writer{}; +@[ end for]@ + + uint32_t num_payload_sent{}; + + void update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace); +}; + +void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace) +{ + int64_t time_offset_us = session->time_offset / 1000; // ns -> us +@[ for idx, pub in enumerate(publications)]@ + + { + @(pub['simple_base_type'])_s data; + + if (@(pub['topic_simple'])_sub.update(&data)) { + + if (@(pub['topic_simple'])_data_writer.id == UXR_INVALID_ID) { + // data writer not created yet + create_data_writer(session, reliable_out_stream_id, participant_id, ORB_ID::@(pub['topic_simple']), client_namespace, "@(pub['topic_simple'])", "@(pub['dds_type'])", @(pub['topic_simple'])_data_writer); + } + + if (@(pub['topic_simple'])_data_writer.id != UXR_INVALID_ID) { + + ucdrBuffer ub; + uint32_t topic_size = ucdr_topic_size_@(pub['simple_base_type'])(); + if (uxr_prepare_output_stream(session, best_effort_stream_id, @(pub['topic_simple'])_data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) { + ucdr_serialize_@(pub['simple_base_type'])(data, ub, time_offset_us); + // TODO: fill up the MTU and then flush, which reduces the packet overhead + uxr_flash_output_streams(session); + num_payload_sent += topic_size; + + } else { + //PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID @(pub['topic_simple'])"); + } + + } else { + //PX4_ERR("Error UXR_INVALID_ID @(pub['topic_simple'])"); + } + + } + } +@[ end for]@ +} + +// Publishers for received messages +struct RcvTopicsPubs { +@[ for sub in subscriptions]@ + uORB::Publication<@(sub['simple_base_type'])_s> @(sub['topic_simple'])_pub{ORB_ID(@(sub['topic_simple']))}; +@[ end for]@ + + uint32_t num_payload_received{}; + + bool init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace); +}; + +static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, + struct ucdrBuffer *ub, uint16_t length, void *args) +{ + RcvTopicsPubs *pubs = (RcvTopicsPubs *)args; + const int64_t time_offset_us = session->time_offset / 1000; // ns -> us + pubs->num_payload_received += length; + + switch (object_id.id) { +@[ for idx, sub in enumerate(subscriptions)]@ + case @(idx)+1000: { + @(sub['simple_base_type'])_s data; + + if (ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) { + //print_message(ORB_ID(@(sub['simple_base_type'])), data); + pubs->@(sub['topic_simple'])_pub.publish(data); + } + } + break; + +@[ end for]@ + + default: + PX4_ERR("unknown object id: %i", object_id.id); + break; + } +} + +bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace) +{ +@[ for idx, sub in enumerate(subscriptions)]@ + create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_simple'])", "@(sub['dds_type'])"); +@[ end for]@ + + uxr_set_topic_callback(session, on_topic_update, this); + + return true; +} diff --git a/src/modules/microdds_client/dds_topics.yaml b/src/modules/microdds_client/dds_topics.yaml new file mode 100644 index 0000000000..3a871848bb --- /dev/null +++ b/src/modules/microdds_client/dds_topics.yaml @@ -0,0 +1,86 @@ +##### +# +# 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/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_global_position + type: px4_msgs::msg::VehicleGlobalPosition + + - topic: /fmu/out/vehicle_gps_position + type: px4_msgs::msg::SensorGps + + - 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 diff --git a/src/modules/microdds_client/generate_dds_topics.py b/src/modules/microdds_client/generate_dds_topics.py new file mode 100644 index 0000000000..8018fe22d0 --- /dev/null +++ b/src/modules/microdds_client/generate_dds_topics.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 +################################################################################ +# +# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). +# Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its contributors +# may be used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +################################################################################ + +# This script can generate the client code based on a set of topics +# to sent and set to receive. + +import sys +import os +import argparse +import re +import em +import yaml + +parser = argparse.ArgumentParser() +parser.add_argument("-m", "--topic-msg-dir", dest='msgdir', type=str, + help="Topics message, by default using relative path 'msg/'", default="msg") + +parser.add_argument("-y", "--dds-topics-file", dest='yaml_file', type=str, + help="Setup topics file path, by default using 'dds_topics.yaml'") + +parser.add_argument("-t", "--template_file", dest='template_file', type=str, + help="DDS topics template file") + +parser.add_argument("-u", "--client-outdir", dest='clientdir', type=str, + help="Client output dir, by default using relative path 'src/modules/microdds_client'", default=None) + +if len(sys.argv) <= 1: + parser.print_usage() + exit(-1) + +# Parse arguments +args = parser.parse_args() + +# Client files output path +client_out_dir = os.path.abspath(args.clientdir) +template_file = os.path.join(args.template_file) + +# Make sure output directory exists: +if not os.path.isdir(client_out_dir): + os.makedirs(client_out_dir) + +output_file = os.path.join(client_out_dir, os.path.basename(template_file).replace(".em", "")) +folder_name = os.path.dirname(output_file) + +if not os.path.exists(folder_name): + os.makedirs(folder_name) + + + +# open yaml file to load dictionary of publications and subscriptions +with open(args.yaml_file, 'r') as file: + msg_map = yaml.safe_load(file) + +merged_em_globals = {} +all_type_includes = [] + +for p in msg_map['publications']: + # eg TrajectoryWaypoint from px4_msgs::msg::TrajectoryWaypoint + simple_base_type = p['type'].split('::')[-1] + + # eg TrajectoryWaypoint -> trajectory_waypoint + base_type_name_snake_case = re.sub(r'(? trajectory_waypoint + base_type_name_snake_case = re.sub(r'(? #include -#include #include "microdds_client.h" @@ -46,27 +45,68 @@ #include #include +#if defined(CONFIG_NET) || defined(__PX4_POSIX) +# define MICRODDS_CLIENT_UDP 1 +#endif + #define STREAM_HISTORY 4 #define BUFFER_SIZE (UXR_CONFIG_SERIAL_TRANSPORT_MTU * STREAM_HISTORY) // MTU==512 by default using namespace time_literals; +void on_time(uxrSession *session, int64_t current_time, int64_t received_timestamp, int64_t transmit_timestamp, + int64_t originate_timestamp, void *args) +{ + // latest round trip time (RTT) + int64_t rtt = current_time - originate_timestamp; + + // HRT to AGENT + int64_t offset_1 = (received_timestamp - originate_timestamp) - (rtt / 2); + int64_t offset_2 = (transmit_timestamp - current_time) - (rtt / 2); + + session->time_offset = (offset_1 + offset_2) / 2; + + if (args) { + Timesync *timesync = static_cast(args); + timesync->update(current_time / 1000, transmit_timestamp, originate_timestamp); + + //fprintf(stderr, "time_offset: %ld, timesync: %ld, diff: %ld\n", session->time_offset/1000, timesync->offset(), session->time_offset/1000 + timesync->offset()); + + session->time_offset = -timesync->offset() * 1000; // us -> ns + } +} + MicroddsClient::MicroddsClient(Transport transport, const char *device, int baudrate, const char *host, - const char *port, bool localhost_only) - : _localhost_only(localhost_only) + const char *port, bool localhost_only, const char *client_namespace) : + ModuleParams(nullptr), + _localhost_only(localhost_only), + _client_namespace(client_namespace) { if (transport == Transport::Serial) { - int fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK); + int fd = -1; - if (fd < 0) { - PX4_ERR("open %s failed (%i)", device, errno); + for (int attempt = 0; attempt < 3; attempt++) { + fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (fd < 0) { + PX4_ERR("open %s failed (%i)", device, errno); + // sleep before trying again + usleep(1'000'000); + + } else { + break; + } } _transport_serial = new uxrSerialTransport(); if (fd >= 0 && setBaudrate(fd, baudrate) == 0 && _transport_serial) { - if (uxr_init_serial_transport(_transport_serial, fd, 0, 1)) { + // TODO: + uint8_t remote_addr = 0; // Identifier of the Agent in the connection + uint8_t local_addr = 1; // Identifier of the Client in the serial connection + + if (uxr_init_serial_transport(_transport_serial, fd, remote_addr, local_addr)) { _comm = &_transport_serial->comm; _fd = fd; @@ -77,7 +117,7 @@ MicroddsClient::MicroddsClient(Transport transport, const char *device, int baud } else if (transport == Transport::Udp) { -#if defined(CONFIG_NET) || defined(__PX4_POSIX) +#if defined(MICRODDS_CLIENT_UDP) _transport_udp = new uxrUDPTransport(); if (_transport_udp) { @@ -127,8 +167,6 @@ void MicroddsClient::run() return; } - int polling_topic_sub = orb_subscribe(ORB_ID(vehicle_imu)); - while (!should_exit()) { bool got_response = false; @@ -142,29 +180,48 @@ void MicroddsClient::run() } // Session + // The key identifier of the Client. All Clients connected to an Agent must have a different key. + const uint32_t key = 0xAAAABBBB; uxrSession session; - uxr_init_session(&session, _comm, 0xAAAABBBB); + uxr_init_session(&session, _comm, key); + // void uxr_create_session_retries(uxrSession* session, size_t retries); if (!uxr_create_session(&session)) { PX4_ERR("uxr_create_session failed"); return; } + // TODO: uxr_set_status_callback + // Streams // Reliable for setup, afterwards best-effort to send the data (important: need to create all 4 streams) uint8_t output_reliable_stream_buffer[BUFFER_SIZE] {}; - uxrStreamId reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream_buffer, BUFFER_SIZE, - STREAM_HISTORY); - uint8_t output_data_stream_buffer[1024] {}; - uxrStreamId data_out = uxr_create_output_best_effort_stream(&session, output_data_stream_buffer, - sizeof(output_data_stream_buffer)); + uxrStreamId reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream_buffer, + sizeof(output_reliable_stream_buffer), STREAM_HISTORY); + + uint8_t output_data_stream_buffer[2048] {}; + uxrStreamId best_effort_out = uxr_create_output_best_effort_stream(&session, output_data_stream_buffer, + sizeof(output_data_stream_buffer)); uint8_t input_reliable_stream_buffer[BUFFER_SIZE] {}; - uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer, BUFFER_SIZE, STREAM_HISTORY); - uxrStreamId input_stream = uxr_create_input_best_effort_stream(&session); + uxrStreamId reliable_in = uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer, + sizeof(input_reliable_stream_buffer), + STREAM_HISTORY); + (void)reliable_in; + + uxrStreamId best_effort_in = uxr_create_input_best_effort_stream(&session); + (void)best_effort_in; // Create entities uxrObjectId participant_id = uxr_object_id(0x01, UXR_PARTICIPANT_ID); + + uint16_t domain_id = _param_xrce_dds_dom_id.get(); + + // const char *participant_name = "px4_micro_xrce_dds"; + // uint16_t participant_req = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id, domain_id, + // participant_name, UXR_REPLACE); + + // TODO: configurable participant name with client namespace? const char *participant_xml = _localhost_only ? "" "" @@ -178,7 +235,7 @@ void MicroddsClient::run() "" "" "" - "default_xrce_participant" + "px4_micro_xrce_dds" "false" "udp_localhost" "" @@ -188,11 +245,11 @@ void MicroddsClient::run() "" "" "" - "default_xrce_participant" + "px4_micro_xrce_dds" "" "" "" ; - uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, 0, + uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, domain_id, participant_xml, UXR_REPLACE); uint8_t request_status; @@ -202,69 +259,60 @@ void MicroddsClient::run() return; } - if (!_subs->init(&session, reliable_out, participant_id)) { - PX4_ERR("subs init failed"); - return; - } - - if (!_pubs->init(&session, reliable_out, input_stream, participant_id)) { + if (!_pubs->init(&session, reliable_out, reliable_in, best_effort_in, participant_id, _client_namespace)) { PX4_ERR("pubs init failed"); return; } _connected = true; + // Set time-callback. + uxr_set_time_callback(&session, on_time, &_timesync); + + // Synchronize with the Agent + bool synchronized = false; + + while (!synchronized) { + synchronized = uxr_sync_session(&session, 1000); + + if (synchronized) { + PX4_INFO("synchronized with time offset %-5" PRId64 "us", session.time_offset / 1000); + //sleep(1); + + } else { + usleep(10000); + } + } + + hrt_abstime last_sync_session = 0; hrt_abstime last_status_update = hrt_absolute_time(); hrt_abstime last_ping = hrt_absolute_time(); int num_pings_missed = 0; bool had_ping_reply = false; uint32_t last_num_payload_sent{}; uint32_t last_num_payload_received{}; - bool error_printed = false; - hrt_abstime last_read = hrt_absolute_time(); while (!should_exit() && _connected) { - px4_pollfd_struct_t fds[1]; - fds[0].fd = polling_topic_sub; - fds[0].events = POLLIN; - // we could poll on the uart/udp fd as well (on nuttx) - int pret = px4_poll(fds, 1, 20); - if (pret < 0) { - if (!error_printed) { - PX4_ERR("poll failed (%i)", pret); - error_printed = true; - } + _subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace); - } else if (pret != 0) { - if (fds[0].revents & POLLIN) { - vehicle_imu_s data; - orb_copy(ORB_ID(vehicle_imu), polling_topic_sub, &data); + uxr_run_session_timeout(&session, 0); + + // time sync session + if (hrt_elapsed_time(&last_sync_session) > 1_s) { + if (uxr_sync_session(&session, 100)) { + //PX4_INFO("synchronized with time offset %-5" PRId64 "ns", session.time_offset); + last_sync_session = hrt_absolute_time(); } } - _subs->update(data_out); - - hrt_abstime read_start = hrt_absolute_time(); - - if (read_start - last_read > 5_ms) { - last_read = read_start; - - // Read as long as there's data or until a timeout - pollfd fd_read; - fd_read.fd = _fd; - fd_read.events = POLLIN; - - do { - uxr_run_session_timeout(&session, 0); - - if (session.on_pong_flag == 1 /* PONG_IN_SESSION_STATUS */) { // Check for a ping response - had_ping_reply = true; - } - } while (poll(&fd_read, 1, 0) > 0 && hrt_absolute_time() - read_start < 2_ms); + // Check for a ping response + /* PONG_IN_SESSION_STATUS */ + if (session.on_pong_flag == 1) { + had_ping_reply = true; } - hrt_abstime now = hrt_absolute_time(); + const hrt_abstime now = hrt_absolute_time(); if (now - last_status_update > 1_s) { float dt = (now - last_status_update) / 1e6f; @@ -287,6 +335,7 @@ void MicroddsClient::run() } uxr_ping_agent_session(&session, 0, 1); + had_ping_reply = false; } @@ -294,14 +343,14 @@ void MicroddsClient::run() PX4_INFO("No ping response, disconnecting"); _connected = false; } + + px4_usleep(1000); } uxr_delete_session_retries(&session, _connected ? 1 : 0); _last_payload_tx_rate = 0; _last_payload_tx_rate = 0; } - - orb_unsubscribe(polling_topic_sub); } int MicroddsClient::setBaudrate(int fd, unsigned baud) @@ -333,6 +382,48 @@ int MicroddsClient::setBaudrate(int fd, unsigned baud) case 921600: speed = B921600; break; +#ifndef B1000000 +#define B1000000 1000000 +#endif + + case 1000000: speed = B1000000; break; + +#ifndef B1500000 +#define B1500000 1500000 +#endif + + case 1500000: speed = B1500000; break; + +#ifndef B2000000 +#define B2000000 2000000 +#endif + + case 2000000: speed = B2000000; break; + +#ifndef B2500000 +#define B2500000 2500000 +#endif + + case 2500000: speed = B2500000; break; + +#ifndef B3000000 +#define B3000000 3000000 +#endif + + case 3000000: speed = B3000000; break; + +#ifndef B3500000 +#define B3500000 3500000 +#endif + + case 3500000: speed = B3500000; break; + +#ifndef B4000000 +#define B4000000 4000000 +#endif + + case 4000000: speed = B4000000; break; + default: PX4_ERR("ERR: unknown baudrate: %d", baud); return -EINVAL; @@ -399,12 +490,6 @@ int MicroddsClient::setBaudrate(int fd, unsigned baud) return 0; } - -int microdds_client_main(int argc, char *argv[]) -{ - return MicroddsClient::main(argc, argv); -} - int MicroddsClient::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); @@ -414,8 +499,8 @@ int MicroddsClient::task_spawn(int argc, char *argv[]) { _task_id = px4_task_spawn_cmd("microdds_client", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 4, - PX4_STACK_ADJUSTED(8000), + SCHED_PRIORITY_DEFAULT, + PX4_STACK_ADJUSTED(10000), (px4_main_t)&run_trampoline, (char *const *)argv); @@ -442,14 +527,22 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; +#if defined(MICRODDS_CLIENT_UDP) Transport transport = Transport::Udp; - const char *device = nullptr; - const char *ip = "127.0.0.1"; - int baudrate = 921600; - const char *port = "15555"; - bool localhost_only = false; - while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:l", &myoptind, &myoptarg)) != EOF) { +#else + Transport transport = Transport::Serial; +#endif + const char *device = nullptr; + int baudrate = 921600; + + const char *port = "8888"; + bool localhost_only = false; + const char *ip = "127.0.0.1"; + + const char *client_namespace = nullptr;//"px4"; + + while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:l:n:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 't': if (!strcmp(myoptarg, "serial")) { @@ -477,6 +570,8 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) break; +#if defined(MICRODDS_CLIENT_UDP) + case 'h': ip = myoptarg; break; @@ -488,6 +583,11 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) case 'l': localhost_only = true; break; +#endif // MICRODDS_CLIENT_UDP + + case 'n': + client_namespace = myoptarg; + break; case '?': error_flag = true; @@ -511,7 +611,7 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) } } - return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only); + return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only, client_namespace); } int MicroddsClient::print_usage(const char *reason) @@ -536,9 +636,15 @@ $ microdds_client start -t udp -h 127.0.0.1 -p 15555 PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "serial device", true); PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, 3000000, "Baudrate (can also be p:)", true); PRINT_MODULE_USAGE_PARAM_STRING('h', "127.0.0.1", "", "Host IP", true); - PRINT_MODULE_USAGE_PARAM_INT('p', 15555, 0, 3000000, "Remote Port", true); + PRINT_MODULE_USAGE_PARAM_INT('p', 8888, 0, 65535, "Remote Port", true); PRINT_MODULE_USAGE_PARAM_FLAG('l', "Restrict to localhost (use in combination with ROS_LOCALHOST_ONLY=1)", true); + PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Client DDS namespace", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } + +extern "C" __EXPORT int microdds_client_main(int argc, char *argv[]) +{ + return MicroddsClient::main(argc, argv); +} diff --git a/src/modules/microdds_client/microdds_client.h b/src/modules/microdds_client/microdds_client.h index 5a770c37ee..7801ddb9d7 100644 --- a/src/modules/microdds_client/microdds_client.h +++ b/src/modules/microdds_client/microdds_client.h @@ -34,13 +34,13 @@ #pragma once #include +#include -#include +#include -extern "C" __EXPORT int microdds_client_main(int argc, char *argv[]); +#include - -class MicroddsClient : public ModuleBase +class MicroddsClient : public ModuleBase, public ModuleParams { public: enum class Transport { @@ -49,7 +49,7 @@ public: }; MicroddsClient(Transport transport, const char *device, int baudrate, const char *host, const char *port, - bool localhost_only); + bool localhost_only, const char *client_namespace); ~MicroddsClient(); @@ -75,6 +75,7 @@ private: int setBaudrate(int fd, unsigned baud); const bool _localhost_only; + const char *_client_namespace; SendTopicsSubs *_subs{nullptr}; RcvTopicsPubs *_pubs{nullptr}; @@ -87,5 +88,11 @@ private: int _last_payload_tx_rate{}; ///< in B/s int _last_payload_rx_rate{}; ///< in B/s bool _connected{false}; + + Timesync _timesync{}; + + DEFINE_PARAMETERS( + (ParamInt) _param_xrce_dds_dom_id + ) }; diff --git a/src/modules/microdds_client/module.yaml b/src/modules/microdds_client/module.yaml new file mode 100644 index 0000000000..1c5bf77856 --- /dev/null +++ b/src/modules/microdds_client/module.yaml @@ -0,0 +1,56 @@ + + +# parameters to auto start +# mode (normal, minimal) +# UDP port +# max rate +# DDS DOMAIN ID +# + + +# multiple instances? + + +module_name: Micro XRCE-DDS +serial_config: + - command: | + if [ $SERIAL_DEV != ethernet ]; then + set XRCE_DDS_ARGS "-t serial -d ${SERIAL_DEV} -b p:${BAUD_PARAM}" + else + set XRCE_DDS_ARGS "-t udp" + fi + microdds_client start ${XRCE_DDS_ARGS} + + port_config_param: + name: XRCE_DDS_${i}_CFG + group: Micro XRCE-DDS + # MAVLink instances: + # 0: Telem1 Port (Telemetry Link) + # 1: Telem2 Port (Companion Link). Disabled by default to reduce RAM usage + # 2: Board-specific / no fixed function or port + #default: [TEL1, "", ""] + supports_networking: true + +parameters: + - group: Micro XRCE-DDS + definitions: + + XRCE_DDS_DOM_ID: + description: + short: XRCE DDS domain ID + long: XRCE DDS domain ID + category: System + type: int32 + reboot_required: true + default: 0 + + XRCE_DDS_UDP_PRT: + description: + short: Micro DDS UDP Port + long: | + If ethernet enabled and selected as configuration for micro DDS, + selected udp port will be set and used. + type: int32 + reboot_required: true + default: 8888 + requires_ethernet: true diff --git a/src/modules/microdds_client/utilities.hpp b/src/modules/microdds_client/utilities.hpp new file mode 100644 index 0000000000..6244eb1adf --- /dev/null +++ b/src/modules/microdds_client/utilities.hpp @@ -0,0 +1,138 @@ + +#pragma once + +#include +#include + +#include + +#define TOPIC_NAME_SIZE 128 + +uxrObjectId topic_id_from_orb(ORB_ID orb_id, uint8_t instance = 0) +{ + if (orb_id != ORB_ID::INVALID) { + uint16_t id = static_cast(orb_id) + (instance * UINT8_MAX); + uxrObjectId topic_id = uxr_object_id(id, UXR_TOPIC_ID); + return topic_id; + } + + return uxrObjectId{}; +} + +static bool generate_topic_name(char *topic, const char *client_namespace, const char *direction, const char *name) +{ + if (client_namespace != nullptr) { + int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/%s/fmu/%s/%s", client_namespace, direction, name); + return (ret > 0 && ret < TOPIC_NAME_SIZE); + } + + int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/fmu/%s/%s", direction, name); + return (ret > 0 && ret < TOPIC_NAME_SIZE); +} + +static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrObjectId participant_id, + ORB_ID orb_id, const char *client_namespace, const char *topic_name_simple, const char *type_name, + uxrObjectId &datawriter_id) +{ + // topic + char topic_name[TOPIC_NAME_SIZE]; + + if (!generate_topic_name(topic_name, client_namespace, "out", topic_name_simple)) { + PX4_ERR("topic path too long"); + return false; + } + + uxrObjectId topic_id = topic_id_from_orb(orb_id); + uint16_t topic_req = uxr_buffer_create_topic_bin(session, reliable_out_stream_id, topic_id, participant_id, topic_name, + type_name, UXR_REPLACE); + + + // publisher + uxrObjectId publisher_id = uxr_object_id(topic_id.id, UXR_PUBLISHER_ID); + uint16_t publisher_req = uxr_buffer_create_publisher_bin(session, reliable_out_stream_id, publisher_id, participant_id, + UXR_REPLACE); + + + // data writer + datawriter_id = uxr_object_id(topic_id.id, UXR_DATAWRITER_ID); + + uxrQoS_t qos = { + .durability = UXR_DURABILITY_TRANSIENT_LOCAL, + .reliability = UXR_RELIABILITY_BEST_EFFORT, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 0, + }; + + uint16_t datawriter_req = uxr_buffer_create_datawriter_bin(session, reliable_out_stream_id, datawriter_id, publisher_id, + topic_id, qos, UXR_REPLACE); + + // Send create entities message and wait its status + uint16_t requests[3] {topic_req, publisher_req, datawriter_req}; + uint8_t status[3]; + + if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) { + PX4_ERR("create entities failed: %s, topic: %i publisher: %i datawriter: %i", + topic_name, status[0], status[1], status[2]); + return false; + + } else { + PX4_INFO("successfully created %s data writer, topic id: %d", topic_name, topic_id.id); + } + + return true; +} + +static bool create_data_reader(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId input_stream_id, + uxrObjectId participant_id, uint16_t index, const char *client_namespace, const char *topic_name_simple, + const char *type_name) +{ + // topic + char topic_name[TOPIC_NAME_SIZE]; + + if (!generate_topic_name(topic_name, client_namespace, "in", topic_name_simple)) { + PX4_ERR("topic path too long"); + return false; + } + + uint16_t id = index + 1000; + + + uxrObjectId topic_id = uxr_object_id(id, UXR_TOPIC_ID); + uint16_t topic_req = uxr_buffer_create_topic_bin(session, reliable_out_stream_id, topic_id, participant_id, topic_name, + type_name, UXR_REPLACE); + + + // subscriber + uxrObjectId subscriber_id = uxr_object_id(id, UXR_SUBSCRIBER_ID); + uint16_t subscriber_req = uxr_buffer_create_subscriber_bin(session, reliable_out_stream_id, subscriber_id, + participant_id, UXR_REPLACE); + + + // data reader + uxrObjectId datareader_id = uxr_object_id(id, UXR_DATAREADER_ID); + + uxrQoS_t qos = { + .durability = UXR_DURABILITY_TRANSIENT_LOCAL, + .reliability = UXR_RELIABILITY_BEST_EFFORT, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 0, + }; + + uint16_t datareader_req = uxr_buffer_create_datareader_bin(session, reliable_out_stream_id, datareader_id, + subscriber_id, topic_id, qos, UXR_REPLACE); + + uint16_t requests[3] {topic_req, subscriber_req, datareader_req}; + uint8_t status[3]; + + if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) { + PX4_ERR("create entities failed: %s %i %i %i", topic_name, + status[0], status[1], status[2]); + return false; + } + + uxrDeliveryControl delivery_control{}; + delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED; + uxr_buffer_request_data(session, reliable_out_stream_id, datareader_id, input_stream_id, &delivery_control); + + return true; +} diff --git a/src/modules/micrortps_bridge/CMakeLists.txt b/src/modules/micrortps_bridge/CMakeLists.txt deleted file mode 100644 index 8af919eb9f..0000000000 --- a/src/modules/micrortps_bridge/CMakeLists.txt +++ /dev/null @@ -1,193 +0,0 @@ -############################################################################ -# -# Copyright (c) 2017 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -#============================================================================= -# RTPS and micro-cdr -# - -find_program(FASTRTPSGEN fastrtpsgen PATHS $ENV{FASTRTPSGEN_DIR}) -if(NOT FASTRTPSGEN) - message(FATAL_ERROR "Unable to find fastrtpsgen") -else() - execute_process( - COMMAND $ENV{FASTRTPSGEN_DIR}fastrtpsgen -version - OUTPUT_VARIABLE FASTRTPSGEN_VERSION - OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_QUIET - ) - message(STATUS "${FASTRTPSGEN_VERSION}") -endif() - -set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS - ${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml -) - -if (EXISTS "${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml") - set(config_rtps_send_topics) - execute_process( - COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_classifier.py -sa - OUTPUT_VARIABLE config_rtps_send_topics - ) - set(config_rtps_send_alias_topics "") - string(FIND ${config_rtps_send_topics} "alias" found_send_alias) - if (NOT ${found_send_alias} EQUAL "-1") - STRING(REGEX REPLACE ".*alias " "" config_rtps_send_alias_topics "${config_rtps_send_topics}") - STRING(REPLACE ", " ";" config_rtps_send_alias_topics "${config_rtps_send_alias_topics}") - STRING(REPLACE "\n" "" config_rtps_send_alias_topics "${config_rtps_send_alias_topics}") - STRING(REGEX REPLACE " alias.*" "" config_rtps_send_topics "${config_rtps_send_topics}") - endif() - STRING(REGEX REPLACE ", " ";" config_rtps_send_topics "${config_rtps_send_topics}") - STRING(REGEX REPLACE "\n" "" config_rtps_send_topics "${config_rtps_send_topics}") - - set(config_rtps_receive_topics) - execute_process( - COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_classifier.py -ra - OUTPUT_VARIABLE config_rtps_receive_topics - ) - set(config_rtps_receive_alias_topics "") - string(FIND ${config_rtps_receive_topics} "alias" found_receive_alias) - if (NOT ${found_receive_alias} EQUAL "-1") - STRING(REGEX REPLACE ".*alias " "" config_rtps_receive_alias_topics "${config_rtps_receive_topics}") - STRING(REPLACE ", " ";" config_rtps_receive_alias_topics "${config_rtps_receive_alias_topics}") - STRING(REPLACE "\n" "" config_rtps_receive_alias_topics "${config_rtps_receive_alias_topics}") - STRING(REGEX REPLACE " alias.*" "" config_rtps_receive_topics "${config_rtps_receive_topics}") - endif() - STRING(REPLACE ", " ";" config_rtps_receive_topics "${config_rtps_receive_topics}") - STRING(REPLACE "\n" "" config_rtps_receive_topics "${config_rtps_receive_topics}") -endif() - -if (FASTRTPSGEN AND (config_rtps_send_topics OR config_rtps_receive_topics)) - option(GENERATE_RTPS_BRIDGE "enable RTPS and microCDR" ON) -endif() - -if (GENERATE_RTPS_BRIDGE) - add_subdirectory(micrortps_client) - - ############################################################################### - # micro-cdr serialization - ############################################################################### - include(px4_git) - px4_add_git_submodule(TARGET git_micro_cdr PATH micro-CDR) - - set(UCDR_SUPERBUILD CACHE BOOL "Disable micro-CDR superbuild compilation.") - add_subdirectory(micro-CDR) - - set(msg_out_path_microcdr ${PX4_BINARY_DIR}/uORB_microcdr/topics) - set(msg_source_out_path_microcdr ${CMAKE_CURRENT_BINARY_DIR}/topics_microcdr_sources) - - set(uorb_headers_microcdr) - set(uorb_sources_microcdr) - - # send topic files - STRING(REGEX REPLACE ";" ", " send_list "${config_rtps_send_topics};${config_rtps_send_alias_topics}") - message(STATUS "microRTPS bridge:") - message(STATUS " Publish to the bridge from: ${send_list}") - set(send_topic_files) - foreach(topic ${config_rtps_send_topics}) - list(APPEND send_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) - list(APPEND uorb_headers_microcdr ${msg_out_path_microcdr}/${topic}.h) - list(APPEND uorb_sources_microcdr ${msg_source_out_path_microcdr}/${topic}.cpp) - endforeach() - - # receive topic files - STRING(REGEX REPLACE ";" ", " rcv_list "${config_rtps_receive_topics};${config_rtps_receive_alias_topics}") - message(STATUS " Subscribe from the bridge to: ${rcv_list}") - set(receive_topic_files) - foreach(topic ${config_rtps_receive_topics}) - list(APPEND receive_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) - list(APPEND uorb_headers_microcdr ${msg_out_path_microcdr}/${topic}.h) - list(APPEND uorb_sources_microcdr ${msg_source_out_path_microcdr}/${topic}.cpp) - endforeach() - - list(REMOVE_DUPLICATES uorb_headers_microcdr) - list(REMOVE_DUPLICATES uorb_sources_microcdr) - - # Generate uORB serialization headers - add_custom_command(OUTPUT ${uorb_headers_microcdr} - COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py - --headers - -f ${send_topic_files} ${receive_topic_files} - -i ${PX4_SOURCE_DIR}/msg/ - -o ${msg_out_path_microcdr} - -e ${PX4_SOURCE_DIR}/msg/templates/uorb_microcdr - -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/headers_microcdr - -q - DEPENDS - ${receive_topic_files} - ${send_topic_files} - ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py - ${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml - COMMENT "Generating uORB microcdr topic headers" - VERBATIM - ) - add_custom_target(uorb_headers_microcdr_gen DEPENDS ${uorb_headers_microcdr}) - - # Generate uORB serialization sources - add_custom_command(OUTPUT ${uorb_sources_microcdr} - COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py - --sources - -f ${send_topic_files} ${receive_topic_files} - -i ${PX4_SOURCE_DIR}/msg/ - -o ${msg_source_out_path_microcdr} - -e ${PX4_SOURCE_DIR}/msg/templates/uorb_microcdr - -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/sources_microcdr - -q - DEPENDS - ${receive_topic_files} - ${send_topic_files} - ${PX4_SOURCE_DIR}/msg/tools/px_generate_uorb_topic_files.py - ${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml - COMMENT "Generating uORB microcdr topic sources" - VERBATIM - ) - add_custom_target(uorb_sources_microcdr_gen DEPENDS ${uorb_sources_microcdr}) - - px4_add_library(uorb_msgs_microcdr ${uorb_sources_microcdr} ${uorb_headers_microcdr}) - add_dependencies(uorb_msgs_microcdr - uorb_headers_microcdr_gen - uorb_sources_microcdr_gen - git_micro_cdr - microcdr - ) - add_dependencies(microcdr prebuild_targets) - - # microCDR - target_include_directories(uorb_msgs_microcdr - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/micro-CDR/include - ${CMAKE_CURRENT_BINARY_DIR}/micro-CDR/include - ${CMAKE_CURRENT_BINARY_DIR}/micro-CDR/include/microcdr - ) - - target_link_libraries(uorb_msgs_microcdr PRIVATE microcdr) -endif() diff --git a/src/modules/micrortps_bridge/Kconfig b/src/modules/micrortps_bridge/Kconfig deleted file mode 100644 index 2262e81f33..0000000000 --- a/src/modules/micrortps_bridge/Kconfig +++ /dev/null @@ -1,12 +0,0 @@ -menuconfig MODULES_MICRORTPS_BRIDGE - bool "micrortps_bridge" - default n - ---help--- - Enable support for micrortps_bridge - -menuconfig USER_MICRORTPS_BRIDGE - bool "micrortps_bridge running as userspace module" - default y - depends on BOARD_PROTECTED && MODULES_MICRORTPS_BRIDGE - ---help--- - Put micrortps_bridge in userspace memory diff --git a/src/modules/micrortps_bridge/README.md b/src/modules/micrortps_bridge/README.md deleted file mode 100644 index 27750f054c..0000000000 --- a/src/modules/micrortps_bridge/README.md +++ /dev/null @@ -1 +0,0 @@ -For see a complete documentation, please follow this [link](https://docs.px4.io/main/en/middleware/micrortps.html) diff --git a/src/modules/micrortps_bridge/micro-CDR b/src/modules/micrortps_bridge/micro-CDR deleted file mode 160000 index 21d3cfe3ae..0000000000 --- a/src/modules/micrortps_bridge/micro-CDR +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 21d3cfe3ae570d1674da0105ab23b80958e0449a diff --git a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt b/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt deleted file mode 100644 index 17e2608d91..0000000000 --- a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt +++ /dev/null @@ -1,120 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -set(msg_out_path ${CMAKE_CURRENT_BINARY_DIR}) -get_filename_component(micrortps_bridge_path ${msg_out_path} PATH) - -option(BUILD_MICRORTPS_AGENT "enable building the micrortps_agent after its generation" OFF) - -if (NOT "${config_rtps_send_topics}" STREQUAL "" OR NOT "${config_rtps_receive_topics}" STREQUAL "") - set(send_topic_files) - foreach(topic ${config_rtps_send_topics}) - list(APPEND send_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) - endforeach() - - set(receive_topic_files) - foreach(topic ${config_rtps_receive_topics}) - list(APPEND receive_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) - endforeach() - - if (NOT "${config_rtps_send_alias_topics}" STREQUAL "") - set(config_rtps_send_topics "${config_rtps_send_topics};${config_rtps_send_alias_topics}") - endif() - - if (NOT "${config_rtps_receive_alias_topics}" STREQUAL "") - set(config_rtps_receive_topics "${config_rtps_receive_topics};${config_rtps_receive_alias_topics}") - endif() - - foreach(topic ${config_rtps_send_topics}) - list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/src/${topic}_Publisher.cpp) - list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/src/${topic}_Publisher.h) - endforeach() - - foreach(topic ${config_rtps_receive_topics}) - list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/src/${topic}_Subscriber.cpp) - list(APPEND topic_bridge_files_out ${micrortps_bridge_path}/micrortps_agent/src/${topic}_Subscriber.h) - endforeach() - - list(APPEND topic_bridge_files_out - ${micrortps_bridge_path}/micrortps_client/microRTPS_client.cpp - ${micrortps_bridge_path}/micrortps_client/microRTPS_transport.cpp - ${micrortps_bridge_path}/micrortps_client/microRTPS_transport.h - ${micrortps_bridge_path}/micrortps_client/dds_topics.h - ) - - add_custom_command(OUTPUT ${topic_bridge_files_out} - COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/generate_microRTPS_bridge.py - --fastrtpsgen-dir $ENV{FASTRTPSGEN_DIR} - --generate-idl - --mkdir-build - --generate-cmakelists - --topic-msg-dir ${PX4_SOURCE_DIR}/msg - --uorb-templates-dir templates/uorb_microcdr - --urtps-templates-dir templates/urtps - --agent-outdir ${micrortps_bridge_path}/micrortps_agent/src - --client-outdir ${micrortps_bridge_path}/micrortps_client - --idl-dir ${micrortps_bridge_path}/micrortps_agent/idl - >micrortps_bridge.log 2>&1 || cat micrortps_bridge.log - DEPENDS ${send_topic_files} ${receive_topic_files} ${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml - ${PX4_SOURCE_DIR}/msg/templates/uorb_microcdr/dds_topics.h.em - COMMENT "Generating RTPS topic bridge" - ) - add_custom_target(topic_bridge_files DEPENDS ${topic_bridge_files_out}) - - px4_add_module( - MODULE modules__micrortps_bridge__micrortps_client - MAIN micrortps_client - STACK_MAIN 4096 - INCLUDES - ${CMAKE_CURRENT_SOURCE_DIR} - ${micrortps_bridge_path}/micrortps_client - SRCS - microRTPS_client_main.cpp - ${micrortps_bridge_path}/micrortps_client/microRTPS_client.cpp - ${micrortps_bridge_path}/micrortps_client/microRTPS_transport.cpp - MODULE_CONFIG - module.yaml - DEPENDS - topic_bridge_files - ) - target_link_libraries(modules__micrortps_bridge__micrortps_client PRIVATE uorb_msgs_microcdr) - - if (BUILD_MICRORTPS_AGENT) - add_custom_command(TARGET modules__micrortps_bridge__micrortps_client POST_BUILD - COMMAND ${PX4_SOURCE_DIR}/Tools/build_micrortps_agent.sh - WORKING_DIRECTORY ${PX4_SOURCE_DIR} - COMMENT "Building micrortps_agent..." - ) - # add_subdirectory(${micrortps_bridge_path}/micrortps_agent) - endif() -endif() diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h deleted file mode 100644 index 029bb2331d..0000000000 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h +++ /dev/null @@ -1,99 +0,0 @@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#define LOOPS -1 -#define SLEEP_US 1000 -#define MAX_SLEEP_US 1000000 -#define BAUDRATE 460800 -#define MAX_DATA_RATE 10000000 -#define DEVICE "/dev/ttyACM0" -#define POLL_MS 1 -#define MAX_POLL_MS 1000 -#define DEFAULT_IP "127.0.0.1" -#define DEFAULT_RECV_PORT 2019 -#define DEFAULT_SEND_PORT 2020 -#define MIN_TX_INTERVAL_US 1000.f -#define MAX_TX_INTERVAL_US 1000000.f - - -void *send(void *args); -void micrortps_start_topics(const uint32_t &datarate, struct timespec &begin, uint64_t &total_rcvd, - uint64_t &total_sent, uint64_t &sent_last_sec, - uint64_t &rcvd_last_sec, uint64_t &received, uint64_t &sent, int &rcvd_loop, int &sent_loop); - -struct baudtype { - speed_t code; - uint32_t val; -}; - -struct options { - enum class eTransports { - UART, - UDP - }; - eTransports transport = options::eTransports::UART; - char device[64] = DEVICE; - char ip[16] = DEFAULT_IP; - uint16_t recv_port = DEFAULT_RECV_PORT; - uint16_t send_port = DEFAULT_SEND_PORT; - uint32_t sleep_us = SLEEP_US; - uint32_t baudrate = BAUDRATE; - uint32_t datarate = 0; - uint32_t poll_ms = POLL_MS; - int loops = LOOPS; - bool sw_flow_control = false; - bool hw_flow_control = false; - bool verbose_debug = false; -}; - -extern struct options _options; -extern bool _should_exit_task; -extern Transport_node *transport_node; diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp deleted file mode 100644 index 1ff41c4682..0000000000 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp +++ /dev/null @@ -1,330 +0,0 @@ -/**************************************************************************** - * - * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its contributors - * may be used to endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -extern "C" __EXPORT int micrortps_client_main(int argc, char *argv[]); - -static int _rtps_task = -1; -bool _should_exit_task = false; -Transport_node *transport_node = nullptr; -struct options _options; - -struct timespec begin; -struct timespec end; - -uint64_t total_rcvd{0}; -uint64_t total_sent{0}; -uint64_t rcvd_last_sec{0}; -uint64_t sent_last_sec{0}; -uint64_t received{0}; -uint64_t sent{0}; -int rcv_loop{0}; -int send_loop{0}; - -static void usage(const char *name) -{ - PRINT_MODULE_USAGE_NAME("micrortps_client", "communication"); - PRINT_MODULE_USAGE_COMMAND("start"); - - PRINT_MODULE_USAGE_PARAM_STRING('t', "UART", "UART|UDP", "Transport protocol", true); - PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyACM0", "", "Select Serial Device", true); - PRINT_MODULE_USAGE_PARAM_INT('b', 460800, 9600, 3000000, "Baudrate (can also be p:)", true); - PRINT_MODULE_USAGE_PARAM_INT('m', 0, 0, MAX_DATA_RATE, "Maximum sending data rate in B/s (0=not limited)", true); - PRINT_MODULE_USAGE_PARAM_INT('p', 1, 1, MAX_POLL_MS, "Poll timeout for UART in milliseconds", true); - PRINT_MODULE_USAGE_PARAM_INT('l', -1, -1, INT32_MAX, "Limit number of iterations until the program exits (-1=infinite)", - true); - PRINT_MODULE_USAGE_PARAM_INT('w', 1000, 0, MAX_SLEEP_US, - "Iteration time for data publishing to the uORB side, in microseconds", true); - PRINT_MODULE_USAGE_PARAM_INT('r', 2019, 0, 65536, "Select UDP Network Port for receiving (local)", true); - PRINT_MODULE_USAGE_PARAM_INT('s', 2020, 0, 65536, "Select UDP Network Port for sending (remote)", true); - PRINT_MODULE_USAGE_PARAM_STRING('i', "127.0.0.1", "", "Select IP address (remote)", true); - PRINT_MODULE_USAGE_PARAM_FLAG('f', "Activate UART link SW flow control", true); - PRINT_MODULE_USAGE_PARAM_FLAG('h', "Activate UART link HW flow control", true); - PRINT_MODULE_USAGE_PARAM_FLAG('v', "Add more verbosity", true); - - PRINT_MODULE_USAGE_COMMAND("stop"); - PRINT_MODULE_USAGE_COMMAND("status"); -} - -static int parse_options(int argc, char *argv[]) -{ - int ch; - int myoptind = 1; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "t:d:l:w:b:m:p:r:s:i:fhv", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 't': _options.transport = strcmp(myoptarg, "UDP") == 0 ? - options::eTransports::UDP - : options::eTransports::UART; break; - - case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break; - - case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break; - - case 'w': _options.sleep_us = strtoul(myoptarg, nullptr, 10); break; - - case 'b': { - int baudrate = 0; - - if (px4_get_parameter_value(myoptarg, baudrate) != 0) { - PX4_ERR("baudrate parsing failed"); - } - - _options.baudrate = baudrate; - - break; - } - - case 'm': { - int datarate = 0; - - if (px4_get_parameter_value(myoptarg, datarate) != 0) { - PX4_ERR("datarate parsing failed"); - } - - _options.datarate = datarate; - - break; - } - - case 'p': _options.poll_ms = strtoul(myoptarg, nullptr, 10); break; - - case 'r': _options.recv_port = strtoul(myoptarg, nullptr, 10); break; - - case 's': _options.send_port = strtoul(myoptarg, nullptr, 10); break; - - case 'i': if (nullptr != myoptarg) strcpy(_options.ip, myoptarg); break; - - case 'f': _options.sw_flow_control = true; break; - - case 'h': _options.hw_flow_control = true; break; - - case 'v': _options.verbose_debug = true; break; - - default: - usage(argv[1]); - return -1; - } - } - - if (_options.datarate > MAX_DATA_RATE) { - _options.datarate = MAX_DATA_RATE; - PX4_WARN("Data rate too high. Using max datarate of %d B/s instead", MAX_DATA_RATE); - } - - if (_options.poll_ms < POLL_MS) { - _options.poll_ms = POLL_MS; - PX4_WARN("Poll timeout too low. Using %d ms instead", POLL_MS); - - } else if (_options.poll_ms > MAX_POLL_MS) { - _options.poll_ms = MAX_POLL_MS; - PX4_WARN("Poll timeout too high. Using %d ms instead", MAX_POLL_MS); - } - - if (_options.sleep_us > MAX_SLEEP_US) { - _options.sleep_us = MAX_SLEEP_US; - PX4_WARN("Publishing iteration cycle too slow. Using %d us instead", MAX_SLEEP_US); - } - - if (_options.hw_flow_control && _options.sw_flow_control) { - PX4_ERR("HW and SW flow control set. Please set only one or another"); - return -1; - } - - return 0; -} - -static int micrortps_start(int argc, char *argv[]) -{ - if (0 > parse_options(argc, argv)) { - PX4_INFO("EXITING..."); - _rtps_task = -1; - return -1; - } - - // Set the system ID to FMU, in order to identify the client side - const uint8_t sys_id = static_cast(MicroRtps::System::FMU); - - switch (_options.transport) { - case options::eTransports::UART: { - transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms, - _options.sw_flow_control, _options.hw_flow_control, sys_id, - _options.verbose_debug); - PX4_INFO("UART transport: device: %s; baudrate: %" PRIu32 "; poll: %" PRIu32 "ms; flow_control: %s", - _options.device, _options.baudrate, _options.poll_ms, - _options.sw_flow_control ? "SW enabled" : (_options.hw_flow_control ? "HW enabled" : "No")); - } - break; - - case options::eTransports::UDP: { - transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port, - sys_id, _options.verbose_debug); - PX4_INFO("UDP transport: ip address: %s; recv port: %" PRIu16 "; send port: %" PRIu16, - _options.ip, _options.recv_port, _options.send_port); - - } - break; - - default: - _rtps_task = -1; - PX4_INFO("EXITING..."); - return -1; - } - - if (0 > transport_node->init()) { - PX4_INFO("EXITING..."); - _rtps_task = -1; - return -1; - } - - micrortps_start_topics(_options.datarate, begin, total_rcvd, total_sent, sent_last_sec, rcvd_last_sec, received, sent, - rcv_loop, - send_loop); - - px4_clock_gettime(CLOCK_REALTIME, &end); - - const double elapsed_secs = static_cast(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9); - - PX4_INFO("RECEIVED: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - avg %.02fKB/s", - received, rcv_loop, total_rcvd, elapsed_secs, static_cast(total_rcvd / (1e3 * elapsed_secs))); - PX4_INFO("SENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - avg %.02fKB/s", - sent, send_loop, total_sent, elapsed_secs, total_sent / (1e3 * elapsed_secs)); - - delete transport_node; - - transport_node = nullptr; - - PX4_INFO("Stopped!"); - - fflush(stdout); - - _rtps_task = -1; - - return 0; -} - -int micrortps_client_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(argv[0]); - return -1; - } - - if (!strcmp(argv[1], "start")) { - if (_rtps_task != -1) { - PX4_INFO("Already running"); - return -1; - } - - _rtps_task = px4_task_spawn_cmd("micrortps_client", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - PX4_STACK_ADJUSTED(2900), - (px4_main_t) micrortps_start, - (char *const *)argv); - - if (_rtps_task < 0) { - PX4_WARN("Could not start task"); - _rtps_task = -1; - return -1; - } - - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (_rtps_task == -1) { - PX4_INFO("Not running"); - - } else { - px4_clock_gettime(CLOCK_REALTIME, &end); - - const double elapsed_secs = static_cast(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9); - - printf("\tup and running for %.03f seconds\n", elapsed_secs); - printf("\tnr. of messages received: %" PRIu64 "\n", received); - printf("\tnr. of messages sent: %" PRIu64 "\n", sent); - printf("\ttotal data read: %" PRIu64 " bytes\n", total_rcvd); - printf("\ttotal data sent: %" PRIu64 " bytes\n", total_sent); - printf("\trates:\n"); - printf("\t rx: %.3f kB/s\n", rcvd_last_sec / 1e3); - printf("\t tx: %.3f kB/s\n", sent_last_sec / 1e3); - printf("\t avg rx: %.3f kB/s\n", total_rcvd / (1e3 * elapsed_secs)); - printf("\t avg tx: %.3f kB/s\n", total_sent / (1e3 * elapsed_secs)); - printf("\t tx rate max:"); - - if (_options.datarate != 0) { - printf(" %.1f kB/s\n", _options.datarate / 1e3); - - } else { - printf(" Unlimited\n"); - } - } - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - if (_rtps_task == -1) { - PX4_INFO("Not running"); - return -1; - } - - _should_exit_task = true; - - if (nullptr != transport_node) { transport_node->close(); } - - _rtps_task = -1; - return 0; - } - - usage(argv[0]); - return -1; -} diff --git a/src/modules/micrortps_bridge/micrortps_client/module.yaml b/src/modules/micrortps_bridge/micrortps_client/module.yaml deleted file mode 100644 index 10049eac32..0000000000 --- a/src/modules/micrortps_bridge/micrortps_client/module.yaml +++ /dev/null @@ -1,38 +0,0 @@ -module_name: RTPS -serial_config: - - command: | - protocol_splitter start ${SERIAL_DEV} - mavlink start -d /dev/mavlink -b p:${BAUD_PARAM} -m onboard -r 5000 -x - micrortps_client start -d /dev/rtps -b p:${BAUD_PARAM} -m p:RTPS_RATE -l -1 - port_config_param: - name: RTPS_MAV_CONFIG - group: RTPS - label: MAVLink + FastRTPS - - command: | - micrortps_client start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} -m p:RTPS_RATE -l -1 - port_config_param: - name: RTPS_CONFIG - group: RTPS - label: FastRTPS - -parameters: - - group: RTPS - definitions: - RTPS_RATE: - description: - short: Maximum RTPS data rate - long: | - Configure the maximum sending rate for the RTPS streams in Bytes/sec. - If the configured streams exceed the maximum rate, the sending rate of - each stream is automatically decreased. - - 0 is unlimited. Note this can cause reliability issues - if enough RTPS topics are selected that exceed the - serial bus limit. - - type: int32 - min: 0 - unit: B/s - reboot_required: true - default: 0 - diff --git a/src/modules/micrortps_bridge/res/basic_example_flow.png b/src/modules/micrortps_bridge/res/basic_example_flow.png deleted file mode 100644 index a48972b7addbc2894d75590df54080d53277ea68..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 182298 zcmd43byU>d8$CLdlmbIa>i~jucL)q1-61V0ATfXp-7&OugMbK#l(ck%w9?((-Q4xz zec$iz{&m;7f84e1@juX+6VG{`{p`Jup(;u;&#=g_Kp@aF*|$(N5D0At1VRqMcme`} zz^4RRAQ0$@iM$LH^Z@+%(wH9y0=)pqLd7*)(|70KIz*P&qK7wx?s4B0Ap%ZgO?r)X31TgL{SXDtiUhIE-IpBL$x)b3=_4HOh>oQhn9Fz7^Z( zKfq1IY|o}3Oh%c_+JTT9q`O&W5;XE(_6uDwpo-(Yf&QQ22#$Bu{=fZTbhP}P^8fiY zEf?=jDB^#^tjj_K{J#;?Kk7*G@4r=)cXj1QMnUO>si@$6Q_f;Fl^PxG?CxHP7Q25V zCx^w!#g(nXU~FsqjD&=w(q;lDL-3P-US8g;IX+mGru*NIRVDbGnE1rf(z0=He0<#X zaIuwu2l8-t;IlFZUl^Ma^*I0dalYXybo(p4_EZ+kEio~%tfC?yEsfm$bknaj0Nu1d znS-5&XKaR#mzUUfrn)%XQR<-K1V^h_7wwD1LH4@}i^IdiHwW{L93~wYPf0nyN^cDG z^t2=yd4~?9@vY1^dT8z!X%#K)?1UqsJTV?EP~BW;78|v$)<7T%TsWSDJnlsSrpN6i zhxhGKH+^co?M$_9N$|F$p&?alT-?#kLGwycX;a%**|_2FnoOP#cNdOH7G(_$!nAaB zOZ_QaI`u9r!8nvy_V)Jaf}ehWxqAQp{o(m}R|vJYj%_X#h@0#gtHH0pDw_$D!^PIg zpXPmko6sN-h{(rrg2%n*fr*QY^JEf`l7L>4qJz%J$lj^VtQKma?r6W?+LHoSi$q zDW*LYT==e8GPg+7ZfH#_+G&WJC$Uaw--z!z9z)v;LI_4!_S8lOExM1=B< zZ0=bGe4b-dN4J=`Shq?-P7VuX+@H)b+3eGNDaf_6rg}d#+1`#)US6&_7>Yo&bc7Pb zq^C36Il*ASyqTGqt)1_EeL+wt6tRL$!urK>D3eF9E3P4AanZOxg$o@G4Q;uaO2mEf zV!>x(2L3z#^<KL7c}MRyF1%;M^*nTi+uV7~L$r=8(?xV=Ie@4y!UC=~hx|9OBU z6dH+B0Z$`d-%m6?X9bVNhlXOlQqE+KvcV5>I^9$)S1na5WdF21s#_YpEz|6MS7tdx z)mt4OPe>u=t^X~|+V1*v%f0B~;c^8xT?nr9lSNF78XboMuUjq;UkJX>U8{-<+MB8A zzCPQTdfdBK;BNBB_0QW*r>gC_BO*Eyn6#g!YM(EkFSZ7Z{50>= zEsfkBsu&Ut7Y}$MTb&Ty&EZnoaE#T^&_Fx$+sFYC5dk$c%;evnsbLdz+0|h?1B1b& z{uhD-VAb50+UKlJ@Evp~_?Z(N$>IQix5WnhOSF!M+cms*;<(7Pvxz-7kS%)WYGvjK^H0nlJkU-hes3i!uxD*M%%&J znXtP^tC;grhb&Us0R{taM3HhyJMK!@QJtKedK;)j+%6n^KACXc7^HmP z>W?Pj;851;geZ8=_`qe=;)~?}0;4UAh^fPtMX#E$Qr9-2d+Miocx`i4Rn_OTG^TXH zPv51p($+3;v9LHVz4V*C4Uz>veK)qTVG@wOVq;@td%yK3ukG&Uf=O6OMdh>E0|C7f zhW`r;OiawS-Kh#Ra&q!U(-Na*!vp@#&d#>rXXJq{d()b_;dOO&?C;x9m%1X!pEM8A zAJJwB<&Bpbv84z(Zw`}X8I=qTsugLMCiYW`de*LjSu zcNAsM;9!a4LmHpG$<^^1(fo);3Ic)Qyf;mxSN&03Nl7V)C!UC!I@)gT7qDfB(9puu zS~N5?3`|VF=xAK6QbSt@k+au)eA=8P%)G@fZRQ*5%qXa+Slt*{z~GqK$shEP9OQJG#`T7+tIpJ@!C=)-$v|O`l#?8o=joa?__Abiz#H^$67;Yb zOn+@SIa?2BtT=-kk+t*_rPpeSZKXu4tbzTTpT{m@|KfbZ)wzxxS2 zn4)+iOHh64^?W-3SM-gA#VclJ=EHf9gZKNhbq-EW1V+)};RM%j}yDUvTNEW~*BEaBtM zLUXgim@uh0p2Ozw%iii^k|q`w77n{v{@v*+`PuWc38R(WsS4Mf;u>~~{^$IHg5&ay zP$-n>eMKe$L6$+vWwA*8aET5wzB%91sdJ=@|NYzk)l8!Y-|@)_5@>vU{5hW;1*>7h zhr_G3_IB{|=L}H+{{HM<*Uka!(lRoponb^6*w{tKF5YZxY`}3O^Vu5)72O8}1aO#k zJ%xELcVIrH;KvIM4ZU2AQ$B3R72BK}EY)BvH*Q64G_~R;^SoM1(y6de%&R#~OG`6d z?TOJTHwl^-3yk!usHia0cS$glRJ|APw6d~tzlk@(yu3VG5iAZxkdh_`d#Zlb7DlV?&&}x;h|F&_3zz>_nrHiHa}(o{EiwGhSaN;(BP@ zcqe*ys!k#7s&ee1t)W3^F_al8;I#flK8bbnZ$56O+dAp#{v5fesOZ`8MLGh3Vf+*a z4Caw|_R?eGxf9&4E3R>d@V~27US8fhS#H`1lTzj17|e*9JyJJ<=iDtk+-1$zewX%g zW6`Z-cU|zg+c6^I-R@)ZI07-e%P=tb*x&qcXMcHp9k$|evA#F!vVepFvjGSC*N6cs(tJ3^s>X8Lhmdj+bw#-^q;^G8~| zaxzM>kzc;N zpx}1^sj8}K^6sy!tZ;nV)}RpcrZDop$`ki7LLd;<{v7;J=qQ6?GP}4Z$lB`MPHJ1o zqb6r|?(=87kJFWfZPFr#mizl_eMwCH$)#_t_V>$8yURXKamwaWF}zFBIj3GpFh2lb z!@+~B)%ku~5O$|+-c*XWL(dfyzH+)ucztu5V*Tm?Lh&pxWepx$JEwr;Y)jjKez zPHm;08&R;OQm6r3T7K3W(yNmpF`7qwYg*H!MVuuBSK?&7iAbIvZ}fl%oZqZOi_xxx zaved&eTgi~LCg?H&UD8=D6%WH(I%p<{!RPf zyzTG0dQb&Cw7@0djyWPM4Bxc#xAbDeVH>tP?r{mz>({SiVFuC%&!-EN6crO9L_|bv zoHLK%atUIFwvH|?UHfzObVdt44+P ze0(64r+srI*Sx5hb!B^75NqhUo15EGPYliGNG^J8Y%B;AfMf$dPOwHLo}Jk^ATAa`O3X zr@0F>7(sMm6G^NFj7_{&e`u3aQpWUHaBy&9VON@ajtmbKmAfc2&tei1=tyT~X2d=z zDz|rbqCH#<`OwY~vl%iQBJ#x&9u$=1H&p z1bid?()KoEU2guzfCf<-2j5s}$9E$mBWLdSVetwr1?>lLSZC7<;Kh-z+f=XW3#QAtCiZQ$H>5dK}t&c)9ioWDDMRC z^BzbSAb7a!7Ay04@CIP?v86$3y^Vsfu<#2E|9NeI4Tz0(SnntOp;v=5B=+E$F{@^j z+IVv^BxbrZUhK}HU^D)s@XQHcwOwOGa$=^+CdAj**S&aU$}%e5feY&wU3>xZ8{4!y4I!M4oX3x8k7KEQ!v6b z<1krnie7EMU`IGr-#=GukXKi4P7Tpdt(U(x^g7dI)~{9hSCE8wT8-s%6X=er)3LSO*=YTTH|J|R+7|dwTmJIoxkw0c#?s?_d%WGPXvYQ(%8XKWqY8ctj zAe{2O&qsH)iw0y+`D_-DH{gfnB3)+^aDQ zHz*YPcs2hTKmGS=4(=MP?~?^j)k8<|o<3&>**Bgg^0P8CchtgG%8Xl)XI!~1{9_sFVBYC6ynSW_1n^uWFz~*5sm>L>VwY9Y! z-kuDZF0}>4is`YjvGr#N)riW|LvmjE4!A=g*7yp71#VM9a1aD$r@P$6nPuN*T*bJYsVylz~2(Y6Bv zQ6tEBu&Ai0o{PA1e^W^D_rAN%)-2M(z`;rEH~MK7N;}`396=#~e}8|!%r787s#pCn zrab4D)7iErvth$?BqXG=@^ZhlH1gZq+ea45W&($omlycnC@K-gl#Sip2oaC-mW~eS zx8I?Nf^ri8M5Lmc_WaZK99bOjp8o?PMxB#WQh#L;vVRn%kk)0YbgsCX8h%brj>Fa;y4iYoX`2+0*Ufonp?dys zo_x|T)+7WX^~K}W?PU+%H8Aa;LnE_1w=*4@LrZRyhbo54<+WX(?ZCGF&_z&-Q4tdp zYwjPctOO7-X>EFSjeX;_oqoZ|iElGeA~jQO%QQFC-~V)_JGvWyu=5@VWA8+&%x^D` zVlA8{B#@gP?mbAEzEfrherg*?Dl55V&ANS&P#lg|dvyg|{^fp;cBd-J-A*h5&Yxjn zeRmAHTDU*w>kV^HkPB+QKP$bwS$K%jD%RcF%SucP7V$V=>|<;0K!9l&^&?3rD40Xf z1Ypa7hPz6dmAWj*$jA=oyYFCW_4Psktr+o~L*i!`2OS;SLE}Z{YNB_Lgr8&x&e%4~ z-P(hvh_EmoIk{f0X<$&$`_Tf`<;k%zA`T7?c0RsR7rQraKoAI|<0uH5WVpg|h%BR- zcV1FcDN{I9Jm5*FDK#~_Ck=&5^oBqDOcDCkQ|FfNmow+{;YimJF+9ijH z-VkecEG#UC>r<ovqUh=A zEA8gU{QUgNDl30nxEHtw1qIc9+99@^ZwQT#Cp;qMk@B1rDkk9(Q-E1vbR8-VN+FDDC*lzAu&`gbec+E-lbB-J( zYsP{3XBZf79344*#VX))b8{obdVD(X-n|qRrFv|n{@^~hKew~mx7(6*kavPFxUM`) zVJafeo2mzW(JC5gaC71p5D?E(d&$K`0E5AZ=Kl=s8bGWO{R0C>vra?32Eovc!HhE# zSWj(AN=oa*1W8jRVzGvxL>~~@5g(k1^Pdi%kfBq3DXt^u^LcF)-v4c!$oYDD*HT;Jof7Snk9s=P+1|Si; zN6kxT1ma`x# zI(nam>R%YZut>#z{)|^A4}n0eRsNfFjx-Ge1BPIRlKsuaf)AS{4>0v$^*#d-z~8+9>A z6Q$G^1F1nrE8Xq0vkg>CbP!1F!c>JNY-gMWChU3X01y!rJ`Z;nkDn7WVgj%YF)`D> zJ~2&KS_SYecoJ9+rWgKH#RZ|Eq49g)@_$oIdjf~Ufx136nEjgro`fh^dfoi}{$xl@ zCW+Oc<7hBLNL52aW5FB<1k%9AIG~Bk%8Cvppf@?+oq8)T|JTDHTcu@wo+>jl(*Xwa zlgs-v0~GVp=6k_c%9;AqbU?V4l$GJtyX@h2dwY8y9v!ttkaBg{MpKDMeE3j6u?Bp) zX82=7N`EaY!((f@geYYSzn-!)HxHVsums2>K#}V2@9&s!7Ni97$ndaAU|=A?f-l?1 z%E~Gm#Qoo-nWNEeS_tI3EjkWG3&1(TwoWv^DP@FphLcF*p@Qg&4*I-HL_$wboo}yq zO5Z9dK;OTAIXgQ$T<2tYd2=H+2J2&DW(Eo#n1m!G1eaPOPc7T+Wc~2uL~^3Uz~)Tf z&CLxUK=DXOLLwrb@(T*){6qUF2VtWCA$R_GOU$72UKa+9(s!%9apmr3*0sNWY4OVx zY5;u1=4>4ozzG1!`)Ikd*8R-7cyzQ-onE>H4;(YXMDCZX!3Z?jAm93Wp(jtDzwmCN z5_ZMIr51&rtPj|DMtNN9zaPnw!obE3kj@?PnCtFFmrrDFjeo7)aa3h90Rr7!pGms9 z){1Y`x$Kc8vgie8d0%6?xw#ST|NXAX#H?G1Vc&4{L_txpNW=o*I9LM+=#`8YTl{)@ zdRn@veIjgUs$U8T>DgyiRB$D-8ieu9yP{wP<+Pa!2?)Ft5izj8=?EoYDz-daY~^>^ z)v5YK00#5WMYO(zKsfPlE)Kf8BFX7y0QUCwqWPh?Qq?FXhDK(2xij4L?rehTPKE~z zRy93dY(=3E_0*xjCF8~b82oPYv{ACiCqgc}1xEsHZEcGyD*;|N=bDoR=)OhTr6${> zg~?(*O|`MjuTaHX(5XE^Vq#*u^Nsa?q(wlquU?sq0;|=RB_I|j7SAH02TRfqoqkBJHX=P%TnUX@<(AfCi-afjr zl3TygUGq;mE*6%=#r_=A?ZbG9fe^RN_)|AGx3@}4#5MJ`wJpDYe{Vafud5@~X99y& z)gKkE81Us$H#L}mfF{b;*7j&j+eqXLc5}9KG1^_ga+Xlv*KYvH`Ii3?H7Y15$naHT zaWRW@G!+YP8=Fy61e112%fv*@wCTp`>f)HTQ3Ch&&BZ}695G zMNx@heKAY8>rdeVQgpzf61($_db8Ee zTWb9&Tr#CdKKHk7pq6aO5O&W?2iQ&UGjf2&M0vo)1B1b)4hRGyq9ynlIqgacmt|XL zr_?v=uWjw^*(wYjrHhM;WCP7Edt|Q^Q&G{;(S( zw!YqUAeG0RiI@qWf`Yj)pF4n`h6V+jm>J+;f^n&%CX9S2fg9Q2c7lh82h!Ho{y1A( zKCOdHPfyQ4h$UNr#Lzp(;xYfzNHX|o;m8941oB-ZiOneS2RAj3HQ~pPA5YJjnx~sS z1~fBf`P>WO0PB&E>rA1VZ@y-G0etUT|{DC_`x-b5so7%^?GmL0?a&FEb zqLOK$03F1}#s-uIN7dz?7#e=p!!P0+C~`Rg`UVED^?p*IUvYotNZ7z6HN<9>+sD1U#W*mN#>B)w$;kU1MgG^X zZ>i+;Vh#@-zJC3RSm}+UCy$ppSX?wd47!MqC)}N@*QSW36e4bPKjZvRQe0fTDon4O z3HA1F5_j@A+YZ{u@@bmB`vpH>y-E*{h~VD5ghHVjPLDOccXxNHA{I83DPsQp>$ygJ zeZAG+S7v5ri!}@0?Ii~Fc=-7KJw0z{YhftB$}U(2=89QMzK!Ncg@2BZN0pJ0`8ZvP zxwW+g(9u!UVo|_fZuX%jpjV3TfA`DzwWE_0lkoi2$p&m=kn-c)FU_A`?ZMA#oi>z$ zjDn?KFUY{`XNtiw2vHC+B zFoXme61hmf^n$_4lc|j|VrXQ@JaO#p?T>CoM>X2RX&{gsvPaG8t%a)vJ}VbI6p_j*N#h`Qw)GdCQqUQ%8y$z@jh2>H-i=Y6qZH@kx{%`uCnp{;@vEqr8vAI=EU)TmH*@l9G&HoyDjV&*n{WB~;R{6? zjE#uO@5w1Cty5EV(|5ncit?N`1`|Yw8*)mOG6ZGUlO7s#IC-wTa(CyjIy*a`3;~Nw z{dbYkR3fnvd%L@4-&ePKWByIBTg?x5Y++$x9Y>W`f1jpm$1$Y;EZQ$FF1FEm-)pJ& z>8WiA`S+j6HOCr^n#Gw(2m~V2@a$ss5tJ41I>N~f z3cB1mV6>oJG4i<^TdePT>h;+zOVq2(ypI^>dAg~3d3{aBJu%5Zh*c;50s^rferyY3 z-(`t<(XK4`JQ#eF%cOFGhaw6}D1}@UJnu0;fc?m6y}$4X_M^DC(?Ue$r*5-ng?EcD zk_rRi^7osA<_9Wl5?1bl*NvZcv;wxDbDAN{Zs*_&OM5j7cAq2d0pmmME{bm`0#V@p z@AfNaiK-Nh4S(PU`BqkPL#3p$Z6-=MwY8l3=jb3gzqCY=Eq}+qR(4L0p^<6p>Uts| zAOP|O1ee)bu$GqAvM3-i4QETjR(rspmOS~SL*NU?w2g|7C$qD&`CWFC#XnVA{e99N z{H%ScJp>7)U83JQVdN9(cCs$gI}@vU_2Jj*s#yx-O9;d|`B6}(y832DK}Z-FA5WO_-OX{0#tY!X)ajHQPUwISDTYSoaN4Hy zeTy$r&){HiRu;9O^QN&1rPuXos|>XlZB%$z7`w}^4p0$mVJp&kCx8C_E%&-{0hlUE zLFX1+v0LNetL?D7JUaLD-Pch*_g9w8`n7oZ%30E$p2BKsY8|CA(Nw)UufgCL*gwr0 z7|`Q@W^Hm(2lPfui+Ft?E#&UzLTn611Az>e8a3+{472}p?izYv{=DF)qyU!qnwN*f z@D4LGIT@iSoVJbI7KGQO=W_*t12phDnVOxUv$HKyBepAM?ym1 zBqNRog#5>;pJ;#;D2dg;*xo)mCkJ9?VX-`3tcQq>_F29#)Fx)u@g?Q5K&(6?=QVB* ze&&Ajj7oTNvET&+lJm`<2{24X_{!zAZI2bv&EQb*`(Dg@9E3(`>+45coowvP{W6-6 zbArn!tnc$effaDFa&T~9;Np^W17-vs>rr(|^@^9o(3)=cg?<<|>K^#RJUxYRsDu%eLM|_8X;HhQ zD2vKe?d|Q2)&#cOPq+R8wuHJq;2wYeCwA~xx+2?+9Q1=-G2%Zg5PMUbj9;c~Dw3pU?w->D@gCSD%*u^n|&`z#jM z%mWrfV@pePY;0`j&0(Zm>Bu%*v0GoU`%9F}%uJAPf4>5(DH|qY-+aewVq$W5K4afL zHdZx#C-iAM5D5C+sS2VJ|syNbj8`hy(xd?lhm zS!HG5o&tJxU~usAT9T2pgG1T8cWY~F?97gGI=`PE3cA)i<-)DM`KCALdmy>{EK%>< z+TV8`7yB$%4|T?^$O@Ulq{X^b{)L5wwR8Rb{R~=M_~4kMx5~;PadC0{hP4ha$BXr} zS61|!y(v{yRo}|XOIldG>K_;w`SFem*v$F)`496AZnlYvP$={m{FsEyp#1{_u){_1 z{$vgmvHQ#1S`j`zlAZD5_FtcNzNj!5+uP&H$jAtL-#*8s77f%YDn4}Go31J<{X>^r zFAs$(&H)U!sgKVCyKyaFj_7B}Il=F4Z_CVj@ntRSJv|%5Bi&4O3PruHozum9 z=0BLAArJ*ilfOO{ncent4fSYX0(u+aKXjd%)5f6G$B)e8rAE>7jUL)JZ-nIK<)4!C zJ{7w?L@_ZnHMbc}>I4$3byyUIfWOzxd8nzFm{>^^Jp__t%>i(C1%5rhf46mplYj{b z0xB!H2}wxW$H$4IqM|k@%V}=!?y|MWFBJc&8Nr+TbM-pS-Xfy6hwav4MS#y0li<}0 zCMNmW^8vnv#i|Lzq8(142)!rxj|_u+Z;K@70|=7j)Krtv0#(Fy^Dj53$C=Cq_>>q|-D5Ud)_ol1j>dgRdPv_B1`rn}C>RJT) z!GA|@XVs^?aq5oSgWwva+tHo8U^_;#oR; zRRT+E>oALf)NI8x-oulVcBdhc5X&qtVkIRdb`B0diR{nyP;386U6!!0u+PcKi1rZN zmC-_VK)R2Ojs2XKh8-zQ0}q9ccD&QoZ5tfK$;`|=ytwEd&Xyc0(q_}q(`)bOfbJh7 z5IrC)aXi$z@E4GrU-THrR3h$yHWMXFY4%O6*^(ijQ&L{T-$9{IU`|f$ljWuy=D!J@ z_h(6Yd3ha^po)Rgx#I5bg1~;-H{TJ{E2SHoW+KQ?6Xb%bZD&k=$G?6q5*ivBA}p+JeITvev>S(zh$ytQP=itD-MiL5f7JBLe`^*&Y3Yl# z!k3p#R@c`de0(J4J_fe)4FUk8`C9#3OJ5(h9GyC4;XYs$>QCW%Osjot&0oHJdE=>A ztkvGt^<;g0U8QL3w*D>j=id+942+CzE8WpCSy^m$+6;t1kOTg!a?|ddPdnqcDN<0g zbRcxCcY#e99#;ABuHwna$jI=PvkC*DgR^sBQWD8@jeT^r?M#kTI5D8F$>qh?n?bCX zfn!KZ3%|KIpn3J`34jLnW@_l*C}#%?)ZxS|ZJ%FC8e3a4npTbK0DKHkU>n@H_UG#9 z4Y{qyz7qjbV709t;Kx-`QYx#gT-~4t_m+J50-BnddaIz&F;@=DEI^fFENmR{P^)`ji3^K?sa|Y zm!D5>A3_JorNuyYbaZsMyZ!`)LKlHXAHGj98HI$%#l^*SDlDGiBs;jH3b>71F4+tptdGIzfGXsI#FZLq=`YA&Q zo>(UbfiOy{cpdlBXZ0AMfx%;}kiwS z#LHn92!y81LIkWz<9u^&`$NADzxm-_$XSrGp|SDkz~_M+1s%t%nhVVHAu%!0#L!ho z=Q%goGb&+M-<%xC>GoLoGjw-GAUpvWLrYJO0?PEcce`Doet0vKDe~Uh`nR+QnCC54 z5OQ^OH9%dvxbWuWR{vnVrl>hl_Q}kufnb zK_F^sY5=#~-{1FtqtAJw{Es$3?S1+32`<&sAPGNze`LT$Mz5S1UoQ-STvG%6I9W=3 zJRvGNdTwp)a}Yb=FO?F}?#EL#eQ#poU+1&|ya~ZWMZ%>LZYekI25>9fYV^%qgBt-* zAyLJBtEzaqqA0`Pw+A0d@PT<4U0hs%7bm;ByDu=1V`F3SC@7v5YcavC{#EVBh>Eqs z!%5hdkJtJ@AbWdzpa*bpc8;&(gyc+Hnwtkng%h{7wIPE5Jr*S{BJPuId|oCPUC zfW_1GYAxyNg$u+wes*r|=zPW=g`rM}g^>}9gv~I%ju8SueoQ|aVPRo-WMo+4cwaOa z!FYJU1_8PAI{{+?R)!F~tHlom351D>i3kgmu(K;^2vQ9=xjZrj_UKKo6ZraUCnSne z=(j!*SamLi%MvG*$L0yB*5^TVFkOJgNeT)@1(dzf0zyK<#nY|7AdrWL2N1x$y|U}y z5s2-mfB=+Ck9`3KLM#k_UncF67c4A_`#%f(>YTUmIXO8&Aix^+`K9D@N*QQmYFr!v za8RJ0{{D_gaz4ON;iL(L0!}tnn%LA-M$zX(0|U?YW@;=e{19Y3PEJm)Zf>nzT~9!G z1Ox#;elVvL>Cjx`V8pQ*iAOWv@fY(BYOOh!}5D4V)=wm!C z_Cs+)9333qgcGxvl^_rWcCQoUCTC{a$Hs`>^h#Er93PvNgps9APU>LzgMuXdh*@+G zOSmD{_?})~od*>^e}2x)WHsga-(JMuq0Io5$4U-5nw*lB);>6h1Ns~n7m$+!Nny$f zenmqg={X-B7e{a)N0aj^d$%pq-yhj<_Z5T_`LUuw%$;71S@VoqS4E8BfyZzYM==#drrZ~Xs4cU544BmIkyx+L&WG5kTr zTG$|!LJbfoQW~XL3p+>xiGYBBA>mJzJyuZ8ybX&y9%}DJ-Wg+pT)~(%10mL?v*6<~ zAtlJsc?>|IP}R99x06?j(51&ijpqoL&ax54~4-v0LQ`S&v=hd!W0w~zGxMx>gZr( zD4~Ncw~I;__x7FzN%)1}Qn$6YqpWmA0uHilt)gtLqNVNaFs)+UF>4h4{J$@SgvgJM zj(U1~{p9i_JUxY7b|?KREQchDwSYbXc*?`c$=Tty;B`S679I}h$zN9Okl5JR0Amm_ zF|os^Pf`c-jZ0fwug8ttWstyN@YrMW1V(|Z>+K~6goXx8OeukfKt7g)jj_wQX# zH)HNm(Ny&HnR)jU7^P$tqLf;yY$iJ9s6;&xJw0#vg@h#WP?OWra+R}0lSMpuuP+X8 ztFE^mwu=95eg6E}cX81e$VL|j3n8JQn1BBMjfsn+*@r*edmVY?$svi;zu@D$7^?Y- zTB*xYV$>Y9Gf|qfo38+CcrZiz%n&IKmX4oNxew&g{i5jTKECg@ySWK z?F=s}8rss)O1JyX6n`>gLlyi2WT9nU>o%QJ@Fsj!XiB9p!NH}*U!93b7%?FJyOHyR zh>ipq2+hEd5EKTh`TAeKAnfdTfP4tN{Uo~RuLD*k^nX?bBycDc3K)s3tgL`kdVhBl zla*z(bZ=dS-*jhSY-j6V8QA?+c`%?ZfvnN+16oxfez`XF()1R(!+}}IJ;4lQd&2D2 z`%-i_y?^N}g3InMGYlOT{obxHSc~aP?Hj;=Yup=42R!TyCuRZQooUbKmu1z}yqh9W z8JU%x(&nzUz9e9G<&#)nva#V}(HUE02Y=H+w=Kt^^bl;%mFJ?kyXW0CU>Ua5u;I41 z4d@||Evw`e&leqZIH{D>m0;ap81#2yVygUL`ivZ%i?)09g{UBbVDXEt+xU`YTujW? zvz>{@7DQLK?VpD~gcbWh-{|r`lSrhTsw((=cPgODW@7n#Z>DExDCx%+xI;O#J2Q%% zL-VDWc0feD@YgO8>A_ry2={h!w<)O}SvIJh7Ff5kT>4~u{KMZ)stk9&kiryvJIcMq zUAxG2D&Dv9t`WXT(_b>OP@>Qm{FIo!?0`tpu|lQnw7`H5pMU^`fPerxnM!Gym3v0d$5f!0Fl>eTG-U~MLrbgdCs$`cZ8xGm6VrnPo7>(t(qeF(okJbM zCKFH)*UP}j(6-i>1STRvtI~ZYBO{~V$Py42N1#1L00xh7K1vb5?Ntqpj+XiG zfgvX+r($y8p2glxYMh?TS>haN!*gch)oJ?ySZ+*%l{3DxX90?1J5DSgj zR&iv!o2u=>#PK7A!pKgL7DNBvmhUOAKT-~SQDOX?m>h05RM$2&hgGj7on7z-`wQ)j zsGuMjlhzO4loV1x;gpx}!`cz^xfccu0!BV}L_tA8%Pv94ANJ;+wp+M~tN#dyOCu6I zg8@h33sP{eVfvWSpRLC95(8QI*Q8{&@1%zPiYacM8d8F6@6M}r#t83Rt{-~?#&(6l z(;ApT$d{icLveup8<(1VR!fA*ZlnO z$m@N{xy9+hCvsKza&%vKRohy}@GZ2g3l!y+^-;xrZR%r#`BnZbYNg4YtP#nf{jSbe z9MELvVVjb%`Yy3CKRA5m>^}{~%mR@RJ}8EGk(XvGl6b6|le`oa#0H)%ZRSP;Z5k>n z>ZbW*cUPCm-gFf|Y&r1KSa0E;wWp6&TFV_Nc<4xCXK>(tBEtSjf-Yv>&##OgNn&Xa zSV72Z9$T+07<+fBqb$l`>~Y+u;aJ=bd&U9f_U@ATD9(aNL2hKwEH6YL`MaLpUf?CN z+`bw>m+JnXddP@_gY#j&p^jfkzQ1!4&A`~oMs15CNCHV}AoK3KOrDm+S+kFMQ9RZq zt`%kwR+$rbh;(jy)3b?~#LwX!9Z=81R&;)cMNoQ1M(35An;X!VobS!h^KQ?;om%2? z>zCX+P?A%NhUbUMD@}yW>xg(y6$i!+eFhaNumzE{X{Z+4vY{Z~pdd71Vc|zwO#}UM zOYR>85E&L1M=(1#*QvR4G?KzkPNscO)T(Js=l;|-2xl&1xOD;Zj=s{bs_`{wad)a> zveGJUHM_)eC{s#BXYI4@w z!`Ubl6cpQ4INRRO|M&j_JW|U4aWU2!fZTL{>-M3_XT3ybH(cu8R58y973tG-zjhkK z(#!%;lOHQP2h&yl<@MPPn1BEU^j2QJ{r>I-OiB3+R94B;`k9n+aq`d4!JkX=)W7tm zW{92N{F$Sey{A!}b@``f-W0?31#~sC|14^w1sz>o%j*MaAW)v1pO&FZ8N9qBH@<;! zgNW!uOoEp0ZFlFVKeaXGe3BO%bHh22&%S^F3BY22>}aXt=6o;U1%?VE(INR}*!dhq zr^4Md1OwSw7pd-@RL=mj|F_!XZ}s1FLfV9%=~k8p2NaJ$&VC!l-*BR|ZT?jSF&ct_ZaEA<>yh8=H3Xiwy_A zE*I$OV+{_fA{!s*H?}Su?<;1@<5kTu#igGVOkFx{;x`jz_5~pg4i0{*p3Xub3SO`L z{?`>~t>NkE33TZNW7Z$SoDqU45$O>U$$FaizVATnHJswuxHgjlDdOt1i|5oOE#jTZG9zzeKqk}`N>IGImJO1O4|LI~=_h-&x{p3b6 z^ZkB;nQS%m&+6Uc<_m+ll&gMyYNLgT4Uq%oo#*$y;;L?e%V8L;!~K1uWTHWA#=~w7 z@lh!4lZoG|s}qY#eql8mSw95gNg#q=9h695yzD4Nq#%O|^$8>}Uasf=`jvVzB$kzR zf13Xk2gkADQ-tM#^((>j)rQD~=S?X)Qnr3m$wBde(u<47e#kr08$tyKJCegCM zZzo}xQn!|;_YGfPrTrQUI)z8(vJ6P#ia?Ews6lm7FSoTwVl$|P_wG<8tJk4Jr~@-M zRts?d+hz7M8>~XZ6lHJO@rbcgoMN640$IbmUe)=jKb@OzkbQBz4+DPaR>|FaK?3n4 zn2|Aeg773TUIyYx-0B`(dD6zo;)nM73J@N>e`Rav^&|4;}#&LaJC3aya&Fk0Q8+RZ{4B>O>Bg-A$xF zUXhtt(uU=IBc23CIR@#sbIx>x9^x(!t^Q@zOBV?DEC_8 zoi`)-2}9waMJIaF;h*TJ!^`Hqm8$NUy3=d-aecKYG5!afim|?vBXmz%Q?2P0w7hqO z@xdF7V!k@uojb~v6{hm^-wFjb!sJBD3d7otF+`hFwOjU2n6!-4lr+^v?S;n%ca+n2 z^NsYGSsZ|~mx71@TZk|Mr%-=MNL>iA1~yYcO%B#emsDmkKnD5|Kk#=!=nba@KE_}( z**xZ+jW*HYRkLXH&b=9rWFx`trCxpA2HX5{&!G568}GYar*O$jNz#M6CuVD?80G_P zPixe8?M2`b2j1_=^&af81*Bezo}8V1yk2&c`(AN`9^v+oChK_8l^qm8(BVkscKkRxPx^$VR<`YR|g)rKjC_~4;SZ@KOWwqk=U1*$|~aA zx9k$^%e@|Te&AN2Z??#`RphiAsXjm*TJS+ct-p7~^8fhDTp-EwXQs+(zvw=_$*X$r z@c}M4FF|f^jJG{&)C2I4hCD#!PePAY)tREky$Q%vJk z)z;n+<>g3Rt@FV7Gea!0a<{>T=`>edbhS!d`t2)YTNF!%@S*m>&$}rcrPHpmJ@w}3 z=%*bV>RUr2yd7O?v|R(FHGkbXza9t*|E_>jFZwml%i7O8?;B!TN^HIXk$dMKMFhC} z(g>knO$ErjrTYjvK0dC!J??wW!XmQb1x(AtUF{tkaC6G0BwR3;d_?+B1qGwYgJaGq ztgL7Q@eahYhLW_<40WiMcKetx%-Q;7mW&g#oJtD5{`kL``s%2t`tNH2MN&{Cq*0{1 z8zrTsOS*-jI|dbyF6o>H5s>boOS%Olhptgz7er#?EA=biuHY|sje7EjiypKa+QuTtETv->TW>{lec9{dB3FH zZvkrW%{TE;6diDy^(dh!L0d1sKY*sZ`7*^v!844|)BIjQztx1+WCkpoU9VA zanI~7^gLd@a$*gFo+qgrahY25^>^tH#++u5;*EDv-l=nq+8-!4XWV>t+fL7(1}zo8M^hkuZV?8yBv&!^e3p5w=`8H&rAgzDTqksx5|-5ffgj~}KB;pSM78I) z6s;tQM?H!2b>3!=KWp*-6z+j4oBWXy`{C_%8lBlcLD@RYP2h1zt{?7oW5k|gCM!8% zI!Rv$TEqXbm_m-Bv@lifuUnqk>yl0R9euyed~bK$A#rL(;n67Uhr|OkcbG+ zg2&%SGA;ZZ_)^b@D~j{0utSUYf{)?&yxn=u!#fSokM--m|- z>WkhC^mc3e9k%d?rKV=hjN}*h3bRnOTKMLU7(6MGIlEKd%@wL-6F-i4gc+S=K1FVz13-vE1y*1Kg7m5IXPkD;VG`ENqWn+ed}5*$z=~KA(e}o zDM=Vr?iEBq62Z`95_2kcIJR%^~3M#Tg0Q;#`g z2alcylw2dnz->v^{1FIbDlB!%Ip@k15wTjMI75q&w4lS5$a#y>7;9(2AdB3Wy`SyCh<$TU`CE}{sIhr|~)Y`+$Z^b7Vvu^=M-X0=r zG&I~yQ|~&_j9r&k9z1A7QAPWIsh0j56iyF@xQRT6ea;U;iZW^ot|HIP)!x^&{9ZD- z>4~6w(LMAkw<%wG-~ui|h0wBurTAS{T6E>RflqT%)6;n_`2i&Izo)|K#6USH=nD** zN3rVX=jR2_^E>HfxJ}v`Ol*TmPO7k^An(@uKM4}yR0@~s5fnZBY(<(l7~OAugYLSM zZ1Y3zCf;2pUJWVB$T2_ZfuFX7D%qPl8sRbZWf9)T4nECU`&L~18RpiNx4LRRmO5UG z^Fr!2^x~w}G~MeqZI+PwvolR#LR`Lqen@e_7TK9^obGI9)4dmJYe+&_sWDzC9V@@< z*YoWz51HHJ#8bDs3%6hM^SrlAoYqBs=X5{F| z-DI{yZ~889*d^8$mV${Q?(MGQS`9wcU}0gY0p)O3HGj&@rF~VlA3?f_*+!wSmfJ$oz~>q*rl%{r+6eLy@AVV&-m$mPYG%Zwu6bkgjFi zf~sqr=7b1Ua9XcKa%xJ}%$tzWs7S@;mT( z!TA3kSBgw4sH^@Og?8gsr4~?5wI>L^cEgELFc#JbGXv>?Afz|5^hy3(^OH&;d+?iy!#a~ zaS8dHGw}-yzpo2o8=ly}^G3hJ#+$EaRMxeh?fmj0VNEA z;^L(TD0FFcHN336YLnCAYUBs82>`OaSpEUpSGgmOFlYdxao&GBS(%EK)*B~qwC>ya zc(pkE{QP#UEgh@-=TB1JjK+0lob zY5O9U6gkpaXX64QHTI>m9=FSUF)>n+)d%{YaTPz5)Rt?%=4Jl)?V zeJwc{a9&$Ti|Hp-o2Q(rsfrpIkg7ZVq?^AM;+X0O_4WM9s>ETHoXHpci1pc@#6rYy z%yoHHYm8g)7LA_1{xCX;R1WW0jrs3g^2lyaJJS?%&SnA9NuJ*-6Gd!G*Qt!f&9&sL zb7iBww|DBr#RbYkK@N|}{s|E-0J;DC{=FWwhNj10Fw87mfw00uuM^cDsk88(p@iobdP@XdQPpNJXdK46@r*#lj_^k*-bjVbkL1AmH0X}EAT$2Dzmg zpH-#`Y!2pupE&D@)~?HDm0&4VH`CO)75V!=WNxr~zX-d!8Jz!|)ccU1s zdsC$IWSWqvuNtrDX+%Usg;`rVYrg-ds;a7nHB92C6VpDe=EH6 zd{x65=c==QN-X5%efGCl*laqEN_@|L;w$m-mpIv`#3z>Pu|rO0-OcaEY{AL>hnjZ?7J5~7RE^6U1P_A zg97s&xx>N+s#a8=Y38_C;LA5)*@Cx2c?Zev3V2Ll@mwDT@L3_laiv=bXXRI!sMkzb z2H$CU^dc$@uk^}Dne4tPZc%Hf!WNn( zr_>(mf++_mlcZRlVD^Iby9f23opZ?LpOF3NRuGI)sn&E?NUS$Z^e{>kRFA>;^G*p2 z3~WX~VGCWMvY{6ap-Q^CWHJz~)eHclSv>g{8@fY89|UFFuY+Yo8xoZY8yXw=MMQQ# z50(%l=Y3c}=b~OPPZCAu@%!6dY|73q6ql1i5G{hk?ewcQ08MMZx(M={2x_ejKs7E@ zf7FSosNhmnRi!u=1Bj-Evxb%yiQlrv!?n9x)H5oodo1NJ=b=sP!X!?8b`2#FaGdU; za}x8QHy8JJ3wa)kQ_)&IJKiCG=XkDLIbl!8N)JUF1fS~!uV!heP%dJcg;W)@1#c{TEEQVSOmBnB%%f2vBZqQ-NCFTh^P$s&FP9AXq^bN~R zOj<}tAt*?)*2qP(pixwanZD#_y;ht&9 zC${r0Z*6ePqY2e&MYRCUyFlw|`1<#H_3{mkjAWnCj@CsDZW*OV%uz!om3@7orVo%S zk%H+h<{6fJZx~748T?qZ)MuEp)OnW79-BCyoSb|wEzR=m)EQSjW~MDv2YQ;6HI7{v*s+tYKb*4lqn?PUL-&BYP^!WAv6JrP5VZFKLo` zwy$A4wiVKJhCeZL^-hJ|N*IG(iyNa#zN=gIh4a@F=$;JJ1VhWb2SwC^avbslKHCZm zH+5k^6EbL*0IetgU-j2a5glseip|fbmyncHVM*~K@CU&L;HQs?a2IQ=aNzktJUx~_ ziK92pdIQ}W+fcQ79Ew~NFR0_EXJ&x)8;)*ntn`T`5yvu+#JW0xs*VejnAq4_P>?E> zn=u6%g@i9UK=O2mWmlRV_NL6u%_pwl5G`F@vX8GAD`UX!?nxQufzdCN6G(}0#rG@f zo43EHMJYT#G$U4J|1!)Bn15kSZhn-gyy&^?uV0^wxGoP(7pY+_ZyBd6y1DTJv#)~* zbY76q;)0Of{kJ+ga`N)>OLSCEV(^&y&<@_-sgVre%phb(#_cWmp6BtYM`2t}+%KfqdjIFsLm-(4o&{a04@zGo0fCQ-U6|1w{M?t|~E z?hzf&1{Gc@;sz+`UTkR#Zg9 zvzJ)ZeZ^;9+t@gI4XH&qn3Izu?zxSpoJe2v?OXU{=4fNB*PyQg+a=DHB0{{@13aH1hKDUM7sQupc`W^%K@Btv zeQx)#_$HXe<|08E93}j9KaeGBTP1S-I2GtX|9DE(jq{^M`?ffre90 zlYPfZc(7mJAG`=TArX-;5QwZ@upAx}MaEoqPtWVaPPFSITCscT7vJwPYs1$BIEHct zmA4Vs_fbX&TD#|n*dyC z0#_~c-pU|j3e<`p1C6NbcD8es@vl>$6Qs}@p>hwnqHFh7_s-ONdwUxi8pf7B^=}Y6 z?C7*Bsz{ChWGS+xwF9UBHTmA<~d6J{56xH5|Bx(f*m3UZJl>E&xa z`a#(8r#Od=o7-n{d@<6B%C(IezR=&_4>UafGGATD^;2?ka@WhM)aUexrvo%@N7Z-N z)lx^@IKXPOt~D(in-}`}`nnX^!bD-Uwfv80#kwyqFB`VJ6Wm}TS65dR7F{xxhFwjY zUFGGm1qDp%Gw*7KpH#QEOJ!$g<3-3hZIQ|0)n2;5c4rMOc1mP1rD#9od(Rf7wA%3X zTvn}6SCS)MgZ;J73!_wbmUQ+IX^D~$DphUeaOZf> zY=OZ2#Ds*C6(l$#I=U48Hw61%r2XwW(b_^okHKy*5#ZW2Hd=}WzH@2)8s-Pxg}mMFTb0sFqQ{_DuZYpZoplTm>7bXn3(Aa&g3^ozjU>LzK3h; z9~J@vg5|Ox)@3MHth?A4tLzoPeElysB3HVXX+B8GW~8NgV9w{gW;8^BxBS>}U&_q0 zS@oKfK3Hx^nVQl@X@1ZzH_&hz-U|N{5)#tT)HGT8ytA}4Iy*a?-H(v1=neCe1}IcO zOsts=t97Ot{Aar8KK4gN<8A{yroJ(q(x*VQOgTRNK#EHf_fn{oHcq@9{(im0m-o(< zZs-0C?|avR77IVmM^~w2`4{32ayZ-)Bj!OaBU=xBQbTJVo$9B1BqYlsF3iD2Vj00Q z-(tjyZTh4T?!L&?aYEkq?#6lZwr&STAo{Z2{H4vgSe3%XPSdlqil(Npib>Sxdd;6* zcB2#kLAwEWp_@g!rStmjJFptazN`iAS1$W+#b07>8&)s*jIoeXyVAD{3si|R*KGT9 zm0O#!yuRo*$s4dN6r^irHoiNav)BLh5M>?W%vG4!OmKrce`YWeEttH}Iy?Y5w$GB7c%buQ8T}OW`=Y1yTx&!}%qtHlUq727hqIE3uhol*O*rvjg;xs>)<>Kh*=#B$M1}GG2Kcbf!iY8-S#ss2wf~tcI z3@FMCt64ZX2}kRS!iucygP+41hDz!iSWdo&LR6nYZOv^Hx z<(`aX+DK78-rmz>=v=n8I(=hXK;L1$@OM92`%k=CZC5ecgZ)a69|vEiN$k|xTNh^i zq~MkqS@D%@J1aF^Y2feaGOE1cVaX`9mK(`DZYEGHOzu_9>$ReL8BiA#!jWc;I$ZJg z_0=&mlScCYCF!^S7~w9PV3Z%2o{r8AK9R?qGHi_6xL>Ea(=xY&_>-?%^j5WHc27`$ zVRDCR!dLURnTd(2zCL-uTSwJ5xvE8Cz6T4bBi;_Q!?s^2ErOGUo#)(7$3=DDzFo)N zn5b2sVWxd_TALUGtfx9!ut|KJILOY!vwJei3r{z7-H73=dLzj@S{J2M*p9h?EqhH$ zyDZkxg}1x@Z!bW9WwBpAoYv;*WG6KvL(90`kiGx+Z#Lybdb{A@W#T^_uRBn!K#$cI zVIo)XGc`oq?~S*wZ)}q>wM+Hs86z&gkstmZjg0Dbg@#@s=Qi_5FU2WcL zi1O1nwqCid3|`WP1uDzp->%B^ElNN*_1QV&X-|&GdbIFxt&}#{!O*oCnBiecd>y$x@PY-^xOpZnw4BHBT>4#y7G2*b|y5> z;8cZ$g+NuFnmXfu$Tmr>(bKFZv%$tWy%mOB4R2PU-7PhZ;>ph40~4nIwkJB8BQm(Umdc;uBnVpTd) z{4Qc>2I~`**vtcp?5EjmEEoxuhx{f7Y!9QY0~3(SkAHM0{=ogRz+77rSCeu7&dG<* zlKCoJTQlv^B-KLo+Y~bV)fyS<`03^4F;v{a7+(ds{0@a*JnG&Pgn7@Uvy zI1MV?DM!X#o(ajwXa)sIzIgFM*StF7ztpdFb<>rassEYdF|As3baW6sdekZlFTeM4 z<}792aiFoEGcbzvVC!J2uq5t}Jk|CVYsS5gDH{PqDNH&}X_qY7Hhp5nHvgfI3x|Fv z-)H!kGJDQ0#Ls1I^i3dPPhML-Q-ZwUT?9MxD?A*UaVnNDiD8c?-oL3Z%*&O^`~eGx z#mcOg-?V!I(<1pb)N%#LpIbb5OYH|U;!+9Fk>mf>yBQ&%CT(qP)qifC zh3}}cXo<2@Wk|Q zq@|^4r31Z3=x{XOBIAPoZB@t`7*OKizZk3f+VKYif!d6zHAnRHx zVAl{pnqg3oR}c*H-fH3cI&g1)e}>|lk5PJ>$gm|9t_)<7_^^XJ^yKu^xW&_MDU-I? zNkB^K;G!IL6?}N*fV(%-D3DDEnN*gS$FZ}sD|v_W8VGAgH#aS`q$CYP%S>lU_<dIluFa4I=8jX21Q-1rXOwJzJ(X5Wqe6XOQU$-yB&EnH)qh= z)>h&IzrVWL-CA9p$O;MmLe6PW6L4z(Jo(L%b=AaaZo2Slh+?5O)MfEU2WoZn0s(u* zzyJ{;LYWH|J#B1kOifMw_5e9?euTyV-M6(n%o=>5)<$<}-e3Os(=SvehV;7-xcj!oolAZQ9?8~gqy0qnTIXB$ouuZz)gOS0?uZ zqka!xhCt{G4F|=)4K5yVmT23xNuQF(ET=eZwik*c*P<2V{z_?^A&h^43cqp|w|2u^ z0~9(vHz(q}PZLR|X{fIcOj&Voa4h=@Bq%(8&5@>BP9g3|0 zP&x5Qyb`=Ywo*8fgkiVqcE8K<{d>TRco>Pws`}`ZeTMogt{eg4OnN%Smtk8IPI_Lq zaz@Ccva#{A#l=M@kY_t6@csMufJ!&RLfMW!cTGu4+xd*tbf*6+Sf-|@*9DyJI?WSh zR*`hhNH|R9OMCnJYF?dgm_^=Wd=mYwk>o^18TejC!CB7rRIE1d3CFF1a!b5PU}1UY9)QcAEZbOX3%7rAuBr5ye0v=YYy#()mr2g{U|m$P$n_99VQe{3M?EaJWgMkW?3GaGvO^Yin% z=H~e#xNw}ip&>lw1X`vn^@(N%UQ{YC&`~ep7Xq9}Vlz$%zR~w3)pH7;dBqqjrm> zanLsHaT?Z>OC>Ls;Rh=|r`S=JjSPFJyBw$N{L6sFee%k*aIrk83I>GWCG z1~6Cv4Z~Ab&OYF82|Gn>t-~Ze3k%C``|oU>m^aKnri;{!>uiTp8EIK?&USb0m6Vld zus5&VKkY9szcZ+Mr($JQ0GsQlu|}Pr1DhjsN`K{E?8n(TIz}logrqjD1`_&LGu(ND z%ydehK72%L>F@79B3D;`9^3@Vp_B9lrrT50S!P&7?91Mu49bHbaN@qa5U~(=N62~J z`s&qU5D~I7P3K^#F?MxzwZRK@xQ1R3hv&+BcpT?e)dT)H@_40|yIb@Oz2uuzNrXuL zOb6`?5u3LbuRfRtH>MPbz9uSKp5yl_`tU(O%!2&Hsql+HF^|i>zQTw7c-8decn{o^ z76L_ut7e_F6Le?b^L)H+TqVB6Dy<_?1Hvv)XD`KyWKq-YMsH-maX%%cOOMZA{GiV9 z*Sa)%8m7-=$XNR(#>aQQ#`1hEDChyb*diY2i2XA+H+K>ldKr1J zP#fkbw0Ge<=|Us862MmE=;X9vx^_*$&CLz?9&T=K8dM%Ka=ZI+9C43&noex*>`eXo zrQqRF8}zP(IAJcEj8(J73qH}HLg>J-`rjJO4-#VH<8B-hJ`s_0U!IvpHEglFn^Sz; zFx|5AqOaz7OcaSA*xC8{@wgi}Wp{VCVa>^%QYTO45x7c}W6>p*Y`|||>zKWtyevE~ zFr3?$q0}j%`on0PwL!U5FyR^fhHgx`{xfNbi4I@orG{{O&sx}6((2UW;;@dpEHFL9De;h>8HE97d&Ww1;W z^&0%N5|q4k zz*&z;urpqO#;+m$?uf>`OI~SfKZfTA9j5LJ%}WXZ&N&N#tZe`;(yU$LR=1yws^<4Bd6UB4;DU{v1u=GaNVGMWmPbP zjX}VgPK$U$qN_s?&dct-z<#+mmVbr#f4(uxXid1qE3)J3=-)CU_e3BIVOmGVz2$q$T)j*f1ynRvP^iT3N9JfM+< z`P6d7R#b3}jEt~xb8D0r`yC2I4Q|m&ct>pwCYD58n(T|O2ybw`*>6vn;TC9fSrka; zHcb~vW@3sYmE+1+c;3TV-TuP@Giz~tyc&<%}CNSN3Ud(TJpt+jQDI6;t- zD_*Ja0M;}~T%Wf#o+0{f@}1BueqrS2&&%NRtk`7<@&mqS2N`K;#~UP}nSaeszKeE; z54~C6XfZ!IJ3oI+gv-vsk^RI(GUci0vO5j}fjC&GwOJ)k$eOo??r*m96ugbv+k4;i zKyw$VtgH+m=4b`$r3fpBC6u>KRuy@IPK?*2bjI@Xa)TrV^E#W3W^fP1+I-%?pg3D_Y=;|(b*iWU78ukifW?5y_NxA&6fI{TrZ zCX&wjl`O7ZgLm)Lb#-UrW={qa8A|F;J4p7jHfm3oTU#Z7hM;T5xq*o5@^9R8O~PkI zPpdmFsQp$0ShZv^LN8vtaN6?gyaCHH+^j!bY!49B*Vp%YGys3!Dv4uRXAvSVkJHl9 z;uJe0u}1RfQJuqNzEjvu)|iOBn?J*&$d>g_gvAYi9?Rj?9yQE)`}%qg()j@fD?go< zOhyQQ*=3mtIYKzHn(X}z*`FHXG0pj2;S#EEjWwSq58GMIc zgRl1h2JV>a>p+>4k!+eukoj2(alhov2t034hppp)~yUEDkj?AX?;Ufi>`zA)s!2vZbtB zTJxS_DZ%5BD7+ihsc&^xpu#Wp0w&m?waz?b2r}s87AUt|jQ`hRiW%Gjl6G`7z8{l* zrnKY5Rd8xrnzpfV7Bs#-(@BWBsHG)fDdzg>{)H+#0RaIpDQ|3S?8MX|IMCTC<9oRL z$6!j*3TXVcwq~2UwnTJY<#btX4a9H5yXK#F^AQ0-v6E(aIT(7Q3_c2D_&GJTaVtX| ze{yp=2@w1z@k%SFUI~?rEL23Ew^wkl6`%Q**fk0L%ZW>^68$(oxgU!bjHLf{cIHn{ zPqWWY8Nc|irR6FW6u0}Lp0#5QsbwnKjh1qXd=XAc$Vs1g3UD8shmV>QL42y0-?ilu zPBfys$&(@>F4OWi)OFu=jJZUJ9jEUQmIW%7HucR|{`VB~MxMjBTOyA%H0jk(=e7-T zo%QH*;&fD{$Y`J-FJuR5dV0FUrqo;fyN(%#oM=l-Z0t|_V8*d5!|=1U}W+$ zghf9v%lST+Mdz~F3&zs`Anz6x?ZnML{D3ll=zu*41)ZFnIKF@Xoc2+P#v9uNUBLB7 zLrc4Thv~Y5cg_ouS{0heu@^jZo|PYA{6^Hic=j6#b35peqJnoxH3Aswl*1pS{ZEW!XWx-%7KhXqwtR} zKNTr+xdpZQ*LmjX?A$A)(J<%gOzSrKm!I^JY1LTw6Ew|->{7jd|Nim@E<2h0 zLfL;O&F~HLlSAeKxejwhMMWT~zGd59Ra~!ZcXM-tUmkBA+GuurNs!@YRX=!TZ2au} z{QQ1cPlK%d>M;RRxDgewu-DAYtb3b#o02M`LZ^kQ zYQe|XdA3F+Y=*IvbLA-;!Br<=yS8s+PmMYMGkVbSlg&0!c!ZSw-j`u!;2(xwmxQXS zs0@FNH^gHKztmv*KVY4O`Q12ou+lbnl=jSZ$BFJ_mMwPiGLXqi_sNNg;_u&wh?6?tPT9qh z;+Dn~(F&iM*1$P!$j-l}i|FWR_W6OUIxfOqzBkNIq)x{K0R`D*wLRp-zh~2bNC6w<=eH5r3*fICVfB|hWlubZC424+Uk?b zLsz06LFqI7s^ADB^(kKFYOVBnkeC+vQR<%6o08?hYczTh3Zj`e^ILhIU;tdWamIgh zIpXee%eDe4YHEaJWMobq>kedi_Z=M_|ARtBKr|)qrK#gRFORIAMBg`Y1`;IH<(as6 zN^FZbR%_Ff)lzH})T|anGb2>XMK~j*Ql!J2qOy4`7On83(p{7&OpjydqTDZVG$?4E z^$GsKz`*6O7Wjw7-MPi-c8XSO$aTk{57ZCaw}ej9(2z1w_Ero80vV<69InAiWJ&RA zoMu>fx^6r_^)Q0FLVK=T9%5rr)6kTbmU;}@!Jt7JxVX3rjn0(7Cr{yb5HMcPbHT$6 z`!zqWtEacE2?ZuSXbTGqUDi5RFfB@Uwd!`0t!4!A=W32>>AysR&VCEUyZ;v}rhI-s z-8}PL&)&LFC0`2sBUD5) z;~gI#XMTc@s$DcG7VAMQH9E6%aCkC!m91g5wzdM6&SIxlD<2CB2J{8u1u0&${@KK7 zM^AGG*^(srF&P=z*~P^d)uQj!)g{f%3o99ua4+;!km;b<>~UdXA+VBo*>5eR0Saw> z-PwfE*bNAjcbgFkI2{)axZH?TVSZxk;LrwN*K0LR?_XHRYy)wmWlvj#c2=jM&TF`0 zaUMP*C5R|qmIh8K%FfAADbmq}O43dEcyDiSH(PZyEm`0(_5E;maH)!}&tt zS%Gnrnq|rm-k7qq{#NYHe+i!IgC)^>R}Zw@q&zi*a*vXBMk4)?E6%{A<{n_}9g<-ypU$H8pi|bMuowZ*hgB zr2)s>E~^Hz>iJ=BCL}y+ZEan)@$vCF{wYs#d@{=0hWRB(nU=?6bb-e7ZF$_YuBF=A z+9G=N2>Y$6shYO-V{jJ)2(paHHzRv*# zCXq2a1W)it{{TxXu`^+Ni6vf5Q)~Wy8pb-f&gJ6l6z`UfDkC6g`GCb*onvPl7Z*nw z+yCzxO(gxJW<&yE4kgqh8C)8RplkYgfTCBcr3GrKL<%L=(;NK#OB)W@Zb_@Pm$Y>2yLs_S!r=|$7!Ifv}^I%V`g@}~y-46+^Hb1+QG{5MH^cc79 z>~2DILLjO|nVFf4yu5C&2iXA|XL0ghA$%z5nUUncz<{HxtG&0k5Td32L9;45kQpi} zyn8*}XfmUt@h=V*w=Tj^hciOs?1Ljj@Q&AYGjaoq6ErnJy*Fd`q$DMCUNOEBqSi2V znSEArS}!^_K{PJd3x<1?SadW!Nbt+~bCD&ky?K)>?g|&55cZMtN8aW}-$VKc67_@eEWnuR0AM;`eR2u5HON<$-*NWAi-5 z1jPw1{+?}SrR(jF_4WyXxo0A-!W%%jV~q8m+Dl3zUnnOesk4-XHRLM(K(M1{A}#0+rdzP8(aa zlWl9Z0xDpz3}B4_cFuu7po6t8NOJOH7Z?kDVjJ1Lg12nkjwooF#{ejsrBsiDyPX6# z?0(p5KzfhHMz)Aa@8|L3hP_sw@n%)V{&+9l4$zWGkUs9%zHqS#o7MRdB+4e6<=B6@ z19yME8a~2V%}o2K-V3hT1n)WxMXo%^sz#`Xx&rpn>S|V24c{A||9Jse9hTHWeneWJb>r8%JyQlUCOn%UXen^h!+ zers!sR@}2M&!~oLl(?#QgU{k>#PAZBiXP?d8uOZzR`&4`E-EUzkBv1UiIkcUhtsmK zuv|ulUOsG?OHEC6pV1IK?&k$3fBpKkAtsQO2!*?qmX_Ag)HJiUHd(BZa~a!pNk~mi zeT2q1ksnW7Xrgm2E__$PXb|9azd0Fgx!TPM*efmZ--w`OA0Y72?Pj4XM zxaswMlrdfK>USp7D^2I*`DWZRxn7+>^Jsbx1aS8a_(%~nX$l(liX zP@%C9`fN4inrjkc^9zwR<(R=WdfQpX7SK8=j>$c=54=7NoIYM`kJ5G6K9y#|G3Qpe@P)E~Qeflwl z#rx;^#MhctC}?SEDN3nuP#EAFad)cft~4@$BwFE9hIt_VpvP|n;$`w1K0(3Wi9E@yyu6(j zc>Wf{-7y0{QP{+UCe&q7P9o19b+{6jn27UBK5#}TTH!en5z)`NIRg@eZt2tccV5I8 z3??ovt|s*E_N7$_BG<6e1OS%NLM#baZ%od;c0_1hyb-j+xZk6C)4^b#?W7e3}tfX2oU^ zhC~g7{_Wpll@U|seQ?=N_}J?qN)PG6Yg0YO{NLf|)A7akR-X~w?InJ(*$`a3w`ugP z)=8hKHTq}qkG|S_8dwl^1fNpGGqaarY8!87vIwy5EckwSNsK;>(-cYm_Bf|Tz(7hl zeqbh>mcGq9KNMeSvC2!C4m~3=8h@!^e3co*##fN{k_^ArD2|Y)`9Cl@z;Bo|^Zx~a z_ODOy+FE`kC8Z-NWH?8KZmC|!gM|H7H96BY6mVPs)w5@%k@Ls?xjy@Ij}r$0dfBuF z21ZM9zw`bea0hIxXhf5stnB2%0+orgh}&wMo$=WCIKQA^{Wie|6H6&RzE=_*KTmy8 zhbu=!mc7UBcm^YinB@R`XVZ+S7L9ks*7#dUOaw7k2;#3dx~I6-Wva639W8X6k-Bqc`yF$@rYt{2{? z5IE0OZ0zQ^+3(L)0&?xuRX|nSp~RPAW_fw}WzSJ=px885sTJuM*WrOGQJ7>dnd*T7 zB@vf}R{~k7N#=M==1j?N!hw!ZFfwQj$MT)z!Q2C$Y4#uQouVqH#&qo3CKyhe#ihVc z2sXpCFJjbaMM-3AvpLmk23Jh;_B}QGlx*u@#lb0of$qOInWD3;s{dAWOh81)ndLM1Qfho2i&$-!QhQL3?n0bV!g+f z678UsqhnNZ@?+1IcOkO9d{Ih;4NXlc>MV;bUaSvE=$Vt>@QH~<>BJNiFg>D|G77y} z0z*Odt}7DHlku*C^QD6>k~B;HAQ1hhQlCG6rea`Vg-p&vNMO)E2M6D3Ym*VMrg}f5 zc}%}GCbO{{JX@yk?c<}(_H-|W+RV(%+s{vz?I~bPV|z*v*@#6D33$676GQ+<%vJ4s zZbVR?MDPZG9ApU14aLld_IuFIRhh-w855C^q>S5V=#?uM0w&=%EGg<58nw>zJOmLx zzFY|Z^=zn=%-nz7O~~8+!Oob7h=??{A1GM>I%;`WwQBv{*EM#HL|JD)r#kg(M@PqM zf>jYJom4JLaBG0ZZF@oz`Av^QdjQU!x1k{<@FrM=@z8l3Y-XLvLqG4qdH*Ic-=+it z4^FdAQJpS$&dguSe>9bPl+|lzl(Fz^J0RJ*qq;5LIz_c^*3l~VeX&`DxJgK-++$sq z9KA2`%mF#BJ0C3*&}TxD-t@?~4{mz{^Ob!*Q@FJ1rLwp@&%Ez=!lh0frz^fUi!<8d zc!*ytzNXIY6khnzYDB73aGS*Js}PnH z@k5#O#w4+fQ4e^YpXVJ0RhU}VM>@N>*dmr1^*G3tl$F^jV$}jy zoGYXIt+WAkR(jM^wU$u`!CEp zH?}=%PhSS+l|W}>sS8vyZ6^=u{T%;{6a09j_Jx?4ciE7ljyc}(I+eCSPyD`adB{s7 zDIXTTT|;r{-96MKmRhOn%|1LYbnhaY?>7aw$R&qyn{hWLx6I$b7Ghi7AN^ zn1o{gf%xEmAs(InRHN5nqsRN(M_I^o>TQ*iUV69B9YBwx40@7T{Zg`jbX45gnSF3z zb@{*_B^E42(eQ`G$J<-8)n2b#K#?(h=lN>7tgsMz#3>^;x1_lld&w=Gf3dF*%FoX) z6k?R;cf0Iem6eu;)38k6t{bmP7iNTq6(c^j%Q|bB5)=dt#hQtnC>7e$(n6VfqJ8wZ zbM~vgn8{c*)_Zf+hqWK4m9IJV2WY?~Gy4#&&KhZqI|MnIPAGB!3gezr2S7*ViCi{Rpl zim1dy^zeuXD|`FW*4A_qgu|yW5GmnEg<=7H066}pLy`=AvDQFQM@PrchcpzeHdd8x z2p|;`1w3DbO{4kx6Ung_Z0m{;$l}9lUsX?Cmgfl_j35Sj{WfVy{n`f~t6BuC^R4 zoXX3~ySlmp%ct1{1r_LVcDk;nij^yvIXNW+^Y^lBY6{!iGd=mI05=WiHMgZLFCsv+ z0`Sx1Y;3;#(_0+nSki@Lao8N%e3;MEnPWdVI4CSE46<8oNgUq=^hyW&`y?1q=Tja% z$w0!dTrs%ju`#LWe~I~b4>vfc`296Uvz>J^%&cY8t2e{C1kge-(ZT`5dEuYzH_zT) z&!D-5Gv7WFl2~cxNG9RZo(DW{QG#`+Qd;rtd?z&JlaX@McJO6p3n9Kg_KV5|{A;Kc z3;HLOrR8drCw1+=A5a7%%56FtFhj__%und>e^GKMOkhlJJ z{!pQMm@+L70P2T-TYxxPL}T80BlGjB-OVy!hOzpA)ge1=^^RJc(?y(EaB~_9aYO!#-B2L>SGxM)Q$jk4~au+d%N$??Bm9T%#HJ>%&jPRa!4PHyG zD*^V5!hifrL~$aHnn^m8E122Y2PYl|9TJHw? zfUFQ?s#7)=wqqvkXBu5%_pOakPv&g~P23srz& znYVWqpi_&ALZlzB)aY6Y_MX1_)$PLt@WWkPTxd$}7h8LPtXom=fnvcrb%e+#w)e5B zw^C78wMq>z&NFuDsljfA!Y}yx0(q0F@x{ zirfCGgN87dt5@;g%&YKU&xods?^4!IUoguj_w)!pK0c<&RsTbguO);qw_X2~eb^Ct zH5ncm!6zc3f*LMouK6Mhvxfuf(rdoJY0Ok09jLlcUI|>!-=l- zNR>c38yg!SH`h#4Bc9&&Q~bkGEia6Wf|4Xhh3Z2r4KU_v3C9~eNBGwJ$rJboHl8Op zx}LonEwk;(e20(s7$nA%^e=Pi22v%+-_lQag+dVhF$hhg=>$jK+Iusxa4X;@*zP+w zRHM1@;`qPsd1_lEbbz6+F?QZq;Ny81-#1k}G6~^?3+P6oh*DIm7@NO9EE0@39byuU=Xa*&ji>5M zg5A2^5<_u82u`<@7>JKgPtqmIHyl#dAiS=A%RdCgRzuhxDFoZRj& zzqPyTfQ@@w^cePQn^&|519j5WY9)kLc<5i3*yUwPFaS4(7P}qpsU((#cs0I+mDG1J zkk_GvR?pnPp=oh2wI8^{{~;BgnvFD^7gZ>ufcz@O*B^EqW0Fj6DtP0ciBbqrQS$wBnpyy)@n*7Ie)jS2YubMv`($xMfOb8lPH zXHDfveLnpYJW1ZYA(5Bu5wJXGicHC)lT9-0m$(2YoxI(h$~d!&mY6%RfI)LTBk|ex zXwcAzW|tpBTkYpE_qU6l#pb9a4x7ap0Fns+Rm*a-(~iXB(-Uf^K=`P3D);FQdnFn% zYsY=)GK64T@&#i}E)vcxUy0eDju!zF!hh`kY3O}rP{JnxYQp(4tm|8vG z$kzhC`z3tGac4pq40?neymZmC@{OTJQs+%brTU^yu(v%EP!biha`Hu>x7n%s`>j4R zaD(c4{B) zqR>D&UqFRufIZX6$%(8z>X)nUFt(pUs^ZKA&2E1Vk1FWO%*@V`z8nJXp=|#l&LA``4jQun*{6xg&N5Lv_qerM==c(B zon1CqrxhSrEiHZ4l!N(58p`dnem~6hb7Q0Tk4L@7e9e`lMPb`d%k4&Up)Kqlh4kix zGR4<>>dM$&(`6mZp_|IoDzz&XpG)GHcH1|#Ds`uWlJq1wD#e4ge{hWTUrL!M4wJL? zk(qrK)h8}{)1Bcsi>8n9o;Q`F6kL z$V4^W$slOd@8j_jrE^yoX&+o^8)s{)hbX|~x!XU zN94zLr6q*%cV&tkZfq%+-@9MWMW*-X%z1n^GHgp{J+kd)t`5yLRU9@nRjj z;@7Nydnt$xifnW^S!t%gh8aa8Eu@>PL767uK+04B-e8fMglfglWVCMAlAo6MKrjidSNWEsP3zn?bzAdR&R#PL(ncJw6S&@0?ElTAza%w|%BcU-nZsrMwL(q%~Sk(#_p07a?h z?f3e+f``Z5e#fCLv0vLqUqc$uA+jO&E@bHs7HWG3k(KprZsQ(fPfQ~q6~O1Jl_SparK<3P#SW* zH+M@_ce_m8jt)C~tcs<&g)i~GHW=K4@5i&5z_U7@b4H1=Vu!Cd7iJ!ZMS*gXtKraA zST*4<-K(J2j%;mhP3N##%xT&AN6Bc0u^8i$ukRaF0Y^cD4$A2m@*8Vjyzf!uCNv*U zV{mnIyNEG(4z^ouVd3YWI6k)ZtDjB1#oa5_HuIywB1@H-U0a(>W6<&3~7!o+zb&*0K)z_=(ef75QJmDl95qY=MF~G(<%zM9?x zJXO+rpDCeSRK4gk_M8`_ig_OnJut=Ljl3i4lYL*@i6?tuiz!lu_+mN9GRk~W2;`-o zet(j?YBUUfE*cGrQG55-YF`cY-1Xa*R&LeX@3pl_H4QyAgH}o4NL$2*-fZWT`X!7# z(=Tsz{}~C7EF-X7@kHNBb5C)5Wa7aFb-m7OZ$ZgErUM~xSfBVinSFG}P*a+Y2ysbQ zjwV4kthG34u9jb{W+x?IxCS9y9O6WY-CvgpP5RxfMVvjgS>`SA&X<}4qyLOd#Kg7m&O zQAKU;Lkt7y`d$p_dhMo``P%+7OUe$t+5TP35wxc~Dwq6u1@gR}p5f=j)c%f~y@|8f z&$9DaK9nE}rzQPd48bYMddrs~6#V{fJjCl_3zfE#6I5s7H;S|$-Rd= z!yL&FufI;|zlr+`)WQ@WY!n}qYF{?!D=QsrY<3{TugnY#WwpdT0G`Fm4R|{#T&&S4 z*vCKr@-Ow^ADzFopp8{j8Rfm-4YY0%=6=Ddk7p#mj@noB`_)4_-sn!hbwla;COupcygU#cu`zZ$cbJ9;pW z21dKoTN`%E%#@Icu|SEVCxmni*z0Z5=9+1y|JeDeS7H11E$o(y`t4z-_(6?%5;d5` zynNB&?Brw@&@3IsiU=RtMsCUHc)ooKWty z6lGU8w|b`?iJG=;ByVqT{i}^t%4%E-)ozB+#eI@K54|RoI*`$PgZ|9;-CR(D&P3wp z_0fqQ$x(TJ$!6ZL%xc5$a!p$%1tM0e$VJj2jyf>ZJ6~yu4X>j*>vvd^s+NlAy{Z7@ z1|g6ZG&iRTJv9oMxVxvp6Z@hqxgiGf1%H)r&797BX~Gy*AJKPLFV2LR^)lxE%uwL8 zg{RqOD;jsj?B|Y$5-pzfE)cb7CutjZj2aFju%6?fO8GtU^6#68)Vmei{wh#k7W_NnALaZ_&GvGP?9IR zpGl)0WV9_GCzhSfBJU#Y*1bq@^3=9V7FPcvcjBA!{0F7-6rpDft(>l|E>7ciMvw;5 zS0_L5Ehv*z+IQ z@K4_qS}_z$F3c^ML7a)Ihr5TU;EO738Mfq!Q}6f-Nq>2kPF(Uti1lSZ5`r#j?WWSC zno~4%Ppy)Sk+>XL1@l%on)$CmIsE%D9#(Qi42L7C6zgGR$*DsX@_z&4>BcOd%+-A$ z|2$;y{JwR~{nOpL4^z7?N?-{0@4sie{K~p^?DhJTmdd0_nxUu+NJnVj`pwd(uFhV6 zIkon)1kzAB(Hm!kvecy(sY7Zt_el*Fps`_O?y=6(U=P1Mq~;khN4k{Eit!m0#ZckF zuw#E_mP|!FIgg!cV93z^c{|K5&uROsZ%^pW3y|c{W2qsM>U)gsvI4PcUHATSx9;QM zN2U^~fw^(#^(Rr%^Yvj5@bn!ae%&Jexw`7p65+NYrOBkg1ilbR2Qfkh?-9tK?pXcK zN%V&bD=Jb=N9d}F!g?F$TOi_<54j(M=eD3|t1Fm4@_b3tFB$SNK zzYr}fvo6>QZw}X=P=OL*wQYRMq^#&9H$4Zngc=(b2Vg5m1y$xa!_I^L1C2j!GJM#RKJxYzaP^b2)U- zZ)nIDp;L}|GBXmP<*YNpi1}70%-_e4XLw3+n!AF+TDX8FHDI`<(`rvi5SKS>p@ra7 z<473sbk>{Sdmq!gSwBJqxl4HMl#gr#YtlO(fm>NwSqCP$HlDMzEW`)AoerBMY~pli zRensc2>5-gpEVkmkf7O>4F#cR>F`RkmtIUlZ6HT-d>>VeRmsR4{21ZL+9=msm)nl3 zZ|Z;M&&qT{mDGgLDyj7Kyo$(vF~T`Gd=XlCZ7hm=oqdG z?q=mSnF~KFG=Y6#Vq{gLuT-Ns=!UJaOH!H9%^WA}?_*9;X0Vz@%4_Nja3A8PuD(w( zE0~Rb(`8RM@FH5e5<^itv0+DnoQ5G_OlOwvM#pGnRF-kz4*oOWy)?rPDte+83l zKJ4Z19T~jwCI~*3H+7@$Ax~f3w+1|M(T{HfD%O*e6AK@yG9WpS3GN#KtJ3WL%_QLK z+Yfq?BP1lx9xqOjuOyZ4XwMu@<3wI9(53x0ttF=2S+|2|9&x9fNnqlB_d(8KBZ$r_ z77AvXw4FCBQOOb}SQ$zo#r00yRnJ)XpK)j}^c8BvTQ6t-n1Af#`J5WL zyWfT;m_emiu%EH6EEiz7Rw9rp^cemSPE_X1PML(u-TcUY3Agq`F*t-D+~_%O?D;X} zf|j^&dxIGnpxv+E)Oa|ZG%o>Vb+SU4SsW9o{)oAS#ltXHProRt0EI|5_nsxV{ivpM z|LADvi@^g@nw$N|+TQK{kXB){zNN>GPs#@^bLwHd<<{&MpF38rd}ul43b>zfaf3-t z0~a(a&K0UvsxsobOQd;Hb2IaU*Rq{>-iKNe`Ccdvyt1+#VN$;anSIF^zPkPphYz$t zm%p8H+S0LHeU=zFFi*K~CKl(z8HuN^qyO|S{rGyvv!8tou2tLAd|F|cs3Tv>1$Qf4 zBE@PRwWv_9aBy&*<2vgDKI>ORfCdwoRPG3d!>m`VwDljGe{`Cd zd)9*}MdJ+E_EVW>nEEBGVa5BrDbWm;3B6beK8z~}-7jvSZ7+z~$P4E0MF>5U>a^PL9#+)~u8Q(fVl2jB7NItG&E`oO*++LX@sCX9 zPP4J{M-P|Q*Jq}snJa%p1fhQd7if;Lre}gW8R=#=L4wJoM2bnLIG1|xr2G(ZcsO$kaqpY3Ci{*YM^Vi1_G@qzG#er5dV93%hIh_(3|2R7A1r+| z+ZD3VIWriyD@wID=yQU}FU~P$;UjNHKv0c98tUoSXqTOrM8c|k$YJ-5xZ7XaG^k?I(rPwfDr!67P zH6WB|OE=x1$&`NmyXCODLYy_m&LWDQfxhBA|3-w6~)

l;x}2&$ku3E|0`89 zZ3qy@1D4WoaB$4>R~b(Tr20aq9Y>7rXX}0~5u}5(E`$+)=Q4G^?`2L+O?sU^Vd~-s zGXfDbrLiei9@fE`R{>t|~0G+m{;_XCyx{uHS8?^Th$&5yBwT~HV0JspQ?4Lv$O)WLsNN$*L3?%==fuO$@dU76{ACPn3TOO%TFG z!^&OA@UR3h@O7NzSUTSec-{LQ@b-d*6o&o8i~4mNG1sU$9s|NpzQMwtm2=z1lwqF0 zNaS=}s1ynGpa7C5s^7jSMe^vJAP(tZ``OdRo4>#l`nGOqv$?0I=79IbO5=UH>t?&E zj5zCDGV)nK(AJAjW=>Y2_KozQ%J=LnOG_#AsBjj!utlO0@NFd+VNvK^)=`eK)O19kRh(HW1TJ>t6;46QE*bm{7A0+yFyl~$PY~qp zOGTjf2>@$OLCW?|S*Bg!9Ts}MNO-ty?VXHa>iKbeOyBc%395m1lw_8vLUI9HQwlSrwUD`rZeWpe8s^c{qR0I+2oJJZ2_2$ z*M8@{ph$h_K%Wv$&8LD8s3u86nv^^$EW5sF3T9)@4@)rr$&3caK!s>r>z?Fi-bu0VpBUKbo|^dG3piA`pEa0=a{IY?^z{*b zG9zp2=-}hwZ5~$D{?qUZ{=oPxjSCc6ywV#W^x*qyiz*s9Ct)*rea+6s#&&&uopZ94 zgN1lBgrRNoTq(-JHvMOxnf&RF+1W5-`2ks-Tf#yT{$Loc=mIaTS?PwzGQB}%*hv=) zZdooW^8kZ^AudCLFigB&iGncho3-sQGenHsh`FyKWz=FXDzsRvh}5_OPl7C(SRv^% zE&5SQJsDh$)_0}#gsiBbkN$29rJqN>U^}w9r0lV&I&1mrkukq}cE$(O&mevFEYO>Q zmN^Kk#CTVUMQcu7Z}Kst5HB1%DH`)X84pW|GOgfkeZs~1T~9hZvF|r{BF{jBmqQJ} z1;x5~jGr+8^VlSV)j8dxXp}1Fxr_`R_THTB8^Orq|Kn4P>CH41(|`RIrqy^ z4tI9P;7g*&v4nj#|V z*n+X;|20ejCb9N1w8r&#I<594g@r-DW*0#HlQM4R%RaqN_~aTTc}<900mrERvHB~E z@t7DG?Y9m4`;)6A#>=Lu^PH)A(Y$?ZH_~+*V~=oB=sYdT8AEOP zUr~&a&C2e&(P1zO6`)P8`xOfF@H$Z{Bur7-KEuLN0~5>h=HZGzv=a#?_(t1X92w#C zoWPDSAapD(b}mEM8t2WmesOL`ix(aNcOEz6=Z&8Hm3zxISIrN_L-i91kN zzq_7EQ82;FGG>9?_rE({e<7<gf0l*th^Jk;!)G$)wKngn|0_=iHxP4Lr-UY3Q0XX+H!qdveg!+Pj@=a*yY0SyegiI1!hKMeUAMJNnn`eJi{pclS6@9{UFs7b)!ihos;D&?8v{>qv4Frdb0n z?Asht1CL8>+6h#jVcSyq?Mq@CYzOKoO0g?93_hI7&rxLAlB~@K?Qs7YH#=6!xouG} z-^0FYr1V{a#tF8E=vsb(6havk&S;y^h_j+oXG#?ye=uXTWnp8Rjz96i`<%Z$sk1I2 zIbvOzH$HRjAAZ~O<44bn=Q7z*R_8KzTjI}otld#W*AgifbaQ=tnvL)EwwRwMi#kz~ zSh0yvYK{s*pp^DS1KK)vF%}&2lxTvn6#`k00_LqtQ0*)>p9n@Ol*Z)H;yH}9(UJL7 z1ML5OpU}3&GD@XYR4{?O2uK@%h7{0DL=}2w&Xg-%SMl&N&?~`-E(8^#*(}unJ8_`$ z?+2;(PfrD9Er89Oks8sVH?>Y5V%j=9Q~4j-->Ft%S4$zKkYUIcBD+}Ofw2muVqIma zYmU$+f`!6gCZf@C_#EyYiKjHKKDdN?bPm z5M}xyEwi|_HU=tvtMH|(&ICTQ0zbt9GojgV)rqb~c0uWnUp~kv&zfFl248fQ6Mv(O zn*+vY8}9pv=ub~sSMa>!C!(kYMO-VyZNr`%gZ!U z`4uE3;Q=KAK%4CB{IuFq5-0@}aJk>=UOCJQKYk39fQR;Pj#bGhVHXk;Pkp_7?g=pDieD@aIfS(^9y%?F<0Z25RjX0Sr}Xji({3`MZ3+A zON8+?O9y7v#>^rd=|k~Z5ncNZ^A>cymrG2YwcNY!KP6otsK8TK;C!jJwxRwUht?*z z&`#f(=O3)vF4n(zpUcw0#r1O?zgW{9AAf9nBGrlQirWQ$!V_y;0Zr8}1tky)jBR;d zsYdrexFwaOG!Mc=YZHI|N$5`-V-*oep+mY7y2pccH5a-~TP;O67vFWsRP>+xoMV(3 zIV&p*fK%W*bqnkpMYy0NH+r!Verb>fBl@QIUCoFDYK-puDWfryr1=|Op2C_@$c_|) zfmEABNuY=>-Gmm+#h{vK6Sv2m(ts~>r8UtLDr0tx*}xwqFzMp4_+y?7t!w} zP-ACtV?Zh?o}wAy5;2++kSo2W1C@sAurR}Mvo2jkzD$xj^0uJPx`FD#nHA6zQH@Gz zQof`W>c*F?RP)&q%mhg`b0^f8BhYCz=?uVD zD`Bmom=GW}MJ(dv9VszRpifO2hGA-M(HI88-v0aK|GYQ=KjhIRW04>jY6HB9wO4^f zRj%$#V`jMgm5^Gtgf&4Wt4y12J6*3!1_w+Q3I%C7U&H7*+xb#A5QprrF?#EBW^~3j z{cZo)bVp4C!#)y2R_uZwR}?zpwT$JwS+X$0%+obAm+82WXKm^so3WsiFZPJ9El;#0 z$Hd(|y1pJ=;`SpOi=cS>wsC~0>EFM!e`Eqhe$_>_L(AtW(#(naLTSju88(c}6C1gtjvTSt z5rYj&6dwt@4VDI`K%4gKm;y#ThoL4Z^0;sja8LYCECw^lj9z%?9Y0qNA@2^bm;8oZ z7$X$fG}1yRyD(>#bA{4h5YZ6mddu(TL%n^b-}im<&pUfvo$~jVFH1(d70>v9Ny6KY z|KWR=cG{hSmjkv2{F|e2H#vd^&d0$IX%JNwoDdF?QQhV-oe)no;i5nUONxGqF$aiSV=j22$Q8!$|;)p#iI9WY7-whf`k+J1So?|J3@ z@8=GyUZv%Cz2Md#eQ7X5)mlb6apsPe>{^Tf`SoP8&6o-#G46iE2qzA8T(Pe*El^*n$@rS&8aNB`6qdcu zvAR>uitHm4Y9-NN@pgn|avY_W3+Ds|3qyiOmDunVd7L(0CYY|7OHEJaMRR!Fve+y* z@lF*i58d6pC?r0qg1;^Pj02_N!V!AimJkYhBXec4cG$&~*K2-^OAR%W!5W6OlU-G% zu`910KqxPxT(#hgdej@(ClSF4bo|9RvS6Z{y|Y44#!ljhPP2xV89rl<_lX=V4u`Wx zxTuv7a z)%5pJU4-Z+p=C*7Mv+?x=lhyb>5dPsd-O5mvV`52NMpI5!>Om8Zs%!N*h>&aZV1`}lsq`sy+-{A88eoU=pCXmM(C>mSx{ha>abSqMc&@#Vd(RZmVLPDnWu6jDR~8k33U3Oz?4PyIOpzl-*Im7+g|O zraJd|`!tYU`?Bgt;Qyj_)#V_oHDa*dJLzW7l^@sDuX%^+->X75wH~a;?Daof0Cg8@ z)e9RTPfFR`d$PEBAJ$8wm*h;B#>ua_kO~~NI9lo>tQ|JhU6e}jtbB+)(u%blAHLC- zphHAcZp9Qj(_fS)#K!!;2k^tNHv>!U%g15LY`%XRl#aH>TC0X^6~p7{yN9k#exAzF z+xtynVXO_CWsm?JmSNb^aqkX_S<0%O zn-B%8Q)!&;$x5OC|3iu7dskb+hyMyC|C#g%&j^E!==$rj58l$_YF@2sxR6kN9F+eF zeEy^{wf8pM1I2ERiJe`Xjzf1U;QBNkg0%(R*iuZ?WP)7w?3t2pJNUC^nn4r5$B(03X z#)Dgu1dCKBA-_L9NVADnlg|hDZKo>xx6ibI@6~C9)K8vmG3RLkkB0hs1vM>t3{t2) z%SwVM5=8R9d+>k6qcDSSZsaR{Wi+{}B@oA*Waub{HJj!&E9z@X7eW4oGsEnjA2(nNd?ZXT?TWW%7{%>F=hvraTw zSHwV`G$rzvsWat476ttlhCodTn^GCHc%U3d-nSJBg$RQtBKGYDv5^r+LCdGs-dQZolH`xsl&yEv%RR@e9WO-h8SZ}jaNP-(Q}Di-Hhuam4FE5_@_|Y@MZgF zzvA@E@9B%jrQW>M)YP-XOCa)|+~2tf);ODnh!$L9csB}b!kZ>x*V4Gb=wuRbh!W!L zX0a{;e^H6ePiLTc$MpsV4dvr&B2tE6ZLD(^G>hwD&|qMq5I{vuiYs~~k(j~UFxUld z;=Q#UcJRZ~;~VQ(Kk{_JxGxBLzle5t>VeODg*cx7$(aIg0X z0b1xu=<`le`>|E;!0TWXrtiKe@pdus%l=(g((C9L9t7aqFunX_0z|ZzjY3b20H+SN z-(wwhF5(6^Ia_UQ;?<5tK8|i!bDbZS4mCyzOziOV#OtGrIpAq9_|#UR4Rs;*l+w918y-7dql8}7?cy1F;bXo ze94d*4i3&QB+(pnONz3ksfN5g+g%>&UN3?j5KcOIbcb_XBqB)U3eM@)|KjcF7PML^M#Mav3)z}@S;ge{w??T^z?Ez!9QhkQXUw5Uj*ZJZSpZf z-5R|N{UwxW-nW(!SS!i0|B9`{K8Xb;XrlYN8|mH=MBor&UB66-hVG6!BhV$bD37)k z*^>7&g?z<0l&-AFEyfBuku<~$4Wk+kJ;@zspMj{Lw@D_aM0F^Gb)#@>q&lB3#G3HR zB|=M2VDR8IaAna|UE^0~yw$Wl+K+D~O(RW}ENbF*>}KnM&9H^!IPvV!k->&76}EU} z_2nI*ErpnHp^F$~KTL%c5qLpK2qI9HgkW_FR+4DZ0eiV*O-_#%Ji8OaFJboBFnDLN z%d0Dt2%>+z-zACUoBGFFtuYhG<6py1)Yig{HcL*Zl9*~259 z+5NAjrP<+&mMSKTlBc!193eG8-_~{4eYUHT|2WS^RxZRe?IL&)r8slq4(tbR|C1!! zbK>Qv8IZ`mS63;%@Ls-0l4rSy(ZlKpQ6|+=Z8pth-ZjF#A&N14hTuE$*BXgNk?#y3 z8d&sUnwm-)j}!GyG_D}(@<|;Zf%D_Mm95&$I4?at6N{#92}6R)v0$Va`Y^so3P~{3 z_dt>iw_lQbA>I+Q98;EmUcaJMGSWzX3bhJw*LuiB)w}p_XwTE9GrMoZ)OlX&<(nKe za_iyR@lm%KfAhG5Yi`kf%KD;pD3&nGVc%e9^!lIlApa0Bm_w@8zp|3C3f0P z)wul2)tjkbdl$KPW%PnMp%v2%r=r0=^v$|K;Hb2Aq^V<|oIqu-anrvkEbG`f8>w3O z2bNeYWoV0t;+-Q(^&tt3<+yeBkak0ujNeZgzib^Rt;TD>>v45rp)5J%? zL?Kfe3w>{8KH6CdXU3>zG}m=HAROSR&Yen>k5G+3STLNZP8WnRCPk=gs-z^2uagd( z#IQvZv5b21`}_AV`W#}3wPO&)XIINu^S1WU^<{Ze{d#sT!-qarmk(DHwM6|HFspJ1 zNyzOR8j5Hn0|=R>2>564H%L`Cw#Up*e6b}{9#LgigI4)8M`s(2Js2bZTgqJVpB>aJ zrU~^rWWPC&i{=n4{2Jl*tZCj=xwD@o7=L@I&7ZuSGYxyO5WK<{d^u25QCnbRt%k>o zlh6%#xp_WEePd?wo{b^8;1{e8209(&iiY#Dejq)z6 zYuSDcf(yn&FVV!``VLEqu0U4ZM-at{J`h%3D$ao|5mF!c-iBjEzT#4VH%2m1L`?1M ztK5{5B4@^_?YD%|6!|_&>mN1|oJllTe|&Uuia~smi?VgaIYUFxS_E^aAHRaBB#hT= zuU43m5g};kQq^crFK6*r(yhqvwFsQ9mC`l3Oz~x{$ z^eftj%83raqpX$M=-f)0rUaN3u)*I(zyJ9oa7S8QqP-DhC7+0Me5&wk^;3NttFga- z;_FYNL;gHF_D|uFBr67b#6_`ic6UC9H?PCJPa@qJ*eg|~--#SBEL>ov;Ync(4F12n zm3@g|+#vV&uF#rm5?gZC$QKv;xZTFPv(3P}x9mOsU5#H34h~lr2?06H&CM3JN&)K? zDrIe3;h*mN;01*V-{zw5h!=%Ka+;eSQmpR?AI{m;8Y7e`hb^ns_swMlmJ8MeVu;R8 zBChepXvrd-L=aeip&kV>lTP78cFKlo;o4A~>=SBRRo8EVay~fXATVQ4r?F-rLEfLZtA`op>lBk7?k69?LzT=Ca0;IxNTM)%IH#m15(quM`L(iI z;-mg^wB!1(Isyb?PR{((Bo#g=bbd0lbQBRnWb}ad8aMn3UZt}b`#Qw| zBSxt}=(!vBe+&Vl@W*VzVt#L#iYr(bV325%fd zOMf^cMj$0-N_+ex*Y7^Q^$QdfBK~_t|1-$r$mj$1@)UupmQq3s&G4s`T~2vEi6bP7 z|3}qX2DKS=YnxIeNFlg;aQEQuPK#5DySr1|0>z=YyA-FmOK~Xf?(SB;$@|Xv=A1Q? zU(YkikG*GS?X~W8U&IsqUxbR6ykCL<7sAQ^dlSUNI}PnwIU8(Uh_H);5-^mE}aH&<1GnqjCUV=xD9 z0g1GP9z%LvkB*!zyvEYhGA1K62P&|~RzmY&^`HE+12a3isVF9G{~DxnBg<{N1mm;W z(@Bl(Y!V+zM+g@~gw!xODcgoz>1YJ87*W4say~XDGjWrhY(hk+1mjrMn9KR!;1H`H zJ*${j^AzL8w!#$1TaP!@rvDb%Cv0sUCwVW50{AJNSWS(LB9kR#WM3503Zs)?DX5i% ziZV}gD~L0!WZ^kvF~rC2v!o2GBxEC9bCdGZ&Vex#(|W?O5i=JpiS_@#&UE0F{qWbc za%a?r%71ItgR`9UJ@;SxJXqM}9nA{=GQjU0hxkxh<@f6de&1R1#zfiYlI*LB{pfpgCum-lc-1110@teoIJpz@x_c z*7ceC1Rq%rdx8to{&IavilJiVDsnEAi<*lqp6045uKIM=AdQC;4`H96bb<>Ml@Dyh z9jZJY9LKcFbdjZxEC>NBMYhuSe^5)I=#3*swyetgBE&?3a3%N$$yP_bUSgf=i-z3F zMLe-^gmAqyC#g}E6l{JK+aBCnzi9`gN$oVoW8@gKf$GeB6FUSb8Eeg9?rV>gESagJ zYrMCK{1%FXa3d5%*ISA^m-2TYsVU9)7}%fL}Ttpcz&v-+70Rvy#Qor{4J`X!p17V+J zYmEc$mE~K?A8WQO z7fMRccZ*T`)LK2Yhp%DTYng3ppUtKxUC38$XTf+JjE!Lq=afo*L>UA;K@|t&b5Sj$ zA5Vtwkw*$BfNq#s@G+=AZuh`Sga2Id^qp}+nmp)e;5CZM2xY23W;Wb!XCp&=b0=j;4`seHhfa&j$@?!V5!7OtY*%R!Z?~K6h*oDWCL7il)L}GnY$~7b7M~}16jnTD6-D2whF?{y z5eUExqN7M!+7QqcH9R|opC=4w|1@ETIVhChA~8d-$(w%nzw#j;@6yISmj{O%#vBo+ z9x*?1cvk&0_EgDLcX!1yGCv%}BGdra1%!w5luG}MPZkpNvW1T&D01*)B^0R>9-h;I z!dgu3_#hR1P-gsutm3D09(#<%n z=`+f-e|`9h0dn31YRAA4;=1M_=0S2Q(^XnU1WBuarbsKGVU-w+7(Tf;--)V47ZE97 zo26rKM{|ytld^r zHfVT;Tw6!CxVb@+KrYBYT90~nMGw42zK2bb9yC4Pfg zmK=jiHzC5=qogC(=TcVM5YPTk+BvqFE|yibKdyy^T=vJPvd{aThvGQHnlj@qGK=wR z@~oWjEA3V1lFT{s36v~{kSkhU5fBf_z@i*@YF0530&py%2=qog;fnllYLLYmt$0ZQ z@eB1ar!g)XkxgxyPVOnwPDeeYX)^BOOnnz#$*z?Gy)Z+P#~Xu5^V%|ZSrPkBRBcJe^FH~@htos*d8%*8ecK##Ny2&O)}r6ZE6 zLXB~1ST+P2G8d~^a{p8UGu7nA?IzMY;U}2-LDYcS`nVmGnRG9-g@e$Uqetc@=PXS%-vGan@-T!=yDtG$rd+wUE zi+NJscV1!zDKh36id2uMRoq?qN!D+&9|X~;ylu1i-J+m!kW68}XSJ^^lQ%MgYqZfD z9}kEpbi0ij^O;M&?6`eti=3e%{xA%TGbApbYM6}^W#RDHWpfEnk=_3Y8MC4}t8}M6 z#Tq0DY6)KXCd4}N-fcUpyZI{bklAO*-kMkCl5Q+6ylT|aq0CP3Ihk9d$||YQ8eZy? z)W-sGqsn4CJ!vmxH3ozm#6vg!5b@31N@p3OQOk5rLNts&91$jUU>ekqy>v8+SPdcM zk-uPPxTxf@h|w6~cFKz@xnr8Nhx~a{C6IrM)NayMM737}4xRxPqKv(ChV8tRNkBJ_ z?cbz#6M%wGhPB$iQos;7Nf;#wZUQVfAJdKxM)MwX-WD-7|H;>cVrqTw|Je!qj?ycmDX^F-e4*HUs_6Rm-~#sfxG>XfPV2s3t#vThBtW{_N@iu3DjLCpGINQ4O( zTD=zYt`GuPCoLB<-#fZVVBf}H!4Q~5h)%Q~4*a8hUJ}Me5AQ|`bUQ^n=NmAM9=Yc$ z77*Co$t+f<+ZkpYr_=h&&>#6-SBJsOszJ(%PGP`Of2M)ZRI+5HAR|mtJF4BhrA;aO z0ALppN}lpT4I8VUf+L0^=GQ#t0J|DKhqiXNT+jWwW0%2DZq)#Pv>Td@g^DUYNF70z zUQM1o8#R(rjyF0UvrcszWP5JQzHpMrJT^PeY@I;G1fY=X)A8tB4R9PX=lK~|h(r@v zhTu0S;M1TH2y^NfCCWjXa(b!=X8VZ=qEHS26PQ>zg=l}6Aaq>;et}mj35Vn_^&(J zTX^5RGA~5_RptI`-%}o&?0m&{qI*2Fv-$fRiGI2;$laVta8pO+!c_R<{n-1W?cnnO z%4VfhGW^~-u3e&uzFcz6XBA${D7$py2(>V_6u7!*MUDjjABFYbC8_!lOeUsxeHsRU z8n~8wz*;J~gE*sKWD{-SC-!ig{Y-^9Z{K;i_e>MF<|QCJ^K{ZU8by|b;T*7LphWpP zUL3_J1dPyza-$@?2I;A@PifJgE9*1$HcT5j#8~wyqEVxT4#QH`I*`q{IpSpLBL(Ow zV=0i>)kF)pP%0RQ4d*|6`^)U(V^XeJkksewAJjL^^GQR)q(Mwpj#$c-pQM+qkO8IG z3AHIag**Yq3{`1`W2EG^jzz6TLQgYdkeuYD;)X<8l!5xE%V7t}tt-T(+fy<_h0fy> zN5tVhA`>y6WD}dr$IP!A{&gAWV_`uTK;B ze_E+O5nYe)B%XyIGBII#AmAOvA9<;KD^bqczR7tPgi3dpmY0ovd|rak3U1mhTA4<* zb#%5axBA+*acVZxZK{o|td5M)M{nAP0@b6SZ6vLLJFQHxQ_$U=o4bbxnOjczfzYfR z{YV`o7!lq1bdkNf^_aon>5mo%KI$vRT=oN30pvdg{2Y7!k&iyqcyY5I9ZPc@&$uVI z=Z0`)du7&FE=5`QH#%cl{xZ|img)vukrW&yO`FPN zTQITQYwM(!Y$&CjpQ*>l*tVsNqCI%_)IUoa4GKM6IBy3MS4ntByTCbxBx7PB6j}|w z=BliY(4~h}sfnp^V{1gSn>QVu`TIepajUqDfC>HepO_l#r^}_Hbc!aqaxjNs64J`k zrg-4xZ4Bo>SJY75 zV-_fzggVAQwspM-l$U#Z&ZeLUK_+gp zk3xX5d>P;0hh>Dt(~DoH(BFVvucKXKDBm7?qlE+~Z%98%sa@g)T;cgX_K`tnOY^I3 zAne*UeySHcD0sr1OqbPCl@SY-b9+B4Xo0@A|TfO6D4!zb*diWpPI8FTTSTA-? z8}`M!?9me9&^!J;wZ9G%_lV50y5y!_4yKaiO^0o??VMs6M1E%U>A}KW-x&(O!xM!O zBgQf(Xj<$&go*ft+hAS0yosv)c{YTPW86_LDjBUnx7;_RY_P#i2<4bro3@81!dY?V zE4iYNcK_*s7=XzTl3!JVUBGYumi6mO z%o0YG=WiTg-g(n$VLl*8x_nF*HEtB=4jaT5x2qkbERqVNC_}~Qjo)Omi!4{aJ|m$y zXadd#)hjTLv62}+ZGPx2{Go}E*~jO|A*vm2O?W*`3QKo`Apawa zRH{6N5?z=xIW7Z;6P@Kc)I9TOiGG{ALC`ozN)&o_!lMX!OTJCl)JU2YD)z~MG)-`< zE*CNA9nVi$Pf6!C6sX7MyD6UF?qDm;vHeWKhuzmgn>u>{gqYZvL|eD_f#%^b(MZ7| zO@@udmN*YpIG|cXZP^I*8GkZb>Nn+7<(g=Kg!~*wHn!p@f=SPBhtQjYu3dqHI6yC5 zF9qA6VWk*2j>E7k;(hA|S1NY?lO`{q6(pzlnOCuxFA!C(9!-~k@W^o>a7_CRx>xZWaScBBhIipL>En5%@J*h02+wCY`@k>tPu(umVYte{kCBLo zsQYyIZ)hld_RH#l^PGm3)9E66N^0tx%gPNYwGDIPNJzcKAgg0}DtA->SEpmY>tw!=+Y&w}9MR9zXRd}hWvO*72O#ECK; z{_CIo%G|~TU8|^enjBm>BM5o!%Ek8#nbgnE@x`ppra-Fa3dl{+AXqxo9BdVtfKMPf z49XKo>(l9bouORTX$s$2La3-(ubCIwD7=VMhcUjVZ0Mr5PnfVaEiad3qB3w{n)0 z{UsiD0Y9pW!rbOZ>8#OJ_lF=cG8{PD3)CZ5PBc<}@JbSIO<6@c*U8jTA%4}gUCeA4 zIY3O77?$GWHW&3Y>>OcJv$P~t&&>75Tdg1L0V6nocd_xCPR0qo<)*#Kf2Wg=$={xE zl(PlpfN;b@-boAX``qnYpBy%mYkaAV4mX+vFKRiml%Hl&JNuz^TWIxga1a4HFfwwJ z3H7BVdkiGI%*#Fyf=@CVfxc4n@3=-ih1!YOLbuE(R?R%+U%=2~ndJnU%KPvICf=k0K znB80M(=D!8e@uKdgzPn8Sv=0YAJqF^O(eeV6#qn=fACz9vERLanYgZgf8*Bow~YVT zk=ES)44&r`cADJZAzVcHz^Z@dGDnT#N|~c4wDkCQ2Nrywsv%X|ro^{iV26ykM4^lb zdebOpkO2p~p;azj)Ov(aO1h^WG7*DF6o*eR3y|YrF%aOK&axeJM(MnZKm3wb;oRvUC8QlVf)LjiKS>}^Iw5N$somZ(NlM_#F+3G`zJ*72 zaQg4)C9nhLgekb^J1COihm55ipwCg{6(O@vSKwsU6)ifql<;W) zK8UWHPU>^2>vo)_a8Uz%Qv+C_t=(r(0g@v&vl?2QvPf zUgI{8PdV>i)t%4QGh7{u61VLoSsxM6E}?#73mxZfzPnUpTMq&64}3oNOaV8%0ey(V zmmk(&)*qbrlC_=-DupiSH~W=)+`hG31O95@LCr3~XUndOI3> z{fhH0^fEFW@X`u1p_}ge$8(2i_G;dix+4U97U!NC$=_CbgkQOa9?dKcki@16LZdvM z`Sv@hn6My8Dk`L*OUM&3}~DwrKb>oV3|rGDVb2{T8Ac= zf>FfI5s9zt;(BD6Bu0QD+u3-=42C;Md~q1Z=ZE6#ls;4JRXViJ9!)hJll?0>4~Y9| zM{FeuZU}9+0u+1)$7!3CmZysaXby0aC?O@A*t46}lBrKxWC|?hD|COy$qnfvHz!NF z_o>c;vEzR7RIC-C9QIb+r?I6K*xmUGu}6|XnmDZ~&VhfM$0t1bC7zH9uP1=ze(zkj zPN@+7gLB-4M!jxfckI)G4yhf`j3ngpZ8`g4a%pjKv-PBUXWi$5I*PLM{iQ1hYKpDz zHmBjwoqHfO;oU(h@-{01gdPx>UZ9u;Ri z-hF#_J8q?kHSxcd2}o-0ysPrvc<{>!SodDvIfY)>Ue+%5AFpLuG-F%N1-$aIjedgf58X)(eX!^YNY79MT4dIcU`+H0{okVi3J|_FT zFP+fY4A6G^@~R`R-*^3knG|U1?J?+GsqW0|3l3owZW)SP3v#NmKndzX%)=%=rET98 z&3S+Njo7kZN3Fi0^@3O zqzF1#5=vyc#pQ{s8XqI)>51b}RZ%BU+yr|eM32HbiDq3mn{!%#>FJa6rW2SHL?uxX zTwGg8Djh99`0OB=RjDFm0s(N_uNwoE*U~NF_1=YXFQH(2kzfyP8@dWu`k-)hq)hl5 z`z9UGcbARwSts(4LTfG^W7+&P^6%y45#l4Suf!S}!Sp&=DxUW^UyCIq>IJB^42vX3 zfiY5g!Hz+yFv@J}S%!OrLEl5!sNI6xlj?WK72($Gr4!`0L6mW@N5tjaNg4x%rSwFj z=jS2gcO)UGo>!wkTyCQj@Rf&5BWdKRTeH}fbP9L_zSEsiPLLrSb#ZyEQ?>qkK7ryx z;?=JcQrrK7%603nGy28+mQXrQZP`dugH>^9c;F+)X&%GgqPJ`vdm zZ&ZC-aZnmEr^N)!e-N$J9v#sKLKj|mUZ&X(V;Dp14xW8Pmg!;8f6Hl0sLTk-;O$uJ zW_h7=eqqaVJ#o=s&iUo{2)x}P1Ds%hNNK>hS@b*kZPUb+mWQC|-4DDnf1=sCEfxD# zlmp_f*TjX(RRvA$W6Aa-oYdK#gijFi3RyRJL zR3TsN?+?2>pW{%-L&(ov564*9*Ysq{kc&$GKH+LdvlHrreeUI>VS?zWWPOG~S$mBf zrd!=OxBEYgV+PX_Ml7?o<_UdHh<5vs+^VdOb@pO7g5>!K8R?S?Y#EZ&cW0O6pV^f8 zz|E4hqY?0m9z&;DdIgJ5?@K;6bZzDQ7ccb>!UwCh`{iE}kbeRw;DX61!RV+};x;#} zaS?rt_b1m@FJdcAdT0KHwJTd|3GIA!*?C2A@*IcQNX;E5=CS*df z&c8Ck-*TI4a`ahb1W?PKg#9ytl1K=Fjb##F0fB2nBCO`r_sYV442mm%DO9}iLx7eh z>^x$CNOkLKf~rPb3aJCPda7a~YZI#m@oW3+Y-6OZxn6W}K@mek%ZL_XDNI_pvxr5N zUvlXhKX#^s{;IGX)OvV_nHNc??rf1eYM>?4A!p0bJ4}g5s+VE$N1gY0xjx`r>PL6O zgmt5*rKcOcOmuL4RAQWxC3Z!X@0Z;vaE%G|OUV4eWejmP(PJIm=|C_Pvo z2i=6wgdQAApDeHMJVITm+j#7lnjG)cpO>_)omB0|QZelb zf)8(Nhd>~-M(sG$5eX8@?oKL!?{0sL4eDPb&s|NJR)#7Nxz{TV9UyxN9+^GY@NZTh z7q?iH@r55f%}neFKXv@jz9?7Bd^e%^ERGiVYtHm_T4vE{fn>~fL(#t_WCBsr=p&?; z@%H4Mh$K-i7R4;abXBeP=_(}q4Dd*j=qEGWJgnUL zQmK6b#x!AS<+R~z_KxC0ib`wRT9E zsbJwyH>{xj{%2sSg^}nX=L%asd#Py?LKQb}xRa(?5GKo2ozcgSkPm3+aUs?(i}cG` zR*Vr67+Pv_$s|d{(Ou4W_DW&mkE%BKQ6}V5s(dQNOjKObhLUl}RZWY$B=9@j+*F4` zANh$RHiK5{T>7_t6B8em2^rKriH(mhG6`yP?6~fA5I?$y=AlDCAOl)kTYEjq`9}}c z@j^dPbq18HP?F>Syv4#yMP)+fyJ4}`>ZYiy40{PZ&OR&iQNP6gjCaBZinFn?buTyC z{QmcRLO?>Yhu8H8Z?jN6-v11NSg^+);!B|iN~is*CoS|N4ERU^2Y<1-H(yNVbXUUz zwCUS>Ke63^AoE6-r0>5t?Va?zkB_`V6q~YDUFnF9l;N#A=t2DHX-ef^C2v%ovHB{= zgFfTpB>7=)AIKw4FPFEfpt1ogFXRU>v*M{70nr?k{4RXi``sm;)A6l==@N&^Rj zGy!)&>#C4N>jZGG1AId|yK-KJey!-Y0Fhq&ZSq1sA-Ne>Bx|?AX)r8tnIkcx44-7) zk2#9?xm;{bIpT#p?PJP8psqs4bRkK9NUidK2R~;>6IJ=jt;mm@892jV?wXZ&+HLQc zl=L{&C)-{ugWCCYl(9Q+_B0S4kX!;!8n??=o1WsCE;4DZ;P30|A1augGN-JYuddB> zW4_OOX4u0?%bBu-Gwr5TYm{mcZqm9cNRf8q_ey7hEU}vPTK*KYP4?jcwQTzD>=Vw3 zopwJjb%c+c`7Cw#M9Je%uZhV1wa1cVZ4asxw28OUO;y{z&#H+C<4rwZlePWV-h!In!(Z zC)8cobTow-%724yl#{Nc_$#Kp(riJ$(7%da>oJ@5 zvskSPtAQ#=bQT%)OftVpq7=SXEkt_OQl??WE_Jd7Un|=S?$M5K-DV!MzRsu0RG=`L zXP?~3Z{@+Ie;BF5E6eyQzvNZO&L=NNEQ296tzU>XZ#-71?T}`pX2aZc6VW(`P8H>a zP7UvbJzdH}Kup-r=#luyP6J^lEN`S)ekdDLOyv6Ltq~jOONtjI{z0ly;630|Jp z9rfhg{WkwY%@uS|oh?GUs6Y>XY0%KWfBVKUdx`E%Nv_N#qSFfV-~2QzBkbHV`dD?k`a6+Cz+KQ}$ISTcvtdFSo#jj>fyK!h@2|HBqEg#6NF1AAb$XGJFIz>|SN$YO3RH2O(Pt#2Hx|nKC@tP%jqXk}-Jk|zpe@zosO-*HGjtTTbxP*$81^ES*45wQ*#ft0RUiW36})cL;2|I>8<;U>lzVAt|J= z(FDDUB*nZ@y25~#1K|rH9@lS=O!8RALP=WOuzvGTWBfBb45Qs>jdQa< zZsO|ddhU|W{d0WS^PWvuSh;=)ZgY<{;K`_(hKlOr%;~dKG;Xcykvgf*DZBgGvJ?<5 zysZ)ffs||pVc|Ny?aQ67tbhy8O5V~G90;W3d-#SX3J!~VpSSZ!h3oD$^smQHtPrOu zV*JjsGPGYJFRlmBGe=cbj^YL%O-k16Z;n&~o*h%K5NWYa0zTR>&s%QiD%qIxa_U`J zIFH&rHmW`2HW288(J5vZQ%Z4WD|t#GDxXPmD3cV1GvG@T1__)gB1Ul4x4oe# z=q}s@1O)ZBcXw(`xPi9<-PuAu+U=fKM%4_^U2w5IXN|M*@PR|iPkOLE!njt}!GRTK zc6L^BIxo_dfPf$Xe?Qq--+P`x*l~@@Hq+WUj>Z$Fcp2mUVV_wx9{Ju&Mt#hu{SJb) z#5ue9Cud!$@rX>_Z(+SQ{^5P){j$vC4M`!)r#`t_2JlmRqenM-x}Ne4+_?5bb>oFx z>`qOQD^L$$@lrWq*4#!d)v2PIUXr$|QNn+N&)lUnxt-iE^c6%&Zm)#dWNxWdVyTfX zc?MTSp~zGvI(zvk-szWmUL!>Wmk}@#AP1xg$r~I@Zx78j{xFFp$%#zC%ma`!4#oNI zEGA!x36jqbg_jX4!LTj+0*Wv;wkhLXhKrYO2@cTH*LEldR!rgUem8zXk`$xrSQ;;)z}b0$Btq<$#XtZ>Phg+L${ ztdm6N+cz~Lu|L)?@w?VF`iO&!MV6u-AC)qu=dUe>#%W97ZoEg^-`~CH#k&_4-qMe} zUvh_(8n$Rpc*#RI3mp7D4UuxEA>GTs$h2#VFbWSDiP~5vjMT=A*|z0?7K0jnF!Dce zP{@&^GV|BU(mM%-XP#{DGc)9n>j$r`n_GMH&aIh!S*MuiPUjRrO4m@tH=>H3M#B}W z#4hC%pKt>eLvQ>zcYdA`wFUHjqJUFJ zPQoOiSl@0vU!4@oZIsJ;uq;eElr)JELk`?2>h)bgXCpaicg9X>K&qNYDA;kO>|N$V zY;6KcB8BRUuaCxCD$P3Vc%uzeb2tI91~K6AT3@bjQN8}!cx8&i#7i1o7D}4Pxr(d) z90Z8X*h}P3&#R*A-QK1AY(At5!%}FQGd6b)s+HjZezAEJ8}8kU_F9h|s@*;`(%0=j zV^B|ZL|G%p*0iaV>pr|)lm`+ffN22ME#q>l>LPy_3o8)o672>txtz;*i zIBM5*6W9zN`W|Y}Cho={LJD#)orpHmx6>bxO2K$;Has2W2vDt`%j_jAM8+T=R**z$8p)M zsI)1t`4%J?lKB?|650pGxWgKcdgP=UV$<4d3|T`&=_3z*Z z+%B{jHq89fl4_6>{@r$BhN$UUqb7nX^<3g$c}$2LOq+^<@-l=+o=Zt2SDzt{br;YV zXWbZE7i3%}yZtiusKoO__r13@o1tTOZqW_cq1Jc9XO&UwEOnn@d4- zD?ddJIn0Y5j89Q(3IB{F%!UP#8vjzFTq1N>R7;Zo+#v*g`bVAtzN1x2d++V0MH8ZaHF)x&tX% z6$2~;glKX(u6e=+_!S)_y5!*0QKQPUmS8w>x3f#S=s@DX_QS8eEouaH+(}bIeLVt{%`ECosJhS6SF*BePpm@mLirpmv%4krzQ8dCz^N6sF%dSpzw-y(7<1K_?#Z@BXhK~RZr z@d*NmiEEAhFvAof!h=)`?~tRRCN|Dzd@tTy3!Ql1F3^Slpb*ut7} zrmRD_ify<`Y&?ZNj)_=Pv^IyAjTb~nN8jI{$)Xw%n~)&>oXl&DQf#*QLc<8>kE**P z8%WiijD{RIsBVMU(3jT#U7EZw-RDBRI0!6W_;l?O>(x^^qVGB(lfQ$UgEN+#&=MQ0 zUPqPfkv^s?mVgh5j4+95G>j3;Q2wmLxxk8B*`pi7O;`p}M{Y2mXsk)9?tT)}>&ZSGQ>xIS{G}!uAS$%6qGh);acn654!ZDk zO&-M`qHfy6RcH}f1ISF5ekrxx5PP43DX)-Jt(Z<8bvhkkH5@=1nr;3?MnSpk9}S9} zmQI8dE?1M{V=2!Cihro9S2oi8rJ}{#F9*5Mx?~V=L^jJbMpq}PB^!c3M7%2-A3G;a zeh3ZTpRHW^<*1kScXz`?<1#;Y@Vr;dS^NIG$vJ>RYcgFXw(_G{@>S_x*Y7>REpPWL zPrZ1Z-S=lJPYVHWTFQb~{l5>~&z1*LJ8!A|ucE(Q-Zyt;HQFr9%+0ysSBwu24-b15 z1A;cc|J)DkLIZ)guZ|7Ca zi_kk%W4G#a(UIwUJp%3FsI={au0}%5cyeZa%>#HaGQTI%-UUVY31 z<5}2AKs1soqmvY#tARqP4%_A2OZMhId8cr~n?jwP6v;RTq0ZHKTh7F7CR~=vuCO0J zT0NhtjwtDi6?Uvr#vdA&5{^-QAOuKa@p}M2gD0gi46Mv1-KWprDJ36RKn$^&QB;Pp z;lCI#w&T}KvrO;&L3JaWxPQc7oj;}5&4?qL{;5|p`p+XNThRP1Dq2&$rs(GW^)2cr zswiqqJI8B2N8(Cl*#??r0cDdF_o$n&UyXzkU(I5LuE5fo%u*3)(v4xU%i6O)GpEvX zm-@%|ob>UI_5%I*F^Ph?5g3_t_40p7RWzmgdI661G1SdQ(>OT(0U~$O7;D-SY69L) zY*(7>SaQC|NA7@Isz(#36~J$|vtBDJ>w{4^jM>v~v~s;rHX@YP@+X<`VU*p!TD9y; z%gcG&QiBy%qpn@oK@fSG&pT_t(`ZiOg$NZD)sF0*OU?^1luM7k$lK>coWi8j%lPd{ ze*G%eD>E_12^?5*^h4CgE$OVcvZsg8T#`em~EL)9>p4yJLM%5@$JiyVu`|z&Yvg&CZT5 zNNcv{HsSR6{%hAqf3;4c(vN{tshpEN7)5#a7UiEvO0{mV7TxIsysE5RZCh$~{0@Kt zJ@K*4iCBS!*2y#?26Xom;i+9_p;xRjj;q41m?piSJ0~Hd;Iw6vPO(7R<^*5fa@9M7 z72>_aiegtjT4Sw8!;jpr17^$=2`@ zSImj&?4UqLdp8>QaLvtX?n7`~Kshx)qvtNIYm0F{{S{1ykog_ye;?-xq z(uuOD)N2maET|1`%Oh&h>C<1e3mSOvDyyKEtM)$&=WX=kBoxkxa}&0V)AY>S4LAMx zTxPt@lN?EM3vu1q134}Bf-riclv!aVh(jO8`R#>&?08w#>)3i1x`o90CaEtfQ~k>Q zTA;BM`uS8D-skQSIRcv6iZ~Iw^uXgm+l7;n{o(-mB21VnO%xku0rykGzOYYB-#T9j z@J={6ppjkig#o9@ZsopEsk+&AsaJ`2tsm-4zR-EkJ;8U7DdHGP0F6m0VtUV>UVGvD zbd$%a))+y-=}gY%f7RM31)O%(ezrk)5ZC%&jOd~s^phq3UK_#kPDc;+{DsfG;H{Gr z|M8P=u1p7=r;ieZR69$f7>x4@Lq!&z!qKtc`sM{LddKNA+hGy6C1>qvTL#25J1%jU zw7t0ugx}#xq@M#K6Gy2`S(00d&ID0mKRE?$Tw)AT#HBy)a>bNSPIUu0BI$P(JYx&l zKT|9>A|lcmUnK=Qg8~}?b|ZW&u(Fi{N5@8vrldQGQ1%w(y-5^_8hHF$DhX{{8u$Fi zO@6fMlPy7?d#`YVsHeNQ-~xEFnund=C7dEHxNf zJdACVuuCUQKZv%?DOY689ee4o;)!p1k;1Nw)Tl%hM1Ud>Bm?L{xTAj+NsO5NKKeDR z2&-Yv0^o{z#Kh?%t=;rL!Y||rXcZUiu(iQt(V&|p3iDkd0W1y2z!jo9H0ZUe+V$o=@@dT@i!Jp1ZSK}WabRH=}ZKu)b& zoZmtcgDrD_l%W!-Z4Jc0z2d?hD(T7a$ zI+Lnl7qNrupPm3oR3l!b^76j$PWXxBzh-jf_S4s+lr8;}ED6VXNxQ zfz*u?@H6oLbad<@A~@c7U4o42!$i0x)N4!{5f~#B#VcoN8lLFHW!Q_<@jkvu7=mK1 z2#962c7+NP`)H5=E0M?v+EtM&cH%Wbk;{@*43xH80Ey1HAtxO1G%jHoacr(D8=uRl z4m#c+s!N!~eKCzG@>IEL9wR=JRkNmpoH9k~TMFNvF=3$Qpeq~yc%t!G*DtswBqTEZ z?w6qHk%hTA!p6gUF8A^TTzS%UF3pECVXT@X`O09buSjO--d*)C*?wd8A{-pO!^6X3 z3r?jM0VzuldD;8rv{i0|LJMA|&>$A9@f=_T?INB)RI`3K1{{?Er z3*j_7EGlPO>eDJ%d9pZ3R;b1vOmlLLZ6_LZr##y`5^8rl=|>H~*184Kz5{h_Wb@

S#rj7F4eLEb<6Hb#Jfu33TO0 zz9corjkXe!G`nCx`1d6r!xCM5x zdbt)_%6Q!7xDt;Z7=voXw8E#q02CgaZ6!?1FSG!=5H!NwB#G+P;2m7mKkzdfW$Xzc zq$@qn-Qyk4Q2SM5^puhZc<|1V5tDDgT5u(}JcoN|%p$iW?&~W@cDDq&kYvSNuM@Gs zUAdN!Sm_2H1>DDvA5|WI&!@|fdWD$Rk6d1|jixXQY~FWX)I04+_p+HD%@lRb`SG%| z_oOiErKP7+qDr*3wnACyOLb=0(E}f|zctJ&Dk}c6Tj41#DY4?o0LMzyY8PrVae%?Y zy`d;aCnr9=P4l|>bjBz1Ri|rhsn(71#6#G~@c%`YG3ho$IR6%G9v>S+U1_qTlEaJ@ zy4Q{xpe!gTfVPsnJ7R%B+&1&Se4m`l=NML+?D`)3G;0i$JRTBx)zin1Nk~YF)XRWj z6dfHM&@ZD_NKs|N&ADD|)Ma)*UmZDLYb%?xb{TT8w1hrmErIHN(fpN9UK}2MGxYm7iW4mR$Hm zL?)BjLSHI0uAc;%CWRAW0 z>Sb&^Jkr=wl?EN@#F!2akO7ergrU1T1Zjq$8$lRCZgS{u7(zfoY6zu~?uK_A z-{*hUny<6Y#M!^SuYE;RciE?h3+d(F0KXl~^{R)r(4BT%_cZSwHC?yKJH$eZ&xP#F zE2hjMnu(dSsS6RZAS23gFBs%C_Q#JLqTA+0{nUz#hCIz-Xv+c~DN_#N(63l}M*|5r) z2%qQ=X#G9tQIsX$A+%G~lb~}h=;VKH_wBccHOG7i3E}%62}#dc#u>HbUT3o>ySh-~ zjSq$|dy%~RL;#5&BOTZ@f(e1`sz8Yf#V^dM(nLTIQzOF_5oTO%B=v+|wt!N5(tB-_O~%{nI3mSmE%AI&S#+u+h6nemmqn`U?qYci;EJpzs=d5yK00#wmnr17sd0aQ z@Q~Bcakr-_w(%5CqVuBtLNf4rGM7(GEP6V#qJm39>dJgn`oVux{6OOR=H{YQ6jy$~ zX_Fez!a24cDJm){Z5{_pjzK?t`uwZ(!wHaljKke}h;@^lp8gw1Y5{~ZfStJC#eN_7 z%cc(y^V^NZa=0Pf;nU%8&~{-A98g-Nzy4(yTwaPS-=8-$?bV~xGc(Pco#SJ;+tV-F z4gw?&JD(c)ZWqF0V~<9h0#C3;hxE(p0Eq^WyIW@10RJlPWd_SDE{*``6F6LrpKHdX zZax#S^n-z7@Nhf!;0%OkT`YR>i;70o*-)ZgP*f;=tFL`mo4I2uEMHGC#_s@;2QWmk z>z4hCE8dx@-0ZmD>G1v?jQM?Lr?En<&fp)(1_rZAN5I9RySili`};vX@q>dZzP`SV zn>l`$Yb?@H2RBA8eXlfq_Zk6W|br$0jq@0+$C5_fG%5$fk9Y z*D2U?AQBF}PBN?PF2I(>_@h-__&LYhPMdfw4LfQe$&#oPt^$zI%xWH3} zz}rl|_IQf6Bdnn=DZc-8#fsO z(wHFpQd|+`K_+3#dji*g>*g2Bcytu+enC9WN*~0Xx19g(ubeyI>|^?6E89&HPlv{o zMqbtSExN_W^ZeR6%zyDWkfxsnzAaF8dvt;A&EM$};zSAd%Hb=W+;$RRVIwpv4O!|iFGN;kZwLlHPGrYEN9)ah7uc^b=J^ikR9)Q zK2c1P8!xjW8>XFy)vM24F2r&o7-IA)iS8V1393aNzSQBPp&{F~sFcKBaONKVn8!BP z`l0i96mukl>Xk&_5?4sQrZ_fPZcYI!qYxacR*?jo_~wQ!Rc*>EZc?Bm-sCL674ox^ zQk6u#B{r8aeV=}k=>3qqM{75}V|!>FmYBUf<)>RQmzJn=v@*hlGGaCs3xPkwsJxmF zlU4cB^+^kiz?y$=P$$_KMlJCxPUMqQFie_JD}RnhY;ZK{If|2Nt@k~SUY$EnO#k4c zPk*|@lrt(M3_1cOf&9N&Xq@DC3JoZ(VS}T^YJV&&E>3!TC!f`9IF%C?8>>~KlXL1J z?z#JWC5HPSi{)2kprBQb4|Ds0+j77KGVsn<$YEXq9|Pct=h%hVm0O%_jQFn*<^Hyk zz9GXQd$|r74&eZZn;4p02lfC z`bOqQ0;Af+qSw>i28WtC^OKDcbxln-;7{K3LYTI2&4SyHEKKe(6!-4#PUf#C4HcEe zWYOE3w~4pRcv6?NfNd&p*T9qWk9aIV4gjBk%b3LV79!MiV7|_7`|saOwBvMa5FpX{ zdf*?ygX?pDk~=nMmHSpI*UHv58tvE;%kBRM$9|zP0jPweZCh_{?kb9li{~0$7-AK_ z0PxrCu9Y-UfceKHhzf~YybkWa^z!|i3v9s94YT*ZkMuIg9`!oE(6aQfAVTQ)f3r{jMxG zCual5ZS-0W5I)%$SzT}&d&AbZwZ#ggpFUjWK13bd-1bP{KJDr0xpA;7Uk5VlfQtR2 z-T|$LG z&DFpkuGNO>fE!QXu&s969JMlU-Ar9^2U$tcF*TVbT%jmY@cx&H*8&w4e>T1qYnthN z9omo3g8U{KDp7Xeh#kTpGNdUGt&8$!)0}CAS}XCsf1oR}Vos2K5~b)v{W=YEOcqCt zQh_mwx!&ev_EDr87xFOjBVN)N0sEj%O)PQh0JxSoRF$swak)wv6Jhv(Oo?96&&jtn z`dxalJL{}zt_mvZ#1xdMK97$D$>4JBj~_qpG_M^~%Y7Egq_rnPVKcp&E^4}&@Ypc3 zqgJ#n;Ap0eY;#5L)tl~>K+?U{ z@?762RJh}(rNEY1&MZ4WQg|XgG*ed7@Lc9I5hAvMV}H2VS9xm8`mi;`wi$u6%6*`X zx$MGoz1}W92_bF${JvC|S3uyjwq_JBRs5iph>a=t{%YWOUJ1VRqopm)j~p*L>^Jm!J@ZW8w;O1RPZtUA8wX6 z%W6hfx^cMI(xGs=wBd2bhKrbytB;rK+(Co4S0`7ODCyBslRH~qKfg_8_WM=o`!B%l zu~#&P0N9qchNy+K>}JdI9oO<@zc6ds@V@}#&1|=qF(jPyX~SCOxKbd}jI{bbpG`%O0vekUWs|C|dmGRQ_HFO2SQ#~!j%*grtH(y&54 zqp1C0V}a*2Ms6d5z)FiU==LbveL@AZY}L->hP+g_e5LZ?x>9r_9XhaiGPUDD!l4)c z*4#?M3JVb$3r*V1{C=`kZ=>O}1hUh`9gAuw8^blA2=pAYYvi!TZJa=@j zBcjSTWoS7vVanydh;ubL31>JERi+bhkrk%#9^@;)zjfXDp}p_qqM0*8#orrJakqHj zSG04R%O2FPIqgI%qLA0#QQB$J`lnWY{yZ+s3^~w8YT^$NH-SQpN#oB$w3EwbnYDQq z$6@Jv11Ty+#|IKsnjcEM_)o=RQxdeDz>Y^(_@yfRo9GL^ZvrC9QYQH#Rq0j6EG`%L z1C;Qo-)xi(pd2O22r!fvLa3r5KCJ*TJN7wl`}i9>UxaxYKa{`^#e#1@6d}c!DI>a8 z+xM)X3S%Ch;)|X7JibsG3(OzJOYABdeFahrai7TOnltjD<9HH`FyEv%9iLBRBtX7e zYmU*w9#uLss z?VY29WLL^FqPM+{Wb6{-a`X}=Q89m^{fnHFF2f!WDVQOZgMw%zuOv(bqACUX#=%f$ z1I~~VY!zP3iqc5p#YQo?h{v)BjR{A($1@{zRG8Z$AhvNK*{rHn*5$>g|KkD_k$#2J zBgG_*744pMD7LgWOxg=!P7J1oNvZMl^GPihKrVdT(E==#{J9_6oY_7W+7Heps3P^& zfH2HGryy}C6bj5gfS$zXWMh1Jc}z=*sxMQ}&byD@xGTowc11eU*`!?eYD-CaD2&v& zaoP2BPGTO3gP{<1G{?1#i1TN=^xD=J9=%tf*(JAbgQ@q zDw={;FEEw8A^q6f+1a^p)lgX(mykdJ60J)L3U1w@^mVMaH(tSrMrDEw`qN=k+<#Vr>8s;66%QY z-v{^m2f(N%da>Z9Uu`}(Ns`y#xHvR5McikW06_9(#%;bGHz8Lin>DWMs)>v~69G&r z-FC(&zP;+5+I)NhX0 zc(whlpU7SEI*0L3zs6zTQRmH~ueW=om4C*mw8tWqrPcCZ_!aHI#ss|WF9c#e2Oj)3 zaNen|D0iJ~7KgMUFoa_@h_YDr{Y+;b#+I@o(E6_|mpkgF`FFQ}$d3-HD+c4H^9b^vD?`!y*piLoBGeer2IW4I28L-{!P4S0-ZV*-HE%# z=%A?8*bUjt^}YAGD1IVOC{M2yfI_H1Gk(%#uv>_X>mCMU-{wN3X)C_ZB_+8 zQcNC1JNz&8Ls+HCj@FH`##)`2hT-DwP6k7UX$a#o8kXfH4fLA+?qYrL#FnubXYdc@ zGpvQuiR{P8~Xc%CSfw7!7#J8}FxaEPz@u}t)27M8nTdX@H?PTMH>qB=`JkOx&iT0~&@WNPQpuiM4q9+QA#H#Chm{vdiLTdg_ZJ0jO;hTM5ioI~xf)Qiwy~ju zLxEO7$YH*>I}As^&G#%&?(XuE3iYQTNyBU~kqPibm$=!j0cs$o`A;uT&*KWOHaqD& zDJCW+ruAQ@us=W-1Pm|nWkd5#ZY-J-M;K#LHyrH;{2qUG0B7kA%bO)_P017@O$nEd z_YidqlsA<80~IbKN!9{pybE5cXl(C81i{rR?e^ z=z%6@j`xS%tM7%ers&_`hr^%)8I?iIQI(2KdnUc9nQl_uk@fSEy@hJi$g`gJ-FlNL zHTr_3Sr?qUR$T=ouIFw=<&q2*Qs-w`m+J>Y1pSqA-#g5iXe6xI^o2h*-P|a@o=(ax z(@TB3nY_K$3^yI_j|DMgNCb&BsU#t*CG{Miqet)+rOpfpsKP#DK!3URed$%A3d->`r$5E%B-c|=oBF+E|p`<{V`|B*x!Oi$o3CLp;pG3(p z5RwLzB|LA{OCktqEgaAz;fwwri;Ea1cJ=8Wf{LU*eX`a_$surPwA9`uLkWd3Ph+D! z1~yN6U7kht)_)Ae_cnf7xzA7@DVO)zVooKe4d1*BS5;PMmD$m4ScBdzls1}y{H%AK zJIjS%z(gMvb%=+LL8nD*s*jDXN5bf%^BI|0rp>c)ugYRZ8`JezhzJyEL!U6ZWP@&y zWTZz)m;Ta`U@G?2b5Dph=Q}5X?>DKoM*su<_Y65MD3|Eu>%2z# zT{z|hUfZt4x>hAdZ^dVboq~5>0G(I&X8v~&L7i& z9Xfy_0zheY(EXKq2H*OdH*pcvzLB(`t?g~ozUWsGf|hg`YH3`Ov}T*m;GhRg3O6e& z7PrmO!gs8Pm#cA_YormT17Gl@BBwJ|l9p;t>6?ZQYW!TxV`%#PSpEV=%C59dw~{kwBDCP_28s=Pt>{=R;G zc#Td*ms0nat1QfJF?}ngBs-g9xtb;>H0XMVELQK1t^63hX7}x3`1_r`J&S%A8Kxxj zT)iak;F7h#p@fs1bXez(+5Xz0evzJd zY{?iccoXfhZ&VY;BD}Z$b$Ije{OYb_CS$dd=|Z~g>rL+beJ0k~Di{jpn!X%>UI=8I+5NDra0p?4V2BQ#VoV!BY1f}g^54DH z-g5Lio6%%*(tse_qv8|g48@b+8NT@KYD}3T zWLY_euUp#L8E{?vt6Lm}__wDZ$nA6!l z#uxu*q?sHvk_Vh&o^>-%Tgz-mtV+Qrm%jqmE|;bI0u{wDRaeXVjGyRQE=#P)iuQHH zWbTZ&*A&1pYxDh_=*eHt+*xnWQ3CRiAr78V&OU;u%DyV`X# zR2^9thAWU+P@oHnjHLq!rZg+{t6;w=Nc@I@#DJoEi!=~={i7m{<(%exbnD~Tg^PYX zvdX12GUYB?rh(43VRTdCJU z*k+>OsqQNnL+H1fnk2cr&fmXjW$_tHn_#A&K0W#j3?kzDjjI-Iy5+h(7k5t`y(lB# zaClD;5gTw}u3xUk@%NJ^!DSVaZbsK_cVSL>miZiUfcfl3my?ob;( z%4NW^26M3;-)trOcNkNf-}!DASiVGOW$_^JVwl_sA4BJL%9kat@9KBS$%quz>0Ppl z!!IUf&^_%IpP6VM^#eQk!ct9Rf(t3+*Gt+X{>o?lVDi8LmisZzPrr}(HDv^EFzuPD zQs&2GALKk=zPdsOwWWv$HYD&&NBA$~;mdHXI{7 zkPG_ka|zjZMf<;87-z{S3OOj5qM)(Sf?D`Q&v<3+Bh}RzqN5(a0I|hGzSZ@Aid->g zgUkKK4lblS_cxpA*AkK~zzV^isUK{~{4Y&m6_n6B5osA7K&X<$F&JIfU(LvyDH4tU zxsbyiO*g@7N}M=oi4VcQd~AyUExFg_V`wEQ;qF3;$!3hla{X(tM$*bdO?yGyvl4cGG$2| zHzKDtI^jpEPt9A3=)%fUf^}u3eK@nfzoiv*8uaojyM)j^Rx2 z@*`_u=#S9h_QFC7xl?bp=9U&bv3>KpC9NRe6yUs4Adou*)&ofzV%vgq_m_gbdGA!) zwve)_qR4}|{H@RMv>sVdSIP(D!TdKdmw)hr20nT%zA4ek*DXCd$$iKMj8U50{=dQ8 zQ6H%`D=~cbOs>rsGx0(Y+K)?#@)_K0on18^J<*2174!8@){5vnU#99Eos(sI?-DnQ zyV_vw4HxLyweDBW9m-4HI6cS{+i^)k37USy94yxg*;7%j(>$DfSUCU5AHg}*u)*(d zeFnMTkJD5OyGDFIwM{G%uh&J0w8kClWf=Kkrn2*Ru7F78h-`9yRzgMfDO+T$g+ck2 zdH=TXCRoT*20MD6MI8W5ofw1~~vvV2Dnx|N6m+AI%es;o{ttR9gJYy6c* zbndqT;?v=or12+I25w}@6UnOYF$i1QUWjLP*kMtF2X#Kc%r0dm|P`Awg?A#hk@*4o8vzBiy|2|SzQBk>){B$d(Y8Nm7{m#2#5w(-E zIc_cR(x=R?y|ncmYwK=$W||zREBX~Xun}5!&8V}RskR&;pDxqy0mKifoQ9VXL6?}< zH#ef2*>3Pza~CTs!oAt0rO~q)lZQ;N{el9@z2yI-8kTAjP&s64Ka#9?t$axr9p;2@ z7p_M}YjZx33VVtGJ=U+wO`5ysus?AhxC#V=4)-6IzsNd=9d-n{3JWjZ5C?j1?Ig?d za!r}IoTIOPFtJZ=Wg$Wlfz6}Vu^%d{1MVj6^)E1>@_sgZHpM~v0 z%%{7Q`!+sICrtm5Ph`Vib&MA zBgtIfw&2ZYy+~O$@u&vgLGgHqdLurOi-DFkTl`2fD_vDeuXQX91_;*$EX2RBk_raL zt1=z&j$mOz&3Jgn22Te=zbF!tfjNI2hNSzq%Lt{>!Q`UX`2r~kl8BoLF=h>thDC_Q z(AznZG1K}rSo31vE%?%>bq6eYK<*6Y_B${C`aK_btzzdzs$nNiO~Q~8uPUi%VPL(J zE33AQ8=53P&y||=M*l6+7%QGIS%Ed<78eU2LT6rsNf#Y?`~24gb?XH6%C{VjFUme@ z_vprNPR1=boAvz;3ti%JJajaZJFwhfcA8I^m%XC+2P?KU2ccp#Xc6x zWen1^CtRMZZN{L+=fv{AU#~ON!^!w9DNkKg*??x{X6Ztz{r;@d?e1*mBq7cp1Bwfb z|8~=5j}uo17Mk3U95D{)dZg7@P8#L1-N?pBR>?10%(k(M06<~iWMnyCYa;^Z*QyYA zT9TlRdOlz8fZh>VNd~SN?BVJvhb-SVDZ1JY{IL@-6;;6CY^HD9=>B30o)3(TB*uQZ zo$B*o5(`ZNbt>t^{QOXcRnDN5^o{=giS$F#SkV2+j~vOYt}faA=A8<8_@1*j8od;b zA3_WAJ{?zzoAuO&K#a2&FDLPaJNTh4|BmhdVd>D1@JjcUk=k+Xn{(Q%Nz0LS^854arQ|wi zw-)!b9hEi}1$6Zw5Y_%s+?gXbYZ5Hso*miO&p7HsW!K%w5G`lV@@2izn*K?pf`>Z` zv_*)g9?Ec*K%l^3<8V;qbnQOR>_La~vYqogDDuSTdSfKtub)w8sb84an3(WyuZtsF zn4HkbS{9`38{2pG&_oRIU*;lHT)L=Ksy772b*lZYY->2+q~4`9H?E3j>G(vb!KnWp z2V-Dp5o=fe!s@}t)q5RfJI<^%z^Miq*o^ANU%rT10hM6Xm;V%F1_j`fE6@kt1Z$QF z8c=le(1YYwW*-rv>jyXa8sTTzrOVY-9ASxssCt4Yw>-32!#|gdX>*73pG`^T(&GM9 z)Lg?AP6}W?y#l|fa^DZ)*cOF zVfh+Fy@;aG{5c_KG(doUnOZwIr?JxM#pzr0*6e=Jso3sXz+c*YY;#KD}+8&oms6wjuq3Kgn`c*5ebcb$*@A(dr_*RR8KUe_|xSw z{1#q7DdoYvmZMaElq82M;enma=tTzl!c#QIi@4_srMXJsz;EIlZ{h zT(Ck5ef;>5wb1)|yEy6Y5C;_Uxn;k3=gsh9|6H}jaIalk{9=pO$rd6GQu+%Ri9_x7 z3%7Q5R*#3dQS}bCb@PGirB=BCLKQm`)&iGZc!4}xrx%Jxv(PxC&1BIRICQe;EdYt} z;Of`g&xOXD>65VQmVGNM4717&NcB5t^D?^%QdLz|9WqRl%abuQq%Z|nLs9TA*vII4 zht+W9D1h!U#7W38CjnebL|fZkWNU={_{0PtC(^IACU_(- zFOMl1A!4KoTc$>DORG+2Q;P`Yl6Aj zNY-|HG!*8fd|JjEKH{f%6;#;~k-K<@QTX>hb_E>h>wV@V?MMKuw}rArB5*FD#g{N zT`4!qu1-c3-bS_aOn&-LLJT~k2kJuUB z`K>j)!Q}+_kwgbs5cH3wQ78`>kpNi{;es#|h$z~@gnG3k=o)tv?lc!3Hkxi=!L%@I z0sq=?BQAM6yLPg1DADy}1D1oalK?X+h636NUI>#5Zy@+x=`R~^DHnTYX0lLq*Wi>| z!asgSx3u{NWe_q>f?wo~^YKrs72^{UVLLx3#ue+CVp6fN=2n|9A7C@(Sjy+g4Ps~eXCyd@}_}!9_+ct>4JKi z)pRtY={ANC2eZ>GET$Y!n*l7M>&e^=OzF$Vh7FFA1<(V4P$YddsQGAXZ*P&?=>v#Y zYoXa=N7u1=yXdZd^+pOngc}xIhlgE%gqaQiB1$2L`P${ZLP!+N!zD`M_OM5p?dmtC zvpEIc8`#vef2x1*{GKd;^*;6(2V~kH0#WquE%UfTU@EvE^ci9o^!^ z{g1i19P}_ezmzFx2W0hu$;jrM#DNFyX8A!%+-u+bPj?uHe)U>p@?wQy1N`QoBiFXh zw^Ov=Tl9K=u_d^7B&Ze%oMT z&r@RexZhUl_|p;Sb1BzW&PXHvfYs5{I3?ytW)M`_!XIUP!J{ z@hJL^18rcfPDylaVT%yYL<(CBA$pQg=i}d}j{XuDSb;{si+zM&!$`2hJc~xt-xlg3 zR(y2o@%Z?Jww9-wm$?xCxyjf6R2roYvtZ+3(juhC<>b^4p=FFlm{<3Hh6_$W zz_`B*UW}b!AwUS!7f17BGVmAt=WGrf=ubSo+&|cao_y&%k-rUB8iAPetZ`h6h1mI23Bm-T9KXN?QHwx)|j-A2jNQ1y*CDbdG1;sia7Q?z-+ zG6PSsku|8S_{Ii;*5ko=CC)dz;LK(#_tM3cn8;~?R)&_uEuMc(etc5I6J7d7_d z0&vM9%*{9CtJ+Ho$*N2Mr(?fy^`+%VX5T`S8?fv6`t_@VR&RU2jrZS@V(+cIh}kwj z;qvlwfPwKNS30O;@AePPJ>Bxd{RLo}mV2|X>3w^I0{Hdq2Pd=8xT9f{`^1V0uH!3- zU$$p&P_>`Bo=^wfx=0--#2qeOw5N%>ZM-YjZQ9K7Yg|t?oNe*q2KFnZA1aqL%O18h zgN6WgDzHAPwI_A78gq;0Xo5K2j|tOfv}-J@7_}Gb_BH!&qEg1QM$4(e<%_x&r_Q zfj|^stABcSW(qT1ZV#}ZtqS(onWigT19E@q5TOv@TJkQ^_&QW=G2FOQ(E?~~ud+I> zD1i9o@yW@a&dx_2cY97g8yQwxdwT(cuil5r1;gPlf!_xZjRUmB465}+(-YfART1>Hj1k7At;ftJCn58HAiGi+4^P-HqT4k6;ChH@nRL=IX{ z^y=;B2q@%-vT&fdJx~v!1M&bw#Bq`3*r8fwXaG|$(sIl>Y~1zBh`5sIABVf_>QArB zZCK_p+=|_gI;7lRo)n!s-#O)ensLUu6C+=Rskm-;+$OaoKwR%>T4TrV(GQ0;%TgM9 zJc7>*RVrFPkp*3#iYHQ12dPsi&&=BJ35Q)W#Js5cy6FBg$gh!;>BPu^CW!|GU5t9T z4-is9p&zK3Y`rnsHOt4pO&=aUjq2>Gro0`1+M9ESjlQg=a;Ww2ztC3A^Uqw7p!q9m?<-h@EBfAAYQprduyx&3pQ9SJn1rFn+FdG=CTOyS)e{e z`S3UExB0&e__&=|-$0+l{C1@vNn_>y=D-RCr&1`;5y=k|4xbg3QrN)pl=|7=F zmxmG3KQ^To_qFniMU4#+W2|&vk2)K;+ z{eypuF>Uw=01|<+U@xSGk3k&sW<_KJ-~|5LsUIxYI{3dT9w303bv9{ZeufuXjp?#L?W+{*v= zQ<`t@MEEv6Wbvy4?|iAfe?Ja9D>X?N+eC8Mt;`y{PDz&v5c}}a- zwrJEtJH3)4MorzNk&Fg;UJyw%Z>Q`q_JiwRz>lsD9ujHpuC=KBeKD6zlLzJKr-E)J zhcD66VkzyTsXgPJwyWQA@CT3M~;wWo$`!8#1;3B}o$g1Sb$YCtRWm6+H zUFJ$qqC9k^_oejy`ivOc&NEp`o7cuEu+zx(J@&()1pk^9bAbKfemP?zZ7Ape?z z>e=9CEd75UtdkX^$lqkku}nD1Mjx3deWBBT^$A8RB!jQb9lK(VS(Po3nx#g09T6-P z9!>)(q4MDsTVE%z>(OiKI>=cXx#sN2ZZ;R&yzR=KvUnhYj%JGjoLm?T=DhjU%N5Wb z_?%dVM??_F{Tv_f$`*AKIh)cJJ!n1I+TWMjs`f_LH(jp804)a~X#!eN zUL9E{qvCio9#^Fu*X;fM{pT^`!Hk#>-8kIlquHVyADfeFM#W+P9n(R;fgfNAYg;$8 z(ihY8I|K(f<D`~xHm`z;5Sch_gTb2Sl=5`_;RNQ-j=#SVODT10Mk zYqqzx(ph_{tRznEfRlNq<6f{~IZ%9OrjiBpL`+;fL(q<7Gsln1>Z`|e;8>Qh6E(nB z`#!Uyo+Y&RMtWzi#_9wa7XS;mIj7>$2R<&qxoPz|(Q9&LQcL4nUvL{+2L`2`>GG_B zEnqP&aX(C-5lhDtc4y5;m~!vrYaBo@uUzOh^}kJms;qu=Dp1n`b1 zgdG`^2CJ;c>ABiYAsUirIzTUHk1lF!`&v;^ahJ0^GlK$XNQk}gDIWD=lp z+kYgBY1Evo$P?&8zaf~^l+;q6@iP;|wMgZn8oAEQjH+(+q)t03t@ij^jE}%DUR<~? zn7owI(YJvrCl_j%~>(KGIDEo@U9 z4ir=WyV9AGl)t@sy>SAjjJ(~7b1&;x&BTS^X(~9lm(Du0`;KK=)@E)#Pfz$3~#!T3$sWB~yBt2;MgBYiCXs5uqo6 zp{2xJQH?t{j}u5`)JXP3yz9uzE7Yc4K#1qYlyp@r^cJ0rFTLoPkGE0^t2n}V#g2U0 zqa5@>bn(oCNK$%r$^(O^d~<9WjHOG7uL&kpEcxojd&T5XPpEc!(<=pK!OTnzIS~h# zU}*f19Ck|NL;iNUlLm$|8_NP^{93YT{j+}h=#bv7NbT0+9 zY=}pMk|*Wz{IG{SCo@hPL|n?)W+WWjtQg|TXnso;cf>w(^IeW_ZA<XFzsY?i8>SA9poXEamYN?bbw@X5@;MX5>29>!6m#WwAX`$h}jS0#VNt zh;kc~Oalh?loz)?=&gf;&EsKi;1#4lCVD;#1zsl8(9Mx7RIAU)OJ5T!E0zPl<(u;s zhxMTpKpJh~aC4=X`o_yk7?_+$jhwsKefR&gTmFOb^7Pw$1xr6vO6_i{v2E|}u05r> zd_;5EiTUz6-C{VEim%<`cx?~}6}egT?0faWx$o77)?Hq6KC<@9K5pvcdmv&&a%(S4 z?$0Z#F_e}n(*MdemD}V#aaHW$_C)mI_5|+=OotIB7fhrnkwBy=ar7z{dVhNbcyai= z83TY18yK&^ZKRSA?%`A_-Kdzw%Tf)o@G`r?>qjc&p^ykK$-8=+w%eQKwlwLxzb_&< zXHgM(lX1ni{)b=Hu67HH`J32Z{YBXbko#||nfM|ijuP#c+wbBRxgz9WPYWlgj!W4P zN=_DQQBd@;`^;ug&JtQ!*z9F!xD+T9ZV`p^l1J4t4OJsKaE4Q7Jrf5V*@GE-OW}V~ zPy_X~znjT?uZwsN8lRtaTA`*N|5y0>3`w6)Ku<1JQOi2Z%W0q`P(&8Mz^h!#s~`)J znok`5vu3!xx22#W&vZ(lGftTNm?C&8lLtG2S;@X7bO@n!Qe~6{gAxdJ7a2+tv=eC= zF^YS!@r;D%s_CmpwPK*F#|3aKd6nrhx!?-wcp)IFQ0u3vAU-lo3#J5qSz-c=$ENtE zDp3|H92_fpt5+{tX;m=-yd5QqM5%eH{Z1CZxk31XJZ1_pd>-VUJTdxs z1tO~7gpUE+o0tDw(C&v93(5Gy^XY>orsU*}&CZ!teU>^chLmxe0z$XDbSCN!^s$_- z?1`L3{E*GxGZm8%Ar;&=Ykrrp*eOVC_@r!o9fd`)s7Hq;?r!N}kdsexpWBVdY5_ET zVKT&F@HCViAa?%mT7Ps)yK#OabbML!wAF3S2rgL^0fRD8g|bETAt(K=u1#9AG=px0 zrtN3Ho{F`HgE~DpxTFFIf*x3`3gqr^zW=hly<2{`qmYEneAOv)q7S7_8XgyRdyepX z{v8)D@vKG}GoQ5xVMiWY@`AkX5kI8Y_P$+}4PW(JS;2P!Hmz667N8#N*dfrBB8#IT zW_kJEk96K7*8gfORuvUJU(iXgHtHYGaZCt;xxW#~FPxfr;?xeP(}VS_n@X{G;)=VEj8`RYB*2Tz;tX!r|*Ev=3JZ z6EE6OcO^P*6~x9Ui=Q}Xe&~+}#Z^@`BldU)PS<3**7H(?l9~1twFp1+wwP)8q&O`V7vup?8e%{u6(R zF_xE4Ihbq)Ew`-gx)PpLLVDO|kdzc^Y?SEkR6+~f*J*S_wVgSST}@$j%0nhl`R`9D zXNt<(B9?4Wh|-&?GBG$JGKIF7*toEN-%5zc6uGNH@_)s5xa4f4GW+!@#tV z8P)jr4>EQJ_E9fz_u0D!c&f)c6Y=}$0*O~CenNgqmdBmn2bv4TO=ogu)hE;lM%&cZ z)|us}(}J*izz8OMqCWBtVK+P;o=}3-R2v&64JS4t%+};5 zp>(+9Q$x^3&OLLqnDZh>nE(G{>aByKZXY*XMY_9dX^>{6yID#?q@_z1SU@CIx_eoq z8$`N8LRvt&S?O++?lYhFoZp=9GsAxjGdshxPu=%*NeIQI4Ub!-L23ds2AKtg#Z^(E zZ)Hmk)?fMmF^xN3=7kr%?jBESaD#(#1}8;qxs>>9>J1f)$Xnus+uUC*L}CUKTJ(Wh zbyP5n;P4Qb25p~>S-2|AzFf$}mjs98-oa;zma2s;2wNg$9i}LZ=W)=DMPYeX9k32* zK?q~&a4wuBMm}Ga9SIH+jlt|8toR_5u56vJ7@-`ool__~`*r6t*#?p52u+Lw_8oZG z%{rgI_sZXYOkseCNn1@zzMX)RU|^_a)7Yz#f8Jj ziyWXwj4}_DR_a9hVSis1=nvZ}=lH-aJaKjCl$6ve1Il0J5mF`zCS))3{#(i-RsV2# zM3wtZp;a*I5<%Oz!DLH}MLK{U+-))5zk2sP-&mo==TEu3YMO?pg`1EVWDEU;pNhcb zYwnXGV|pg!V)pjz6MaF0<=Lo?*9niup@3AT_r1;b*e+YnhvnV8qQd_fZCF&CGdEAi zpAjWZxUiw1Qd*fGB0PCg!2d$WF6DVnW|z#PLB}5U2UStW3^hUFrJ0a8?RN5tjLpVqB^fP_AfLPH2DJr!_3{+wTMb?hXM%#*tFHYRNf zQaN3qGp0{S83`iPg*PS*e(BN82lJq%X&{_=<86Onq6)^rVzk(3@4vJmA#hjLZw571 zfk!h~UQp#w0T|`#fPet{nyBavt?q!Kc(Jf9O9bwgd&>S#!XXEAag*Jz(`p?B!KTvD z0w}SJ%K4%4(JYrLbMJp?P&t|yBBCQ-Sr&x7r7AMz70|C_G7-d$$E|T4GccC8`_V6XlGPIB;v0aOtE1rXdC1#@5{&jxGjj9lq<|*ojWjWHol* zo^mOzUz!Up7`TU$i)-}$x$*c0SMhPW^J?qjs;qIVqjHC$p=xhTtlQ=s;610%Kj&1$&Qe@O;efd9VPrBWq|YC2@@j1P;d}&RaTr(amw= zj4RF=|7y@nk=QqQBGQ5{ee@g#dkocr1=rYfS z#bo3)u&VQB3?yPBzns6Qg{&K5zs%?w-Epg>vRWDNrbh@EqO2Q--ezZc-v~n#dVaZM zkty!-GsS>A_!V_?1@V_2mg+x#)L9ZyhPp8@|F~ibQ^wlBq*MB48i!6^FcWcUg@=R@ zHR2ec(<%4%vvju$D_VZ}W>D>nF^ug;F%;~%X#VU5cMBN8L3d_}NV zZ9sc#-nvz9WA3FaWN=7n;PmN!!RhhRq|;&~%&f}E4*|bl_@TXBmztf87%Zku(l3|6 zl*cp{m)On9hL6lf;bD7`+YWG;Bo8-DEY7GO`t=sa{bbBxKr*EV)e&OaZah96w)-To zssx!>Dja-NO@+KVI)~|8d<1%evmMPr2aF`o6PKmya+ZkI}leSe$Qre4jeJaVdAmQ_T!jjcB zQF4eQ?N~A8wG9`G4yW#X04#uKYN`BY>V@ItK1IqKD#*(a^7mONMQ>JumoRT#9H^nF z$`>zgj~seA1wM}~D;c|2Bpi-GU2*MtG>YH>Wm%$X5%~!$QO`65++c3s5HGq#iz)gH zCqa5qR(Xo6xA15*#p3D$OchR5YtsS_{g>AbT!#2MRnbnuuaDRaI-8osnX@DyP78*x zh&X2A3e{K;Jcw|-v5?YRQFlD5mD0pQ5@ROzS3a*x(XUm+2q`*-DsE<`NjHqO?`M%I zuf+|kst`QtwY)V8WkqC9gmSGcZGSm0_oS(*RRh(;zOpKkHJX(DQd%CyYV#WY4NqH_ zl2-nge|3OBZoQpdjZy(!K79Ml+v8k=(K=@L{~eYkzt{VHFC?Ikn3?1f@F z-0$J9tjsnq*-nDw)Zw1R}RrT}sGBDquwtlN*(dbUXd32_7)YHk0E}TjJq= z%_+n$DQjy5Un~{~;WIKaQ@-KVFEd1F3k=>aEPr_~;n_oP!1foK+B@<(5xqdpFu;WD z%p*Ge)APlS)nid%#Bub#<9CY+erHk@jv}4!417g~e1^!e>{OYtukR=}&~#N}#J27l z%(uKA{um*MLWy9sNo)d*aCj7&q=^;Q?uGn17>xx{|?8jKRSWnqhzKi8!$(j$vUtIDXu%n(pWaI;k6XioRm3Q05yGM=xIRPvg3F@W zK!HP30y5zkbZ|GVtipkZ>u#~~++ZUi-`Kio4gT-&q2CZCr^piOe_1s@dj0yQ<#-f_7=-HA82h(V`Z}ZS?(_X;ZR6^u!c{2| z^FfA=J!-aa_dMyHzk_|Z1G1QMrN)+4J6c-wq8fjOPqE2|q*Kh^<>}M09DUFcNBvn# z+a^y{Whe*1;lt#Z+xHHAR-sp0A1-TE-{pnk-zf`qi^=li%`;>uZm)&-`K0tuZ2an@ z*LtaFLyZ%dMdewlM(H)KZnnQ@S1lbvgNoX~Wy06-<*%8^-X1*xop(ZkN*u{06UrPb zEMTZZNvX&)lu?s}_$8~lp7U_=s<)z}suLJx;R?Ru_6pbdeawV+b~qs~EQwHApr4W@ zh#N;ODTZZ`Q2w&w%dFt8Hr8cH?XGrSgFN<)2f^Jd9D3+&O+cPBOQd|rIBJJx%j?|L zn|fo-n4|GMMel6&7XAJY{@8NQF@pI}2c1Y{B1;X)D+wJqcd@Dq6(Kkwp%b|&%25sZ zO}{>Mrm$)1=|X?eV)#=?hMQM~%je^UEPdtH!<~I8+7YcoP2{bbk~bMM`j3lC$J5U4 zFO+5`&VlC@XUfogwn|m%xS>muZIpY<7yk6ij}jt(|1S$LBp6%tX7Px=-Ec~i^oU=9 z!yZKB6HjL*qWkAqeJl3%drj0+%0@2}?CLY}TF_{KtSGuah+ zXJzo?c61o1?5wWDxTX9tf7n*Hqj0Ii{BwqW61HDrTjp1XsFVu#o^Be1u?Yv_aqT7b zYSuPuB8qo$W%Cog8{>EGaGUAnS|YZ7j&0lT(&uz#np*xeY=&4-1@@1dKx_XvCS!Gl znB>03r~JAokJ)Q%tOCMuM;K|U?&%km`COi+BiCF}#0 zx-n`;^d6Y-A*r77*66gPSGEF79Xu;D)~KCKnqp_cqJ~Id;q>KEFjoznkYPaWP0wi~ zv?{Q0H9Q+Wv7f&pxyCa}Ck_KGnbQa=mA=l^#mDqd)o`SaWlPg#Da|!5)!{(B&W?XC zW@v=nE)1LRnW5@{&!aozD$Dl0Toa`i*Tl*g_IztlU5op6G2qGDRy8I+l{Lgzp+ymD zgIQwEtXS>4%IKIB_{{O@PEw-t_xe2(bMOarcIDks-g6xEkFCww1Gz`9%VynDSe%u> z2xSXrWQVq~`;9Ry8}<_VL^fzCYd$6-ZbPUPyL?CX)a zCfZ1Sr)24X*`zN$aw4-68gH%MA3of6neNf-wlnXsl$H6vV~4ZEi3@=ep8BpabyYozPHjR*GEkQ@Q6XW0jCXAKnp)}HQKO}~x#bZ=Tc7`aKt z(@`Ad{is$Y|1iju5Z6B&mHWC_Tr%{)pcK9j=OVk8|1u%a$emm9iM-t)y(5lC){w(i zbqdpwDy06@G(56Kpw&cpgB$!h3$v4KgECMtiF)ZOQiPa_v<6%0UOo}h60z&$Sj7$7 zEqUtu{g3vn!nW3+*R5gnSFh+yAhjc(oo#8KMUCpGfu_a}qGN%bUyQp}b zY-SPWupH)l9(Hp^o=DwvmO+OGg<3FkRDMT5tkGH8XMNrdIC0T5`rm7chdmL}B(mIC zHgz%`QsOWv{q)y7&GUwEb+r1~dTJ;kNkmnjb;JxA$@Qc+I+9%MIXa8*3E!0k z9T?)yM45UWi_Ue}(4ejORQ`Y`|Iav+?5kx-2^wfYhWpICk)WjE*v1MNdnS*`o0h%G zw;Uk#`LXkf;Ny{m7HJ8Mz5odIB5BOE?GWX0v#HYdO!Y|Yz;zTS-n3i>2Vtv2MQLGk zCya=bl%I~R!AauQbTf?)4AiOBr5h$T>`t(iioiu2SsKwJj*=srbD+Ga{g^SM5q>-| zHPx9+j@Vw?{9+ws0!u}7wh6yCim5Ab#ivwpvk67B<4r0U8~TmSiZVH?ovE|fVikzI zihd$W)%FWskiI8NVzCs!#@e7S-B@pt4$R2^pv;C>3M~-u6qgzb3_oTqG`?s*Sc3=S zp~#W;vB08csf$R_+s!Ev4I2K|pUk<*4Jo$f`g}m5ukJk#3iP=Prb3#La)nsv!K`3&fm~rpS#+@dw(NUIw=#90 zEiRKDVgEOmRF;@sP9-aFzbqS=IMP~;J9lcI5kd3C!pc8WgC=h5_vEn`;rlQ-@i!qy z2}K-v+qZ__W%3A>1wI+Arwkb)UO=YIoS3A-CsorJIV|$&+m*w&b(3fM*j|qCO0oW| z{TsqqEw~@3AQn**5bK2sxqQpzz%yuEMtgtUl%OjBeG-2$wR2ttlV+4Tx%Mdv8yH9m z3UmrQYb5X(=`;CC$-6U5=8^rOmb+lWVcexjEeX{cxAH)jo4jkjQweI`tlLU`x-&fwt98x?H1($(4kr!Qrf8nEL= zOH=ly!2q^I-G8-GCN~Tx4<8r~P?QN}FYzjfoiJzO{I^rB+EXzRBa zO=hMz7z;`uHi57ee{OxMwrX7&C=S+h<3pT(W#&UZgZq#G^U)1D&e#R5B=#iG9cZYgU&1UN0@IKtc0TYN<#dLbSl}oF9eyh)tN6 zxH=Rxg{^>&2o-q1um+(*-h+KBWSyUOI4zegw9Yi%P2C8*zZ5$_c}#aMeFI5i>4hmG zdJRz^*_x|9s2$orWW48u5(v_VY>5(WfQ8K|}l10@G7<%tSw zIJ#qtaGXb@?zH6+q70$Mo0)_+AO67_T@X}V-C8&o8{-|ZO9qIe0GM!Gfj~e)LcL^4 zj#H^r-^$zj>)xItK&S-*NMT`N^k=NMzY>`Ng4*w~u^d<@6!~yxSMzIpoKR0s&(y68 z4Hq9@+U}|2;m8$u-OMz2fp$yq8}J9yU-)JP20jPSs>e$|ILV{6kAnW&MM?62sqp~* zGrjj}Ap3_uwy9?Qu_DV6J3eEt$Fe11v>xZXI<}P@C|s7dT(bJ2E?~I^<|y`#bp58mQc(I#K%9b1YBA78zYqR zUgsKD2nq}LpPz#zybis8*UVA&OimKZJnU%$u~bpF)n~)Q!@aOJ-pRLRZ~6xY05OWS zwKaf|vvhMiefw#WjeVzdRh%5jwr|?zml^F9r(1D6@fD`L*C62ikhGDwx?@!n)|9@atsoNq{0 zHC;)uFYny`O662jtfBLEl&QVZ_^#V)jQ(+@Lxpk6n5{*ZBW3a`ujwx8-uP0o+JM`O zCwcj6huJ9KCdvDXajt^7qXZT#u3-)|UbL~k20GLvuR1*WB(^9`M6E`)3wQWn+VH;zIP3d;;!=dTN&V~(A0Qi68Ks%bHp_3j1r(doE?$>j$7KcMMe zrZyNghT5vp{&+$YenH-TEU-=R^mWaeg1_E_z#u#DU)#oCa~@~)+V=;9smJv8SB;kF z|1iok?Wu}L@2*aEUcS4*2I4+`SI0$j2U!BMFxp1fmC=Sd^IftoZ`;<% z0CvyeV#5I}{=YM*&$gkN+1A0kCviZ?lQRuF`E>Q;rs3UP!%m8!jEu|}kk_kgZN)wx zr~&eh0GFUs|3`FmwEz7EgUtI(c#?1X==nLA&u?Ma`wqD>db5)g4@LeS9hJ@#9_=3y zS@{igN0V6AGFI-{nwEVJjN)sc8HtHR!1saq!~WYdL{nR+W`;2I>$FJ!>1hwa8SwEw z-w%A=JYDOFmncphw(TDnxB-BLpYL3sj?`?AcQ)H7zfzaF?X{3(c3Ov&CPF%x?!@v} zCE7MKInCWi`9vHul0BkK%RdKUa#B$;l*~ERc%Kt;)%<+p0Did3Lo;-k;Hyk8#uAMD zo5|JqmTg2g22#y}Ic)i%p=X+YG z1?(Le*7m)Jy*z=?;*h*ves z0_de=HrS51u`OsZTl}p75)vN|Enr@8F0?wT#D$ZXKLyiSVhlC)_|b=nQM=g)Q5JA? z(0ySVi%v|0;DQ7V+uk!xmL&PokRu;T-9NQTw!icA>)a%R;l(F)t}XKj9OHso)I8G3 zI=3NwZ>p?O5c%a(#D1@xVyV|6Bs_D6outRMvcp^GMtx)ifnG#fQ}6O*Wh_r2y!mR`?*@qG9=v6|6k-z_&0?&6n#r>~-tk`;ak?%(D zjBR7{R=gS>9_HrbJFClih$&Es{|99HkC06=?*l|hj1U4sLOs8K;{uGmZ~}(y0)__; zYisKRs=v|E(f3O$4|F*AvO{QfC*vuWLV#8=1nh z79Sy_Jf0@f8RN|!bf;uYmXN|&@URN zmK*_&6@ciK!D~W(ex$8sVI9^reED7CbrmyDhI>ocAn|0idODkpW7IEV=cy*p{ul`J zmtc}INj@djOJ(%%1hnwQUH&8Gj2XLVgY*PDs_xlca|#j_%!&l%HAtkbnltpZBcu?V zCQJ#^S)!l#CwRHZV)QLSdvqO~j?47AQPT;laEFnHCd*C~vVlYMHYh&Sp;2{_t{!q{ zo?M9`&y>DjbO+Dx#|Y*(w32LEY)PQ z+O~tU2_d^WH{FknwPXsx<|C%UcfXI^vIq3l>G|3ngeZ<_t=$V_tak_u#m1!jDYY_1 zy9Q()VI4OkuG!maKm4v-E)N%iH$-Wr{l5aOD}LO-4#rP&W*`3orXm1vJ;<7xdEu`M z`7fYK7zR4>@(V7C6$k_3VE&hNIa@&Q!w6Z&9bIA4RB1o*QJf9{Iw25KP#Y4S@i38;}&iG$3l=H}i?gjjl~ zpLVph{RTqhfHwdj(Xac^8^6CiLJm|kEw_S}VYIv**H(=#OOR6i{0RrQ!+8OGI`Q)m z)Q8MW3X(XU77Jf^8Q#9!*7pgYr#ob2p7RE!h{Ij9I*9Xe{3%uK7}HF~yT!pU6Q zmDHtOQjO18x!r^+X8MSAQGA_g8{0h=93ghY^fdOpBJb>Yo>ys6j=W>}$@q@V@~;{C z;ezF**rPxBU7*iO>J$;Z^~QLU=27~ff($2k7;S4Abn$Fh)F{R2HH%b7#PNk&*_-v} zIZ74PLuim-vhyI7Npwd>O_EHOA_p;>4sE^|{x77&bBJ&H1p)`_KZMaayBeKCY+pk_ zqZY?C=H4?7^|OTsXf}SiWO2-#q&T<+!}StUNnlVc2`ahVYsOQlp-^JS@25O!RZFZc zyjRitkjM)hG<~w-lh_>2T62m4O67j#Q;agOpC_uqz6B3lwWxR zqLzEt7svye5NFn*F2JL{rGJnoY*@@}{_MKq&VHyPN4P*vg#N>{P!#-yFz9kntQ^c- z4TW@fzhW#PwKSQ}4t`e!q9p+FXdsBgrGnhCxUmF_z&3i+)roOkS5Yxg>G&f*^U+ z?IfPDLyGM^*>X+Nth}$ALXneUPY)x0MvkG{e#k~vG1oi(sx~W$jD9Cn^G`IRKF1Lo zUjYWF{|3#8hOc-$-pBwp|&0X@vL-h!70Ql!|nH{#^R6>;OH4 z_^0dmz)y{9_xF;(KBeX9@osLz&fdP|q~i|Qd}QA(ZEtRFO2%K$Hg)vPR+un&e6BDk zc+HUsdzZxGDNdI@!iKQbDb@crWdw+5NHgRc&`Jfz(sSb3b>X?;2N{_)|M`F^u2 zt)$%yWZ_=eT~;~YRav2+ul^}JH8uI{rA8&OlvkDzSoFgQYM>48Ay;zC;gBlSva66{Dnx|HZ*VKPYnnouAh zrcM0jMIa)i%E_-xQdOCokm<2dRJb5#Tp=9Dz*tmBQ(;vYDqyhkPx(@5>JyO+K@pkp z#StuoXtyD%R{o1zf^8H>)NZI$l4!9hCMHU%J1EzH=j>NvfoPm|x*h5Lvz3>xx!!(D zd0E&_-!WaUB^@-Vi-J4!g&&=B*MxxTHq?M6Q4FeJr6 zi`l4cuGgUKk1;04N0HEb$ZwaEwR9s~^TR~S4>!EQl3$0bH>2gS?5|SbZ9T`B6B2D4 zo1boJJ1<_b_P#@F{$#0XvHx^3&E6=`c7!tYB-X$7<1y658--PuQFVem7fw@`#e*lVj|4rshQJ9s~Bk=zRY}!SZJf}7;muk5iU01XkjQc2i&dsXUyxL?I+Sqr22#F=xUTiQEYVY1NXBlHZ}9kcXw_}XVz zmJ`ZrND&=N^iAU#dG~I|Lu`FQagJscoK4-T+r0Bj`G)FH1M!S55hr6T$+lf;f@pK2 zrWnU-ZiCbIXDY^DwKAMP5G59)Iu3DqfI|0j-dE&SOk!8yb=MM-Qx>UqC(N!B@BI@T zX5W6@j=Cp4&BormyTw=H@eegxrX>T63)0*x=9Ge}#)3$KG`=JeqsnZg?ECLO-|HvQ83f_joKsSw)| zt5BeDi5~VNS|1(s(;X}~!^H7JRb7XM)*hK%Mz!oqpZ!kpe!ZuZP<8D!Ewv@`O;!F6 z(c!@!tUP1c#0lr$?*F=;3Pqc-*f~J-DP~XI!(0Kpw|D27gsLD8RQ$JQ&5>`-YO