Skip to content

Commit

Permalink
Update variable scope and make gps init global
Browse files Browse the repository at this point in the history
  • Loading branch information
@JHartzer
JHartzer committed Jun 24, 2024
1 parent ecec47f commit 1b7f15d
Show file tree
Hide file tree
Showing 13 changed files with 288 additions and 177 deletions.
19 changes: 10 additions & 9 deletions config/example.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,16 @@
1.0e-5, 1.0e-5, 1.0e-5, # Angular Acceleration
]

pos_l_in_g: [0.0, 0.0, 0.0] # Local frame position in global frame
ang_l_to_g: 0.0 # Local frame heading in global frame
gps_init_type: 0 # GPS Initialization Type
# 0: CONSTANT
# 1: BASELINE_DIST
# 2: ERROR_THRESHOLD
gps_init_baseline_dist: 100.0 # Baseline distance threshold to initialize
gps_init_pos_thresh: 1.0 # Local frame position error threshold
gps_init_ang_thresh: 0.1 # Local frame heading error threshold

sim_params:
seed: 0.0 # Seed to provide to random number generator
use_seed: true # Flag to use seed (required for deterministic runs)
Expand Down Expand Up @@ -57,8 +67,6 @@
pos_errors: [1.0, 1.0, 1.0] # Error in achieving translational vectors
ang_errors: [0.1, 0.1, 0.1] # Error in achieving rotational vectors

pos_l_in_g: [0.0, 0.0, 0.0] # Local frame position in global frame
ang_l_to_g: 0.0 # Local frame heading in global frame
pos_l_in_g_err: [1.0, 1.0, 1.0] # Local frame position error
ang_l_to_g_err: 0.1 # Local frame heading error

Expand Down Expand Up @@ -198,13 +206,6 @@
pos_a_in_b: [1.0, 0, 0] # Antenna position in body frame
variance: [5.0, 5.0, 5.0] # Antenna position variance
data_log_rate: 5.0 # Data log rate
initialization_type: 0 # Initialization Type
# 0: CONSTANT
# 1: BASELINE_DIST
# 2: ERROR_THRESHOLD
init_baseline_dist: 100.0 # Baseline distance threshold to initialize
init_pos_thresh: 1.0 # Local frame position error threshold
init_ang_thresh: 0.1 # Local frame heading error threshold
is_extrinsic: false # Flag to calibrate GPS extrinsics
sim_params:
no_errors: false # Flag to disable errors in simulation
Expand Down
101 changes: 92 additions & 9 deletions src/application/ros/node/ekf_cal_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,18 +54,28 @@ using std::placeholders::_1;
EkfCalNode::EkfCalNode()
: Node("EkfCalNode")
{
// Declare Parameters
// Declare EKF Parameters
this->declare_parameter("debug_log_level", 0);
this->declare_parameter("data_logging_on", false);
this->declare_parameter("body_data_rate", 0.0);
this->declare_parameter("augmenting_type", 0);
this->declare_parameter("augmenting_time", 0.0);
this->declare_parameter("augmenting_pos_error", 0.0);
this->declare_parameter("augmenting_ang_error", 0.0);
this->declare_parameter("process_noise", 0.0);
this->declare_parameter(
"process_noise", std::vector<double>{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
this->declare_parameter("pos_l_in_g", std::vector<double>{0, 0, 0});
this->declare_parameter("ang_l_to_g", 0.0);
this->declare_parameter("init_type", 0);
this->declare_parameter("init_pos_thresh", 1.0);
this->declare_parameter("init_ang_thresh", 1.0);
this->declare_parameter("init_baseline_dist", 1.0);

// Declare Sensor Lists
this->declare_parameter("imu_list", std::vector<std::string>{});
this->declare_parameter("camera_list", std::vector<std::string>{});
this->declare_parameter("tracker_list", std::vector<std::string>{});
this->declare_parameter("fiducial_list", std::vector<std::string>{});
this->declare_parameter("gps_list", std::vector<std::string>{});

m_state_pub_timer =
Expand Down Expand Up @@ -94,12 +104,19 @@ void EkfCalNode::Initialize()
ekf_params.augmenting_pos_error = this->get_parameter("augmenting_pos_error").as_double();
ekf_params.augmenting_ang_error = this->get_parameter("augmenting_ang_error").as_double();
ekf_params.process_noise = StdToEigVec(this->get_parameter("process_noise").as_double_array());
ekf_params.pos_l_in_g = StdToEigVec(this->get_parameter("pos_l_in_g").as_double_array());
ekf_params.ang_l_to_g = this->get_parameter("ang_l_to_g").as_double();
ekf_params.gps_init_type = static_cast<GpsInitType>(this->get_parameter("init_type").as_int());
ekf_params.gps_init_baseline_dist = this->get_parameter("init_pos_thresh").as_double();
ekf_params.gps_init_pos_thresh = this->get_parameter("init_ang_thresh").as_double();
ekf_params.gps_init_ang_thresh = this->get_parameter("init_baseline_dist").as_double();
m_ekf = std::make_shared<EKF>(ekf_params);

// Load lists of sensors
m_imu_list = this->get_parameter("imu_list").as_string_array();
m_camera_list = this->get_parameter("camera_list").as_string_array();
m_tracker_list = this->get_parameter("tracker_list").as_string_array();
m_fiducial_list = this->get_parameter("fiducial_list").as_string_array();
m_gps_list = this->get_parameter("gps_list").as_string_array();
}

Expand All @@ -114,6 +131,9 @@ void EkfCalNode::DeclareSensors()
for (std::string & tracker_name : m_tracker_list) {
DeclareTrackerParameters(tracker_name);
}
for (std::string & fiducial_name : m_fiducial_list) {
DeclareFiducialParameters(fiducial_name);
}
for (std::string & gps_name : m_gps_list) {
DeclareGpsParameters(gps_name);
}
Expand Down Expand Up @@ -256,6 +276,8 @@ void EkfCalNode::DeclareCameraParameters(std::string camera_name)
this->declare_parameter(cam_prefix + ".ang_c_to_b", std::vector<double>{1, 0, 0, 0});
this->declare_parameter(cam_prefix + ".variance", std::vector<double>{1, 1, 1, 1, 1, 1});
this->declare_parameter(cam_prefix + ".tracker", "");
this->declare_parameter(cam_prefix + ".pos_stability", 1e-9);
this->declare_parameter(cam_prefix + ".ang_stability", 1e-9);
DeclareIntrinsicParameters(cam_prefix + ".intrinsics");
}

Expand Down Expand Up @@ -332,6 +354,56 @@ FeatureTracker::Parameters EkfCalNode::GetTrackerParameters(std::string tracker_
return tracker_params;
}

void EkfCalNode::DeclareFiducialParameters(std::string fid_name)
{
// Declare parameters
std::string fiducial_prefix = "tracker." + fid_name;
this->declare_parameter(fiducial_prefix + ".fiducial_type", 0);
this->declare_parameter(fiducial_prefix + ".squares_x", 0);
this->declare_parameter(fiducial_prefix + ".squares_y", 0);
this->declare_parameter(fiducial_prefix + ".square_length", 0.0);
this->declare_parameter(fiducial_prefix + ".marker_length", 0.0);
this->declare_parameter(fiducial_prefix + ".pos_f_in_l", std::vector<double>{0, 0, 0});
this->declare_parameter(fiducial_prefix + ".ang_f_to_l", 0.0);
this->declare_parameter(fiducial_prefix + ".variance", std::vector<double>{0, 0, 0, 0, 0, 0});
this->declare_parameter(fiducial_prefix + ".min_track_length", 2);
this->declare_parameter(fiducial_prefix + ".max_track_length", 20);
this->declare_parameter(fiducial_prefix + ".data_log_rate", 0.0);
}

FiducialTracker::Parameters EkfCalNode::GetFiducialParameters(std::string fiducial_name)
{
// Get parameters
std::string fiducial_prefix = "fiducial." + fiducial_name;
auto fiducial_type = this->get_parameter(fiducial_prefix + ".fiducial_type").as_int();
auto squares_x = this->get_parameter(fiducial_prefix + ".squares_x").as_int();
auto squares_y = this->get_parameter(fiducial_prefix + ".squares_y").as_int();
auto square_length = this->get_parameter(fiducial_prefix + ".square_length").as_double();
auto marker_length = this->get_parameter(fiducial_prefix + ".marker_length").as_double();
auto pos_f_in_l = this->get_parameter(fiducial_prefix + ".pos_f_in_l").as_double_array();
auto ang_f_to_l = this->get_parameter(fiducial_prefix + ".ang_f_to_l").as_double_array();
auto variance = this->get_parameter(fiducial_prefix + ".variance").as_double_array();
auto min_track_length = this->get_parameter(fiducial_prefix + ".min_track_length").as_int();
auto max_track_length = this->get_parameter(fiducial_prefix + ".max_track_length").as_int();
auto data_log_rate = this->get_parameter(fiducial_prefix + ".data_log_rate").as_double();

FiducialTracker::Parameters fiducial_params;
fiducial_params.detector_type = static_cast<FiducialType>(fiducial_type);
fiducial_params.squares_x = squares_x;
fiducial_params.squares_y = squares_y;
fiducial_params.square_length = square_length;
fiducial_params.marker_length = marker_length;
fiducial_params.pos_f_in_l = StdToEigVec(pos_f_in_l);
fiducial_params.ang_f_to_l = StdToEigQuat(ang_f_to_l);
fiducial_params.variance = StdToEigVec(variance);
fiducial_params.min_track_length = min_track_length;
fiducial_params.max_track_length = max_track_length;
fiducial_params.data_log_rate = data_log_rate;
fiducial_params.ekf = m_ekf;
fiducial_params.logger = m_logger;
return fiducial_params;
}

void EkfCalNode::DeclareGpsParameters(std::string gps_name)
{
// Declare parameters
Expand All @@ -341,6 +413,7 @@ void EkfCalNode::DeclareGpsParameters(std::string gps_name)
this->declare_parameter(gps_prefix + ".pos_a_in_b", std::vector<double>{0, 0, 0});
this->declare_parameter(gps_prefix + ".pos_l_in_g", std::vector<double>{0, 0, 0});
this->declare_parameter(gps_prefix + ".ang_l_to_g", 0.0);
this->declare_parameter(gps_prefix + ".pos_stability", 1e-9);
this->declare_parameter(gps_prefix + ".variance", std::vector<double>{1, 1, 1});
}

Expand All @@ -356,6 +429,7 @@ GPS::Parameters EkfCalNode::GetGpsParameters(std::string gps_name)
this->get_parameter(gps_prefix + ".pos_l_in_g").as_double_array();
double ang_l_to_g = this->get_parameter(gps_prefix + ".ang_l_to_g").as_double();
std::vector<double> variance = this->get_parameter(gps_prefix + ".variance").as_double_array();
double pos_stability = this->get_parameter(gps_prefix + ".pos_stability").as_double();
GPS::Parameters gps_params;
gps_params.name = gps_name;
gps_params.topic = topic;
Expand All @@ -364,6 +438,7 @@ GPS::Parameters EkfCalNode::GetGpsParameters(std::string gps_name)
gps_params.pos_l_in_g = StdToEigVec(pos_l_in_g);
gps_params.ang_l_to_g = ang_l_to_g;
gps_params.variance = StdToEigVec(variance);
gps_params.pos_stability = pos_stability;
gps_params.ekf = m_ekf;
gps_params.logger = m_logger;
return gps_params;
Expand Down Expand Up @@ -397,17 +472,25 @@ void EkfCalNode::RegisterImu(std::shared_ptr<RosIMU> imu_ptr, std::string topic)

void EkfCalNode::LoadCamera(std::string camera_name)
{
// Load parameters
// Load camera parameters
Camera::Parameters camera_params = GetCameraParameters(camera_name);
FeatureTracker::Parameters tParams = GetTrackerParameters(camera_params.tracker);
std::shared_ptr<RosCamera> camera_ptr = std::make_shared<RosCamera>(camera_params);
m_logger->Log(LogLevel::INFO, "Loaded Camera: " + camera_name);

// Create new RosCamera and bind callback to ID
std::shared_ptr<RosCamera> camera_ptr = std::make_shared<RosCamera>(camera_params);
tParams.camera_id = camera_ptr->GetId();
std::shared_ptr<FeatureTracker> trkPtr = std::make_shared<FeatureTracker>(tParams);
camera_ptr->AddTracker(trkPtr);
if (!camera_params.tracker.empty()) {
FeatureTracker::Parameters trk_params = GetTrackerParameters(camera_params.tracker);
trk_params.camera_id = camera_ptr->GetId();
std::shared_ptr<FeatureTracker> trk_ptr = std::make_shared<FeatureTracker>(trk_params);
camera_ptr->AddTracker(trk_ptr);
}
if (!camera_params.fiducial.empty()) {
FiducialTracker::Parameters fid_params = GetFiducialParameters(camera_params.fiducial);
fid_params.camera_id = camera_ptr->GetId();
std::shared_ptr<FiducialTracker> fid_ptr = std::make_shared<FiducialTracker>(fid_params);
camera_ptr->AddFiducial(fid_ptr);
}

// Create new RosCamera and bind callback to ID
RegisterCamera(camera_ptr, camera_params.topic);
}

Expand Down
16 changes: 15 additions & 1 deletion src/application/ros/node/ekf_cal_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,6 +136,12 @@ class EkfCalNode : public rclcpp::Node
///
void DeclareIntrinsicParameters(std::string intrinsics_prefix);

///
/// @brief Declare fiducial parameters
/// @param fid_name Fiducial name
///
void DeclareFiducialParameters(std::string fid_name);

///
/// @brief Load camera intrinsic parameters
/// @param intrinsics_prefix Camera intrinsic prefix
Expand All @@ -151,10 +157,17 @@ class EkfCalNode : public rclcpp::Node
///
/// @brief Load tracker parameters
/// @param tracker_name Name of parameter structure
/// @return trackerParameters
/// @return Tracker parameters
///
FeatureTracker::Parameters GetTrackerParameters(std::string tracker_name);

///
/// @brief Load fiducial parameters
/// @param fiducial_name Name of parameter structure
/// @return Fiducial parameters
///
FiducialTracker::Parameters GetFiducialParameters(std::string fiducial_name);

///
/// @brief Callback method for IMU sensor messages
/// @param msg Sensor message pointer
Expand Down Expand Up @@ -215,6 +228,7 @@ class EkfCalNode : public rclcpp::Node
std::vector<std::string> m_imu_list {};
std::vector<std::string> m_camera_list {};
std::vector<std::string> m_tracker_list {};
std::vector<std::string> m_fiducial_list {};
std::vector<std::string> m_gps_list {};

rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr m_img_publisher;
Expand Down
58 changes: 29 additions & 29 deletions src/application/ros/node/test/ekf_cal_node_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,72 +48,72 @@ TEST_F(EkfCalNode_test, hello_world)

node.set_parameter(rclcpp::Parameter("debug_log_level", 1));
node.set_parameter(rclcpp::Parameter("data_logging_on", true));
node.set_parameter(rclcpp::Parameter("imu_list", std::vector<std::string>{"TestImu"}));
node.set_parameter(rclcpp::Parameter("camera_list", std::vector<std::string>{"TestCamera"}));
node.set_parameter(rclcpp::Parameter("tracker_list", std::vector<std::string>{"TestTracker"}));
node.set_parameter(rclcpp::Parameter("gps_list", std::vector<std::string>{"TestGps"}));
node.set_parameter(rclcpp::Parameter("imu_list", std::vector<std::string>{"imu_1"}));
node.set_parameter(rclcpp::Parameter("camera_list", std::vector<std::string>{"cam_2"}));
node.set_parameter(rclcpp::Parameter("tracker_list", std::vector<std::string>{"tracker_3"}));
node.set_parameter(rclcpp::Parameter("gps_list", std::vector<std::string>{"gps_4"}));

node.Initialize();
node.DeclareSensors();

node.set_parameter(rclcpp::Parameter("imu.TestImu.is_extrinsic", false));
node.set_parameter(rclcpp::Parameter("imu.TestImu.is_intrinsic", false));
node.set_parameter(rclcpp::Parameter("imu.TestImu.rate", 400.0));
node.set_parameter(rclcpp::Parameter("imu.TestImu.topic", "/ImuTopic"));
node.set_parameter(rclcpp::Parameter("imu.imu_1.is_extrinsic", false));
node.set_parameter(rclcpp::Parameter("imu.imu_1.is_intrinsic", false));
node.set_parameter(rclcpp::Parameter("imu.imu_1.rate", 400.0));
node.set_parameter(rclcpp::Parameter("imu.imu_1.topic", "/ImuTopic"));
node.set_parameter(
rclcpp::Parameter(
"imu.TestImu.variance",
"imu.imu_1.variance",
std::vector<double>{0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01}));
node.set_parameter(
rclcpp::Parameter(
"imu.TestImu.pos_i_in_b",
"imu.imu_1.pos_i_in_b",
std::vector<double>{0.0, 0.0, 0.0}));
node.set_parameter(
rclcpp::Parameter(
"imu.TestImu.ang_i_to_b",
"imu.imu_1.ang_i_to_b",
std::vector<double>{1.0, 0.0, 0.0, 0.0}));
node.set_parameter(
rclcpp::Parameter(
"imu.TestImu.acc_bias", std::vector<double>{0.0, 0.0,
"imu.imu_1.acc_bias", std::vector<double>{0.0, 0.0,
0.0}));
node.set_parameter(
rclcpp::Parameter(
"imu.TestImu.omg_bias", std::vector<double>{0.0, 0.0,
"imu.imu_1.omg_bias", std::vector<double>{0.0, 0.0,
0.0}));

node.set_parameter(rclcpp::Parameter("camera.TestCamera.rate", 5.0));
node.set_parameter(rclcpp::Parameter("camera.TestCamera.topic", "/CameraTopic"));
node.set_parameter(rclcpp::Parameter("camera.cam_2.rate", 5.0));
node.set_parameter(rclcpp::Parameter("camera.cam_2.topic", "/CameraTopic"));
node.set_parameter(
rclcpp::Parameter(
"camera.TestCamera.pos_c_in_b",
"camera.cam_2.pos_c_in_b",
std::vector<double>{0.0, 0.0, 0.0}));
node.set_parameter(
rclcpp::Parameter(
"camera.TestCamera.ang_c_to_b",
"camera.cam_2.ang_c_to_b",
std::vector<double>{1.0, 0.0, 0.0, 0.0}));
node.set_parameter(
rclcpp::Parameter(
"camera.TestCamera.variance",
"camera.cam_2.variance",
std::vector<double>{0.1, 0.1, 0.1, 0.1, 0.1, 0.1}));
node.set_parameter(rclcpp::Parameter("camera.TestCamera.tracker", "TestTracker"));
node.set_parameter(rclcpp::Parameter("camera.cam_2.tracker", "tracker_3"));

node.set_parameter(rclcpp::Parameter("tracker.TestTracker.feature_detector", 4));
node.set_parameter(rclcpp::Parameter("tracker.TestTracker.descriptor_extractor", 0));
node.set_parameter(rclcpp::Parameter("tracker.TestTracker.descriptor_matcher", 0));
node.set_parameter(rclcpp::Parameter("tracker.TestTracker.detector_threshold", 10.0));
node.set_parameter(rclcpp::Parameter("tracker.tracker_3.feature_detector", 4));
node.set_parameter(rclcpp::Parameter("tracker.tracker_3.descriptor_extractor", 0));
node.set_parameter(rclcpp::Parameter("tracker.tracker_3.descriptor_matcher", 0));
node.set_parameter(rclcpp::Parameter("tracker.tracker_3.detector_threshold", 10.0));

node.set_parameter(rclcpp::Parameter("gps.TestGps.topic", "/gps1"));
node.set_parameter(rclcpp::Parameter("gps.TestGps.rate", 10.0));
node.set_parameter(rclcpp::Parameter("gps.TestGps.variance", std::vector<double>{1.0, 1.0, 1.0}));
node.set_parameter(rclcpp::Parameter("gps.gps_4.topic", "/gps1"));
node.set_parameter(rclcpp::Parameter("gps.gps_4.rate", 10.0));
node.set_parameter(rclcpp::Parameter("gps.gps_4.variance", std::vector<double>{1.0, 1.0, 1.0}));
node.set_parameter(
rclcpp::Parameter(
"gps.TestGps.pos_a_in_b",
"gps.gps_4.pos_a_in_b",
std::vector<double>{0.0, 0.0, 0.0}));
node.set_parameter(
rclcpp::Parameter(
"gps.TestGps.pos_l_in_g",
"gps.gps_4.pos_l_in_g",
std::vector<double>{0.0, 0.0, 0.0}));
node.set_parameter(rclcpp::Parameter("gps.TestGps.ang_l_to_g", 0.0));
node.set_parameter(rclcpp::Parameter("gps.gps_4.ang_l_to_g", 0.0));

node.LoadSensors();
auto imu_msg = std::make_shared<sensor_msgs::msg::Imu>();
Expand Down
Loading

0 comments on commit 1b7f15d

Please sign in to comment.