diff --git a/.github/workflows/ccpcheck.yml b/.github/workflows/ccpcheck.yml new file mode 100644 index 00000000..b3109f7b --- /dev/null +++ b/.github/workflows/ccpcheck.yml @@ -0,0 +1,21 @@ +# GitHub Action to run cppcheck +# +name: cppcheck + +on: [push, pull_request] + +jobs: + cpplint: + runs-on: ubuntu-20.04 + name: cppcheck + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Install cppcheck + run: | + sudo apt-get update + sudo apt-get install cppcheck + - name: Run cppcheck + run: | + cppcheck --std=c++17 ./include/*.hh + cppcheck --std=c++17 ./src/*.cc diff --git a/.github/workflows/ccplint.yml b/.github/workflows/ccplint.yml new file mode 100644 index 00000000..67ccb3f2 --- /dev/null +++ b/.github/workflows/ccplint.yml @@ -0,0 +1,28 @@ +# GitHub Action to run cpplint +# +# The cpplint configuration file is: +# +# ./CPPLINT.cfg +# +name: ccplint + +on: [push, pull_request] + +jobs: + cpplint: + runs-on: ubuntu-20.04 + name: cpplint + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Setup + uses: actions/setup-python@v4 + with: + python-version: '3.10' + - name: Install cpplint + run: | + pip install cpplint + - name: Run cpplint + run: | + cpplint ./include/*.hh + cpplint ./src/*.cc diff --git a/CPPLINT.cfg b/CPPLINT.cfg new file mode 100644 index 00000000..24eb31c9 --- /dev/null +++ b/CPPLINT.cfg @@ -0,0 +1,56 @@ +# filters +# +# 1. [-whitespace/braces] +# { should almost always be at the end of the previous line +# +# Allow: +# bool SocketAPM::pollout(uint32_t timeout_ms) +# { +# +# Instead of: +# bool SocketAPM::pollout(uint32_t timeout_ms) { +# +# 2. [-runtime/references] +# Is this a non-const reference? If so, make const or use a pointer +# +# Allow: +# void last_recv_address(const char *&ip_addr, uint16_t &port); +# +# Instead of: +# void last_recv_address(const char *&ip_addr, uint16_t *port); +# +# 3. [-whitespace/indent] +# private: should be indented +1 space +# +# 4. [-whitespace/blank_line] +# Do not leave a blank line after "private:" +# +# 3 and 4 are to allow Gazebo Sim class formatting where each +# function / method must have an access specifier. +# +# +# 5. [-whitespace/newline] +# An else should appear on the same line as the preceding } +# +# Allow: +# } +# else if (time > this->dataPtr->lastUpdateTime) +# { +# +# Instead of: +# } else if (time > this->dataPtr->lastUpdateTime) { +# +# 6. [-build/include_subdir] +# Include the directory when naming header files +# +# This prevent an error when including the plugin headers as: +# #include "GimbalSmall2dPlugin.hh" +# +# 7. [-build/c++11], [+build/c++14] +# +# This is to allow headers not approved for C++11 such as etc. +# +set noparent +filter=-whitespace/braces,-runtime/references,-whitespace/indent,-whitespace/blank_line,-whitespace/newline,-build/include_subdir,-build/c++11,+build/c++14 +linelength=80 +root=include diff --git a/README.md b/README.md index 863e739c..29e64399 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ # ArduPilot Gazebo Plugin -[![ubuntu-build](https://github.com/srmainwaring/ardupilot_gazebo-1/actions/workflows/ubuntu-build.yml/badge.svg)](https://github.com/srmainwaring/ardupilot_gazebo-1/actions/workflows/ubuntu-build.yml) +[![ubuntu-build](https://github.com/ArduPilot/ardupilot_gazebo/actions/workflows/ubuntu-build.yml/badge.svg)](https://github.com/ArduPilot/ardupilot_gazebo/actions/workflows/ubuntu-build.yml) +[![ccplint](https://github.com/ArduPilot/ardupilot_gazebo/actions/workflows/ccplint.yml/badge.svg)](https://github.com/ArduPilot/ardupilot_gazebo/actions/workflows/ccplint.yml) +[![cppcheck](https://github.com/ArduPilot/ardupilot_gazebo/actions/workflows/ccpcheck.yml/badge.svg)](https://github.com/ArduPilot/ardupilot_gazebo/actions/workflows/ccpcheck.yml) This is the official ArduPilot plugin for [Gazebo Sim](https://gazebosim.org/home). It replaces the previous diff --git a/include/ArduCopterIRLockPlugin.hh b/include/ArduCopterIRLockPlugin.hh index 7aebef91..1cb8c9ec 100755 --- a/include/ArduCopterIRLockPlugin.hh +++ b/include/ArduCopterIRLockPlugin.hh @@ -15,8 +15,8 @@ * */ -#ifndef _GAZEBO_ARDUCOPTERIRLOCK_PLUGIN_HH_ -#define _GAZEBO_ARDUCOPTERIRLOCK_PLUGIN_HH_ +#ifndef ARDUCOPTERIRLOCKPLUGIN_HH_ +#define ARDUCOPTERIRLOCKPLUGIN_HH_ #include #include @@ -26,41 +26,43 @@ namespace gazebo { - // Forward declare private class. - class ArduCopterIRLockPluginPrivate; +// 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 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 Destructor - public: virtual ~ArduCopterIRLockPlugin(); + /// \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); - // Documentation Inherited. - public: void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf); + /// \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); - /// \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); + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; +}; - /// \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); +} // namespace gazebo - /// \internal - /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; - }; -} -#endif +#endif // ARDUCOPTERIRLOCKPLUGIN_HH_ diff --git a/include/ArduPilotPlugin.hh b/include/ArduPilotPlugin.hh index c21e7352..40420e02 100755 --- a/include/ArduPilotPlugin.hh +++ b/include/ArduPilotPlugin.hh @@ -14,147 +14,154 @@ * limitations under the License. * */ -#ifndef GAZEBO_PLUGINS_ARDUPILOTPLUGIN_HH_ -#define GAZEBO_PLUGINS_ARDUPILOTPLUGIN_HH_ +#ifndef ARDUPILOTPLUGIN_HH_ +#define ARDUPILOTPLUGIN_HH_ + +#include #include #include -namespace gz { -namespace sim { +namespace gz +{ +namespace sim +{ namespace systems { - // The servo packet received from ArduPilot SITL. Defined in SIM_JSON.h. - struct servo_packet { - uint16_t magic; // 18458 expected magic value - uint16_t frame_rate; - uint32_t frame_count; - uint16_t pwm[16]; - }; - - // 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 - /// command multiplier - /// command offset - /// upper limit for PWM input - /// lower limit for PWM input - /// - /// type of control, VELOCITY, POSITION, EFFORT or COMMAND - /// 1 if joint forces are applied, 0 to set joint directly - /// 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 - /// topic to publish commands that are processed by other plugins - /// - /// rotor turning direction, 'cw' or 'ccw' - /// filter incoming joint state - /// 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 GZ_SIM_VISIBLE ArduPilotPlugin: - public gz::sim::System, - public gz::sim::ISystemConfigure, - public gz::sim::ISystemPostUpdate, - public gz::sim::ISystemPreUpdate, - public gz::sim::ISystemReset +// The servo packet received from ArduPilot SITL. Defined in SIM_JSON.h. +struct servo_packet { + uint16_t magic; // 18458 expected magic value + uint16_t frame_rate; + uint32_t frame_count; + uint16_t pwm[16]; +}; + +// 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 +/// command multiplier +/// command offset +/// upper limit for PWM input +/// lower limit for PWM input +/// +/// type of control, VELOCITY, POSITION, EFFORT or COMMAND +/// 1 if joint forces are applied, 0 to set joint directly +/// 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 +/// topic to publish commands that are processed +/// by other plugins +/// +/// rotor turning direction, 'cw' or 'ccw' +/// filter incoming joint state +/// 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 GZ_SIM_VISIBLE ArduPilotPlugin: + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemReset { - /// \brief Constructor. - public: ArduPilotPlugin(); - - /// \brief Destructor. - public: ~ArduPilotPlugin(); - - public: void Reset(const UpdateInfo &_info, - EntityComponentManager &_ecm) final; - - /// \brief Load configuration from SDF on startup. - public: void Configure(const gz::sim::Entity &_entity, - const std::shared_ptr &_sdf, - gz::sim::EntityComponentManager &_ecm, - gz::sim::EventManager &_eventMgr) final; - - /// \brief Do the part of one update loop that involves making changes to simulation. - public: void PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) final; - - /// \brief Do the part of one update loop that involves reading results from simulation. - public: void PostUpdate(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm) final; - - /// \brief Load control channels - private: void LoadControlChannels( - sdf::ElementPtr _sdf, - gz::sim::EntityComponentManager &_ecm); - - /// \brief Load IMU sensors - private: void LoadImuSensors( - sdf::ElementPtr _sdf, - gz::sim::EntityComponentManager &_ecm); - - /// \brief Load GPS sensors - private: void LoadGpsSensors( - sdf::ElementPtr _sdf, - gz::sim::EntityComponentManager &_ecm); - - /// \brief Load Range sensors - private: void LoadRangeSensors( - sdf::ElementPtr _sdf, - gz::sim::EntityComponentManager &_ecm); - - /// \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, - gz::sim::EntityComponentManager &_ecm); - - /// \brief Reset PID Joint controllers. - private: void ResetPIDs(); - - /// \brief Receive a servo packet from ArduPilot - /// - /// Returns true if a servo packet was received, otherwise false. - private: bool ReceiveServoPacket(); - - /// \brief Update the motor commands given servo PWM values - private: void UpdateMotorCommands(const servo_packet &_pkt); - - /// \brief Create the state JSON - private: void CreateStateJSON( - double _simTime, - const gz::sim::EntityComponentManager &_ecm) const; - - /// \brief Send state to ArduPilot - private: void SendState() const; - - /// \brief Initialise flight dynamics model socket - private: bool InitSockets(sdf::ElementPtr _sdf) const; - - /// \brief Private data pointer. - private: std::unique_ptr dataPtr; - }; - -} // namespace systems -} // namespace gazebo -} // namespace gz - -#endif // GAZEBO_PLUGINS_ARDUPILOTPLUGIN_HH_ + /// \brief Constructor. + public: ArduPilotPlugin(); + + /// \brief Destructor. + public: ~ArduPilotPlugin(); + + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + /// \brief Load configuration from SDF on startup. + public: void Configure(const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) final; + + /// \brief Do the part of one update loop that involves making + /// changes to simulation. + public: void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + + /// \brief Do the part of one update loop that involves + /// reading results from simulation. + public: void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) final; + + /// \brief Load control channels + private: void LoadControlChannels( + sdf::ElementPtr _sdf, + gz::sim::EntityComponentManager &_ecm); + + /// \brief Load IMU sensors + private: void LoadImuSensors( + sdf::ElementPtr _sdf, + gz::sim::EntityComponentManager &_ecm); + + /// \brief Load GPS sensors + private: void LoadGpsSensors( + sdf::ElementPtr _sdf, + gz::sim::EntityComponentManager &_ecm); + + /// \brief Load Range sensors + private: void LoadRangeSensors( + sdf::ElementPtr _sdf, + gz::sim::EntityComponentManager &_ecm); + + /// \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, + gz::sim::EntityComponentManager &_ecm); + + /// \brief Reset PID Joint controllers. + private: void ResetPIDs(); + + /// \brief Receive a servo packet from ArduPilot + /// + /// Returns true if a servo packet was received, otherwise false. + private: bool ReceiveServoPacket(); + + /// \brief Update the motor commands given servo PWM values + private: void UpdateMotorCommands(const servo_packet &_pkt); + + /// \brief Create the state JSON + private: void CreateStateJSON( + double _simTime, + const gz::sim::EntityComponentManager &_ecm) const; + + /// \brief Send state to ArduPilot + private: void SendState() const; + + /// \brief Initialise flight dynamics model socket + private: bool InitSockets(sdf::ElementPtr _sdf) const; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; +}; + +} // namespace systems +} // namespace sim +} // namespace gz + +#endif // ARDUPILOTPLUGIN_HH_ diff --git a/include/GimbalSmall2dPlugin.hh b/include/GimbalSmall2dPlugin.hh index 85ae840b..41b4bb1e 100644 --- a/include/GimbalSmall2dPlugin.hh +++ b/include/GimbalSmall2dPlugin.hh @@ -14,35 +14,39 @@ * limitations under the License. * */ -#ifndef GAZEBO_PLUGINS_GIMBALSMALL2DPLUGIN_HH_ -#define GAZEBO_PLUGINS_GIMBALSMALL2DPLUGIN_HH_ +#ifndef GIMBALSMALL2DPLUGIN_HH_ +#define GIMBALSMALL2DPLUGIN_HH_ -#include "gazebo/common/Plugin.hh" -#include "gazebo/physics/physics.hh" -#include "gazebo/util/system.hh" +#include + +#include +#include +#include namespace gazebo { - // Forward declare private data class - class GimbalSmall2dPluginPrivate; +// Forward declare private data class +class GimbalSmall2dPluginPrivate; + +/// \brief A plugin for controlling the angle of a gimbal joint +class GAZEBO_VISIBLE GimbalSmall2dPlugin : public ModelPlugin +{ + /// \brief Constructor + public: GimbalSmall2dPlugin(); + + // Documentation Inherited. + public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - /// \brief A plugin for controlling the angle of a gimbal joint - class GAZEBO_VISIBLE GimbalSmall2dPlugin : public ModelPlugin - { - /// \brief Constructor - public: GimbalSmall2dPlugin(); + // Documentation Inherited. + public: virtual void Init(); - // Documentation Inherited. - public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + /// \brief Callback on world update + private: void OnUpdate(); - // Documentation Inherited. - public: virtual void Init(); + /// \brief Private data pointer + private: std::unique_ptr dataPtr; +}; - /// \brief Callback on world update - private: void OnUpdate(); +} // namespace gazebo - /// \brief Private data pointer - private: std::unique_ptr dataPtr; - }; -} -#endif +#endif // GIMBALSMALL2DPLUGIN_HH_ diff --git a/include/SelectionBuffer.hh b/include/SelectionBuffer.hh index 5cf00627..7bcaa082 100644 --- a/include/SelectionBuffer.hh +++ b/include/SelectionBuffer.hh @@ -14,65 +14,68 @@ * limitations under the License. * */ -#ifndef _GAZEBO_RENDERING_SELECTION_BUFFER_SELECTIONBUFFER_HH_ -#define _GAZEBO_RENDERING_SELECTION_BUFFER_SELECTIONBUFFER_HH_ +#ifndef SELECTIONBUFFER_HH_ +#define SELECTIONBUFFER_HH_ #include #include -#include "gazebo/util/system.hh" + +#include namespace Ogre { - class Entity; - class RenderTarget; - class SceneManager; -} +class Entity; +class RenderTarget; +class SceneManager; +} // Ogre namespace gazebo { - namespace rendering - { - struct SelectionBufferPrivate; +namespace rendering +{ +struct SelectionBufferPrivate; + +class GZ_RENDERING_VISIBLE SelectionBuffer +{ + /// \brief Constructor + /// \param[in] _camera Name of the camera to generate a selection + /// buffer for. + /// \param[in] _mgr Pointer to the scene manager. + /// \param[in] _renderTarget Pointer to the render target. + public: SelectionBuffer(const std::string &_cameraName, + Ogre::SceneManager *_mgr, Ogre::RenderTarget *_renderTarget); + + /// \brief Destructor + public: ~SelectionBuffer(); - class GZ_RENDERING_VISIBLE SelectionBuffer - { - /// \brief Constructor - /// \param[in] _camera Name of the camera to generate a selection - /// buffer for. - /// \param[in] _mgr Pointer to the scene manager. - /// \param[in] _renderTarget Pointer to the render target. - public: SelectionBuffer(const std::string &_cameraName, - Ogre::SceneManager *_mgr, Ogre::RenderTarget *_renderTarget); + /// \brief Handle on mouse click + /// \param[in] _x X coordinate in pixels. + /// \param[in] _y Y coordinate in pixels. + /// \return Returns the Ogre entity at the coordinate. + public: Ogre::Entity *OnSelectionClick(int _x, int _y); - /// \brief Destructor - public: ~SelectionBuffer(); + /// \brief Debug show overlay + /// \param[in] _show True to show the selection buffer in an overlay. + public: void ShowOverlay(bool _show); - /// \brief Handle on mouse click - /// \param[in] _x X coordinate in pixels. - /// \param[in] _y Y coordinate in pixels. - /// \return Returns the Ogre entity at the coordinate. - public: Ogre::Entity *OnSelectionClick(int _x, int _y); + /// \brief Call this to update the selection buffer contents + public: void Update(); - /// \brief Debug show overlay - /// \param[in] _show True to show the selection buffer in an overlay. - public: void ShowOverlay(bool _show); + /// \brief Delete the render texture + private: void DeleteRTTBuffer(); - /// \brief Call this to update the selection buffer contents - public: void Update(); + /// \brief Create the render texture + private: void CreateRTTBuffer(); - /// \brief Delete the render texture - private: void DeleteRTTBuffer(); + /// \brief Create the selection buffer offscreen render texture. + private: void CreateRTTOverlays(); - /// \brief Create the render texture - private: void CreateRTTBuffer(); + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; +}; - /// \brief Create the selection buffer offscreen render texture. - private: void CreateRTTOverlays(); +} // namespace rendering +} // namespace gazebo - /// \internal - /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; - }; - } -} -#endif +#endif // SELECTIONBUFFER_HH_ diff --git a/include/Socket.h b/include/Socket.h index 3143fa9c..cae666c1 100644 --- a/include/Socket.h +++ b/include/Socket.h @@ -1,4 +1,6 @@ /* + Copyright (C) 2022 ardupilot.org + 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 @@ -28,42 +30,44 @@ class SocketAPM { public: - SocketAPM(bool _datagram); - SocketAPM(bool _datagram, int _fd); - ~SocketAPM(); + explicit SocketAPM(bool _datagram); + SocketAPM(bool _datagram, int _fd); + ~SocketAPM(); - bool connect(const char *address, uint16_t port); - bool bind(const char *address, uint16_t port); - bool reuseaddress(); - bool set_blocking(bool blocking); - bool set_cloexec(); - void set_broadcast(void); + bool connect(const char *address, uint16_t port); + bool bind(const char *address, uint16_t port); + bool reuseaddress(); + bool set_blocking(bool blocking); + bool set_cloexec(); + void set_broadcast(void); - ssize_t send(const void *pkt, size_t size); - ssize_t sendto(const void *buf, size_t size, const char *address, uint16_t port); - ssize_t recv(void *pkt, size_t size, uint32_t timeout_ms); + ssize_t send(const void *pkt, size_t size); + ssize_t sendto(const void *buf, size_t size, + const char *address, uint16_t port); + ssize_t recv(void *pkt, size_t size, uint32_t timeout_ms); - // return the IP address and port of the last received packet - void last_recv_address(const char *&ip_addr, uint16_t &port); + // return the IP address and port of the last received packet + void last_recv_address(const char *&ip_addr, uint16_t &port); - // return true if there is pending data for input - bool pollin(uint32_t timeout_ms); + // return true if there is pending data for input + bool pollin(uint32_t timeout_ms); - // return true if there is room for output data - bool pollout(uint32_t timeout_ms); + // return true if there is room for output data + bool pollout(uint32_t timeout_ms); - // start listening for new tcp connections - bool listen(uint16_t backlog); + // start listening for new tcp connections + bool listen(uint16_t backlog); - // accept a new connection. Only valid for TCP connections after - // listen has been used. A new socket is returned - SocketAPM *accept(uint32_t timeout_ms); + // accept a new connection. Only valid for TCP connections after + // listen has been used. A new socket is returned + SocketAPM *accept(uint32_t timeout_ms); private: - bool datagram; - struct sockaddr_in in_addr {}; + bool datagram; + struct sockaddr_in in_addr {}; - int fd = -1; + int fd = -1; - void make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr); + void make_sockaddr(const char *address, uint16_t port, + struct sockaddr_in &sockaddr); }; diff --git a/src/ArduCopterIRLockPlugin.cc b/src/ArduCopterIRLockPlugin.cc index 30644521..24b9310e 100755 --- a/src/ArduCopterIRLockPlugin.cc +++ b/src/ArduCopterIRLockPlugin.cc @@ -15,6 +15,8 @@ * */ +#include "ArduCopterIRLockPlugin.hh" + #include #include @@ -45,48 +47,47 @@ #include #include #include -#include -#include "include/ArduCopterIRLockPlugin.hh" +#include -using namespace gazebo; -GZ_REGISTER_SENSOR_PLUGIN(ArduCopterIRLockPlugin) +GZ_REGISTER_SENSOR_PLUGIN(gazebo::ArduCopterIRLockPlugin) namespace gazebo { - class ArduCopterIRLockPluginPrivate - { - /// \brief Pointer to the parent camera sensor - public: sensors::CameraSensorPtr parentSensor; +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 Selection buffer used for occlusion detection + public: std::unique_ptr selectionBuffer; - /// \brief All event connections. - public: std::vector connections; + /// \brief All event connections. + public: std::vector connections; - /// \brief A list of fiducials tracked by this camera. - public: std::vector fiducials; + /// \brief A list of fiducials tracked by this camera. + public: std::vector fiducials; - /// \brief Irlock address - public: std::string irlock_addr; + /// \brief Irlock address + public: std::string irlock_addr; - /// \brief Irlock port for receiver socket - public: uint16_t irlock_port; + /// \brief Irlock port for receiver socket + public: uint16_t irlock_port; - public: int handle; + 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; - }; - }; -} + public: struct irlockPacket + { + uint64_t timestamp; + uint16_t num_targets; + float pos_x; + float pos_y; + float size_x; + float size_y; + }; +}; + +} // namespace gazebo ///////////////////////////////////////////////// gz::math::Vector2i GetScreenSpaceCoords(gz::math::Vector3d _pt, diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 466c9cd7..e4ba4f5e 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -15,7 +15,16 @@ * */ #include "ArduPilotPlugin.hh" -#include "Socket.h" + +#include +#include + +#include +#include +#include +#include +#include +#include #include #include @@ -38,18 +47,9 @@ #include #include - #include -#include -#include - -#include -#include -#include -#include -#include -#include +#include "Socket.h" #define DEBUG_JSON_IO 0 @@ -217,7 +217,8 @@ class gz::sim::systems::ArduPilotPluginPrivate /// \brief This mutex should be used when accessing imuMsg or imuMsgValid public: std::mutex imuMsgMutex; - /// \brief This subscriber callback latches the most recently received IMU data message for later use. + /// \brief This subscriber callback latches the most recently received + /// IMU data message for later use. public: void imuCb(const gz::msgs::IMU &_msg) { std::lock_guard lock(this->imuMsgMutex); @@ -244,7 +245,8 @@ class gz::sim::systems::ArduPilotPluginPrivate /// \brief Number of consecutive missed ArduPilot controller messages public: int connectionTimeoutCount{0}; - /// \brief Max number of consecutive missed ArduPilot controller messages before timeout + /// \brief Max number of consecutive missed ArduPilot controller + /// messages before timeout public: int connectionTimeoutMaxCount; /// \brief Transform from model orientation to x-forward and z-up @@ -291,13 +293,17 @@ gz::sim::systems::ArduPilotPlugin::~ArduPilotPlugin() void gz::sim::systems::ArduPilotPlugin::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) { - if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink, components::WorldPose::typeId)) + if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink, + components::WorldPose::typeId)) { - _ecm.CreateComponent(this->dataPtr->modelLink, gz::sim::components::WorldPose()); + _ecm.CreateComponent(this->dataPtr->modelLink, + gz::sim::components::WorldPose()); } - if(!_ecm.EntityHasComponentType(this->dataPtr->modelLink, components::WorldLinearVelocity::typeId)) + if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink, + components::WorldLinearVelocity::typeId)) { - _ecm.CreateComponent(this->dataPtr->modelLink, gz::sim::components::WorldLinearVelocity()); + _ecm.CreateComponent(this->dataPtr->modelLink, + gz::sim::components::WorldLinearVelocity()); } // update velocity PID for controls and apply force to joint @@ -305,7 +311,8 @@ void gz::sim::systems::ArduPilotPlugin::Reset(const UpdateInfo &_info, { gz::sim::components::JointForceCmd* jfcComp = nullptr; gz::sim::components::JointVelocityCmd* jvcComp = nullptr; - if (this->dataPtr->controls[i].useForce || this->dataPtr->controls[i].type == "EFFORT") + if (this->dataPtr->controls[i].useForce || + this->dataPtr->controls[i].type == "EFFORT") { jfcComp = _ecm.Component( this->dataPtr->controls[i].joint); @@ -375,7 +382,8 @@ void gz::sim::systems::ArduPilotPlugin::Configure( this->dataPtr->gazeboXYZToNED = gz::math::Pose3d(0, 0, 0, GZ_PI, 0, 0); if (sdfClone->HasElement("gazeboXYZToNED")) { - this->dataPtr->gazeboXYZToNED = sdfClone->Get("gazeboXYZToNED"); + this->dataPtr->gazeboXYZToNED = + sdfClone->Get("gazeboXYZToNED"); } // Load control channel params @@ -472,7 +480,8 @@ void gz::sim::systems::ArduPilotPlugin::LoadControlChannels( { gzwarn << "[" << this->dataPtr->modelName << "] " << "Control type [" << control.type - << "] not recognized, must be one of VELOCITY, POSITION, EFFORT, COMMAND." + << "] not recognized, must be one of" + << "VELOCITY, POSITION, EFFORT, COMMAND." << " default to VELOCITY.\n"; control.type = "VELOCITY"; } @@ -523,7 +532,8 @@ void gz::sim::systems::ArduPilotPlugin::LoadControlChannels( gzmsg << "[" << this->dataPtr->modelName << "] " << "Advertising on " << control.cmdTopic << ".\n"; - control.pub = this->dataPtr->node.Advertise(control.cmdTopic); + control.pub = this->dataPtr-> + node.Advertise(control.cmdTopic); } if (controlSDF->HasElement("multiplier")) @@ -558,7 +568,8 @@ void gz::sim::systems::ArduPilotPlugin::LoadControlChannels( { gzdbg << "[" << this->dataPtr->modelName << "] " << "channel[" << control.channel - << "]: (or deprecated ) not specified, " + << "]: (or deprecated )" + << " not specified, " << " default to " << control.multiplier << " (or deprecated 'ccw').\n"; } @@ -696,8 +707,10 @@ void gz::sim::systems::ArduPilotPlugin::LoadGpsSensors( /* NOT MERGED IN MASTER YET // Get GPS - std::string gpsName = _sdf->Get("imuName", static_cast("gps_sensor")).first; - std::vector gpsScopedName = SensorScopedName(this->dataPtr->model, gpsName); + std::string gpsName = _sdf->Get("imuName", + static_cast("gps_sensor")).first; + std::vector gpsScopedName = + SensorScopedName(this->dataPtr->model, gpsName); if (gpsScopedName.size() > 1) { gzwarn << "[" << this->dataPtr->modelName << "] " @@ -724,7 +737,8 @@ void gz::sim::systems::ArduPilotPlugin::LoadGpsSensors( << "] 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 + this->dataPtr->gpsSensor = + std::dynamic_pointer_cast (sensors::SensorManager::Instance()->GetSensor(gpsScopedName[k])); if (this->dataPtr->gpsSensor) { @@ -768,11 +782,13 @@ void gz::sim::systems::ArduPilotPlugin::LoadRangeSensors( // TODO add sonar std::string rangefinderName = _sdf->Get("rangefinderName", static_cast("rangefinder_sensor")).first; - std::vector rangefinderScopedName = SensorScopedName(this->dataPtr->model, rangefinderName); + std::vector rangefinderScopedName = + SensorScopedName(this->dataPtr->model, rangefinderName); if (rangefinderScopedName.size() > 1) { gzwarn << "[" << this->dataPtr->modelName << "] " - << "multiple names match [" << rangefinderName << "] using first found" + << "multiple names match [" << rangefinderName << "]" + << " using first found" << " name.\n"; for (unsigned k = 0; k < rangefinderScopedName.size(); ++k) { @@ -782,8 +798,10 @@ void gz::sim::systems::ArduPilotPlugin::LoadRangeSensors( if (rangefinderScopedName.size() > 0) { - this->dataPtr->rangefinderSensor = std::dynamic_pointer_cast - (sensors::SensorManager::Instance()->GetSensor(rangefinderScopedName[0])); + this->dataPtr->rangefinderSensor = + std::dynamic_pointer_cast + (sensors::SensorManager::Instance()-> + GetSensor(rangefinderScopedName[0])); } if (!this->dataPtr->rangefinderSensor) @@ -791,12 +809,15 @@ void gz::sim::systems::ArduPilotPlugin::LoadRangeSensors( if (rangefinderScopedName.size() > 1) { gzwarn << "[" << this->dataPtr->modelName << "] " - << "first rangefinder_sensor scoped name [" << rangefinderScopedName[0] + << "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])); + this->dataPtr->rangefinderSensor = + std::dynamic_pointer_cast + (sensors::SensorManager::Instance()-> + GetSensor(rangefinderScopedName[k])); if (this->dataPtr->rangefinderSensor) { gzwarn << "found [" << rangefinderScopedName[k] << "]\n"; @@ -813,8 +834,9 @@ void gz::sim::systems::ArduPilotPlugin::LoadRangeSensors( /// 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)); + this->dataPtr->rangefinderSensor = + std::dynamic_pointer_cast + (sensors::SensorManager::Instance()->GetSensor(rangefinderName)); } if (!this->dataPtr->rangefinderSensor) { @@ -836,7 +858,8 @@ void gz::sim::systems::ArduPilotPlugin::PreUpdate( const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) { - // This lookup is done in PreUpdate() because in Configure() it's not possible to get the fully qualified topic name we want + // This lookup is done in PreUpdate() because in Configure() + // it's not possible to get the fully qualified topic name we want if (!this->dataPtr->imuInitialized) { // Set unconditionally because we're only going to try this once. @@ -853,16 +876,20 @@ void gz::sim::systems::ArduPilotPlugin::PreUpdate( gz::sim::Entity parent = _ecm.ParentEntity(_imu_entity); if (parent != gz::sim::kNullEntity) { - // The grandparent of the imu is the quad itself, which is where this plugin is attached + // The grandparent of the imu is the quad itself, + // which is where this plugin is attached gz::sim::Entity gparent = _ecm.ParentEntity(parent); if (gparent != gz::sim::kNullEntity) { gz::sim::Model gparent_model(gparent); - if (gparent_model.Name(_ecm) == this->dataPtr->modelName) + if (gparent_model.Name(_ecm) == + this->dataPtr->modelName) { this->dataPtr->modelLink = parent; - imuTopicName = gz::sim::scopedName(_imu_entity, _ecm) + "/imu"; - gzdbg << "Computed IMU topic to be: " << imuTopicName << std::endl; + imuTopicName = gz::sim::scopedName( + _imu_entity, _ecm) + "/imu"; + gzdbg << "Computed IMU topic to be: " + << imuTopicName << std::endl; } } } @@ -870,7 +897,7 @@ void gz::sim::systems::ArduPilotPlugin::PreUpdate( return true; }); - if(imuTopicName.empty()) + if (imuTopicName.empty()) { gzerr << "[" << this->dataPtr->modelName << "] " << "imu_sensor [" << this->dataPtr->imuName @@ -878,27 +905,35 @@ void gz::sim::systems::ArduPilotPlugin::PreUpdate( return; } - this->dataPtr->node.Subscribe(imuTopicName, &gz::sim::systems::ArduPilotPluginPrivate::imuCb, this->dataPtr.get()); + this->dataPtr->node.Subscribe(imuTopicName, + &gz::sim::systems::ArduPilotPluginPrivate::imuCb, + this->dataPtr.get()); - // Make sure that the "imu_link" entity has WorldPose and WorldLinearVelocity - // components, which we'll need later. - if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink, components::WorldPose::typeId)) + // Make sure that the "imu_link" entity has WorldPose + // and WorldLinearVelocity components, which we'll need later. + if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink, + components::WorldPose::typeId)) { - _ecm.CreateComponent(this->dataPtr->modelLink, gz::sim::components::WorldPose()); + _ecm.CreateComponent(this->dataPtr->modelLink, + gz::sim::components::WorldPose()); } - if(!_ecm.EntityHasComponentType(this->dataPtr->modelLink, components::WorldLinearVelocity::typeId)) + if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink, + components::WorldLinearVelocity::typeId)) { - _ecm.CreateComponent(this->dataPtr->modelLink, gz::sim::components::WorldLinearVelocity()); + _ecm.CreateComponent(this->dataPtr->modelLink, + gz::sim::components::WorldLinearVelocity()); } } else { // Update the control surfaces. - if (!_info.paused && _info.simTime > this->dataPtr->lastControllerUpdateTime) + if (!_info.paused && _info.simTime > + this->dataPtr->lastControllerUpdateTime) { if (this->dataPtr->isLockStep) { - while (!this->ReceiveServoPacket() && this->dataPtr->arduPilotOnline) + while (!this->ReceiveServoPacket() && + this->dataPtr->arduPilotOnline) { // SIGNINT should interrupt this loop. if (this->dataPtr->signal != 0) @@ -915,8 +950,10 @@ void gz::sim::systems::ArduPilotPlugin::PreUpdate( if (this->dataPtr->arduPilotOnline) { - double dt = std::chrono::duration_cast >( - _info.simTime - this->dataPtr->lastControllerUpdateTime).count(); + double dt = + std::chrono::duration_cast >( + _info.simTime - this->dataPtr-> + lastControllerUpdateTime).count(); this->ApplyMotorForces(dt, _ecm); } } @@ -934,7 +971,9 @@ void gz::sim::systems::ArduPilotPlugin::PostUpdate( if (!_info.paused && _info.simTime > this->dataPtr->lastControllerUpdateTime && this->dataPtr->arduPilotOnline) { - double t = std::chrono::duration_cast>(_info.simTime).count(); + double t = + std::chrono::duration_cast>( + _info.simTime).count(); this->CreateStateJSON(t, _ecm); this->SendState(); this->dataPtr->lastControllerUpdateTime = _info.simTime; @@ -968,14 +1007,17 @@ bool gz::sim::systems::ArduPilotPlugin::InitSockets(sdf::ElementPtr _sdf) const // output port configuration is automatic if (_sdf->HasElement("listen_addr")) { - gzwarn << "Param is deprecated, connection is auto detected\n"; + gzwarn << "Param is deprecated," + << " connection is auto detected\n"; } if (_sdf->HasElement("fdm_port_out")) { - gzwarn << "Param is deprecated, connection is auto detected\n"; + gzwarn << "Param is deprecated," + << " connection is auto detected\n"; } // bind the socket - if (!this->dataPtr->sock.bind(this->dataPtr->fdm_address.c_str(), this->dataPtr->fdm_port_in)) + if (!this->dataPtr->sock.bind(this->dataPtr->fdm_address.c_str(), + this->dataPtr->fdm_port_in)) { gzerr << "[" << this->dataPtr->modelName << "] " << "failed to bind with " @@ -1009,7 +1051,8 @@ void gz::sim::systems::ArduPilotPlugin::ApplyMotorForces( gz::sim::components::JointForceCmd* jfcComp = nullptr; gz::sim::components::JointVelocityCmd* jvcComp = nullptr; - if (this->dataPtr->controls[i].useForce || this->dataPtr->controls[i].type == "EFFORT") + if (this->dataPtr->controls[i].useForce || + this->dataPtr->controls[i].type == "EFFORT") { jfcComp = _ecm.Component( this->dataPtr->controls[i].joint); @@ -1075,7 +1118,8 @@ void gz::sim::systems::ArduPilotPlugin::ApplyMotorForces( } else if (this->dataPtr->controls[i].type == "POSITION") { - //TODO: figure out whether position control matters, and if so, how to use it. + /// \todo(anyone) figure out whether position control matters, + /// and if so, how to use it. gzwarn << "Failed to do position control on joint " << i << " because there's no JointPositionCmd component (yet?)" << "/n"; } @@ -1108,8 +1152,8 @@ bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket() if (this->dataPtr->arduPilotOnline) { // Increase timeout for recv once we detect a packet from ArduPilot FCS. - // If this value is too high then it will block the main Gazebo update loop - // and adversely affect the RTF. + // If this value is too high then it will block the main Gazebo + // update loop and adversely affect the RTF. waitMs = 10; } else @@ -1119,16 +1163,19 @@ bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket() } servo_packet pkt; - auto recvSize = this->dataPtr->sock.recv(&pkt, sizeof(servo_packet), waitMs); + auto recvSize = this->dataPtr->sock.recv(&pkt, + sizeof(servo_packet), waitMs); - this->dataPtr->sock.last_recv_address(this->dataPtr->fcu_address, this->dataPtr->fcu_port_out); + this->dataPtr->sock.last_recv_address(this->dataPtr->fcu_address, + this->dataPtr->fcu_port_out); // drain the socket in the case we're backed up int counter = 0; while (true) { servo_packet last_pkt; - auto recvSize_last = this->dataPtr->sock.recv(&last_pkt, sizeof(servo_packet), 0ul); + auto recvSize_last = this->dataPtr->sock.recv(&last_pkt, + sizeof(servo_packet), 0ul); if (recvSize_last == -1) { break; @@ -1162,7 +1209,8 @@ bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket() { this->dataPtr->arduPilotOnline = false; gzwarn << "[" << this->dataPtr->modelName << "] " - << "Broken ArduPilot connection, resetting motor control.\n"; + << "Broken ArduPilot connection," + << " resetting motor control.\n"; this->ResetPIDs(); } } @@ -1174,7 +1222,8 @@ bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket() // debug: inspect sitl packet std::ostringstream oss; oss << "recv " << recvSize << " bytes from " - << this->dataPtr->fcu_address << ":" << this->dataPtr->fcu_port_out << "\n"; + << this->dataPtr->fcu_address << ":" + << this->dataPtr->fcu_port_out << "\n"; // oss << "magic: " << pkt.magic << "\n"; // oss << "frame_rate: " << pkt.frame_rate << "\n"; oss << "frame_count: " << pkt.frame_count << "\n"; @@ -1252,7 +1301,8 @@ bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket() } ///////////////////////////////////////////////// -void gz::sim::systems::ArduPilotPlugin::UpdateMotorCommands(const servo_packet &_pkt) +void gz::sim::systems::ArduPilotPlugin::UpdateMotorCommands( + const servo_packet &_pkt) { // compute command based on requested motorSpeed for (unsigned i = 0; i < this->dataPtr->controls.size(); ++i) @@ -1273,10 +1323,12 @@ void gz::sim::systems::ArduPilotPlugin::UpdateMotorCommands(const servo_packet & // bound incoming cmd between 0 and 1 double raw_cmd = (pwm - pwm_min)/(pwm_max - pwm_min); raw_cmd = gz::math::clamp(raw_cmd, 0.0, 1.0); - this->dataPtr->controls[i].cmd = multiplier * (raw_cmd + offset); + this->dataPtr->controls[i].cmd = + multiplier * (raw_cmd + offset); #if 0 - gzdbg << "apply input chan[" << this->dataPtr->controls[i].channel + gzdbg << "apply input chan[" + << this->dataPtr->controls[i].channel << "] to control chan[" << i << "] with joint name [" << this->dataPtr->controls[i].jointName @@ -1310,19 +1362,21 @@ void gz::sim::systems::ArduPilotPlugin::CreateStateJSON( double _simTime, const gz::sim::EntityComponentManager &_ecm) const { - // Make a local copy of the latest IMU data (it's filled in on receipt by imuCb()). + // Make a local copy of the latest IMU data (it's filled in + // on receipt by imuCb()). gz::msgs::IMU imuMsg; { std::lock_guard lock(this->dataPtr->imuMsgMutex); // Wait until we've received a valid message. - if(!this->dataPtr->imuMsgValid) + if (!this->dataPtr->imuMsgValid) { return; } imuMsg = this->dataPtr->imuMsg; } - // it is assumed that the imu orientation conforms to the aircraft convention: + // it is assumed that the imu orientation conforms to the + // aircraft convention: // x-forward // y-right // z-down @@ -1355,7 +1409,8 @@ void gz::sim::systems::ArduPilotPlugin::CreateStateJSON( Gazebo world frame is: x-east, y-north, z-up (ENU) Gazebo body frame is: x-forward, y-left, z-up - Reference: https://gazebosim.org/api/gazebo/7.0/spherical_coordinates.html + Reference: + https://gazebosim.org/api/gazebo/7.0/spherical_coordinates.html In some cases the Gazebo body frame may use a non-standard convention, for example the Zephyr delta wing model has x-left, y-back, z-up. @@ -1366,8 +1421,9 @@ void gz::sim::systems::ArduPilotPlugin::CreateStateJSON( wldAToBdyA = ^{w_A}\xi_{b_A} - By which we mean that a vector expressed in Aircraft body frame coordinates - may be represented in Aircraft world frame coordinates by the transform: + By which we mean that a vector expressed in Aircraft body frame + coordinates may be represented in Aircraft world frame coordinates by + the transform: ^{wldA}v = ^{wldA}\xi_{bdyA} ^{bdyA}v = ^{wldA}rot_{bdyA} ^{bdyA}v + ^{wldA}pos_{bdyA} @@ -1377,8 +1433,8 @@ void gz::sim::systems::ArduPilotPlugin::CreateStateJSON( Combining transforms ==================== - 1. Gazebo supplies us with the transform from the Gazebo world frame to the - Gazebo body frame (which we recall may be non-standard) + 1. Gazebo supplies us with the transform from the Gazebo world frame + to the Gazebo body frame (which we recall may be non-standard) wldGToBdyG = ^{wldG}\xi_{bdyG} @@ -1388,22 +1444,27 @@ void gz::sim::systems::ArduPilotPlugin::CreateStateJSON( wldAToWldG = ^{wldA}\xi_{wldG} = Pose3d(0, 0, 0, -GZ_PI, 0, 0) Note that this is the inverse of the plugin parameter - which tells us how to rotate a Gazebo world frame into an Aircraft world frame. + which tells us how to rotate a Gazebo world frame into an Aircraft + world frame. - 3. The transform from the Aircraft body frame to the Gazebo body frame is denoted: + 3. The transform from the Aircraft body frame to the Gazebo body frame + is denoted: bdyAToBdyG = ^{bdyA}\xi_{bdyG} - This will typically be Pose3d(0, 0, 0, -GZ_PI, 0, 0) but in some cases will vary. - For instance the Zephyr uses the transform Pose3d(0, 0, 0, -GZ_PI, 0, GZ_PI/2) + This will typically be Pose3d(0, 0, 0, -GZ_PI, 0, 0) but in some cases + will vary. For instance the Zephyr uses the transform + Pose3d(0, 0, 0, -GZ_PI, 0, GZ_PI/2) - Note this is also the inverse of the plugin parameter - which tells us how to rotate a Gazebo model frame into an Aircraft body frame + Note this is also the inverse of the plugin parameter + which tells us how to rotate a Gazebo + model frame into an Aircraft body frame - 4. Finally we compose the transforms to get the one required for the ArduPilot - JSON interface: + 4. Finally we compose the transforms to get the one required for the + ArduPilot JSON interface: - ^{wldA}\xi_{bdyA} = ^{wldA}\xi_{wldG} * ^{wldG}\xi_{bdyG} * (^{bdyA}\xi_{bdyG})^{-1} + ^{wldA}\xi_{bdyA} = ^{wldA}\xi_{wldG} * ^{wldG}\xi_{bdyG} + * (^{bdyA}\xi_{bdyG})^{-1} where we use: @@ -1424,10 +1485,12 @@ void gz::sim::systems::ArduPilotPlugin::CreateStateJSON( this->dataPtr->modelLink); // position and orientation transform (Aircraft world to Aircraft body) - gz::math::Pose3d bdyAToBdyG = this->dataPtr->modelXYZToAirplaneXForwardZDown.Inverse(); + gz::math::Pose3d bdyAToBdyG = + this->dataPtr->modelXYZToAirplaneXForwardZDown.Inverse(); gz::math::Pose3d wldAToWldG = this->dataPtr->gazeboXYZToNED.Inverse(); gz::math::Pose3d wldGToBdyG = worldPose->Data(); - gz::math::Pose3d wldAToBdyA = wldAToWldG * wldGToBdyG * bdyAToBdyG.Inverse(); + gz::math::Pose3d wldAToBdyA = + wldAToWldG * wldGToBdyG * bdyAToBdyG.Inverse(); // velocity transformation gz::math::Vector3d velWldG = worldLinearVel->Data(); @@ -1436,11 +1499,9 @@ void gz::sim::systems::ArduPilotPlugin::CreateStateJSON( // require the duration since sim start in seconds double timestamp = _simTime; - using namespace rapidjson; - // build JSON document - StringBuffer s; - Writer writer(s); + rapidjson::StringBuffer s; + rapidjson::Writer writer(s); writer.StartObject(); @@ -1525,7 +1586,8 @@ void gz::sim::systems::ArduPilotPlugin::SendState() const #if DEBUG_JSON_IO gzdbg << "sent " << bytes_sent << " bytes to " - << this->dataPtr->fcu_address << ":" << this->dataPtr->fcu_port_out << "\n" + << this->dataPtr->fcu_address << ":" + << this->dataPtr->fcu_port_out << "\n" << "frame_count: " << this->dataPtr->fcu_frame_count << "\n"; #endif } diff --git a/src/GimbalSmall2dPlugin.cc b/src/GimbalSmall2dPlugin.cc index 5d1b8f72..46e09a42 100644 --- a/src/GimbalSmall2dPlugin.cc +++ b/src/GimbalSmall2dPlugin.cc @@ -14,19 +14,19 @@ * limitations under the License. * */ +#include "GimbalSmall2dPlugin.hh" + #include #include -#include "gazebo/common/PID.hh" -#include "gazebo/physics/physics.hh" -#include "gazebo/transport/transport.hh" -#include "GimbalSmall2dPlugin.hh" - -using namespace gazebo; -using namespace std; +#include +#include +#include GZ_REGISTER_MODEL_PLUGIN(GimbalSmall2dPlugin) +namespace gazebo +{ /// \brief Private data class class gazebo::GimbalSmall2dPluginPrivate { @@ -160,3 +160,5 @@ void GimbalSmall2dPlugin::OnUpdate() this->dataPtr->pub->Publish(m); } } + +} // namespace gazebo diff --git a/src/Socket.cpp b/src/Socket.cpp index ab447bba..0ef3d785 100644 --- a/src/Socket.cpp +++ b/src/Socket.cpp @@ -1,4 +1,6 @@ /* + Copyright (C) 2022 ardupilot.org + 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 @@ -23,7 +25,7 @@ constructor */ SocketAPM::SocketAPM(bool _datagram) : - SocketAPM(_datagram, + SocketAPM(_datagram, socket(AF_INET, _datagram?SOCK_DGRAM:SOCK_STREAM, 0)) {} @@ -46,7 +48,8 @@ SocketAPM::~SocketAPM() } } -void SocketAPM::make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr) +void SocketAPM::make_sockaddr(const char *address, uint16_t port, + struct sockaddr_in &sockaddr) { memset(&sockaddr, 0, sizeof(sockaddr)); @@ -129,11 +132,13 @@ ssize_t SocketAPM::send(const void *buf, size_t size) /* send some data */ -ssize_t SocketAPM::sendto(const void *buf, size_t size, const char *address, uint16_t port) +ssize_t SocketAPM::sendto(const void *buf, size_t size, + const char *address, uint16_t port) { struct sockaddr_in sockaddr; make_sockaddr(address, port, sockaddr); - return ::sendto(fd, buf, size, 0, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); + return ::sendto(fd, buf, size, 0, + (struct sockaddr *)&sockaddr, sizeof(sockaddr)); } /* @@ -145,7 +150,8 @@ ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms) return -1; } socklen_t len = sizeof(in_addr); - return ::recvfrom(fd, buf, size, MSG_DONTWAIT, (sockaddr *)&in_addr, &len); + return ::recvfrom(fd, buf, size, MSG_DONTWAIT, + reinterpret_cast(&in_addr), &len); } /* @@ -160,7 +166,8 @@ void SocketAPM::last_recv_address(const char *&ip_addr, uint16_t &port) void SocketAPM::set_broadcast(void) { int one = 1; - setsockopt(fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one)); + setsockopt(fd, SOL_SOCKET, SO_BROADCAST, + reinterpret_cast(&one), sizeof(one)); } /* @@ -209,7 +216,7 @@ bool SocketAPM::pollout(uint32_t timeout_ms) */ bool SocketAPM::listen(uint16_t backlog) { - return ::listen(fd, (int)backlog) == 0; + return ::listen(fd, static_cast(backlog)) == 0; } /*