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

Parse new param for enabling / disabling IMU orientation output #899

Merged
merged 10 commits into from
Sep 30, 2021
Merged

Parse new param for enabling / disabling IMU orientation output #899

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
96f50bc
parse imu orientation param
iche033 Jul 1, 2021
d3e1343
Merge branch 'ign-gazebo4' into imu_orientation
iche033 Jul 2, 2021
09c8b35
Merge branch 'ign-gazebo4' into imu_orientation
chapulina Jul 14, 2021
48e171f
use enable_orientation sdf elem
iche033 Aug 3, 2021
95d4854
use joinPaths
iche033 Aug 3, 2021
90009be
Merge branch 'ign-gazebo4' into imu_orientation
iche033 Aug 3, 2021
8b3c314
Merge branch 'ign-gazebo4' into imu_orientation
iche033 Sep 15, 2021
fc22dad
add test world file
iche033 Sep 15, 2021
59a2760
Merge branch 'ign-gazebo4' into imu_orientation
iche033 Sep 15, 2021
9c745a4
Merge branch 'ign-gazebo4' into imu_orientation
Sep 27, 2021
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
7 changes: 7 additions & 0 deletions src/systems/imu/Imu.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -186,6 +186,13 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
// Set topic
_ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));

// Set whether orientation is enabled
if (data.ImuSensor())
{
sensor->SetOrientationEnabled(
data.ImuSensor()->OrientationEnabled());
}

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

Expand Down
40 changes: 40 additions & 0 deletions test/integration/imu_system.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -206,3 +206,43 @@ TEST_F(ImuTest, ModelFalling)
EXPECT_EQ(imuMsgs.back().entity_name(), scopedName);
mutex.unlock();
}

/////////////////////////////////////////////////
// The test checks to make sure orientation is not published if it is deabled
TEST_F(ImuTest, OrientationDisabled)
{
imuMsgs.clear();

// Start server
ServerConfig serverConfig;
const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
"test", "worlds", "imu_no_orientation.sdf");
serverConfig.SetSdfFile(sdfFile);

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

const std::string sensorName = "imu_sensor";

auto topic =
"world/imu_sensor/model/imu_model/link/link/sensor/imu_sensor/imu";
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

common::joinPaths

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

hmm this string if for topic name so I think it's not necessary to use joinPaths as that's intended for file paths?


// subscribe to imu topic
transport::Node node;
node.Subscribe(topic, &imuCb);

// step world and verify imu's orientation is not published
// Run server
size_t iters200 = 200u;
server.Run(true, iters200, false);

// Check we received messages
EXPECT_GT(imuMsgs.size(), 0u);
mutex.lock();
for (const auto &msg : imuMsgs)
{
EXPECT_FALSE(msg.has_orientation());
}
mutex.unlock();
}
83 changes: 83 additions & 0 deletions test/worlds/imu_no_orientation.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="imu_sensor">
<gravity>0 0 -5</gravity>
<physics name="1ms" type="ode">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="ignition-gazebo-physics-system"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="ignition-gazebo-imu-system"
name="ignition::gazebo::systems::Imu">
</plugin>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="imu_model">
<pose>4 0 3.0 0 0.0 3.14</pose>
<link name="link">
<pose>0.05 0.05 0.05 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000166667</ixx>
<iyy>0.000166667</iyy>
<izz>0.000166667</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name="imu_sensor" type="imu">
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>true</visualize>
<imu>
<enable_orientation>false</enable_orientation>
</imu>
</sensor>
</link>
</model>

</world>
</sdf>