fuse -> ROS 2 fuse_core : Parameters and Tests #286

methylDragon · 2022-11-17T19:58:41Z

Description

With this PR, tests now run (and pass) on fuse_core! (Sans linting)

This PR incorporates all remaining changes @BrettRD made to fuse_core, and then applies patches on top of them to fix parameters and the tests.

Additionally, it brings in changes from #273 to allow for Ceres support in the tests. (That PR's been merged but not on the rolling branch. I've cherry-picked the changes.)

Also, this PR adds the NodeInterfaces class proposed in ros2/rclcpp#2041 to fuse_core, to port passing of ROS node handles around.

Notes

The launch testing is a little hacky (I have to assert stdout for a gtest).
I had to change the behavior of two tests due to changes in how ROS 2 handles parameters (strongly typed), I think what I did made sense, but I'd like a confirmation

Pinging @svwilliams for visibility.

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Signed-off-by: methylDragon <methylDragon@gmail.com>

Signed-off-by: methylDragon <methylDragon@gmail.com> Authored-by: Brett Downing - moves publishers mostly to ros2 APIs - adds convenience functions - start building publishers and models - builds and links as shared libraries - uses ros2 services for optimizer reset - removes redundant diagnostic updater timer - adds ParameterDescriptors to load parameter descriptions - adds a rough compatibility patch for ros1 style parameters - loads sensor models and publishers with ros2 - loads motion_models from ros2 params - add descriptions to CLI parameter tools - makes config files slightly closer to ROS1 layout - fixes type errors - cleanup, deprecate XmlRpc API for parameters - uses Node directly in parameter helpers to simplify the port - add missing dependency - Adding Ceres options - Adding more classes to the CMakeLists - converts stored parameters to double, allowing dynamic reconf - uses rclcpp::ok - binds timer callbacks - tweaks order of arguments - fix params and chrono, build fuse_graphs - change params, fix chrono, build fuse_graphs

Signed-off-by: methylDragon <methylDragon@gmail.com>

Ceres added this argument in ceres-solver/ceres-solver@e7a3035 we need to pass it else template substitution fails.

Upstream commit made me assume kGlobalSize, but that threw at runtime when the tests ran. This seems to work, also put a using statement there to make roslint happy.

Signed-off-by: methylDragon <methylDragon@gmail.com>

sloretz

Quick feedback - will come back to review parameters use and tests

fuse_core/CMakeLists.txt

fuse_core/include/fuse_core/throttled_callback.h

fuse_core/src/async_motion_model.cpp

methylDragon · 2022-11-18T23:18:08Z

Wait a sec.. that caused a test regression :X

Edit: Fixed https://github.com/locusrobotics/fuse/compare/329a3dc486673b87ced0c3576a4d61894bf8798e..67c6fd9a7f97eb49dc851a119ea0940d2c7706ff

With these changes merged into the downstream PRs, no non-lint tests are broken.

Signed-off-by: methylDragon <methylDragon@gmail.com>

sloretz

A bunch of small comments! I still have 6 more files to review.

fuse_core/include/fuse_core/async_publisher.h

sloretz · 2022-11-19T00:10:08Z

fuse_core/test/test_async_publisher.cpp

-  rclcpp::Time wait_time_elapsed = rclcpp::Clock(RCL_SYSTEM_TIME).now() + rclcpp::Duration::from_seconds(10.0);
-  while (!publisher.callback_processed && rclcpp::Clock(RCL_SYSTEM_TIME).now() < wait_time_elapsed)
+  rclcpp::Time wait_time_elapsed =
+    rclcpp::Clock(RCL_SYSTEM_TIME).now() + rclcpp::Duration::from_seconds(10.0);


nit, I'd recommend constructing the clock once and then using it.

sloretz · 2022-11-19T00:16:15Z

fuse_core/test/test_async_publisher.cpp

  {
-    rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.1));
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
  }
  EXPECT_TRUE(publisher.callback_processed);
 }

 int main(int argc, char** argv)


Nit, i think the whole int main() function can be deleted since ament_add_gtest links to gtest_main.

sloretz · 2022-11-19T00:16:41Z

fuse_core/test/test_async_sensor_model.cpp

@@ -121,12 +136,5 @@ TEST(AsyncSensorModel, SendTransaction)
 int main(int argc, char** argv)


nit, same comment about int main() can be deleted.

sloretz · 2022-11-19T00:29:26Z

fuse_core/test/test_async_motion_model.cpp

@@ -120,12 +135,5 @@ TEST(AsyncMotionModel, ApplyCallback)
 int main(int argc, char** argv)


nit same comment about int main()

fuse_core/test/launch_tests/test_parameters.cpp

fuse_core/test/CMakeLists.txt

sloretz · 2022-11-21T22:44:43Z

fuse_core/include/fuse_core/parameter.h

+  std::string default_value = "";
+  value = getParam(interfaces, key, default_value);
+
+  if (value == default_value)


What if the value was explicitly set to empty string?

This might have to use one of the decalare_parameter() overrides that doesn't accept a default value. It looks like the second getParam() above already implements that.

Actually all of the declare_parameter signatures require a default value... The one with value.get_type() just uses the default constructed value of that type (which is an empty string)... I'm not sure how you'd distinguish between the two in this case :/

You have to declare a param before you can query it, and declaring it requires the input of a default value.

sloretz · 2022-11-21T22:48:46Z

fuse_core/src/ceres_options.cpp

 {
+  rcl_interfaces::msg::ParameterDescriptor tmp_descr;
+
+  std::string namespace_;


nit, namespace_ with a trailing underscore means "member variable" (in ROS 2 style via Google Style guide).

Ah, I was trying to get around the keyword namespace, I'll rename it to ns instead then.

sloretz · 2022-11-21T22:52:36Z

fuse_core/src/ceres_options.cpp

+  } else {
+    namespace_ = namespace_string + ".";
+  }
+
 #if CERES_VERSION_AT_LEAST(1, 13, 0)


libceres-dev is version 1.13.0 in Bionic - since we're targeting Rolling I think it's safe to remove this check.

fuse_core/src/ceres_options.cpp

sloretz · 2022-11-21T22:57:15Z

fuse_core/src/ceres_options.cpp

+    namespace_ + "enable_fast_removal", problem_options.enable_fast_removal, tmp_descr
+  );
+
+  tmp_descr.description = "If true, trades memory for faster Problem::RemoveResidualBlock()";


It looks like this description was copied from the above, and it seems different from upstream's description:

https://github.com/ceres-solver/ceres-solver/blob/352b320ab1b5438a0838aea09cbbf07fa4ff5d71/include/ceres/problem.h#L149-L158

fuse_core/src/ceres_options.cpp

sloretz

Most of the way through re-reviewing, only one comment so far!

fuse_core/test/test_throttled_callback.cpp

sloretz

Last two comments!

sloretz · 2022-11-30T04:34:47Z

fuse_core/test/launch_tests/test_parameters.py

+@pytest.fixture
+def test_proc():
+    test_root = os.path.join(
+        get_package_prefix('fuse_core'), '..', '..', 'build', 'fuse_core', 'test'


It looks like this assumes the test is being run after being built with colcon, with the default name for the build directory. It also assumes the software has been installed. I would recommend not using get_package_prefix() at all. There are at least a couple good alternatives.

One alternative would be to set the WORKING_DIRECTORY argument to ament_add_pytest_test to ${CMAKE_CURRENT_SOURCE_DIR} and use a relative path.

Another alternative would be to use the ENV arugment to pass in the path to the parameters file as an environment variable and use that as the path here.

sloretz · 2022-11-30T04:36:23Z

fuse_core/test/launch_tests/test_parameters.py

+def test_no_failed_gtests(test_proc, launch_context):
+    def validate_output(output):
+        assert '[  PASSED  ] 2 tests.' in output.splitlines(), (
+            'process never printed required passes')


It looks like the test is returning RUN_ALL_TESTS();. I'd bet the exit code would be non-zero on failure, and so that could be the check here.

sloretz

The substance looks good to me, just nits about unused imports

fuse_core/test/launch_tests/test_parameters.py

sloretz

One last important change - the package.xml needs a <test_depend>launch_pytest</test_depend>

Signed-off-by: methylDragon <methylDragon@gmail.com>

sloretz

LGTM!

All tests pass except cpplint, uncrustify, and xmllint. Approving since linting is to be fixed in the next PR.

sloretz and others added 10 commits November 14, 2022 14:50

Add gtest tests

5f96167

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Fix tests

f0f687c

Signed-off-by: methylDragon <methylDragon@gmail.com>

Implement NodeInterfaces class

40516a0

Signed-off-by: methylDragon <methylDragon@gmail.com>

Merge branch 'rolling-core-tests' into rolling-params

42dd928

Build all fuse_core non-launch tests

3fca2d9

Signed-off-by: methylDragon <methylDragon@gmail.com>

Pass kNumResiduals to the internal AutoDiff function.

3fbd8d2

Ceres added this argument in ceres-solver/ceres-solver@e7a3035 we need to pass it else template substitution fails.

Pass kLocalSize instead of kGlobalSize

3dbb69e

Upstream commit made me assume kGlobalSize, but that threw at runtime when the tests ran. This seems to work, also put a using statement there to make roslint happy.

Fix tests and update params

7f41093

Signed-off-by: methylDragon <methylDragon@gmail.com>

methylDragon requested a review from sloretz November 17, 2022 19:58

methylDragon marked this pull request as ready for review November 17, 2022 20:02

methylDragon force-pushed the rolling-params branch from fde4286 to ec59f4d Compare November 17, 2022 20:03

methylDragon mentioned this pull request Nov 17, 2022

Fuse -> ROS 2 Rolling Porting Progress #276

Closed

54 tasks

methylDragon force-pushed the rolling-params branch 2 times, most recently from 260dcb3 to 0b66a96 Compare November 17, 2022 22:01

methylDragon mentioned this pull request Nov 17, 2022

fuse -> ROS 2 fuse_loss: Port fuse_loss #287

Merged

methylDragon force-pushed the rolling-params branch from 0b66a96 to 1d9e8fd Compare November 17, 2022 23:38

Fix all fuse_core tests

de22e91

Signed-off-by: methylDragon <methylDragon@gmail.com>

methylDragon force-pushed the rolling-params branch from 1d9e8fd to de22e91 Compare November 18, 2022 01:31

methylDragon mentioned this pull request Nov 18, 2022

fuse -> ROS 2 fuse_graphs : Port fuse_graphs #289

Merged

sloretz reviewed Nov 18, 2022

View reviewed changes

methylDragon force-pushed the rolling-params branch 2 times, most recently from 51dbb91 to 329a3dc Compare November 18, 2022 23:14

Update executor usage and throttled callback

67c6fd9

Signed-off-by: methylDragon <methylDragon@gmail.com>

methylDragon force-pushed the rolling-params branch from 329a3dc to 67c6fd9 Compare November 18, 2022 23:23

sloretz reviewed Nov 19, 2022

View reviewed changes

sloretz reviewed Nov 21, 2022

View reviewed changes

methylDragon force-pushed the rolling-params branch from 50dfe90 to fb126b2 Compare November 30, 2022 01:12

methylDragon force-pushed the rolling-params branch 4 times, most recently from dc149ff to d126a1e Compare November 30, 2022 02:25

methylDragon requested a review from sloretz November 30, 2022 02:26

sloretz reviewed Nov 30, 2022

View reviewed changes

fuse_core/test/test_throttled_callback.cpp Outdated Show resolved Hide resolved

methylDragon force-pushed the rolling-params branch from d126a1e to 5367431 Compare November 30, 2022 03:16

sloretz requested changes Nov 30, 2022

View reviewed changes

methylDragon force-pushed the rolling-params branch from 5367431 to ccdca17 Compare November 30, 2022 19:36

sloretz reviewed Nov 30, 2022

View reviewed changes

methylDragon force-pushed the rolling-params branch from ccdca17 to bb0383a Compare November 30, 2022 19:50

sloretz requested changes Nov 30, 2022

View reviewed changes

Update fuse_core tests and params

89147de

Signed-off-by: methylDragon <methylDragon@gmail.com>

methylDragon force-pushed the rolling-params branch from bb0383a to 89147de Compare November 30, 2022 20:56

sloretz approved these changes Nov 30, 2022

View reviewed changes

methylDragon merged commit df291bd into locusrobotics:rolling Nov 30, 2022

methylDragon deleted the rolling-params branch November 30, 2022 21:06

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fuse -> ROS 2 fuse_core : Parameters and Tests #286

fuse -> ROS 2 fuse_core : Parameters and Tests #286

methylDragon commented Nov 17, 2022 •

edited

Loading

sloretz left a comment

methylDragon commented Nov 18, 2022 •

edited

Loading

sloretz left a comment

sloretz Nov 19, 2022

sloretz Nov 19, 2022

sloretz Nov 19, 2022

sloretz Nov 19, 2022

sloretz Nov 21, 2022

methylDragon Nov 29, 2022 •

edited

Loading

sloretz Nov 21, 2022

methylDragon Nov 29, 2022

sloretz Nov 21, 2022

sloretz Nov 21, 2022

sloretz left a comment

sloretz left a comment

sloretz Nov 30, 2022

sloretz Nov 30, 2022

sloretz left a comment

sloretz left a comment

sloretz left a comment

		@@ -121,12 +136,5 @@ TEST(AsyncSensorModel, SendTransaction)
		int main(int argc, char** argv)

		@@ -120,12 +135,5 @@ TEST(AsyncMotionModel, ApplyCallback)
		int main(int argc, char** argv)

fuse -> ROS 2 fuse_core : Parameters and Tests #286

fuse -> ROS 2 fuse_core : Parameters and Tests #286

Conversation

methylDragon commented Nov 17, 2022 • edited Loading

Description

Notes

sloretz left a comment

Choose a reason for hiding this comment

methylDragon commented Nov 18, 2022 • edited Loading

sloretz left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

methylDragon Nov 29, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sloretz left a comment

Choose a reason for hiding this comment

sloretz left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sloretz left a comment

Choose a reason for hiding this comment

sloretz left a comment

Choose a reason for hiding this comment

sloretz left a comment

Choose a reason for hiding this comment

methylDragon commented Nov 17, 2022 •

edited

Loading

methylDragon commented Nov 18, 2022 •

edited

Loading

methylDragon Nov 29, 2022 •

edited

Loading