Skip to content

Commit

Permalink
unicycle motion model: revert previous changes which created a separa…
Browse files Browse the repository at this point in the history 
…te static function Unicycle2D::generateMotionModel and make changes to unit test to test that now again non-static protected member function which needs setting private member variables before
  • Loading branch information
@fabianhirmann
fabianhirmann committed May 16, 2023
1 parent 7f8b97a commit 0c2c81e
Show file tree
Hide file tree
Showing 3 changed files with 71 additions and 106 deletions.
39 changes: 1 addition & 38 deletions fuse_models/include/fuse_models/unicycle_2d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,26 +123,6 @@ class Unicycle2D : public fuse_core::AsyncMotionModel
*/
bool applyCallback(fuse_core::Transaction& transaction) override;

/**
* @brief Generator function for providing to the \c TimestampManager to create a single motion model segment
* between the specified timestamps.
*
* This function is effectively using \c generateMotionModel.
*
* @param[in] beginning_stamp The beginning timestamp of the motion model constraints to be generated.
* \p beginning_stamp is guaranteed to be less than \p ending_stamp.
* @param[in] ending_stamp The ending timestamp of the motion model constraints to be generated.
* \p ending_stamp is guaranteed to be greater than \p beginning_stamp.
* @param[out] constraints One or more motion model constraints between the requested timestamps.
* @param[out] variables One or more variables at both the \p beginning_stamp and \p ending_stamp. The
* variables should include initial values for the optimizer.
*/
void generateMotionModelGenerator(
const ros::Time& beginning_stamp,
const ros::Time& ending_stamp,
std::vector<fuse_core::Constraint::SharedPtr>& constraints,
std::vector<fuse_core::Variable::SharedPtr>& variables);

/**
* @brief Generate a single motion model segment between the specified timestamps.
*
Expand All @@ -153,30 +133,13 @@ class Unicycle2D : public fuse_core::AsyncMotionModel
* \p beginning_stamp is guaranteed to be less than \p ending_stamp.
* @param[in] ending_stamp The ending timestamp of the motion model constraints to be generated.
* \p ending_stamp is guaranteed to be greater than \p beginning_stamp.
* @param[in] state_history The state history object to be updated
* @param[in] device_id The UUID of the device to be published
* @param[in] name The unique name for this motion model instance
* @param[in] process_noise_covariance Process noise covariance matrix
* @param[in] scale_process_noise Whether to scale the process noise covariance pose by the norm
of the current state twist.
* @param[in] velocity_norm_min The minimum velocity/twist norm allowed when scaling the
process noise covariance
* @param[in] disable_checks Whether to disable the validation checks for the current and predicted state,
including the process noise covariance after it is scaled and multiplied by dt.
* @param[out] constraints One or more motion model constraints between the requested timestamps.
* @param[out] variables One or more variables at both the \p beginning_stamp and \p ending_stamp. The
* variables should include initial values for the optimizer.
*/
static void generateMotionModel(
void generateMotionModel(
const ros::Time& beginning_stamp,
const ros::Time& ending_stamp,
StateHistory& state_history,
const fuse_core::UUID& device_id,
const std::string name,
fuse_core::Matrix8d process_noise_covariance,
const bool scale_process_noise,
const double velocity_norm_min,
const bool disable_checks,
std::vector<fuse_core::Constraint::SharedPtr>& constraints,
std::vector<fuse_core::Variable::SharedPtr>& variables);

Expand Down
78 changes: 30 additions & 48 deletions fuse_models/src/unicycle_2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,7 @@ Unicycle2D::Unicycle2D() :
fuse_core::AsyncMotionModel(1),
buffer_length_(ros::DURATION_MAX),
device_id_(fuse_core::uuid::NIL),
timestamp_manager_(&Unicycle2D::generateMotionModelGenerator, this, ros::DURATION_MAX)
timestamp_manager_(&Unicycle2D::generateMotionModel, this, ros::DURATION_MAX)
{
}

Expand Down Expand Up @@ -204,9 +204,7 @@ void Unicycle2D::onInit()
process_noise_covariance_ = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal();

private_node_handle_.param("scale_process_noise", scale_process_noise_, scale_process_noise_);

private_node_handle_.param("velocity_norm_min", velocity_norm_min_, velocity_norm_min_);

private_node_handle_.param("disable_checks", disable_checks_, disable_checks_);

double buffer_length = 3.0;
Expand All @@ -229,41 +227,23 @@ void Unicycle2D::onStart()
state_history_.clear();
}

void Unicycle2D::generateMotionModelGenerator(
const ros::Time& beginning_stamp,
const ros::Time& ending_stamp,
std::vector<fuse_core::Constraint::SharedPtr>& constraints,
std::vector<fuse_core::Variable::SharedPtr>& variables)
{
generateMotionModel(beginning_stamp, ending_stamp, state_history_, device_id_, name(),
process_noise_covariance_, scale_process_noise_, velocity_norm_min_, disable_checks_,
constraints, variables);
}

void Unicycle2D::generateMotionModel(
const ros::Time& beginning_stamp,
const ros::Time& ending_stamp,
StateHistory& state_history,
const fuse_core::UUID& device_id,
const std::string name,
fuse_core::Matrix8d process_noise_covariance,
const bool scale_process_noise,
const double velocity_norm_min,
const bool disable_checks,
std::vector<fuse_core::Constraint::SharedPtr>& constraints,
std::vector<fuse_core::Variable::SharedPtr>& variables)
{
assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history.empty()));
assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history_.empty()));

StateHistoryElement base_state;
ros::Time base_time;

// Find an entry that is > beginning_stamp
// The entry that is <= will be the one before it
auto base_state_pair_it = state_history.upper_bound(beginning_stamp);
if (base_state_pair_it == state_history.begin())
auto base_state_pair_it = state_history_.upper_bound(beginning_stamp);
if (base_state_pair_it == state_history_.begin())
{
ROS_WARN_STREAM_COND_NAMED(!state_history.empty(), "UnicycleModel", "Unable to locate a state in this history "
ROS_WARN_STREAM_COND_NAMED(!state_history_.empty(), "UnicycleModel", "Unable to locate a state in this history "
"with stamp <= " << beginning_stamp << ". Variables will all be initialized to 0.");
base_time = beginning_stamp;
}
Expand Down Expand Up @@ -300,13 +280,13 @@ void Unicycle2D::generateMotionModel(

if (dt == 0.0)
{
state1.position_uuid = fuse_variables::Position2DStamped(beginning_stamp, device_id).uuid();
state1.yaw_uuid = fuse_variables::Orientation2DStamped(beginning_stamp, device_id).uuid();
state1.vel_linear_uuid = fuse_variables::VelocityLinear2DStamped(beginning_stamp, device_id).uuid();
state1.vel_yaw_uuid = fuse_variables::VelocityAngular2DStamped(beginning_stamp, device_id).uuid();
state1.acc_linear_uuid = fuse_variables::AccelerationLinear2DStamped(beginning_stamp, device_id).uuid();
state1.position_uuid = fuse_variables::Position2DStamped(beginning_stamp, device_id_).uuid();
state1.yaw_uuid = fuse_variables::Orientation2DStamped(beginning_stamp, device_id_).uuid();
state1.vel_linear_uuid = fuse_variables::VelocityLinear2DStamped(beginning_stamp, device_id_).uuid();
state1.vel_yaw_uuid = fuse_variables::VelocityAngular2DStamped(beginning_stamp, device_id_).uuid();
state1.acc_linear_uuid = fuse_variables::AccelerationLinear2DStamped(beginning_stamp, device_id_).uuid();

state_history.emplace(beginning_stamp, std::move(state1));
state_history_.emplace(beginning_stamp, std::move(state1));

return;
}
Expand All @@ -325,16 +305,16 @@ void Unicycle2D::generateMotionModel(
state2.acceleration_linear);

// Define the fuse variables required for this constraint
auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id);
auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(beginning_stamp, device_id);
auto velocity_linear1 = fuse_variables::VelocityLinear2DStamped::make_shared(beginning_stamp, device_id);
auto velocity_yaw1 = fuse_variables::VelocityAngular2DStamped::make_shared(beginning_stamp, device_id);
auto acceleration_linear1 = fuse_variables::AccelerationLinear2DStamped::make_shared(beginning_stamp, device_id);
auto position2 = fuse_variables::Position2DStamped::make_shared(ending_stamp, device_id);
auto yaw2 = fuse_variables::Orientation2DStamped::make_shared(ending_stamp, device_id);
auto velocity_linear2 = fuse_variables::VelocityLinear2DStamped::make_shared(ending_stamp, device_id);
auto velocity_yaw2 = fuse_variables::VelocityAngular2DStamped::make_shared(ending_stamp, device_id);
auto acceleration_linear2 = fuse_variables::AccelerationLinear2DStamped::make_shared(ending_stamp, device_id);
auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id_);
auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(beginning_stamp, device_id_);
auto velocity_linear1 = fuse_variables::VelocityLinear2DStamped::make_shared(beginning_stamp, device_id_);
auto velocity_yaw1 = fuse_variables::VelocityAngular2DStamped::make_shared(beginning_stamp, device_id_);
auto acceleration_linear1 = fuse_variables::AccelerationLinear2DStamped::make_shared(beginning_stamp, device_id_);
auto position2 = fuse_variables::Position2DStamped::make_shared(ending_stamp, device_id_);
auto yaw2 = fuse_variables::Orientation2DStamped::make_shared(ending_stamp, device_id_);
auto velocity_linear2 = fuse_variables::VelocityLinear2DStamped::make_shared(ending_stamp, device_id_);
auto velocity_yaw2 = fuse_variables::VelocityAngular2DStamped::make_shared(ending_stamp, device_id_);
auto acceleration_linear2 = fuse_variables::AccelerationLinear2DStamped::make_shared(ending_stamp, device_id_);

position1->data()[fuse_variables::Position2DStamped::X] = state1.pose.x();
position1->data()[fuse_variables::Position2DStamped::Y] = state1.pose.y();
Expand Down Expand Up @@ -364,8 +344,10 @@ void Unicycle2D::generateMotionModel(
state2.vel_yaw_uuid = velocity_yaw2->uuid();
state2.acc_linear_uuid = acceleration_linear2->uuid();

state_history.emplace(beginning_stamp, std::move(state1));
state_history.emplace(ending_stamp, std::move(state2));
state_history_.emplace(beginning_stamp, std::move(state1));
state_history_.emplace(ending_stamp, std::move(state2));

auto process_noise_covariance = process_noise_covariance_;

// Rotate the process noise covariance with the yaw angle of the current state orientation
const auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix();
Expand All @@ -375,31 +357,31 @@ void Unicycle2D::generateMotionModel(
rotation_matrix * process_noise_covariance.topLeftCorner<2, 2>() * rotation_matrix.transpose();

// Scale process noise covariance pose by the norm of the current state twist
if (scale_process_noise)
if (scale_process_noise_)
{
common::scaleProcessNoiseCovariance(process_noise_covariance, state1.velocity_linear, state1.velocity_yaw,
velocity_norm_min);
velocity_norm_min_);
}

// Validate
process_noise_covariance *= dt;

if (!disable_checks)
if (!disable_checks_)
{
try
{
validateMotionModel(state1, state2, process_noise_covariance);
}
catch (const std::runtime_error& ex)
{
ROS_ERROR_STREAM_THROTTLE(10.0, "Invalid '" << name << "' motion model: " << ex.what());
ROS_ERROR_STREAM_THROTTLE(10.0, "Invalid '" << name() << "' motion model: " << ex.what());
return;
}
}

// Create the constraints for this motion model segment
auto constraint = fuse_models::Unicycle2DStateKinematicConstraint::make_shared(
name,
name(),
*position1,
*yaw1,
*velocity_linear1,
Expand Down
60 changes: 40 additions & 20 deletions fuse_models/test/test_unicycle_2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,6 @@
***************************************************************************/
#include <fuse_models/unicycle_2d.h>

#include <string>
#include <vector>

#include <fuse_models/unicycle_2d_state_kinematic_constraint.h>
#include <fuse_graphs/hash_graph.h>
#include <fuse_variables/acceleration_linear_2d_stamped.h>
Expand All @@ -21,17 +18,40 @@
#include <gtest/gtest.h>
#include <fuse_core/eigen_gtest.h>

#include <string>
#include <vector>

/**
* @brief Derived class used in unit tests to expose protected functions
* @brief Derived class used in unit tests to expose protected/private functions and variables
*/
class Unicycle2DModelTest : public fuse_models::Unicycle2D
{
public:
using fuse_models::Unicycle2D::updateStateHistoryEstimates;
using fuse_models::Unicycle2D::StateHistoryElement;
using fuse_models::Unicycle2D::StateHistory;

using fuse_models::Unicycle2D::generateMotionModel;

void setProcessNoiseCovariance(const fuse_core::Matrix8d& process_noise_covariance)
{
process_noise_covariance_ = process_noise_covariance;
}

void setStateHistory(const StateHistory& state_history)
{
state_history_ = state_history;
}

void setScaleProcessNoise(const bool& scale_process_noise)
{
scale_process_noise_ = scale_process_noise;
}

void setDisableChecks(const bool& disable_checks)
{
disable_checks_ = disable_checks;
}

};

TEST(Unicycle2D, UpdateStateHistoryEstimates)
Expand Down Expand Up @@ -331,29 +351,23 @@ TEST(Unicycle2D, generateMotionModel)
yaw_velocity1->yaw(),
tf2_2d::Vector2(linear_acceleration1->x(), linear_acceleration1->y())}); // NOLINT(whitespace/braces)

const fuse_core::UUID device_id{{0}};
const std::string name = "";
Unicycle2DModelTest unicycle_2d_model_test;

unicycle_2d_model_test.setStateHistory(state_history);

std::vector<double> process_noise_diagonal{1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};
fuse_core::Matrix8d process_noise_covariance = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal();
unicycle_2d_model_test.setProcessNoiseCovariance(fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal());

unicycle_2d_model_test.setScaleProcessNoise(false);
unicycle_2d_model_test.setDisableChecks(false);

const bool scale_process_noise = false;
const double velocity_norm_min = 0.0;
const bool disable_checks = false;
std::vector<fuse_core::Constraint::SharedPtr> constraints;
std::vector<fuse_core::Variable::SharedPtr> variables;

// Generate the motion model
Unicycle2DModelTest::generateMotionModel(
unicycle_2d_model_test.generateMotionModel(
beginning_stamp,
ending_stamp,
state_history,
device_id,
name,
process_noise_covariance,
scale_process_noise,
velocity_norm_min,
disable_checks,
constraints,
variables);

Expand Down Expand Up @@ -440,5 +454,11 @@ TEST(Unicycle2D, generateMotionModel)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
ros::init(argc, argv, "unicycle_2d_test");
auto spinner = ros::AsyncSpinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

0 comments on commit 0c2c81e

Please sign in to comment.