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

Jump to bottom

Checking for models with dispensable in names to not be considered an obstacle by slotcar #74

Merged
merged 3 commits into from
Jun 27, 2022
Merged

Checking for models with dispensable in names to not be considered an obstacle by slotcar #74

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7dc1f3b
Checking for models with dispensable in names to not be considered an…
aaronchongth Jun 23, 2022
8d159ef
Merge branch 'main' into feature/dispensable_models_not_obstacles
aaronchongth Jun 24, 2022
96254f1
Add explanation that it will not be checked in the future again
aaronchongth Jun 27, 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
26 changes: 15 additions & 11 deletions rmf_robot_sim_gazebo_plugins/src/slotcar.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,12 +30,12 @@ class SlotcarPlugin : public gazebo::ModelPlugin

std::array<gazebo::physics::JointPtr, 2> joints;

std::unordered_set<gazebo::physics::Model*> infrastructure;
std::unordered_set<gazebo::physics::Model*> obstacle_exclusions;

// Book keeping
double last_update_time = 0.0;

void init_infrastructure();
void init_obstacle_exclusions();

std::vector<Eigen::Vector3d> get_obstacle_positions(
const gazebo::physics::WorldPtr& world);
Expand Down Expand Up @@ -115,14 +115,14 @@ void SlotcarPlugin::charge_state_cb(ConstSelectionPtr& msg)
dataPtr->charge_state_cb(msg->name(), msg->selected());
}

void SlotcarPlugin::init_infrastructure()
void SlotcarPlugin::init_obstacle_exclusions()
{
const auto& world = _model->GetWorld();
infrastructure.insert(_model.get());
obstacle_exclusions.insert(_model.get());
const auto& all_models = world->Models();
for (const auto& m : all_models)
{
// Object should not be static and part of infrastructure
// Object should not be static, be part of infrastructure, or dispensable
if (!m->IsStatic())
{
std::string name = m->GetName();
Expand All @@ -131,8 +131,9 @@ void SlotcarPlugin::init_infrastructure()
c = ::tolower(c);
});
if (name.find("door") != std::string::npos ||
name.find("lift") != std::string::npos)
infrastructure.insert(m.get());
name.find("lift") != std::string::npos ||
name.find("dispensable") != std::string::npos)
obstacle_exclusions.insert(m.get());
}
}
}
Expand All @@ -144,11 +145,11 @@ std::vector<Eigen::Vector3d> SlotcarPlugin::get_obstacle_positions(

for (const auto& m : world->Models())
{
// Object should not be static, not part of infrastructure
// Object should not be static, not part of obstacle_exclusions,
// and close than a threshold (checked by common function)
const auto p_obstacle = m->WorldPose().Pos();
if (m->IsStatic() == false &&
infrastructure.find(m.get()) == infrastructure.end())
obstacle_exclusions.find(m.get()) == obstacle_exclusions.end())
obstacle_positions.push_back(rmf_plugins_utils::convert_vec(p_obstacle));
}

Expand All @@ -158,8 +159,11 @@ std::vector<Eigen::Vector3d> SlotcarPlugin::get_obstacle_positions(
void SlotcarPlugin::OnUpdate()
{
const auto& world = _model->GetWorld();
if (infrastructure.empty())
init_infrastructure();

// After initialization once, this set will have at least one exclusion, which
// is the itself.
if (obstacle_exclusions.empty())
init_obstacle_exclusions();

const double time = world->SimTime().Double();
const double dt = time - last_update_time;
Expand Down
24 changes: 14 additions & 10 deletions rmf_robot_sim_ignition_plugins/src/slotcar.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ class IGNITION_GAZEBO_VISIBLE SlotcarPlugin

Entity _entity;
std::unordered_set<Entity> _payloads;
std::unordered_set<Entity> _infrastructure;
std::unordered_set<Entity> _obstacle_exclusions;
double _height = 0;

PhysEnginePlugin phys_plugin = PhysEnginePlugin::DEFAULT;
Expand All @@ -73,7 +73,7 @@ class IGNITION_GAZEBO_VISIBLE SlotcarPlugin
const double target_linear_speed_now,
const double target_linear_speed_destination,
const std::optional<double>& max_linear_velocity);
void init_infrastructure(EntityComponentManager& ecm);
void init_obstacle_exclusions(EntityComponentManager& ecm);
void item_dispensed_cb(const ignition::msgs::UInt64_V& msg);
void item_ingested_cb(const ignition::msgs::Entity& msg);
bool get_slotcar_height(const ignition::msgs::Entity& req,
Expand Down Expand Up @@ -207,7 +207,7 @@ void SlotcarPlugin::send_control_signals(EntityComponentManager& ecm,
}
}

void SlotcarPlugin::init_infrastructure(EntityComponentManager& ecm)
void SlotcarPlugin::init_obstacle_exclusions(EntityComponentManager& ecm)
{
// Cycle through all the static entities with Model and Name components
ecm.Each<components::Model, components::Name, components::Pose,
Expand All @@ -227,13 +227,14 @@ void SlotcarPlugin::init_infrastructure(EntityComponentManager& ecm)
c = ::tolower(c);
});
if (n.find("door") != std::string::npos ||
n.find("lift") != std::string::npos)
_infrastructure.insert(entity);
n.find("lift") != std::string::npos ||
n.find("dispensable") != std::string::npos)
_obstacle_exclusions.insert(entity);
}
return true;
});
// Also add itself
_infrastructure.insert(_entity);
_obstacle_exclusions.insert(_entity);
}

std::vector<Eigen::Vector3d> SlotcarPlugin::get_obstacle_positions(
Expand All @@ -250,11 +251,11 @@ std::vector<Eigen::Vector3d> SlotcarPlugin::get_obstacle_positions(
) -> bool
{
// Object should not be static
// It should not be part of infrastructure (doors / lifts)
// It should not be part of obstacle exclusions (doors/lifts/dispensables)
// And it should be closer than the "stop" range (checked by common)
const auto obstacle_position = pose->Data().Pos();
if (is_static->Data() == false &&
_infrastructure.find(entity) == _infrastructure.end())
_obstacle_exclusions.find(entity) == _obstacle_exclusions.end())
{
obstacle_positions.push_back(rmf_plugins_utils::convert_vec(
obstacle_position));
Expand Down Expand Up @@ -457,8 +458,11 @@ void SlotcarPlugin::PreUpdate(const UpdateInfo& info,

// TODO parallel thread executor?
rclcpp::spin_some(_ros_node);
if (_infrastructure.empty())
init_infrastructure(ecm);

// After initialization once, this set will have at least one exclusion, which
// is the itself.
if (_obstacle_exclusions.empty())
init_obstacle_exclusions(ecm);

// Don't update the pose if the simulation is paused
if (info.paused)
Expand Down