diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc
index 9c9140c55d..496b6cd50f 100644
--- a/src/systems/diff_drive/DiffDrive.cc
+++ b/src/systems/diff_drive/DiffDrive.cc
@@ -136,6 +136,12 @@ class ignition::gazebo::systems::DiffDrivePrivate
 
   /// \brief A mutex to protect the target velocity command.
   public: std::mutex mutex;
+
+  /// \brief frame_id from sdf.
+  public: std::string sdfFrameId;
+
+  /// \brief child_frame_id from sdf.
+  public: std::string sdfChildFrameId;
 };
 
 //////////////////////////////////////////////////
@@ -265,6 +271,12 @@ void DiffDrive::Configure(const Entity &_entity,
   this->dataPtr->odomPub = this->dataPtr->node.Advertise<msgs::Odometry>(
       odomTopic);
 
+  if (_sdf->HasElement("frame_id"))
+    this->dataPtr->sdfFrameId = _sdf->Get<std::string>("frame_id");
+
+  if (_sdf->HasElement("child_frame_id"))
+    this->dataPtr->sdfChildFrameId = _sdf->Get<std::string>("child_frame_id");
+
   ignmsg << "DiffDrive subscribing to twist messages on [" << topic << "]"
          << std::endl;
 }
@@ -427,16 +439,33 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info,
   // Set the frame id.
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->model.Name(_ecm) + "/odom");
+  if (this->sdfFrameId.empty())
+  {
+    frame->add_value(this->model.Name(_ecm) + "/odom");
+  }
+  else
+  {
+    frame->add_value(this->sdfFrameId);
+  }
 
   std::optional<std::string> linkName = this->canonicalLink.Name(_ecm);
-  if (linkName)
+  if (this->sdfChildFrameId.empty())
+  {
+    if (linkName)
+    {
+      auto childFrame = msg.mutable_header()->add_data();
+      childFrame->set_key("child_frame_id");
+      childFrame->add_value(this->model.Name(_ecm) + "/" + *linkName);
+    }
+  }
+  else
   {
     auto childFrame = msg.mutable_header()->add_data();
     childFrame->set_key("child_frame_id");
-    childFrame->add_value(this->model.Name(_ecm) + "/" + *linkName);
+    childFrame->add_value(this->sdfChildFrameId);
   }
 
+
   // Publish the message
   this->odomPub.Publish(msg);
 }
diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc
index 1a1d2888ff..2d4da67079 100644
--- a/test/integration/diff_drive_system.cc
+++ b/test/integration/diff_drive_system.cc
@@ -310,6 +310,113 @@ TEST_P(DiffDriveTest, SkidPublishCmd)
   EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z());
 }
 
+/////////////////////////////////////////////////
+TEST_P(DiffDriveTest, OdomFrameId)
+{
+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+      "/test/worlds/diff_drive.sdf");
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  server.SetUpdatePeriod(0ns);
+
+  unsigned int odomPosesCount = 0;
+  std::function<void(const msgs::Odometry &)> odomCb =
+    [&odomPosesCount](const msgs::Odometry &_msg)
+    {
+      ASSERT_TRUE(_msg.has_header());
+      ASSERT_TRUE(_msg.header().has_stamp());
+
+      ASSERT_GT(_msg.header().data_size(), 1);
+
+      EXPECT_STREQ(_msg.header().data(0).key().c_str(), "frame_id");
+      EXPECT_STREQ(
+            _msg.header().data(0).value().Get(0).c_str(), "vehicle/odom");
+
+      EXPECT_STREQ(_msg.header().data(1).key().c_str(), "child_frame_id");
+      EXPECT_STREQ(
+            _msg.header().data(1).value().Get(0).c_str(), "vehicle/chassis");
+
+      odomPosesCount++;
+    };
+
+  transport::Node node;
+  auto pub = node.Advertise<msgs::Twist>("/model/vehicle/cmd_vel");
+  node.Subscribe("/model/vehicle/odometry", odomCb);
+
+  msgs::Twist msg;
+  msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0));
+  msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2));
+
+  pub.Publish(msg);
+
+  server.Run(true, 100, false);
+
+  int sleep = 0;
+  int maxSleep = 30;
+  for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+  ASSERT_NE(maxSleep, sleep);
+
+  EXPECT_EQ(5u, odomPosesCount);
+}
+
+/////////////////////////////////////////////////
+TEST_P(DiffDriveTest, OdomCustomFrameId)
+{
+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+      "/test/worlds/diff_drive_custom_frame_id.sdf");
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  server.SetUpdatePeriod(0ns);
+
+  unsigned int odomPosesCount = 0;
+  std::function<void(const msgs::Odometry &)> odomCb =
+    [&odomPosesCount](const msgs::Odometry &_msg)
+    {
+      ASSERT_TRUE(_msg.has_header());
+      ASSERT_TRUE(_msg.header().has_stamp());
+
+      ASSERT_GT(_msg.header().data_size(), 1);
+
+      EXPECT_STREQ(_msg.header().data(0).key().c_str(), "frame_id");
+      EXPECT_STREQ(_msg.header().data(0).value().Get(0).c_str(), "odom");
+
+      EXPECT_STREQ(_msg.header().data(1).key().c_str(), "child_frame_id");
+      EXPECT_STREQ(
+            _msg.header().data(1).value().Get(0).c_str(), "base_footprint");
+
+      odomPosesCount++;
+    };
+
+  transport::Node node;
+  auto pub = node.Advertise<msgs::Twist>("/model/vehicle/cmd_vel");
+  node.Subscribe("/model/vehicle/odometry", odomCb);
+
+  server.Run(true, 100, false);
+
+  int sleep = 0;
+  int maxSleep = 30;
+  for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+  ASSERT_NE(maxSleep, sleep);
+
+  EXPECT_EQ(5u, odomPosesCount);
+}
+
 // Run multiple times
 INSTANTIATE_TEST_CASE_P(ServerRepeat, DiffDriveTest,
     ::testing::Range(1, 2));
diff --git a/test/worlds/diff_drive_custom_frame_id.sdf b/test/worlds/diff_drive_custom_frame_id.sdf
new file mode 100644
index 0000000000..073bfc41b8
--- /dev/null
+++ b/test/worlds/diff_drive_custom_frame_id.sdf
@@ -0,0 +1,245 @@
+<?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>1.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>0 0 0.325 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-diff-drive-system"
+        name="ignition::gazebo::systems::DiffDrive">
+        <left_joint>left_wheel_joint</left_joint>
+        <right_joint>right_wheel_joint</right_joint>
+        <wheel_separation>1.25</wheel_separation>
+        <wheel_radius>0.3</wheel_radius>
+        <max_acceleration>1</max_acceleration>
+        <max_velocity>0.5</max_velocity>
+        <frame_id>odom</frame_id>
+        <child_frame_id>base_footprint</child_frame_id>
+        <!-- no odom_publisher_frequency defaults to 50 Hz -->
+      </plugin>
+
+      <plugin
+        filename="ignition-gazebo-joint-state-publisher-system"
+        name="ignition::gazebo::systems::JointStatePublisher">
+      </plugin>
+
+    </model>
+
+  </world>
+</sdf>
+