From 3d132168f7cd0bb0196b0d2882c23962b18a25d6 Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Tue, 23 Jan 2024 16:25:07 +0100 Subject: [PATCH 01/14] mecanum src added Signed-off-by: Stefano Mutti --- .vscode/c_cpp_properties.json | 21 + .vscode/settings.json | 3 + log/COLCON_IGNORE | 0 log/latest | 1 + log/latest_list | 1 + log/list_2024-01-23_15-12-53/logger_all.log | 22 + src/systems/mecanum_drive/MecanumDrive.cc | 131 +++- test/integration/CMakeLists.txt | 1 + test/integration/mecanum_drive_system.cc | 764 ++++++++++++++++++++ test/worlds/mecanum_drive.sdf | 357 +++++++++ 10 files changed, 1289 insertions(+), 12 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/settings.json create mode 100644 log/COLCON_IGNORE create mode 120000 log/latest create mode 120000 log/latest_list create mode 100644 log/list_2024-01-23_15-12-53/logger_all.log create mode 100644 test/integration/mecanum_drive_system.cc create mode 100644 test/worlds/mecanum_drive.sdf diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000000..c0877c3bb2 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,21 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/rolling/include/**", + "/home/stefanomutti/Libs/gz-sim/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000000..98f2d1e701 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "sonarlint.pathToCompileCommands": "${workspaceFolder}/build/compile_commands.json" +} \ No newline at end of file diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/log/latest b/log/latest new file mode 120000 index 0000000000..17156673ec --- /dev/null +++ b/log/latest @@ -0,0 +1 @@ +latest_list \ No newline at end of file diff --git a/log/latest_list b/log/latest_list new file mode 120000 index 0000000000..59e94cf04f --- /dev/null +++ b/log/latest_list @@ -0,0 +1 @@ +list_2024-01-23_15-12-53 \ No newline at end of file diff --git a/log/list_2024-01-23_15-12-53/logger_all.log b/log/list_2024-01-23_15-12-53/logger_all.log new file mode 100644 index 0000000000..73e5d847e0 --- /dev/null +++ b/log/list_2024-01-23_15-12-53/logger_all.log @@ -0,0 +1,22 @@ +[0.845s] DEBUG:colcon:Command line arguments: ['/usr/local/bin/colcon', 'list', '-p', '--base-paths', '/home/stefanomutti/Libs/gz-sim'] +[0.845s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/stefanomutti/Libs/gz-sim'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('list',)) +[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.881s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/stefanomutti/Libs/gz-sim' +[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ignore', 'ignore_ament_install'] +[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore' +[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore_ament_install' +[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_pkg'] +[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_pkg' +[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_meta'] +[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_meta' +[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ros'] +[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ros' +[0.896s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['cmake', 'python'] +[0.896s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'cmake' +[1.150s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'python' +[1.150s] DEBUG:colcon.colcon_core.package_identification:Package '/home/stefanomutti/Libs/gz-sim' with type 'cmake' and name 'gz-sim8' +[1.150s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index 6d77de4c77..f991126bc7 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include #include @@ -66,12 +66,19 @@ class gz::sim::systems::MecanumDrivePrivate /// \param[in] _msg Velocity message public: void OnCmdVel(const gz::msgs::Twist &_msg); + /// \brief Update odometry and publish an odometry message. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateOdometry(const UpdateInfo &_info, + const EntityComponentManager &_ecm); + /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateVelocity(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm); + public: void UpdateVelocity(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Gazebo communication node. public: transport::Node node; @@ -133,13 +140,13 @@ class gz::sim::systems::MecanumDrivePrivate /// \brief Last sim time odom was published. public: std::chrono::steady_clock::duration lastOdomPubTime{0}; - /// \brief Diff drive odometry. - public: math::DiffDriveOdometry odom; + /// \brief Mecanum drive odometry. + public: math::MecanumDriveOdometry odom; - /// \brief Diff drive odometry message publisher. + /// \brief Mecanum drive odometry message publisher. public: transport::Node::Publisher odomPub; - /// \brief Diff drive tf message publisher. + /// \brief Mecanum drive tf message publisher. public: transport::Node::Publisher tfPub; /// \brief Linear velocity limiter. @@ -280,7 +287,7 @@ void MecanumDrive::Configure(const Entity &_entity, // Setup odometry. this->dataPtr->odom.SetWheelParams(this->dataPtr->wheelSeparation, - this->dataPtr->wheelRadius, this->dataPtr->wheelRadius); + this->dataPtr->wheelRadius, this->dataPtr->wheelRadius, this->dataPtr->wheelRadius); // Subscribe to commands std::vector topics; @@ -324,8 +331,8 @@ void MecanumDrive::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void MecanumDrive::PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) +void MecanumDrive::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { GZ_PROFILE("MecanumDrive::PreUpdate"); @@ -487,6 +494,7 @@ void MecanumDrive::PreUpdate(const gz::sim::UpdateInfo &_info, *vel = components::JointVelocityCmd({this->dataPtr->backRightJointSpeed}); } } + } ////////////////////////////////////////////////// @@ -499,12 +507,111 @@ void MecanumDrive::PostUpdate(const UpdateInfo &_info, return; this->dataPtr->UpdateVelocity(_info, _ecm); + this->dataPtr->UpdateOdometry(_info, _ecm); +} + +////////////////////////////////////////////////// +void MecanumDrivePrivate::UpdateOdometry(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + GZ_PROFILE("MecanumDrive::UpdateOdometry"); + // Initialize, if not already initialized. + if (!this->odom.Initialized()) + { + this->odom.Init(std::chrono::steady_clock::time_point(_info.simTime)); + return; + } + + if (this->frontLeftJoints.empty() || this->frontRightJoints.empty() || this->backLeftJoints.empty() || this->backRightJoints.empty()) + return; + + // Get the first joint positions for each wheel joint. + auto frontLeftPos = _ecm.Component(this->frontLeftJoints[0]); + auto frontRightPos = _ecm.Component(this->frontRightJoints[0]); + auto backLeftPos = _ecm.Component(this->backLeftJoints[0]); + auto backRightPos = _ecm.Component(this->backRightJoints[0]); + + // Abort if the joints were not found or just created. + if (!frontLeftPos || !frontRightPos || !backLeftPos || !backRightPos || + frontLeftPos->Data().empty() || frontRightPos->Data().empty() || backLeftPos->Data().empty() || backRightPos->Data().empty()) + { + return; + } + + this->odom.Update(frontLeftPos->Data()[0], frontRightPos->Data()[0], backLeftPos->Data()[0], backRightPos->Data()[0], + std::chrono::steady_clock::time_point(_info.simTime)); + + // Throttle publishing + auto diff = _info.simTime - this->lastOdomPubTime; + if (diff > std::chrono::steady_clock::duration::zero() && + diff < this->odomPubPeriod) + { + return; + } + this->lastOdomPubTime = _info.simTime; + + // Construct the odometry message and publish it. + msgs::Odometry msg; + msg.mutable_pose()->mutable_position()->set_x(this->odom.X()); + msg.mutable_pose()->mutable_position()->set_y(this->odom.Y()); + + math::Quaterniond orientation(0, 0, *this->odom.Heading()); + msgs::Set(msg.mutable_pose()->mutable_orientation(), orientation); + + msg.mutable_twist()->mutable_linear()->set_x(this->odom.LinearVelocity()); + msg.mutable_twist()->mutable_linear()->set_y(this->odom.LateralVelocity()); + msg.mutable_twist()->mutable_angular()->set_z(*this->odom.AngularVelocity()); + + // Set the time stamp in the header + msg.mutable_header()->mutable_stamp()->CopyFrom( + convert(_info.simTime)); + + // Set the frame id. + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + if (this->sdfFrameId.empty()) + { + frame->add_value(this->model.Name(_ecm) + "/odom"); + } + else + { + frame->add_value(this->sdfFrameId); + } + + std::optional linkName = this->canonicalLink.Name(_ecm); + 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->sdfChildFrameId); + } + + // Construct the Pose_V/tf message and publish it. + msgs::Pose_V tfMsg; + gz::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 messages + this->odomPub.Publish(msg); + this->tfPub.Publish(tfMsg); + } ////////////////////////////////////////////////// void MecanumDrivePrivate::UpdateVelocity( - const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &/*_ecm*/) + const UpdateInfo &_info, + const EntityComponentManager &/*_ecm*/) { GZ_PROFILE("MecanumDrive::UpdateVelocity"); diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 61f350bd73..82b176022c 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -49,6 +49,7 @@ set(tests logical_audio_sensor_plugin.cc magnetometer_system.cc material.cc + mecanum_drive_system.cc mesh_inertia_calculation.cc model.cc model_photo_shoot_default_joints.cc diff --git a/test/integration/mecanum_drive_system.cc b/test/integration/mecanum_drive_system.cc new file mode 100644 index 0000000000..7dbdba4a83 --- /dev/null +++ b/test/integration/mecanum_drive_system.cc @@ -0,0 +1,764 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "test_config.hh" + +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +#define tol 10e-4 + +using namespace gz; +using namespace sim; +using namespace std::chrono_literals; + +/// \brief Test MecanumDrive system +class MecanumDriveTest : public InternalFixture<::testing::TestWithParam> +{ + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + /// \param[in] _odomTopic Odometry topic. + protected: void TestPublishCmd(const std::string &_sdfFile, + const std::string &_cmdVelTopic, + const std::string &_odomTopic) + { + /// \param[in] forward If forward is true, the 'max_acceleration' + // and 'max_velocity' properties are tested, as the movement + // is forward, otherwise 'min_acceleration' and 'min_velocity' + // properties are tested. + auto testCmdVel = [&](bool forward){ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle_blue")); + 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); + } + + // Get odometry messages + double period{1.0 / 50.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())); + }; + + // Publish command and check that vehicle moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + node.Subscribe(_odomTopic, odomCb); + + msgs::Twist msg; + + // Avoid wheel slip by limiting acceleration (1 m/s^2) + // and max velocity (0.5 m/s). + // See and parameters + // in "/test/worlds/diff_drive.sdf". + // See , , and + // parameters in "/test/worlds/diff_drive.sdf". + test::Relay velocityRamp; + const int movementDirection = (forward ? 1 : -1); + double desiredLinVel = movementDirection * 10.5; + double desiredAngVel = 0.2; + velocityRamp.OnPreUpdate( + [&](const UpdateInfo &/*_info*/, + const EntityComponentManager &) + { + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, desiredAngVel)); + pub.Publish(msg); + }); + + server.AddSystem(velocityRamp.systemPtr); + + server.Run(true, 3000, false); + + // Poses for 4s + ASSERT_EQ(4000u, poses.size()); + + // 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); + + // Odometry calculates the pose of a point that is located half way + // between the two wheels, not the origin of the model. For example, + // if the vehicle is commanded to rotate in place, the vehicle will + // rotate about the point half way between the two wheels, thus, + // the odometry position will remain zero. + // However, since the model origin is offset, the model position will + // change. To find the final pose of the model, we have to do the + // following similarity transformation + + // math::Pose3d tOdomModel(0.554283, 0, -0.325, 0, 0, 0); + // auto finalModelFramePose = + // tOdomModel * odomPoses.back() * tOdomModel.Inverse(); + + // Odom for 3s + ASSERT_FALSE(odomPoses.empty()); + EXPECT_EQ(150u, odomPoses.size()); + + auto expectedLowerPosition = + (forward ? poses[0].Pos() : poses[3999].Pos()); + auto expectedGreaterPosition = + (forward ? poses[3999].Pos() : poses[0].Pos()); + + EXPECT_LT(expectedLowerPosition.X(), expectedGreaterPosition.X()); + EXPECT_LT(expectedLowerPosition.Y(), expectedGreaterPosition.Y()); + EXPECT_NEAR(expectedLowerPosition.Z(), expectedGreaterPosition.Z(), tol); + EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); + EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); + EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); + + // The value from odometry will be close, but not exactly the ground truth + // pose of the robot model. This is partially due to throttling the + // odometry publisher, partially due to a frame difference between the + // odom frame and the model frame, and partially due to wheel slip. + EXPECT_NEAR(poses[1020].Pos().X(), odomPoses[0].Pos().X(), 1e-2); + EXPECT_NEAR(poses[1020].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); + // EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2); + // EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2); + + // Verify velocity and acceleration boundaries. + // Moving time. + double t = 3.0; + double d = poses[3999].Pos().Distance(poses[0].Pos()); + double v0 = 0; + double v = d / t; + double a = (v - v0) / t; + + EXPECT_LT(v, 0.5); + EXPECT_LT(a, 1); + EXPECT_GT(v, -0.5); + EXPECT_GT(a, -1); + + }; + + testCmdVel(true /*test forward movement*/); + testCmdVel(false /*test backward movement*/); + } +}; + +///////////////////////////////////////////////// +// See: https://github.com/gazebosim/gz-sim/issues/1175 +// See: https://github.com/gazebosim/gz-sim/issues/630 +TEST_P(MecanumDriveTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PublishCmd)) +{ + TestPublishCmd( + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/mecanum_drive.sdf", + "/model/vehicle_blue/cmd_vel", "/model/vehicle_blue/odometry"); +} + +// ///////////////////////////////////////////////// +// // See: https://github.com/gazebosim/gz-sim/issues/630 +// TEST_P(MecanumDriveTest, +// GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PublishCmdCustomTopics)) +// { +// TestPublishCmd( +// std::string(PROJECT_SOURCE_PATH) + +// "/test/worlds/diff_drive_custom_topics.sdf", +// "/model/foo/cmdvel", "/model/bar/odom"); +// } + +// ///////////////////////////////////////////////// +// TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SkidPublishCmd)) +// { +// // Start server +// ServerConfig serverConfig; +// serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + +// "/test/worlds/diff_drive_skid.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 UpdateInfo &, +// const 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())); +// }; + +// 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(200)); +// } +// EXPECT_NE(maxSleep, sleep); + +// // Odom for 3s +// ASSERT_FALSE(odomPoses.empty()); +// EXPECT_EQ(3u, odomPoses.size()); + +// EXPECT_LT(poses[0].Pos().X(), poses[3999].Pos().X()); +// EXPECT_NEAR(poses[0].Pos().Z(), poses[3999].Pos().Z(), tol); +// EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); +// EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); + +// // Slip works on DART>=6.10, which isn't available on Ubuntu Focal +// #ifndef __linux__ +// EXPECT_LT(poses[0].Pos().Y(), poses[3999].Pos().Y()); +// EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); +// #endif +// } + +// ///////////////////////////////////////////////// +// TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(EnableDisableCmd)) +// { +// // Start server +// ServerConfig serverConfig; +// serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + +// "/test/worlds/diff_drive_skid.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 UpdateInfo &, +// const 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())); +// }; + +// 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.0)); + +// pub.Publish(msg); + +// server.Run(true, 3000, false); + +// // Poses for 4s +// EXPECT_EQ(4000u, poses.size()); + +// // Disable controller +// auto pub_enable = node.Advertise("/model/vehicle/enable"); + +// msgs::Boolean msg_enable; +// msg_enable.set_data(false); + +// pub_enable.Publish(msg_enable); + +// // Run for 2s and expect no movement +// server.Run(true, 2000, false); + +// EXPECT_EQ(6000u, poses.size()); + +// // Re-enable controller +// msg_enable.set_data(true); + +// pub_enable.Publish(msg_enable); + +// pub.Publish(msg); + +// // Run for 2s and expect movement again +// server.Run(true, 2000, false); + +// EXPECT_EQ(8000u, poses.size()); + +// int sleep = 0; +// int maxSleep = 70; +// for (; odomPoses.size() < 7 && sleep < maxSleep; ++sleep) +// { +// std::this_thread::sleep_for(std::chrono::milliseconds(100)); +// } +// EXPECT_NE(maxSleep, sleep); + +// // Odom for 7s +// ASSERT_FALSE(odomPoses.empty()); +// EXPECT_EQ(7u, odomPoses.size()); + +// EXPECT_LT(poses[0].Pos().X(), poses[3999].Pos().X()); + +// // Should no be moving from 5s to 6s (stopped at 3s and time to slow down) +// EXPECT_NEAR(poses[4999].Pos().X(), poses[5999].Pos().X(), tol); + +// // Should be moving from 6s to 8s +// EXPECT_LT(poses[5999].Pos().X(), poses[7999].Pos().X()); +// } + +// ///////////////////////////////////////////////// +// TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(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 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("/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; +// // cppcheck-suppress knownConditionTrueFalse +// for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) // NOLINT +// { +// std::this_thread::sleep_for(std::chrono::milliseconds(100)); +// } +// ASSERT_NE(maxSleep, sleep); + +// EXPECT_EQ(5u, odomPosesCount); +// } + +// ///////////////////////////////////////////////// +// TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(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 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("/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; +// // cppcheck-suppress knownConditionTrueFalse +// 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(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(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; +// // cppcheck-suppress knownConditionTrueFalse +// 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(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(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; +// // cppcheck-suppress knownConditionTrueFalse +// 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(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(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; +// int maxSleep = 30; +// // cppcheck-suppress knownConditionTrueFalse +// 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_SUITE_P(ServerRepeat, MecanumDriveTest, + ::testing::Range(1, 2)); diff --git a/test/worlds/mecanum_drive.sdf b/test/worlds/mecanum_drive.sdf new file mode 100644 index 0000000000..6433eebb50 --- /dev/null +++ b/test/worlds/mecanum_drive.sdf @@ -0,0 +1,357 @@ + + + + + + 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 + 100 100 + + + + + + 50 + + + + + + + + 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 2 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 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 -1 0 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + 0 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 1 0 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + 0 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 1 0 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + 0 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 -1 0 + + + + + + + + + chassis + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + front_right_wheel_joint + rear_left_wheel_joint + rear_right_wheel_joint + 1.25 + 1.511 + 0.3 + -5 + 5 + + + + + + From 63db52b9e437019e6b8ea77849d4c970b20d139d Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Tue, 23 Jan 2024 16:25:50 +0100 Subject: [PATCH 02/14] mecanum src added Signed-off-by: Stefano Mutti --- .vscode/c_cpp_properties.json | 21 --------------------- .vscode/settings.json | 3 --- 2 files changed, 24 deletions(-) delete mode 100644 .vscode/c_cpp_properties.json delete mode 100644 .vscode/settings.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index c0877c3bb2..0000000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "configurations": [ - { - "browse": { - "databaseFilename": "${default}", - "limitSymbolsToIncludedHeaders": false - }, - "includePath": [ - "/opt/ros/rolling/include/**", - "/home/stefanomutti/Libs/gz-sim/include/**", - "/usr/include/**" - ], - "name": "ROS", - "intelliSenseMode": "gcc-x64", - "compilerPath": "/usr/bin/gcc", - "cStandard": "gnu11", - "cppStandard": "c++14" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 98f2d1e701..0000000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "sonarlint.pathToCompileCommands": "${workspaceFolder}/build/compile_commands.json" -} \ No newline at end of file From fdc24cbbcac1e8b73471c416da95d11ca26b0555 Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Tue, 23 Jan 2024 16:26:01 +0100 Subject: [PATCH 03/14] mecanum src added Signed-off-by: Stefano Mutti --- log/COLCON_IGNORE | 0 log/latest | 1 - log/latest_list | 1 - log/list_2024-01-23_15-12-53/logger_all.log | 22 --------------------- 4 files changed, 24 deletions(-) delete mode 100644 log/COLCON_IGNORE delete mode 120000 log/latest delete mode 120000 log/latest_list delete mode 100644 log/list_2024-01-23_15-12-53/logger_all.log diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/log/latest b/log/latest deleted file mode 120000 index 17156673ec..0000000000 --- a/log/latest +++ /dev/null @@ -1 +0,0 @@ -latest_list \ No newline at end of file diff --git a/log/latest_list b/log/latest_list deleted file mode 120000 index 59e94cf04f..0000000000 --- a/log/latest_list +++ /dev/null @@ -1 +0,0 @@ -list_2024-01-23_15-12-53 \ No newline at end of file diff --git a/log/list_2024-01-23_15-12-53/logger_all.log b/log/list_2024-01-23_15-12-53/logger_all.log deleted file mode 100644 index 73e5d847e0..0000000000 --- a/log/list_2024-01-23_15-12-53/logger_all.log +++ /dev/null @@ -1,22 +0,0 @@ -[0.845s] DEBUG:colcon:Command line arguments: ['/usr/local/bin/colcon', 'list', '-p', '--base-paths', '/home/stefanomutti/Libs/gz-sim'] -[0.845s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/stefanomutti/Libs/gz-sim'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('list',)) -[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters -[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters -[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters -[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters -[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover -[0.881s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/stefanomutti/Libs/gz-sim' -[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ignore', 'ignore_ament_install'] -[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore' -[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore_ament_install' -[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_pkg'] -[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_pkg' -[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_meta'] -[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_meta' -[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ros'] -[0.881s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ros' -[0.896s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['cmake', 'python'] -[0.896s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'cmake' -[1.150s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'python' -[1.150s] DEBUG:colcon.colcon_core.package_identification:Package '/home/stefanomutti/Libs/gz-sim' with type 'cmake' and name 'gz-sim8' -[1.150s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults From c1a98b2d011ff340362a2fc71622e99c926ed777 Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Wed, 24 Jan 2024 10:53:04 +0100 Subject: [PATCH 04/14] Added MecanumDrive plugin and Tests with relative test sdf Signed-off-by: Stefano Mutti --- .vscode/c_cpp_properties.json | 21 + log/COLCON_IGNORE | 0 log/latest | 1 + log/latest_list | 1 + log/list_2024-01-24_07-27-30/logger_all.log | 22 + src/systems/mecanum_drive/MecanumDrive.cc | 36 + test/integration/mecanum_drive_system.cc | 768 +++++++----------- test/worlds/mecanum_drive.sdf | 2 +- test/worlds/mecanum_drive_custom_frame_id.sdf | 359 ++++++++ test/worlds/mecanum_drive_custom_tf_topic.sdf | 360 ++++++++ test/worlds/mecanum_drive_custom_topics.sdf | 359 ++++++++ 11 files changed, 1432 insertions(+), 497 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 log/COLCON_IGNORE create mode 120000 log/latest create mode 120000 log/latest_list create mode 100644 log/list_2024-01-24_07-27-30/logger_all.log create mode 100644 test/worlds/mecanum_drive_custom_frame_id.sdf create mode 100644 test/worlds/mecanum_drive_custom_tf_topic.sdf create mode 100644 test/worlds/mecanum_drive_custom_topics.sdf diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000000..c0877c3bb2 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,21 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/rolling/include/**", + "/home/stefanomutti/Libs/gz-sim/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/log/latest b/log/latest new file mode 120000 index 0000000000..17156673ec --- /dev/null +++ b/log/latest @@ -0,0 +1 @@ +latest_list \ No newline at end of file diff --git a/log/latest_list b/log/latest_list new file mode 120000 index 0000000000..8daabd30f9 --- /dev/null +++ b/log/latest_list @@ -0,0 +1 @@ +list_2024-01-24_07-27-30 \ No newline at end of file diff --git a/log/list_2024-01-24_07-27-30/logger_all.log b/log/list_2024-01-24_07-27-30/logger_all.log new file mode 100644 index 0000000000..cf1f3761e7 --- /dev/null +++ b/log/list_2024-01-24_07-27-30/logger_all.log @@ -0,0 +1,22 @@ +[0.710s] DEBUG:colcon:Command line arguments: ['/usr/local/bin/colcon', 'list', '-p', '--base-paths', '/home/stefanomutti/Libs/gz-sim'] +[0.710s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/stefanomutti/Libs/gz-sim'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('list',)) +[0.742s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.742s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.742s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.742s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.742s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.742s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/stefanomutti/Libs/gz-sim' +[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ignore', 'ignore_ament_install'] +[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore' +[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore_ament_install' +[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_pkg'] +[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_pkg' +[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_meta'] +[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_meta' +[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ros'] +[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ros' +[0.755s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['cmake', 'python'] +[0.755s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'cmake' +[0.993s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'python' +[0.993s] DEBUG:colcon.colcon_core.package_identification:Package '/home/stefanomutti/Libs/gz-sim' with type 'cmake' and name 'gz-sim8' +[0.993s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index f991126bc7..0a4ae0ed77 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -326,6 +326,9 @@ void MecanumDrive::Configure(const Entity &_entity, if (_sdf->HasElement("child_frame_id")) this->dataPtr->sdfChildFrameId = _sdf->Get("child_frame_id"); + gzmsg << "MecanumDrive publishing odom messages on [" << odomTopic << "]" + << std::endl; + gzmsg << "MecanumDrive subscribing to twist messages on [" << topic << "]" << std::endl; } @@ -495,6 +498,39 @@ void MecanumDrive::PreUpdate(const UpdateInfo &_info, } } + // Create the joint position components if they don't exist. + auto frontLeftPos = _ecm.Component( + this->dataPtr->frontLeftJoints[0]); + if (!frontLeftPos && _ecm.HasEntity(this->dataPtr->frontLeftJoints[0])) + { + _ecm.CreateComponent(this->dataPtr->frontLeftJoints[0], + components::JointPosition()); + } + + auto frontRightPos = _ecm.Component( + this->dataPtr->frontRightJoints[0]); + if (!frontRightPos && _ecm.HasEntity(this->dataPtr->frontRightJoints[0])) + { + _ecm.CreateComponent(this->dataPtr->frontRightJoints[0], + components::JointPosition()); + } + + auto backLeftPos = _ecm.Component( + this->dataPtr->backLeftJoints[0]); + if (!backLeftPos && _ecm.HasEntity(this->dataPtr->backLeftJoints[0])) + { + _ecm.CreateComponent(this->dataPtr->backLeftJoints[0], + components::JointPosition()); + } + + auto backRightPos = _ecm.Component( + this->dataPtr->backRightJoints[0]); + if (!backRightPos && _ecm.HasEntity(this->dataPtr->backRightJoints[0])) + { + _ecm.CreateComponent(this->dataPtr->backRightJoints[0], + components::JointPosition()); + } + } ////////////////////////////////////////////////// diff --git a/test/integration/mecanum_drive_system.cc b/test/integration/mecanum_drive_system.cc index 7dbdba4a83..7e66d6999e 100644 --- a/test/integration/mecanum_drive_system.cc +++ b/test/integration/mecanum_drive_system.cc @@ -130,16 +130,17 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam> // parameters in "/test/worlds/diff_drive.sdf". test::Relay velocityRamp; const int movementDirection = (forward ? 1 : -1); - double desiredLinVel = movementDirection * 10.5; + double desiredLinVelX = movementDirection * 0.15; + double desiredLinVelY = movementDirection * 0.15; double desiredAngVel = 0.2; velocityRamp.OnPreUpdate( [&](const UpdateInfo &/*_info*/, const EntityComponentManager &) { msgs::Set(msg.mutable_linear(), - math::Vector3d(desiredLinVel, 0, 0)); - msgs::Set(msg.mutable_angular(), - math::Vector3d(0.0, 0, desiredAngVel)); + math::Vector3d(desiredLinVelX, desiredLinVelY, 0)); + // msgs::Set(msg.mutable_angular(), + // math::Vector3d(0.0, 0, desiredAngVel)); pub.Publish(msg); }); @@ -150,13 +151,13 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam> // Poses for 4s ASSERT_EQ(4000u, poses.size()); - // 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); + 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); // Odometry calculates the pose of a point that is located half way // between the two wheels, not the origin of the model. For example, @@ -167,9 +168,9 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam> // change. To find the final pose of the model, we have to do the // following similarity transformation - // math::Pose3d tOdomModel(0.554283, 0, -0.325, 0, 0, 0); - // auto finalModelFramePose = - // tOdomModel * odomPoses.back() * tOdomModel.Inverse(); + math::Pose3d tOdomModel(0.554283, 0, -0.325, 0, 0, 0); + auto finalModelFramePose = + tOdomModel * odomPoses.back() * tOdomModel.Inverse(); // Odom for 3s ASSERT_FALSE(odomPoses.empty()); @@ -185,14 +186,14 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam> EXPECT_NEAR(expectedLowerPosition.Z(), expectedGreaterPosition.Z(), tol); EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); - EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); + EXPECT_NEAR(poses[0].Rot().Z(), poses[3999].Rot().Z(), tol); // The value from odometry will be close, but not exactly the ground truth // pose of the robot model. This is partially due to throttling the // odometry publisher, partially due to a frame difference between the // odom frame and the model frame, and partially due to wheel slip. - EXPECT_NEAR(poses[1020].Pos().X(), odomPoses[0].Pos().X(), 1e-2); - EXPECT_NEAR(poses[1020].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); + EXPECT_NEAR(poses.back().Pos().X(), odomPoses.back().Pos().X(), 1e-2); + EXPECT_NEAR(poses.back().Pos().Y(), odomPoses.back().Pos().Y(), 1e-2); // EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2); // EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2); @@ -226,538 +227,313 @@ TEST_P(MecanumDriveTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PublishCmd)) "/model/vehicle_blue/cmd_vel", "/model/vehicle_blue/odometry"); } -// ///////////////////////////////////////////////// -// // See: https://github.com/gazebosim/gz-sim/issues/630 -// TEST_P(MecanumDriveTest, -// GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PublishCmdCustomTopics)) -// { -// TestPublishCmd( -// std::string(PROJECT_SOURCE_PATH) + -// "/test/worlds/diff_drive_custom_topics.sdf", -// "/model/foo/cmdvel", "/model/bar/odom"); -// } - -// ///////////////////////////////////////////////// -// TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SkidPublishCmd)) -// { -// // Start server -// ServerConfig serverConfig; -// serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + -// "/test/worlds/diff_drive_skid.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 UpdateInfo &, -// const 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())); -// }; - -// 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(200)); -// } -// EXPECT_NE(maxSleep, sleep); - -// // Odom for 3s -// ASSERT_FALSE(odomPoses.empty()); -// EXPECT_EQ(3u, odomPoses.size()); - -// EXPECT_LT(poses[0].Pos().X(), poses[3999].Pos().X()); -// EXPECT_NEAR(poses[0].Pos().Z(), poses[3999].Pos().Z(), tol); -// EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); -// EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); - -// // Slip works on DART>=6.10, which isn't available on Ubuntu Focal -// #ifndef __linux__ -// EXPECT_LT(poses[0].Pos().Y(), poses[3999].Pos().Y()); -// EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); -// #endif -// } +///////////////////////////////////////////////// +// See: https://github.com/gazebosim/gz-sim/issues/630 +TEST_P(MecanumDriveTest, + GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PublishCmdCustomTopics)) +{ + TestPublishCmd( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/mecanum_drive_custom_topics.sdf", + "/model/foo/cmdvel", "/model/bar/odom"); +} + + +///////////////////////////////////////////////// +TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OdomFrameId)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/mecanum_drive.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); -// ///////////////////////////////////////////////// -// TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(EnableDisableCmd)) -// { -// // Start server -// ServerConfig serverConfig; -// serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + -// "/test/worlds/diff_drive_skid.sdf"); + server.SetUpdatePeriod(0ns); -// Server server(serverConfig); -// EXPECT_FALSE(server.Running()); -// EXPECT_FALSE(*server.Running(0)); + unsigned int odomPosesCount = 0; + std::function odomCb = + [&odomPosesCount](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); -// server.SetUpdatePeriod(0ns); + ASSERT_GT(_msg.header().data_size(), 1); -// // Create a system that records the vehicle poses -// test::Relay testSystem; + EXPECT_STREQ(_msg.header().data(0).key().c_str(), "frame_id"); + EXPECT_STREQ( + _msg.header().data(0).value().Get(0).c_str(), "vehicle_blue/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_blue/chassis"); + + odomPosesCount++; + }; + + transport::Node node; + auto pub = node.Advertise("/model/vehicle_blue/cmd_vel"); + node.Subscribe("/model/vehicle_blue/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; + // cppcheck-suppress knownConditionTrueFalse + for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) // NOLINT + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); + + EXPECT_EQ(5u, odomPosesCount); +} -// std::vector poses; -// testSystem.OnPostUpdate([&poses](const UpdateInfo &, -// const EntityComponentManager &_ecm) -// { -// auto id = _ecm.EntityByComponents( -// components::Model(), -// components::Name("vehicle")); -// EXPECT_NE(kNullEntity, id); +///////////////////////////////////////////////// +TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OdomCustomFrameId)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/mecanum_drive_custom_frame_id.sdf"); -// auto poseComp = _ecm.Component(id); -// ASSERT_NE(nullptr, poseComp); + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); -// poses.push_back(poseComp->Data()); -// }); -// server.AddSystem(testSystem.systemPtr); + server.SetUpdatePeriod(0ns); -// // Run server and check that vehicle didn't move -// server.Run(true, 1000, false); + unsigned int odomPosesCount = 0; + std::function odomCb = + [&odomPosesCount](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); -// EXPECT_EQ(1000u, poses.size()); + ASSERT_GT(_msg.header().data_size(), 1); -// for (const auto &pose : poses) -// { -// EXPECT_EQ(poses[0], pose); -// } + EXPECT_STREQ(_msg.header().data(0).key().c_str(), "frame_id"); + EXPECT_STREQ(_msg.header().data(0).value().Get(0).c_str(), "odom"); -// // 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()); + 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"); -// double msgTime = -// static_cast(_msg.header().stamp().sec()) + -// static_cast(_msg.header().stamp().nsec()) * 1e-9; + odomPosesCount++; + }; -// EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); -// lastMsgTime = msgTime; + transport::Node node; + auto pub = node.Advertise("/model/vehicle_blue/cmd_vel"); + node.Subscribe("/model/vehicle_blue/odometry", odomCb); -// odomPoses.push_back(msgs::Convert(_msg.pose())); -// }; + 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)); -// transport::Node node; -// auto pub = node.Advertise("/model/vehicle/cmd_vel"); -// node.Subscribe("/model/vehicle/odometry", odomCb); + pub.Publish(msg); -// 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.0)); + server.Run(true, 100, false); -// pub.Publish(msg); + int sleep = 0; + int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse + for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); -// server.Run(true, 3000, false); + EXPECT_EQ(5u, odomPosesCount); +} -// // Poses for 4s -// EXPECT_EQ(4000u, poses.size()); +///////////////////////////////////////////////// +TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Pose_VFrameId)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/mecanum_drive.sdf"); -// // Disable controller -// auto pub_enable = node.Advertise("/model/vehicle/enable"); + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); -// msgs::Boolean msg_enable; -// msg_enable.set_data(false); + server.SetUpdatePeriod(0ns); -// pub_enable.Publish(msg_enable); + 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()); -// // Run for 2s and expect no movement -// server.Run(true, 2000, false); + ASSERT_GT(_msg.pose(0).header().data_size(), 1); -// EXPECT_EQ(6000u, poses.size()); + 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_blue/odom"); -// // Re-enable controller -// msg_enable.set_data(true); + 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_blue/chassis"); -// pub_enable.Publish(msg_enable); + odomPosesCount++; + }; -// pub.Publish(msg); + transport::Node node; + auto pub = node.Advertise("/model/vehicle_blue/cmd_vel"); + node.Subscribe("/model/vehicle_blue/tf", pose_VCb); -// // Run for 2s and expect movement again -// server.Run(true, 2000, false); + 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)); -// EXPECT_EQ(8000u, poses.size()); + pub.Publish(msg); -// int sleep = 0; -// int maxSleep = 70; -// for (; odomPoses.size() < 7 && sleep < maxSleep; ++sleep) -// { -// std::this_thread::sleep_for(std::chrono::milliseconds(100)); -// } -// EXPECT_NE(maxSleep, sleep); + server.Run(true, 100, false); -// // Odom for 7s -// ASSERT_FALSE(odomPoses.empty()); -// EXPECT_EQ(7u, odomPoses.size()); + int sleep = 0; + int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse + for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); -// EXPECT_LT(poses[0].Pos().X(), poses[3999].Pos().X()); + EXPECT_EQ(5u, odomPosesCount); +} -// // Should no be moving from 5s to 6s (stopped at 3s and time to slow down) -// EXPECT_NEAR(poses[4999].Pos().X(), poses[5999].Pos().X(), tol); +///////////////////////////////////////////////// +TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Pose_VCustomFrameId)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/mecanum_drive_custom_frame_id.sdf"); -// // Should be moving from 6s to 8s -// EXPECT_LT(poses[5999].Pos().X(), poses[7999].Pos().X()); -// } + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); -// ///////////////////////////////////////////////// -// TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OdomFrameId)) -// { -// // Start server -// ServerConfig serverConfig; -// serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + -// "/test/worlds/diff_drive.sdf"); + server.SetUpdatePeriod(0ns); -// Server server(serverConfig); -// EXPECT_FALSE(server.Running()); -// EXPECT_FALSE(*server.Running(0)); + 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()); -// server.SetUpdatePeriod(0ns); + ASSERT_GT(_msg.pose(0).header().data_size(), 1); -// unsigned int odomPosesCount = 0; -// std::function odomCb = -// [&odomPosesCount](const msgs::Odometry &_msg) -// { -// ASSERT_TRUE(_msg.has_header()); -// ASSERT_TRUE(_msg.header().has_stamp()); + 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"); -// ASSERT_GT(_msg.header().data_size(), 1); + 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"); -// EXPECT_STREQ(_msg.header().data(0).key().c_str(), "frame_id"); -// EXPECT_STREQ( -// _msg.header().data(0).value().Get(0).c_str(), "vehicle/odom"); + odomPosesCount++; + }; -// 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"); + transport::Node node; + auto pub = node.Advertise("/model/vehicle_blue/cmd_vel"); + node.Subscribe("/model/vehicle_blue/tf", Pose_VCb); -// odomPosesCount++; -// }; + 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)); -// transport::Node node; -// auto pub = node.Advertise("/model/vehicle/cmd_vel"); -// node.Subscribe("/model/vehicle/odometry", odomCb); + pub.Publish(msg); -// 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)); + server.Run(true, 100, false); -// pub.Publish(msg); + int sleep = 0; + int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse + for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); -// server.Run(true, 100, false); + EXPECT_EQ(5u, odomPosesCount); +} -// int sleep = 0; -// int maxSleep = 30; -// // cppcheck-suppress knownConditionTrueFalse -// for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) // NOLINT -// { -// std::this_thread::sleep_for(std::chrono::milliseconds(100)); -// } -// ASSERT_NE(maxSleep, sleep); +///////////////////////////////////////////////// +TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Pose_VCustomTfTopic)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/mecanum_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_blue/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_blue/chassis"); + + odomPosesCount++; + }; -// EXPECT_EQ(5u, odomPosesCount); -// } + transport::Node node; + auto pub = node.Advertise("/model/vehicle_blue/cmd_vel"); + node.Subscribe("/tf_foo", pose_VCb); -// ///////////////////////////////////////////////// -// TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OdomCustomFrameId)) -// { -// // Start server -// ServerConfig serverConfig; -// serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + -// "/test/worlds/diff_drive_custom_frame_id.sdf"); + 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)); -// Server server(serverConfig); -// EXPECT_FALSE(server.Running()); -// EXPECT_FALSE(*server.Running(0)); + pub.Publish(msg); -// server.SetUpdatePeriod(0ns); + server.Run(true, 100, false); -// unsigned int odomPosesCount = 0; -// std::function 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("/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; -// // cppcheck-suppress knownConditionTrueFalse -// 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(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Pose_VFrameId)) -// { -// // Start server -// ServerConfig serverConfig; -// serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + -// "/test/worlds/diff_drive.sdf"); + int sleep = 0; + int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse + for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); -// 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; -// // cppcheck-suppress knownConditionTrueFalse -// 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(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(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; -// // cppcheck-suppress knownConditionTrueFalse -// 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(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(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; -// int maxSleep = 30; -// // cppcheck-suppress knownConditionTrueFalse -// for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) -// { -// std::this_thread::sleep_for(std::chrono::milliseconds(100)); -// } -// ASSERT_NE(maxSleep, sleep); - -// EXPECT_EQ(5u, odomPosesCount); -// } + EXPECT_EQ(5u, odomPosesCount); +} // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, MecanumDriveTest, diff --git a/test/worlds/mecanum_drive.sdf b/test/worlds/mecanum_drive.sdf index 6433eebb50..56a854de08 100644 --- a/test/worlds/mecanum_drive.sdf +++ b/test/worlds/mecanum_drive.sdf @@ -68,7 +68,7 @@ - 0 2 0.325 0 -0 0 + 0 0 0.325 0 -0 0 -0.151427 -0 0.175 0 -0 0 diff --git a/test/worlds/mecanum_drive_custom_frame_id.sdf b/test/worlds/mecanum_drive_custom_frame_id.sdf new file mode 100644 index 0000000000..7ac6c92e8f --- /dev/null +++ b/test/worlds/mecanum_drive_custom_frame_id.sdf @@ -0,0 +1,359 @@ + + + + + + 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 + 100 100 + + + + + + 50 + + + + + + + + 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 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 -1 0 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + 0 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 1 0 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + 0 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 1 0 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + 0 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 -1 0 + + + + + + + + + chassis + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + front_right_wheel_joint + rear_left_wheel_joint + rear_right_wheel_joint + 1.25 + 1.511 + 0.3 + -5 + 5 + odom + base_footprint + + + + + + diff --git a/test/worlds/mecanum_drive_custom_tf_topic.sdf b/test/worlds/mecanum_drive_custom_tf_topic.sdf new file mode 100644 index 0000000000..29591181f3 --- /dev/null +++ b/test/worlds/mecanum_drive_custom_tf_topic.sdf @@ -0,0 +1,360 @@ + + + + + + 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 + 100 100 + + + + + + 50 + + + + + + + + 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 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 -1 0 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + 0 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 1 0 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + 0 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 1 0 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + 0 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 -1 0 + + + + + + + + + chassis + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + front_right_wheel_joint + rear_left_wheel_joint + rear_right_wheel_joint + 1.25 + 1.511 + 0.3 + -5 + 5 + /model/foo/cmdvel + /model/bar/odom + tf_foo + + + + + + diff --git a/test/worlds/mecanum_drive_custom_topics.sdf b/test/worlds/mecanum_drive_custom_topics.sdf new file mode 100644 index 0000000000..14e5612602 --- /dev/null +++ b/test/worlds/mecanum_drive_custom_topics.sdf @@ -0,0 +1,359 @@ + + + + + + 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 + 100 100 + + + + + + 50 + + + + + + + + 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 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 -1 0 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + 0 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 1 0 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + 0 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 1 0 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + 0 0 0 1.5707 0 0 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL + 0.006 0.006 0.006 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 -1 0 + + + + + + + + + chassis + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + front_right_wheel_joint + rear_left_wheel_joint + rear_right_wheel_joint + 1.25 + 1.511 + 0.3 + -5 + 5 + /model/foo/cmdvel + /model/bar/odom + + + + + + From 7a9e1bbec180cdece856f9ec3ccdc346f51c0408 Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Wed, 24 Jan 2024 10:54:11 +0100 Subject: [PATCH 05/14] Added MecanumDrive plugin and Tests with relative test sdf Signed-off-by: Stefano Mutti --- .vscode/c_cpp_properties.json | 21 -------------------- log/COLCON_IGNORE | 0 log/latest | 1 - log/latest_list | 1 - log/list_2024-01-24_07-27-30/logger_all.log | 22 --------------------- 5 files changed, 45 deletions(-) delete mode 100644 .vscode/c_cpp_properties.json delete mode 100644 log/COLCON_IGNORE delete mode 120000 log/latest delete mode 120000 log/latest_list delete mode 100644 log/list_2024-01-24_07-27-30/logger_all.log diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index c0877c3bb2..0000000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "configurations": [ - { - "browse": { - "databaseFilename": "${default}", - "limitSymbolsToIncludedHeaders": false - }, - "includePath": [ - "/opt/ros/rolling/include/**", - "/home/stefanomutti/Libs/gz-sim/include/**", - "/usr/include/**" - ], - "name": "ROS", - "intelliSenseMode": "gcc-x64", - "compilerPath": "/usr/bin/gcc", - "cStandard": "gnu11", - "cppStandard": "c++14" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/log/latest b/log/latest deleted file mode 120000 index 17156673ec..0000000000 --- a/log/latest +++ /dev/null @@ -1 +0,0 @@ -latest_list \ No newline at end of file diff --git a/log/latest_list b/log/latest_list deleted file mode 120000 index 8daabd30f9..0000000000 --- a/log/latest_list +++ /dev/null @@ -1 +0,0 @@ -list_2024-01-24_07-27-30 \ No newline at end of file diff --git a/log/list_2024-01-24_07-27-30/logger_all.log b/log/list_2024-01-24_07-27-30/logger_all.log deleted file mode 100644 index cf1f3761e7..0000000000 --- a/log/list_2024-01-24_07-27-30/logger_all.log +++ /dev/null @@ -1,22 +0,0 @@ -[0.710s] DEBUG:colcon:Command line arguments: ['/usr/local/bin/colcon', 'list', '-p', '--base-paths', '/home/stefanomutti/Libs/gz-sim'] -[0.710s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/stefanomutti/Libs/gz-sim'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('list',)) -[0.742s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters -[0.742s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters -[0.742s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters -[0.742s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters -[0.742s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover -[0.742s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/stefanomutti/Libs/gz-sim' -[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ignore', 'ignore_ament_install'] -[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore' -[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore_ament_install' -[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_pkg'] -[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_pkg' -[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_meta'] -[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_meta' -[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ros'] -[0.742s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ros' -[0.755s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['cmake', 'python'] -[0.755s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'cmake' -[0.993s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'python' -[0.993s] DEBUG:colcon.colcon_core.package_identification:Package '/home/stefanomutti/Libs/gz-sim' with type 'cmake' and name 'gz-sim8' -[0.993s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults From 6c9cec1e5c5fa2ff7c568ff3cc90ee63fb4ee7c1 Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Thu, 25 Jan 2024 16:36:18 +0100 Subject: [PATCH 06/14] Corrected setWheelParams Signed-off-by: Stefano Mutti --- .vscode/c_cpp_properties.json | 21 ++++++++++++++++++++ log/COLCON_IGNORE | 0 log/latest | 1 + log/latest_list | 1 + log/list_2024-01-25_09-23-01/logger_all.log | 22 +++++++++++++++++++++ src/systems/mecanum_drive/MecanumDrive.cc | 2 +- 6 files changed, 46 insertions(+), 1 deletion(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 log/COLCON_IGNORE create mode 120000 log/latest create mode 120000 log/latest_list create mode 100644 log/list_2024-01-25_09-23-01/logger_all.log diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000000..c0877c3bb2 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,21 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/rolling/include/**", + "/home/stefanomutti/Libs/gz-sim/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/log/latest b/log/latest new file mode 120000 index 0000000000..17156673ec --- /dev/null +++ b/log/latest @@ -0,0 +1 @@ +latest_list \ No newline at end of file diff --git a/log/latest_list b/log/latest_list new file mode 120000 index 0000000000..1b677f8bdc --- /dev/null +++ b/log/latest_list @@ -0,0 +1 @@ +list_2024-01-25_09-23-01 \ No newline at end of file diff --git a/log/list_2024-01-25_09-23-01/logger_all.log b/log/list_2024-01-25_09-23-01/logger_all.log new file mode 100644 index 0000000000..c3d2cf08e8 --- /dev/null +++ b/log/list_2024-01-25_09-23-01/logger_all.log @@ -0,0 +1,22 @@ +[1.123s] DEBUG:colcon:Command line arguments: ['/usr/local/bin/colcon', 'list', '-p', '--base-paths', '/home/stefanomutti/Libs/gz-sim'] +[1.123s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/stefanomutti/Libs/gz-sim'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('list',)) +[1.167s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[1.167s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[1.167s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[1.167s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[1.167s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[1.167s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/stefanomutti/Libs/gz-sim' +[1.167s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ignore', 'ignore_ament_install'] +[1.167s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore' +[1.167s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore_ament_install' +[1.167s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_pkg'] +[1.167s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_pkg' +[1.167s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_meta'] +[1.168s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_meta' +[1.168s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ros'] +[1.168s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ros' +[1.185s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['cmake', 'python'] +[1.185s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'cmake' +[1.511s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'python' +[1.511s] DEBUG:colcon.colcon_core.package_identification:Package '/home/stefanomutti/Libs/gz-sim' with type 'cmake' and name 'gz-sim8' +[1.511s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index 0a4ae0ed77..407991dc56 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -287,7 +287,7 @@ void MecanumDrive::Configure(const Entity &_entity, // Setup odometry. this->dataPtr->odom.SetWheelParams(this->dataPtr->wheelSeparation, - this->dataPtr->wheelRadius, this->dataPtr->wheelRadius, this->dataPtr->wheelRadius); + this->dataPtr->wheelbase, this->dataPtr->wheelRadius, this->dataPtr->wheelRadius); // Subscribe to commands std::vector topics; From 740c45eb08558a7a58df80748672df20991e99ea Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Thu, 25 Jan 2024 16:36:49 +0100 Subject: [PATCH 07/14] Corrected setWheelParams Signed-off-by: Stefano Mutti --- .vscode/c_cpp_properties.json | 21 -------------------- log/COLCON_IGNORE | 0 log/latest | 1 - log/latest_list | 1 - log/list_2024-01-25_09-23-01/logger_all.log | 22 --------------------- 5 files changed, 45 deletions(-) delete mode 100644 .vscode/c_cpp_properties.json delete mode 100644 log/COLCON_IGNORE delete mode 120000 log/latest delete mode 120000 log/latest_list delete mode 100644 log/list_2024-01-25_09-23-01/logger_all.log diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index c0877c3bb2..0000000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "configurations": [ - { - "browse": { - "databaseFilename": "${default}", - "limitSymbolsToIncludedHeaders": false - }, - "includePath": [ - "/opt/ros/rolling/include/**", - "/home/stefanomutti/Libs/gz-sim/include/**", - "/usr/include/**" - ], - "name": "ROS", - "intelliSenseMode": "gcc-x64", - "compilerPath": "/usr/bin/gcc", - "cStandard": "gnu11", - "cppStandard": "c++14" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/log/latest b/log/latest deleted file mode 120000 index 17156673ec..0000000000 --- a/log/latest +++ /dev/null @@ -1 +0,0 @@ -latest_list \ No newline at end of file diff --git a/log/latest_list b/log/latest_list deleted file mode 120000 index 1b677f8bdc..0000000000 --- a/log/latest_list +++ /dev/null @@ -1 +0,0 @@ -list_2024-01-25_09-23-01 \ No newline at end of file diff --git a/log/list_2024-01-25_09-23-01/logger_all.log b/log/list_2024-01-25_09-23-01/logger_all.log deleted file mode 100644 index c3d2cf08e8..0000000000 --- a/log/list_2024-01-25_09-23-01/logger_all.log +++ /dev/null @@ -1,22 +0,0 @@ -[1.123s] DEBUG:colcon:Command line arguments: ['/usr/local/bin/colcon', 'list', '-p', '--base-paths', '/home/stefanomutti/Libs/gz-sim'] -[1.123s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/stefanomutti/Libs/gz-sim'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('list',)) -[1.167s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters -[1.167s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters -[1.167s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters -[1.167s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters -[1.167s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover -[1.167s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/stefanomutti/Libs/gz-sim' -[1.167s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ignore', 'ignore_ament_install'] -[1.167s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore' -[1.167s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore_ament_install' -[1.167s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_pkg'] -[1.167s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_pkg' -[1.167s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_meta'] -[1.168s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_meta' -[1.168s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ros'] -[1.168s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ros' -[1.185s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['cmake', 'python'] -[1.185s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'cmake' -[1.511s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'python' -[1.511s] DEBUG:colcon.colcon_core.package_identification:Package '/home/stefanomutti/Libs/gz-sim' with type 'cmake' and name 'gz-sim8' -[1.511s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults From fe5d267b0247b02ff7157655210eadd8a9aa5bee Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Fri, 1 Mar 2024 13:32:00 +0100 Subject: [PATCH 08/14] remove white spaces Signed-off-by: Stefano Mutti --- src/systems/mecanum_drive/MecanumDrive.cc | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index 407991dc56..e323775f22 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -566,7 +566,7 @@ void MecanumDrivePrivate::UpdateOdometry(const UpdateInfo &_info, auto frontRightPos = _ecm.Component(this->frontRightJoints[0]); auto backLeftPos = _ecm.Component(this->backLeftJoints[0]); auto backRightPos = _ecm.Component(this->backRightJoints[0]); - + // Abort if the joints were not found or just created. if (!frontLeftPos || !frontRightPos || !backLeftPos || !backRightPos || frontLeftPos->Data().empty() || frontRightPos->Data().empty() || backLeftPos->Data().empty() || backRightPos->Data().empty()) @@ -574,8 +574,7 @@ void MecanumDrivePrivate::UpdateOdometry(const UpdateInfo &_info, return; } - this->odom.Update(frontLeftPos->Data()[0], frontRightPos->Data()[0], backLeftPos->Data()[0], backRightPos->Data()[0], - std::chrono::steady_clock::time_point(_info.simTime)); + this->odom.Update(frontLeftPos->Data()[0], frontRightPos->Data()[0], backLeftPos->Data()[0], backRightPos->Data()[0],std::chrono::steady_clock::time_point(_info.simTime)); // Throttle publishing auto diff = _info.simTime - this->lastOdomPubTime; From ee0fc38c7475aea5a43705d4bc62037ef19d5a24 Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Tue, 5 Mar 2024 10:50:34 +0100 Subject: [PATCH 09/14] remove white spaces Signed-off-by: Stefano Mutti --- src/systems/mecanum_drive/MecanumDrive.cc | 2 +- test/integration/mecanum_drive_system.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index e323775f22..c7a2f8d8b8 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -574,7 +574,7 @@ void MecanumDrivePrivate::UpdateOdometry(const UpdateInfo &_info, return; } - this->odom.Update(frontLeftPos->Data()[0], frontRightPos->Data()[0], backLeftPos->Data()[0], backRightPos->Data()[0],std::chrono::steady_clock::time_point(_info.simTime)); + this->odom.Update(frontLeftPos->Data()[0],frontRightPos->Data()[0],backLeftPos->Data()[0],backRightPos->Data()[0],std::chrono::steady_clock::time_point(_info.simTime)); // Throttle publishing auto diff = _info.simTime - this->lastOdomPubTime; diff --git a/test/integration/mecanum_drive_system.cc b/test/integration/mecanum_drive_system.cc index 7e66d6999e..bf8bd3d301 100644 --- a/test/integration/mecanum_drive_system.cc +++ b/test/integration/mecanum_drive_system.cc @@ -168,7 +168,7 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam> // change. To find the final pose of the model, we have to do the // following similarity transformation - math::Pose3d tOdomModel(0.554283, 0, -0.325, 0, 0, 0); + math::Pose3d tOdomModel(0.554283,0,-0.325,0,0,0); auto finalModelFramePose = tOdomModel * odomPoses.back() * tOdomModel.Inverse(); From 5623fbacac31d3b5790bb9bdd7a27ab2e94776ea Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Tue, 5 Mar 2024 10:59:21 +0100 Subject: [PATCH 10/14] remove white spaces Signed-off-by: Stefano Mutti --- src/systems/mecanum_drive/MecanumDrive.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index c7a2f8d8b8..c2fae8d1a4 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -566,7 +566,7 @@ void MecanumDrivePrivate::UpdateOdometry(const UpdateInfo &_info, auto frontRightPos = _ecm.Component(this->frontRightJoints[0]); auto backLeftPos = _ecm.Component(this->backLeftJoints[0]); auto backRightPos = _ecm.Component(this->backRightJoints[0]); - + // Abort if the joints were not found or just created. if (!frontLeftPos || !frontRightPos || !backLeftPos || !backRightPos || frontLeftPos->Data().empty() || frontRightPos->Data().empty() || backLeftPos->Data().empty() || backRightPos->Data().empty()) @@ -574,7 +574,7 @@ void MecanumDrivePrivate::UpdateOdometry(const UpdateInfo &_info, return; } - this->odom.Update(frontLeftPos->Data()[0],frontRightPos->Data()[0],backLeftPos->Data()[0],backRightPos->Data()[0],std::chrono::steady_clock::time_point(_info.simTime)); + this->odom.Update(frontLeftPos->Data()[0],frontRightPos->Data()[0],backLeftPos->Data()[0],backRightPos->Data()[0],std::chrono::steady_clock::time_point(_info.simTime)); // Throttle publishing auto diff = _info.simTime - this->lastOdomPubTime; From e0d5a21f1e409cc84fedccb1a4762dba7e2ea54d Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Wed, 27 Mar 2024 10:34:46 +0100 Subject: [PATCH 11/14] remove unused var in test Signed-off-by: Stefano Mutti --- .vscode/c_cpp_properties.json | 21 ++++++++++++++++++++ .vscode/settings.json | 8 ++++++++ log/COLCON_IGNORE | 0 log/latest | 1 + log/latest_list | 1 + log/list_2024-03-27_10-32-07/logger_all.log | 22 +++++++++++++++++++++ test/integration/mecanum_drive_system.cc | 3 --- 7 files changed, 53 insertions(+), 3 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/settings.json create mode 100644 log/COLCON_IGNORE create mode 120000 log/latest create mode 120000 log/latest_list create mode 100644 log/list_2024-03-27_10-32-07/logger_all.log diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000000..c0877c3bb2 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,21 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/rolling/include/**", + "/home/stefanomutti/Libs/gz-sim/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000000..7cfa4965cd --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,8 @@ +{ + "python.autoComplete.extraPaths": [ + "/opt/ros/rolling/lib/python3.10/site-packages" + ], + "python.analysis.extraPaths": [ + "/opt/ros/rolling/lib/python3.10/site-packages" + ] +} \ No newline at end of file diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/log/latest b/log/latest new file mode 120000 index 0000000000..17156673ec --- /dev/null +++ b/log/latest @@ -0,0 +1 @@ +latest_list \ No newline at end of file diff --git a/log/latest_list b/log/latest_list new file mode 120000 index 0000000000..588b5e28a9 --- /dev/null +++ b/log/latest_list @@ -0,0 +1 @@ +list_2024-03-27_10-32-07 \ No newline at end of file diff --git a/log/list_2024-03-27_10-32-07/logger_all.log b/log/list_2024-03-27_10-32-07/logger_all.log new file mode 100644 index 0000000000..751c42b018 --- /dev/null +++ b/log/list_2024-03-27_10-32-07/logger_all.log @@ -0,0 +1,22 @@ +[0.113s] DEBUG:colcon:Command line arguments: ['/home/stefanomutti/.local/bin/colcon', 'list', '-p', '--base-paths', '/home/stefanomutti/Libs/gz-sim'] +[0.113s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/stefanomutti/Libs/gz-sim'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('list',)) +[0.133s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.133s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.134s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.134s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.134s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.134s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/stefanomutti/Libs/gz-sim' +[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ignore', 'ignore_ament_install'] +[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore' +[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore_ament_install' +[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_pkg'] +[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_pkg' +[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_meta'] +[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_meta' +[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ros'] +[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ros' +[0.147s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['cmake', 'python'] +[0.147s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'cmake' +[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'python' +[0.165s] DEBUG:colcon.colcon_core.package_identification:Package '/home/stefanomutti/Libs/gz-sim' with type 'cmake' and name 'gz-sim8' +[0.165s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults diff --git a/test/integration/mecanum_drive_system.cc b/test/integration/mecanum_drive_system.cc index bf8bd3d301..2f51e12a18 100644 --- a/test/integration/mecanum_drive_system.cc +++ b/test/integration/mecanum_drive_system.cc @@ -132,15 +132,12 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam> const int movementDirection = (forward ? 1 : -1); double desiredLinVelX = movementDirection * 0.15; double desiredLinVelY = movementDirection * 0.15; - double desiredAngVel = 0.2; velocityRamp.OnPreUpdate( [&](const UpdateInfo &/*_info*/, const EntityComponentManager &) { msgs::Set(msg.mutable_linear(), math::Vector3d(desiredLinVelX, desiredLinVelY, 0)); - // msgs::Set(msg.mutable_angular(), - // math::Vector3d(0.0, 0, desiredAngVel)); pub.Publish(msg); }); From 24d708ea9183d431948fa808cc51178aaf5deeb9 Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Wed, 29 May 2024 19:29:47 +0200 Subject: [PATCH 12/14] removed .vscode and log Signed-off-by: Stefano Mutti --- .vscode/c_cpp_properties.json | 21 -------------------- .vscode/settings.json | 8 -------- log/COLCON_IGNORE | 0 log/latest | 1 - log/latest_list | 1 - log/list_2024-03-27_10-32-07/logger_all.log | 22 --------------------- 6 files changed, 53 deletions(-) delete mode 100644 .vscode/c_cpp_properties.json delete mode 100644 .vscode/settings.json delete mode 100644 log/COLCON_IGNORE delete mode 120000 log/latest delete mode 120000 log/latest_list delete mode 100644 log/list_2024-03-27_10-32-07/logger_all.log diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index c0877c3bb2..0000000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "configurations": [ - { - "browse": { - "databaseFilename": "${default}", - "limitSymbolsToIncludedHeaders": false - }, - "includePath": [ - "/opt/ros/rolling/include/**", - "/home/stefanomutti/Libs/gz-sim/include/**", - "/usr/include/**" - ], - "name": "ROS", - "intelliSenseMode": "gcc-x64", - "compilerPath": "/usr/bin/gcc", - "cStandard": "gnu11", - "cppStandard": "c++14" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 7cfa4965cd..0000000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "python.autoComplete.extraPaths": [ - "/opt/ros/rolling/lib/python3.10/site-packages" - ], - "python.analysis.extraPaths": [ - "/opt/ros/rolling/lib/python3.10/site-packages" - ] -} \ No newline at end of file diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/log/latest b/log/latest deleted file mode 120000 index 17156673ec..0000000000 --- a/log/latest +++ /dev/null @@ -1 +0,0 @@ -latest_list \ No newline at end of file diff --git a/log/latest_list b/log/latest_list deleted file mode 120000 index 588b5e28a9..0000000000 --- a/log/latest_list +++ /dev/null @@ -1 +0,0 @@ -list_2024-03-27_10-32-07 \ No newline at end of file diff --git a/log/list_2024-03-27_10-32-07/logger_all.log b/log/list_2024-03-27_10-32-07/logger_all.log deleted file mode 100644 index 751c42b018..0000000000 --- a/log/list_2024-03-27_10-32-07/logger_all.log +++ /dev/null @@ -1,22 +0,0 @@ -[0.113s] DEBUG:colcon:Command line arguments: ['/home/stefanomutti/.local/bin/colcon', 'list', '-p', '--base-paths', '/home/stefanomutti/Libs/gz-sim'] -[0.113s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/stefanomutti/Libs/gz-sim'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('list',)) -[0.133s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters -[0.133s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters -[0.134s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters -[0.134s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters -[0.134s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover -[0.134s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/stefanomutti/Libs/gz-sim' -[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ignore', 'ignore_ament_install'] -[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore' -[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ignore_ament_install' -[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_pkg'] -[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_pkg' -[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['colcon_meta'] -[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'colcon_meta' -[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['ros'] -[0.134s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'ros' -[0.147s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extensions ['cmake', 'python'] -[0.147s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'cmake' -[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(/home/stefanomutti/Libs/gz-sim) by extension 'python' -[0.165s] DEBUG:colcon.colcon_core.package_identification:Package '/home/stefanomutti/Libs/gz-sim' with type 'cmake' and name 'gz-sim8' -[0.165s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults From d075b984d4c5578d1ccfcdffefcd2e78c6d1c557 Mon Sep 17 00:00:00 2001 From: muttistefano Date: Fri, 8 Nov 2024 16:06:31 +0100 Subject: [PATCH 13/14] Update test/integration/mecanum_drive_system.cc Co-authored-by: Addisu Z. Taddese Signed-off-by: muttistefano --- test/integration/mecanum_drive_system.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/mecanum_drive_system.cc b/test/integration/mecanum_drive_system.cc index 2f51e12a18..3ba1c7a8cc 100644 --- a/test/integration/mecanum_drive_system.cc +++ b/test/integration/mecanum_drive_system.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2024 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. From c69c40a187ef3f38c0c111dd141be5fe6a3737c5 Mon Sep 17 00:00:00 2001 From: muttistefano Date: Fri, 8 Nov 2024 16:06:40 +0100 Subject: [PATCH 14/14] Update test/integration/mecanum_drive_system.cc Co-authored-by: Addisu Z. Taddese Signed-off-by: muttistefano --- test/integration/mecanum_drive_system.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/mecanum_drive_system.cc b/test/integration/mecanum_drive_system.cc index 3ba1c7a8cc..d2d734afe4 100644 --- a/test/integration/mecanum_drive_system.cc +++ b/test/integration/mecanum_drive_system.cc @@ -44,7 +44,7 @@ using namespace sim; using namespace std::chrono_literals; /// \brief Test MecanumDrive system -class MecanumDriveTest : public InternalFixture<::testing::TestWithParam> +class MecanumDriveTest : public InternalFixture<::testing::Test> { /// \param[in] _sdfFile SDF file to load. /// \param[in] _cmdVelTopic Command velocity topic.