From 684b4a4b8a2abffc00e5835b2c6dfc7ab971d878 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Tue, 24 Jan 2023 19:01:45 -0500 Subject: [PATCH] simulation/gz_bridge: rc_cessna plane model working, Gazebo Garden updates, and prepare for proper airspeed (#20989) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Benjamin Perseghetti Co-authored-by: Alejandro Hernández Cordero --- .../{4003_gz_plane => 4003_gz_rc_cessna} | 4 +- .../init.d-posix/airframes/CMakeLists.txt | 2 +- .../init.d-posix/px4-rc.simulator | 14 +- .../{plane => rc_cessna}/meshes/body.dae | 0 .../{plane => rc_cessna}/meshes/elevators.dae | 0 .../meshes/iris_prop_ccw.dae | 0 .../meshes/iris_prop_cw.dae | 0 .../meshes/left_aileron.dae | 0 .../{plane => rc_cessna}/meshes/left_flap.dae | 0 .../meshes/propeller_ccw.dae | 0 .../meshes/propeller_cw.dae | 0 .../meshes/right_aileron.dae | 0 .../meshes/right_flap.dae | 0 .../{plane => rc_cessna}/meshes/rudder.dae | 0 .../models/{plane => rc_cessna}/model.config | 4 +- .../gz/models/{plane => rc_cessna}/model.sdf | 255 ++++++++++++++++-- Tools/simulation/gz/models/x500/model.sdf | 6 +- .../simulation/gz_bridge/CMakeLists.txt | 4 +- src/modules/simulation/gz_bridge/GZBridge.cpp | 98 ++++--- src/modules/simulation/gz_bridge/GZBridge.hpp | 22 +- .../gz_bridge/GZMixingInterfaceESC.cpp | 6 +- .../gz_bridge/GZMixingInterfaceESC.hpp | 10 +- .../gz_bridge/GZMixingInterfaceServo.cpp | 8 +- .../gz_bridge/GZMixingInterfaceServo.hpp | 8 +- src/modules/simulation/gz_bridge/gz_env.sh.in | 3 - .../sitl_targets_gazebo-classic.cmake | 4 +- 26 files changed, 348 insertions(+), 100 deletions(-) rename ROMFS/px4fmu_common/init.d-posix/airframes/{4003_gz_plane => 4003_gz_rc_cessna} (96%) rename Tools/simulation/gz/models/{plane => rc_cessna}/meshes/body.dae (100%) rename Tools/simulation/gz/models/{plane => rc_cessna}/meshes/elevators.dae (100%) rename Tools/simulation/gz/models/{plane => rc_cessna}/meshes/iris_prop_ccw.dae (100%) rename Tools/simulation/gz/models/{plane => rc_cessna}/meshes/iris_prop_cw.dae (100%) rename Tools/simulation/gz/models/{plane => rc_cessna}/meshes/left_aileron.dae (100%) rename Tools/simulation/gz/models/{plane => rc_cessna}/meshes/left_flap.dae (100%) rename Tools/simulation/gz/models/{plane => rc_cessna}/meshes/propeller_ccw.dae (100%) rename Tools/simulation/gz/models/{plane => rc_cessna}/meshes/propeller_cw.dae (100%) rename Tools/simulation/gz/models/{plane => rc_cessna}/meshes/right_aileron.dae (100%) rename Tools/simulation/gz/models/{plane => rc_cessna}/meshes/right_flap.dae (100%) rename Tools/simulation/gz/models/{plane => rc_cessna}/meshes/rudder.dae (100%) rename Tools/simulation/gz/models/{plane => rc_cessna}/model.config (78%) rename Tools/simulation/gz/models/{plane => rc_cessna}/model.sdf (70%) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna similarity index 96% rename from ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_plane rename to ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index 56fdc02cd7..6544835154 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna @@ -1,6 +1,6 @@ #!/bin/sh # -# @name Gazebo plane +# @name Gazebo rc_cessna # @type Fixedwing # @@ -8,7 +8,7 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz} PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} -PX4_SIM_MODEL=${PX4_SIM_MODEL:=plane} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna} param set-default EKF2_MAG_ACCLIM 0 param set-default EKF2_MAG_YAWLIM 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 131bf9d45f..358222859b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -72,7 +72,7 @@ px4_add_romfs_files( 4001_gz_x500 4002_gz_x500_depth - 4003_gz_plane + 4003_gz_rc_cessna 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index e871954940..2ea8171e89 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -21,7 +21,7 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then - # source generated gz_env.sh for IGN_GAZEBO_RESOURCE_PATH + # source generated gz_env.sh for GZ_SIM_RESOURCE_PATH if [ -f ./gz_env.sh ]; then . ./gz_env.sh @@ -37,16 +37,8 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th gz_command="gz" gz_sub_command="sim" else - IGN_GAZEBO_VERSIONS=$(ign gazebo --versions 2>&1) - if [ $? -eq 0 ] && [ "${IGN_GAZEBO_VERSIONS}" != "" ] - then - # "ign gazebo" for Fortress and earlier - gz_command="ign" - gz_sub_command="gazebo" - else - echo "ERROR [init] Gazebo gz and ign commands unavailable" - exit 1 - fi + echo "ERROR [init] Gazebo gz please install gz-garden" + exit 1 fi # look for running ${gz_command} gazebo world diff --git a/Tools/simulation/gz/models/plane/meshes/body.dae b/Tools/simulation/gz/models/rc_cessna/meshes/body.dae similarity index 100% rename from Tools/simulation/gz/models/plane/meshes/body.dae rename to Tools/simulation/gz/models/rc_cessna/meshes/body.dae diff --git a/Tools/simulation/gz/models/plane/meshes/elevators.dae b/Tools/simulation/gz/models/rc_cessna/meshes/elevators.dae similarity index 100% rename from Tools/simulation/gz/models/plane/meshes/elevators.dae rename to Tools/simulation/gz/models/rc_cessna/meshes/elevators.dae diff --git a/Tools/simulation/gz/models/plane/meshes/iris_prop_ccw.dae b/Tools/simulation/gz/models/rc_cessna/meshes/iris_prop_ccw.dae similarity index 100% rename from Tools/simulation/gz/models/plane/meshes/iris_prop_ccw.dae rename to Tools/simulation/gz/models/rc_cessna/meshes/iris_prop_ccw.dae diff --git a/Tools/simulation/gz/models/plane/meshes/iris_prop_cw.dae b/Tools/simulation/gz/models/rc_cessna/meshes/iris_prop_cw.dae similarity index 100% rename from Tools/simulation/gz/models/plane/meshes/iris_prop_cw.dae rename to Tools/simulation/gz/models/rc_cessna/meshes/iris_prop_cw.dae diff --git a/Tools/simulation/gz/models/plane/meshes/left_aileron.dae b/Tools/simulation/gz/models/rc_cessna/meshes/left_aileron.dae similarity index 100% rename from Tools/simulation/gz/models/plane/meshes/left_aileron.dae rename to Tools/simulation/gz/models/rc_cessna/meshes/left_aileron.dae diff --git a/Tools/simulation/gz/models/plane/meshes/left_flap.dae b/Tools/simulation/gz/models/rc_cessna/meshes/left_flap.dae similarity index 100% rename from Tools/simulation/gz/models/plane/meshes/left_flap.dae rename to Tools/simulation/gz/models/rc_cessna/meshes/left_flap.dae diff --git a/Tools/simulation/gz/models/plane/meshes/propeller_ccw.dae b/Tools/simulation/gz/models/rc_cessna/meshes/propeller_ccw.dae similarity index 100% rename from Tools/simulation/gz/models/plane/meshes/propeller_ccw.dae rename to Tools/simulation/gz/models/rc_cessna/meshes/propeller_ccw.dae diff --git a/Tools/simulation/gz/models/plane/meshes/propeller_cw.dae b/Tools/simulation/gz/models/rc_cessna/meshes/propeller_cw.dae similarity index 100% rename from Tools/simulation/gz/models/plane/meshes/propeller_cw.dae rename to Tools/simulation/gz/models/rc_cessna/meshes/propeller_cw.dae diff --git a/Tools/simulation/gz/models/plane/meshes/right_aileron.dae b/Tools/simulation/gz/models/rc_cessna/meshes/right_aileron.dae similarity index 100% rename from Tools/simulation/gz/models/plane/meshes/right_aileron.dae rename to Tools/simulation/gz/models/rc_cessna/meshes/right_aileron.dae diff --git a/Tools/simulation/gz/models/plane/meshes/right_flap.dae b/Tools/simulation/gz/models/rc_cessna/meshes/right_flap.dae similarity index 100% rename from Tools/simulation/gz/models/plane/meshes/right_flap.dae rename to Tools/simulation/gz/models/rc_cessna/meshes/right_flap.dae diff --git a/Tools/simulation/gz/models/plane/meshes/rudder.dae b/Tools/simulation/gz/models/rc_cessna/meshes/rudder.dae similarity index 100% rename from Tools/simulation/gz/models/plane/meshes/rudder.dae rename to Tools/simulation/gz/models/rc_cessna/meshes/rudder.dae diff --git a/Tools/simulation/gz/models/plane/model.config b/Tools/simulation/gz/models/rc_cessna/model.config similarity index 78% rename from Tools/simulation/gz/models/plane/model.config rename to Tools/simulation/gz/models/rc_cessna/model.config index d6dcd1db5f..d41b4c6180 100644 --- a/Tools/simulation/gz/models/plane/model.config +++ b/Tools/simulation/gz/models/rc_cessna/model.config @@ -1,6 +1,6 @@ - Plane + rc_cessna 1.0 model.sdf @@ -10,6 +10,6 @@ - This is a model of a standard plane. + This is a model of an RC Cessna 182. diff --git a/Tools/simulation/gz/models/plane/model.sdf b/Tools/simulation/gz/models/rc_cessna/model.sdf similarity index 70% rename from Tools/simulation/gz/models/plane/model.sdf rename to Tools/simulation/gz/models/rc_cessna/model.sdf index fa358d8072..266689aa30 100644 --- a/Tools/simulation/gz/models/plane/model.sdf +++ b/Tools/simulation/gz/models/rc_cessna/model.sdf @@ -1,6 +1,6 @@ - + 0 0 0.246 0 0 0 0 0 0 0 0 0 @@ -16,11 +16,11 @@ 0.1477 - - 0 0 -0.07 0 0 0 + + -.14 0 0 0 0 0 - 0.47 0.47 0.11 + 0.65 .08 0.10 @@ -32,12 +32,44 @@ + + -0.01 0 0.07 0 0 0 + + + 0.1 1.0 0.01 + + + + + + 10 + 0.01 + + + + + 0.07 0 -0.08 0 0 0 0.1 0.1 0.1 - model://plane/meshes/body.dae + model://rc_cessna/meshes/body.dae @@ -67,7 +99,7 @@ 1e-05 - + 0.01 @@ -98,7 +130,7 @@ base_link - 0.22 0 0.0 0 1.57 0 + 0.22 0 0.0 0 1.57079632679 0 0 0 0 0 0 0 0.005 @@ -112,7 +144,7 @@ - 0.0 0 0 0 0 0 + 0 0 0 0 -1.57079632679 0 0.005 0.22 0.02 @@ -128,7 +160,7 @@ - 0 0 0 0.0 0 0.0 + 0 0 0 0 0 -1.57079632679 1 1 1 - model://plane/meshes/iris_prop_ccw.dae + model://rc_cessna/meshes/iris_prop_ccw.dae @@ -186,7 +218,7 @@ 0.1 0.1 0.1 - model://plane/meshes/left_aileron.dae + model://rc_cessna/meshes/left_aileron.dae @@ -195,6 +227,135 @@ + + 0 0 0 0 0 0 + + .05 + + 0.00003331 + 0 + 0 + 0.0000204 + 0 + 0.0000204 + + + true + + + 0 0 0 -1.57079632679 0 0 + + + 0.01 + 0.03 + + + + + 0 0 0 -1.57079632679 0 0 + + + 0.01 + 0.03 + + + + + + 1.0 + 0.5 + 0 0 1 + + + + + + + 0 0 0 0 0 0 + + .05 + + 0.00003331 + 0 + 0 + 0.0000204 + 0 + 0.0000204 + + + true + + + 0 0 0 -1.57079632679 0 0 + + + 0.01 + 0.03 + + + + + 0 0 0 -1.57079632679 0 0 + + + 0.01 + 0.03 + + + + + + 1.0 + 0.5 + 0 0 1 + + + + + + + 0 0 0 0 0 0 + + .05 + + 0.00003331 + 0 + 0 + 0.0000204 + 0 + 0.0000204 + + + true + + + 0 0 0 -1.57079632679 0 0 + + + 0.01 + 0.025 + + + + + 0 0 0 -1.57079632679 0 0 + + + 0.01 + 0.025 + + + + + + 1.0 + 0.5 + 0 0 1 + + + + + 0.00000001 @@ -213,7 +374,7 @@ 0.1 0.1 0.1 - model://plane/meshes/right_aileron.dae + model://rc_cessna/meshes/right_aileron.dae @@ -240,7 +401,7 @@ 0.1 0.1 0.1 - model://plane/meshes/left_flap.dae + model://rc_cessna/meshes/left_flap.dae @@ -267,7 +428,7 @@ 0.1 0.1 0.1 - model://plane/meshes/right_flap.dae + model://rc_cessna/meshes/right_flap.dae @@ -294,7 +455,7 @@ 0.1 0.1 0.1 - model://plane/meshes/elevators.dae + model://rc_cessna/meshes/elevators.dae @@ -321,7 +482,7 @@ 0.1 0.1 0.1 - model://plane/meshes/rudder.dae + model://rc_cessna/meshes/rudder.dae @@ -451,6 +612,57 @@ + + base_link + LeftWheel + -.035 .13 -0.12 0 0 0 + + 0 1 0 + + -1.79769e+308 + 1.79769e+308 + + + 0 + 0 + + 1 + + + + base_link + RightWheel + -.035 -.13 -0.12 0 0 0 + + 0 1 0 + + -1.79769e+308 + 1.79769e+308 + + + 0 + 0 + + 1 + + + + base_link + CenterWheel + .135 0 -0.12 0 0 0 + + 0 1 0 + + -1.79769e+308 + 1.79769e+308 + + + 0 + 0 + + 1 + + 0.05984281113 4.752798721 @@ -472,6 +684,7 @@ px4_servo_0 + px4_servo_0 0.05984281113 @@ -494,6 +707,7 @@ px4_servo_1 + px4_servo_1 0.05984281113 @@ -531,7 +745,7 @@ right_flap_joint -0.1 - + -0.2 4.752798721 0.6417112299 @@ -552,6 +766,7 @@ px4_servo_2 + px4_servo_2 0.0 @@ -575,7 +790,7 @@ filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController"> rudder_joint - + rotor_puller_joint rotor_puller cw @@ -585,7 +800,7 @@ 2.44858e-05 0.016 command/motor_speed - 3 + 0 8.06428e-05 1e-06 10 diff --git a/Tools/simulation/gz/models/x500/model.sdf b/Tools/simulation/gz/models/x500/model.sdf index 7dc600b8cb..719352b7a4 100644 --- a/Tools/simulation/gz/models/x500/model.sdf +++ b/Tools/simulation/gz/models/x500/model.sdf @@ -528,7 +528,7 @@ 10 velocity - + rotor_1_joint rotor_1 ccw @@ -544,7 +544,7 @@ 10 velocity - + rotor_2_joint rotor_2 cw @@ -560,7 +560,7 @@ 10 velocity - + rotor_3_joint rotor_3 cw diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index f3973561f7..8462dec6d3 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -35,9 +35,7 @@ find_package(gz-transport #REQUIRED COMPONENTS core NAMES - ignition-transport8 - ignition-transport10 - ignition-transport11 + #ignition-transport11 # IGN (Fortress and earlier) no longer supported gz-transport12 #QUIET ) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 765c051e2a..db8bc6c69b 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -35,6 +35,7 @@ #include +#include #include #include @@ -70,8 +71,7 @@ int GZBridge::init() if (!_model_sim.empty()) { // service call to create model - // ign service -s /world/${PX4_GZ_WORLD}/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 1000 --req "sdf_filename: \"${PX4_GZ_MODEL}/model.sdf\"" - ignition::msgs::EntityFactory req{}; + 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. @@ -95,16 +95,16 @@ int GZBridge::init() model_pose_v.push_back(0.0); } - ignition::msgs::Pose *p = req.mutable_pose(); - ignition::msgs::Vector3d *position = p->mutable_position(); + 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]); - ignition::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]); + gz::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]); q.Normalize(); - ignition::msgs::Quaternion *orientation = p->mutable_orientation(); + gz::msgs::Quaternion *orientation = p->mutable_orientation(); orientation->set_x(q.X()); orientation->set_y(q.Y()); orientation->set_z(q.Z()); @@ -112,7 +112,7 @@ int GZBridge::init() } //world/$WORLD/create service. - ignition::msgs::Boolean rep; + gz::msgs::Boolean rep; bool result; std::string create_service = "/world/" + _world_name + "/create"; @@ -152,6 +152,18 @@ int GZBridge::init() return PX4_ERROR; } +#if 0 + // Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed + std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/airspeed_link/sensor/air_speed/air_speed"; + + if (!_node.Subscribe(airpressure_topic, &GZBridge::airpressureCallback, this)) { + PX4_ERR("failed to subscribe to %s", airpressure_topic.c_str()); + return PX4_ERROR; + } + +#endif + if (!_mixing_interface_esc.init(_model_name)) { PX4_ERR("failed to init ESC output"); return PX4_ERROR; @@ -304,7 +316,7 @@ bool GZBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec) return false; } -void GZBridge::clockCallback(const ignition::msgs::Clock &clock) +void GZBridge::clockCallback(const gz::msgs::Clock &clock) { pthread_mutex_lock(&_node_mutex); @@ -317,7 +329,33 @@ void GZBridge::clockCallback(const ignition::msgs::Clock &clock) pthread_mutex_unlock(&_node_mutex); } -void GZBridge::imuCallback(const ignition::msgs::IMU &imu) +#if 0 +void GZBridge::airpressureCallback(const gz::msgs::FluidPressure &air_pressure) +{ + if (hrt_absolute_time() == 0) { + return; + } + + pthread_mutex_lock(&_mutex); + + const uint64_t time_us = (air_pressure.header().stamp().sec() * 1000000) + + (air_pressure.header().stamp().nsec() / 1000); + + double air_pressure_value = air_pressure.pressure(); + + 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_pressure_value); // hPa to Pa; + report.temperature = static_cast(air_pressure.variance()) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // K to C + report.timestamp = hrt_absolute_time();; + _differential_pressure_pub.publish(report); + + pthread_mutex_unlock(&_node_mutex); +} +#endif + +void GZBridge::imuCallback(const gz::msgs::IMU &imu) { if (hrt_absolute_time() == 0) { return; @@ -332,12 +370,12 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu) } // FLU -> FRD - static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0); + static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0); - ignition::math::Vector3d accel_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d( - imu.linear_acceleration().x(), - imu.linear_acceleration().y(), - imu.linear_acceleration().z())); + 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())); // publish accel sensor_accel_s sensor_accel{}; @@ -357,10 +395,10 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu) _sensor_accel_pub.publish(sensor_accel); - ignition::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d( - imu.angular_velocity().x(), - imu.angular_velocity().y(), - imu.angular_velocity().z())); + 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())); // publish gyro sensor_gyro_s sensor_gyro{}; @@ -382,7 +420,7 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu) pthread_mutex_unlock(&_node_mutex); } -void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose) +void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) { if (hrt_absolute_time() == 0) { return; @@ -402,10 +440,10 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose) const double dt = math::constrain((time_us - _timestamp_prev) * 1e-6, 0.001, 0.1); _timestamp_prev = time_us; - ignition::msgs::Vector3d pose_position = pose.pose(p).position(); - ignition::msgs::Quaternion pose_orientation = pose.pose(p).orientation(); + gz::msgs::Vector3d pose_position = pose.pose(p).position(); + gz::msgs::Quaternion pose_orientation = pose.pose(p).orientation(); - static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0); + static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0); /** * @brief Quaternion for rotation between ENU and NED frames @@ -414,17 +452,17 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose) * ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North) * This rotation is symmetric, so q_ENU_to_NED == q_NED_to_ENU. */ - static const auto q_ENU_to_NED = ignition::math::Quaterniond(0, 0.70711, 0.70711, 0); + static const auto q_ENU_to_NED = gz::math::Quaterniond(0, 0.70711, 0.70711, 0); // ground truth - ignition::math::Quaterniond q_gr = ignition::math::Quaterniond( - pose_orientation.w(), - pose_orientation.x(), - pose_orientation.y(), - pose_orientation.z()); + gz::math::Quaterniond q_gr = gz::math::Quaterniond( + pose_orientation.w(), + pose_orientation.x(), + pose_orientation.y(), + pose_orientation.z()); - ignition::math::Quaterniond q_gb = q_gr * q_FLU_to_FRD.Inverse(); - ignition::math::Quaterniond q_nb = q_ENU_to_NED * q_gb; + gz::math::Quaterniond q_gb = q_gr * q_FLU_to_FRD.Inverse(); + gz::math::Quaterniond q_nb = q_ENU_to_NED * q_gb; // publish attitude groundtruth vehicle_attitude_s vehicle_attitude_groundtruth{}; diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index b63d6447aa..6a8a0d435e 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -54,10 +55,12 @@ #include #include -#include -#include -#include -#include +#include +#include +#include + +// #include +#include using namespace time_literals; @@ -89,13 +92,16 @@ private: bool updateClock(const uint64_t tv_sec, const uint64_t tv_nsec); - void clockCallback(const ignition::msgs::Clock &clock); - void imuCallback(const ignition::msgs::IMU &imu); - void poseInfoCallback(const ignition::msgs::Pose_V &pose); + void clockCallback(const gz::msgs::Clock &clock); + + //void airpressureCallback(const gz::msgs::FluidPressure &air_pressure); + void imuCallback(const gz::msgs::IMU &imu); + void poseInfoCallback(const gz::msgs::Pose_V &pose); // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; @@ -123,7 +129,7 @@ private: const std::string _model_sim; const std::string _model_pose; - ignition::transport::Node _node; + gz::transport::Node _node; DEFINE_PARAMETERS( (ParamFloat) _param_sim_home_lat, diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp index 9adbbbdcac..38eeadf11f 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp @@ -46,7 +46,7 @@ bool GZMixingInterfaceESC::init(const std::string &model_name) // output eg /X500/command/motor_speed std::string actuator_topic = "/" + model_name + "/command/motor_speed"; - _actuators_pub = _node.Advertise(actuator_topic); + _actuators_pub = _node.Advertise(actuator_topic); if (!_actuators_pub.Valid()) { PX4_ERR("failed to advertise %s", actuator_topic.c_str()); @@ -73,7 +73,7 @@ bool GZMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ } if (active_output_count > 0) { - ignition::msgs::Actuators rotor_velocity_message; + gz::msgs::Actuators rotor_velocity_message; rotor_velocity_message.mutable_velocity()->Resize(active_output_count, 0); for (unsigned i = 0; i < active_output_count; i++) { @@ -96,7 +96,7 @@ void GZMixingInterfaceESC::Run() pthread_mutex_unlock(&_node_mutex); } -void GZMixingInterfaceESC::motorSpeedCallback(const ignition::msgs::Actuators &actuators) +void GZMixingInterfaceESC::motorSpeedCallback(const gz::msgs::Actuators &actuators) { if (hrt_absolute_time() == 0) { return; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp index ab88f1867f..e8e3c5a244 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp @@ -35,7 +35,7 @@ #include -#include +#include #include #include @@ -49,7 +49,7 @@ class GZMixingInterfaceESC : public OutputModuleInterface public: static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - GZMixingInterfaceESC(ignition::transport::Node &node, pthread_mutex_t &node_mutex) : + GZMixingInterfaceESC(gz::transport::Node &node, pthread_mutex_t &node_mutex) : OutputModuleInterface(MODULE_NAME "-actuators-esc", px4::wq_configurations::rate_ctrl), _node(node), _node_mutex(node_mutex) @@ -73,14 +73,14 @@ private: void Run() override; - void motorSpeedCallback(const ignition::msgs::Actuators &actuators); + void motorSpeedCallback(const gz::msgs::Actuators &actuators); - ignition::transport::Node &_node; + gz::transport::Node &_node; pthread_mutex_t &_node_mutex; MixingOutput _mixing_output{"SIM_GZ_EC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; - ignition::transport::Node::Publisher _actuators_pub; + gz::transport::Node::Publisher _actuators_pub; uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 996272ef26..48bed53b95 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -35,12 +35,12 @@ bool GZMixingInterfaceServo::init(const std::string &model_name) { - // /model/plane_0/joint/left_elevon_joint/0/cmd_pos + // /model/rascal_110_0/px4_servo_2 for (int i = 0; i < 8; i++) { std::string joint_name = "px4_servo_" + std::to_string(i); - std::string servo_topic = "/model/" + model_name + "/joint/" + joint_name + "/0/cmd_pos"; + std::string servo_topic = "/model/" + model_name + "/" + joint_name; //std::cout << "Servo topic: " << servo_topic << std::endl; - _servos_pub.push_back(_node.Advertise(servo_topic)); + _servos_pub.push_back(_node.Advertise(servo_topic)); if (!_servos_pub.back().Valid()) { PX4_ERR("failed to advertise %s", servo_topic.c_str()); @@ -63,7 +63,7 @@ bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MA for (auto &servo_pub : _servos_pub) { if (_mixing_output.isFunctionSet(i)) { - ignition::msgs::Double servo_output; + gz::msgs::Double servo_output; ///TODO: Normalize output data double output = (outputs[i] - 500) / 500.0; // std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp index 881072a078..63345b3aed 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -35,7 +35,7 @@ #include -#include +#include // GZBridge mixing class for Servos. // It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling @@ -43,7 +43,7 @@ class GZMixingInterfaceServo : public OutputModuleInterface { public: - GZMixingInterfaceServo(ignition::transport::Node &node, pthread_mutex_t &node_mutex) : + GZMixingInterfaceServo(gz::transport::Node &node, pthread_mutex_t &node_mutex) : OutputModuleInterface(MODULE_NAME "-actuators-servo", px4::wq_configurations::rate_ctrl), _node(node), _node_mutex(node_mutex) @@ -67,10 +67,10 @@ private: void Run() override; - ignition::transport::Node &_node; + gz::transport::Node &_node; pthread_mutex_t &_node_mutex; MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; - std::vector _servos_pub; + std::vector _servos_pub; }; diff --git a/src/modules/simulation/gz_bridge/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index 4241851087..810bc88948 100644 --- a/src/modules/simulation/gz_bridge/gz_env.sh.in +++ b/src/modules/simulation/gz_bridge/gz_env.sh.in @@ -3,7 +3,4 @@ export PX4_GZ_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/gz/models export PX4_GZ_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/gz/worlds -export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS - -# IGN -> GZ as of Garden export GZ_SIM_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index 2aae2343f9..10c50b9c45 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -1,5 +1,7 @@ -find_package(gazebo) +find_package(gazebo + QUIET +) if(gazebo_FOUND)