From 3b2d74b0172ce6be931de155cb6bb07b9ee45833 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Mon, 3 Mar 2025 11:29:21 -0900 Subject: [PATCH] gz: Refactor GZBridge and px4-rc.simulator (#24421) * disable SENS_EN_GPSSIM for all gz airframes * add GPS + noise to GZBridge * remove mutex from gz callbacks. Callbacks run synchronously after sim update step and run() loop does not share resources. * remove hrt check in callbacks * format * remove param set-default for already default params * update submodule * remove unnecessary comments * overhaul of the GZBridge and px4-rc.simulator script * remove arg * shellcheck disable * add bus/address * start gz_bridge before adjusting sim speed or camera follow --- .../init.d-posix/airframes/4001_gz_x500 | 2 - .../init.d-posix/airframes/4003_gz_rc_cessna | 2 - .../airframes/4004_gz_standard_vtol | 2 - .../init.d-posix/airframes/4006_gz_px4vision | 2 - .../airframes/4008_gz_advanced_plane | 1 - .../init.d-posix/airframes/4009_gz_r1_rover | 3 - .../init.d-posix/airframes/4011_gz_lawnmower | 3 - .../airframes/4012_gz_rover_ackermann | 3 - .../airframes/4015_gz_r1_rover_mecanum | 3 - .../airframes/4018_gz_quadtailsitter | 4 - .../init.d-posix/airframes/4020_gz_tiltrotor | 3 - .../airframes/71002_gz_spacecraft_2d | 2 - .../init.d-posix/airframes/8011_gz_omnicopter | 2 - .../init.d-posix/px4-rc.simulator | 118 +- Tools/simulation/gz | 2 +- src/modules/simulation/gz_bridge/GZBridge.cpp | 1009 ++++++----------- src/modules/simulation/gz_bridge/GZBridge.hpp | 132 +-- src/modules/simulation/gz_bridge/GZGimbal.cpp | 2 + src/modules/simulation/gz_bridge/GZGimbal.hpp | 7 +- .../gz_bridge/GZMixingInterfaceESC.cpp | 2 + .../gz_bridge/GZMixingInterfaceESC.hpp | 7 +- .../gz_bridge/GZMixingInterfaceServo.cpp | 2 + .../gz_bridge/GZMixingInterfaceServo.hpp | 7 +- .../gz_bridge/GZMixingInterfaceWheel.cpp | 2 + .../gz_bridge/GZMixingInterfaceWheel.hpp | 7 +- 25 files changed, 518 insertions(+), 811 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 index bca3feddfb..3923b44cb4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default CA_AIRFRAME 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index 04fc1acfd8..ea7b3a2f03 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna @@ -12,8 +12,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol index 96bb25d69a..69b0cec559 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol @@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index 967051b042..e988f1f9b0 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -14,8 +14,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=px4vision} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 # Commander Parameters diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane index 5d5c50df33..8479f2e38c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane @@ -11,7 +11,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=advanced_plane} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index f05bbdcf45..5ff39b3f96 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -46,10 +46,7 @@ param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 # Simulated sensors -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 101 # right wheel diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index 0fec76b5c4..a56cf93005 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -12,10 +12,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower} param set-default SIM_GZ_EN 1 # Gazebo bridge # Simulated sensors -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 # We can arm and drive in manual mode when it slides and GPS check fails: param set-default COM_ARM_WO_GPS 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index cd870caabb..5867b4d3e3 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -45,10 +45,7 @@ param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 # Simulated sensors -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 # Wheels param set-default SIM_GZ_WH_FUNC1 101 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum index 724e7613df..45f80383e8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum @@ -45,10 +45,7 @@ param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 # Simulated sensors -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 102 # right wheel front diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter index 9856b1bf27..1af6e721bc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter @@ -13,11 +13,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter} param set-default SIM_GZ_EN 1 # Gazebo bridge -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 - param set-default MAV_TYPE 20 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor index c798650b54..8325f67a2c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor @@ -13,10 +13,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=tiltrotor} param set-default SIM_GZ_EN 1 # Gazebo bridge -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 param set-default MAV_TYPE 21 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d index 480454d72b..4cc0a02e41 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d @@ -15,8 +15,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=spacecraft_2d} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 1 param set-default SENS_EN_MAGSIM 1 param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later? diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter index b43b61025c..0f6748edaf 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter @@ -83,8 +83,6 @@ param set-default CA_ROTOR7_AY -0.211325 param set-default CA_ROTOR7_AZ -0.57735 param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default SIM_GZ_EC_FUNC1 101 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index fd0954744e..ede64dcd34 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -73,13 +73,13 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then exit 1 fi - # look for running ${gz_command} gazebo world + # Look for an already running world gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' ) # shellcheck disable=SC2153 if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLD}" ]; then - # source generated gz_env.sh for GZ_SIM_RESOURCE_PATH + # Setup gz environment variables if [ -f ./gz_env.sh ]; then . ./gz_env.sh @@ -87,62 +87,124 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then . ../gz_env.sh fi - echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" - + # Start gazebo with default world + echo "INFO [init] Starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" ${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" & if [ -z "${HEADLESS}" ]; then - # HEADLESS not set, starting gui - ${gz_command} ${gz_sub_command} -g & + echo "INFO [init] Starting gz gui" + ${gz_command} ${gz_sub_command} -g > /dev/null 2>&1 & fi - else - # Gazebo is already running, do not start the simulator, nor the GUI + # Gazebo is already running echo "INFO [init] gazebo already running world: ${gz_world}" PX4_GZ_WORLD=${gz_world} fi + else echo "INFO [init] Standalone PX4 launch, waiting for Gazebo" fi - # start gz_bridge + # Start gz_bridge - either spawn a model or connect to existing one if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then - # model specified, gz_bridge will spawn model + # Spawn a model + MODEL_NAME="${PX4_SIM_MODEL#*gz_}" + MODEL_NAME_INSTANCE="${MODEL_NAME}_${px4_instance}" + POSE_ARG="" if [ -n "${PX4_GZ_MODEL_POSE}" ]; then - # model pose provided: [x, y, z, roll, pitch, yaw] + pos_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}') + pos_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}') + pos_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}') + pos_x=${pos_x:-0} + pos_y=${pos_y:-0} + pos_z=${pos_z:-0} - # Clean potential input line formatting. - model_pose="$( echo "${PX4_GZ_MODEL_POSE}" | sed -e 's/^[ \t]*//; s/[ \t]*$//; s/,/ /g; s/ / /g; s/ /,/g' )" - echo "INFO [init] PX4_GZ_MODEL_POSE set, spawning at: ${model_pose}" - - else - # model pose not provided, origin will be used - - echo "WARN [init] PX4_GZ_MODEL_POSE not set, spawning at origin." - model_pose="0,0,0,0,0,0" + POSE_ARG=", pose: { position: { x: ${pos_x}, y: ${pos_y}, z: ${pos_z} } }" + echo "INFO [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z}" fi - # start gz bridge with pose arg. - if ! gz_bridge start -p "${model_pose}" -m "${PX4_SIM_MODEL#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then + # Spawn model + ${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \ + --reptype gz.msgs.Boolean --timeout 1000 \ + --req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1 + + # Start gz_bridge + if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${MODEL_NAME_INSTANCE}"; then echo "ERROR [init] gz_bridge failed to start and spawn model" exit 1 fi - elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then - # model name specificed, gz_bridge will attach to existing model + # Set physics parameters for faster-than-realtime simulation if needed + check_scene_info() { + SERVICE_INFO=$(${gz_command} service -i --service "/world/${PX4_GZ_WORLD}/scene/info" 2>&1) + if echo "$SERVICE_INFO" | grep -q "Service providers"; then + return 0 + else + return 1 + fi + } + ATTEMPTS=30 + while [ $ATTEMPTS -gt 0 ]; do + if check_scene_info; then + echo "INFO [init] Gazebo world is ready" + break + fi + ATTEMPTS=$((ATTEMPTS-1)) + if [ $ATTEMPTS -eq 0 ]; then + echo "ERROR [init] Timed out waiting for Gazebo world" + exit 1 + fi + echo "INFO [init] Waiting for Gazebo world..." + sleep 1 + done + + if [ -n "${PX4_SIM_SPEED_FACTOR}" ]; then + echo "INFO [init] Setting simulation speed factor: ${PX4_SIM_SPEED_FACTOR}" + ${gz_command} service -s "/world/${PX4_GZ_WORLD}/set_physics" --reqtype gz.msgs.Physics \ + --reptype gz.msgs.Boolean --timeout 1000 \ + --req "real_time_factor: ${PX4_SIM_SPEED_FACTOR}" > /dev/null 2>&1 + fi + + + # Set up camera to follow the model if requested + if [ -n "${PX4_GZ_FOLLOW}" ]; then + + # Wait for model to spawn + sleep 1 + + echo "INFO [init] Setting camera to follow ${MODEL_NAME_INSTANCE}" + + # Set camera to follow the model + ${gz_command} service -s "/gui/follow" --reqtype gz.msgs.StringMsg \ + --reptype gz.msgs.Boolean --timeout 5000 \ + --req "data: \"${MODEL_NAME_INSTANCE}\"" > /dev/null 2>&1 + + # Set default camera offset if not specified + follow_x=${PX4_GZ_FOLLOW_OFFSET_X:--2.0} + follow_y=${PX4_GZ_FOLLOW_OFFSET_Y:--2.0} + follow_z=${PX4_GZ_FOLLOW_OFFSET_Z:-2.0} + + # Set camera offset + ${gz_command} service -s "/gui/follow/offset" --reqtype gz.msgs.Vector3d \ + --reptype gz.msgs.Boolean --timeout 5000 \ + --req "x: ${follow_x}, y: ${follow_y}, z: ${follow_z}" > /dev/null 2>&1 + + echo "INFO [init] Camera follow offset set to ${follow_x}, ${follow_y}, ${follow_z}" + fi + + elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then + # Connect to existing model echo "INFO [init] PX4_GZ_MODEL_NAME set, PX4 will attach to existing model" - if ! gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then + if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${PX4_GZ_MODEL_NAME}"; then echo "ERROR [init] gz_bridge failed to start and attach to existing model" exit 1 fi - else - echo "ERROR [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_SIM_MODEL" + echo "ERROR [init] failed to pass either PX4_GZ_MODEL_NAME or PX4_SIM_MODEL" exit 1 fi - # Start the sensor simulator modules if param compare -s SENS_EN_BAROSIM 1 then diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 183cbee9e2..23170a9125 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 183cbee9e2250d1ca5517cd76c5d9a7e47cc0b7a +Subproject commit 23170a91255d99aea8960d1101541afce0f209d9 diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index f5d10989fa..bb2e7282da 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -1,20 +1,20 @@ /**************************************************************************** * - * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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. + * 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. + * 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. + * 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 @@ -43,24 +43,17 @@ #include #include -GZBridge::GZBridge(const char *world, const char *name, const char *model, - const char *pose_str) : +GZBridge::GZBridge(const std::string &world, const std::string &model_name) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _world_name(world), - _model_name(name), - _model_sim(model), - _model_pose(pose_str) + _model_name(model_name) { - pthread_mutex_init(&_node_mutex, nullptr); - updateParams(); } GZBridge::~GZBridge() { - // TODO: unsubscribe - for (auto &sub_topic : _node.SubscribedTopics()) { _node.Unsubscribe(sub_topic); } @@ -68,126 +61,6 @@ GZBridge::~GZBridge() int GZBridge::init() { - if (!_model_sim.empty()) { - - // Set Physics rtf - const char *speed_factor_str = std::getenv("PX4_SIM_SPEED_FACTOR"); - - if (speed_factor_str) { - double speed_factor = std::atof(speed_factor_str); - gz::msgs::Physics p_req; - p_req.set_max_step_size(speed_factor * 0.004); - p_req.set_real_time_factor(-1.0); - std::string world_physics = "/world/" + _world_name + "/set_physics"; - std::string physics_service{world_physics}; - - if (!callPhysicsMsgService(physics_service, p_req)) { - return PX4_ERROR; - } - } - - // service call to create model - gz::msgs::EntityFactory req{}; - req.set_sdf_filename(_model_sim + "/model.sdf"); - - req.set_name(_model_name); // New name for the entity, overrides the name on the SDF. - - req.set_allow_renaming(false); // allowed to rename the entity in case of overlap with existing entities - - if (!_model_pose.empty()) { - PX4_INFO("Requested Model Position: %s", _model_pose.c_str()); - - std::vector model_pose_v; - - std::stringstream ss(_model_pose); - - while (ss.good()) { - std::string substr; - std::getline(ss, substr, ','); - model_pose_v.push_back(std::stod(substr)); - } - - while (model_pose_v.size() < 6) { - model_pose_v.push_back(0.0); - } - - gz::msgs::Pose *p = req.mutable_pose(); - gz::msgs::Vector3d *position = p->mutable_position(); - position->set_x(model_pose_v[0]); - position->set_y(model_pose_v[1]); - position->set_z(model_pose_v[2]); - - gz::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]); - - q.Normalize(); - gz::msgs::Quaternion *orientation = p->mutable_orientation(); - orientation->set_x(q.X()); - orientation->set_y(q.Y()); - orientation->set_z(q.Z()); - orientation->set_w(q.W()); - } - - //world/$WORLD/create service. - gz::msgs::Boolean rep; - bool result; - std::string create_service = "/world/" + _world_name + "/create"; - - bool gz_called = false; - // Check if PX4_GZ_STANDALONE has been set. - char *standalone_val = std::getenv("PX4_GZ_STANDALONE"); - - if ((standalone_val != nullptr) && (std::strcmp(standalone_val, "1") == 0)) { - // Check if Gazebo has been called and if not attempt to reconnect. - while (gz_called == false) { - if (_node.Request(create_service, req, 1000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("EntityFactory service call failed"); - return PX4_ERROR; - - } else { - gz_called = true; - } - } - - // If Gazebo has not been called, wait 2 seconds and try again. - else { - PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying..."); - system_usleep(2000000); - } - } - } - - - // If PX4_GZ_STANDALONE has been set, you can try to connect but GZ_SIM_RESOURCE_PATH needs to be set correctly to work. - else { - if (!callEntityFactoryService(create_service, req)) { - return PX4_ERROR; - } - - std::string scene_info_service = "/world/" + _world_name + "/scene/info"; - bool scene_created = false; - - while (scene_created == false) { - if (!callSceneInfoMsgService(scene_info_service)) { - PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying..."); - system_usleep(2000000); - - } else { - scene_created = true; - } - } - - gz::msgs::StringMsg follow_msg{}; - follow_msg.set_data(_model_name); - callStringMsgService("/gui/follow", follow_msg); - gz::msgs::Vector3d follow_offset_msg{}; - follow_offset_msg.set_x(-2.0); - follow_offset_msg.set_y(-2.0); - follow_offset_msg.set_z(2.0); - callVector3dService("/gui/follow/offset", follow_offset_msg); - } - } - // clock std::string clock_topic = "/world/" + _world_name + "/clock"; @@ -212,7 +85,6 @@ int GZBridge::init() return PX4_ERROR; } - // IMU: /world/$WORLD/model/$MODEL/link/base_link/sensor/imu_sensor/imu std::string odometry_topic = "/model/" + _model_name + "/odometry_with_covariance"; @@ -221,7 +93,6 @@ int GZBridge::init() return PX4_ERROR; } - // Laser Scan: optional std::string laser_scan_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/link/sensor/lidar_2d_v2/scan"; @@ -288,295 +159,120 @@ int GZBridge::init() return OK; } -int GZBridge::task_spawn(int argc, char *argv[]) +void GZBridge::clockCallback(const gz::msgs::Clock &msg) { - const char *world_name = "default"; - const char *model_name = nullptr; - const char *model_pose = nullptr; - const char *model_sim = nullptr; - const char *px4_instance = nullptr; - std::string model_name_std; - - - bool error_flag = false; - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "w:m:p:i:n:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'w': - // world - world_name = myoptarg; - break; - - case 'n': - // model - model_name = myoptarg; - break; - - case 'p': - // pose - model_pose = myoptarg; - break; - - case 'm': - // pose - model_sim = myoptarg; - break; - - case 'i': - // pose - px4_instance = myoptarg; - break; - - case '?': - error_flag = true; - break; - - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } - } - - if (error_flag) { - return PX4_ERROR; - } - - if (!model_pose) { - model_pose = ""; - } - - if (!model_sim) { - model_sim = ""; - } - - if (!px4_instance) { - if (!model_name) { - model_name = model_sim; - } - - } else if (!model_name) { - model_name_std = std::string(model_sim) + "_" + std::string(px4_instance); - model_name = model_name_std.c_str(); - } - - PX4_INFO("world: %s, model name: %s, simulation model: %s", world_name, model_name, model_sim); - - GZBridge *instance = new GZBridge(world_name, model_name, model_sim, model_pose); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init() == PX4_OK) { - -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - // lockstep scheduler wait for initial clock set before returning - int sleep_count_limit = 10000; - - while ((instance->world_time_us() == 0) && sleep_count_limit > 0) { - // wait for first clock message - system_usleep(1000); - sleep_count_limit--; - } - - if (instance->world_time_us() == 0) { - PX4_ERR("timed out waiting for clock message"); - instance->request_stop(); - instance->ScheduleNow(); - - } else { - return PX4_OK; - } - -#else - return PX4_OK; -#endif // ENABLE_LOCKSTEP_SCHEDULER - - //return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -bool GZBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec) -{ -#if defined(ENABLE_LOCKSTEP_SCHEDULER) + // NOTE: PX4-SITL time needs to stay in sync with gz, so this clock-sync will happen on every callback. struct timespec ts; - ts.tv_sec = tv_sec; - ts.tv_nsec = tv_nsec; - - if (px4_clock_settime(CLOCK_MONOTONIC, &ts) == 0) { - _world_time_us.store(ts_to_abstime(&ts)); - return true; - } - -#endif // ENABLE_LOCKSTEP_SCHEDULER - - return false; + ts.tv_sec = msg.sim().sec(); + ts.tv_nsec = msg.sim().nsec(); + px4_clock_settime(CLOCK_MONOTONIC, &ts); } -void GZBridge::clockCallback(const gz::msgs::Clock &clock) +void GZBridge::barometerCallback(const gz::msgs::FluidPressure &msg) { - pthread_mutex_lock(&_node_mutex); + const uint64_t timestamp = hrt_absolute_time(); - const uint64_t time_us = (clock.sim().sec() * 1000000) + (clock.sim().nsec() / 1000); + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_BARO_DEVTYPE_BAROSIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; - if (time_us > _world_time_us.load()) { - updateClock(clock.sim().sec(), clock.sim().nsec()); - } - - pthread_mutex_unlock(&_node_mutex); -} - -void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) -{ - if (hrt_absolute_time() == 0) { - return; - } - - pthread_mutex_lock(&_node_mutex); - - const uint64_t time_us = (air_pressure.header().stamp().sec() * 1000000) - + (air_pressure.header().stamp().nsec() / 1000); - - // publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = time_us; - sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION - sensor_baro.pressure = air_pressure.pressure(); - sensor_baro.temperature = this->_temperature; - sensor_baro.timestamp = hrt_absolute_time(); - _sensor_baro_pub.publish(sensor_baro); - - pthread_mutex_unlock(&_node_mutex); + sensor_baro_s report{}; + report.timestamp = timestamp; + report.timestamp_sample = timestamp; + report.device_id = id.devid; + report.pressure = msg.pressure(); + report.temperature = this->_temperature; + _sensor_baro_pub.publish(report); } -void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed) +void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &msg) { - if (hrt_absolute_time() == 0) { - return; - } + const uint64_t timestamp = hrt_absolute_time(); - pthread_mutex_lock(&_node_mutex); - - const uint64_t time_us = (air_speed.header().stamp().sec() * 1000000) - + (air_speed.header().stamp().nsec() / 1000); - - double air_speed_value = air_speed.diff_pressure(); + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; differential_pressure_s report{}; - report.timestamp_sample = time_us; - report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION - report.differential_pressure_pa = static_cast(air_speed_value); // hPa to Pa; - report.temperature = static_cast(air_speed.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C - report.timestamp = hrt_absolute_time();; + report.timestamp = timestamp; + report.timestamp_sample = timestamp; + report.device_id = id.devid; + report.differential_pressure_pa = msg.diff_pressure(); // hPa to Pa; + report.temperature = static_cast(msg.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C _differential_pressure_pub.publish(report); this->_temperature = report.temperature; - - pthread_mutex_unlock(&_node_mutex); } -void GZBridge::imuCallback(const gz::msgs::IMU &imu) +void GZBridge::imuCallback(const gz::msgs::IMU &msg) { - if (hrt_absolute_time() == 0) { - return; - } - pthread_mutex_lock(&_node_mutex); - - const uint64_t time_us = (imu.header().stamp().sec() * 1000000) + (imu.header().stamp().nsec() / 1000); - - if (time_us > _world_time_us.load()) { - updateClock(imu.header().stamp().sec(), imu.header().stamp().nsec()); - } + const uint64_t timestamp = hrt_absolute_time(); // FLU -> FRD static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0); gz::math::Vector3d accel_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d( - imu.linear_acceleration().x(), - imu.linear_acceleration().y(), - imu.linear_acceleration().z())); + msg.linear_acceleration().x(), + msg.linear_acceleration().y(), + msg.linear_acceleration().z())); + + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_IMU_DEVTYPE_SIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; // publish accel - sensor_accel_s sensor_accel{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - sensor_accel.timestamp_sample = time_us; - sensor_accel.timestamp = time_us; -#else - sensor_accel.timestamp_sample = hrt_absolute_time(); - sensor_accel.timestamp = hrt_absolute_time(); -#endif - sensor_accel.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - sensor_accel.x = accel_b.X(); - sensor_accel.y = accel_b.Y(); - sensor_accel.z = accel_b.Z(); - sensor_accel.temperature = NAN; - sensor_accel.samples = 1; - _sensor_accel_pub.publish(sensor_accel); + sensor_accel_s accel{}; + + accel.timestamp_sample = timestamp; + accel.timestamp = timestamp; + accel.device_id = id.devid; + + accel.x = accel_b.X(); + accel.y = accel_b.Y(); + accel.z = accel_b.Z(); + accel.temperature = NAN; + accel.samples = 1; + _sensor_accel_pub.publish(accel); gz::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d( - imu.angular_velocity().x(), - imu.angular_velocity().y(), - imu.angular_velocity().z())); + msg.angular_velocity().x(), + msg.angular_velocity().y(), + msg.angular_velocity().z())); // publish gyro - sensor_gyro_s sensor_gyro{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - sensor_gyro.timestamp_sample = time_us; - sensor_gyro.timestamp = time_us; -#else - sensor_gyro.timestamp_sample = hrt_absolute_time(); - sensor_gyro.timestamp = hrt_absolute_time(); -#endif - sensor_gyro.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - sensor_gyro.x = gyro_b.X(); - sensor_gyro.y = gyro_b.Y(); - sensor_gyro.z = gyro_b.Z(); - sensor_gyro.temperature = NAN; - sensor_gyro.samples = 1; - _sensor_gyro_pub.publish(sensor_gyro); - - pthread_mutex_unlock(&_node_mutex); + sensor_gyro_s gyro{}; + gyro.timestamp_sample = timestamp; + gyro.timestamp = timestamp; + gyro.device_id = id.devid; + gyro.x = gyro_b.X(); + gyro.y = gyro_b.Y(); + gyro.z = gyro_b.Z(); + gyro.temperature = NAN; + gyro.samples = 1; + _sensor_gyro_pub.publish(gyro); } -void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) +void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &msg) { - if (hrt_absolute_time() == 0) { - return; - } + const uint64_t timestamp = hrt_absolute_time(); - pthread_mutex_lock(&_node_mutex); + for (int p = 0; p < msg.pose_size(); p++) { + if (msg.pose(p).name() == _model_name) { - for (int p = 0; p < pose.pose_size(); p++) { - if (pose.pose(p).name() == _model_name) { + const double dt = math::constrain((timestamp - _timestamp_prev) * 1e-6, 0.001, 0.1); + _timestamp_prev = timestamp; - const uint64_t time_us = (pose.header().stamp().sec() * 1000000) + (pose.header().stamp().nsec() / 1000); - - if (time_us > _world_time_us.load()) { - updateClock(pose.header().stamp().sec(), pose.header().stamp().nsec()); - } - - const double dt = math::constrain((time_us - _timestamp_prev) * 1e-6, 0.001, 0.1); - _timestamp_prev = time_us; - - gz::msgs::Vector3d pose_position = pose.pose(p).position(); - gz::msgs::Quaternion pose_orientation = pose.pose(p).orientation(); + gz::msgs::Vector3d pose_position = msg.pose(p).position(); + gz::msgs::Quaternion pose_orientation = msg.pose(p).orientation(); // ground truth gz::math::Quaterniond q_gr = gz::math::Quaterniond( @@ -590,39 +286,27 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) // publish attitude groundtruth vehicle_attitude_s vehicle_attitude_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - vehicle_attitude_groundtruth.timestamp_sample = time_us; -#else - vehicle_attitude_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif + vehicle_attitude_groundtruth.timestamp_sample = timestamp; vehicle_attitude_groundtruth.q[0] = q_nb.W(); vehicle_attitude_groundtruth.q[1] = q_nb.X(); vehicle_attitude_groundtruth.q[2] = q_nb.Y(); vehicle_attitude_groundtruth.q[3] = q_nb.Z(); - vehicle_attitude_groundtruth.timestamp = hrt_absolute_time(); + vehicle_attitude_groundtruth.timestamp = timestamp; _attitude_ground_truth_pub.publish(vehicle_attitude_groundtruth); // publish angular velocity groundtruth const matrix::Eulerf euler{matrix::Quatf(vehicle_attitude_groundtruth.q)}; vehicle_angular_velocity_s vehicle_angular_velocity_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - vehicle_angular_velocity_groundtruth.timestamp_sample = time_us; -#else - vehicle_angular_velocity_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif + vehicle_angular_velocity_groundtruth.timestamp_sample = timestamp; const matrix::Vector3f angular_velocity = (euler - _euler_prev) / dt; _euler_prev = euler; angular_velocity.copyTo(vehicle_angular_velocity_groundtruth.xyz); - vehicle_angular_velocity_groundtruth.timestamp = hrt_absolute_time(); + vehicle_angular_velocity_groundtruth.timestamp = timestamp; _angular_velocity_ground_truth_pub.publish(vehicle_angular_velocity_groundtruth); vehicle_local_position_s local_position_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - local_position_groundtruth.timestamp_sample = time_us; -#else - local_position_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif + local_position_groundtruth.timestamp_sample = timestamp; // position ENU -> NED const matrix::Vector3d position{pose_position.y(), pose_position.x(), -pose_position.z()}; const matrix::Vector3d velocity{(position - _position_prev) / dt}; @@ -661,48 +345,29 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) local_position_groundtruth.z_global = false; } - local_position_groundtruth.timestamp = hrt_absolute_time(); + local_position_groundtruth.timestamp = timestamp; _lpos_ground_truth_pub.publish(local_position_groundtruth); - - pthread_mutex_unlock(&_node_mutex); return; } } - - pthread_mutex_unlock(&_node_mutex); } -void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry) +void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &msg) { - if (hrt_absolute_time() == 0) { - return; - } + const uint64_t timestamp = hrt_absolute_time(); - pthread_mutex_lock(&_node_mutex); - - const uint64_t time_us = (odometry.header().stamp().sec() * 1000000) + (odometry.header().stamp().nsec() / 1000); - - if (time_us > _world_time_us.load()) { - updateClock(odometry.header().stamp().sec(), odometry.header().stamp().nsec()); - } - - vehicle_odometry_s odom{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - odom.timestamp_sample = time_us; - odom.timestamp = time_us; -#else - odom.timestamp_sample = hrt_absolute_time(); - odom.timestamp = hrt_absolute_time(); -#endif + vehicle_odometry_s report{}; + report.timestamp_sample = timestamp; + report.timestamp = timestamp; // gz odometry position is in ENU frame and needs to be converted to NED - odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; - odom.position[0] = odometry.pose_with_covariance().pose().position().y(); - odom.position[1] = odometry.pose_with_covariance().pose().position().x(); - odom.position[2] = -odometry.pose_with_covariance().pose().position().z(); + report.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; + report.position[0] = msg.pose_with_covariance().pose().position().y(); + report.position[1] = msg.pose_with_covariance().pose().position().x(); + report.position[2] = -msg.pose_with_covariance().pose().position().z(); // gz odometry orientation is "body FLU->ENU" and needs to be converted in "body FRD->NED" - gz::msgs::Quaternion pose_orientation = odometry.pose_with_covariance().pose().orientation(); + gz::msgs::Quaternion pose_orientation = msg.pose_with_covariance().pose().orientation(); gz::math::Quaterniond q_gr = gz::math::Quaterniond( pose_orientation.w(), pose_orientation.x(), @@ -710,101 +375,214 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry pose_orientation.z()); gz::math::Quaterniond q_nb; GZBridge::rotateQuaternion(q_nb, q_gr); - odom.q[0] = q_nb.W(); - odom.q[1] = q_nb.X(); - odom.q[2] = q_nb.Y(); - odom.q[3] = q_nb.Z(); + report.q[0] = q_nb.W(); + report.q[1] = q_nb.X(); + report.q[2] = q_nb.Y(); + report.q[3] = q_nb.Z(); // gz odometry linear velocity is in body FLU and needs to be converted in body FRD - odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD; - odom.velocity[0] = odometry.twist_with_covariance().twist().linear().x(); - odom.velocity[1] = -odometry.twist_with_covariance().twist().linear().y(); - odom.velocity[2] = -odometry.twist_with_covariance().twist().linear().z(); + report.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD; + report.velocity[0] = msg.twist_with_covariance().twist().linear().x(); + report.velocity[1] = -msg.twist_with_covariance().twist().linear().y(); + report.velocity[2] = -msg.twist_with_covariance().twist().linear().z(); // gz odometry angular velocity is in body FLU and need to be converted in body FRD - odom.angular_velocity[0] = odometry.twist_with_covariance().twist().angular().x(); - odom.angular_velocity[1] = -odometry.twist_with_covariance().twist().angular().y(); - odom.angular_velocity[2] = -odometry.twist_with_covariance().twist().angular().z(); + report.angular_velocity[0] = msg.twist_with_covariance().twist().angular().x(); + report.angular_velocity[1] = -msg.twist_with_covariance().twist().angular().y(); + report.angular_velocity[2] = -msg.twist_with_covariance().twist().angular().z(); // VISION_POSITION_ESTIMATE covariance // pose 6x6 cross-covariance matrix // (states: x, y, z, roll, pitch, yaw). // If unknown, assign NaN value to first element in the array. - odom.position_variance[0] = odometry.pose_with_covariance().covariance().data(7); // Y row 1, col 1 - odom.position_variance[1] = odometry.pose_with_covariance().covariance().data(0); // X row 0, col 0 - odom.position_variance[2] = odometry.pose_with_covariance().covariance().data(14); // Z row 2, col 2 + report.position_variance[0] = msg.pose_with_covariance().covariance().data(7); // Y row 1, col 1 + report.position_variance[1] = msg.pose_with_covariance().covariance().data(0); // X row 0, col 0 + report.position_variance[2] = msg.pose_with_covariance().covariance().data(14); // Z row 2, col 2 - odom.orientation_variance[0] = odometry.pose_with_covariance().covariance().data(21); // R row 3, col 3 - odom.orientation_variance[1] = odometry.pose_with_covariance().covariance().data(28); // P row 4, col 4 - odom.orientation_variance[2] = odometry.pose_with_covariance().covariance().data(35); // Y row 5, col 5 + report.orientation_variance[0] = msg.pose_with_covariance().covariance().data(21); // R row 3, col 3 + report.orientation_variance[1] = msg.pose_with_covariance().covariance().data(28); // P row 4, col 4 + report.orientation_variance[2] = msg.pose_with_covariance().covariance().data(35); // Y row 5, col 5 - odom.velocity_variance[0] = odometry.twist_with_covariance().covariance().data(7); // Y row 1, col 1 - odom.velocity_variance[1] = odometry.twist_with_covariance().covariance().data(0); // X row 0, col 0 - odom.velocity_variance[2] = odometry.twist_with_covariance().covariance().data(14); // Z row 2, col 2 + report.velocity_variance[0] = msg.twist_with_covariance().covariance().data(7); // Y row 1, col 1 + report.velocity_variance[1] = msg.twist_with_covariance().covariance().data(0); // X row 0, col 0 + report.velocity_variance[2] = msg.twist_with_covariance().covariance().data(14); // Z row 2, col 2 - // odom.reset_counter = vpe.reset_counter; - _visual_odometry_pub.publish(odom); - - pthread_mutex_unlock(&_node_mutex); + // report.reset_counter = vpe.reset_counter; + _visual_odometry_pub.publish(report); } -void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat) +static float generate_wgn() { - if (hrt_absolute_time() == 0) { - return; + // generate white Gaussian noise sample with std=1 + + // algorithm 1: + // float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f)); + // return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX); + // algorithm 2: from BlockRandGauss.hpp + static float V1, V2, S; + static bool phase = true; + float X; + + if (phase) { + do { + float U1 = (float)rand() / (float)RAND_MAX; + float U2 = (float)rand() / (float)RAND_MAX; + V1 = 2.0f * U1 - 1.0f; + V2 = 2.0f * U2 - 1.0f; + S = V1 * V1 + V2 * V2; + } while (S >= 1.0f || fabsf(S) < 1e-8f); + + X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); + + } else { + X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); } - pthread_mutex_lock(&_node_mutex); + phase = !phase; + return X; +} - const uint64_t time_us = (nav_sat.header().stamp().sec() * 1000000) + (nav_sat.header().stamp().nsec() / 1000); +void GZBridge::addRealisticGpsNoise(double &latitude, double &longitude, double &altitude, + float &vel_north, float &vel_east, float &vel_down) +{ + _gps_pos_noise_n = _pos_markov_time * _gps_pos_noise_n + + _pos_random_walk * generate_wgn() * _pos_noise_amplitude - + 0.02f * _gps_pos_noise_n; - if (time_us > _world_time_us.load()) { - updateClock(nav_sat.header().stamp().sec(), nav_sat.header().stamp().nsec()); - } + _gps_pos_noise_e = _pos_markov_time * _gps_pos_noise_e + + _pos_random_walk * generate_wgn() * _pos_noise_amplitude - + 0.02f * _gps_pos_noise_e; + + _gps_pos_noise_d = _pos_markov_time * _gps_pos_noise_d + + _pos_random_walk * generate_wgn() * _pos_noise_amplitude * 1.5f - + 0.02f * _gps_pos_noise_d; + + latitude += math::degrees((double)_gps_pos_noise_n / CONSTANTS_RADIUS_OF_EARTH); + longitude += math::degrees((double)_gps_pos_noise_e / CONSTANTS_RADIUS_OF_EARTH); + altitude += (double)_gps_pos_noise_d; + + _gps_vel_noise_n = _vel_markov_time * _gps_vel_noise_n + + _vel_noise_density * generate_wgn() * _vel_noise_amplitude; + + _gps_vel_noise_e = _vel_markov_time * _gps_vel_noise_e + + _vel_noise_density * generate_wgn() * _vel_noise_amplitude; + + _gps_vel_noise_d = _vel_markov_time * _gps_vel_noise_d + + _vel_noise_density * generate_wgn() * _vel_noise_amplitude * 1.2f; + + vel_north += _gps_vel_noise_n; + vel_east += _gps_vel_noise_e; + vel_down += _gps_vel_noise_d; +} + +void GZBridge::navSatCallback(const gz::msgs::NavSat &msg) +{ + const uint64_t timestamp = hrt_absolute_time(); // initialize gps position if (!_pos_ref.isInitialized()) { - _pos_ref.initReference(nav_sat.latitude_deg(), nav_sat.longitude_deg(), hrt_absolute_time()); - _alt_ref = nav_sat.altitude(); - - } else { - // publish GPS groundtruth - vehicle_global_position_s global_position_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - global_position_groundtruth.timestamp_sample = time_us; -#else - global_position_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif - global_position_groundtruth.lat = nav_sat.latitude_deg(); - global_position_groundtruth.lon = nav_sat.longitude_deg(); - global_position_groundtruth.alt = nav_sat.altitude(); - _gpos_ground_truth_pub.publish(global_position_groundtruth); - } - - pthread_mutex_unlock(&_node_mutex); -} -void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) -{ - if (hrt_absolute_time() == 0) { + _pos_ref.initReference(msg.latitude_deg(), msg.longitude_deg(), timestamp); + _alt_ref = msg.altitude(); return; } - distance_sensor_s distance_sensor{}; - distance_sensor.timestamp = hrt_absolute_time(); - device::Device::DeviceId id; - id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - id.devid_s.bus = 0; - id.devid_s.address = 0; - id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; - distance_sensor.device_id = id.devid; - distance_sensor.min_distance = static_cast(scan.range_min()); - distance_sensor.max_distance = static_cast(scan.range_max()); - distance_sensor.current_distance = static_cast(scan.ranges()[0]); - distance_sensor.variance = 0.0f; - distance_sensor.signal_quality = -1; - distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + double latitude = msg.latitude_deg(); + double longitude = msg.longitude_deg(); + double altitude = msg.altitude(); + float vel_north = msg.velocity_north(); + float vel_east = msg.velocity_east(); + float vel_down = -msg.velocity_up(); - gz::msgs::Quaternion pose_orientation = scan.world_pose().orientation(); + vehicle_global_position_s gps_truth{}; + + // Publish GPS groundtruth + gps_truth.timestamp = timestamp; + gps_truth.timestamp_sample = timestamp; + gps_truth.lat = latitude; + gps_truth.lon = longitude; + gps_truth.alt = altitude; + _gpos_ground_truth_pub.publish(gps_truth); + + // Apply noise model (based on ublox F9P) + addRealisticGpsNoise(latitude, longitude, altitude, vel_north, vel_east, vel_down); + + // Device ID + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; + + sensor_gps_s sensor_gps{}; + + if (_sim_gps_used.get() >= 4) { + // fix + sensor_gps.fix_type = 3; // 3D fix + sensor_gps.s_variance_m_s = 0.4f; + sensor_gps.c_variance_rad = 0.1f; + sensor_gps.eph = 0.9f; + sensor_gps.epv = 1.78f; + sensor_gps.hdop = 0.7f; + sensor_gps.vdop = 1.1f; + + } else { + // no fix + sensor_gps.fix_type = 0; // No fix + sensor_gps.s_variance_m_s = 100.f; + sensor_gps.c_variance_rad = 100.f; + sensor_gps.eph = 100.f; + sensor_gps.epv = 100.f; + sensor_gps.hdop = 100.f; + sensor_gps.vdop = 100.f; + } + + sensor_gps.timestamp = timestamp; + sensor_gps.timestamp_sample = timestamp; + sensor_gps.time_utc_usec = 0; + sensor_gps.device_id = id.devid; + sensor_gps.latitude_deg = latitude; + sensor_gps.longitude_deg = longitude; + sensor_gps.altitude_msl_m = altitude; + sensor_gps.altitude_ellipsoid_m = altitude; + sensor_gps.noise_per_ms = 0; + sensor_gps.jamming_indicator = 0; + sensor_gps.vel_m_s = sqrtf(vel_north * vel_north + vel_east * vel_east); + sensor_gps.vel_n_m_s = vel_north; + sensor_gps.vel_e_m_s = vel_east; + sensor_gps.vel_d_m_s = vel_down; + sensor_gps.cog_rad = atan2(vel_east, vel_north); + sensor_gps.timestamp_time_relative = 0; + sensor_gps.heading = NAN; + sensor_gps.heading_offset = NAN; + sensor_gps.heading_accuracy = 0; + sensor_gps.automatic_gain_control = 0; + sensor_gps.jamming_state = 0; + sensor_gps.spoofing_state = 0; + sensor_gps.vel_ned_valid = true; + sensor_gps.satellites_used = _sim_gps_used.get(); + + _sensor_gps_pub.publish(sensor_gps); +} + +void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg) +{ + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; + + distance_sensor_s report{}; + report.timestamp = hrt_absolute_time(); + report.device_id = id.devid; + report.min_distance = static_cast(msg.range_min()); + report.max_distance = static_cast(msg.range_max()); + report.current_distance = static_cast(msg.ranges()[0]); + report.variance = 0.0f; + report.signal_quality = -1; + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + + gz::msgs::Quaternion pose_orientation = msg.world_pose().orientation(); gz::math::Quaterniond q_sensor = gz::math::Quaterniond( pose_orientation.w(), pose_orientation.x(), @@ -818,34 +596,34 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) const gz::math::Quaterniond q_down(0, 1, 0, 0); if (q_sensor.Equal(q_front, 0.03)) { - distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + report.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; } else if (q_sensor.Equal(q_down, 0.03)) { - distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + report.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; } else if (q_sensor.Equal(q_left, 0.03)) { - distance_sensor.orientation = distance_sensor_s::ROTATION_LEFT_FACING; + report.orientation = distance_sensor_s::ROTATION_LEFT_FACING; } else { - distance_sensor.orientation = distance_sensor_s::ROTATION_CUSTOM; - distance_sensor.q[0] = q_sensor.W(); - distance_sensor.q[1] = q_sensor.X(); - distance_sensor.q[2] = q_sensor.Y(); - distance_sensor.q[3] = q_sensor.Z(); + report.orientation = distance_sensor_s::ROTATION_CUSTOM; + report.q[0] = q_sensor.W(); + report.q[1] = q_sensor.X(); + report.q[2] = q_sensor.Y(); + report.q[3] = q_sensor.Z(); } - _distance_sensor_pub.publish(distance_sensor); + _distance_sensor_pub.publish(report); } -void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) +void GZBridge::laserScanCallback(const gz::msgs::LaserScan &msg) { static constexpr int SECTOR_SIZE_DEG = 5; // PX4 Collision Prevention uses 5 degree sectors - double angle_min_deg = scan.angle_min() * 180 / M_PI; - double angle_step_deg = scan.angle_step() * 180 / M_PI; + double angle_min_deg = msg.angle_min() * 180 / M_PI; + double angle_step_deg = msg.angle_step() * 180 / M_PI; int samples_per_sector = std::round(SECTOR_SIZE_DEG / angle_step_deg); - int number_of_sectors = scan.ranges_size() / samples_per_sector; + int number_of_sectors = msg.ranges_size() / samples_per_sector; std::vector ds_array(number_of_sectors, UINT16_MAX); @@ -858,7 +636,7 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) for (int j = 0; j < samples_per_sector; j++) { - double distance = scan.ranges()[i * samples_per_sector + j]; + double distance = msg.ranges()[i * samples_per_sector + j]; // inf values mean no object if (isinf(distance)) { @@ -871,7 +649,7 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) // If all samples in a sector are inf then it means the sector is clear if (samples_used_in_sector == 0) { - ds_array[i] = scan.range_max(); + ds_array[i] = msg.range_max(); } else { ds_array[i] = sum / samples_used_in_sector; @@ -879,20 +657,20 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) } // Publish to uORB - obstacle_distance_s obs {}; + obstacle_distance_s report {}; // Initialize unknown - for (auto &i : obs.distances) { + for (auto &i : report.distances) { i = UINT16_MAX; } - obs.timestamp = hrt_absolute_time(); - obs.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; - obs.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; - obs.min_distance = static_cast(scan.range_min() * 100.); - obs.max_distance = static_cast(scan.range_max() * 100.); - obs.angle_offset = static_cast(angle_min_deg); - obs.increment = static_cast(SECTOR_SIZE_DEG); + report.timestamp = hrt_absolute_time(); + report.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; + report.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; + report.min_distance = static_cast(msg.range_min() * 100.); + report.max_distance = static_cast(msg.range_max() * 100.); + report.angle_offset = static_cast(angle_min_deg); + report.increment = static_cast(SECTOR_SIZE_DEG); // Map samples in FOV into sectors in ObstacleDistance int index = 0; @@ -902,131 +680,22 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) uint16_t distance_cm = (*i) * 100.; - if (distance_cm >= obs.max_distance) { - obs.distances[index] = obs.max_distance + 1; + if (distance_cm >= report.max_distance) { + report.distances[index] = report.max_distance + 1; - } else if (distance_cm < obs.min_distance) { - obs.distances[index] = 0; + } else if (distance_cm < report.min_distance) { + report.distances[index] = 0; } else { - obs.distances[index] = distance_cm; + report.distances[index] = distance_cm; } index++; } - _obstacle_distance_pub.publish(obs); + _obstacle_distance_pub.publish(report); } -bool GZBridge::callEntityFactoryService(const std::string &service, const gz::msgs::EntityFactory &req) -{ - bool result; - gz::msgs::Boolean rep; - - if (_node.Request(service, req, 1000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("EntityFactory service call failed."); - return false; - } - - } else { - PX4_WARN("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); - return false; - } - - return true; -} - -bool GZBridge::callSceneInfoMsgService(const std::string &service) -{ - bool result; - gz::msgs::Empty req; - gz::msgs::Scene rep; - - if (_node.Request(service, req, 3000, rep, result)) { - if (!result) { - PX4_ERR("Scene Info service call failed."); - return false; - - } else { - return true; - } - - } else { - PX4_WARN("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); - return false; - } - - return true; -} - -bool GZBridge::callPhysicsMsgService(const std::string &service, const gz::msgs::Physics &req) -{ - bool result; - gz::msgs::Boolean rep; - - if (_node.Request(service, req, 5000, rep, result)) { - if (!result) { - PX4_ERR("Physics service call failed."); - return false; - - } else { - return true; - } - - } else { - PX4_ERR("Physics Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); - return false; - } - - return true; -} - -bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs::StringMsg &req) -{ - bool result; - - gz::msgs::Boolean rep; - - if (_node.Request(service, req, 3000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("String service call failed"); - return false; - - } - } - - else { - PX4_WARN("Service call timed out: %s", service.c_str()); - return false; - } - - return true; -} - -bool GZBridge::callVector3dService(const std::string &service, const gz::msgs::Vector3d &req) -{ - bool result; - - gz::msgs::Boolean rep; - - if (_node.Request(service, req, 3000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("String service call failed"); - return false; - - } - } - - else { - PX4_WARN("Service call timed out: %s", service.c_str()); - return false; - } - - return true; -} - - void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU) { // FLU (ROS) to FRD (PX4) static rotation @@ -1059,8 +728,6 @@ void GZBridge::Run() return; } - pthread_mutex_lock(&_node_mutex); - if (_parameter_update_sub.updated()) { parameter_update_s pupdate; _parameter_update_sub.copy(&pupdate); @@ -1074,8 +741,53 @@ void GZBridge::Run() } ScheduleDelayed(10_ms); +} - pthread_mutex_unlock(&_node_mutex); +int GZBridge::task_spawn(int argc, char *argv[]) +{ + std::string world_name; + std::string model_name; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "w:n:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'w': + world_name = myoptarg; + break; + + case 'n': + model_name = myoptarg; + break; + + default: + print_usage(); + return PX4_ERROR; + } + } + + PX4_INFO("world: %s, model: %s", world_name.c_str(), model_name.c_str()); + + GZBridge *instance = new GZBridge(world_name, model_name); + + if (!instance) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() != PX4_OK) { + delete instance; + _object.store(nullptr); + _task_id = -1; + return PX4_ERROR; + } + + return PX4_OK; } int GZBridge::print_status() @@ -1111,11 +823,8 @@ int GZBridge::print_usage(const char *reason) PRINT_MODULE_USAGE_NAME("gz_bridge", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, nullptr, "Fuel model name", false); - PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, nullptr, "Model Pose", false); - PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Model name", false); - PRINT_MODULE_USAGE_PARAM_STRING('i', nullptr, nullptr, "PX4 instance", false); PRINT_MODULE_USAGE_PARAM_STRING('w', nullptr, nullptr, "World name", true); + PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Model name", false); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 32b445decd..5e157f43b2 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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,24 +44,26 @@ #include #include #include +#include #include + #include #include #include #include #include #include -#include #include #include +#include +#include +#include +#include #include #include #include #include -#include #include -#include -#include #include #include @@ -81,7 +83,7 @@ using namespace time_literals; class GZBridge : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - GZBridge(const char *world, const char *name, const char *model, const char *pose_str); + GZBridge(const std::string &world, const std::string &model_name); ~GZBridge() override; /** @see ModuleBase */ @@ -98,78 +100,26 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; - uint64_t world_time_us() const { return _world_time_us.load(); } - private: void Run() override; - bool updateClock(const uint64_t tv_sec, const uint64_t tv_nsec); + void clockCallback(const gz::msgs::Clock &msg); + void airspeedCallback(const gz::msgs::AirSpeed &msg); + void barometerCallback(const gz::msgs::FluidPressure &msg); + void imuCallback(const gz::msgs::IMU &msg); + void poseInfoCallback(const gz::msgs::Pose_V &msg); + void odometryCallback(const gz::msgs::OdometryWithCovariance &msg); + void navSatCallback(const gz::msgs::NavSat &msg); + void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg); + void laserScanCallback(const gz::msgs::LaserScan &msg); - void clockCallback(const gz::msgs::Clock &clock); - - void airspeedCallback(const gz::msgs::AirSpeed &air_speed); - void barometerCallback(const gz::msgs::FluidPressure &air_pressure); - void imuCallback(const gz::msgs::IMU &imu); - void poseInfoCallback(const gz::msgs::Pose_V &pose); - void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry); - void navSatCallback(const gz::msgs::NavSat &nav_sat); - void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan); - void laserScanCallback(const gz::msgs::LaserScan &scan); - - /** - * @brief Call Entityfactory service - * - * @param req - * @return true - * @return false - */ - bool callEntityFactoryService(const std::string &service, const gz::msgs::EntityFactory &req); - - - /** - * @brief Call scene info service - * - * @param service - * @param req - * @return true - * @return false - */ - bool callSceneInfoMsgService(const std::string &service); - - /** - * @brief Call String service - * - * @param service - * @param req - * @return true - * @return false - */ - bool callStringMsgService(const std::string &service, const gz::msgs::StringMsg &req); - - /** - * @brief Call Vector3d Service - * - * @param service - * @param req - * @return true - * @return false - */ - bool callVector3dService(const std::string &service, const gz::msgs::Vector3d &req); - /** - * - * Convert a quaterion from FLU_to_ENU frames (ROS convention) - * to FRD_to_NED frames (PX4 convention) - * - * @param q_FRD_to_NED output quaterion in PX4 conventions - * @param q_FLU_to_ENU input quaterion in ROS conventions - */ static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU); - bool callPhysicsMsgService(const std::string &service, const gz::msgs::Physics &req); + void addRealisticGpsNoise(double &latitude, double &longitude, double &altitude, + float &vel_north, float &vel_east, float &vel_down); - // Subscriptions - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; @@ -178,23 +128,21 @@ private: uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; - uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; - uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; - uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; + uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; + uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; - GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex}; - GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex}; - GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex}; - GZGimbal _gimbal{_node, _node_mutex}; + GZMixingInterfaceESC _mixing_interface_esc{_node}; + GZMixingInterfaceServo _mixing_interface_servo{_node}; + GZMixingInterfaceWheel _mixing_interface_wheel{_node}; - px4::atomic _world_time_us{0}; - - pthread_mutex_t _node_mutex; + GZGimbal _gimbal{_node}; MapProjection _pos_ref{}; - double _alt_ref{}; // starting altitude reference + double _alt_ref{}; matrix::Vector3d _position_prev{}; matrix::Vector3d _velocity_prev{}; @@ -203,10 +151,26 @@ private: const std::string _world_name; const std::string _model_name; - const std::string _model_sim; - const std::string _model_pose; float _temperature{288.15}; // 15 degrees gz::transport::Node _node; + + // GPS noise model + float _gps_pos_noise_n = 0.0f; + float _gps_pos_noise_e = 0.0f; + float _gps_pos_noise_d = 0.0f; + float _gps_vel_noise_n = 0.0f; + float _gps_vel_noise_e = 0.0f; + float _gps_vel_noise_d = 0.0f; + const float _pos_noise_amplitude = 0.8f; // Position noise amplitude [m] + const float _pos_random_walk = 0.01f; // Position random walk coefficient + const float _pos_markov_time = 0.95f; // Position Markov process coefficient + const float _vel_noise_amplitude = 0.05f; // Velocity noise amplitude [m/s] + const float _vel_noise_density = 0.2f; // Velocity noise process density + const float _vel_markov_time = 0.85f; // Velocity Markov process coefficient + + DEFINE_PARAMETERS( + (ParamInt) _sim_gps_used + ) }; diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp index 976200020e..2d7ed9e069 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.cpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp @@ -49,6 +49,8 @@ bool GZGimbal::init(const std::string &world_name, const std::string &model_name return false; } + pthread_mutex_init(&_node_mutex, nullptr); + updateParameters(); ScheduleOnInterval(200_ms); // @5Hz diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp index 662268abd2..897c43150f 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.hpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -27,18 +27,17 @@ using namespace time_literals; class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams { public: - GZGimbal(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + GZGimbal(gz::transport::Node &node) : px4::ScheduledWorkItem(MODULE_NAME "-gimbal", px4::wq_configurations::rate_ctrl), ModuleParams(nullptr), - _node(node), - _node_mutex(node_mutex) + _node(node) {} private: friend class GZBridge; gz::transport::Node &_node; - pthread_mutex_t &_node_mutex; + pthread_mutex_t _node_mutex; uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)}; uORB::Subscription _gimbal_controls_sub{ORB_ID(gimbal_controls)}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp index c1a88f7bda..82ff4cf3da 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp @@ -55,6 +55,8 @@ bool GZMixingInterfaceESC::init(const std::string &model_name) _esc_status_pub.advertise(); + pthread_mutex_init(&_node_mutex, nullptr); + ScheduleNow(); return true; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp index 316ff6195a..d724c5c6f9 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp @@ -50,10 +50,9 @@ class GZMixingInterfaceESC : public OutputModuleInterface public: static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - GZMixingInterfaceESC(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + GZMixingInterfaceESC(gz::transport::Node &node) : OutputModuleInterface(MODULE_NAME "-actuators-esc", px4::wq_configurations::rate_ctrl), - _node(node), - _node_mutex(node_mutex) + _node(node) {} bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], @@ -77,7 +76,7 @@ private: void motorSpeedCallback(const gz::msgs::Actuators &actuators); gz::transport::Node &_node; - pthread_mutex_t &_node_mutex; + pthread_mutex_t _node_mutex; MixingOutput _mixing_output{"SIM_GZ_EC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 2fc656074c..6fd069ec09 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -121,6 +121,8 @@ bool GZMixingInterfaceServo::init(const std::string &model_name) _angular_range_rad.push_back(max_val - min_val); } + pthread_mutex_init(&_node_mutex, nullptr); + ScheduleNow(); return true; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp index 180f6cbca2..9980918233 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -44,10 +44,9 @@ class GZMixingInterfaceServo : public OutputModuleInterface { public: - GZMixingInterfaceServo(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + GZMixingInterfaceServo(gz::transport::Node &node) : OutputModuleInterface(MODULE_NAME "-actuators-servo", px4::wq_configurations::rate_ctrl), - _node(node), - _node_mutex(node_mutex) + _node(node) {} bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], @@ -83,7 +82,7 @@ private: float get_servo_angle_min(const size_t index); gz::transport::Node &_node; - pthread_mutex_t &_node_mutex; + pthread_mutex_t _node_mutex; MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp index 2198bacbaa..cd015892de 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp @@ -51,6 +51,8 @@ bool GZMixingInterfaceWheel::init(const std::string &model_name) return false; } + pthread_mutex_init(&_node_mutex, nullptr); + _wheel_encoders_pub.advertise(); ScheduleNow(); diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp index 281606dd61..8e14ddf958 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp @@ -51,10 +51,9 @@ class GZMixingInterfaceWheel : public OutputModuleInterface public: static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - GZMixingInterfaceWheel(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + GZMixingInterfaceWheel(gz::transport::Node &node) : OutputModuleInterface(MODULE_NAME "-actuators-wheel", px4::wq_configurations::rate_ctrl), - _node(node), - _node_mutex(node_mutex) + _node(node) {} bool updateOutputs(bool stop_wheels, uint16_t outputs[MAX_ACTUATORS], @@ -78,7 +77,7 @@ private: void wheelSpeedCallback(const gz::msgs::Actuators &actuators); gz::transport::Node &_node; - pthread_mutex_t &_node_mutex; + pthread_mutex_t _node_mutex; MixingOutput _mixing_output{"SIM_GZ_WH", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};