From 9028b6897cd34aea1292f8c3b732af0a63ee0245 Mon Sep 17 00:00:00 2001 From: Andrew Ealovega Date: Mon, 4 Jul 2022 17:18:08 -0400 Subject: [PATCH 1/8] Port DiffDrive tf generation to AckermannSteer. Signed-off-by: Andrew Ealovega --- .../ackermann_steering/AckermannSteering.cc | 18 ++++++++++++++++++ .../ackermann_steering/AckermannSteering.hh | 16 ++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 530c3e3289..1320b99b1f 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -143,6 +143,9 @@ class ignition::gazebo::systems::AckermannSteeringPrivate /// \brief Ackermann steering odometry message publisher. public: transport::Node::Publisher odomPub; + /// \brief Ackermann tf message publisher. + public: transport::Node::Publisher tfPub; + /// \brief Odometry X value public: double odomX{0.0}; @@ -343,6 +346,13 @@ void AckermannSteering::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"); @@ -667,8 +677,16 @@ void AckermannSteeringPrivate::UpdateOdometry( childFrame->add_value(this->sdfChildFrameId); } + // Construct the Pose_V/tf message and publish it. + msgs::Pose_V tfMsg; + ignition::msgs::Pose *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 this->odomPub.Publish(msg); + this->tfPub.Publish(tfMsg); } ////////////////////////////////////////////////// diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index b501bcfde8..fbe6a049b1 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -90,6 +90,22 @@ namespace systems /// 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 is 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 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}/{name_of_link}`. + /// /// A robot with rear drive and front steering would have one each /// of left_joint, right_joint, left_steering_joint and /// right_steering_joint From 4c1461a4f96f4cc83dc557647f479068b2d118ff Mon Sep 17 00:00:00 2001 From: Andrew Ealovega Date: Mon, 4 Jul 2022 17:19:47 -0400 Subject: [PATCH 2/8] Fix doc formatting issues. Signed-off-by: Andrew Ealovega --- src/systems/ackermann_steering/AckermannSteering.hh | 12 ++++++------ src/systems/diff_drive/DiffDrive.hh | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index fbe6a049b1..893a5220d8 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -75,12 +75,12 @@ namespace systems /// ``: Odometry publication frequency. This /// element is optional, and the default value is 50Hz. /// - /// '': Minimum velocity [m/s], usually <= 0. - /// '': Maximum velocity [m/s], usually >= 0. - /// '': Minimum acceleration [m/s^2], usually <= 0. - /// '': Maximum acceleration [m/s^2], usually >= 0. - /// '': jerk [m/s^3], usually <= 0. - /// '': jerk [m/s^3], usually >= 0. + /// ``: Minimum velocity [m/s], usually <= 0. + /// ``: Maximum velocity [m/s], usually >= 0. + /// ``: Minimum acceleration [m/s^2], usually <= 0. + /// ``: Maximum acceleration [m/s^2], usually >= 0. + /// ``: jerk [m/s^3], usually <= 0. + /// ``: jerk [m/s^3], usually >= 0. /// /// ``: Custom topic that this system will subscribe to in order to /// receive command velocity messages. This element if optional, and the diff --git a/src/systems/diff_drive/DiffDrive.hh b/src/systems/diff_drive/DiffDrive.hh index 6da5719320..52f481f206 100644 --- a/src/systems/diff_drive/DiffDrive.hh +++ b/src/systems/diff_drive/DiffDrive.hh @@ -73,7 +73,7 @@ namespace systems /// 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 `` + /// the target 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}/{name_of_link}`. From 3eaf708ef338729f04f05de4868d62a5491a1ebf Mon Sep 17 00:00:00 2001 From: Andrew Ealovega Date: Thu, 7 Jul 2022 18:10:55 -0400 Subject: [PATCH 3/8] Apply suggestions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Andrew Ealovega --- .gitignore | 3 +++ .../ackermann_steering/AckermannSteering.cc | 19 +++++++++++++++---- .../ackermann_steering/AckermannSteering.hh | 4 ++-- 3 files changed, 20 insertions(+), 6 deletions(-) diff --git a/.gitignore b/.gitignore index 51c7dd858d..40d7c0f5e3 100644 --- a/.gitignore +++ b/.gitignore @@ -12,3 +12,6 @@ build_* .ycm_extra_conf.py *.orig + +# clangd index +.cache diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 1320b99b1f..c0fe761697 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -346,10 +346,21 @@ void AckermannSteering::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"); + std::vector tfTopics; + if (_sdf->HasElement("tf_topic")) + { + tfTopics.push_back(_sdf->Get("tf_topic")); + } + tfTopics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + + "/tf"); + auto tfTopic = validTopic(tfTopics); + if (tfTopic.empty()) + { + ignerr << "AckermannSteering plugin invalid tf topic name " + << "Failed to initialize." << std::endl; + return; + } + this->dataPtr->tfPub = this->dataPtr->node.Advertise( tfTopic); diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index 893a5220d8..11aacd9159 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -92,7 +92,7 @@ namespace systems /// /// ``: Custom topic on which this system will publish the /// transform from `frame_id` to `child_frame_id`. This element is optional, - /// and the default value is `/model/{name_of_model}/tf`. + /// 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 `` @@ -104,7 +104,7 @@ namespace systems /// the target 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}/{name_of_link}`. + /// and the default value is `{name_of_model}/{name_of_link}`. /// /// A robot with rear drive and front steering would have one each /// of left_joint, right_joint, left_steering_joint and From afaf2d87dc138954405dfa989723347156bd420c Mon Sep 17 00:00:00 2001 From: Andrew Ealovega Date: Fri, 8 Jul 2022 00:15:35 -0400 Subject: [PATCH 4/8] Remove singular whitespace Signed-off-by: Andrew Ealovega --- src/systems/ackermann_steering/AckermannSteering.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index c0fe761697..d2dc5f39e7 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -347,7 +347,7 @@ void AckermannSteering::Configure(const Entity &_entity, odomTopic); std::vector tfTopics; - if (_sdf->HasElement("tf_topic")) + if (_sdf->HasElement("tf_topic")) { tfTopics.push_back(_sdf->Get("tf_topic")); } From e045f0a3be001d8d98541f1a7ae420656b4204c3 Mon Sep 17 00:00:00 2001 From: Andrew Ealovega Date: Fri, 8 Jul 2022 16:55:02 -0400 Subject: [PATCH 5/8] Add Tf test Signed-off-by: Andrew Ealovega --- test/integration/ackermann_steering_system.cc | 120 ++++++++++++++++++ 1 file changed, 120 insertions(+) diff --git a/test/integration/ackermann_steering_system.cc b/test/integration/ackermann_steering_system.cc index 3718d44069..bbfdf3bb6f 100644 --- a/test/integration/ackermann_steering_system.cc +++ b/test/integration/ackermann_steering_system.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -309,6 +310,125 @@ TEST_P(AckermannSteeringTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(SkidPublishCmd)) EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); } +///////////////////////////////////////////////// +TEST_P(AckermannSteeringTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TfPublishes)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "/test/worlds/ackermann_steering_slow_odom.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()); + + double msgTime = + static_cast(_msg.header().stamp().sec()) + + static_cast(_msg.header().stamp().nsec()) * 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); + lastMsgTime = msgTime; + + odomPoses.push_back(msgs::Convert(_msg.pose())); + }; + + // Capture Tf data to compare to odom + double periodTf{1.0}; + double lastMsgTimeTf{1.0}; + std::vector tfPoses; + std::function tfCb = + [&](const msgs::Pose_V &_msg) + { + ASSERT_TRUE(_msg.pose()[0].has_header()); + + double msgTime = + static_cast(_msg.pose()[0].header().stamp().sec()) + + static_cast(_msg.pose()[0].header().stamp().nsec()) * 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTimeTf + periodTf); + lastMsgTimeTf = msgTime; + + // Use position pose to match odom (index 0) + tfPoses.push_back(msgs::Convert(_msg.pose()[0])); + }; + + transport::Node node; + auto pub = node.Advertise("/model/vehicle/cmd_vel"); + node.Subscribe("/model/vehicle/odometry", odomCb); + node.Subscribe("/model/vehicle/tf", tfCb); + + 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); + + // There should have been the same amount of odom and tf + ASSERT_FALSE(odomPoses.empty()); + ASSERT_FALSE(tfPoses.empty()); + ASSERT_EQ(odomPoses.size(), tfPoses.size()); + + // Ensure all data is equal between the two topics + for (unsigned long i = 0; i < odomPoses.size(); i++) + { + ASSERT_EQ(odomPoses[i], tfPoses[i]); + } +} + ///////////////////////////////////////////////// TEST_P(AckermannSteeringTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(OdomFrameId)) { From 549104bdb5596843cb538ac950435818827fbd72 Mon Sep 17 00:00:00 2001 From: Andrew Ealovega Date: Sun, 17 Jul 2022 23:57:16 -0400 Subject: [PATCH 6/8] Adjust test path joining. Signed-off-by: Andrew Ealovega --- test/integration/ackermann_steering_system.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/test/integration/ackermann_steering_system.cc b/test/integration/ackermann_steering_system.cc index bbfdf3bb6f..cf8d9c867d 100644 --- a/test/integration/ackermann_steering_system.cc +++ b/test/integration/ackermann_steering_system.cc @@ -15,6 +15,7 @@ * */ +#include #include #include #include @@ -316,7 +317,7 @@ TEST_P(AckermannSteeringTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TfPublishes)) // Start server ServerConfig serverConfig; serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), - "/test/worlds/ackermann_steering_slow_odom.sdf")); + "test", "worlds", "ackermann_steering_slow_odom.sdf")); Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -372,7 +373,7 @@ TEST_P(AckermannSteeringTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TfPublishes)) odomPoses.push_back(msgs::Convert(_msg.pose())); }; - + // Capture Tf data to compare to odom double periodTf{1.0}; double lastMsgTimeTf{1.0}; @@ -423,7 +424,7 @@ TEST_P(AckermannSteeringTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TfPublishes)) ASSERT_EQ(odomPoses.size(), tfPoses.size()); // Ensure all data is equal between the two topics - for (unsigned long i = 0; i < odomPoses.size(); i++) + for (uint64_t i = 0; i < odomPoses.size(); i++) { ASSERT_EQ(odomPoses[i], tfPoses[i]); } From d363ea01a7f3dbb5380565b15ebe3ade57c56b48 Mon Sep 17 00:00:00 2001 From: Andrew Ealovega Date: Mon, 18 Jul 2022 17:41:28 -0400 Subject: [PATCH 7/8] Use .Get() to support libprotobuf-dev 3.0.0 on bionic. Signed-off-by: Andrew Ealovega --- test/integration/ackermann_steering_system.cc | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/test/integration/ackermann_steering_system.cc b/test/integration/ackermann_steering_system.cc index cf8d9c867d..9749e7e637 100644 --- a/test/integration/ackermann_steering_system.cc +++ b/test/integration/ackermann_steering_system.cc @@ -381,17 +381,18 @@ TEST_P(AckermannSteeringTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TfPublishes)) std::function tfCb = [&](const msgs::Pose_V &_msg) { - ASSERT_TRUE(_msg.pose()[0].has_header()); + ASSERT_TRUE(_msg.pose().Get(0).has_header()); double msgTime = - static_cast(_msg.pose()[0].header().stamp().sec()) + - static_cast(_msg.pose()[0].header().stamp().nsec()) * 1e-9; + static_cast(_msg.pose().Get(0).header().stamp().sec()) + + static_cast(_msg.pose().Get(0).header().stamp().nsec()) * + 1e-9; EXPECT_DOUBLE_EQ(msgTime, lastMsgTimeTf + periodTf); lastMsgTimeTf = msgTime; // Use position pose to match odom (index 0) - tfPoses.push_back(msgs::Convert(_msg.pose()[0])); + tfPoses.push_back(msgs::Convert(_msg.pose().Get(0))); }; transport::Node node; From 82ab5e07db9c2264050d7a1312731b51757f8cfb Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 22 Jul 2022 16:53:53 -0700 Subject: [PATCH 8/8] codecheck Signed-off-by: Louise Poubel --- test/integration/ackermann_steering_system.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/integration/ackermann_steering_system.cc b/test/integration/ackermann_steering_system.cc index 9749e7e637..ec28d2e668 100644 --- a/test/integration/ackermann_steering_system.cc +++ b/test/integration/ackermann_steering_system.cc @@ -17,10 +17,11 @@ #include #include +#include + #include #include #include -#include #include #include