diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 354c7a2320..3b3bc0b44f 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 354c7a23200ddfe388a468165efea48fe81d2623 +Subproject commit 3b3bc0b44fadbd76aa64ef64fe6ac96126142b1f diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index c9c5ea455e..74a60fe3a7 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -242,6 +242,8 @@ private: _gyro_pub(nullptr), _mag_pub(nullptr), _flow_pub(nullptr), + _vision_position_pub(nullptr), + _vision_attitude_pub(nullptr), _dist_pub(nullptr), _battery_pub(nullptr), _initialized(false), @@ -320,6 +322,8 @@ private: orb_advert_t _gyro_pub; orb_advert_t _mag_pub; orb_advert_t _flow_pub; + orb_advert_t _vision_position_pub; + orb_advert_t _vision_attitude_pub; orb_advert_t _dist_pub; orb_advert_t _battery_pub; @@ -334,6 +338,7 @@ private: // class methods int publish_sensor_topics(mavlink_hil_sensor_t *imu); int publish_flow_topic(mavlink_hil_optical_flow_t *flow); + int publish_ev_topic(mavlink_vision_position_estimate_t *ev_mavlink); int publish_distance_topic(mavlink_distance_sensor_t *dist); #ifndef __PX4_QURT diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 7e3a432624..d8bc8b635e 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -345,6 +345,12 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) publish_flow_topic(&flow); break; + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + mavlink_vision_position_estimate_t ev; + mavlink_msg_vision_position_estimate_decode(msg, &ev); + publish_ev_topic(&ev); + break; + case MAVLINK_MSG_ID_DISTANCE_SENSOR: mavlink_distance_sensor_t dist; mavlink_msg_distance_sensor_decode(msg, &dist); @@ -1059,6 +1065,34 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink) return OK; } +int Simulator::publish_ev_topic(mavlink_vision_position_estimate_t *ev_mavlink) +{ + uint64_t timestamp = hrt_absolute_time(); + + struct vehicle_local_position_s vision_position = {}; + + // Use the estimator type to identify the simple vision estimate + vision_position.estimator_type = MAV_ESTIMATOR_TYPE_VISION; + + vision_position.timestamp = timestamp; + vision_position.x = ev_mavlink->x; + vision_position.y = ev_mavlink->y; + vision_position.z = ev_mavlink->z; + + struct vehicle_attitude_s vision_attitude = {}; + + vision_attitude.timestamp = timestamp; + + matrix::Quatf q(matrix::Eulerf(ev_mavlink->roll, ev_mavlink->pitch, ev_mavlink->yaw)); + q.copyTo(vision_attitude.q); + + int inst = 0; + orb_publish_auto(ORB_ID(vehicle_vision_position), &_vision_position_pub, &vision_position, &inst, ORB_PRIO_HIGH); + orb_publish_auto(ORB_ID(vehicle_vision_attitude), &_vision_attitude_pub, &vision_attitude, &inst, ORB_PRIO_HIGH); + + return OK; +} + int Simulator::publish_distance_topic(mavlink_distance_sensor_t *dist_mavlink) { uint64_t timestamp = hrt_absolute_time();