Skip to content

Commit

Permalink
Merge branch 'ign-gazebo3' into chapulina/3/joint_pos_error
Browse files Browse the repository at this point in the history
  • Loading branch information
chapulina authored Oct 14, 2021
2 parents f2e536a + e9be884 commit d4bab48
Showing 1 changed file with 95 additions and 55 deletions.
150 changes: 95 additions & 55 deletions src/systems/imu/Imu.cc
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,6 @@

#include <ignition/common/Profiler.hh>

#include <ignition/transport/Node.hh>

#include <ignition/sensors/SensorFactory.hh>
#include <ignition/sensors/ImuSensor.hh>

Expand Down Expand Up @@ -58,8 +56,14 @@ class ignition::gazebo::systems::ImuPrivate
/// \brief Ign-sensors sensor factory for creating sensors
public: sensors::SensorFactory sensorFactory;

/// \brief Keep track of world ID, which is equivalent to the scene's
/// root visual.
/// Defaults to zero, which is considered invalid by Ignition Gazebo.
public: Entity worldEntity = kNullEntity;

/// True if the rendering component is initialized
public: bool initialized = false;

/// \brief Create IMU sensor
/// \param[in] _ecm Mutable reference to ECM.
public: void CreateImuEntities(EntityComponentManager &_ecm);
Expand All @@ -68,6 +72,17 @@ class ignition::gazebo::systems::ImuPrivate
/// \param[in] _ecm Immutable reference to ECM.
public: void Update(const EntityComponentManager &_ecm);

/// \brief Create sensor
/// \param[in] _ecm Mutable reference to ECM.
/// \param[in] _entity Entity of the IMU
/// \param[in] _imu IMU component.
/// \param[in] _parent Parent entity component.
public: void addIMU(
EntityComponentManager &_ecm,
const Entity _entity,
const components::Imu *_imu,
const components::ParentEntity *_parent);

/// \brief Remove IMU sensors if their entities have been removed from
/// simulation.
/// \param[in] _ecm Immutable reference to ECM.
Expand Down Expand Up @@ -121,6 +136,63 @@ void Imu::PostUpdate(const UpdateInfo &_info,
this->dataPtr->RemoveImuEntities(_ecm);
}

//////////////////////////////////////////////////
void ImuPrivate::addIMU(
EntityComponentManager &_ecm,
const Entity _entity,
const components::Imu *_imu,
const components::ParentEntity *_parent)
{
// Get the world acceleration (defined in world frame)
auto gravity = _ecm.Component<components::Gravity>(worldEntity);
if (nullptr == gravity)
{
ignerr << "World missing gravity." << std::endl;
return;
}

// create sensor
std::string sensorScopedName =
removeParentScope(scopedName(_entity, _ecm, "::", false), "::");
sdf::Sensor data = _imu->Data();
data.SetName(sensorScopedName);
// check topic
if (data.Topic().empty())
{
std::string topic = scopedName(_entity, _ecm) + "/imu";
data.SetTopic(topic);
}
std::unique_ptr<sensors::ImuSensor> sensor =
this->sensorFactory.CreateSensor<
sensors::ImuSensor>(data);
if (nullptr == sensor)
{
ignerr << "Failed to create sensor [" << sensorScopedName << "]"
<< std::endl;
return;
}

// set sensor parent
std::string parentName = _ecm.Component<components::Name>(
_parent->Data())->Data();
sensor->SetParent(parentName);

// set gravity - assume it remains fixed
sensor->SetGravity(gravity->Data());

// Get initial pose of sensor and set the reference z pos
// The WorldPose component was just created and so it's empty
// We'll compute the world pose manually here
math::Pose3d p = worldPose(_entity, _ecm);
sensor->SetOrientationReference(p.Rot());

// Set topic
_ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));

this->entitySensorMap.insert(
std::make_pair(_entity, std::move(sensor)));
}

//////////////////////////////////////////////////
void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
{
Expand All @@ -134,63 +206,31 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
return;
}

// Get the world acceleration (defined in world frame)
auto gravity = _ecm.Component<components::Gravity>(worldEntity);
if (nullptr == gravity)
if (!this->initialized)
{
ignerr << "World missing gravity." << std::endl;
return;
}

// Create IMUs
_ecm.EachNew<components::Imu, components::ParentEntity>(
[&](const Entity &_entity,
const components::Imu *_imu,
const components::ParentEntity *_parent)->bool
{
// create sensor
std::string sensorScopedName =
removeParentScope(scopedName(_entity, _ecm, "::", false), "::");
sdf::Sensor data = _imu->Data();
data.SetName(sensorScopedName);
// check topic
if (data.Topic().empty())
// Create IMUs
_ecm.Each<components::Imu, components::ParentEntity>(
[&](const Entity &_entity,
const components::Imu *_imu,
const components::ParentEntity *_parent)->bool
{
std::string topic = scopedName(_entity, _ecm) + "/imu";
data.SetTopic(topic);
}
std::unique_ptr<sensors::ImuSensor> sensor =
this->sensorFactory.CreateSensor<
sensors::ImuSensor>(data);
if (nullptr == sensor)
addIMU(_ecm, _entity, _imu, _parent);
return true;
});
this->initialized = true;
}
else
{
// Create IMUs
_ecm.EachNew<components::Imu, components::ParentEntity>(
[&](const Entity &_entity,
const components::Imu *_imu,
const components::ParentEntity *_parent)->bool
{
ignerr << "Failed to create sensor [" << sensorScopedName << "]"
<< std::endl;
addIMU(_ecm, _entity, _imu, _parent);
return true;
}

// set sensor parent
std::string parentName = _ecm.Component<components::Name>(
_parent->Data())->Data();
sensor->SetParent(parentName);

// set gravity - assume it remains fixed
sensor->SetGravity(gravity->Data());

// Get initial pose of sensor and set the reference z pos
// The WorldPose component was just created and so it's empty
// We'll compute the world pose manually here
math::Pose3d p = worldPose(_entity, _ecm);
sensor->SetOrientationReference(p.Rot());

// Set topic
_ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));

this->entitySensorMap.insert(
std::make_pair(_entity, std::move(sensor)));

return true;
});
});
}
}

//////////////////////////////////////////////////
Expand Down

0 comments on commit d4bab48

Please sign in to comment.