Skip to content

Commit

Permalink
Static code check error fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
dtdavi1 committed Aug 27, 2021
1 parent 6db5121 commit 23ec064
Show file tree
Hide file tree
Showing 4 changed files with 29 additions and 19 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ namespace dave_simulator_ros
dave_gazebo_ros_plugins::SetCurrentDirection::Request& _req,
dave_gazebo_ros_plugins::SetCurrentDirection::Response& _res);

/// \brief Service call to update a stratified current horizontal angle mean value
/// \brief Service call to update a stratified crnt horizontal angle mean value
public: bool UpdateStratHorzAngle(
dave_gazebo_ros_plugins::SetStratifiedCurrentDirection::Request& _req,
dave_gazebo_ros_plugins::SetStratifiedCurrentDirection::Response& _res);
Expand Down
19 changes: 12 additions & 7 deletions gazebo/dave_gazebo_ros_plugins/src/ocean_current_plugin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -277,7 +277,8 @@ bool UnderwaterCurrentROSPlugin::UpdateStratHorzAngle(
}
_res.success =
this->stratifiedCurrentModels[_req.layer][1].SetMean(_req.angle);
if (_res.success) {
if (_res.success)
{
// Update the database values (new angle, unchanged velocity)
double velocity = hypot(this->stratifiedDatabase[_req.layer].X(),
this->stratifiedDatabase[_req.layer].Y());
Expand Down Expand Up @@ -351,15 +352,17 @@ bool UnderwaterCurrentROSPlugin::UpdateStratCurrentVelocity(
return true;
}
if (this->stratifiedCurrentModels[_req.layer][0].SetMean(_req.velocity) &&
this->stratifiedCurrentModels[_req.layer][1].SetMean(_req.horizontal_angle) &&
this->stratifiedCurrentModels[_req.layer][2].SetMean(_req.vertical_angle))
this->stratifiedCurrentModels[_req.layer][1].SetMean(
_req.horizontal_angle) &&
this->stratifiedCurrentModels[_req.layer][2].SetMean(
_req.vertical_angle))
{
// Update the database values as well
this->stratifiedDatabase[_req.layer].X() = cos(_req.horizontal_angle) *
_req.velocity;
this->stratifiedDatabase[_req.layer].Y() = sin(_req.horizontal_angle) *
_req.velocity;
gzmsg << "Layer " << _req.layer << " current velocity [m/s] = "
gzmsg << "Layer " << _req.layer << " current velocity [m/s] = "
<< _req.velocity << std::endl
<< " Horizontal angle [rad] = " << _req.horizontal_angle
<< std::endl
Expand Down Expand Up @@ -396,7 +399,7 @@ bool UnderwaterCurrentROSPlugin::GetCurrentHorzAngleModel(
dave_gazebo_ros_plugins::GetCurrentModel::Response& _res)
{
_res.mean = this->currentHorzAngleModel.mean;
_res.max = this->currentHorzAngleModel.min;
_res.min = this->currentHorzAngleModel.min;
_res.max = this->currentHorzAngleModel.max;
_res.noise = this->currentHorzAngleModel.noiseAmp;
_res.mu = this->currentHorzAngleModel.mu;
Expand Down Expand Up @@ -429,7 +432,8 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentVelocityModel(
_req.mu,
_req.noise);

for (int i = 0; i < this->stratifiedCurrentModels.size(); i++) {
for (int i = 0; i < this->stratifiedCurrentModels.size(); i++)
{
gazebo::GaussMarkovProcess model = this->stratifiedCurrentModels[i][0];
model.SetModel(
model.mean,
Expand All @@ -452,7 +456,8 @@ bool UnderwaterCurrentROSPlugin::UpdateCurrentHorzAngleModel(
{
_res.success = this->currentHorzAngleModel.SetModel(_req.mean, _req.min,
_req.max, _req.mu, _req.noise);
for (int i = 0; i < this->stratifiedCurrentModels.size(); i++) {
for (int i = 0; i < this->stratifiedCurrentModels.size(); i++)
{
gazebo::GaussMarkovProcess model = this->stratifiedCurrentModels[i][1];
model.SetModel(model.mean,
std::max(-M_PI, _req.min),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -126,8 +126,9 @@ namespace gazebo
/// \brief Gauss-Markov process instance for vertical angle model
protected: GaussMarkovProcess currentVertAngleModel;

/// \brief Vector of Gauss-Markov process instances for strativied velocity
protected: std::vector<std::vector<GaussMarkovProcess>> stratifiedCurrentModels;
/// \brief Vector of Gauss-Markov process instances for stratified velocity
protected: std::vector<std::vector<GaussMarkovProcess>>
stratifiedCurrentModels;

/// \brief Vector of dateGMT for tidal oscillation
protected: std::vector<std::array<int, 5>> dateGMT;
Expand Down
22 changes: 13 additions & 9 deletions gazebo/dave_gazebo_world_plugins/src/ocean_current_world_plugin.cc
Original file line number Diff line number Diff line change
Expand Up @@ -130,13 +130,14 @@ void UnderwaterCurrentPlugin::Update(const common::UpdateInfo & /** _info */)

// Generate the depth-specific velocities
this->currentStratifiedVelocity.clear();
for (int i=0; i < this->stratifiedDatabase.size(); i++) {
for (int i = 0; i < this->stratifiedDatabase.size(); i++)
{
double depth = this->stratifiedDatabase[i].Z();
currentVelMag =
this->stratifiedCurrentModels[i][0].Update(time.Double());
horzAngle =
this->stratifiedCurrentModels[i][1].Update(time.Double());
vertAngle =
vertAngle =
this->stratifiedCurrentModels[i][2].Update(time.Double());
ignition::math::Vector4d depthVel(
currentVelMag * cos(horzAngle) * cos(vertAngle),
Expand Down Expand Up @@ -322,7 +323,8 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase()
// boost::filesystem::path
// concatPath(boost::filesystem::initial_path().parent_path());
// concatPath +=
// "/uuv_ws/src/dave/models/dave_worlds/worlds/transientOceanCurrentDatabase.csv";
// ("/uuv_ws/src/dave/models/dave_worlds/" +
// "worlds/transientOceanCurrentDatabase.csv");
// this->databaseFilePath = concatPath.generic_string();
//
// Use ros package path:
Expand Down Expand Up @@ -402,7 +404,7 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase()
hAngleModel.mu = this->currentHorzAngleModel.mu;
hAngleModel.noiseAmp = this->currentHorzAngleModel.noiseAmp;
hAngleModel.lastUpdate = this->lastUpdate.Double();

GaussMarkovProcess vAngleModel;
vAngleModel.mean = 0.0;
vAngleModel.var = vAngleModel.mean;
Expand All @@ -411,7 +413,7 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase()
vAngleModel.mu = this->currentVertAngleModel.mu;
vAngleModel.noiseAmp = this->currentVertAngleModel.noiseAmp;
vAngleModel.lastUpdate = this->lastUpdate.Double();

std::vector<GaussMarkovProcess> depthModels;
depthModels.push_back(magnitudeModel);
depthModels.push_back(hAngleModel);
Expand All @@ -421,8 +423,9 @@ void UnderwaterCurrentPlugin::LoadStratifiedCurrentDatabase()
csvFile.close();

this->publishers[this->stratifiedCurrentVelocityTopic] =
this->node->Advertise<dave_gazebo_world_plugins_msgs::msgs::StratifiedCurrentVelocity>(
this->ns + "/" + this->stratifiedCurrentVelocityTopic);
this->node->Advertise<dave_gazebo_world_plugins_msgs::msgs::
StratifiedCurrentVelocity>(
this->ns + "/" + this->stratifiedCurrentVelocityTopic);
gzmsg << "Stratified current velocity topic name: " <<
this->ns + "/" + this->stratifiedCurrentVelocityTopic << std::endl;
}
Expand Down Expand Up @@ -613,10 +616,11 @@ void UnderwaterCurrentPlugin::PublishCurrentVelocity()
void UnderwaterCurrentPlugin::PublishStratifiedCurrentVelocity()
{
dave_gazebo_world_plugins_msgs::msgs::StratifiedCurrentVelocity currentVel;
for (std::vector<ignition::math::Vector4d>::iterator it = this->currentStratifiedVelocity.begin();
for (std::vector<ignition::math::Vector4d>::iterator it =
this->currentStratifiedVelocity.begin();
it != this->currentStratifiedVelocity.end(); ++it)
{
msgs::Set(currentVel.add_velocity(),
msgs::Set(currentVel.add_velocity(),
ignition::math::Vector3d(it->X(), it->Y(), it->Z()));
currentVel.add_depth(it->W());
}
Expand Down

0 comments on commit 23ec064

Please sign in to comment.