fuse -> ROS 2 fuse_core : Messages and Services #285

methylDragon · 2022-11-12T02:46:21Z

Description

Just a small PR migrating messages and services.

Namespaces and headers were hit.

The only catch is the migration away from ConstPtrs. I tried to make a judgement call for what types to migrate to, and I left notes in the source for when I thought it was iffy.

I'm also kinda suspicious of the number of ConstSharedPtrs that I threw around.

See 43a59d7 for those changes

EDIT: This will just feature changes for fuse_core, the changes from this will be sprinkled around the other packages as we get to them

Pinging @svwilliams for visibility.

svwilliams

In ROS1, I reflexively publish and subscribe using shared ptrs to messages. I notice that has been changed to const refs to messages. I don't know enough about ROS2 yet. Is that the standard practice in ROS2?

svwilliams · 2022-11-17T20:11:20Z

fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp

 #include <fuse_core/uuid.h>
 #include <fuse_variables/orientation_3d_stamped.h>
-#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/msg/Quaternion.hpp>


If the message isn't used here, feel free to drop the include

svwilliams · 2022-11-17T23:46:39Z

fuse_models/include/fuse_models/pose_2d.h

  ParameterType params_;

-  geometry_msgs::PoseWithCovarianceStamped::ConstPtr previous_pose_msg_;
+  geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr previous_pose_msg_;


If process() is not receiving a shared ptr, there's no reason for the previous pose to be a shared ptr either.

It would make sense to leave it a ptr type to prevent copy assignment in the implementation of processDifferential I think?
It's also necessary for the conditional

void Pose2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, const bool validate, fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique<geometry_msgs::msg::PoseWithCovarianceStamped>(); transformed_pose->header.frame_id = params_.target_frame.empty() ? pose.header.frame_id : params_.target_frame; if (!common::transformMessage(tf_buffer_, pose, *transformed_pose)) { RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 5.0 * 1000, "Cannot transform pose message with stamp " << pose.header.stamp << " to target frame " << params_.target_frame); return; } if (previous_pose_msg_) { common::processDifferentialPoseWithCovariance( name(), device_id_, *previous_pose_msg_, *transformed_pose, params_.independent, params_.minimum_pose_relative_covariance, params_.loss, params_.position_indices, params_.orientation_indices, validate, transaction); } previous_pose_msg_ = std::move(transformed_pose); <----------------- here }

(I will convert the constsharedptr to unique_ptr, though)

svwilliams · 2022-11-17T23:56:29Z

fuse_optimizers/test/test_fixed_lag_ignition.cpp

+    // TODO(CH3): Oh no, there's no equivalent rclcpp method I think...
+    //            Probably just wait a bit?? It's in a test anyway


You can make a subscriber with a callback that populates a std::future. You can then wait on the future here. It is more work, but it means you are not blocking the unit test longer than is needed.

Seems that there actually is a wait_for_message method in rclcpp. I'll use that when it's time to get around to fuse_optimizers. And if that fails I'll go the condition variable route (instead of futures) 😬

svwilliams · 2022-11-18T00:03:53Z

fuse_tutorials/src/range_sensor_simulator.cpp

 * @brief Convert the set of beacons into a pointcloud for visualization purposes
 */
-sensor_msgs::PointCloud2::ConstPtr beaconsToPointcloud(
+sensor_msgs::msg::PointCloud2::ConstSharedPtr beaconsToPointcloud(


These methods are constructing ROS messages to be published. In ROS1 I always publish messages as shared ptrs. But it seems you have been publishing and subscribing with bare objects in ROS2. If that is the case, you can drop the shared pointer complexity here entirely.

svwilliams · 2022-11-18T00:08:08Z

fuse_core/test/test_throttled_callback.cpp

   */
-  void keepCallback(const geometry_msgs::Point::ConstPtr& msg)
+  // TODO(CH3): I'm iffy on this...
+  void keepCallback(const geometry_msgs::msg::Point::ConstSharedPtr msg)


The callback signature for the ThrottledMessageCallback is void(const typename M&). So shouldn't this be void keepCallback(const geometry_msgs::msg::Point& msg)?

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

In ROS1, I reflexively publish and subscribe using shared ptrs to messages. I notice that has been changed to const refs to messages. I don't know enough about ROS2 yet. Is that the standard practice in ROS2?

@svwilliams Yes, most of the time. It has to do with how intra-process communication is handled. There's more information here: https://docs.ros.org/en/rolling/Tutorials/Demos/Intra-Process-Communication.html

If I understand correctly, in most cases const references are the most efficient. If the code is going to modify the message and re-publish it (like in a sensor processing pipeline), then non-const std::unique_ptr may be a better option as rclcpp can choose to std::move() the message from the publisher to the subscriber.

sloretz · 2022-12-02T21:41:21Z

@ros-pull-request-builder retest this please!

sloretz

I think I'd recommend making these changes on a per-package basis as each package is ported rather than all at once. That will allow using the compiler to check the paths and types are correct.

sloretz · 2022-12-02T21:48:45Z

fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h

 #include <fuse_variables/orientation_3d_stamped.h>
-#include <geometry_msgs/PoseWithCovariance.h>
-#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/msg/PoseWithCovariance.hpp>


I think this is actually geometry_msgs/msg/pose_with_covariance.hpp. As far as I know all generated headers in ROS 2 are lowercase with underscores, so same comment throughout the PR

It also looks like PoseWithCovariance is unused in this file, so this include could be removed.

methylDragon · 2022-12-02T22:07:32Z

I think I'd recommend making these changes on a per-package basis as each package is ported rather than all at once. That will allow using the compiler to check the paths and types are correct.

Could we just get this in first and adjust as we go up the stack? Otherwise it'll be a guaranteed extra step for each package..

methylDragon · 2022-12-02T22:35:23Z

@ros-pull-request-builder retest this please

fuse_core/test/test_throttled_callback.cpp

sloretz · 2022-12-02T23:10:37Z

fuse_core/include/fuse_core/graph_deserializer.hpp

   */
  inline fuse_core::Graph::UniquePtr deserialize(
-    const fuse_msgs::msg::SerializedGraph::ConstSharedPtr msg) const
+    const fuse_msgs::msg::SerializedGraph::SharedPtr msg) const


Since this is just passing through to a method that treats the value as const, I think ConstSharedPtr makes more sense as it would allow both std::shared_ptr<const MessageType> and std::shared_ptr<MessageType> to be pasesd in.

Small example in C++

#include <iostream> #include <memory> void const_func(std::shared_ptr<const double> dp) { std::cout << "Const Got some double: " << *dp << "\n"; } void non_const_func(std::shared_ptr<double> dp) { std::cout << "non Const Got some double: " << *dp << "\n"; } int main() { auto const_dp = std::make_shared<const double>(42.0); auto non_const_dp = std::make_shared<double>(13.0); const_func(const_dp); const_func(non_const_dp); // Can't do this // non_const_func(const_dp); // // because: // $ g++ -std=c++17 a.cpp // a.cpp: In function ‘int main()’: // a.cpp:23:18: error: could not convert ‘const_dp’ from ‘shared_ptr<const double>’ to ‘shared_ptr<double>’ // 23 | non_const_func(const_dp); // | ^~~~~~~~ // | | // | shared_ptr<const double> // non_const_func(non_const_dp); return 0; }

sloretz · 2022-12-02T23:11:38Z

fuse_core/include/fuse_core/transaction_deserializer.hpp

-  fuse_core::Transaction deserialize(const fuse_msgs::msg::SerializedTransaction::SharedPtr msg)
-  const;
+  inline fuse_core::Transaction::UniquePtr deserialize(
+    const fuse_msgs::msg::SerializedTransaction::SharedPtr msg) const


same comment about ConstSharedPtr

sloretz · 2022-12-02T23:27:45Z

@ros-pull-request-builder retest this please

sloretz

LGTM if the Rpr job comes back green

methylDragon · 2022-12-03T00:14:08Z

@ros-pull-request-builder retest this please

methylDragon · 2022-12-03T05:01:05Z

Rpr is unstable due to deadlocking issues.
#294 should be merged first to fix it.

methylDragon · 2022-12-07T21:14:32Z

@ros-pull-request-builder retest this please

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

methylDragon marked this pull request as ready for review November 12, 2022 02:48

methylDragon requested a review from sloretz November 12, 2022 02:48

methylDragon force-pushed the rolling-msg-srv branch from 4475255 to 43a59d7 Compare November 12, 2022 02:49

methylDragon mentioned this pull request Nov 14, 2022

Fuse -> ROS 2 Rolling Porting Progress #276

Closed

54 tasks

svwilliams reviewed Nov 18, 2022

View reviewed changes

methylDragon force-pushed the rolling-msg-srv branch from 43a59d7 to 4dc3c6e Compare December 2, 2022 21:13

sloretz suggested changes Dec 2, 2022

View reviewed changes

methylDragon force-pushed the rolling-msg-srv branch from 4dc3c6e to 252df96 Compare December 2, 2022 21:58

methylDragon changed the title ~~fuse -> ROS 2 : Messages and Services~~ fuse -> ROS 2 fuse_core : Messages and Services Dec 2, 2022

sloretz reviewed Dec 2, 2022

View reviewed changes

methylDragon force-pushed the rolling-msg-srv branch from d01de9b to 71d6264 Compare December 2, 2022 23:25

sloretz approved these changes Dec 2, 2022

View reviewed changes

methylDragon force-pushed the rolling-msg-srv branch from a1a38ef to acc654e Compare December 3, 2022 00:14

methylDragon force-pushed the rolling-msg-srv branch 2 times, most recently from 71d6264 to 15499e1 Compare December 3, 2022 05:05

methylDragon added 6 commits December 7, 2022 13:15

Migrate msg and srv includes

1c4c07e

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

Migrate msg and srv namespaces

ab50847

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

Migrate away from ConstPtrs

f713220

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

Refine msg usage

19dbeaf

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

Fix message includes

1c16236

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

Revert all changes except fuse_core

d5952d6

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

methylDragon force-pushed the rolling-msg-srv branch from 15499e1 to d5952d6 Compare December 7, 2022 21:16

methylDragon merged commit 4c87579 into locusrobotics:rolling Dec 7, 2022

methylDragon deleted the rolling-msg-srv branch December 7, 2022 21:35

		// TODO(CH3): Oh no, there's no equivalent rclcpp method I think...
		// Probably just wait a bit?? It's in a test anyway

fuse -> ROS 2 fuse_core : Messages and Services #285

fuse -> ROS 2 fuse_core : Messages and Services #285

Uh oh!

Conversation

methylDragon commented Nov 12, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Uh oh!

svwilliams left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

sloretz commented Nov 30, 2022

Uh oh!

sloretz commented Dec 2, 2022

Uh oh!

sloretz left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

methylDragon commented Dec 2, 2022

Uh oh!

methylDragon commented Dec 2, 2022

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

sloretz commented Dec 2, 2022

Uh oh!

sloretz left a comment

Choose a reason for hiding this comment

Uh oh!

methylDragon commented Dec 3, 2022

Uh oh!

methylDragon commented Dec 3, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

methylDragon commented Dec 7, 2022

Uh oh!

Reviewers

Assignees

Labels

Milestone

Development

Uh oh!

3 participants

methylDragon commented Nov 12, 2022 •

edited

Loading

methylDragon commented Dec 3, 2022 •

edited

Loading