-
Notifications
You must be signed in to change notification settings - Fork 114
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
fuse -> ROS 2 fuse_core : Messages and Services #285
fuse -> ROS 2 fuse_core : Messages and Services #285
Conversation
4475255
to
43a59d7
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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?
@@ -36,7 +36,7 @@ | |||
#include <fuse_core/eigen_gtest.h> | |||
#include <fuse_core/uuid.h> | |||
#include <fuse_variables/orientation_3d_stamped.h> | |||
#include <geometry_msgs/Quaternion.h> | |||
#include <geometry_msgs/msg/Quaternion.hpp> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If the message isn't used here, feel free to drop the include
fuse_core::Transaction& transaction); | ||
|
||
ParameterType params_; | ||
|
||
geometry_msgs::PoseWithCovarianceStamped::ConstPtr previous_pose_msg_; | ||
geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr previous_pose_msg_; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If process()
is not receiving a shared ptr, there's no reason for the previous pose to be a shared ptr either.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
}
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
(I will convert the constsharedptr to unique_ptr, though)
// TODO(CH3): Oh no, there's no equivalent rclcpp method I think... | ||
// Probably just wait a bit?? It's in a test anyway |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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) 😬
@@ -117,26 +117,26 @@ std::vector<Beacon> createNoisyBeacons(const std::vector<Beacon>& beacons) | |||
/** | |||
* @brief Convert the set of beacons into a pointcloud for visualization purposes | |||
*/ | |||
sensor_msgs::PointCloud2::ConstPtr beaconsToPointcloud( | |||
sensor_msgs::msg::PointCloud2::ConstSharedPtr beaconsToPointcloud( |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
*/ | ||
void keepCallback(const geometry_msgs::Point::ConstPtr& msg) | ||
// TODO(CH3): I'm iffy on this... | ||
void keepCallback(const geometry_msgs::msg::Point::ConstSharedPtr msg) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The callback signature for the ThrottledMessageCallback is void(const typename M&)
. So shouldn't this be void keepCallback(const geometry_msgs::msg::Point& msg)
?
@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 |
43a59d7
to
4dc3c6e
Compare
@ros-pull-request-builder retest this please! |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
@@ -40,8 +40,8 @@ | |||
#include <fuse_core/serialization.hpp> | |||
#include <fuse_core/uuid.hpp> | |||
#include <fuse_variables/orientation_3d_stamped.h> | |||
#include <geometry_msgs/PoseWithCovariance.h> | |||
#include <geometry_msgs/Quaternion.h> | |||
#include <geometry_msgs/msg/PoseWithCovariance.hpp> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
It also looks like PoseWithCovariance
is unused in this file, so this include could be removed.
4dc3c6e
to
252df96
Compare
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.. |
@ros-pull-request-builder retest this please |
@@ -74,7 +74,7 @@ class GraphDeserializer | |||
* @return A unique_ptr to a derived Graph object | |||
*/ | |||
inline fuse_core::Graph::UniquePtr deserialize( | |||
const fuse_msgs::msg::SerializedGraph::ConstSharedPtr msg) const | |||
const fuse_msgs::msg::SerializedGraph::SharedPtr msg) const |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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;
}
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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
same comment about ConstSharedPtr
d01de9b
to
71d6264
Compare
@ros-pull-request-builder retest this please |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
LGTM if the Rpr job comes back green
a1a38ef
to
acc654e
Compare
@ros-pull-request-builder retest this please |
Rpr is unstable due to deadlocking issues. |
71d6264
to
15499e1
Compare
@ros-pull-request-builder retest this please |
Signed-off-by: methylDragon <methylDragon@gmail.com>
Signed-off-by: methylDragon <methylDragon@gmail.com>
Signed-off-by: methylDragon <methylDragon@gmail.com>
Signed-off-by: methylDragon <methylDragon@gmail.com>
Signed-off-by: methylDragon <methylDragon@gmail.com>
Signed-off-by: methylDragon <methylDragon@gmail.com>
15499e1
to
d5952d6
Compare
See: #276
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.