Skip to content

Commit

Permalink
All: fix compile errors/warnings
Browse files Browse the repository at this point in the history
  • Loading branch information
@khancyr
khancyr committed Jul 31, 2024
1 parent ea1d9b1 commit 7f3fa7a
Show file tree
Hide file tree
Showing 3 changed files with 51 additions and 49 deletions.
17 changes: 8 additions & 9 deletions include/ArduPilotPlugin.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,6 @@ struct servo_packet_32 {
};

// Forward declare private data class
class ArduPilotSocketPrivate;
class ArduPilotPluginPrivate;

/// \brief Interface ArduPilot from ardupilot stack
Expand Down Expand Up @@ -86,7 +85,7 @@ class ArduPilotPluginPrivate;
/// controller synchronization
/// <have_32_channels> 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,
Expand All @@ -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;
Expand All @@ -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.
Expand Down Expand Up @@ -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<ArduPilotPluginPrivate> dataPtr;
Expand Down
67 changes: 35 additions & 32 deletions src/ArduPilotPlugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,14 +84,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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand All @@ -380,15 +380,12 @@ 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,
components::WorldPose::typeId))
{
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -555,7 +552,7 @@ void gz::sim::systems::ArduPilotPlugin::LoadControlChannels(
}
else
{
control.channel = this->dataPtr->controls.size();
control.channel = static_cast<int>(this->dataPtr->controls.size());
gzwarn << "[" << this->dataPtr->modelName << "] "
<< "id/channel attribute not specified, use order parsed ["
<< control.channel << "].\n";
Expand Down Expand Up @@ -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 =
Expand All @@ -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*/)
{
/*
Expand Down Expand Up @@ -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<SensorIdentifier> sensorIds;
Expand Down Expand Up @@ -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 =
Expand Down Expand Up @@ -1251,14 +1248,14 @@ 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<std::string>("127.0.0.1")).first;

this->dataPtr->fdm_port_in =
_sdf->Get("fdm_port_in", static_cast<uint32_t>(9002)).first;
static_cast<uint16_t>(_sdf->Get("fdm_port_in", static_cast<uint32_t>(9002)).first);

// output port configuration is automatic
if (_sdf->HasElement("listen_addr")) {
Expand Down Expand Up @@ -1419,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)
{
Expand Down Expand Up @@ -1467,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<uint16_t, 32> pkt_pwm;
uint32_t pkt_frame_count{0};
std::array<uint16_t, 32> 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,
Expand All @@ -1487,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,
Expand Down Expand Up @@ -1858,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"
Expand Down Expand Up @@ -1938,21 +1935,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;
}
Expand Down
16 changes: 8 additions & 8 deletions src/GstCameraPlugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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())
Expand Down Expand Up @@ -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<GstCameraPlugin::Impl *>(param);
impl->StartGstThread();
return nullptr;
}
Expand Down Expand Up @@ -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<int>(ret) << std::endl;
}

// this call blocks until the main_loop is killed
Expand Down Expand Up @@ -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<guint>(width * height * 1.5);
GstBuffer *buffer = gst_buffer_new_allocate(nullptr, size, nullptr);

if (!buffer)
{
Expand All @@ -520,8 +520,8 @@ 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<int>(height), static_cast<int>(width), CV_8UC3);
cv::Mat frameYUV = cv::Mat(static_cast<int>(height), static_cast<int>(width), CV_8UC3);
frame.data = reinterpret_cast<unsigned char *>(
const_cast<char *>(msg.data().c_str()));

Expand Down Expand Up @@ -568,7 +568,7 @@ void GstCameraPlugin::Impl::StopStreaming()
{
StopGstThread();

pthread_join(threadId, NULL);
pthread_join(threadId, nullptr);
isGstMainLoopActive = false;
}
}
Expand Down

0 comments on commit 7f3fa7a

Please sign in to comment.