Skip to content

Commit 4a5f22d

Browse files
Chris-BeeChris-Bee
committed
Merge branch 'feat/new_buffer_structure' into development
2 parents 5d386f2 + 03205e7 commit 4a5f22d

35 files changed

+1973
-1012
lines changed

‎source/mars/include/mars/buffer.h

+19-31
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ class Buffer
3838
/// \brief Buffer default constructor
3939
/// max buffer size is set to 400 by default
4040
///
41-
Buffer() = default;
41+
Buffer();
4242

4343
///
4444
/// \brief Buffer constructor
@@ -79,7 +79,10 @@ class Buffer
7979
/// \brief get_length
8080
/// \return current number of elements stored in the buffer
8181
///
82-
int get_length() const;
82+
inline int get_length() const
83+
{
84+
return static_cast<int>(data_.size());
85+
}
8386

8487
///
8588
/// \brief PrintBufferEntries prints all buffer entries in a formatted way
@@ -199,7 +202,10 @@ class Buffer
199202
/// \return Index of the added entry. The index is -1 if the entry was removed because the max_buffer_size was
200203
/// reached.
201204
///
202-
int AddEntrySorted(const BufferEntryType& new_entry);
205+
/// The method finds the closest timestamp based on the shortest 'time' distance between the new and existing buffer
206+
/// elements. It then determines if the entry needs to be added before or after the closest entry.
207+
///
208+
int AddEntrySorted(const BufferEntryType& new_entry, const bool& after = true);
203209

204210
///
205211
/// \brief FindClosestTimestamp Returns the index of the entry which is the closest to the specified 'timestamp' when
@@ -211,44 +217,25 @@ class Buffer
211217

212218
///
213219
/// \brief Deletes all states after, and including the given index
220+
/// This also deletes all auto generated states
221+
///
214222
/// \param idx Start index after which all states are deleted
215223
/// \return true if function was performed correct, false otherwise
216224
///
217-
bool DeleteStatesStartingAtIdx(const int& idx);
225+
bool ClearStatesStartingAtIdx(const int& idx);
218226

219227
///
220228
/// \brief Checks if all buffer entrys are correctly sorted by time
221229
/// \return true if sorted, false otherwise
222230
///
223231
bool IsSorted() const;
224232

225-
///
226-
/// \brief Inserting new element before the element at the specified position
227-
/// \param new_entry ntry that is added to the buffer
228-
///
229-
/// The method finds the closest timestamp based on the shortest 'time' distance between the new and existing buffer
230-
/// elements. It then determines if the entry needs to be added before or after the closest entry.
231-
///
232-
int InsertDataAtTimestamp(const BufferEntryType& new_entry);
233-
234233
///
235234
/// \brief InsertDataAtIndex Adds 'entry' at buffer position 'index'
236235
/// \param new_entry Entry buffer entry to be added
237236
/// \param index position at which the entry is added
238237
///
239-
bool InsertDataAtIndex(const BufferEntryType& new_entry, const int& index);
240-
241-
///
242-
/// \brief InsertIntermediateData Insert data during the intermediate propagation step
243-
///
244-
/// This function is intended for inserting a auto generated measurement and state before a current measurement entry.
245-
/// This is required for intermediate propagation information.
246-
///
247-
/// \param measurement
248-
/// \param state
249-
/// \return
250-
///
251-
bool InsertIntermediateData(const BufferEntryType& measurement, const BufferEntryType& state);
238+
bool OverwriteDataAtIndex(const BufferEntryType& new_entry, const int& index);
252239

253240
///
254241
/// \brief get_latest_interm_entrie Get last state pair of imu prop and sensor update
@@ -261,14 +248,15 @@ class Buffer
261248
///
262249
/// \return True if successfull, false of no pair was found
263250
///
264-
bool get_intermediate_entry_pair(const std::shared_ptr<SensorAbsClass>& sensor_handle, BufferEntryType* imu_state, BufferEntryType* sensor_state) const;
251+
bool get_intermediate_entry_pair(const std::shared_ptr<SensorAbsClass>& sensor_handle, BufferEntryType* imu_state,
252+
BufferEntryType* sensor_state) const;
265253

266254
///
267-
/// \brief CheckForLastHandle Checks if the given sensor handle only exists once in the buffer
268-
/// \param sensor_handle
269-
/// \return true if current sensor handle is the last in the buffer, false otherwise
255+
/// \brief CheckForLastSensorHandleWithState Checks if the given sensor handle only exists once in the buffer and if
256+
/// it has a state \param sensor_handle \return true if current sensor handle is the last in the buffer, false
257+
/// otherwise
270258
///
271-
bool CheckForLastSensorHandlePair(const std::shared_ptr<SensorAbsClass>& sensor_handle) const;
259+
bool CheckForLastSensorHandleWithState(const std::shared_ptr<SensorAbsClass>& sensor_handle) const;
272260

273261
///
274262
/// \brief RemoveOverflowEntrys Removes the oldest entries if max buffer size is reached

‎source/mars/include/mars/core_logic.h

+9-7
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,8 @@ class CoreLogic
3030
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
3131

3232
std::shared_ptr<CoreState> core_states_{ nullptr }; /// Holds a pointer to the core_states
33-
Buffer buffer_{ 300 }; /// Main buffer of the filter
34-
Buffer buffer_prior_core_init_{ 100 }; /// Buffer that holds measurements prior initialization
33+
Buffer buffer_; /// Main buffer of the filter
34+
Buffer buffer_prior_core_init_; /// Buffer that holds measurements prior initialization
3535
SensorManager sensor_manager_;
3636
bool core_is_initialized_{ false }; /// core_is_initialized_ = true if the core state was initialized, false
3737
/// otherwise
@@ -72,8 +72,10 @@ class CoreLogic
7272
///
7373
/// \brief PerformSensorUpdate Returns new state with corrected state and updated covariance
7474
///
75-
bool PerformSensorUpdate(BufferEntryType* state_buffer_entry_return, std::shared_ptr<SensorAbsClass> sensor,
76-
const Time& timestamp, std::shared_ptr<BufferDataType> data);
75+
bool PerformSensorUpdate(std::shared_ptr<SensorAbsClass> sensor, const Time& timestamp, BufferEntryType* sensor_data);
76+
bool PerformSensorUpdate(std::shared_ptr<SensorAbsClass> sensor, const Time& timestamp, BufferEntryType* sensor_data,
77+
bool* added_interm_state);
78+
7779
///
7880
/// \brief PerformCoreStatePropagation Propagates the core state and returns the new state entry
7981
///
@@ -84,9 +86,9 @@ class CoreLogic
8486
/// The core_state propagation function needs to be able to
8587
/// handle the data structure of the propagation sensor.
8688
///
87-
BufferEntryType PerformCoreStatePropagation(std::shared_ptr<SensorAbsClass> sensor, const Time& timestamp,
88-
const std::shared_ptr<BufferDataType>& data_measurement,
89-
const std::shared_ptr<BufferEntryType>& prior_state_entry);
89+
void PerformCoreStatePropagation(std::shared_ptr<SensorAbsClass> sensor, const Time& timestamp,
90+
const BufferEntryType& prior_state_entry, BufferEntryType* sensor_entry);
91+
9092
///
9193
/// \brief ReworkBufferStartingAtIndex Reprocesses the buffer after an out of order update,
9294
/// starting at given 'idx'

‎source/mars/include/mars/data_utils/read_baro_data.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -45,9 +45,9 @@ class ReadBarometerData
4545
double pressure(csv_data["p"][k]);
4646

4747
BufferDataType data;
48-
data.set_sensor_data(std::make_shared<PressureMeasurementType>(pressure, temperature));
48+
data.set_measurement(std::make_shared<PressureMeasurementType>(pressure, temperature));
4949

50-
BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
50+
BufferEntryType current_entry(time, data, sensor);
5151
data_out->at(k) = current_entry;
5252
}
5353
}

‎source/mars/include/mars/data_utils/read_csv.h

+16-11
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,9 @@ class ReadCsv
3030
{
3131
char delim{ ',' };
3232
HeaderMapType header_map;
33+
3334
public:
34-
ReadCsv(CsvDataType* csv_data, const std::string& file_path, char delim_=',') : delim(delim_)
35+
ReadCsv(CsvDataType* csv_data, const std::string& file_path, char delim_ = ',') : delim(delim_)
3536
{
3637
if (!mars::filesystem::IsFile(file_path))
3738
{
@@ -62,42 +63,45 @@ class ReadCsv
6263
}
6364

6465
CsvDataType csv_data_int;
65-
for(auto it=header_map.begin(); it != header_map.end(); it++) {
66-
csv_data_int[it->second].resize(rows-1, 0.0);
66+
for (auto it = header_map.begin(); it != header_map.end(); it++)
67+
{
68+
csv_data_int[it->second].resize(rows - 1, 0.0);
6769
}
6870

6971
// Read columns associated to header tokens
7072
std::string line;
7173
int line_counter = 0;
72-
int parsed_row_counter = first_value_row; // header already parsed.
74+
int parsed_row_counter = first_value_row; // header already parsed.
7375

7476
while (std::getline(file_, line))
7577
{
76-
7778
std::stringstream row_stream(line);
7879
std::string token;
7980
int column_counter = 0;
8081

8182
double item;
8283
while (std::getline(row_stream, token, delim))
8384
{
84-
if(column_counter >= (int)header_map.size()) {
85+
if (column_counter >= (int)header_map.size())
86+
{
8587
std::cout << "ReadCsv(): Warning: too many entries in row!" << std::endl;
86-
++column_counter; // to indicate a corrupted row
88+
++column_counter; // to indicate a corrupted row
8789
break;
8890
}
8991

9092
std::istringstream is(token);
9193
is >> item;
92-
csv_data_int[header_map[column_counter]][line_counter]=(item);
94+
csv_data_int[header_map[column_counter]][line_counter] = (item);
9395
++column_counter;
9496
}
9597

9698
// check if row was corrupted, if so, overwrite current line with the next one
97-
if(column_counter != (int)header_map.size()) {
99+
if (column_counter != (int)header_map.size())
100+
{
98101
std::cout << "ReadCsv(): Warning: corrupted row=" << parsed_row_counter << " will be skipped!" << std::endl;
99102
}
100-
else {
103+
else
104+
{
101105
line_counter++;
102106
}
103107

@@ -106,7 +110,8 @@ class ReadCsv
106110
}
107111

108112
// shrink to the actual size
109-
for(auto it=header_map.begin(); it != header_map.end(); it++) {
113+
for (auto it = header_map.begin(); it != header_map.end(); it++)
114+
{
110115
csv_data_int[it->second].resize(line_counter);
111116
}
112117

‎source/mars/include/mars/data_utils/read_gps_data.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -42,10 +42,10 @@ class ReadGpsData
4242
Time time = csv_data["t"][k] + time_offset;
4343

4444
BufferDataType data;
45-
data.set_sensor_data(
45+
data.set_measurement(
4646
std::make_shared<GpsMeasurementType>(csv_data["lat"][k], csv_data["long"][k], csv_data["alt"][k]));
4747

48-
BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
48+
BufferEntryType current_entry(time, data, sensor);
4949
data_out->at(k) = current_entry;
5050
}
5151
}

‎source/mars/include/mars/data_utils/read_gps_w_vel_data.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -42,11 +42,11 @@ class ReadGpsWithVelData
4242
Time time = csv_data["t"][k] + time_offset;
4343

4444
BufferDataType data;
45-
data.set_sensor_data(std::make_shared<GpsVelMeasurementType>(csv_data["lat"][k], csv_data["long"][k],
45+
data.set_measurement(std::make_shared<GpsVelMeasurementType>(csv_data["lat"][k], csv_data["long"][k],
4646
csv_data["alt"][k], csv_data["v_x"][k],
4747
csv_data["v_y"][k], csv_data["v_z"][k]));
4848

49-
BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
49+
BufferEntryType current_entry(time, data, sensor);
5050
data_out->at(k) = current_entry;
5151
}
5252
}

‎source/mars/include/mars/data_utils/read_imu_data.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -45,9 +45,9 @@ class ReadImuData
4545
Eigen::Vector3d angular_velocity(csv_data["w_x"][k], csv_data["w_y"][k], csv_data["w_z"][k]);
4646

4747
BufferDataType data;
48-
data.set_sensor_data(std::make_shared<IMUMeasurementType>(linear_acceleration, angular_velocity));
48+
data.set_measurement(std::make_shared<IMUMeasurementType>(linear_acceleration, angular_velocity));
4949

50-
BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
50+
BufferEntryType current_entry(time, data, sensor);
5151
data_out->at(k) = current_entry;
5252
}
5353
}

‎source/mars/include/mars/data_utils/read_mag_data.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -45,9 +45,9 @@ class ReadMagData
4545
Eigen::Vector3d mag_vec(csv_data["cart_x"][k], csv_data["cart_y"][k], csv_data["cart_z"][k]);
4646

4747
BufferDataType data;
48-
data.set_sensor_data(std::make_shared<MagMeasurementType>(mag_vec));
48+
data.set_measurement(std::make_shared<MagMeasurementType>(mag_vec));
4949

50-
BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
50+
BufferEntryType current_entry(time, data, sensor);
5151
data_out->at(k) = current_entry;
5252
}
5353
}

‎source/mars/include/mars/data_utils/read_pose_data.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -47,9 +47,9 @@ class ReadPoseData
4747
orientation = Utils::NormalizeQuaternion(orientation, "pose csv reader");
4848

4949
BufferDataType data;
50-
data.set_sensor_data(std::make_shared<PoseMeasurementType>(position, orientation));
50+
data.set_measurement(std::make_shared<PoseMeasurementType>(position, orientation));
5151

52-
BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
52+
BufferEntryType current_entry(time, data, sensor);
5353
data_out->at(k) = current_entry;
5454
}
5555
}

‎source/mars/include/mars/data_utils/read_position_data.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -43,9 +43,9 @@ class ReadPositionData
4343
Eigen::Vector3d position(csv_data["p_x"][k], csv_data["p_y"][k], csv_data["p_z"][k]);
4444

4545
BufferDataType data;
46-
data.set_sensor_data(std::make_shared<PositionMeasurementType>(position));
46+
data.set_measurement(std::make_shared<PositionMeasurementType>(position));
4747

48-
BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
48+
BufferEntryType current_entry(time, data, sensor);
4949
data_out->at(k) = current_entry;
5050
}
5151
}

‎source/mars/include/mars/data_utils/read_sim_data.h

+4-3
Original file line numberDiff line numberDiff line change
@@ -65,10 +65,11 @@ class ReadSimData
6565
core_ground_truth.b_a_ = bAcc;
6666

6767
BufferDataType data;
68-
data.set_core_data(std::make_shared<CoreStateType>(core_ground_truth));
69-
data.set_sensor_data(std::make_shared<IMUMeasurementType>(a_imu, w_imu));
68+
// TODO
69+
// data.set_core_state(std::make_shared<CoreStateType>(core_ground_truth));
70+
data.set_measurement(std::make_shared<IMUMeasurementType>(a_imu, w_imu));
7071

71-
BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
72+
BufferEntryType current_entry(time, data, sensor);
7273
data_out->at(k) = current_entry;
7374
}
7475
}

‎source/mars/include/mars/data_utils/read_velocity_data.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -43,9 +43,9 @@ class ReadVelocityData
4343
Eigen::Vector3d velocity(csv_data["v_x"][k], csv_data["v_y"][k], csv_data["v_z"][k]);
4444

4545
BufferDataType data;
46-
data.set_sensor_data(std::make_shared<VelocityMeasurementType>(velocity));
46+
data.set_measurement(std::make_shared<VelocityMeasurementType>(velocity));
4747

48-
BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
48+
BufferEntryType current_entry(time, data, sensor);
4949
data_out->at(k) = current_entry;
5050
}
5151
}

‎source/mars/include/mars/data_utils/read_vision_data.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -46,9 +46,9 @@ class ReadVisionData
4646
orientation = Utils::NormalizeQuaternion(orientation, "vision csv reader");
4747

4848
BufferDataType data;
49-
data.set_sensor_data(std::make_shared<VisionMeasurementType>(position, orientation));
49+
data.set_measurement(std::make_shared<VisionMeasurementType>(position, orientation));
5050

51-
BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
51+
BufferEntryType current_entry(time, data, sensor);
5252
data_out->at(k) = current_entry;
5353
}
5454
}

‎source/mars/include/mars/data_utils/write_csv.h

-1
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,6 @@ class WriteCsv
2323
inline static std::string vec_to_csv(const Eigen::VectorXd& a)
2424
{
2525
std::stringstream os;
26-
// TODO(chb) option to pre or post add comma
2726
for (int k = 0; k < a.size(); k++)
2827
{
2928
os << ", " << a(k);

‎source/mars/include/mars/ekf.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -80,7 +80,7 @@ class Chi2
8080
int dof_{ 3 }; /// Degrees of freedom for the setup
8181
double chi_value_{ 0.05 }; /// Chi value for the confidence intervall (0.05 represents 95% test)
8282
double ucv_; /// Upper critival value
83-
bool do_test_{ false }; /// Determine if the test is performed or not
83+
bool do_test_{ true }; /// Determine if the test is performed or not
8484
bool passed_{ false }; /// Shows if the test passed or not (true=passed)
8585

8686
private:

‎source/mars/include/mars/sensors/gps/gps_utils.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,7 @@ mars::GpsCoordinates mars::GPSInit::get_gps_mean(const std::shared_ptr<mars::Sen
6969
{
7070
if ((cur_time - (*it)->timestamp_).get_seconds() <= init_duration_)
7171
{
72-
const GpsCoordinates meas = (*static_cast<GpsMeasurementType*>((*it)->data_.sensor_.get())).coordinates_;
72+
const GpsCoordinates meas = (*static_cast<GpsMeasurementType*>((*it)->data_.measurement_.get())).coordinates_;
7373

7474
avg_ref += meas;
7575
cnt_meas++;

‎source/mars/include/mars/sensors/pressure/pressure_utils.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ mars::Pressure mars::PressureInit::get_press_mean(const std::shared_ptr<mars::Se
6060
{
6161
if ((cur_time - (*it)->timestamp_).get_seconds() <= init_duration_)
6262
{
63-
const PressureMeasurementType meas = *static_cast<PressureMeasurementType*>((*it)->data_.sensor_.get());
63+
const PressureMeasurementType meas = *static_cast<PressureMeasurementType*>((*it)->data_.sensor_state_.get());
6464

6565
avg_pressure += meas.pressure_;
6666
cnt_meas++;

‎source/mars/include/mars/sensors/update_sensor_abs_class.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,6 @@ class UpdateSensorAbsClass : public SensorAbsClass
4141

4242
std::shared_ptr<CoreState> core_states_;
4343
};
44-
}; // namespace mars
44+
} // namespace mars
4545

4646
#endif // UPDATE_SENSOR_ABS_CLASS_H

0 commit comments

Comments
 (0)