Skip to content

Commit

Permalink
fuse -> ROS 2 : Port Time (#283)
Browse files Browse the repository at this point in the history
  • Loading branch information
@methylDragon
methylDragon authored Nov 11, 2022
1 parent 3c488a2 commit f27a0b4
Show file tree
Hide file tree
Showing 120 changed files with 1,453 additions and 1,142 deletions.
23 changes: 12 additions & 11 deletions doc/Variables.md
View file
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
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
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
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
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

0 comments on commit f27a0b4

Please sign in to comment.