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

Add xyz and rpy offset to published odometry pose #1341

Merged
merged 7 commits into from
Mar 11, 2022
Merged

Add xyz and rpy offset to published odometry pose #1341

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
62e29ea
Added xyz and rpy offset to published pose
adityapande-1995 Feb 16, 2022
f03559e
Added test case
adityapande-1995 Feb 16, 2022
efabe27
codecheck fixes
adityapande-1995 Feb 23, 2022
6315256
Merge branch 'ign-gazebo6' into aditya/odom_offset
adityapande-1995 Feb 24, 2022
7a9cea5
offset is wrt body frame, updated doc in header file
adityapande-1995 Mar 2, 2022
d314d42
Updated example, initialized offset
adityapande-1995 Mar 8, 2022
1c565de
Merge branch 'ign-gazebo6' into aditya/odom_offset
adityapande-1995 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
19 changes: 18 additions & 1 deletion src/systems/odometry_publisher/OdometryPublisher.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,9 @@ class ignition::gazebo::systems::OdometryPublisherPrivate

/// \brief Current timestamp.
public: math::clock::time_point lastUpdateTime;

/// \brief Allow specifying constant xyz and rpy offsets
public: ignition::math::Pose3d offset = {0, 0, 0, 0, 0, 0};
};

//////////////////////////////////////////////////
Expand Down Expand Up @@ -142,6 +145,19 @@ void OdometryPublisher::Configure(const Entity &_entity,
this->dataPtr->odomFrame = _sdf->Get<std::string>("odom_frame");
}

if (_sdf->HasElement("xyz_offset"))
{
this->dataPtr->offset.Pos() = _sdf->Get<ignition::math::Vector3d>(
"xyz_offset");
}

if (_sdf->HasElement("rpy_offset"))
{
this->dataPtr->offset.Rot() =
ignition::math::Quaterniond(_sdf->Get<ignition::math::Vector3d>(
"rpy_offset"));
}

this->dataPtr->robotBaseFrame = this->dataPtr->model.Name(_ecm)
+ "/" + "base_footprint";
if (!_sdf->HasElement("robot_base_frame"))
Expand Down Expand Up @@ -257,7 +273,8 @@ void OdometryPublisherPrivate::UpdateOdometry(
return;

// Get and set robotBaseFrame to odom transformation.
const math::Pose3d pose = worldPose(this->model.Entity(), _ecm);
const math::Pose3d rawPose = worldPose(this->model.Entity(), _ecm);
math::Pose3d pose = rawPose * this->offset;
msg.mutable_pose()->mutable_position()->set_x(pose.Pos().X());
msg.mutable_pose()->mutable_position()->set_y(pose.Pos().Y());
msgs::Set(msg.mutable_pose()->mutable_orientation(), pose.Rot());
Expand Down
8 changes: 8 additions & 0 deletions src/systems/odometry_publisher/OdometryPublisher.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,14 @@ namespace systems
/// `<dimensions>`: Number of dimensions to represent odometry. Only 2 and 3
/// dimensional spaces are supported. This element is optional, and the
/// default value is 2.
///
/// `<xyz_offset>`: Position offset relative to the body fixed frame, the
/// default value is 0 0 0. This offset will be added to the odometry
/// message.
///
/// `<rpy_offset>`: Rotation offset relative to the body fixed frame, the
/// default value is 0 0 0. This offset will be added to the odometry
// message.
class OdometryPublisher
: public System,
public ISystemConfigure,
Expand Down
56 changes: 56 additions & 0 deletions test/integration/odometry_publisher.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -394,6 +394,52 @@ class OdometryPublisherTest

EXPECT_EQ(5u, odomPosesCount);
}

/// \param[in] _sdfFile SDF file to load.
/// \param[in] _odomTopic Odometry topic.
protected: void TestOffsetTags(const std::string &_sdfFile,
const std::string &_odomTopic)
{
// Start server
ServerConfig serverConfig;
serverConfig.SetSdfFile(_sdfFile);

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

std::vector<math::Pose3d> odomPoses;
// Create function to store data from odometry messages
std::function<void(const msgs::Odometry &)> odomCb =
[&](const msgs::Odometry &_msg)
{
odomPoses.push_back(msgs::Convert(_msg.pose()));
};
transport::Node node;
node.Subscribe(_odomTopic, odomCb);

// Run server while the model moves with the velocities set earlier
server.Run(true, 3000, false);

int sleep = 0;
int maxSleep = 30;
for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep)
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
ASSERT_NE(maxSleep, sleep);

// Run for 3s and check the pose in the last message
ASSERT_FALSE(odomPoses.empty());
auto lastPose = odomPoses[odomPoses.size() - 1];
EXPECT_NEAR(lastPose.Pos().X(), 11, 1e-2);
EXPECT_NEAR(lastPose.Pos().Y(), -11, 1e-2);
EXPECT_NEAR(lastPose.Pos().Z(), 0, 1e-2);

EXPECT_NEAR(lastPose.Rot().Roll(), 1.57, 1e-2);
EXPECT_NEAR(lastPose.Rot().Pitch(), 0, 1e-2);
EXPECT_NEAR(lastPose.Rot().Yaw(), 0, 1e-2);
}
};

/////////////////////////////////////////////////
Expand Down Expand Up @@ -446,6 +492,16 @@ TEST_P(OdometryPublisherTest,
"baseCustom");
}

/////////////////////////////////////////////////
TEST_P(OdometryPublisherTest,
IGN_UTILS_TEST_DISABLED_ON_WIN32(OffsetTagTest))
{
TestOffsetTags(
std::string(PROJECT_SOURCE_PATH) +
"/test/worlds/odometry_offset.sdf",
"/model/vehicle/odometry");
}

// Run multiple times
INSTANTIATE_TEST_SUITE_P(ServerRepeat, OdometryPublisherTest,
::testing::Range(1, 2));
232 changes: 232 additions & 0 deletions test/worlds/odometry_offset.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,232 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="diff_drive">

<physics name="1ms" type="ode">
<max_step_size>0.001</max_step_size>
<real_time_factor>0</real_time_factor>
</physics>
<plugin
filename="ignition-gazebo-physics-system"
name="ignition::gazebo::systems::Physics">
</plugin>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>1 1 1 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</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='vehicle'>
<pose>10 -10 0 0 0 0</pose>

<link name='chassis'>
<pose>-0.151427 -0 0.175 0 -0 0</pose>
<inertial>
<mass>1.14395</mass>
<inertia>
<ixx>0.126164</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.416519</iyy>
<iyz>0</iyz>
<izz>0.481014</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<box>
<size>2.01142 1 0.568726</size>
</box>
</geometry>
<material>
<ambient>0.5 0.5 1.0 1</ambient>
<diffuse>0.5 0.5 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<box>
<size>2.01142 1 0.568726</size>
</box>
</geometry>
</collision>
</link>

<link name='left_wheel'>
<pose>0.554283 0.625029 -0.025 -1.5707 0 0</pose>
<inertial>
<mass>2</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.3</radius>
</sphere>
</geometry>
<material>
<ambient>0.2 0.2 0.2 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.3</radius>
</sphere>
</geometry>
</collision>
</link>

<link name='right_wheel'>
<pose>0.554282 -0.625029 -0.025 -1.5707 0 0</pose>
<inertial>
<mass>2</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.3</radius>
</sphere>
</geometry>
<material>
<ambient>0.2 0.2 0.2 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.3</radius>
</sphere>
</geometry>
</collision>
</link>

<link name='caster'>
<pose>-0.957138 -0 -0.125 0 -0 0</pose>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
<material>
<ambient>0.2 0.2 0.2 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
</collision>
</link>

<joint name='left_wheel_joint' type='revolute'>
<parent>chassis</parent>
<child>left_wheel</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
</limit>
</axis>
</joint>

<joint name='right_wheel_joint' type='revolute'>
<parent>chassis</parent>
<child>right_wheel</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
</limit>
</axis>
</joint>

<joint name='caster_wheel' type='ball'>
<parent>chassis</parent>
<child>caster</child>
</joint>

<plugin
filename="ignition-gazebo-odometry-publisher-system"
name="ignition::gazebo::systems::OdometryPublisher">
<xyz_offset>1 -1 0</xyz_offset>
<rpy_offset>1.5708 0 0</rpy_offset>
</plugin>
</model>

</world>
</sdf>