Skip to content
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.

Sign up for GitHub

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 : Port Time #283

Merged
merged 21 commits into from
Nov 11, 2022
Merged

fuse -> ROS 2 : Port Time #283

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
21 commits
Select commit Hold shift + click to select a range
9443258
Implement new TimeStamp class
methylDragon Oct 12, 2022
ddc12f4
Overhaul TimeStamp class to use rclcpp instead
methylDragon Oct 13, 2022
f7a384f
Port ros::Duration
methylDragon Oct 13, 2022
f017b0a
Implement waitForValid
methylDragon Oct 13, 2022
b5b4b66
Port timers
methylDragon Oct 13, 2022
73a7bbf
Port ros::Time (and now())
methylDragon Nov 3, 2022
1e21c06
Do explicit byte packing for uuids
methylDragon Oct 14, 2022
e3e8d55
Introduce and use is_valid and wait_for_valid methods
methylDragon Nov 3, 2022
710270e
Make TimeStamp obsolete
methylDragon Nov 4, 2022
8cf2f59
Update optimizers
methylDragon Nov 4, 2022
cfd4d21
Preliminarily decide to use earliest/uninitialized time
methylDragon Nov 7, 2022
1118d16
Update docs
methylDragon Nov 7, 2022
a2fe96e
Add Time::is_valid() and fix docstrings
methylDragon Nov 8, 2022
62296cf
Fix Duration usage and initializations
methylDragon Nov 9, 2022
eb1ef49
Use is_valid() for time
methylDragon Nov 9, 2022
96fabdd
Refine time comparison
methylDragon Nov 9, 2022
ad6056c
Use node timer and fix timer logic
methylDragon Nov 9, 2022
9156f25
Migrate graph std::chrono to rclcpp::Duration
methylDragon Nov 10, 2022
bdedc95
Remove TIME_ZERO
methylDragon Nov 10, 2022
c18049b
Refine clock usage in tutorials
methylDragon Nov 10, 2022
ad1b1a3
More clock changes
methylDragon Nov 11, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 12 additions & 11 deletions doc/Variables.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -156,13 +156,13 @@ class Position2DStamped : public fuse_core::Variable
private:
std::array<double, 2> data_;
fuse_core::UUID device_id_;
ros::Time stamp_;
rclcpp::Time stamp_;
fuse_core::UUID uuid_;

public:
SMART_PTR_DEFINITIONS(Position2DStamped);

Position2DStamped(const ros::Time& stamp, const fuse_core::UUID& device_id) :
Position2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) :
data{},
device_id_(device_id),
stamp_(stamp),
Expand Down Expand Up @@ -193,8 +193,8 @@ public:
return Position2DStamped::make_unique(*this);
}

const ros::Time& deviceId() const { return device_id_; }
const ros::Time& stamp() const { return stamp_; }
const rclcpp::Time& deviceId() const { return device_id_; }
const rclcpp::Time& stamp() const { return stamp_; }

double& x() { return data_[0]; }
const double& x() const { return data_[0]; }
Expand Down Expand Up @@ -223,14 +223,15 @@ private:

Our Variable also needs to hold the identity information. For this Variable we want to support both multi-robot
scenarios as well as time-varying processes, so we need some sort of "robot id" and a timestamp. Since this is a
ROS library, we will use a `ros::Time` to hold the timestamp. And we will choose a `fuse_core::UUID` to act as a
generic "robot id". fuse ships with several functions for converting strings and other types into a UUID
([UUID functions](../fuse_core/include/fuse_core/uuid.h)), so this choice should support most use-cases.
ROS library, we will use a `rclcpp::Time` (which subclasses `rclcpp::Time`) to hold the timestamp.
And we will choose a `fuse_core::UUID` to act as a generic "robot id". fuse ships with several functions
for converting strings and other types into a UUID ([UUID functions](../fuse_core/include/fuse_core/uuid.h)),
so this choice should support most use-cases.

```C++
private:
fuse_core::UUID device_id_;
ros::Time stamp_;
rclcpp::Time stamp_;
```

fuse expects the identity portion of the Variable to be distilled into a `fuse_core::UUID` value. We choose to make
Expand All @@ -247,7 +248,7 @@ construction, these values cannot be changed.

```C++
public:
Position2DStamped(const ros::Time& stamp, const fuse_core::UUID& device_id) :
Position2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) :
data{},
device_id_(device_id),
stamp_(stamp),
Expand Down Expand Up @@ -357,8 +358,8 @@ but it is very strongly recommended. This creates some standard aliases for vari
e.g.`Position2dStamped::make_shared()` and `Position2dStamped::make_unique()`.

```C++
const ros::Time& deviceId() const { return device_id_; }
const ros::Time& stamp() const { return stamp_; }
const rclcpp::Time& deviceId() const { return device_id_; }
const rclcpp::Time& stamp() const { return stamp_; }
```

Providing accessor functions for the identity information, not just the identity UUID, is also recommended.
Expand Down
30 changes: 15 additions & 15 deletions fuse_constraints/test/test_absolute_constraint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ TEST(AbsoluteConstraint, Constructor)
{
// Construct a constraint for every type, just to make sure they compile.
{
fuse_variables::AccelerationAngular2DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("robby"));
fuse_variables::AccelerationAngular2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby"));
fuse_core::Vector1d mean;
mean << 3.0;
fuse_core::Matrix1d cov;
Expand All @@ -66,7 +66,7 @@ TEST(AbsoluteConstraint, Constructor)
fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint constraint("test", variable, mean, cov));
}
{
fuse_variables::AccelerationLinear2DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("bender"));
fuse_variables::AccelerationLinear2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("bender"));
fuse_core::Vector2d mean;
mean << 1.0, 2.0;
fuse_core::Matrix2d cov;
Expand All @@ -75,7 +75,7 @@ TEST(AbsoluteConstraint, Constructor)
fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint("test", variable, mean, cov));
}
{
fuse_variables::Orientation2DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("johnny5"));
fuse_variables::Orientation2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("johnny5"));
fuse_core::Vector1d mean;
mean << 3.0;
fuse_core::Matrix1d cov;
Expand All @@ -84,7 +84,7 @@ TEST(AbsoluteConstraint, Constructor)
fuse_constraints::AbsoluteOrientation2DStampedConstraint constraint("test", variable, mean, cov));
}
{
fuse_variables::Position2DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("rosie"));
fuse_variables::Position2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("rosie"));
fuse_core::Vector2d mean;
mean << 1.0, 2.0;
fuse_core::Matrix2d cov;
Expand All @@ -93,7 +93,7 @@ TEST(AbsoluteConstraint, Constructor)
fuse_constraints::AbsolutePosition2DStampedConstraint constraint("test", variable, mean, cov));
}
{
fuse_variables::Position3DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("clank"));
fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("clank"));
fuse_core::Vector3d mean;
mean << 1.0, 2.0, 3.0;
fuse_core::Matrix3d cov;
Expand All @@ -102,7 +102,7 @@ TEST(AbsoluteConstraint, Constructor)
fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov));
}
{
fuse_variables::VelocityAngular2DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("gort"));
fuse_variables::VelocityAngular2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("gort"));
fuse_core::Vector1d mean;
mean << 3.0;
fuse_core::Matrix1d cov;
Expand All @@ -111,7 +111,7 @@ TEST(AbsoluteConstraint, Constructor)
fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint constraint("test", variable, mean, cov));
}
{
fuse_variables::VelocityLinear2DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("bishop"));
fuse_variables::VelocityLinear2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("bishop"));
fuse_core::Vector2d mean;
mean << 1.0, 2.0;
fuse_core::Matrix2d cov;
Expand All @@ -123,7 +123,7 @@ TEST(AbsoluteConstraint, Constructor)

TEST(AbsoluteConstraint, PartialMeasurement)
{
fuse_variables::Position3DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("vici"));
fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("vici"));
fuse_core::Vector2d mean;
mean << 3.0, 1.0;
fuse_core::Matrix2d cov;
Expand All @@ -138,7 +138,7 @@ TEST(AbsoluteConstraint, Covariance)
// Test the covariance of a full measurement
{
// Verify the covariance <--> sqrt information conversions are correct
fuse_variables::AccelerationLinear2DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("chappie"));
fuse_variables::AccelerationLinear2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("chappie"));
fuse_core::Vector2d mean;
mean << 1.0, 2.0;
fuse_core::Matrix2d cov;
Expand All @@ -155,7 +155,7 @@ TEST(AbsoluteConstraint, Covariance)
}
// Test the covariance of a partial measurement
{
fuse_variables::Position3DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("astroboy"));
fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("astroboy"));
fuse_core::Vector2d mean;
mean << 3.0, 1.0;
fuse_core::Matrix2d cov;
Expand Down Expand Up @@ -184,7 +184,7 @@ TEST(AbsoluteConstraint, Optimization)
// Optimize a single variable and single constraint, verify the expected value and covariance are generated.
// Create a variable
auto variable = fuse_variables::AccelerationLinear2DStamped::make_shared(
ros::Time(1234, 5678),
rclcpp::Time(1234, 5678),
fuse_core::uuid::generate("t800"));
variable->x() = 10.7;
variable->y() = -3.2;
Expand Down Expand Up @@ -237,7 +237,7 @@ TEST(AbsoluteConstraint, Optimization)
// Optimize a single variable with a full measurement and a partial measurement
// Verify the expected value and covariance are generated.
// Create a variable
auto var = fuse_variables::Position3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("t1000"));
auto var = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("t1000"));
var->x() = 10.7;
var->y() = -3.2;
var->z() = 0.9;
Expand Down Expand Up @@ -311,7 +311,7 @@ TEST(AbsoluteConstraint, Optimization)
TEST(AbsoluteConstraint, PartialOptimization)
{
// Create a variable
auto var = fuse_variables::Position3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("t1000"));
auto var = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("t1000"));
var->x() = 10.7;
var->y() = -3.2;
var->z() = 0.9;
Expand Down Expand Up @@ -376,7 +376,7 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization)
// Optimize a single variable and single constraint, verify the expected value and covariance are generated.
// Create a variable
auto variable = fuse_variables::Orientation2DStamped::make_shared(
ros::Time(1234, 5678),
rclcpp::Time(1234, 5678),
fuse_core::uuid::generate("tiktok"));
variable->yaw() = 0.7;
// Create an absolute constraint
Expand Down Expand Up @@ -424,7 +424,7 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization)
TEST(AbsoluteConstraint, Serialization)
{
// Construct a constraint
fuse_variables::AccelerationAngular2DStamped variable(ros::Time(1234, 5678), fuse_core::uuid::generate("robby"));
fuse_variables::AccelerationAngular2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby"));
fuse_core::Vector1d mean;
mean << 3.0;
fuse_core::Matrix1d cov;
Expand Down
8 changes: 4 additions & 4 deletions fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ using fuse_variables::Orientation3DStamped;
TEST(AbsoluteOrientation3DStampedConstraint, Constructor)
{
// Construct a constraint just to make sure it compiles.
Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle"));
fuse_core::Vector4d mean;
mean << 1.0, 0.0, 0.0, 0.0;
fuse_core::Matrix3d cov;
Expand All @@ -74,7 +74,7 @@ TEST(AbsoluteOrientation3DStampedConstraint, Constructor)
TEST(AbsoluteOrientation3DStampedConstraint, Covariance)
{
// Verify the covariance <--> sqrt information conversions are correct
Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo"));
Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo"));
fuse_core::Vector4d mean;
mean << 1.0, 0.0, 0.0, 0.0;
fuse_core::Matrix3d cov;
Expand All @@ -97,7 +97,7 @@ TEST(AbsoluteOrientation3DStampedConstraint, Optimization)
{
// Optimize a single pose and single constraint, verify the expected value and covariance are generated.
// Create the variables
auto orientation_variable = Orientation3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra"));
auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra"));
orientation_variable->w() = 0.952;
orientation_variable->x() = 0.038;
orientation_variable->y() = -0.189;
Expand Down Expand Up @@ -167,7 +167,7 @@ TEST(AbsoluteOrientation3DStampedConstraint, Optimization)
TEST(AbsoluteOrientation3DStampedConstraint, Serialization)
{
// Construct a constraint
Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle"));
fuse_core::Vector4d mean;
mean << 1.0, 0.0, 0.0, 0.0;
fuse_core::Matrix3d cov;
Expand Down
10 changes: 5 additions & 5 deletions fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ using fuse_variables::Orientation3DStamped;
TEST(AbsoluteOrientation3DStampedEulerConstraint, Constructor)
{
// Construct a constraint just to make sure it compiles.
Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle"));
fuse_core::Vector3d mean;
mean << 1.0, 2.0, 3.0;
fuse_core::Matrix3d cov;
Expand All @@ -68,7 +68,7 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, Constructor)
TEST(AbsoluteOrientation3DStampedEulerConstraint, Covariance)
{
// Verify the covariance <--> sqrt information conversions are correct
Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo"));
Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo"));
fuse_core::Vector3d mean;
mean << 1.0, 2.0, 3.0;
fuse_core::Matrix3d cov;
Expand All @@ -93,7 +93,7 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull)
{
// Optimize a single pose and single constraint, verify the expected value and covariance are generated.
// Create the variables
auto orientation_variable = Orientation3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra"));
auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra"));
orientation_variable->w() = 0.952;
orientation_variable->x() = 0.038;
orientation_variable->y() = -0.189;
Expand Down Expand Up @@ -150,7 +150,7 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial)
{
// Optimize a single pose and single constraint, verify the expected value and covariance are generated.
// Create the variables
auto orientation_variable = Orientation3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra"));
auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra"));
orientation_variable->w() = 0.952;
orientation_variable->x() = 0.038;
orientation_variable->y() = -0.189;
Expand Down Expand Up @@ -223,7 +223,7 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial)
TEST(AbsoluteOrientation3DStampedEulerConstraint, Serialization)
{
// Construct a constraint
Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle"));
fuse_core::Vector3d mean;
mean << 1.0, 2.0, 3.0;
fuse_core::Matrix3d cov;
Expand Down
20 changes: 10 additions & 10 deletions fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,8 +55,8 @@ using fuse_constraints::AbsolutePose2DStampedConstraint;
TEST(AbsolutePose2DStampedConstraint, Constructor)
{
// Construct a constraint just to make sure it compiles.
Orientation2DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
Position2DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle"));
Position2DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle"));
fuse_core::Vector3d mean;
mean << 1.0, 2.0, 3.0;
fuse_core::Matrix3d cov;
Expand All @@ -68,8 +68,8 @@ TEST(AbsolutePose2DStampedConstraint, Constructor)
TEST(AbsolutePose2DStampedConstraint, Covariance)
{
// Verify the covariance <--> sqrt information conversions are correct
Orientation2DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo"));
Position2DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo"));
Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo"));
Position2DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo"));
fuse_core::Vector3d mean;
mean << 1.0, 2.0, 3.0;
fuse_core::Matrix3d cov;
Expand All @@ -90,9 +90,9 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull)
{
// Optimize a single pose and single constraint, verify the expected value and covariance are generated.
// Create the variables
auto orientation_variable = Orientation2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra"));
auto orientation_variable = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra"));
orientation_variable->yaw() = 0.8;
auto position_variable = Position2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra"));
auto position_variable = Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra"));
position_variable->x() = 1.5;
position_variable->y() = -3.0;
// Create an absolute pose constraint
Expand Down Expand Up @@ -166,9 +166,9 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial)
{
// Optimize a single pose and single constraint, verify the expected value and covariance are generated.
// Create the variables
auto orientation_variable = Orientation2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra"));
auto orientation_variable = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra"));
orientation_variable->yaw() = 0.8;
auto position_variable = Position2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra"));
auto position_variable = Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra"));
position_variable->x() = 1.5;
position_variable->y() = -3.0;

Expand Down Expand Up @@ -281,8 +281,8 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial)
TEST(AbsolutePose2DStampedConstraint, Serialization)
{
// Construct a constraint
Orientation2DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
Position2DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle"));
Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle"));
Position2DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle"));
fuse_core::Vector3d mean;
mean << 1.0, 2.0, 3.0;
fuse_core::Matrix3d cov;
Expand Down
Loading