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

Upgrade to Ignition Fortress #70

Merged
merged 9 commits into from
Mar 8, 2022
Merged

Upgrade to Ignition Fortress #70

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
bcc6f53
WIP cleanup and migrating to fortress
luca-della-vedova Sep 27, 2021
c3e278a
Some cleanup and port building plugins
luca-della-vedova Sep 27, 2021
53a2441
Merge branch 'main' into feature/ignition_fortress
luca-della-vedova Feb 21, 2022
cadf244
Update rosdep keys to fortress
luca-della-vedova Feb 21, 2022
cdd5b95
Fix use of uninitialized joint position / velocity
luca-della-vedova Feb 22, 2022
ed07d0d
Fix wrong component during refactoring
luca-della-vedova Feb 22, 2022
ab94ed3
Remove old ign-math conversion functions
luca-della-vedova Mar 3, 2022
c2b0090
Pose component const correctness
luca-della-vedova Mar 8, 2022
254d720
Fix crash in lift plugin
luca-della-vedova Mar 8, 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
18 changes: 9 additions & 9 deletions rmf_building_sim_ignition_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,21 +23,21 @@ find_package(rclcpp REQUIRED)

find_package(ignition-cmake2 REQUIRED)

ign_find_package(ignition-gazebo5 REQUIRED)
set(IGN_GAZEBO_VER 5)
ign_find_package(ignition-gazebo6 REQUIRED)
set(IGN_GAZEBO_VER 6)
ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register)
set(IGN_PLUGIN_VER 1)
ign_find_package(ignition-common4 REQUIRED)
set(IGN_COMMON_VER 4)
ign_find_package(ignition-math6 REQUIRED)
set(IGN_MATH_VER 6)
ign_find_package(ignition-gui5 REQUIRED)
set(IGN_GUI_VER 5)
ign_find_package(ignition-msgs7 REQUIRED)
set(IGN_MSGS_VER 7)
ign_find_package(ignition-transport10 REQUIRED)
set(IGN_TRANSPORT_VER 10)
ign_find_package(sdformat11 REQUIRED)
ign_find_package(ignition-gui6 REQUIRED)
set(IGN_GUI_VER 6)
ign_find_package(ignition-msgs8 REQUIRED)
set(IGN_MSGS_VER 8)
ign_find_package(ignition-transport11 REQUIRED)
set(IGN_TRANSPORT_VER 11)
ign_find_package(sdformat12 REQUIRED)

find_package(rmf_building_sim_common REQUIRED)
find_package (Qt5
Expand Down
2 changes: 1 addition & 1 deletion rmf_building_sim_ignition_plugins/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
<depend>libqt5-qml</depend>
<depend>libqt5-quick</depend>
<depend>menge_vendor</depend>
<depend>ignition-edifice</depend>
<depend>ignition-fortress</depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
157 changes: 50 additions & 107 deletions rmf_building_sim_ignition_plugins/src/crowd_simulator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@

#include <ignition/math/Pose3.hh>

#include <ignition/gazebo/Util.hh>
#include <ignition/gazebo/components/Name.hh>
#include <ignition/gazebo/components/Model.hh>
#include <ignition/gazebo/components/Actor.hh>
Expand All @@ -30,16 +31,18 @@

#include "crowd_simulator.hpp"

using namespace ignition::gazebo;

namespace crowd_simulation_ign {

//=================================================
void CrowdSimulatorPlugin::Configure(
const ignition::gazebo::Entity& entity,
const Entity& entity,
const std::shared_ptr<const sdf::Element>& sdf,
ignition::gazebo::EntityComponentManager& ecm,
ignition::gazebo::EventManager& /*event_mgr*/)
EntityComponentManager& ecm,
EventManager& /*event_mgr*/)
{
_world = std::make_shared<ignition::gazebo::Model>(entity);
_world = std::make_shared<Model>(entity);
RCLCPP_INFO(_crowd_sim_interface->logger(),
"Initializing world plugin with name: %s",
_world->Name(ecm).c_str());
Expand Down Expand Up @@ -78,8 +81,8 @@ void CrowdSimulatorPlugin::Configure(

//=================================================
void CrowdSimulatorPlugin::PreUpdate(
const ignition::gazebo::UpdateInfo& info,
ignition::gazebo::EntityComponentManager& ecm)
const UpdateInfo& info,
EntityComponentManager& ecm)
{
// check if crowd sim is enabled
if (!_crowd_sim_interface->enabled())
Expand Down Expand Up @@ -136,7 +139,7 @@ bool CrowdSimulatorPlugin::_spawn_agents_in_world()

//==========================================================
void CrowdSimulatorPlugin::_init_spawned_agents(
ignition::gazebo::EntityComponentManager& ecm)
EntityComponentManager& ecm)
{
// check all the models are in the world
std::unordered_map<std::string, size_t> objects_name;
Expand All @@ -150,11 +153,11 @@ void CrowdSimulatorPlugin::_init_spawned_agents(
objects_name.insert({obj->model_name, id});
}
// for external agent
ecm.Each<ignition::gazebo::components::Model,
ignition::gazebo::components::Name>(
[&](const ignition::gazebo::Entity& entity,
const ignition::gazebo::components::Model*,
const ignition::gazebo::components::Name* name) -> bool
ecm.Each<components::Model,
components::Name>(
[&](const Entity& entity,
const components::Model*,
const components::Name* name) -> bool
{
auto it_objects_name = objects_name.find(name->Data());
if (it_objects_name != objects_name.end())
Expand All @@ -176,11 +179,11 @@ void CrowdSimulatorPlugin::_init_spawned_agents(
}
);
// for internal agent
ecm.Each<ignition::gazebo::components::Actor,
ignition::gazebo::components::Name>(
[&](const ignition::gazebo::Entity& entity,
const ignition::gazebo::components::Actor*,
const ignition::gazebo::components::Name* name) -> bool
ecm.Each<components::Actor,
components::Name>(
[&](const Entity& entity,
const components::Actor*,
const components::Name* name) -> bool
{
auto it_objects_name = objects_name.find(name->Data());
if (it_objects_name != objects_name.end())
Expand Down Expand Up @@ -264,8 +267,8 @@ bool CrowdSimulatorPlugin::_create_entity(
//==================================================
void CrowdSimulatorPlugin::_config_spawned_agents(
const crowd_simulator::CrowdSimInterface::ObjectPtr obj_ptr,
const ignition::gazebo::Entity& entity,
ignition::gazebo::EntityComponentManager& ecm) const
const Entity& entity,
EntityComponentManager& ecm) const
{
assert(obj_ptr);
auto agent_ptr = obj_ptr->agent_ptr;
Expand All @@ -278,43 +281,16 @@ void CrowdSimulatorPlugin::_config_spawned_agents(
0, 0, 0
);

// get pose component for entity
auto pose_comp = ecm.Component<ignition::gazebo::components::Pose>(entity);
if (nullptr == pose_comp)
{
// use the initial_pose for actor type
ignition::math::Pose3d initial_pose =
model_type->pose.convert_to_ign_math_pose_3d<ignition::math::Pose3d>();
ecm.CreateComponent(entity,
ignition::gazebo::components::Pose(initial_pose));
}
else
{
// original pose in the world
*pose_comp =
ignition::gazebo::components::Pose(ignition::math::Pose3d(0, 0, 0, 0, 0,
0));
}

// initialize agent animationName
std::string animation_name = model_type->animation;
assert(!animation_name.empty());

auto animation_name_comp =
ecm.Component<ignition::gazebo::components::AnimationName>(entity);
if (nullptr == animation_name_comp)
{
ecm.CreateComponent(entity,
ignition::gazebo::components::AnimationName(animation_name));
}
else
{
*animation_name_comp = ignition::gazebo::components::AnimationName(
animation_name);
}
enableComponent<components::AnimationName>(ecm, entity);
ecm.Component<components::AnimationName>(entity)->Data() = animation_name;

// check idle animation name
auto actor_comp =
ecm.Component<ignition::gazebo::components::Actor>(entity);
ecm.Component<components::Actor>(entity);
for (auto idle_anim : _crowd_sim_interface->get_switch_anim_name())
{
if (actor_comp->Data().AnimationNameExists(idle_anim))
Expand All @@ -327,29 +303,18 @@ void CrowdSimulatorPlugin::_config_spawned_agents(
// mark as one-time-change
ecm.SetChanged(
entity,
ignition::gazebo::components::AnimationName::typeId,
ignition::gazebo::ComponentState::OneTimeChange);
components::AnimationName::typeId,
ComponentState::OneTimeChange);
// initialize agent animationTime
auto anim_time_comp =
ecm.Component<ignition::gazebo::components::AnimationTime>(entity);
if (nullptr == anim_time_comp)
{
ecm.CreateComponent(entity, ignition::gazebo::components::AnimationTime());
}
enableComponent<components::AnimationTime>(ecm, entity);
// having a trajectory pose prevents the actor from moving with the sdf script
auto traj_pose_comp =
ecm.Component<ignition::gazebo::components::TrajectoryPose>(entity);
if (nullptr == traj_pose_comp)
{
ecm.CreateComponent(entity,
ignition::gazebo::components::TrajectoryPose(actor_pose));
}
enableComponent<components::TrajectoryPose>(ecm, entity);
}

//============================================================================
void CrowdSimulatorPlugin::_update_all_objects(
double delta_sim_time,
ignition::gazebo::EntityComponentManager& ecm) const
EntityComponentManager& ecm) const
{
auto objects_count = _crowd_sim_interface->get_num_objects();
for (size_t id = 0; id < objects_count; id++)
Expand All @@ -369,7 +334,7 @@ void CrowdSimulatorPlugin::_update_all_objects(
if (obj_ptr->is_external)
{
auto model_pose =
ecm.Component<ignition::gazebo::components::Pose>(entity)->Data();
ecm.Component<components::Pose>(entity)->Data();
_crowd_sim_interface->update_external_agent(obj_ptr->agent_ptr,
model_pose);
continue;
Expand All @@ -383,8 +348,8 @@ void CrowdSimulatorPlugin::_update_all_objects(
void CrowdSimulatorPlugin::_update_internal_object(
double delta_sim_time,
const crowd_simulator::CrowdSimInterface::ObjectPtr obj_ptr,
const ignition::gazebo::Entity& entity,
ignition::gazebo::EntityComponentManager& ecm) const
const Entity& entity,
EntityComponentManager& ecm) const
{
double animation_speed = _crowd_sim_interface->_model_type_db_ptr->get(
obj_ptr->type_name)->animation_speed;
Expand All @@ -398,32 +363,11 @@ void CrowdSimulatorPlugin::_update_internal_object(

// get components to be updated
auto traj_pose_comp =
ecm.Component<ignition::gazebo::components::TrajectoryPose>(entity);
if (nullptr == traj_pose_comp)
{
RCLCPP_ERROR(_crowd_sim_interface->logger(),
"Model [ %s ] has no TrajectoryPose component.",
obj_ptr->model_name.c_str());
exit(EXIT_FAILURE);
}
ecm.Component<components::TrajectoryPose>(entity);
auto anim_name_comp =
ecm.Component<ignition::gazebo::components::AnimationName>(entity);
if (nullptr == anim_name_comp)
{
RCLCPP_ERROR(_crowd_sim_interface->logger(),
"Model [ %s ] has no AnimationName component.",
obj_ptr->model_name.c_str());
exit(EXIT_FAILURE);
}
ecm.Component<components::AnimationName>(entity);
auto anim_time_comp =
ecm.Component<ignition::gazebo::components::AnimationTime>(entity);
if (nullptr == anim_name_comp)
{
RCLCPP_ERROR(_crowd_sim_interface->logger(),
"Model [ %s ] has no AnimationTime component.",
obj_ptr->model_name.c_str());
exit(EXIT_FAILURE);
}
ecm.Component<components::AnimationTime>(entity);
ignition::math::Pose3d current_pose = traj_pose_comp->Data();
auto distance_traveled_vector = agent_pose.Pos() - current_pose.Pos();
// might need future work on 3D case
Expand Down Expand Up @@ -457,28 +401,27 @@ void CrowdSimulatorPlugin::_update_internal_object(
}

ecm.SetChanged(entity,
ignition::gazebo::components::AnimationName::typeId,
ignition::gazebo::ComponentState::PeriodicChange);
components::AnimationName::typeId,
ComponentState::PeriodicChange);
obj_ptr->current_state = next_state;

// set trajectory
traj_pose_comp->Data() = agent_pose;
ecm.SetChanged(entity,
ignition::gazebo::components::TrajectoryPose::typeId,
ignition::gazebo::ComponentState::PeriodicChange);
components::TrajectoryPose::typeId,
ComponentState::PeriodicChange);
ecm.SetChanged(entity,
ignition::gazebo::components::AnimationTime::typeId,
ignition::gazebo::ComponentState::PeriodicChange);
components::AnimationTime::typeId,
ComponentState::PeriodicChange);
}

} //namespace crowd_simulation_ign

IGNITION_ADD_PLUGIN(
crowd_simulation_ign::CrowdSimulatorPlugin,
ignition::gazebo::System,
crowd_simulation_ign::CrowdSimulatorPlugin::ISystemConfigure,
crowd_simulation_ign::CrowdSimulatorPlugin::ISystemPreUpdate)
CrowdSimulatorPlugin,
System,
CrowdSimulatorPlugin::ISystemConfigure,
CrowdSimulatorPlugin::ISystemPreUpdate)

// TODO would prefer namespace
IGNITION_ADD_PLUGIN_ALIAS(crowd_simulation_ign::CrowdSimulatorPlugin,
IGNITION_ADD_PLUGIN_ALIAS(CrowdSimulatorPlugin,
"crowd_simulation")

} //namespace crowd_simulation_ign
Loading