Skip to content

Commit

Permalink
Merge branch 'maint/mars_2_0_0_api_update' into development
Browse files Browse the repository at this point in the history
  • Loading branch information
@Chris-Bee
Chris-Bee committed Aug 20, 2024
2 parents d1b152d + 9c62a40 commit 1236b27
Show file tree
Hide file tree
Showing 8 changed files with 55 additions and 55 deletions.
4 changes: 2 additions & 2 deletions include/mars_msg_conv.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,9 +208,9 @@ class MarsMsgConv
}

// add core state to path msg if vaild
if (buffer_entry.IsState())
if (buffer_entry.HasStates())
{
mars::CoreStateType buffer_core_state = static_cast<mars::CoreType*>(buffer_entry.data_.core_.get())->state_;
mars::CoreStateType buffer_core_state = static_cast<mars::CoreType*>(buffer_entry.data_.core_state_.get())->state_;
path_msg.poses.push_back(
ExtCoreStateToPoseMsg(buffer_entry.timestamp_.get_seconds(), buffer_core_state, frame_id));
}
Expand Down
2 changes: 1 addition & 1 deletion mars_lib
Submodule mars_lib updated 488 files
26 changes: 13 additions & 13 deletions src/mars_wrapper_dualpose_full.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -322,7 +322,7 @@ void MarsWrapperDualPoseFull::set_common_gps_reference(const GpsCoordinates& ref
if ((timestamp - tmp_meas.timestamp_).get_seconds() < 1.0)
{
// add measruement to average reference
GpsCoordinates tmp_coords = static_cast<GpsMeasurementType*>(tmp_meas.data_.sensor_.get())->coordinates_;
GpsCoordinates tmp_coords = static_cast<GpsMeasurementType*>(tmp_meas.data_.measurement_.get())->coordinates_;
avg_ref.altitude_ += tmp_coords.altitude_;
avg_ref.latitude_ += tmp_coords.latitude_;
avg_ref.longitude_ += tmp_coords.longitude_;
Expand Down Expand Up @@ -383,7 +383,7 @@ void MarsWrapperDualPoseFull::ImuMeasurementCallback(const sensor_msgs::ImuConst

// Generate a measurement data block
BufferDataType data;
data.set_sensor_data(std::make_shared<IMUMeasurementType>(MarsMsgConv::ImuMsgToImuMeas(*meas)));
data.set_measurement(std::make_shared<IMUMeasurementType>(MarsMsgConv::ImuMsgToImuMeas(*meas)));

// Call process measurement
core_logic_.ProcessMeasurement(imu_sensor_sptr_, timestamp, data);
Expand Down Expand Up @@ -506,7 +506,7 @@ void MarsWrapperDualPoseFull::Gps1MeasurementCallback(const sensor_msgs::NavSatF
mars::BufferEntryType latest_sensor_state;
core_logic_.buffer_.get_latest_sensor_handle_state(gps1_sensor_sptr_, &latest_sensor_state);

mars::GpsSensorStateType gps_sensor_state = gps1_sensor_sptr_.get()->get_state(latest_sensor_state.data_.sensor_);
mars::GpsSensorStateType gps_sensor_state = gps1_sensor_sptr_.get()->get_state(latest_sensor_state.data_.sensor_state_);
pub_gps1_state_.publish(MarsMsgConv::GpsStateToMsg(latest_sensor_state.timestamp_.get_seconds(), gps_sensor_state));
}
}
Expand Down Expand Up @@ -543,7 +543,7 @@ void MarsWrapperDualPoseFull::Gps1MeasurementCallback(const sensor_msgs::NavSatF
}

mars::GpsVelSensorStateType gps_sensor_state =
gps1_sensor_sptr_.get()->get_state(latest_sensor_state.data_.sensor_);
gps1_sensor_sptr_.get()->get_state(latest_sensor_state.data_.sensor_state_);

pub_gps1_state_.publish(
MarsMsgConv::GpsVelStateToMsg(latest_sensor_state.timestamp_.get_seconds(), gps_sensor_state));
Expand Down Expand Up @@ -589,7 +589,7 @@ void MarsWrapperDualPoseFull::RunCoreStatePublisher()
// only publish valid states
if (core_logic_.buffer_.get_latest_state(&latest_state))
{
mars::CoreStateType latest_core_state = static_cast<mars::CoreType*>(latest_state.data_.core_.get())->state_;
mars::CoreStateType latest_core_state = static_cast<mars::CoreType*>(latest_state.data_.core_state_.get())->state_;

pub_ext_core_state_.publish(
MarsMsgConv::ExtCoreStateToMsg(latest_state.timestamp_.get_seconds(), latest_core_state));
Expand Down Expand Up @@ -653,7 +653,7 @@ bool MarsWrapperDualPoseFull::PoseMeasurementUpdate(std::shared_ptr<mars::PoseSe
// get current core state
mars::BufferEntryType latest_state;
core_logic_.buffer_.get_latest_state(&latest_state);
mars::CoreStateType latest_core_state = static_cast<mars::CoreType*>(latest_state.data_.core_.get())->state_;
mars::CoreStateType latest_core_state = static_cast<mars::CoreType*>(latest_state.data_.core_state_.get())->state_;

// save sensor 1 to sensor 2 calibration!
/// \note NOTE(scm): variable names are wrong here, reusage of existing variables with their naming conventions,
Expand All @@ -678,7 +678,7 @@ bool MarsWrapperDualPoseFull::PoseMeasurementUpdate(std::shared_ptr<mars::PoseSe
// Generate a measurement data block
BufferDataType data;
// data.set_sensor_data(std::make_shared<PoseMeasurementType>(pose_meas));
data.set_sensor_data(std::make_shared<PoseMeasurementType>(rotated_pose_meas));
data.set_measurement(std::make_shared<PoseMeasurementType>(rotated_pose_meas));

// Call process measurement
if (!core_logic_.ProcessMeasurement(sensor_sptr, timestamp_corr, data))
Expand All @@ -692,7 +692,7 @@ bool MarsWrapperDualPoseFull::PoseMeasurementUpdate(std::shared_ptr<mars::PoseSe
// Publish the latest sensor state
mars::BufferEntryType latest_result;
core_logic_.buffer_.get_latest_sensor_handle_state(sensor_sptr, &latest_result);
mars::PoseSensorStateType pose_sensor_state = sensor_sptr.get()->get_state(latest_result.data_.sensor_);
mars::PoseSensorStateType pose_sensor_state = sensor_sptr.get()->get_state(latest_result.data_.sensor_state_);

pub_pose1_state_.publish(MarsMsgConv::PoseStateToPoseMsg(latest_result.timestamp_.get_seconds(), pose_sensor_state));

Expand All @@ -709,10 +709,10 @@ bool MarsWrapperDualPoseFull::GpsMeasurementUpdate(std::shared_ptr<mars::GpsSens

// Generate a measurement data block
BufferDataType data;
data.set_sensor_data(std::make_shared<GpsMeasurementType>(gps_meas));
data.set_measurement(std::make_shared<GpsMeasurementType>(gps_meas));

// Update init buffer
BufferEntryType gps_meas_entry(timestamp, data, gps1_sensor_sptr_, BufferMetadataType::measurement);
BufferEntryType gps_meas_entry(timestamp, data, gps1_sensor_sptr_);
gps_meas_buffer_.AddEntrySorted(gps_meas_entry);

// only continue if we have common gps ref set
Expand All @@ -738,10 +738,10 @@ bool MarsWrapperDualPoseFull::GpsVelMeasurementUpdate(std::shared_ptr<mars::GpsV

// Generate a measurement data block
BufferDataType data;
data.set_sensor_data(std::make_shared<GpsVelMeasurementType>(gps_meas));
data.set_measurement(std::make_shared<GpsVelMeasurementType>(gps_meas));

// Update init buffer
BufferEntryType gps_meas_entry(timestamp, data, gps1_sensor_sptr_, BufferMetadataType::measurement);
BufferEntryType gps_meas_entry(timestamp, data, gps1_sensor_sptr_);
gps_meas_buffer_.AddEntrySorted(gps_meas_entry);

// Call process measurement
Expand All @@ -763,7 +763,7 @@ bool MarsWrapperDualPoseFull::PressureMeasurementUpdate(std::shared_ptr<mars::Pr
{
// Generate a measurement data block
BufferDataType data;
data.set_sensor_data(std::make_shared<PressureMeasurementType>(press_meas));
data.set_measurement(std::make_shared<PressureMeasurementType>(press_meas));

// Call process measurement
if (!core_logic_.ProcessMeasurement(sensor_sptr, timestamp, data))
Expand Down
10 changes: 5 additions & 5 deletions src/mars_wrapper_gps.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -192,7 +192,7 @@ void MarsWrapperGps::ImuMeasurementCallback(const sensor_msgs::ImuConstPtr& meas

// Generate a measurement data block
BufferDataType data;
data.set_sensor_data(std::make_shared<IMUMeasurementType>(MarsMsgConv::ImuMsgToImuMeas(*meas)));
data.set_measurement(std::make_shared<IMUMeasurementType>(MarsMsgConv::ImuMsgToImuMeas(*meas)));

// Call process measurement
const bool valid_update = core_logic_.ProcessMeasurement(imu_sensor_sptr_, timestamp, data);
Expand Down Expand Up @@ -245,7 +245,7 @@ void MarsWrapperGps::Gps1MeasurementCallback(const sensor_msgs::NavSatFixConstPt
return;
}

mars::GpsSensorStateType gps_sensor_state = gps1_sensor_sptr_.get()->get_state(latest_sensor_state.data_.sensor_);
mars::GpsSensorStateType gps_sensor_state = gps1_sensor_sptr_.get()->get_state(latest_sensor_state.data_.sensor_state_);

pub_gps1_state_.publish(MarsMsgConv::GpsStateToMsg(latest_sensor_state.timestamp_.get_seconds(), gps_sensor_state));
}
Expand All @@ -260,11 +260,11 @@ void MarsWrapperGps::RunCoreStatePublisher()
return;
}

mars::CoreStateType latest_core_state = static_cast<mars::CoreType*>(latest_state.data_.core_.get())->state_;
mars::CoreStateType latest_core_state = static_cast<mars::CoreType*>(latest_state.data_.core_state_.get())->state_;

if (m_sett_.pub_cov_)
{
mars::CoreStateMatrix cov = static_cast<mars::CoreType*>(latest_state.data_.core_.get())->cov_;
mars::CoreStateMatrix cov = static_cast<mars::CoreType*>(latest_state.data_.core_state_.get())->cov_;
pub_ext_core_state_.publish(
MarsMsgConv::ExtCoreStateToMsgCov(latest_state.timestamp_.get_seconds(), latest_core_state, cov));
}
Expand Down Expand Up @@ -299,7 +299,7 @@ void MarsWrapperGps::GpsMeasurementUpdate(std::shared_ptr<mars::GpsSensorClass>

// Generate a measurement data block
BufferDataType data;
data.set_sensor_data(std::make_shared<GpsMeasurementType>(gps_meas));
data.set_measurement(std::make_shared<GpsMeasurementType>(gps_meas));

// Call process measurement
if (!core_logic_.ProcessMeasurement(sensor_sptr, timestamp, data))
Expand Down
16 changes: 8 additions & 8 deletions src/mars_wrapper_gps_mag.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -232,7 +232,7 @@ void MarsWrapperGpsMag::ImuMeasurementCallback(const sensor_msgs::ImuConstPtr& m

// Generate a measurement data block
BufferDataType data;
data.set_sensor_data(std::make_shared<IMUMeasurementType>(MarsMsgConv::ImuMsgToImuMeas(*meas)));
data.set_measurement(std::make_shared<IMUMeasurementType>(MarsMsgConv::ImuMsgToImuMeas(*meas)));

// Call process measurement
const bool valid_update = core_logic_.ProcessMeasurement(imu_sensor_sptr_, timestamp, data);
Expand Down Expand Up @@ -289,7 +289,7 @@ void MarsWrapperGpsMag::Gps1MeasurementCallback(const sensor_msgs::NavSatFixCons
}

mars::GpsVelSensorStateType gps_sensor_state =
gps1_sensor_sptr_.get()->get_state(latest_sensor_state.data_.sensor_);
gps1_sensor_sptr_.get()->get_state(latest_sensor_state.data_.sensor_state_);

pub_gps1_state_.publish(
MarsMsgConv::GpsVelStateToMsg(latest_sensor_state.timestamp_.get_seconds(), gps_sensor_state));
Expand All @@ -314,7 +314,7 @@ void MarsWrapperGpsMag::Mag1MeasurementCallback(const sensor_msgs::MagneticField
};

mars::IMUMeasurementType imu_meas =
*static_cast<mars::IMUMeasurementType*>(latest_imu_meas_entry.data_.sensor_.get());
*static_cast<mars::IMUMeasurementType*>(latest_imu_meas_entry.data_.sensor_state_.get());

// Get current Magnetometer measurement
mars::MagMeasurementType mag_meas = MarsMsgConv::MagMsgToMagMeas(*meas);
Expand Down Expand Up @@ -364,7 +364,7 @@ void MarsWrapperGpsMag::Mag1MeasurementCallback(const sensor_msgs::MagneticField
return;
}

mars::MagSensorStateType mag_sensor_state = mag1_sensor_sptr_.get()->get_state(latest_sensor_state.data_.sensor_);
mars::MagSensorStateType mag_sensor_state = mag1_sensor_sptr_.get()->get_state(latest_sensor_state.data_.sensor_state_);

pub_mag1_state_.publish(MarsMsgConv::MagStateToMsg(latest_sensor_state.timestamp_.get_seconds(), mag_sensor_state));
}
Expand All @@ -380,11 +380,11 @@ void MarsWrapperGpsMag::RunCoreStatePublisher()
return;
}

mars::CoreStateType latest_core_state = static_cast<mars::CoreType*>(latest_state.data_.core_.get())->state_;
mars::CoreStateType latest_core_state = static_cast<mars::CoreType*>(latest_state.data_.core_state_.get())->state_;

if (m_sett_.pub_cov_)
{
mars::CoreStateMatrix cov = static_cast<mars::CoreType*>(latest_state.data_.core_.get())->cov_;
mars::CoreStateMatrix cov = static_cast<mars::CoreType*>(latest_state.data_.core_state_.get())->cov_;
pub_ext_core_state_.publish(
MarsMsgConv::ExtCoreStateToMsgCov(latest_state.timestamp_.get_seconds(), latest_core_state, cov));
}
Expand Down Expand Up @@ -415,7 +415,7 @@ bool MarsWrapperGpsMag::GpsVelMeasurementUpdate(std::shared_ptr<mars::GpsVelSens
{
// Generate a measurement data block
BufferDataType data;
data.set_sensor_data(std::make_shared<GpsVelMeasurementType>(gps_meas));
data.set_measurement(std::make_shared<GpsVelMeasurementType>(gps_meas));

// Call process measurement
if (!core_logic_.ProcessMeasurement(sensor_sptr, timestamp, data))
Expand All @@ -434,7 +434,7 @@ bool MarsWrapperGpsMag::MagMeasurementUpdate(std::shared_ptr<MagSensorClass> sen
{
// Generate a measurement data block
BufferDataType data;
data.set_sensor_data(std::make_shared<MagMeasurementType>(mag_meas));
data.set_measurement(std::make_shared<MagMeasurementType>(mag_meas));

// Call process measurement
if (!core_logic_.ProcessMeasurement(sensor_sptr, timestamp, data))
Expand Down
Loading

0 comments on commit 1236b27

Please sign in to comment.