diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4005_gz_x500_vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4005_gz_x500_vision new file mode 100644 index 0000000000..bda4adf6a9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4005_gz_x500_vision @@ -0,0 +1,12 @@ +#!/bin/sh +# +# @name Gazebo x500 vision +# +# @type Quadrotor +# + +. ${R}etc/init.d-posix/airframes/4001_gz_x500 + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_vision} diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index b251d019e1..ae80ed2057 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -74,6 +74,7 @@ px4_add_romfs_files( 4002_gz_x500_depth 4003_gz_rc_cessna 4004_gz_standard_vtol + 4005_gz_x500_vision 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/Tools/simulation/gz/models/x500_vision/model.config b/Tools/simulation/gz/models/x500_vision/model.config new file mode 100644 index 0000000000..1740459a7f --- /dev/null +++ b/Tools/simulation/gz/models/x500_vision/model.config @@ -0,0 +1,11 @@ + + + x500-vision + 1.0 + model.sdf + + Jaeyoung Lim + jalim@ethz.ch + + Model of the X500 with a odometry/external vision input. + diff --git a/Tools/simulation/gz/models/x500_vision/model.sdf b/Tools/simulation/gz/models/x500_vision/model.sdf new file mode 100644 index 0000000000..17c7e4cc88 --- /dev/null +++ b/Tools/simulation/gz/models/x500_vision/model.sdf @@ -0,0 +1,13 @@ + + + + + https://fuel.gazebosim.org/1.0/RudisLaboratories/models/x500-Base + + + 3 + + + diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 545cbfa4ef..4d7632e612 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -152,6 +152,15 @@ 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"; + + if (!_node.Subscribe(odometry_topic, &GZBridge::odometryCallback, this)) { + PX4_ERR("failed to subscribe to %s", odometry_topic.c_str()); + 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 + @@ -590,6 +599,64 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) pthread_mutex_unlock(&_node_mutex); } +void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry) +{ + if (hrt_absolute_time() == 0) { + return; + } + + 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 + 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(); + + odom.velocity[0] = odometry.twist_with_covariance().twist().linear().y(); + odom.velocity[1] = odometry.twist_with_covariance().twist().linear().x(); + odom.velocity[2] = -odometry.twist_with_covariance().twist().linear().z(); + + odom.angular_velocity[0] = odometry.twist_with_covariance().twist().angular().y(); + odom.angular_velocity[1] = odometry.twist_with_covariance().twist().angular().x(); + odom.angular_velocity[2] = -odometry.twist_with_covariance().twist().angular().z(); + + // VISION_POSITION_ESTIMATE covariance + // Row-major representation of pose 6x6 cross-covariance matrix upper right triangle + // (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). + // If unknown, assign NaN value to first element in the array. + odom.position_variance[0] = odometry.pose_with_covariance().covariance().data(0); // X row 0, col 0 + odom.position_variance[1] = odometry.pose_with_covariance().covariance().data(7); // Y row 1, col 1 + odom.position_variance[2] = odometry.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 + + odom.velocity_variance[0] = odometry.twist_with_covariance().covariance().data(0); // R row 3, col 3 + odom.velocity_variance[1] = odometry.twist_with_covariance().covariance().data(7); // P row 4, col 4 + odom.velocity_variance[2] = odometry.twist_with_covariance().covariance().data(14); // Y row 5, col 5 + + // odom.reset_counter = vpe.reset_counter; + _visual_odometry_pub.publish(odom); + + pthread_mutex_unlock(&_node_mutex); +} + void GZBridge::Run() { if (should_exit()) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 42ac2ae828..9ea89d36a7 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -62,6 +63,7 @@ #include #include +#include using namespace time_literals; @@ -99,6 +101,7 @@ private: 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); // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -112,6 +115,7 @@ private: 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};