diff --git a/CMakeLists.txt b/CMakeLists.txt index c12fe13f..a2d3acdc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,6 +57,10 @@ find_package(OpenCV REQUIRED) pkg_check_modules(GST REQUIRED gstreamer-1.0 gstreamer-app-1.0) +# Make external include directories system includes to suppress warnings. +include_directories(SYSTEM ${RapidJSON_INCLUDE_DIRS}) +include_directories(SYSTEM ${OpenCV_INCLUDE_DIRS}) +include_directories(SYSTEM ${GST_INCLUDE_DIRS}) # --------------------------------------------------------------------------- # # Build plugin. @@ -113,6 +117,34 @@ target_link_libraries(GstCameraPlugin PRIVATE ${GST_LINK_LIBRARIES} ) +# Retrieve the list of current targets +get_property(current_targets DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY BUILDSYSTEM_TARGETS) +# Filter out targets that end with _uninstall +list(FILTER current_targets EXCLUDE REGEX "uninstall$") + +# Apply compile options to each target, cannot use global compile options as gazebo don't comply with them +foreach(target ${current_targets}) + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + target_compile_options(${target} PRIVATE -Wall -Wextra -Wpedantic) + target_compile_options(${target} PRIVATE -Wshadow -Wformat) + target_compile_options(${target} PRIVATE -Wcast-align -Woverloaded-virtual) + target_compile_options(${target} PRIVATE -Wnull-dereference) + target_compile_options(${target} PRIVATE -Wconversion) + target_compile_options(${target} PRIVATE -Wmissing-declarations -Wmissing-include-dirs -Wredundant-decls) + target_compile_options(${target} PRIVATE -Wcast-qual -Wctor-dtor-privacy -Wdisabled-optimization -Wformat=2 -Wsign-promo -Wswitch -Wundef) + target_compile_options(${target} PRIVATE -Wold-style-cast) + target_compile_options(${target} PRIVATE -Werror) + target_compile_options(${target} PRIVATE -Werror=return-type) + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + target_compile_options(${target} PRIVATE -Wuseless-cast ) + target_compile_options(${target} PRIVATE -Wduplicated-cond -Wduplicated-branches -Wlogical-op -Wstrict-null-sentinel) + endif() + + target_compile_options(${target} PRIVATE -ffunction-sections -fdata-sections) + target_link_options(${target} PRIVATE -Wl,--gc-sections) + endif() +endforeach() + # --------------------------------------------------------------------------- # # Install. diff --git a/include/ArduPilotPlugin.hh b/include/ArduPilotPlugin.hh index f53ff18b..a96be8d0 100755 --- a/include/ArduPilotPlugin.hh +++ b/include/ArduPilotPlugin.hh @@ -47,7 +47,6 @@ struct servo_packet_32 { }; // Forward declare private data class -class ArduPilotSocketPrivate; class ArduPilotPluginPrivate; /// \brief Interface ArduPilot from ardupilot stack @@ -86,7 +85,7 @@ class ArduPilotPluginPrivate; /// controller synchronization /// set true if 32 channels are enabled /// -class GZ_SIM_VISIBLE ArduPilotPlugin: +class GZ_SIM_VISIBLE ArduPilotPlugin final: public gz::sim::System, public gz::sim::ISystemConfigure, public gz::sim::ISystemPostUpdate, @@ -97,7 +96,7 @@ class GZ_SIM_VISIBLE ArduPilotPlugin: public: ArduPilotPlugin(); /// \brief Destructor. - public: ~ArduPilotPlugin(); + public: ~ArduPilotPlugin() final = default; public: void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) final; @@ -120,27 +119,27 @@ class GZ_SIM_VISIBLE ArduPilotPlugin: /// \brief Load control channels private: void LoadControlChannels( - sdf::ElementPtr _sdf, + const sdf::ElementPtr& _sdf, gz::sim::EntityComponentManager &_ecm); /// \brief Load IMU sensors private: void LoadImuSensors( - sdf::ElementPtr _sdf, + const sdf::ElementPtr &_sdf, gz::sim::EntityComponentManager &_ecm); /// \brief Load GPS sensors private: void LoadGpsSensors( - sdf::ElementPtr _sdf, + const sdf::ElementPtr &_sdf, gz::sim::EntityComponentManager &_ecm); /// \brief Load range sensors private: void LoadRangeSensors( - sdf::ElementPtr _sdf, + const sdf::ElementPtr &_sdf, gz::sim::EntityComponentManager &_ecm); /// \brief Load wind sensors private: void LoadWindSensors( - sdf::ElementPtr _sdf, + const sdf::ElementPtr &_sdf, gz::sim::EntityComponentManager &_ecm); /// \brief Update the control surfaces controllers. @@ -173,7 +172,7 @@ class GZ_SIM_VISIBLE ArduPilotPlugin: private: void SendState() const; /// \brief Initialise flight dynamics model socket - private: bool InitSockets(sdf::ElementPtr _sdf) const; + private: bool InitSockets(const sdf::ElementPtr& _sdf) const; /// \brief Private data pointer. private: std::unique_ptr dataPtr; diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 3ef599bf..20d8b2b3 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -24,9 +24,11 @@ #include #include +#include #include #include #include +#include #include #include @@ -84,14 +86,14 @@ class Control public: Control() { // most of these coefficients are not used yet. - this->rotorVelocitySlowdownSim = this->kDefaultRotorVelocitySlowdownSim; - this->frequencyCutoff = this->kDefaultFrequencyCutoff; - this->samplingRate = this->kDefaultSamplingRate; + this->rotorVelocitySlowdownSim = kDefaultRotorVelocitySlowdownSim; + this->frequencyCutoff = kDefaultFrequencyCutoff; + this->samplingRate = kDefaultSamplingRate; this->pid.Init(0.1, 0, 0, 0, 0, 1.0, -1.0); } - public: ~Control() {} + public: ~Control() = default; /// \brief The PWM channel used to command this control public: int channel = 0; @@ -121,7 +123,7 @@ class Control public: std::string cmdTopic; /// \brief The joint being controlled - public: gz::sim::Entity joint; + public: gz::sim::Entity joint{gz::sim::kNullEntity}; /// \brief A multiplier to scale the raw input command public: double multiplier = 1.0; @@ -169,8 +171,8 @@ class OnMessageWrapper public: callback_t callback; /// \brief Constructor - public: OnMessageWrapper(const callback_t &_callback) - : callback(_callback) + public: explicit OnMessageWrapper(callback_t _callback) + : callback(std::move(_callback)) { } @@ -231,7 +233,7 @@ class gz::sim::systems::ArduPilotPluginPrivate public: uint16_t fdm_port_in{9002}; /// \brief The port for the SITL flight controller - auto detected - public: uint16_t fcu_port_out; + public: uint16_t fcu_port_out{0}; /// \brief The name of the IMU sensor public: std::string imuName; @@ -285,12 +287,12 @@ class gz::sim::systems::ArduPilotPluginPrivate { // Extract data double range_max = _msg.range_max(); - auto&& ranges = _msg.ranges(); - auto&& intensities = _msg.intensities(); + auto&& m_ranges = _msg.ranges(); + // auto&& intensities = _msg.intensities(); // unused // If there is no return, the range should be greater than range_max double sample_min = 2.0 * range_max; - for (auto&& range : ranges) + for (auto&& range : m_ranges) { sample_min = std::min( sample_min, std::isinf(range) ? 2.0 * range_max : range); @@ -298,7 +300,7 @@ class gz::sim::systems::ArduPilotPluginPrivate // Aquire lock and update the range data std::lock_guard lock(this->rangeMsgMutex); - this->ranges[_sensorIndex] = sample_min; + this->ranges[static_cast(_sensorIndex)] = sample_min; } // Anemometer @@ -343,7 +345,7 @@ class gz::sim::systems::ArduPilotPluginPrivate /// \brief Max number of consecutive missed ArduPilot controller /// messages before timeout - public: int connectionTimeoutMaxCount; + public: int connectionTimeoutMaxCount{}; /// \brief Transform from model orientation to x-forward and z-up public: gz::math::Pose3d modelXYZToAirplaneXForwardZDown; @@ -352,10 +354,10 @@ class gz::sim::systems::ArduPilotPluginPrivate public: gz::math::Pose3d gazeboXYZToNED; /// \brief Last received frame rate from the ArduPilot controller - public: uint16_t fcu_frame_rate; + public: uint16_t fcu_frame_rate{}; /// \brief Last received frame count from the ArduPilot controller - public: uint32_t fcu_frame_count = -1; + public: uint32_t fcu_frame_count = UINT32_MAX; /// \brief Last sent JSON string, so we can resend if needed. public: std::string json_str; @@ -380,13 +382,9 @@ gz::sim::systems::ArduPilotPlugin::ArduPilotPlugin() { } -///////////////////////////////////////////////// -gz::sim::systems::ArduPilotPlugin::~ArduPilotPlugin() -{ -} ///////////////////////////////////////////////// -void gz::sim::systems::ArduPilotPlugin::Reset(const UpdateInfo &_info, +void gz::sim::systems::ArduPilotPlugin::Reset(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { if (!_ecm.EntityHasComponentType(this->dataPtr->imuLink, @@ -509,11 +507,10 @@ void gz::sim::systems::ArduPilotPlugin::Configure( sdfClone->Get("have_32_channels", false).first; // Add the signal handler - this->dataPtr->sigHandler.AddCallback( - std::bind( - &gz::sim::systems::ArduPilotPluginPrivate::OnSignal, - this->dataPtr.get(), - std::placeholders::_1)); + this->dataPtr->sigHandler.AddCallback( + [capture0 = this->dataPtr.get()](auto &&PH1) { + capture0->OnSignal(std::forward(PH1)); + }); gzlog << "[" << this->dataPtr->modelName << "] " << "ArduPilot ready to fly. The force will be with you" << "\n"; @@ -521,7 +518,7 @@ void gz::sim::systems::ArduPilotPlugin::Configure( ///////////////////////////////////////////////// void gz::sim::systems::ArduPilotPlugin::LoadControlChannels( - sdf::ElementPtr _sdf, + const sdf::ElementPtr& _sdf, gz::sim::EntityComponentManager &_ecm) { // per control channel @@ -555,7 +552,7 @@ void gz::sim::systems::ArduPilotPlugin::LoadControlChannels( } else { - control.channel = this->dataPtr->controls.size(); + control.channel = static_cast(this->dataPtr->controls.size()); gzwarn << "[" << this->dataPtr->modelName << "] " << "id/channel attribute not specified, use order parsed [" << control.channel << "].\n"; @@ -793,7 +790,7 @@ void gz::sim::systems::ArduPilotPlugin::LoadControlChannels( ///////////////////////////////////////////////// void gz::sim::systems::ArduPilotPlugin::LoadImuSensors( - sdf::ElementPtr _sdf, + const sdf::ElementPtr &_sdf, gz::sim::EntityComponentManager &/*_ecm*/) { this->dataPtr->imuName = @@ -802,7 +799,7 @@ void gz::sim::systems::ArduPilotPlugin::LoadImuSensors( ///////////////////////////////////////////////// void gz::sim::systems::ArduPilotPlugin::LoadGpsSensors( - sdf::ElementPtr /*_sdf*/, + const sdf::ElementPtr &/*_sdf*/, gz::sim::EntityComponentManager &/*_ecm*/) { /* @@ -875,13 +872,13 @@ void gz::sim::systems::ArduPilotPlugin::LoadGpsSensors( ///////////////////////////////////////////////// void gz::sim::systems::ArduPilotPlugin::LoadRangeSensors( - sdf::ElementPtr _sdf, + const sdf::ElementPtr &_sdf, gz::sim::EntityComponentManager &/*_ecm*/) { struct SensorIdentifier { std::string type; - int index; + int index{}; std::string topic; }; std::vector sensorIds; @@ -962,15 +959,15 @@ void gz::sim::systems::ArduPilotPlugin::LoadRangeSensors( // Bind the sensor index to the callback function // (adjust from unit to zero offset) OnMessageWrapper::callback_t fn = - std::bind( - &gz::sim::systems::ArduPilotPluginPrivate::RangeCb, - this->dataPtr.get(), - std::placeholders::_1, - sensorId.index - 1); + [capture0 = this->dataPtr.get(), capture1 = sensorId.index - 1]( + auto &&PH1) { + capture0->RangeCb(std::forward(PH1), + capture1); + }; // Wrap the std::function so we can register the callback - auto callbackWrapper = RangeOnMessageWrapperPtr( - new OnMessageWrapper(fn)); + auto callbackWrapper = + std::make_shared>(fn); auto callback = &OnMessageWrapper::OnMessage; @@ -991,7 +988,7 @@ void gz::sim::systems::ArduPilotPlugin::LoadRangeSensors( ///////////////////////////////////////////////// void gz::sim::systems::ArduPilotPlugin::LoadWindSensors( - sdf::ElementPtr _sdf, + const sdf::ElementPtr &_sdf, gz::sim::EntityComponentManager &/*_ecm*/) { this->dataPtr->anemometerName = @@ -1243,22 +1240,23 @@ void gz::sim::systems::ArduPilotPlugin::PostUpdate( void gz::sim::systems::ArduPilotPlugin::ResetPIDs() { // Reset velocity PID for controls - for (size_t i = 0; i < this->dataPtr->controls.size(); ++i) + for (auto & control : this->dataPtr->controls) { - this->dataPtr->controls[i].cmd = 0; + control.cmd = 0; // this->dataPtr->controls[i].pid.Reset(); } } ///////////////////////////////////////////////// -bool gz::sim::systems::ArduPilotPlugin::InitSockets(sdf::ElementPtr _sdf) const -{ +bool gz::sim::systems::ArduPilotPlugin::InitSockets( + const sdf::ElementPtr &_sdf) const { // get the fdm address if provided, otherwise default to localhost this->dataPtr->fdm_address = _sdf->Get("fdm_addr", static_cast("127.0.0.1")).first; this->dataPtr->fdm_port_in = - _sdf->Get("fdm_port_in", static_cast(9002)).first; + static_cast(_sdf->Get("fdm_port_in", + static_cast(9002)).first); // output port configuration is automatic if (_sdf->HasElement("listen_addr")) { @@ -1419,7 +1417,7 @@ ssize_t getServoPacket( int counter = 0; while (true) { - TServoPacket last_pkt; + TServoPacket last_pkt{}; auto recvSize_last = _sock.recv(&last_pkt, sizeof(TServoPacket), 0ul); if (recvSize_last == -1) { @@ -1465,14 +1463,14 @@ bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket() } // 16 / 32 channel compatibility - uint16_t pkt_magic{0}; - uint16_t pkt_frame_rate{0}; - uint16_t pkt_frame_count{0}; - std::array pkt_pwm; + uint16_t pkt_magic{}; + uint16_t pkt_frame_rate{}; + uint32_t pkt_frame_count{}; + std::array pkt_pwm{}; ssize_t recvSize{-1}; if (this->dataPtr->have32Channels) { - servo_packet_32 pkt; + servo_packet_32 pkt{}; recvSize = getServoPacket( this->dataPtr->sock, this->dataPtr->fcu_address, @@ -1487,7 +1485,7 @@ bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket() } else { - servo_packet_16 pkt; + servo_packet_16 pkt{}; recvSize = getServoPacket( this->dataPtr->sock, this->dataPtr->fcu_address, @@ -1631,7 +1629,8 @@ void gz::sim::systems::ArduPilotPlugin::UpdateMotorCommands( { // convert pwm to raw cmd: [servo_min, servo_max] => [0, 1], // default is: [1000, 2000] => [0, 1] - const double pwm = _pwm[this->dataPtr->controls[i].channel]; + const double pwm = _pwm[static_cast( + this->dataPtr->controls[i].channel)]; const double pwm_min = this->dataPtr->controls[i].servo_min; const double pwm_max = this->dataPtr->controls[i].servo_max; const double multiplier = this->dataPtr->controls[i].multiplier; @@ -1858,11 +1857,11 @@ void gz::sim::systems::ArduPilotPlugin::CreateStateJSON( windSpdBdyA = std::sqrt(windXBdyA * windXBdyA + windYBdyA * windYBdyA); windDirBdyA = atan2(windYBdyA, windXBdyA); - double windXSnsG = windVelSnsG.X(); - double windYSnsG = windVelSnsG.Y(); - auto windSpdSnsG = std::sqrt( - windXSnsG * windXSnsG + windYSnsG * windYSnsG); - auto windDirSnsG = atan2(windYSnsG, windXSnsG); +// double windXSnsG = windVelSnsG.X(); +// double windYSnsG = windVelSnsG.Y(); +// auto windSpdSnsG = std::sqrt( +// windXSnsG * windXSnsG + windYSnsG * windYSnsG); +// auto windDirSnsG = atan2(windYSnsG, windXSnsG); // gzdbg << "\nEuler angles:\n" // << "bdyAToBdyG: " << bdyAToBdyG.Rot().Euler() << "\n" @@ -1938,21 +1937,27 @@ void gz::sim::systems::ArduPilotPlugin::CreateStateJSON( case 6: writer.Key("rng_6"); writer.Double(this->dataPtr->ranges[5]); + [[fallthrough]]; // Intentional fall-through case 5: writer.Key("rng_5"); writer.Double(this->dataPtr->ranges[4]); + [[fallthrough]]; // Intentional fall-through case 4: writer.Key("rng_4"); writer.Double(this->dataPtr->ranges[3]); + [[fallthrough]]; // Intentional fall-through case 3: writer.Key("rng_3"); writer.Double(this->dataPtr->ranges[2]); + [[fallthrough]]; // Intentional fall-through case 2: writer.Key("rng_2"); writer.Double(this->dataPtr->ranges[1]); + [[fallthrough]]; // Intentional fall-through case 1: writer.Key("rng_1"); writer.Double(this->dataPtr->ranges[0]); + break; default: break; } diff --git a/src/GstCameraPlugin.cc b/src/GstCameraPlugin.cc index 7c7f9842..2ff528fb 100644 --- a/src/GstCameraPlugin.cc +++ b/src/GstCameraPlugin.cc @@ -184,7 +184,7 @@ void GstCameraPlugin::Configure( std::bind(&GstCameraPlugin::Impl::OnRenderTeardown, impl.get()))); } -void GstCameraPlugin::PreUpdate(const UpdateInfo &_info, +void GstCameraPlugin::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { if (impl->cameraName.empty()) @@ -293,7 +293,7 @@ void GstCameraPlugin::Impl::StartStreaming() void *GstCameraPlugin::Impl::StartThread(void *param) { - GstCameraPlugin::Impl *impl = (GstCameraPlugin::Impl *)param; + GstCameraPlugin::Impl *impl = static_cast(param); impl->StartGstThread(); return nullptr; } @@ -351,7 +351,7 @@ void GstCameraPlugin::Impl::StartGstThread() if (ret != GST_STATE_CHANGE_SUCCESS) { gzmsg << "GstCameraPlugin: GStreamer element set state returned: " - << ret << std::endl; + << static_cast(ret) << std::endl; } // this call blocks until the main_loop is killed @@ -501,8 +501,8 @@ void GstCameraPlugin::Impl::OnImage(const msgs::Image &msg) if (!isGstMainLoopActive) return; // Alloc buffer - const guint size = width * height * 1.5; - GstBuffer *buffer = gst_buffer_new_allocate(NULL, size, NULL); + const auto size = static_cast(width * height * 1.5); + GstBuffer *buffer = gst_buffer_new_allocate(nullptr, size, nullptr); if (!buffer) { @@ -520,8 +520,10 @@ void GstCameraPlugin::Impl::OnImage(const msgs::Image &msg) } // Color Conversion from RGB to YUV - cv::Mat frame = cv::Mat(height, width, CV_8UC3); - cv::Mat frameYUV = cv::Mat(height, width, CV_8UC3); + cv::Mat frame = cv::Mat(static_cast(height), static_cast(width), + CV_8UC3); + cv::Mat frameYUV = cv::Mat(static_cast(height), + static_cast(width), CV_8UC3); frame.data = reinterpret_cast( const_cast(msg.data().c_str())); @@ -568,7 +570,7 @@ void GstCameraPlugin::Impl::StopStreaming() { StopGstThread(); - pthread_join(threadId, NULL); + pthread_join(threadId, nullptr); isGstMainLoopActive = false; } }