fuse -> ROS 2 fuse_optimizers: Port fuse_optimizers #307

methylDragon · 2023-01-10T21:15:03Z

See: #276

Description

This PR ports the entirety of fuse_optimizers, including:

CMakeLists and packages
Source code
Tests
Other stuff like docs are in other PRs.
This introduces changes to the usage of rclcpp::Time (defaulting to use of RCL_ROS_TIME where time is default initialized.)

Pinging @svwilliams for visibility.

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

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

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

sloretz

In progress review - will continue tomorrow

fuse_core/src/timestamp_manager.cpp

fuse_optimizers/CMakeLists.txt

fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h

fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h

fuse_optimizers/src/fixed_lag_smoother.cpp

fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp

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

Uses user provided timestamp to construct TimestampManager::MotionModelSegment to ensure all rclcpp::Time points use the same clock type that's being fed in. Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

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

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

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

fuse_optimizers/src/batch_optimizer.cpp

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

fuse_optimizers/src/fixed_lag_smoother.cpp

fuse_optimizers/src/optimizer.cpp

sloretz

These are my last comments on this PR. Once they're addressed and methylDragon#5 is included then this PR will LGTM 🚀

sloretz · 2023-01-24T18:52:05Z

fuse_models/src/graph_ignition.cpp

+    // It needs to be free to handle the response to this service call.
+    // Have a callback do the rest of the work when a response comes.
+    auto result_future = reset_client_->async_send_request(srv,
+      [this, post_process, &graph, msg](rclcpp::Client<std_srvs::srv::Empty>::SharedFuture result){


It looks like graph is a local variable (of type std::unique_ptr<...>), so if I understand correctly it will probably already be destructed by the time this lambda is run. I think this needs to be a move capture , graph = std::move(graph), instead

I had to convert the graph uniqueptr into a shared_ptr so it can be copyable.

This is because the async service call signature we're using requires a std::function, which must be copyable (and so requires all args to be copyable)

But aside from that, this bug is fixed! Nice catch!

sloretz · 2023-01-24T19:04:12Z

fuse_optimizers/src/optimizer.cpp


-#include <XmlRpcValue.h>
+//#include <XmlRpcValue.h>


nit, delete this line

sloretz · 2023-01-24T19:04:33Z

fuse_optimizers/src/optimizer.cpp

-// TODO(CH3): pass a rclcpp::Context, and a CallbackGroup
-Optimizer::Optimizer(rclcpp::NodeOptions options, fuse_core::Graph::UniquePtr graph) :
-    Node("optimizer_node", options),
+// XXX pass a rclcpp::Context, and a CallbackGroup


Nit, comment no longer required

sloretz · 2023-01-24T19:08:24Z

fuse_optimizers/test/CMakeLists.txt

+  test_variable_stamp_index
+)
+
+foreach(test_name ${TEST_TARGETS})


nit, looks like the loop and TEST_TARGETS variable are no longer necessary. I'd recommend removing them

sloretz · 2023-01-24T19:17:34Z

fuse_optimizers/test/launch_tests/test_parameters.cpp

+  // Load a positive parameter:
+  {
+    double parameter{default_value};
+    fuse_core::getPositiveParam(node, "positive_parameter", parameter);


This looks like a copy of a test from fuse_core. I think this file can be deleted.

…ist (#5)

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

methylDragon force-pushed the rolling-optimizers branch 4 times, most recently from 9fa6009 to d69cc5b Compare January 13, 2023 08:08

methylDragon changed the title ~~fuse -> ROS 2 fuse_optimizers: WIP~~ fuse -> ROS 2 fuse_optimizers Jan 13, 2023

methylDragon requested a review from sloretz January 13, 2023 08:37

methylDragon marked this pull request as ready for review January 13, 2023 08:37

methylDragon mentioned this pull request Jan 13, 2023

Fuse -> ROS 2 Rolling Porting Progress #276

Closed

54 tasks

methylDragon and others added 6 commits January 18, 2023 06:02

Port messages

9179e30

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

Preliminary port of fuse_optimizers

ec3c224

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

Time fixes

a92611f

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

Use async service and client to avoid executor deadlock

ca5dc11

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

Use async service and client and fix param loading

16d652f

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

Port fuse_optimizers optimizer test

3733599

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

methylDragon force-pushed the rolling-optimizers branch from e82af9e to 3733599 Compare January 17, 2023 22:03

sloretz mentioned this pull request Jan 18, 2023

Add rclcpp::RosTime ros2/rclcpp#2084

Closed

sloretz reviewed Jan 19, 2023

View reviewed changes

methylDragon force-pushed the rolling-optimizers branch from e4f55b6 to e2b585e Compare January 19, 2023 08:20

Rectify ROS namespaces, fix tests, and refine

92fa8ef

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

methylDragon force-pushed the rolling-optimizers branch from e2b585e to 92fa8ef Compare January 19, 2023 08:27

sloretz mentioned this pull request Jan 20, 2023

Allow != and == of Time objects with different sources ros2/rclcpp#2087

Closed

sloretz and others added 5 commits January 20, 2023 17:51

Use consistent clock type in Timestamp Manager

070b03d

Uses user provided timestamp to construct TimestampManager::MotionModelSegment to ensure all rclcpp::Time points use the same clock type that's being fed in. Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Avoid floating point rounding in nanosecond precise test

0606e8e

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

Revert RCL_ROS_TIME insertion since no longer required

7a3d3a1

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

undo accidental whitespace change

322ceac

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

Use NodeInterfaces with optimizers

864acc5

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

methylDragon changed the title ~~fuse -> ROS 2 fuse_optimizers~~ fuse -> ROS 2 fuse_optimizers: Port fuse_optimizers Jan 22, 2023

sloretz added 3 commits January 23, 2023 15:05

lint timestamp_manager.cpp

469b7e5

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

Add missing test_depend on launch_ros

d78a083

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

No need to convert to chrono for create_timer

ba487fa

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

sloretz reviewed Jan 23, 2023

View reviewed changes

fuse_optimizers/src/batch_optimizer.cpp Outdated Show resolved Hide resolved

No need to convert to chrono for create_timer

0ce06cd

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

sloretz reviewed Jan 23, 2023

View reviewed changes

fuse_optimizers/src/fixed_lag_smoother.cpp Outdated Show resolved Hide resolved

sloretz reviewed Jan 24, 2023

View reviewed changes

fuse_optimizers/src/optimizer.cpp Outdated Show resolved Hide resolved

sloretz mentioned this pull request Jan 24, 2023

Remove need for sensor_model_list, motion_model_list, and publisher_list methylDragon/fuse#5

Merged

sloretz reviewed Jan 24, 2023

View reviewed changes

sloretz mentioned this pull request Jan 25, 2023

Add rclcpp::list_parameter_override_prefixes ros2/rclcpp#2090

Open

sloretz and others added 2 commits January 24, 2023 22:56

Remove need for sensor_model_list, motion_model_list, and publisher_l…

3cba177

…ist (#5)

Refine changes and fix graph lifetime bug

81d9202

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

methylDragon force-pushed the rolling-optimizers branch from b96524e to 81d9202 Compare January 26, 2023 17:31

methylDragon merged commit 8e76155 into locusrobotics:rolling Jan 26, 2023

methylDragon deleted the rolling-optimizers branch January 26, 2023 17:37

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fuse -> ROS 2 fuse_optimizers: Port fuse_optimizers #307

fuse -> ROS 2 fuse_optimizers: Port fuse_optimizers #307

methylDragon commented Jan 10, 2023 •

edited

sloretz left a comment

sloretz left a comment

sloretz Jan 24, 2023

methylDragon Jan 26, 2023

sloretz Jan 24, 2023

sloretz Jan 24, 2023

sloretz Jan 24, 2023

sloretz Jan 24, 2023


		#include <XmlRpcValue.h>
		//#include <XmlRpcValue.h>

fuse -> ROS 2 fuse_optimizers: Port fuse_optimizers #307

fuse -> ROS 2 fuse_optimizers: Port fuse_optimizers #307

Conversation

methylDragon commented Jan 10, 2023 • edited

Description

sloretz left a comment

Choose a reason for hiding this comment

sloretz left a comment

Choose a reason for hiding this comment

sloretz Jan 24, 2023

Choose a reason for hiding this comment

methylDragon Jan 26, 2023

Choose a reason for hiding this comment

sloretz Jan 24, 2023

Choose a reason for hiding this comment

sloretz Jan 24, 2023

Choose a reason for hiding this comment

sloretz Jan 24, 2023

Choose a reason for hiding this comment

sloretz Jan 24, 2023

Choose a reason for hiding this comment

methylDragon commented Jan 10, 2023 •

edited