Skip to content

Commit

Permalink
Add xyz and rpy offset to published odometry pose (#1341)
Browse files Browse the repository at this point in the history 
* Added xyz and rpy offset to published pose

Signed-off-by: Aditya <[email protected]>
  • Loading branch information
@adityapande-1995
adityapande-1995 authored Mar 11, 2022
1 parent f5bb284 commit c24a4e9
Show file tree
Hide file tree
Showing 4 changed files with 314 additions and 1 deletion.
19 changes: 18 additions & 1 deletion src/systems/odometry_publisher/OdometryPublisher.cc
View file
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
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
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
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>

0 comments on commit c24a4e9

Please sign in to comment.