From 14e462706d5a270e8712c7e5bd7cba93b7280c1e Mon Sep 17 00:00:00 2001 From: John Hsu Date: Fri, 17 Jul 2015 08:31:24 +1000 Subject: [PATCH] SITL: added gazebo SITL shim --- libraries/SITL/SIM_Gazebo.cpp | 130 ++++++++++++++++++++++++++++++++++ libraries/SITL/SIM_Gazebo.h | 73 +++++++++++++++++++ 2 files changed, 203 insertions(+) create mode 100644 libraries/SITL/SIM_Gazebo.cpp create mode 100644 libraries/SITL/SIM_Gazebo.h diff --git a/libraries/SITL/SIM_Gazebo.cpp b/libraries/SITL/SIM_Gazebo.cpp new file mode 100644 index 0000000000..fbf4194d49 --- /dev/null +++ b/libraries/SITL/SIM_Gazebo.cpp @@ -0,0 +1,130 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ +/* + simulator connector for ardupilot version of Gazebo +*/ + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include "SIM_Gazebo.h" +#include + +extern const AP_HAL::HAL& hal; + +/* + constructor + */ +Gazebo::Gazebo(const char *home_str, const char *frame_str) : + Aircraft(home_str, frame_str), + last_timestamp(0), + sock(true) +{ + // try to bind to a specific port so that if we restart ArduPilot + // Gazebo keeps sending us packets. Not strictly necessary but + // useful for debugging + sock.bind("127.0.0.1", 9003); + + sock.reuseaddress(); + sock.set_blocking(false); + fprintf(stdout, "bind\n"); +} + +/* + decode and send servos +*/ +void Gazebo::send_servos(const struct sitl_input &input) +{ + servo_packet pkt; + pkt.motor_speed[0] = (input.servos[0]-1000) / 1000.0f; + pkt.motor_speed[1] = (input.servos[1]-1000) / 1000.0f; + pkt.motor_speed[2] = (input.servos[2]-1000) / 1000.0f; + pkt.motor_speed[3] = (input.servos[3]-1000) / 1000.0f; + sock.sendto(&pkt, sizeof(pkt), "127.0.0.1", 9002); +} + +/* + receive an update from the FDM + This is a blocking function + */ +void Gazebo::recv_fdm(const struct sitl_input &input) +{ + fdm_packet pkt; + + /* + we re-send the servo packet every 0.1 seconds until we get a + reply. This allows us to cope with some packet loss to the FDM + */ + while (sock.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) { + send_servos(input); + } + + // get imu stuff + accel_body = Vector3f(pkt.imu_linear_acceleration_xyz[0], + pkt.imu_linear_acceleration_xyz[1], + pkt.imu_linear_acceleration_xyz[2]); + + gyro = Vector3f(pkt.imu_angular_velocity_rpy[0], + pkt.imu_angular_velocity_rpy[1], + pkt.imu_angular_velocity_rpy[2]); + + // compute dcm from imu orientation + Quaternion quat(pkt.imu_orientation_quat[0], + pkt.imu_orientation_quat[1], + pkt.imu_orientation_quat[2], + pkt.imu_orientation_quat[3]); + quat.rotation_matrix(dcm); + + double speedN = pkt.velocity_xyz[0]; + double speedE = pkt.velocity_xyz[1]; + double speedD = pkt.velocity_xyz[2]; + velocity_ef = Vector3f(speedN, speedE, speedD); + + position = Vector3f(pkt.position_xyz[0], + pkt.position_xyz[1], + pkt.position_xyz[2]); + + + // auto-adjust to simulation frame rate + double deltat = pkt.timestamp - last_timestamp; + time_now_us += deltat * 1.0e6; + + if (deltat < 0.01 && deltat > 0) { + adjust_frame_time(1.0/deltat); + } + last_timestamp = pkt.timestamp; + + /* copied below from iris_ros.py */ + /* + bearing = to_degrees(atan2(position.y, position.x)); + distance = math.sqrt(self.position.x**2 + self.position.y**2) + (self.latitude, self.longitude) = util.gps_newpos( + self.home_latitude, self.home_longitude, bearing, distance) + self.altitude = self.home_altitude - self.position.z + velocity_body = self.dcm.transposed() * self.velocity + self.accelerometer = self.accel_body.copy() + */ +} + +/* + update the Gazebo simulation by one time step + */ +void Gazebo::update(const struct sitl_input &input) +{ + send_servos(input); + recv_fdm(input); + update_position(); +} +#endif // CONFIG_HAL_BOARD diff --git a/libraries/SITL/SIM_Gazebo.h b/libraries/SITL/SIM_Gazebo.h new file mode 100644 index 0000000000..0adc21902f --- /dev/null +++ b/libraries/SITL/SIM_Gazebo.h @@ -0,0 +1,73 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ +/* + simulator connection for ardupilot version of Gazebo +*/ + +#ifndef _SIM_GAZEBO_H +#define _SIM_GAZEBO_H + +#include "SIM_Aircraft.h" +#include + +/* + Gazebo simulator + */ +class Gazebo : public Aircraft +{ +public: + Gazebo(const char *home_str, const char *frame_str); + + /* update model by one time step */ + void update(const struct sitl_input &input); + + /* static object creator */ + static Aircraft *create(const char *home_str, const char *frame_str) { + return new Gazebo(home_str, frame_str); + } + +private: + /* + packet sent to Gazebo + */ + struct servo_packet { + float motor_speed[4]; + }; + + /* + reply packet sent from Gazebo to ArduPilot + */ + struct fdm_packet { + double timestamp; + double imu_angular_velocity_rpy[3]; + double imu_linear_acceleration_xyz[3]; + double imu_orientation_quat[3]; + double velocity_xyz[3]; + double position_xyz[3]; + }; + + void send_servos_heli(const struct sitl_input &input); + void send_servos_fixed_wing(const struct sitl_input &input); + void recv_fdm(const struct sitl_input &input); + void send_servos(const struct sitl_input &input); + + bool heli_servos; + double last_timestamp; + SocketAPM sock; +}; + + +#endif // _SIM_GAZEBO_H