fuse -> ROS 2 fuse_models: Port fuse_models #304
Conversation
methylDragon
force-pushed
the
rolling-models
branch
2 times, most recently
from
55f7666
to
7ee50a0
Compare
| @@ -110,7 +110,7 @@ class BatchOptimizer : public Optimizer | |||
| BatchOptimizer( | |||
| fuse_core::Graph::UniquePtr graph, | |||
| const ros::NodeHandle& node_handle = ros::NodeHandle(), | |||
| const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); | |||
| const ros::NodeHandle& private_node_handle = ros::NodeHandle("")); | |||
There was a problem hiding this comment.
probably could remove the fuse_optimizers changes from this PR
There was a problem hiding this comment.
fuse_models/CMakeLists.txt
Outdated
|
|
||
| ########### | ||
| ## Build ## | ||
| ########### | ||
| add_compile_options(-Wall -Werror) | ||
|
|
||
| include_directories(include) |
There was a problem hiding this comment.
target_include_directories() used below should be sufficient
There was a problem hiding this comment.
fuse_models/CMakeLists.txt
Outdated
| ) | ||
| target_link_libraries(${PROJECT_NAME} | ||
| ${catkin_LIBRARIES} | ||
| ${CERES_LIBRARIES} |
There was a problem hiding this comment.
nit, I'd recommend the target name Ceres::ceres instead. It looks like we're already using it in fuse_core.
Line 55 in 5f4c5ed
There was a problem hiding this comment.
fuse_models/CMakeLists.txt
Outdated
| ${geometry_msgs_TARGETS} | ||
| ${nav_msgs_TARGETS} | ||
| pluginlib::pluginlib | ||
| ${rclcpp_components_TARGETS} |
There was a problem hiding this comment.
I'd recommend against rclcpp_components_TARGETS as it includes the component manager. I think the target to use instead is rclcpp_components::component.
There was a problem hiding this comment.
fuse_models/benchmark/CMakeLists.txt
Outdated
| benchmark_unicycle_2d_state_cost_function | ||
| benchmark | ||
| ${PROJECT_NAME} | ||
| ${catkin_LIBRARIES} |
There was a problem hiding this comment.
nit, remove catkin_LIBRARIES
There was a problem hiding this comment.
fuse_models/benchmark/CMakeLists.txt
Outdated
| benchmark | ||
| ${PROJECT_NAME} | ||
| ${catkin_LIBRARIES} | ||
| ${CERES_LIBRARIES} |
There was a problem hiding this comment.
fuse_models/benchmark/CMakeLists.txt
Outdated
| ) | ||
| set_target_properties(benchmark_unicycle_2d_state_cost_function | ||
| PROPERTIES | ||
| CXX_STANDARD 14 |
There was a problem hiding this comment.
| differential = fuse_core::getParam(interfaces, ns + "differential", differential); | ||
| differential = fuse_core::getParam(interfaces, ns + "differential", differential); | ||
| differential = fuse_core::getParam(interfaces, ns + "differential", differential); | ||
| differential = fuse_core::getParam(interfaces, ns + "differential", differential); |
There was a problem hiding this comment.
It looks like all the parameter and variable names got changed to "differential" in this file.
There was a problem hiding this comment.
Looks like my regex engine hit a bug... Fixed!
https://github.com/locusrobotics/fuse/compare/cfe9352ddd4b3fd14865ff6692b219a6d1336962..136e98e0578314dfc8495257404250aff9cf3199
| pub_options.callback_group = cb_group_; | ||
|
|
||
| // NOTE(CH3): We might need to prepend the name here like with the other publishers | ||
| // ... But will that cause the subscriptions on the models to fail??? |
There was a problem hiding this comment.
What name might need to be prepended? I see jointTopicName is already used below.
There was a problem hiding this comment.
Ah, this comment was about using that joinTopicName call. I had mistakenly thought that the original ROS 1 code didn't use the same sort of private node handle namespacing like the other publishers. Sorry for the noise 😬
Comment removed: https://github.com/locusrobotics/fuse/compare/cfe9352ddd4b3fd14865ff6692b219a6d1336962..136e98e0578314dfc8495257404250aff9cf3199
methylDragon
force-pushed
the
rolling-models
branch
11 times, most recently
from
e633ec1
to
424beef
Compare
|
@ros-pull-request-builder retest this please! |
|
Looks like we'll need |
| } | ||
| auto srv = std::make_shared<std_srvs::srv::Empty::Request>(); | ||
| // No need to spin since node is optimizer node, which should be spinning | ||
| auto result_future = reset_client_->async_send_request(srv); |
There was a problem hiding this comment.
No need to spin but maybe need to wait for the future to complete? result_future->wait(). That would make sure the reset has been processed before the graph is sent below.
There was a problem hiding this comment.
Fixed! 85aa1bc
There was a problem hiding this comment.
I realize we discovered and fixed an issue with this in the fuse_optimizers PR by avoiding blocking in the set_pose service callback, but I'm fine to merge this PR as is and continue the discussion there.
| } | ||
| auto srv = std::make_shared<std_srvs::srv::Empty::Request>(); | ||
| // No need to spin since node is optimizer node, which should be spinning | ||
| auto result_future = reset_client_->async_send_request(srv); |
There was a problem hiding this comment.
Same comment about maybe needing to wait on the result future
There was a problem hiding this comment.
Fixed! 85aa1bc
Signed-off-by: methylDragon <methylDragon@gmail.com>
Signed-off-by: methylDragon <methylDragon@gmail.com>
Signed-off-by: methylDragon <methylDragon@gmail.com>
methylDragon
force-pushed
the
rolling-models
branch
from
424beef
to
85aa1bc
Compare
Signed-off-by: methylDragon <methylDragon@gmail.com>
methylDragon
force-pushed
the
rolling-models
branch
from
85aa1bc
to
c0a8f46
Compare
|
@ros-pull-request-builder retest this please |
There was a problem hiding this comment.
Approving knowing that this PR has a bug in graph_ignition and unicycle_2d_ignition that causes the executor to be blocked waiting for the reply to the reset service, because the fix is in the next PR #307
CI fails because tf2_2d needs to be released to Rolling. In the meantime I did colcon build, colcon test locally and I only see linting test failures, which we've also been addressing in follow up PRs.
rolling:osrf> colcon test-result
build/fuse_models/Testing/20230113-2357/Test.xml: 12 tests, 0 errors, 4 failures, 0 skipped
build/fuse_models/test_results/fuse_models/copyright.xunit.xml: 48 tests, 0 errors, 2 failures, 0 skipped
build/fuse_models/test_results/fuse_models/cpplint.xunit.xml: 512 tests, 0 errors, 509 failures, 0 skipped
build/fuse_models/test_results/fuse_models/uncrustify.xunit.xml: 48 tests, 0 errors, 48 failures, 0 skipped
build/fuse_models/test_results/fuse_models/xmllint.xunit.xml: 2 tests, 0 errors, 1 failure, 0 skipped
Summary: 3029 tests, 0 errors, 564 failures, 529 skipped
| } | ||
| auto srv = std::make_shared<std_srvs::srv::Empty::Request>(); | ||
| // No need to spin since node is optimizer node, which should be spinning | ||
| auto result_future = reset_client_->async_send_request(srv); |
There was a problem hiding this comment.
I realize we discovered and fixed an issue with this in the fuse_optimizers PR by avoiding blocking in the set_pose service callback, but I'm fine to merge this PR as is and continue the discussion there.
See: #276
Description
This PR ports the entirety of fuse_models, including:
Pay special attention to the parameter and service namespacing.
In ROS 1 nodehandles were used to attach a node namespace, and then also the plugin's name (as a sub namespace). In the ROS 2 version I prepend the plugin's name, and ignore the node's namespace (since parameters are meant to be local to the node in ROS 2.)
I fixed some bugs in the parameter code (namely a missing empty check), and, perhaps more importantly, I had to shift the service description and generation from fuse_models to fuse_msgs due to:
I needed to shift it because otherwise there wouldn't be a way to generate the appropriate libraries and targets without also masking the core fuse_models library that needs to be generated.
The shift now means every service namespace will be shifted to fuse_msgs instead.
Notes
Pinging @svwilliams for visibility.