From a693461273aaa669d2b792e2851d1b296fd319ab Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 00:48:57 +0200 Subject: [PATCH 01/24] CmakeList: add some compile options --- CMakeLists.txt | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index c12fe13f..f667a7d0 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,29 @@ 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}) + target_compile_options(${target} PRIVATE -Wall -Wextra -Wpedantic) + target_compile_options(${target} PRIVATE -Wshadow -Wformat) + target_compile_options(${target} PRIVATE -Wcast-align -Woverloaded-virtual -Wuseless-cast) + target_compile_options(${target} PRIVATE -Wduplicated-cond -Wduplicated-branches -Wlogical-op) + 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 -Wstrict-null-sentinel -Wswitch -Wundef) + target_compile_options(${target} PRIVATE -Wold-style-cast) + target_compile_options(${target} PRIVATE -Werror) + target_compile_options(${target} PRIVATE -Werror=return-type) + + target_compile_options(${target} PRIVATE -ffunction-sections -fdata-sections) + target_link_options(${target} PRIVATE -Wl,--gc-sections) +endforeach() + # --------------------------------------------------------------------------- # # Install. From b76b5d8c471325548e2cb9c3281956623c55e9a1 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 00:49:13 +0200 Subject: [PATCH 02/24] ArduPilotPlugin.hh: remove unexisting forward declaration --- include/ArduPilotPlugin.hh | 1 - 1 file changed, 1 deletion(-) diff --git a/include/ArduPilotPlugin.hh b/include/ArduPilotPlugin.hh index f53ff18b..2814733e 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 From ab89928e3833397c0c41135d761fa8abad6fda94 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:44:27 +0200 Subject: [PATCH 03/24] ArduPilotPlugin.hh: make class final to make clang-tidy happy --- include/ArduPilotPlugin.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ArduPilotPlugin.hh b/include/ArduPilotPlugin.hh index 2814733e..5d71b451 100755 --- a/include/ArduPilotPlugin.hh +++ b/include/ArduPilotPlugin.hh @@ -85,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, From 4acd2ebce20c79a72e75d26fed770fb8ac4211b3 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:45:09 +0200 Subject: [PATCH 04/24] ArduPilotPlugin.hh: make destructor default and final to make clang-tidy happy --- include/ArduPilotPlugin.hh | 2 +- src/ArduPilotPlugin.cc | 4 ---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/include/ArduPilotPlugin.hh b/include/ArduPilotPlugin.hh index 5d71b451..88c96945 100755 --- a/include/ArduPilotPlugin.hh +++ b/include/ArduPilotPlugin.hh @@ -96,7 +96,7 @@ class GZ_SIM_VISIBLE ArduPilotPlugin final: public: ArduPilotPlugin(); /// \brief Destructor. - public: ~ArduPilotPlugin(); + public: ~ArduPilotPlugin() final = default; public: void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) final; diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 3ef599bf..3828bee4 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -380,10 +380,6 @@ gz::sim::systems::ArduPilotPlugin::ArduPilotPlugin() { } -///////////////////////////////////////////////// -gz::sim::systems::ArduPilotPlugin::~ArduPilotPlugin() -{ -} ///////////////////////////////////////////////// void gz::sim::systems::ArduPilotPlugin::Reset(const UpdateInfo &_info, From 097e6290df2259fd6d9e3ce9d7a51b20ddafc5b2 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:46:44 +0200 Subject: [PATCH 05/24] ArduPilotPlugin.hh: sdf::ElementPtr can be access as const reference --- include/ArduPilotPlugin.hh | 12 ++++++------ src/ArduPilotPlugin.cc | 14 +++++++------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/include/ArduPilotPlugin.hh b/include/ArduPilotPlugin.hh index 88c96945..a96be8d0 100755 --- a/include/ArduPilotPlugin.hh +++ b/include/ArduPilotPlugin.hh @@ -119,27 +119,27 @@ class GZ_SIM_VISIBLE ArduPilotPlugin final: /// \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. @@ -172,7 +172,7 @@ class GZ_SIM_VISIBLE ArduPilotPlugin final: 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 3828bee4..2260da9d 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -517,7 +517,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 @@ -789,7 +789,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 = @@ -798,7 +798,7 @@ void gz::sim::systems::ArduPilotPlugin::LoadImuSensors( ///////////////////////////////////////////////// void gz::sim::systems::ArduPilotPlugin::LoadGpsSensors( - sdf::ElementPtr /*_sdf*/, + const sdf::ElementPtr &/*_sdf*/, gz::sim::EntityComponentManager &/*_ecm*/) { /* @@ -871,7 +871,7 @@ 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 @@ -987,7 +987,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 = @@ -1247,8 +1247,8 @@ void gz::sim::systems::ArduPilotPlugin::ResetPIDs() } ///////////////////////////////////////////////// -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; From 00aa2194319361593e1560c848d9064ee18de442 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:47:12 +0200 Subject: [PATCH 06/24] ArduPilotPlugin.cc: make Control destructor default --- src/ArduPilotPlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 2260da9d..e397f6fa 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -91,7 +91,7 @@ class Control 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; From 1e1e837692378045073d7eced0e4617ec68cafcd Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:47:54 +0200 Subject: [PATCH 07/24] ArduPilotPlugin.cc: fix static member access from instance --- src/ArduPilotPlugin.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index e397f6fa..d07e9d13 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -84,9 +84,9 @@ 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); } From 8293db4231274eadf6de46bcc540a46cfa1fb405 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:48:32 +0200 Subject: [PATCH 08/24] ArduPilotPlugin.cc: fix ranges variable shadowing --- src/ArduPilotPlugin.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index d07e9d13..b7c4e699 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -285,12 +285,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); From 46c5d46b9322e8cfde86f3e0e002ace708e0d3ce Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:48:50 +0200 Subject: [PATCH 09/24] ArduPilotPlugin.cc: fix fcu_frame_count implict cast --- src/ArduPilotPlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index b7c4e699..98546b91 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -355,7 +355,7 @@ class gz::sim::systems::ArduPilotPluginPrivate 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; From e53c3e24e984fe345adb15ab2af9b1efcf2e589d Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:49:16 +0200 Subject: [PATCH 10/24] ArduPilotPlugin.cc: fix unused parameter --- src/ArduPilotPlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 98546b91..ce38de16 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -382,7 +382,7 @@ 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, From b2c5158cc5cd1b33238dd7eeb16b980a490ec7f3 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:49:33 +0200 Subject: [PATCH 11/24] ArduPilotPlugin.cc: fix implicit fallthrough --- src/ArduPilotPlugin.cc | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index ce38de16..57f4e44c 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -1934,21 +1934,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; } From 60b979b7fe30181bce5d2f52de0827c8a579c574 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:50:01 +0200 Subject: [PATCH 12/24] ArduPilotPlugin.cc: fix implicast --- src/ArduPilotPlugin.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 57f4e44c..e1914ed5 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -551,7 +551,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"; @@ -1254,7 +1254,8 @@ bool gz::sim::systems::ArduPilotPlugin::InitSockets( _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")) { From 2c612746ae3a62c2338797c315f5d357fdb89f00 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:50:42 +0200 Subject: [PATCH 13/24] ArduPilotPlugin.cc: fix pkt_frame_count implicit cast --- src/ArduPilotPlugin.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index e1914ed5..345da239 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -1464,12 +1464,12 @@ 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; + uint32_t pkt_frame_count{0}; + 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, @@ -1484,7 +1484,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, From b5537b7fd1362038a4a64104888051cc25299492 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:51:01 +0200 Subject: [PATCH 14/24] ArduPilotPlugin.cc: comment unused variables --- src/ArduPilotPlugin.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 345da239..0db68c5b 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -1855,11 +1855,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" From 145bde5e5aafdbd9cf95aa3117a86c2e007324c3 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:51:28 +0200 Subject: [PATCH 15/24] ArduPilotPlugin.cc: zero init variable (clang-tidy check) --- src/ArduPilotPlugin.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 0db68c5b..818d2267 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -877,7 +877,7 @@ void gz::sim::systems::ArduPilotPlugin::LoadRangeSensors( struct SensorIdentifier { std::string type; - int index; + int index{}; std::string topic; }; std::vector sensorIds; @@ -1416,7 +1416,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) { From 2e2c252c0db9bea196bbde2407565ef25c4267de Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:52:03 +0200 Subject: [PATCH 16/24] GstCameraPlugin: fix unused parameter --- src/GstCameraPlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/GstCameraPlugin.cc b/src/GstCameraPlugin.cc index 7c7f9842..d4474ec7 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()) From 66bd3eece6054b128c49927fd97ab5926560e0be Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 1 Aug 2024 01:52:32 +0200 Subject: [PATCH 17/24] GstCameraPlugin: correct cast and nullptr usage --- src/GstCameraPlugin.cc | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/GstCameraPlugin.cc b/src/GstCameraPlugin.cc index d4474ec7..2ff528fb 100644 --- a/src/GstCameraPlugin.cc +++ b/src/GstCameraPlugin.cc @@ -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; } } From 68362da4f925f592251dded950978bad4b849048 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 5 Aug 2024 20:07:55 +0200 Subject: [PATCH 18/24] CMakeList: Correct compile flags for clang --- CMakeLists.txt | 33 +++++++++++++++++++-------------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f667a7d0..a2d3acdc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -124,20 +124,25 @@ 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}) - target_compile_options(${target} PRIVATE -Wall -Wextra -Wpedantic) - target_compile_options(${target} PRIVATE -Wshadow -Wformat) - target_compile_options(${target} PRIVATE -Wcast-align -Woverloaded-virtual -Wuseless-cast) - target_compile_options(${target} PRIVATE -Wduplicated-cond -Wduplicated-branches -Wlogical-op) - 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 -Wstrict-null-sentinel -Wswitch -Wundef) - target_compile_options(${target} PRIVATE -Wold-style-cast) - target_compile_options(${target} PRIVATE -Werror) - target_compile_options(${target} PRIVATE -Werror=return-type) - - target_compile_options(${target} PRIVATE -ffunction-sections -fdata-sections) - target_link_options(${target} PRIVATE -Wl,--gc-sections) + 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() # --------------------------------------------------------------------------- # From 9f0924260a4321df6ee67876695e25df48fe997d Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 5 Aug 2024 20:08:44 +0200 Subject: [PATCH 19/24] ArduPilotPlugin: fix implicit conversion changes signedness: 'int' to 'std::vector::size_type' (aka 'unsigned long') [-Werror,-Wsign-conversion] --- src/ArduPilotPlugin.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 818d2267..1bd859d6 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -298,7 +298,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 @@ -1628,7 +1628,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; From 60bc9624c286ca0a52d9edbf63593eb31d26bf47 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 5 Aug 2024 20:09:10 +0200 Subject: [PATCH 20/24] ArduPilotPlugin: use ranged base loop --- src/ArduPilotPlugin.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 1bd859d6..fae5a145 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -1239,9 +1239,9 @@ 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(); } } From 4af5cea5b7120e7df45b55f641befe36936d2746 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 5 Aug 2024 20:09:33 +0200 Subject: [PATCH 21/24] ArduPilotPlugin: used lambda instead of bind --- src/ArduPilotPlugin.cc | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index fae5a145..a2a2ba00 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -505,11 +505,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"; @@ -958,11 +957,11 @@ 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( From c5f1916fa88fd4bb9bef20d9730212b83629c2ce Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 5 Aug 2024 20:09:58 +0200 Subject: [PATCH 22/24] ArduPilotPlugin: use make_shared to create object --- src/ArduPilotPlugin.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index a2a2ba00..3586bcb1 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -964,8 +964,8 @@ void gz::sim::systems::ArduPilotPlugin::LoadRangeSensors( }; // 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; From 8d6727ea188d06d26c1d2d1733c46662b74fb55d Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 5 Aug 2024 20:10:25 +0200 Subject: [PATCH 23/24] ArduPilotPlugin: use std::move to get OnMessageWrapper callback --- src/ArduPilotPlugin.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 3586bcb1..7603ee52 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -169,8 +169,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)) { } From 551c0aa4e0b8f9125cb5979d9784c87f5ffa958c Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 5 Aug 2024 20:10:57 +0200 Subject: [PATCH 24/24] ArduPilotPlugin: fix initial value and import --- src/ArduPilotPlugin.cc | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 7603ee52..20d8b2b3 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -24,9 +24,11 @@ #include #include +#include #include #include #include +#include #include #include @@ -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; @@ -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; @@ -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,7 +354,7 @@ 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 = UINT32_MAX; @@ -1461,9 +1463,9 @@ bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket() } // 16 / 32 channel compatibility - uint16_t pkt_magic{0}; - uint16_t pkt_frame_rate{0}; - uint32_t pkt_frame_count{0}; + 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)