Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[Garden] Wildlife scoring plugin - Part2 #547

Merged
merged 6 commits into from
Dec 1, 2022
Merged

[Garden] Wildlife scoring plugin - Part2 #547

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
7736275
All code ported.
caguero Nov 21, 2022
928721f
Plugin added to the wildlife world.
caguero Nov 21, 2022
a7626f6
Plugin working.
caguero Nov 24, 2022
d03679b
Fix contacts.
caguero Nov 24, 2022
32b09fa
tweaks to comments
M1chaelM Dec 1, 2022
2f77bdf
Feedback
caguero Dec 1, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions vrx_gz/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,7 @@ list(APPEND VRX_GZ_PLUGINS
StationkeepingScoringPlugin
Surface
WaveVisual
WildlifeScoringPlugin
)

foreach(PLUGIN ${VRX_GZ_PLUGINS})
Expand Down
24 changes: 20 additions & 4 deletions vrx_gz/src/ScoringPlugin.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <gz/sim/Entity.hh>
#include <gz/sim/Events.hh>
#include <gz/sim/Model.hh>
#include <gz/sim/Util.hh>
#include <gz/plugin/Register.hh>
#include <gz/transport/Node.hh>
#include <sdf/sdf.hh>
Expand Down Expand Up @@ -61,7 +62,7 @@ class ScoringPlugin::Implementation
public: std::string vehicleName;

/// \brief Vehicle to score.
public: sim::Model vehicleModel{gz::sim::kNullEntity};
public: sim::Entity vehicleModel{gz::sim::kNullEntity};

/// \brief Silent mode enabled?
public: bool silent = false;
Expand Down Expand Up @@ -413,6 +414,9 @@ void ScoringPlugin::Configure(const sim::Entity &_entity,
this->dataPtr->releasePub = this->dataPtr->node.Advertise<msgs::Empty>(
this->dataPtr->releaseTopic);

this->dataPtr->node.Subscribe("/vrx/contacts",
&ScoringPlugin::OnContacts, this);

if (char *envDbg = std::getenv("VRX_DEBUG"))
{
if (std::string(envDbg) == "false")
Expand All @@ -431,6 +435,13 @@ void ScoringPlugin::PreUpdate(const sim::UpdateInfo &_info,
{
GZ_PROFILE("ScoringPlugin::PreUpdate");

if (this->dataPtr->vehicleModel == sim::kNullEntity)
{
auto entity = sim::entitiesFromScopedName(this->dataPtr->vehicleName, _ecm);
if (!entity.empty())
this->dataPtr->vehicleModel = *entity.begin();
}

this->dataPtr->UpdateTime(_info.simTime);
this->UpdateTaskState();
this->dataPtr->PublishStats();
Expand Down Expand Up @@ -501,13 +512,18 @@ void ScoringPlugin::Finish()
this->OnFinished();
}


//////////////////////////////////////////////////
std::string ScoringPlugin::VehicleName() const
{
return this->dataPtr->vehicleName;
}

//////////////////////////////////////////////////
gz::sim::Entity ScoringPlugin::VehicleEntity() const
{
return this->dataPtr->vehicleModel;
}

//////////////////////////////////////////////////
uint16_t ScoringPlugin::NumCollisions() const
{
Expand Down Expand Up @@ -566,8 +582,8 @@ void ScoringPlugin::OnContacts(const gz::msgs::Contacts &_contacts)
std::string wamvCollisionStr2 = _contacts.contact(i).collision2().name();

bool isWamvHit =
wamvCollisionStr1.find("wamv::base_link::") != std::string::npos ||
wamvCollisionStr2.find("wamv::base_link::") != std::string::npos;
wamvCollisionStr1.find("wamv/base_link::") != std::string::npos ||
wamvCollisionStr2.find("wamv/base_link::") != std::string::npos;

bool isHitBufferPassed =
(this->dataPtr->currentTime - this->dataPtr->lastCollisionTime).count() >
Expand Down
9 changes: 6 additions & 3 deletions vrx_gz/src/ScoringPlugin.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -135,9 +135,8 @@ namespace vrx
gz::sim::EventManager &_eventMgr) override;

// Documentation inherited.
public: void PreUpdate(
const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm) override;
public: void PreUpdate(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm) override;

/// \brief Get the current score.
/// \return The current score.
Expand Down Expand Up @@ -183,6 +182,10 @@ namespace vrx
/// \return The vehicle name.
protected: std::string VehicleName() const;

/// \brief Get the vehicle entity.
/// \return The vehicle entity.
protected: gz::sim::Entity VehicleEntity() const;

/// \brief Get the number of WAM-V collisions.
/// \return Number of collisions
protected: uint16_t NumCollisions() const;
Expand Down
5 changes: 3 additions & 2 deletions vrx_gz/src/StationkeepingScoringPlugin.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,8 +132,9 @@ void StationkeepingScoringPlugin::Configure(const sim::Entity &_entity,
{
ScoringPlugin::Configure(_entity, _sdf, _ecm, _eventMgr);
gzmsg << "Task [" << this->TaskName() << "]" << std::endl;
auto worldEntity =
_ecm.EntityByComponents(sim::components::World());

// Initialize spherical coordinates instance.
auto worldEntity = _ecm.EntityByComponents(sim::components::World());
sim::World world(worldEntity);
this->dataPtr->sc = world.SphericalCoordinates(_ecm).value();

Expand Down
Loading