From 9ccd7d9b628df63805788dd91a95d8e882762596 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 3 Apr 2017 10:52:39 +0200 Subject: [PATCH] init commit --- CMakeLists.txt | 49 ++ include/ArduCopterIRLockPlugin.hh | 66 ++ include/ArduPilotPlugin.hh | 96 +++ src/ArduCopterIRLockPlugin.cc | 308 +++++++ src/ArduPilotPlugin.cc | 1291 +++++++++++++++++++++++++++++ 5 files changed, 1810 insertions(+) create mode 100644 CMakeLists.txt create mode 100755 include/ArduCopterIRLockPlugin.hh create mode 100755 include/ArduPilotPlugin.hh create mode 100755 src/ArduCopterIRLockPlugin.cc create mode 100755 src/ArduPilotPlugin.cc diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..d6e2a7d1 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 2.8 FATAL_ERROR) + + +project(ardupilot_sitl_gazebo) + + +####################### +## Find Dependencies ## +####################### + +find_package(gazebo REQUIRED) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") + +if("${GAZEBO_VERSION}" VERSION_LESS "7.0") + message(FATAL_ERROR "You need at least Gazebo 7.0. Your version: ${GAZEBO_VERSION}") +else() + message("Gazebo version: ${GAZEBO_VERSION}") +endif() + +########### +## Build ## +########### + +include_directories( + ${PROJECT_SOURCE_DIR} + include + ${GAZEBO_INCLUDE_DIRS} + ) + +link_libraries( + ${CMAKE_CURRENT_BINARY_DIR} + ${GAZEBO_LIBRARIES} + ) + +link_directories( + ${GAZEBO_LIBRARY_DIRS} + ) + +set (plugins_single_header + ArduPilotPlugin + ) +#add_library(ArduCopterIRLockPlugin SHARED src/ArduCopterIRLockPlugin.cc) +#target_link_libraries(ArduCopterIRLockPlugin ${GAZEBO_LIBRARIES}) + +add_library(ArduPilotPlugin SHARED src/ArduPilotPlugin.cc) +target_link_libraries(ArduPilotPlugin ${GAZEBO_LIBRARIES}) + +#install(TARGETS ArduCopterIRLockPlugin DESTINATION ${GAZEBO_PLUGIN_PATH}) +install(TARGETS ArduPilotPlugin DESTINATION ${GAZEBO_PLUGIN_PATH}) \ No newline at end of file diff --git a/include/ArduCopterIRLockPlugin.hh b/include/ArduCopterIRLockPlugin.hh new file mode 100755 index 00000000..7aebef91 --- /dev/null +++ b/include/ArduCopterIRLockPlugin.hh @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef _GAZEBO_ARDUCOPTERIRLOCK_PLUGIN_HH_ +#define _GAZEBO_ARDUCOPTERIRLOCK_PLUGIN_HH_ + +#include +#include + +#include "gazebo/common/Plugin.hh" +#include "gazebo/util/system.hh" + +namespace gazebo +{ + // Forward declare private class. + class ArduCopterIRLockPluginPrivate; + + /// \brief A camera sensor plugin for fiducial detection + class GAZEBO_VISIBLE ArduCopterIRLockPlugin : public SensorPlugin + { + /// \brief Constructor + public: ArduCopterIRLockPlugin(); + + /// \brief Destructor + public: virtual ~ArduCopterIRLockPlugin(); + + // Documentation Inherited. + public: void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf); + + /// \brief Callback when a new camera frame is available + /// \param[in] _image image data + /// \param[in] _width image width + /// \param[in] _height image height + /// \param[in] _depth image depth + /// \param[in] _format image format + public: virtual void OnNewFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format); + + /// \brief Publish the result + /// \param[in] _name Name of fiducial + /// \param[in] _x x position in image + /// \param[in] _y y position in image + public: virtual void Publish(const std::string &_fiducial, unsigned int _x, + unsigned int _y); + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +#endif diff --git a/include/ArduPilotPlugin.hh b/include/ArduPilotPlugin.hh new file mode 100755 index 00000000..b449cacb --- /dev/null +++ b/include/ArduPilotPlugin.hh @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GAZEBO_PLUGINS_ARDUPILOTPLUGIN_HH_ +#define GAZEBO_PLUGINS_ARDUPILOTPLUGIN_HH_ + +#include +#include +#include + +namespace gazebo +{ + // Forward declare private data class + class ArduPilotSocketPrivate; + class ArduPilotPluginPrivate; + + /// \brief Interface ArduPilot from ardupilot stack + /// modeled after SITL/SIM_* + /// + /// The plugin requires the following parameters: + /// control description block + /// + /// channel attribute, ardupilot control channel + /// multiplier command multiplier + /// + /// type type of control, VELOCITY, POSITION or EFFORT + /// velocity pid p gain + /// velocity pid i gain + /// velocity pid d gain + /// velocity pid max integral correction + /// velocity pid min integral correction + /// velocity pid max command torque + /// velocity pid min command torque + /// motor joint, torque applied here + /// rotor turning direction, 'cw' or 'ccw' + /// frequencyCutoff filter incoming joint state + /// samplingRate sampling rate for filtering incoming joint state + /// for rotor aliasing problem, experimental + /// scoped name for the imu sensor + /// timeout before giving up on + /// controller synchronization + class GAZEBO_VISIBLE ArduPilotPlugin : public ModelPlugin + { + /// \brief Constructor. + public: ArduPilotPlugin(); + + /// \brief Destructor. + public: ~ArduPilotPlugin(); + + // Documentation Inherited. + public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + /// \brief Update the control surfaces controllers. + /// \param[in] _info Update information provided by the server. + private: void OnUpdate(); + + /// \brief Update PID Joint controllers. + /// \param[in] _dt time step size since last update. + private: void ApplyMotorForces(const double _dt); + + /// \brief Reset PID Joint controllers. + private: void ResetPIDs(); + + /// \brief Receive motor commands from ArduPilot + private: void ReceiveMotorCommand(); + + /// \brief Send state to ArduPilot + private: void SendState() const; + + /// \brief Init ardupilot socket + private: bool InitArduPilotSockets(sdf::ElementPtr _sdf) const; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + + /// \brief transform from model orientation to x-forward and z-up + private: ignition::math::Pose3d modelXYZToAirplaneXForwardZDown; + + /// \brief transform from world frame to NED frame + private: ignition::math::Pose3d gazeboXYZToNED; + }; +} +#endif diff --git a/src/ArduCopterIRLockPlugin.cc b/src/ArduCopterIRLockPlugin.cc new file mode 100755 index 00000000..129b75d0 --- /dev/null +++ b/src/ArduCopterIRLockPlugin.cc @@ -0,0 +1,308 @@ +/* + * Copyright (C) 2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#ifdef _WIN32 + #include + #include + #include + #include + using raw_type = char; +#else + #include + #include + #include + #include + using raw_type = void; +#endif + +#if defined(_MSC_VER) + #include + typedef SSIZE_T ssize_t; +#endif + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "include/ArduCopterIRLockPlugin.hh" + +using namespace gazebo; +GZ_REGISTER_SENSOR_PLUGIN(ArduCopterIRLockPlugin) + +namespace gazebo +{ + /// \brief Obtains a parameter from sdf. + /// \param[in] _sdf Pointer to the sdf object. + /// \param[in] _name Name of the parameter. + /// \param[out] _param Param Variable to write the parameter to. + /// \param[in] _default_value Default value, if the parameter not available. + /// \param[in] _verbose If true, gzerror if the parameter is not available. + /// \return True if the parameter was found in _sdf, false otherwise. + template + bool getSdfParam(sdf::ElementPtr _sdf, const std::string &_name, + T &_param, const T &_defaultValue, const bool &_verbose = false) + { + if (_sdf->HasElement(_name)) + { + _param = _sdf->GetElement(_name)->Get(); + return true; + } + + _param = _defaultValue; + if (_verbose) + { + gzerr << "[ArduPilotPlugin] Please specify a value for parameter [" + << _name << "].\n"; + } + return false; + } + + class ArduCopterIRLockPluginPrivate + { + /// \brief Pointer to the parent camera sensor + public: sensors::CameraSensorPtr parentSensor; + + /// \brief Selection buffer used for occlusion detection + public: std::unique_ptr selectionBuffer; + + /// \brief All event connections. + public: std::vector connections; + + /// \brief A list of fiducials tracked by this camera. + public: std::vector fiducials; + + /// \brief Irlock address + public: std::string irlock_addr; + + /// \brief Irlock port for receiver socket + public: uint16_t irlock_port; + + public: int handle; + + public: struct irlockPacket + { + uint64_t timestamp; + uint16_t num_targets; + float pos_x; + float pos_y; + float size_x; + float size_y; + }; + }; +} + +///////////////////////////////////////////////// +ignition::math::Vector2i GetScreenSpaceCoords(ignition::math::Vector3d _pt, + gazebo::rendering::CameraPtr _cam) +{ + // Convert from 3D world pos to 2D screen pos + Ogre::Vector3 pos = _cam->OgreCamera()->getProjectionMatrix() * + _cam->OgreCamera()->getViewMatrix() * + gazebo::rendering::Conversions::Convert(_pt); + + ignition::math::Vector2i screenPos; + screenPos.X() = ((pos.x / 2.0) + 0.5) * _cam->ViewportWidth(); + screenPos.Y() = (1 - ((pos.y / 2.0) + 0.5)) * _cam->ViewportHeight(); + + return screenPos; +} + +///////////////////////////////////////////////// +ArduCopterIRLockPlugin::ArduCopterIRLockPlugin() + : SensorPlugin(), + dataPtr(new ArduCopterIRLockPluginPrivate) +{ + // socket + this->dataPtr->handle = socket(AF_INET, SOCK_DGRAM /*SOCK_STREAM*/, 0); + #ifndef _WIN32 + // Windows does not support FD_CLOEXEC + fcntl(this->dataPtr->handle, F_SETFD, FD_CLOEXEC); + #endif + int one = 1; + setsockopt(this->dataPtr->handle, IPPROTO_TCP, TCP_NODELAY, + reinterpret_cast(&one), sizeof(one)); + setsockopt(this->dataPtr->handle, SOL_SOCKET, SO_REUSEADDR, + reinterpret_cast(&one), sizeof(one)); + + #ifdef _WIN32 + u_long on = 1; + ioctlsocket(this->dataPtr->handle, FIONBIO, + reinterpret_cast(&on)); + #else + fcntl(this->dataPtr->handle, F_SETFL, + fcntl(this->dataPtr->handle, F_GETFL, 0) | O_NONBLOCK); + #endif +} + +///////////////////////////////////////////////// +ArduCopterIRLockPlugin::~ArduCopterIRLockPlugin() +{ + this->dataPtr->connections.clear(); + this->dataPtr->parentSensor.reset(); +} + +///////////////////////////////////////////////// +void ArduCopterIRLockPlugin::Load(sensors::SensorPtr _sensor, + sdf::ElementPtr _sdf) +{ + this->dataPtr->parentSensor = + std::dynamic_pointer_cast(_sensor); + + if (!this->dataPtr->parentSensor) + { + gzerr << "ArduCopterIRLockPlugin not attached to a camera sensor\n"; + return; + } + + // load the fiducials + if (_sdf->HasElement("fiducial")) + { + sdf::ElementPtr elem = _sdf->GetElement("fiducial"); + while (elem) + { + this->dataPtr->fiducials.push_back(elem->Get()); + elem = elem->GetNextElement("fiducial"); + } + } + else + { + gzerr << "No fidicuals specified. ArduCopterIRLockPlugin will not be run." + << std::endl; + return; + } + getSdfParam(_sdf, "irlock_addr", + this->dataPtr->irlock_addr, "127.0.0.1"); + getSdfParam(_sdf, "irlock_port", + this->dataPtr->irlock_port, 9005); + + this->dataPtr->parentSensor->SetActive(true); + + this->dataPtr->connections.push_back( + this->dataPtr->parentSensor->Camera()->ConnectNewImageFrame( + std::bind(&ArduCopterIRLockPlugin::OnNewFrame, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5))); +} + +///////////////////////////////////////////////// +void ArduCopterIRLockPlugin::OnNewFrame(const unsigned char * /*_image*/, + unsigned int /*_width*/, unsigned int /*_height*/, unsigned int /*_depth*/, + const std::string &/*_format*/) +{ + rendering::CameraPtr camera = this->dataPtr->parentSensor->Camera(); + rendering::ScenePtr scene = camera->GetScene(); + + if (!this->dataPtr->selectionBuffer) + { + std::string cameraName = camera->OgreCamera()->getName(); + this->dataPtr->selectionBuffer.reset( + new rendering::SelectionBuffer(cameraName, scene->OgreSceneManager(), + camera->RenderTexture()->getBuffer()->getRenderTarget())); + } + + for (const auto &f : this->dataPtr->fiducials) + { + // check if fiducial is visible within the frustum + rendering::VisualPtr vis = scene->GetVisual(f); + if (!vis) + continue; + + if (!camera->IsVisible(vis)) + continue; + + ignition::math::Vector2i pt = GetScreenSpaceCoords( + vis->WorldPose().Pos(), camera); + + // use selection buffer to check if visual is occluded by other entities + // in the camera view + Ogre::Entity *entity = + this->dataPtr->selectionBuffer->OnSelectionClick(pt.X(), pt.Y()); + + rendering::VisualPtr result; + if (entity && !entity->getUserObjectBindings().getUserAny().isEmpty()) + { + try + { + result = scene->GetVisual( + Ogre::any_cast( + entity->getUserObjectBindings().getUserAny())); + } + catch(Ogre::Exception &e) + { + gzerr << "Ogre Error:" << e.getFullDescription() << "\n"; + continue; + } + } + + if (result && result->GetRootVisual() == vis) + { + this->Publish(vis->Name(), pt.X(), pt.Y()); + } + } +} + +///////////////////////////////////////////////// +void ArduCopterIRLockPlugin::Publish(const std::string &/*_fiducial*/, + unsigned int _x, unsigned int _y) +{ + rendering::CameraPtr camera = this->dataPtr->parentSensor->Camera(); + + const double imageWidth = this->dataPtr->parentSensor->ImageWidth(); + const double imageHeight = this->dataPtr->parentSensor->ImageHeight(); + const double hfov = camera->HFOV().Radian(); + const double vfov = camera->VFOV().Radian(); + const double pixelsPerRadianX = imageWidth / hfov; + const double pixelsPerRadianY = imageHeight / vfov; + const float angleX = static_cast( + (static_cast(_x) - (imageWidth * 0.5)) / pixelsPerRadianX); + const float angleY = static_cast( + -((imageHeight * 0.5) - static_cast(_y)) / pixelsPerRadianY); + + // send_packet + ArduCopterIRLockPluginPrivate::irlockPacket pkt; + + pkt.timestamp = static_cast + (1.0e3*this->dataPtr->parentSensor->LastMeasurementTime().Double()); + pkt.num_targets = static_cast(1); + pkt.pos_x = angleX; + pkt.pos_y = angleY; + // 1x1 pixel box for now + pkt.size_x = static_cast(1); + pkt.size_y = static_cast(1); + + // std::cerr << "fiducial '" << _fiducial << "':" << _x << ", " << _y + // << ", pos: " << pkt.pos_x << ", " << pkt.pos_y << std::endl; + + struct sockaddr_in sockaddr; + memset(&sockaddr, 0, sizeof(sockaddr)); + sockaddr.sin_port = htons(this->dataPtr->irlock_port); + sockaddr.sin_family = AF_INET; + sockaddr.sin_addr.s_addr = inet_addr(this->dataPtr->irlock_addr.c_str()); + ::sendto(this->dataPtr->handle, + reinterpret_cast(&pkt), + sizeof(pkt), 0, + (struct sockaddr *)&sockaddr, sizeof(sockaddr)); +} diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc new file mode 100755 index 00000000..bfa9f931 --- /dev/null +++ b/src/ArduPilotPlugin.cc @@ -0,0 +1,1291 @@ +/* + * Copyright (C) 2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include +#ifdef _WIN32 + #include + #include + #include + #include + using raw_type = char; +#else + #include + #include + #include + #include + using raw_type = void; +#endif + +#if defined(_MSC_VER) + #include + typedef SSIZE_T ssize_t; +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "include/ArduPilotPlugin.hh" + +// DON'T MERGE +#include "gazebo/common/CommonTypes.hh" +#include "gazebo/physics/Link.hh" +#include "gazebo/physics/World.hh" +#include "gazebo/physics/PhysicsEngine.hh" +#include "gazebo/physics/Model.hh" +// END DON'T MERGE + +#define MAX_MOTORS 255 + +using namespace gazebo; + +GZ_REGISTER_MODEL_PLUGIN(ArduPilotPlugin) + +/// \brief Obtains a parameter from sdf. +/// \param[in] _sdf Pointer to the sdf object. +/// \param[in] _name Name of the parameter. +/// \param[out] _param Param Variable to write the parameter to. +/// \param[in] _default_value Default value, if the parameter not available. +/// \param[in] _verbose If true, gzerror if the parameter is not available. +/// \return True if the parameter was found in _sdf, false otherwise. +template +bool getSdfParam(sdf::ElementPtr _sdf, const std::string &_name, + T &_param, const T &_defaultValue, const bool &_verbose = false) +{ + if (_sdf->HasElement(_name)) + { + _param = _sdf->GetElement(_name)->Get(); + return true; + } + + _param = _defaultValue; + if (_verbose) + { + gzerr << "[ArduPilotPlugin] Please specify a value for parameter [" + << _name << "].\n"; + } + return false; +} + +// DON'T MERGE +std::vector getSensorScopedName(physics::ModelPtr _model, + const std::string _name) +{ + std::vector names; + for (gazebo::physics::Link_V::const_iterator iter = _model->GetLinks().begin(); + iter != _model->GetLinks().end(); ++iter) + { + for (unsigned int j = 0; j < (*iter)->GetSensorCount(); ++j) + { + if ((*iter)->GetSensorName(j).size() < _name.size()) + { + continue; + } + if ((*iter)->GetSensorName(j).substr( + (*iter)->GetSensorName(j).size() + - _name.size(), _name.size()) == + _name) + { + names.push_back((*iter)->GetSensorName(j)); + } + } + } + return names; +} +// END DON'T MERGE +/// \brief A servo packet. +struct ServoPacket +{ + /// \brief Motor speed data. + /// should rename to servo_command here and in ArduPilot SIM_Gazebo.cpp + float motorSpeed[MAX_MOTORS] = {0.0f}; +}; + +/// \brief Flight Dynamics Model packet that is sent back to the ArduPilot +struct fdmPacket +{ + /// \brief packet timestamp + double timestamp; + + /// \brief IMU angular velocity + double imuAngularVelocityRPY[3]; + + /// \brief IMU linear acceleration + double imuLinearAccelerationXYZ[3]; + + /// \brief IMU quaternion orientation + double imuOrientationQuat[4]; + + /// \brief Model velocity in NED frame + double velocityXYZ[3]; + + /// \brief Model position in NED frame + double positionXYZ[3]; + + /// \brief Model latitude in WGS84 system + double latitude = 0.0; + + /// \brief Model longitude in WGS84 system + double longitude = 0.0; + + /// \brief Model altitude from GPS + double altitude = 0.0; + + /// \brief Model estimated from airspeed sensor (e.g. Pitot) in m/s + double airspeed = 0.0; + + /// \brief Battery voltage. Default to -1 to use sitl estimator. + double battery_voltage = -1.0; + + /// \brief Battery Current. + double battery_current = 0.0; + + /// \brief Model rangefinder value. Default to -1 to use sitl rangefinder. + double rangefinder = -1.0; +}; + +/// \brief Control class +class Control +{ + /// \brief Constructor + public: Control() + { + // most of these coefficients are not used yet. + this->rotorVelocitySlowdownSim = this->kDefaultRotorVelocitySlowdownSim; + this->frequencyCutoff = this->kDefaultFrequencyCutoff; + this->samplingRate = this->kDefaultSamplingRate; + + this->pid.Init(0.1, 0, 0, 0, 0, 1.0, -1.0); + } + + /// \brief control id / channel + public: int channel = 0; + + /// \brief Next command to be applied to the propeller + public: double cmd = 0; + + /// \brief Velocity PID for motor control + public: common::PID pid; + + /// \brief Control type. Can be: + /// VELOCITY control velocity of joint + /// POSITION control position of joint + /// EFFORT control effort of joint + public: std::string type; + + /// \brief use force controler + public: bool useForce = true; + + /// \brief Control propeller joint. + public: std::string jointName; + + /// \brief Control propeller joint. + public: physics::JointPtr joint; + + /// \brief direction multiplier for this control + public: double multiplier = 1; + + /// \brief input command offset + public: double offset = 0; + + /// \brief unused coefficients + public: double rotorVelocitySlowdownSim; + public: double frequencyCutoff; + public: double samplingRate; + public: ignition::math::OnePole filter; + + public: static double kDefaultRotorVelocitySlowdownSim; + public: static double kDefaultFrequencyCutoff; + public: static double kDefaultSamplingRate; +}; + +double Control::kDefaultRotorVelocitySlowdownSim = 10.0; +double Control::kDefaultFrequencyCutoff = 5.0; +double Control::kDefaultSamplingRate = 0.2; + +// Private data class +class gazebo::ArduPilotSocketPrivate +{ + /// \brief constructor + public: ArduPilotSocketPrivate() + { + // initialize socket udp socket + fd = socket(AF_INET, SOCK_DGRAM, 0); + #ifndef _WIN32 + // Windows does not support FD_CLOEXEC + fcntl(fd, F_SETFD, FD_CLOEXEC); + #endif + } + + /// \brief destructor + public: ~ArduPilotSocketPrivate() + { + if (fd != -1) + { + ::close(fd); + fd = -1; + } + } + + /// \brief Bind to an adress and port + /// \param[in] _address Address to bind to. + /// \param[in] _port Port to bind to. + /// \return True on success. + public: bool Bind(const char *_address, const uint16_t _port) + { + struct sockaddr_in sockaddr; + this->MakeSockAddr(_address, _port, sockaddr); + + if (bind(this->fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) + { + shutdown(this->fd, 0); + #ifdef _WIN32 + closesocket(this->fd); + #else + close(this->fd); + #endif + return false; + } + int one = 1; + setsockopt(this->fd, SOL_SOCKET, SO_REUSEADDR, + reinterpret_cast(&one), sizeof(one)); + + #ifdef _WIN32 + u_long on = 1; + ioctlsocket(this->fd, FIONBIO, + reinterpret_cast(&on)); + #else + fcntl(this->fd, F_SETFL, + fcntl(this->fd, F_GETFL, 0) | O_NONBLOCK); + #endif + return true; + } + + /// \brief Connect to an adress and port + /// \param[in] _address Address to connect to. + /// \param[in] _port Port to connect to. + /// \return True on success. + public : bool Connect(const char *_address, const uint16_t _port) + { + struct sockaddr_in sockaddr; + this->MakeSockAddr(_address, _port, sockaddr); + + if (connect(this->fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) + { + shutdown(this->fd, 0); + #ifdef _WIN32 + closesocket(this->fd); + #else + close(this->fd); + #endif + return false; + } + int one = 1; + setsockopt(this->fd, SOL_SOCKET, SO_REUSEADDR, + reinterpret_cast(&one), sizeof(one)); + + #ifdef _WIN32 + u_long on = 1; + ioctlsocket(this->fd, FIONBIO, + reinterpret_cast(&on)); + #else + fcntl(this->fd, F_SETFL, + fcntl(this->fd, F_GETFL, 0) | O_NONBLOCK); + #endif + return true; + } + + /// \brief Make a socket + /// \param[in] _address Socket address. + /// \param[in] _port Socket port + /// \param[out] _sockaddr New socket address structure. + public: void MakeSockAddr(const char *_address, const uint16_t _port, + struct sockaddr_in &_sockaddr) + { + memset(&_sockaddr, 0, sizeof(_sockaddr)); + + #ifdef HAVE_SOCK_SIN_LEN + _sockaddr.sin_len = sizeof(_sockaddr); + #endif + + _sockaddr.sin_port = htons(_port); + _sockaddr.sin_family = AF_INET; + _sockaddr.sin_addr.s_addr = inet_addr(_address); + } + + public: ssize_t Send(const void *_buf, size_t _size) + { + return send(this->fd, _buf, _size, 0); + } + + /// \brief Receive data + /// \param[out] _buf Buffer that receives the data. + /// \param[in] _size Size of the buffer. + /// \param[in] _timeoutMS Milliseconds to wait for data. + public: ssize_t Recv(void *_buf, const size_t _size, uint32_t _timeoutMs) + { + fd_set fds; + struct timeval tv; + + FD_ZERO(&fds); + FD_SET(this->fd, &fds); + + tv.tv_sec = _timeoutMs / 1000; + tv.tv_usec = (_timeoutMs % 1000) * 1000UL; + + if (select(this->fd+1, &fds, NULL, NULL, &tv) != 1) + { + return -1; + } + + #ifdef _WIN32 + return recv(this->fd, reinterpret_cast(_buf), _size, 0); + #else + return recv(this->fd, _buf, _size, 0); + #endif + } + + /// \brief Socket handle + private: int fd; +}; + +// Private data class +class gazebo::ArduPilotPluginPrivate +{ + /// \brief Pointer to the update event connection. + public: event::ConnectionPtr updateConnection; + + /// \brief Pointer to the model; + public: physics::ModelPtr model; + + /// \brief String of the model name; + public: std::string modelName; + + /// \brief array of propellers + public: std::vector controls; + + /// \brief keep track of controller update sim-time. + public: gazebo::common::Time lastControllerUpdateTime; + + /// \brief Controller update mutex. + public: std::mutex mutex; + + /// \brief Ardupilot Socket for receive motor command on gazebo + public: ArduPilotSocketPrivate socket_in; + + /// \brief Ardupilot Socket to send state to Ardupilot + public: ArduPilotSocketPrivate socket_out; + + /// \brief Ardupilot address + public: std::string fdm_addr; + + /// \brief The Ardupilot listen address + public: std::string listen_addr; + + /// \brief Ardupilot port for receiver socket + public: uint16_t fdm_port_in; + + /// \brief Ardupilot port for sender socket + public: uint16_t fdm_port_out; + + /// \brief Pointer to an IMU sensor + public: sensors::ImuSensorPtr imuSensor; + + /// \brief Pointer to an GPS sensor + public: sensors::GpsSensorPtr gpsSensor; + + /// \brief Pointer to an Rangefinder sensor + public: sensors::RaySensorPtr rangefinderSensor; + + /// \brief false before ardupilot controller is online + /// to allow gazebo to continue without waiting + public: bool arduPilotOnline; + + /// \brief number of times ArduCotper skips update + public: int connectionTimeoutCount; + + /// \brief number of times ArduCotper skips update + /// before marking ArduPilot offline + public: int connectionTimeoutMaxCount; +}; + +///////////////////////////////////////////////// +ArduPilotPlugin::ArduPilotPlugin() + : dataPtr(new ArduPilotPluginPrivate) +{ + this->dataPtr->arduPilotOnline = false; + this->dataPtr->connectionTimeoutCount = 0; +} + +///////////////////////////////////////////////// +ArduPilotPlugin::~ArduPilotPlugin() +{ +} + +///////////////////////////////////////////////// +void ArduPilotPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + GZ_ASSERT(_model, "ArduPilotPlugin _model pointer is null"); + GZ_ASSERT(_sdf, "ArduPilotPlugin _sdf pointer is null"); + + this->dataPtr->model = _model; + this->dataPtr->modelName = this->dataPtr->model->GetName(); + + // modelXYZToAirplaneXForwardZDown brings us from gazebo model frame: + // x-forward, y-right, z-down + // to the aerospace convention: x-forward, y-left, z-up + this->modelXYZToAirplaneXForwardZDown = + ignition::math::Pose3d(0, 0, 0, 0, 0, 0); + if (_sdf->HasElement("modelXYZToAirplaneXForwardZDown")) + { + this->modelXYZToAirplaneXForwardZDown = + _sdf->Get("modelXYZToAirplaneXForwardZDown"); + } + + // gazeboXYZToNED: from gazebo model frame: x-forward, y-right, z-down + // to the aerospace convention: x-forward, y-left, z-up + this->gazeboXYZToNED = ignition::math::Pose3d(0, 0, 0, IGN_PI, 0, 0); + if (_sdf->HasElement("gazeboXYZToNED")) + { + this->gazeboXYZToNED = _sdf->Get("gazeboXYZToNED"); + } + + // per control channel + sdf::ElementPtr controlSDF; + if (_sdf->HasElement("control")) + { + controlSDF = _sdf->GetElement("control"); + } + else if (_sdf->HasElement("rotor")) + { + gzwarn << "[" << this->dataPtr->modelName << "] " + << "please deprecate block, use block instead.\n"; + controlSDF = _sdf->GetElement("rotor"); + } + + while (controlSDF) + { + Control control; + + if (controlSDF->HasAttribute("channel")) + { + control.channel = + atoi(controlSDF->GetAttribute("channel")->GetAsString().c_str()); + } + else if (controlSDF->HasAttribute("id")) + { + gzwarn << "[" << this->dataPtr->modelName << "] " + << "please deprecate attribute id, use channel instead.\n"; + control.channel = + atoi(controlSDF->GetAttribute("id")->GetAsString().c_str()); + } + else + { + control.channel = this->dataPtr->controls.size(); + gzwarn << "[" << this->dataPtr->modelName << "] " + << "id/channel attribute not specified, use order parsed [" + << control.channel << "].\n"; + } + + if (controlSDF->HasElement("type")) + { + control.type = controlSDF->Get("type"); + } + else + { + gzerr << "[" << this->dataPtr->modelName << "] " + << "Control type not specified," + << " using velocity control by default.\n"; + control.type = "VELOCITY"; + } + + if (control.type != "VELOCITY" && + control.type != "POSITION" && + control.type != "EFFORT") + { + gzwarn << "[" << this->dataPtr->modelName << "] " + << "Control type [" << control.type + << "] not recognized, must be one of VELOCITY, POSITION, EFFORT." + << " default to VELOCITY.\n"; + control.type = "VELOCITY"; + } + + if (controlSDF->HasElement("useForce")) + { + control.useForce = controlSDF->Get("useForce"); + } + + if (controlSDF->HasElement("jointName")) + { + control.jointName = controlSDF->Get("jointName"); + } + else + { + gzerr << "[" << this->dataPtr->modelName << "] " + << "Please specify a jointName," + << " where the control channel is attached.\n"; + } + + // Get the pointer to the joint. + control.joint = _model->GetJoint(control.jointName); + if (control.joint == nullptr) + { + gzerr << "[" << this->dataPtr->modelName << "] " + << "Couldn't find specified joint [" + << control.jointName << "]. This plugin will not run.\n"; + return; + } + + if (controlSDF->HasElement("multiplier")) + { + // overwrite turningDirection, deprecated. + control.multiplier = controlSDF->Get("multiplier"); + } + else if (controlSDF->HasElement("turningDirection")) + { + gzwarn << "[" << this->dataPtr->modelName << "] " + << " is deprecated. Please use" + << " . Map 'cw' to '-1' and 'ccw' to '1'.\n"; + std::string turningDirection = controlSDF->Get( + "turningDirection"); + // special cases mimic from controls_gazebo_plugins + if (turningDirection == "cw") + { + control.multiplier = -1; + } + else if (turningDirection == "ccw") + { + control.multiplier = 1; + } + else + { + gzdbg << "[" << this->dataPtr->modelName << "] " + << "not string, check turningDirection as float\n"; + control.multiplier = controlSDF->Get("turningDirection"); + } + } + else + { + gzdbg << "[" << this->dataPtr->modelName << "] " + << " (or deprecated ) not specified," + << " Default 1 (or deprecated 'ccw').\n"; + control.multiplier = 1; + } + + if (controlSDF->HasElement("offset")) + { + control.offset = controlSDF->Get("offset"); + } + else + { + gzdbg << "[" << this->dataPtr->modelName << "] " + << " not specified, default to 0.\n"; + control.offset = 0; + } + + getSdfParam(controlSDF, "rotorVelocitySlowdownSim", + control.rotorVelocitySlowdownSim, 1); + + if (ignition::math::equal(control.rotorVelocitySlowdownSim, 0.0)) + { + gzwarn << "[" << this->dataPtr->modelName << "] " + << "control for joint [" << control.jointName + << "] rotorVelocitySlowdownSim is zero," + << " assume no slowdown.\n"; + control.rotorVelocitySlowdownSim = 1.0; + } + + getSdfParam(controlSDF, "frequencyCutoff", + control.frequencyCutoff, control.frequencyCutoff); + getSdfParam(controlSDF, "samplingRate", + control.samplingRate, control.samplingRate); + + // use gazebo::math::Filter + control.filter.Fc(control.frequencyCutoff, control.samplingRate); + + // initialize filter to zero value + control.filter.Set(0.0); + + // note to use this filter, do + // stateFiltered = filter.Process(stateRaw); + + // Overload the PID parameters if they are available. + double param; + // carry over from ArduCopter plugin + getSdfParam(controlSDF, "vel_p_gain", param, + control.pid.GetPGain()); + control.pid.SetPGain(param); + + getSdfParam(controlSDF, "vel_i_gain", param, + control.pid.GetIGain()); + control.pid.SetIGain(param); + + getSdfParam(controlSDF, "vel_d_gain", param, + control.pid.GetDGain()); + control.pid.SetDGain(param); + + getSdfParam(controlSDF, "vel_i_max", param, + control.pid.GetIMax()); + control.pid.SetIMax(param); + + getSdfParam(controlSDF, "vel_i_min", param, + control.pid.GetIMin()); + control.pid.SetIMin(param); + + getSdfParam(controlSDF, "vel_cmd_max", param, + control.pid.GetCmdMax()); + control.pid.SetCmdMax(param); + + getSdfParam(controlSDF, "vel_cmd_min", param, + control.pid.GetCmdMin()); + control.pid.SetCmdMin(param); + + // new params, overwrite old params if exist + getSdfParam(controlSDF, "p_gain", param, + control.pid.GetPGain()); + control.pid.SetPGain(param); + + getSdfParam(controlSDF, "i_gain", param, + control.pid.GetIGain()); + control.pid.SetIGain(param); + + getSdfParam(controlSDF, "d_gain", param, + control.pid.GetDGain()); + control.pid.SetDGain(param); + + getSdfParam(controlSDF, "i_max", param, + control.pid.GetIMax()); + control.pid.SetIMax(param); + + getSdfParam(controlSDF, "i_min", param, + control.pid.GetIMin()); + control.pid.SetIMin(param); + + getSdfParam(controlSDF, "cmd_max", param, + control.pid.GetCmdMax()); + control.pid.SetCmdMax(param); + + getSdfParam(controlSDF, "cmd_min", param, + control.pid.GetCmdMin()); + control.pid.SetCmdMin(param); + + // set pid initial command + control.pid.SetCmd(0.0); + + this->dataPtr->controls.push_back(control); + controlSDF = controlSDF->GetNextElement("control"); + } + + // Get sensors + std::string imuName; + getSdfParam(_sdf, "imuName", imuName, "imu_sensor"); + // std::string imuScopedName = this->dataPtr->model->GetWorld()->GetName() + // + "::" + this->dataPtr->model->GetScopedName() + // + "::" + imuName; + std::vector imuScopedName = getSensorScopedName(this->dataPtr->model, imuName); + if (imuScopedName.size() > 1) + { + gzwarn << "[" << this->dataPtr->modelName << "] " + << "multiple names match [" << imuName << "] using first found" + << " name.\n"; + for (unsigned k = 0; k < imuScopedName.size(); ++k) + { + gzwarn << " sensor " << k << " [" << imuScopedName[k] << "].\n"; + } + } + + if (imuScopedName.size() > 0) + { + this->dataPtr->imuSensor = std::dynamic_pointer_cast + (sensors::SensorManager::Instance()->GetSensor(imuScopedName[0])); + } + + if (!this->dataPtr->imuSensor) + { + if (imuScopedName.size() > 1) + { + gzwarn << "[" << this->dataPtr->modelName << "] " + << "first imu_sensor scoped name [" << imuScopedName[0] + << "] not found, trying the rest of the sensor names.\n"; + for (unsigned k = 1; k < imuScopedName.size(); ++k) + { + this->dataPtr->imuSensor = std::dynamic_pointer_cast + (sensors::SensorManager::Instance()->GetSensor(imuScopedName[k])); + if (this->dataPtr->imuSensor) + { + gzwarn << "found [" << imuScopedName[k] << "]\n"; + break; + } + } + } + + if (!this->dataPtr->imuSensor) + { + gzwarn << "[" << this->dataPtr->modelName << "] " + << "imu_sensor scoped name [" << imuName + << "] not found, trying unscoped name.\n" << "\n"; + // TODO: this fails for multi-nested models. + // TODO: and transforms fail for rotated nested model, + // joints point the wrong way. + this->dataPtr->imuSensor = std::dynamic_pointer_cast + (sensors::SensorManager::Instance()->GetSensor(imuName)); + } + + if (!this->dataPtr->imuSensor) + { + gzerr << "[" << this->dataPtr->modelName << "] " + << "imu_sensor [" << imuName + << "] not found, abort ArduPilot plugin.\n" << "\n"; + return; + } + } + + // Get GPS + std::string gpsName; + getSdfParam(_sdf, "gpsName", gpsName, "gps_sensor"); + std::vector gpsScopedName = getSensorScopedName(this->dataPtr->model, gpsName); + if (gpsScopedName.size() > 1) + { + gzwarn << "multiple names match [" << gpsName << "] using first found" + << " name.\n"; + for (unsigned k = 0; k < gpsScopedName.size(); ++k) + { + gzwarn << " sensor " << k << " [" << gpsScopedName[k] << "].\n"; + } + } + + if (gpsScopedName.size() > 0) + { + this->dataPtr->gpsSensor = std::dynamic_pointer_cast + (sensors::SensorManager::Instance()->GetSensor(gpsScopedName[0])); + } + + if (!this->dataPtr->gpsSensor) + { + if (gpsScopedName.size() > 1) + { + gzwarn << "first gps_sensor scoped name [" << gpsScopedName[0] + << "] not found, trying the rest of the sensor names.\n"; + for (unsigned k = 1; k < gpsScopedName.size(); ++k) + { + this->dataPtr->gpsSensor = std::dynamic_pointer_cast + (sensors::SensorManager::Instance()->GetSensor(gpsScopedName[k])); + if (this->dataPtr->gpsSensor) + { + gzwarn << "found [" << gpsScopedName[k] << "]\n"; + break; + } + } + } + + if (!this->dataPtr->gpsSensor) + { + gzwarn << "gps_sensor scoped name [" << gpsName + << "] not found, trying unscoped name.\n" << "\n"; + /// TODO: this fails for multi-nested models. + /// TODO: and transforms fail for rotated nested model, + /// joints point the wrong way. + this->dataPtr->gpsSensor = std::dynamic_pointer_cast + (sensors::SensorManager::Instance()->GetSensor(gpsName)); + } + + if (!this->dataPtr->gpsSensor) + { + gzerr << "gps [" << gpsName + << "] not found, abort ArduPilot plugin.\n" << "\n"; + } + else { + gzwarn << " found " << " [" << gpsName << "].\n"; + } + } + + // Get Rangefinder + // TODO add sonar + std::string rangefinderName; + getSdfParam(_sdf, "rangefinderName", rangefinderName, "rangefinder_sensor"); + std::vector rangefinderScopedName = getSensorScopedName(this->dataPtr->model, rangefinderName); + if (rangefinderScopedName.size() > 1) + { + gzwarn << "multiple names match [" << rangefinderName << "] using first found" + << " name.\n"; + for (unsigned k = 0; k < rangefinderScopedName.size(); ++k) + { + gzwarn << " sensor " << k << " [" << rangefinderScopedName[k] << "].\n"; + } + } + + if (rangefinderScopedName.size() > 0) + { + this->dataPtr->rangefinderSensor = std::dynamic_pointer_cast + (sensors::SensorManager::Instance()->GetSensor(rangefinderScopedName[0])); + } + + if (!this->dataPtr->rangefinderSensor) + { + if (rangefinderScopedName.size() > 1) + { + gzwarn << "first rangefinder_sensor scoped name [" << rangefinderScopedName[0] + << "] not found, trying the rest of the sensor names.\n"; + for (unsigned k = 1; k < rangefinderScopedName.size(); ++k) + { + this->dataPtr->rangefinderSensor = std::dynamic_pointer_cast + (sensors::SensorManager::Instance()->GetSensor(rangefinderScopedName[k])); + if (this->dataPtr->rangefinderSensor) + { + gzwarn << "found [" << rangefinderScopedName[k] << "]\n"; + break; + } + } + } + + if (!this->dataPtr->rangefinderSensor) + { + gzwarn << "rangefinder_sensor scoped name [" << rangefinderName + << "] not found, trying unscoped name.\n" << "\n"; + /// TODO: this fails for multi-nested models. + /// TODO: and transforms fail for rotated nested model, + /// joints point the wrong way. + this->dataPtr->rangefinderSensor = std::dynamic_pointer_cast + (sensors::SensorManager::Instance()->GetSensor(rangefinderName)); + } + if (!this->dataPtr->rangefinderSensor) + { + gzerr << "ranfinder [" << rangefinderName + << "] not found, abort ArduPilot plugin.\n" << "\n"; + } + else { + gzwarn << " found " << " [" << rangefinderName << "].\n"; + } + } + + + + // Controller time control. + this->dataPtr->lastControllerUpdateTime = 0; + + // Initialise ardupilot sockets + if (!InitArduPilotSockets(_sdf)) + { + return; + } + + // Missed update count before we declare arduPilotOnline status false + getSdfParam(_sdf, "connectionTimeoutMaxCount", + this->dataPtr->connectionTimeoutMaxCount, 10); + + // Listen to the update event. This event is broadcast every simulation + // iteration. + this->dataPtr->updateConnection = event::Events::ConnectWorldUpdateBegin( + std::bind(&ArduPilotPlugin::OnUpdate, this)); + + gzlog << "[" << this->dataPtr->modelName << "] " + << "ArduPilot ready to fly. The force will be with you" << std::endl; +} + +///////////////////////////////////////////////// +void ArduPilotPlugin::OnUpdate() +{ + std::lock_guard lock(this->dataPtr->mutex); + + const gazebo::common::Time curTime = + this->dataPtr->model->GetWorld()->GetSimTime(); + + // Update the control surfaces and publish the new state. + if (curTime > this->dataPtr->lastControllerUpdateTime) + { + this->ReceiveMotorCommand(); + if (this->dataPtr->arduPilotOnline) + { + this->ApplyMotorForces((curTime - + this->dataPtr->lastControllerUpdateTime).Double()); + this->SendState(); + } + } + + this->dataPtr->lastControllerUpdateTime = curTime; +} + +///////////////////////////////////////////////// +void ArduPilotPlugin::ResetPIDs() +{ + // Reset velocity PID for controls + for (size_t i = 0; i < this->dataPtr->controls.size(); ++i) + { + this->dataPtr->controls[i].cmd = 0; + // this->dataPtr->controls[i].pid.Reset(); + } +} + +///////////////////////////////////////////////// +bool ArduPilotPlugin::InitArduPilotSockets(sdf::ElementPtr _sdf) const +{ + getSdfParam(_sdf, "fdm_addr", + this->dataPtr->fdm_addr, "127.0.0.1"); + getSdfParam(_sdf, "listen_addr", + this->dataPtr->listen_addr, "127.0.0.1"); + getSdfParam(_sdf, "fdm_port_in", + this->dataPtr->fdm_port_in, 9002); + getSdfParam(_sdf, "fdm_port_out", + this->dataPtr->fdm_port_out, 9003); + + if (!this->dataPtr->socket_in.Bind(this->dataPtr->listen_addr.c_str(), + this->dataPtr->fdm_port_in)) + { + gzerr << "[" << this->dataPtr->modelName << "] " + << "failed to bind with " << this->dataPtr->listen_addr + << ":" << this->dataPtr->fdm_port_in << " aborting plugin.\n"; + return false; + } + + if (!this->dataPtr->socket_out.Connect(this->dataPtr->fdm_addr.c_str(), + this->dataPtr->fdm_port_out)) + { + gzerr << "[" << this->dataPtr->modelName << "] " + << "failed to bind with " << this->dataPtr->fdm_addr + << ":" << this->dataPtr->fdm_port_out << " aborting plugin.\n"; + return false; + } + + return true; +} + +///////////////////////////////////////////////// +void ArduPilotPlugin::ApplyMotorForces(const double _dt) +{ + // update velocity PID for controls and apply force to joint + for (size_t i = 0; i < this->dataPtr->controls.size(); ++i) + { + if (this->dataPtr->controls[i].useForce) + { + if (this->dataPtr->controls[i].type == "VELOCITY") + { + const double velTarget = this->dataPtr->controls[i].cmd / + this->dataPtr->controls[i].rotorVelocitySlowdownSim; + const double vel = this->dataPtr->controls[i].joint->GetVelocity(0); + const double error = vel - velTarget; + const double force = this->dataPtr->controls[i].pid.Update(error, _dt); + this->dataPtr->controls[i].joint->SetForce(0, force); + } + else if (this->dataPtr->controls[i].type == "POSITION") + { + const double posTarget = this->dataPtr->controls[i].cmd; + const double pos = this->dataPtr->controls[i].joint->GetAngle(0).Radian(); + const double error = pos - posTarget; + const double force = this->dataPtr->controls[i].pid.Update(error, _dt); + this->dataPtr->controls[i].joint->SetForce(0, force); + } + else if (this->dataPtr->controls[i].type == "EFFORT") + { + const double force = this->dataPtr->controls[i].cmd; + this->dataPtr->controls[i].joint->SetForce(0, force); + } + else + { + // do nothing + } + } + else + { + if (this->dataPtr->controls[i].type == "VELOCITY") + { + this->dataPtr->controls[i].joint->SetVelocity(0, this->dataPtr->controls[i].cmd); + } + else if (this->dataPtr->controls[i].type == "POSITION") + { + this->dataPtr->controls[i].joint->SetPosition(0, this->dataPtr->controls[i].cmd); + } + else if (this->dataPtr->controls[i].type == "EFFORT") + { + const double force = this->dataPtr->controls[i].cmd; + this->dataPtr->controls[i].joint->SetForce(0, force); + } + else + { + // do nothing + } + } + } +} + +///////////////////////////////////////////////// +void ArduPilotPlugin::ReceiveMotorCommand() +{ + // Added detection for whether ArduPilot is online or not. + // If ArduPilot is detected (receive of fdm packet from someone), + // then socket receive wait time is increased from 1ms to 1 sec + // to accomodate network jitter. + // If ArduPilot is not detected, receive call blocks for 1ms + // on each call. + // Once ArduPilot presence is detected, it takes this many + // missed receives before declaring the FCS offline. + + ServoPacket pkt; + int waitMs = 1; + if (this->dataPtr->arduPilotOnline) + { + // increase timeout for receive once we detect a packet from + // ArduPilot FCS. + waitMs = 1000; + } + else + { + // Otherwise skip quickly and do not set control force. + waitMs = 1; + } + ssize_t recvSize = + this->dataPtr->socket_in.Recv(&pkt, sizeof(ServoPacket), waitMs); + + // Drain the socket in the case we're backed up + int counter = 0; + ServoPacket last_pkt; + while (true) + { + // last_pkt = pkt; + const ssize_t recvSize_last = + this->dataPtr->socket_in.Recv(&last_pkt, sizeof(ServoPacket), 0ul); + if (recvSize_last == -1) + { + break; + } + counter++; + pkt = last_pkt; + recvSize = recvSize_last; + } + if (counter > 0) + { + gzdbg << "[" << this->dataPtr->modelName << "] " + << "Drained n packets: " << counter << std::endl; + } + + if (recvSize == -1) + { + // didn't receive a packet + // gzdbg << "no packet\n"; + gazebo::common::Time::NSleep(100); + if (this->dataPtr->arduPilotOnline) + { + gzwarn << "[" << this->dataPtr->modelName << "] " + << "Broken ArduPilot connection, count [" + << this->dataPtr->connectionTimeoutCount + << "/" << this->dataPtr->connectionTimeoutMaxCount + << "]\n"; + if (++this->dataPtr->connectionTimeoutCount > + this->dataPtr->connectionTimeoutMaxCount) + { + this->dataPtr->connectionTimeoutCount = 0; + this->dataPtr->arduPilotOnline = false; + gzwarn << "[" << this->dataPtr->modelName << "] " + << "Broken ArduPilot connection, resetting motor control.\n"; + this->ResetPIDs(); + } + } + } + else + { + const ssize_t expectedPktSize = + sizeof(pkt.motorSpeed[0]) * this->dataPtr->controls.size(); + if (recvSize < expectedPktSize) + { + gzerr << "[" << this->dataPtr->modelName << "] " + << "got less than model needs. Got: " << recvSize + << "commands, expected size: " << expectedPktSize << "\n"; + } + const ssize_t recvChannels = recvSize / sizeof(pkt.motorSpeed[0]); + // for(unsigned int i = 0; i < recvChannels; ++i) + // { + // gzdbg << "servo_command [" << i << "]: " << pkt.motorSpeed[i] << "\n"; + // } + + if (!this->dataPtr->arduPilotOnline) + { + gzdbg << "[" << this->dataPtr->modelName << "] " + << "ArduPilot controller online detected.\n"; + // made connection, set some flags + this->dataPtr->connectionTimeoutCount = 0; + this->dataPtr->arduPilotOnline = true; + } + + // compute command based on requested motorSpeed + for (unsigned i = 0; i < this->dataPtr->controls.size(); ++i) + { + if (i < MAX_MOTORS) + { + if (this->dataPtr->controls[i].channel < recvChannels) + { + // bound incoming cmd between 0 and 1 + const double cmd = ignition::math::clamp( + pkt.motorSpeed[this->dataPtr->controls[i].channel], + -1.0f, 1.0f); + this->dataPtr->controls[i].cmd = + this->dataPtr->controls[i].multiplier * + (this->dataPtr->controls[i].offset + cmd); + // gzdbg << "apply input chan[" << this->dataPtr->controls[i].channel + // << "] to control chan[" << i + // << "] with joint name [" + // << this->dataPtr->controls[i].jointName + // << "] raw cmd [" + // << pkt.motorSpeed[this->dataPtr->controls[i].channel] + // << "] adjusted cmd [" << this->dataPtr->controls[i].cmd + // << "].\n"; + } + else + { + gzerr << "[" << this->dataPtr->modelName << "] " + << "control[" << i << "] channel [" + << this->dataPtr->controls[i].channel + << "] is greater than incoming commands size[" + << recvChannels + << "], control not applied.\n"; + } + } + else + { + gzerr << "[" << this->dataPtr->modelName << "] " + << "too many motors, skipping [" << i + << " > " << MAX_MOTORS << "].\n"; + } + } + } +} + +///////////////////////////////////////////////// +void ArduPilotPlugin::SendState() const +{ + // send_fdm + fdmPacket pkt; + + pkt.timestamp = this->dataPtr->model->GetWorld()->GetSimTime().Double(); + + // asssumed that the imu orientation is: + // x forward + // y right + // z down + + // get linear acceleration in body frame + const ignition::math::Vector3d linearAccel = + this->dataPtr->imuSensor->LinearAcceleration(); + + // copy to pkt + pkt.imuLinearAccelerationXYZ[0] = linearAccel.X(); + pkt.imuLinearAccelerationXYZ[1] = linearAccel.Y(); + pkt.imuLinearAccelerationXYZ[2] = linearAccel.Z(); + // gzerr << "lin accel [" << linearAccel << "]\n"; + + // get angular velocity in body frame + const ignition::math::Vector3d angularVel = + this->dataPtr->imuSensor->AngularVelocity(); + + // copy to pkt + pkt.imuAngularVelocityRPY[0] = angularVel.X(); + pkt.imuAngularVelocityRPY[1] = angularVel.Y(); + pkt.imuAngularVelocityRPY[2] = angularVel.Z(); + + // get inertial pose and velocity + // position of the uav in world frame + // this position is used to calcualte bearing and distance + // from starting location, then use that to update gps position. + // The algorithm looks something like below (from ardupilot helper + // libraries): + // 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) + // where xyz is in the NED directions. + // Gazebo world xyz is assumed to be N, -E, -D, so flip some stuff + // around. + // orientation of the uav in world NED frame - + // assuming the world NED frame has xyz mapped to NED, + // imuLink is NED - z down + + // model world pose brings us to model, + // which for example zephyr has -y-forward, x-left, z-up + // adding modelXYZToAirplaneXForwardZDown rotates + // from: model XYZ + // to: airplane x-forward, y-left, z-down + const ignition::math::Pose3d gazeboXYZToModelXForwardZDown = + this->modelXYZToAirplaneXForwardZDown + + this->dataPtr->model->GetWorldPose().Ign(); + + // get transform from world NED to Model frame + const ignition::math::Pose3d NEDToModelXForwardZUp = + gazeboXYZToModelXForwardZDown - this->gazeboXYZToNED; + + // gzerr << "ned to model [" << NEDToModelXForwardZUp << "]\n"; + + // N + pkt.positionXYZ[0] = NEDToModelXForwardZUp.Pos().X(); + + // E + pkt.positionXYZ[1] = NEDToModelXForwardZUp.Pos().Y(); + + // D + pkt.positionXYZ[2] = NEDToModelXForwardZUp.Pos().Z(); + + // imuOrientationQuat is the rotation from world NED frame + // to the uav frame. + pkt.imuOrientationQuat[0] = NEDToModelXForwardZUp.Rot().W(); + pkt.imuOrientationQuat[1] = NEDToModelXForwardZUp.Rot().X(); + pkt.imuOrientationQuat[2] = NEDToModelXForwardZUp.Rot().Y(); + pkt.imuOrientationQuat[3] = NEDToModelXForwardZUp.Rot().Z(); + + // gzdbg << "imu [" << gazeboXYZToModelXForwardZDown.rot.GetAsEuler() + // << "]\n"; + // gzdbg << "ned [" << this->gazeboXYZToNED.rot.GetAsEuler() << "]\n"; + // gzdbg << "rot [" << NEDToModelXForwardZUp.rot.GetAsEuler() << "]\n"; + + // Get NED velocity in body frame * + // or... + // Get model velocity in NED frame + const ignition::math::Vector3d velGazeboWorldFrame = + this->dataPtr->model->GetLink()->GetWorldLinearVel().Ign(); + const ignition::math::Vector3d velNEDFrame = + this->gazeboXYZToNED.Rot().RotateVectorReverse(velGazeboWorldFrame); + pkt.velocityXYZ[0] = velNEDFrame.X(); + pkt.velocityXYZ[1] = velNEDFrame.Y(); + pkt.velocityXYZ[2] = velNEDFrame.Z(); + + if (!this->dataPtr->gpsSensor) + { + + } + else { + pkt.longitude = this->dataPtr->gpsSensor->Longitude().Degree(); + pkt.latitude = this->dataPtr->gpsSensor->Latitude().Degree(); + pkt.altitude = this->dataPtr->gpsSensor->Altitude(); + } + + // TODO : make generic enough to accept sonar/gpuray etc. too + if (!this->dataPtr->rangefinderSensor) + { + + } else { + // Rangefinder value can not be send as Inf to ardupilot + const double range = this->dataPtr->rangefinderSensor->Range(0); + pkt.rangefinder = std::isinf(range) ? 0.0 : range; + } + + // airspeed : wind = Vector3(environment.wind.x, environment.wind.y, environment.wind.z) + // pkt.airspeed = (pkt.velocity - wind).length() + + this->dataPtr->socket_out.Send(&pkt, sizeof(pkt)); +}