From 7b4b258e869a6d5bbb7ae0e2de609df534faff79 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 22 Feb 2021 14:19:19 -0800 Subject: [PATCH 1/3] =?UTF-8?q?=F0=9F=91=A9=E2=80=8D=F0=9F=8C=BE=20Relax?= =?UTF-8?q?=20performance=20test=20(#640)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel Co-authored-by: Michael Carroll --- test/performance/each.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/performance/each.cc b/test/performance/each.cc index 272d04bff9..d67f343ac1 100644 --- a/test/performance/each.cc +++ b/test/performance/each.cc @@ -54,10 +54,10 @@ TEST(EntityComponentManagerPerfrormance, Each) // Initial allocation of resources can throw off calculations. warmstart(); - for (int matchingEntityCount = 1; matchingEntityCount < maxEntityCount; + for (int matchingEntityCount = 10; matchingEntityCount < maxEntityCount; matchingEntityCount += step) { - for (int nonmatchingEntityCount = 1; + for (int nonmatchingEntityCount = 10; nonmatchingEntityCount < maxEntityCount; nonmatchingEntityCount += step) { From 60d9eb87464f4d410d1927101f437f952a9bf934 Mon Sep 17 00:00:00 2001 From: "G.Doisy" Date: Tue, 23 Feb 2021 01:17:38 +0100 Subject: [PATCH 2/3] Add TF/Pose_V publisher in DiffDrive (#548) Signed-off-by: Guillaume Doisy --- src/systems/diff_drive/DiffDrive.cc | 20 +- src/systems/diff_drive/DiffDrive.hh | 16 ++ test/integration/diff_drive_system.cc | 184 ++++++++++++++++ test/worlds/diff_drive_custom_tf_topic.sdf | 244 +++++++++++++++++++++ 4 files changed, 463 insertions(+), 1 deletion(-) create mode 100644 test/worlds/diff_drive_custom_tf_topic.sdf diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 496b6cd50f..e8a49f4356 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -119,6 +119,9 @@ class ignition::gazebo::systems::DiffDrivePrivate /// \brief Diff drive odometry message publisher. public: transport::Node::Publisher odomPub; + /// \brief Diff drive tf message publisher. + public: transport::Node::Publisher tfPub; + /// \brief Linear velocity limiter. public: std::unique_ptr limiterLin; @@ -271,6 +274,13 @@ void DiffDrive::Configure(const Entity &_entity, this->dataPtr->odomPub = this->dataPtr->node.Advertise( odomTopic); + std::string tfTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/tf"}; + if (_sdf->HasElement("tf_topic")) + tfTopic = _sdf->Get("tf_topic"); + this->dataPtr->tfPub = this->dataPtr->node.Advertise( + tfTopic); + if (_sdf->HasElement("frame_id")) this->dataPtr->sdfFrameId = _sdf->Get("frame_id"); @@ -465,9 +475,17 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, childFrame->add_value(this->sdfChildFrameId); } + // Construct the Pose_V/tf message and publish it. + msgs::Pose_V tfMsg; + ignition::msgs::Pose *tfMsgPose = nullptr; + tfMsgPose = tfMsg.add_pose(); + tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); + tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); + tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); - // Publish the message + // Publish the messages this->odomPub.Publish(msg); + this->tfPub.Publish(tfMsg); } ////////////////////////////////////////////////// diff --git a/src/systems/diff_drive/DiffDrive.hh b/src/systems/diff_drive/DiffDrive.hh index adedf7b002..d2905a23ca 100644 --- a/src/systems/diff_drive/DiffDrive.hh +++ b/src/systems/diff_drive/DiffDrive.hh @@ -61,6 +61,22 @@ namespace systems /// ``: Custom topic on which this system will publish odometry /// messages. This element if optional, and the default value is /// `/model/{name_of_model}/odometry`. + /// + /// ``: Custom topic on which this system will publish the + /// transform from `frame_id` to `child_frame_id`. This element if optional, + /// and the default value is `/model/{name_of_model}/tf`. + /// + /// ``: Custom `frame_id` field that this system will use as the + /// origin of the odometry transform in both the `` + /// `ignition.msgs.Pose_V` message and the `` + /// `ignition.msgs.Odometry` message. This element if optional, and the + /// default value is `{name_of_model}/odom`. + /// + /// ``: Custom `child_frame_id` that this system will use as + /// the target of the odometry trasnform in both the `` + /// `ignition.msgs.Pose_V` message and the `` + /// `ignition.msgs.Odometry` message. This element if optional, + /// and the default value is `{name_of_model}/{name_of_link}`. class IGNITION_GAZEBO_VISIBLE DiffDrive : public System, public ISystemConfigure, diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 1d55d2b576..d1c6f86d32 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -404,6 +404,190 @@ TEST_P(DiffDriveTest, OdomCustomFrameId) 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, 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, Pose_VFrameId) +{ + // 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 pose_VCb = + [&odomPosesCount](const msgs::Pose_V &_msg) + { + ASSERT_TRUE(_msg.pose(0).has_header()); + ASSERT_TRUE(_msg.pose(0).header().has_stamp()); + + ASSERT_GT(_msg.pose(0).header().data_size(), 1); + + EXPECT_STREQ(_msg.pose(0).header().data(0).key().c_str(), + "frame_id"); + EXPECT_STREQ(_msg.pose(0).header().data(0).value().Get(0).c_str(), + "vehicle/odom"); + + EXPECT_STREQ(_msg.pose(0).header().data(1).key().c_str(), + "child_frame_id"); + EXPECT_STREQ(_msg.pose(0).header().data(1).value().Get(0).c_str(), + "vehicle/chassis"); + + odomPosesCount++; + }; + + transport::Node node; + auto pub = node.Advertise("/model/vehicle/cmd_vel"); + node.Subscribe("/model/vehicle/tf", pose_VCb); + + 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, Pose_VCustomFrameId) +{ + // 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 Pose_VCb = + [&odomPosesCount](const msgs::Pose_V &_msg) + { + ASSERT_TRUE(_msg.pose(0).has_header()); + ASSERT_TRUE(_msg.pose(0).header().has_stamp()); + + ASSERT_GT(_msg.pose(0).header().data_size(), 1); + + EXPECT_STREQ(_msg.pose(0).header().data(0).key().c_str(), + "frame_id"); + EXPECT_STREQ(_msg.pose(0).header().data(0).value().Get(0).c_str(), + "odom"); + + EXPECT_STREQ(_msg.pose(0).header().data(1).key().c_str(), + "child_frame_id"); + EXPECT_STREQ(_msg.pose(0).header().data(1).value().Get(0).c_str(), + "base_footprint"); + + odomPosesCount++; + }; + + transport::Node node; + auto pub = node.Advertise("/model/vehicle/cmd_vel"); + node.Subscribe("/model/vehicle/tf", Pose_VCb); + + 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, Pose_VCustomTfTopic) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/diff_drive_custom_tf_topic.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + unsigned int odomPosesCount = 0; + std::function pose_VCb = + [&odomPosesCount](const msgs::Pose_V &_msg) + { + ASSERT_TRUE(_msg.pose(0).has_header()); + ASSERT_TRUE(_msg.pose(0).header().has_stamp()); + + ASSERT_GT(_msg.pose(0).header().data_size(), 1); + + EXPECT_STREQ(_msg.pose(0).header().data(0).key().c_str(), "frame_id"); + EXPECT_STREQ( + _msg.pose(0).header().data(0).value().Get(0).c_str(), + "vehicle/odom"); + + EXPECT_STREQ( + _msg.pose(0).header().data(1).key().c_str(), "child_frame_id"); + EXPECT_STREQ( + _msg.pose(0).header().data(1).value().Get(0).c_str(), + "vehicle/chassis"); + + odomPosesCount++; + }; + + transport::Node node; + auto pub = node.Advertise("/model/vehicle/cmd_vel"); + node.Subscribe("/tf_foo", pose_VCb); + + 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; diff --git a/test/worlds/diff_drive_custom_tf_topic.sdf b/test/worlds/diff_drive_custom_tf_topic.sdf new file mode 100644 index 0000000000..131207f3ef --- /dev/null +++ b/test/worlds/diff_drive_custom_tf_topic.sdf @@ -0,0 +1,244 @@ + + + + + + 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 + tf_foo + + + + + + + + + + + From 4a4951e838a98f3c94352887583213f417971243 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 23 Feb 2021 15:23:10 -0800 Subject: [PATCH 3/3] Fix flaky SceneBoradcaster test (#641) Signed-off-by: Louise Poubel Co-authored-by: Michael Carroll --- test/integration/scene_broadcaster_system.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index b5ae9c0f9c..bee470ef2e 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -438,6 +438,7 @@ TEST_P(SceneBroadcasterTest, State) while (!received && sleep++ < maxSleep) IGN_SLEEP_MS(100); EXPECT_TRUE(received); + EXPECT_TRUE(node.Unsubscribe("/world/default/state")); // test async state request received = false;