diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index a579c99d8e44f..8269b708ed9bd 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -79,6 +79,9 @@ def default_frame(self): def uses_vicon(self): return True + def uses_viso(self): + return True + def close(self): super(AutoTestCopter, self).close() @@ -1512,6 +1515,96 @@ def fly_vision_position(self): if ex is not None: raise ex + def fly_visual_odometry(self): + """Disable GPS navigation, enable visual odometry input.""" + # scribble down a location we can set origin to: + self.progress("Waiting for location") + self.mavproxy.send('switch 6\n') # stabilize mode + self.wait_heartbeat() + self.wait_mode('STABILIZE') + self.wait_ready_to_arm() + + old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) + print("old_pos=%s" % str(old_pos)) + + self.context_push() + + ex = None + try: + self.set_parameter("SIM_VISO_ENABLE", 1) + self.set_parameter("SIM_VISO_ERROR", 0) + self.set_parameter("SIM_VISO_SCALE", 1) + self.set_parameter("GPS_TYPE", 0) + self.set_parameter("EK2_GPS_TYPE", 3) + self.set_parameter("SERIAL6_PROTOCOL", 1) + + self.reboot_sitl() + # without a GPS or some sort of external prompting, AP + # doesn't send system_time messages. So prompt it: + self.mav.mav.system_time_send(int(time.time() * 1000000), 0) + self.progress("Waiting for non-zero-lat") + tstart = self.get_sim_time() + while True: + self.mav.mav.set_gps_global_origin_send(1, + old_pos.lat, + old_pos.lon, + old_pos.alt) + gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', + blocking=True) + self.progress("gpi=%s" % str(gpi)) + if gpi.lat != 0: + break + + if self.get_sim_time_cached() - tstart > 60: + raise AutoTestTimeoutException("Did not get non-zero lat") + + self.takeoff() + self.set_rc(1, 1600) + tstart = self.get_sim_time() + while True: + vision_pos = self.mav.recv_match(type='VISION_POSITION_ESTIMATE', + blocking=True) + # print("vpe=%s" % str(vision_pos)) + self.mav.recv_match(type='GLOBAL_POSITION_INT', + blocking=True) + # self.progress("gpi=%s" % str(gpi)) + if vision_pos.x > 40: + break + + if self.get_sim_time_cached() - tstart > 100: + raise AutoTestTimeoutException("Vicon showed no movement") + + # recenter controls: + self.set_rc(1, 1500) + self.progress("# Enter RTL") + self.mavproxy.send('switch 3\n') + self.set_rc(3, 1500) + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 200: + raise NotAchievedException("Did not disarm") + self.mav.recv_match(type='GLOBAL_POSITION_INT', + blocking=True) + # print("gpi=%s" % str(gpi)) + self.mav.recv_match(type='SIMSTATE', + blocking=True) + # print("ss=%s" % str(ss)) + # wait for RTL disarm: + if not self.armed(): + break + + except Exception as e: + self.progress("Exception caught: %s" % ( + self.get_exception_stacktrace(e))) + ex = e + + self.context_pop() + self.zero_throttle() + self.reboot_sitl() + + if ex is not None: + raise ex + def fly_rtl_speed(self): """Test RTL Speed parameters""" rtl_speed_ms = 7 @@ -3726,6 +3819,10 @@ def tests(self): "Fly Vision Position", self.fly_vision_position), + ("VisualOdometry", + "Fly Visual Odometry", + self.fly_visual_odometry), + ("BeaconPosition", "Fly Beacon Position", self.fly_beacon_position), diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index ce9baeea85afc..5df5d8519bcd3 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -2112,6 +2112,9 @@ def check_test_syntax(self, test_file): def uses_vicon(self): return False + def uses_viso(self): + return False + def defaults_filepath(self): return None @@ -2151,6 +2154,7 @@ def init(self): speedup=self.speedup, valgrind=self.valgrind, vicon=self.uses_vicon(), + viso=self.uses_viso(), wipe=True, ) diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index 4d6d8ef171486..2f01ebb46321f 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -241,6 +241,7 @@ def start_SITL(binary, breakpoints=[], disable_breakpoints=False, vicon=False, + viso=False, lldb=False): """Launch a SITL instance.""" cmd = [] @@ -320,6 +321,8 @@ def start_SITL(binary, cmd.extend(['--unhide-groups']) if vicon: cmd.extend(["--uartF=sim:vicon:"]) + if viso: + cmd.extend(["--uartG=sim:viso:"]) if gdb and not os.getenv('DISPLAY'): subprocess.Popen(cmd) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index fea3d0179f472..c81020ccd571e 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -222,6 +222,12 @@ int SITL_State::sim_fd(const char *name, const char *arg) } vicon = new SITL::Vicon(); return vicon->fd(); + } else if (streq(name, "viso")) { + if (viso != nullptr) { + AP_HAL::panic("Only one visual odometry system at a time"); + } + viso = new SITL::VisualOdometry(); + return viso->fd(); } AP_HAL::panic("unknown simulated device: %s", name); } @@ -358,6 +364,12 @@ void SITL_State::_fdm_input_local(void) vicon->update(sitl_model->get_location(), sitl_model->get_position(), attitude); + } else if (viso != nullptr) { + Quaternion attitude; + sitl_model->get_attitude(attitude); + viso->update(sitl_model->get_location(), + sitl_model->get_position(), + attitude); } if (_sitl && _use_fg_view) { diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index f3f170fead077..153f0b4c0aeac 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -24,6 +24,7 @@ #include #include #include +#include #include class HAL_SITL; @@ -222,6 +223,9 @@ class HALSITL::SITL_State { // simulated vicon system: SITL::Vicon *vicon; + // simulated visual odometry system: + SITL::VisualOdometry *viso; + // output socket for flightgear viewing SocketAPM fg_socket{true}; diff --git a/libraries/SITL/SIM_VisualOdometry.cpp b/libraries/SITL/SIM_VisualOdometry.cpp new file mode 100644 index 0000000000000..3afe6ad0c9488 --- /dev/null +++ b/libraries/SITL/SIM_VisualOdometry.cpp @@ -0,0 +1,195 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + visual odometry simulator class +*/ + +#include "SIM_VisualOdometry.h" +#include +#include +#include + +using namespace SITL; + +VisualOdometry::VisualOdometry() +{ + int tmp[2]; + if (pipe(tmp) == -1) { + AP_HAL::panic("pipe() failed"); + } + fd_my_end = tmp[1]; + fd_their_end = tmp[0]; + + // close file descriptors on exec: + fcntl(fd_my_end, F_SETFD, FD_CLOEXEC); + fcntl(fd_their_end, F_SETFD, FD_CLOEXEC); + + // make sure we don't screw the simulation up by blocking: + fcntl(fd_my_end, F_SETFL, fcntl(fd_my_end, F_GETFL, 0) | O_NONBLOCK); + fcntl(fd_their_end, F_SETFL, fcntl(fd_their_end, F_GETFL, 0) | O_NONBLOCK); + + if (!valid_channel(mavlink_ch)) { + AP_HAL::panic("Invalid mavlink channel"); + } +} + +void VisualOdometry::maybe_send_heartbeat() +{ + const uint32_t now = AP_HAL::millis(); + + if (now - last_heartbeat_ms < 100) { + // we only provide a heartbeat every so often + return; + } + last_heartbeat_ms = now; + + mavlink_message_t msg; + mavlink_msg_heartbeat_pack(system_id, + component_id, + &msg, + MAV_TYPE_GCS, + MAV_AUTOPILOT_INVALID, + 0, + 0, + 0); +} + +void VisualOdometry::update_vision_position_estimate(Vector3f &position, Quaternion &attitude) +{ + const uint64_t now_us = AP_HAL::micros64(); + + if (time_offset_us == 0) { + time_offset_us = (unsigned(random()) % 7000) * 1000000ULL; + printf("time_offset_us %llu\n", (long long unsigned)time_offset_us); + } + + if (time_send_us && now_us >= time_send_us) { + uint8_t msgbuf[300]; + uint16_t msgbuf_len = mavlink_msg_to_send_buffer(msgbuf, &obs_msg); + + if (::write(fd_my_end, (void*)&msgbuf, msgbuf_len) != msgbuf_len) { + ::fprintf(stderr, "VisualOdometry: write failure\n"); + } + time_send_us = 0; + } + if (time_send_us != 0) { + // waiting for the last msg to go out + return; + } + + if (now_us - last_observation_usec < 70000) { + // create observations at 70ms intervals (matches EK2 max rate) + return; + } + + float roll; + float pitch; + float yaw; + attitude.to_euler(roll, pitch, yaw); + + // VISION_POSITION_ESTIMATE message + mavlink_msg_vision_position_estimate_pack_chan( + system_id, + component_id, + mavlink_ch, + &obs_msg, + now_us + time_offset_us, + position.x, + position.y, + position.z, + roll, + pitch, + yaw, + NULL, 0); + + uint32_t delay_ms = 25 + unsigned(random()) % 300; + time_send_us = now_us + delay_ms * 1000UL; +} + +bool VisualOdometry::init_sitl_pointer() +{ + if (_sitl == nullptr) { + _sitl = AP::sitl(); + if (_sitl == nullptr) { + return false; + } + } + return true; +} + +void VisualOdometry::save_original_state(const Vector3f &position, const Quaternion &attitude) +{ + original_position = position; + original_attitude = attitude; +} +/* + * simulate the normal behaviour of visual odometry + */ +void VisualOdometry::simulate_normal_behaviour(Vector3f &simulated_position, + Quaternion &simulated_attitude, + const Vector3f &true_position, + const Quaternion &true_attitude, + float scale_factor) +{ + // apply offsets so that the origin is (0,0,0) and heading is 0 + simulated_position = true_position - original_position; + simulated_attitude = true_attitude * original_attitude.inverse(); + + // position from visual odometry might be off from ground truth by a scale factor + const Vector3f scale(scale_factor, scale_factor, scale_factor); + simulated_position *= scale; +} + +/* + update simulated visual odometry state based on true state + */ +void VisualOdometry::update(const Location &true_loc, const Vector3f &true_position, const Quaternion &true_attitude) +{ + if (!init_sitl_pointer()) { + return; + } + + // use SIM_VISO_ENABLE param to start and stop sending visual odometry messages + if (_sitl->viso_enable) { + Vector3f simulated_position; + Quaternion simulated_attitude; + + maybe_send_heartbeat(); + + simulate_normal_behaviour(simulated_position, simulated_attitude, true_position, true_attitude, _sitl->viso_scale_factor); + + // different scenarios that might happen during operation + // for now, we only handle position-related cases + if (_sitl->viso_error_type == SITL::SITL::SITL_VISOFail_Diverge) { + // when divergence happens, position will gradually move to infinity from last good position + static Vector3f diverged_position = simulated_position; + + diverged_position *= _sitl->viso_divergence_rate; + + simulated_position = diverged_position; + } else if (_sitl->viso_error_type == SITL::SITL::SITL_VISOFail_Jump) { + // when loop closure or relocalization occur, position will suddenly jump erratically in any direction + // after which the position will resume normal behavior. + static Vector3f jump_vector(1, 1, 1); // an example jump of 1m in all directions + + simulated_position += jump_vector; + } + + update_vision_position_estimate(simulated_position, simulated_attitude); + } else { + // original position and heading of visual odometry are reset everytime viso is enabled + save_original_state(true_position, true_attitude); + } +} \ No newline at end of file diff --git a/libraries/SITL/SIM_VisualOdometry.h b/libraries/SITL/SIM_VisualOdometry.h new file mode 100644 index 0000000000000..c60771a881a58 --- /dev/null +++ b/libraries/SITL/SIM_VisualOdometry.h @@ -0,0 +1,79 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + visual odometry simulator class +*/ + +#pragma once + +#include "SIM_Aircraft.h" + +#include + +#include + +namespace SITL { + +class VisualOdometry { +public: + + VisualOdometry(); + + // update state + void update(const Location &true_loc, const Vector3f &true_position, const Quaternion &true_attitude); + + // return fd on which data from the device can be read, and data + // to the device can be written + int fd() { return fd_their_end; } + +private: + + SITL *_sitl; + + // TODO: make these parameters: + const uint8_t system_id = 17; + const uint8_t component_id = 18; + + // we share channels with the ArduPilot binary! + const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+5); + + int fd_their_end; + int fd_my_end; + + uint32_t last_heartbeat_ms; + uint64_t last_observation_usec; + uint64_t time_send_us; + uint64_t time_offset_us; + mavlink_message_t obs_msg; + + Vector3f original_position; + Quaternion original_attitude; + + void save_original_state(const Vector3f &position, const Quaternion &attitude); + + void simulate_normal_behaviour(Vector3f &simulated_position, + Quaternion &simulated_attitude, + const Vector3f &true_position, + const Quaternion &true_attitude, + float scale_factor); + + void update_vision_position_estimate(Vector3f &true_position, Quaternion &true_attitude); + + void maybe_send_heartbeat(); + + bool init_sitl_pointer(); +}; + +} diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index fee7772760c37..05e0723920cd8 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -189,6 +189,12 @@ const AP_Param::GroupInfo SITL::var_info2[] = { // extra delay per main loop AP_GROUPINFO("LOOP_DELAY", 55, SITL, loop_delay, 0), + // parameters for visual odometry + AP_GROUPINFO("VISO_ENABLE", 56, SITL, viso_enable, 0), + AP_GROUPINFO("VISO_SCALE", 57, SITL, viso_scale_factor, 1.0f), + AP_GROUPINFO("VISO_ERROR", 58, SITL, viso_error_type, 0), + AP_GROUPINFO("VISO_DIVRATE", 59, SITL, viso_divergence_rate, 1.01f), + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index f33b0723e770a..a4e63350fe6e3 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -236,6 +236,18 @@ class SITL { AP_Int8 gyro_fail_mask; AP_Int8 accel_fail_mask; + // visual odometry: scale factor and error types + AP_Int8 viso_enable; + AP_Int8 viso_error_type; + AP_Float viso_scale_factor; + AP_Float viso_divergence_rate; + + enum SITL_VISOFail { + SITL_VISOFail_None = 0, + SITL_VISOFail_Diverge = 1, + SITL_VISOFail_Jump = 2, + }; + struct { AP_Float x; AP_Float y;