From 803298f33c25b5ab45ce99947fc613d0d8b83595 Mon Sep 17 00:00:00 2001 From: Guillaume Date: Sun, 20 Sep 2020 23:24:55 +0200 Subject: [PATCH 01/10] add frame_id and child_frame_id attribute support for DiffDrive Signed-off-by: Guillaume Signed-off-by: Guillaume Doisy --- src/systems/diff_drive/DiffDrive.cc | 31 +++++++++++++++++++++++++---- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 9c9140c55d..60cb54b6ca 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 sdf_frame_id; + + /// \brief frame_id from sdf. + public: std::string sdf_child_frame_id; }; ////////////////////////////////////////////////// @@ -265,6 +271,12 @@ void DiffDrive::Configure(const Entity &_entity, this->dataPtr->odomPub = this->dataPtr->node.Advertise( odomTopic); + if (_sdf->HasElement("frame_id")) + this->dataPtr->sdf_frame_id=_sdf->Get("frame_id"); + + if (_sdf->HasElement("child_frame_id")) + this->dataPtr->sdf_child_frame_id=_sdf->Get("child_frame_id"); + ignmsg << "DiffDrive subscribing to twist messages on [" << topic << "]" << std::endl; } @@ -427,16 +439,27 @@ 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->sdf_frame_id.empty()){ + frame->add_value(this->model.Name(_ecm) + "/odom"); + }else{ + frame->add_value(this->sdf_frame_id); + } std::optional linkName = this->canonicalLink.Name(_ecm); - if (linkName) - { + if (this->sdf_child_frame_id.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->sdf_child_frame_id); } + // Publish the message this->odomPub.Publish(msg); } From 00dda2fc92787b53c97900ff2fcc03d99a182c53 Mon Sep 17 00:00:00 2001 From: Guillaume Date: Tue, 6 Oct 2020 22:40:21 +0200 Subject: [PATCH 02/10] Style Signed-off-by: Guillaume Doisy --- src/systems/diff_drive/DiffDrive.cc | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 60cb54b6ca..27df10763b 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -138,10 +138,10 @@ class ignition::gazebo::systems::DiffDrivePrivate public: std::mutex mutex; /// \brief frame_id from sdf. - public: std::string sdf_frame_id; + public: std::string sdfFrameId; - /// \brief frame_id from sdf. - public: std::string sdf_child_frame_id; + /// \brief child_frame_id from sdf. + public: std::string sdfChildFrameId; }; ////////////////////////////////////////////////// @@ -272,10 +272,10 @@ void DiffDrive::Configure(const Entity &_entity, odomTopic); if (_sdf->HasElement("frame_id")) - this->dataPtr->sdf_frame_id=_sdf->Get("frame_id"); + this->dataPtr->sdfFrameId=_sdf->Get("frame_id"); if (_sdf->HasElement("child_frame_id")) - this->dataPtr->sdf_child_frame_id=_sdf->Get("child_frame_id"); + this->dataPtr->sdfChildFrameId=_sdf->Get("child_frame_id"); ignmsg << "DiffDrive subscribing to twist messages on [" << topic << "]" << std::endl; @@ -439,14 +439,15 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, // Set the frame id. auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - if (this->sdf_frame_id.empty()){ + if (this->sdfFrameId.empty()){ frame->add_value(this->model.Name(_ecm) + "/odom"); }else{ - frame->add_value(this->sdf_frame_id); + frame->add_value(this->sdfFrameId); } std::optional linkName = this->canonicalLink.Name(_ecm); - if (this->sdf_child_frame_id.empty()){ + if (this->sdfChildFrameId.empty()) + { if (linkName) { auto childFrame = msg.mutable_header()->add_data(); @@ -456,7 +457,7 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, }else { auto childFrame = msg.mutable_header()->add_data(); childFrame->set_key("child_frame_id"); - childFrame->add_value(this->sdf_child_frame_id); + childFrame->add_value(this->sdfChildFrameId); } From ce7bdbf9f7819c73cd63252feb1e87e60fd98346 Mon Sep 17 00:00:00 2001 From: Guillaume Date: Tue, 24 Nov 2020 01:10:48 +0100 Subject: [PATCH 03/10] added OdomFrameId and OdomCustomFrameId tests Signed-off-by: Guillaume Doisy --- test/integration/diff_drive_system.cc | 176 +++++++++++++++ test/worlds/diff_drive_custom_frame_id.sdf | 245 +++++++++++++++++++++ 2 files changed, 421 insertions(+) create mode 100644 test/worlds/diff_drive_custom_frame_id.sdf diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 1a1d2888ff..578f426b63 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -310,6 +310,182 @@ 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); + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, id); + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, poses.size()); + + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + + // Publish command and check that vehicle moved + double period{1.0}; + double lastMsgTime{1.0}; + std::vector odomPoses; + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + ASSERT_TRUE(_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"); + + odomPoses.push_back(msgs::Convert(_msg.pose())); + }; + + transport::Node node; + auto pub = node.Advertise("/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, 3000, false); + + // Poses for 4s + EXPECT_EQ(4000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + for (; odomPoses.size() < 3 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); +} + +///////////////////////////////////////////////// +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); + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, id); + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, poses.size()); + + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + + // Publish command and check that vehicle moved + double period{1.0}; + double lastMsgTime{1.0}; + std::vector odomPoses; + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + ASSERT_TRUE(_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"); + + odomPoses.push_back(msgs::Convert(_msg.pose())); + }; + + transport::Node node; + auto pub = node.Advertise("/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, 3000, false); + + // Poses for 4s + EXPECT_EQ(4000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + for (; odomPoses.size() < 3 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); +} + // 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 @@ + + + + + + 0.001 + 1.0 + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + left_wheel_joint + right_wheel_joint + 1.25 + 0.3 + 1 + 0.5 + odom + base_footprint + + + + + + + + + + + From e6a723ad96dd73a2beb1a256a7054f0d24ab2e50 Mon Sep 17 00:00:00 2001 From: Guillaume Date: Thu, 3 Dec 2020 01:10:15 +0100 Subject: [PATCH 04/10] wip Signed-off-by: Guillaume Doisy --- test/integration/diff_drive_system.cc | 97 ++++----------------------- 1 file changed, 14 insertions(+), 83 deletions(-) diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 578f426b63..8de9191dd2 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -324,41 +324,9 @@ TEST_P(DiffDriveTest, OdomFrameId) server.SetUpdatePeriod(0ns); - // Create a system that records the vehicle poses - test::Relay testSystem; - - std::vector poses; - testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) - { - auto id = _ecm.EntityByComponents( - components::Model(), - components::Name("vehicle")); - EXPECT_NE(kNullEntity, id); - - auto poseComp = _ecm.Component(id); - ASSERT_NE(nullptr, poseComp); - - poses.push_back(poseComp->Data()); - }); - server.AddSystem(testSystem.systemPtr); - - // Run server and check that vehicle didn't move - server.Run(true, 1000, false); - - EXPECT_EQ(1000u, poses.size()); - - for (const auto &pose : poses) - { - EXPECT_EQ(poses[0], pose); - } - - // Publish command and check that vehicle moved - double period{1.0}; - double lastMsgTime{1.0}; - std::vector odomPoses; + unsigned int odomPosesCount=0; std::function odomCb = - [&](const msgs::Odometry &_msg) + [&odomPosesCount](const msgs::Odometry &_msg) { ASSERT_TRUE(_msg.has_header()); ASSERT_TRUE(_msg.header().has_stamp()); @@ -371,7 +339,7 @@ TEST_P(DiffDriveTest, OdomFrameId) 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"); - odomPoses.push_back(msgs::Convert(_msg.pose())); + odomPosesCount++; }; transport::Node node; @@ -384,14 +352,14 @@ TEST_P(DiffDriveTest, OdomFrameId) pub.Publish(msg); - server.Run(true, 3000, false); + server.Run(true, 10, false); - // Poses for 4s - EXPECT_EQ(4000u, poses.size()); + // Poses for 10 iterations + EXPECT_EQ(10u, odomPosesCount); int sleep = 0; int maxSleep = 30; - for (; odomPoses.size() < 3 && sleep < maxSleep; ++sleep) + for (; odomPosesCount < 3 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } @@ -412,41 +380,10 @@ TEST_P(DiffDriveTest, OdomCustomFrameId) server.SetUpdatePeriod(0ns); - // Create a system that records the vehicle poses - test::Relay testSystem; - std::vector poses; - testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) - { - auto id = _ecm.EntityByComponents( - components::Model(), - components::Name("vehicle")); - EXPECT_NE(kNullEntity, id); - - auto poseComp = _ecm.Component(id); - ASSERT_NE(nullptr, poseComp); - - poses.push_back(poseComp->Data()); - }); - server.AddSystem(testSystem.systemPtr); - - // Run server and check that vehicle didn't move - server.Run(true, 1000, false); - - EXPECT_EQ(1000u, poses.size()); - - for (const auto &pose : poses) - { - EXPECT_EQ(poses[0], pose); - } - - // Publish command and check that vehicle moved - double period{1.0}; - double lastMsgTime{1.0}; - std::vector odomPoses; + unsigned int odomPosesCount=0; std::function odomCb = - [&](const msgs::Odometry &_msg) + [&odomPosesCount](const msgs::Odometry &_msg) { ASSERT_TRUE(_msg.has_header()); ASSERT_TRUE(_msg.header().has_stamp()); @@ -459,27 +396,21 @@ TEST_P(DiffDriveTest, OdomCustomFrameId) 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"); - odomPoses.push_back(msgs::Convert(_msg.pose())); + odomPosesCount++; }; transport::Node node; auto pub = node.Advertise("/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, 10, false); - server.Run(true, 3000, false); - - // Poses for 4s - EXPECT_EQ(4000u, poses.size()); + // Poses for 10 iterations + EXPECT_EQ(10u, odomPosesCount); int sleep = 0; int maxSleep = 30; - for (; odomPoses.size() < 3 && sleep < maxSleep; ++sleep) + for (; odomPosesCount < 3 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } From 918572d893801600b0aec498e15104778b9fe95e Mon Sep 17 00:00:00 2001 From: Guillaume Date: Thu, 3 Dec 2020 17:43:35 +0100 Subject: [PATCH 05/10] simplify tests Signed-off-by: Guillaume Doisy --- test/integration/diff_drive_system.cc | 26 ++++---------------------- 1 file changed, 4 insertions(+), 22 deletions(-) diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 8de9191dd2..fe05539a3d 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -352,18 +352,9 @@ TEST_P(DiffDriveTest, OdomFrameId) pub.Publish(msg); - server.Run(true, 10, false); + server.Run(true, 100, false); - // Poses for 10 iterations - EXPECT_EQ(10u, odomPosesCount); - - int sleep = 0; - int maxSleep = 30; - for (; odomPosesCount < 3 && sleep < maxSleep; ++sleep) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - EXPECT_NE(maxSleep, sleep); + EXPECT_EQ(5u, odomPosesCount); } ///////////////////////////////////////////////// @@ -403,18 +394,9 @@ TEST_P(DiffDriveTest, OdomCustomFrameId) auto pub = node.Advertise("/model/vehicle/cmd_vel"); node.Subscribe("/model/vehicle/odometry", odomCb); - server.Run(true, 10, false); + server.Run(true, 100, false); - // Poses for 10 iterations - EXPECT_EQ(10u, odomPosesCount); - - int sleep = 0; - int maxSleep = 30; - for (; odomPosesCount < 3 && sleep < maxSleep; ++sleep) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - EXPECT_NE(maxSleep, sleep); + EXPECT_EQ(5u, odomPosesCount); } // Run multiple times From 75892f5944dc9aa532351b7798796542a971fb96 Mon Sep 17 00:00:00 2001 From: Guillaume Date: Thu, 3 Dec 2020 17:59:22 +0100 Subject: [PATCH 06/10] code style Signed-off-by: Guillaume Doisy --- test/integration/diff_drive_system.cc | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index fe05539a3d..bea40e19cf 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -324,20 +324,22 @@ TEST_P(DiffDriveTest, OdomFrameId) server.SetUpdatePeriod(0ns); - unsigned int odomPosesCount=0; + unsigned int odomPosesCount = 0; std::function odomCb = [&odomPosesCount](const msgs::Odometry &_msg) { ASSERT_TRUE(_msg.has_header()); ASSERT_TRUE(_msg.header().has_stamp()); - ASSERT_TRUE(_msg.header().data_size() > 1); + 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(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"); + EXPECT_STREQ( + _msg.header().data(1).value().Get(0).c_str(), "vehicle/chassis"); odomPosesCount++; }; @@ -371,21 +373,21 @@ TEST_P(DiffDriveTest, OdomCustomFrameId) server.SetUpdatePeriod(0ns); - - unsigned int odomPosesCount=0; + unsigned int odomPosesCount = 0; std::function odomCb = [&odomPosesCount](const msgs::Odometry &_msg) { ASSERT_TRUE(_msg.has_header()); ASSERT_TRUE(_msg.header().has_stamp()); - ASSERT_TRUE(_msg.header().data_size() > 1); + 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"); + EXPECT_STREQ( + _msg.header().data(1).value().Get(0).c_str(), "base_footprint"); odomPosesCount++; }; From d8d2d7a2a4856dbeed24a3f5f0b7fe3b05313a12 Mon Sep 17 00:00:00 2001 From: Guillaume Date: Thu, 3 Dec 2020 18:11:51 +0100 Subject: [PATCH 07/10] style src Signed-off-by: Guillaume Doisy --- src/systems/diff_drive/DiffDrive.cc | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 27df10763b..2bfd5ec8bd 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -272,10 +272,10 @@ void DiffDrive::Configure(const Entity &_entity, odomTopic); if (_sdf->HasElement("frame_id")) - this->dataPtr->sdfFrameId=_sdf->Get("frame_id"); + this->dataPtr->sdfFrameId = _sdf->Get("frame_id"); if (_sdf->HasElement("child_frame_id")) - this->dataPtr->sdfChildFrameId=_sdf->Get("child_frame_id"); + this->dataPtr->sdfChildFrameId = _sdf->Get("child_frame_id"); ignmsg << "DiffDrive subscribing to twist messages on [" << topic << "]" << std::endl; @@ -439,9 +439,10 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, // Set the frame id. auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - if (this->sdfFrameId.empty()){ + if (this->sdfFrameId.empty()) { frame->add_value(this->model.Name(_ecm) + "/odom"); - }else{ + } + else { frame->add_value(this->sdfFrameId); } @@ -454,7 +455,8 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, childFrame->set_key("child_frame_id"); childFrame->add_value(this->model.Name(_ecm) + "/" + *linkName); } - }else { + } + else { auto childFrame = msg.mutable_header()->add_data(); childFrame->set_key("child_frame_id"); childFrame->add_value(this->sdfChildFrameId); From 34c403a43f5ec931d6389173ad5a36cbc27b67c1 Mon Sep 17 00:00:00 2001 From: Guillaume Date: Thu, 3 Dec 2020 18:29:15 +0100 Subject: [PATCH 08/10] more style Signed-off-by: Guillaume Doisy --- src/systems/diff_drive/DiffDrive.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 2bfd5ec8bd..18d048478a 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -442,7 +442,8 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, if (this->sdfFrameId.empty()) { frame->add_value(this->model.Name(_ecm) + "/odom"); } - else { + else + { frame->add_value(this->sdfFrameId); } @@ -456,7 +457,8 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, childFrame->add_value(this->model.Name(_ecm) + "/" + *linkName); } } - else { + else + { auto childFrame = msg.mutable_header()->add_data(); childFrame->set_key("child_frame_id"); childFrame->add_value(this->sdfChildFrameId); From 6ead52594a54c2f86c6b003c93a67032d0e23e8e Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 8 Dec 2020 13:21:22 +0100 Subject: [PATCH 09/10] final style Signed-off-by: Guillaume Doisy --- src/systems/diff_drive/DiffDrive.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 18d048478a..496b6cd50f 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -439,7 +439,8 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, // Set the frame id. auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); - if (this->sdfFrameId.empty()) { + if (this->sdfFrameId.empty()) + { frame->add_value(this->model.Name(_ecm) + "/odom"); } else From 6a1d72eec7bea9476e4a78dbee0a95c4c8e01b83 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 15 Dec 2020 23:02:41 +0100 Subject: [PATCH 10/10] add test check timeout Signed-off-by: Guillaume Doisy --- test/integration/diff_drive_system.cc | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index bea40e19cf..2d4da67079 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -356,6 +356,14 @@ TEST_P(DiffDriveTest, OdomFrameId) 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); } @@ -398,6 +406,14 @@ TEST_P(DiffDriveTest, OdomCustomFrameId) 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); }