Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

buffer queues added #12

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

buffer queues added #12

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,3 +30,5 @@
*.exe
*.out
*.app

dataset
3 changes: 1 addition & 2 deletions include/rio/common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -133,9 +133,8 @@ struct CfarDetection {
}
};

std::vector<CfarDetection> parseRadarMsg(
std::vector<CfarDetection> parseRadarMsgRioDataset(
const sensor_msgs::PointCloud2Ptr& msg);

double computeBaroHeight(double pressure);

} // namespace rio
7 changes: 7 additions & 0 deletions include/rio/rio.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,8 @@ class Rio {
void imuFilterCallback(const sensor_msgs::ImuConstPtr& msg);
void cfarDetectionsCallback(const sensor_msgs::PointCloud2Ptr& msg);
void pressureCallback(const sensor_msgs::FluidPressurePtr& msg);
void processIMUMeasurements(const sensor_msgs::ImuConstPtr& msg);
void processRadarFrames();

ros::Publisher odom_navigation_pub_;
ros::Publisher odom_optimizer_pub_;
Expand Down Expand Up @@ -107,5 +109,10 @@ class Rio {
bool baro_active_{false};
std::optional<double> baro_height_bias_;
std::deque<std::pair<double, double>> baro_height_bias_history_;

std::deque<sensor_msgs::ImuConstPtr> imu_queue;
std::deque<sensor_msgs::PointCloud2Ptr> radar_queue;

std::string dataset_ = "rio";
};
} // namespace rio
2 changes: 1 addition & 1 deletion src/common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -213,7 +213,7 @@ bool rio::loadNoiseRadarTrack(const ros::NodeHandle& nh,
return true;
}

std::vector<CfarDetection> rio::parseRadarMsg(
std::vector<CfarDetection> rio::parseRadarMsgRioDataset(
const sensor_msgs::PointCloud2Ptr& msg) {
std::vector<CfarDetection> detections(msg->height * msg->width);
sensor_msgs::PointCloud2Iterator<float> iter_x(*msg, "x");
Expand Down
60 changes: 49 additions & 11 deletions src/rio.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -184,6 +184,12 @@ void Rio::imuRawCallback(const sensor_msgs::ImuConstPtr& msg) {
return;
}

imu_queue.push_back(msg);

if (!radar_queue.empty()) processRadarFrames();
}

void Rio::processIMUMeasurements(const sensor_msgs::ImuConstPtr& msg) {
// Get update from optimization.
std::map<std::string, Timing> timing;
auto new_result = optimization_.getResult(&propagation_, &timing);
Expand Down Expand Up @@ -219,6 +225,7 @@ void Rio::imuRawCallback(const sensor_msgs::ImuConstPtr& msg) {
}
}

// TODO: Not sure this will work
void Rio::imuFilterCallback(const sensor_msgs::ImuConstPtr& msg) {
LOG_FIRST(I, 1, "Received first filtered IMU message.");
Eigen::Quaterniond q_IB;
Expand All @@ -236,17 +243,32 @@ void Rio::imuFilterCallback(const sensor_msgs::ImuConstPtr& msg) {
}
}

void Rio::cfarDetectionsCallback(const sensor_msgs::PointCloud2Ptr& msg) {
void Rio::processRadarFrames() {
sensor_msgs::PointCloud2Ptr radar_msg = radar_queue.front();

if (imu_queue.empty() || radar_queue.empty()) return;

LOG_FIRST(I, 1, "Received first CFAR detections.");
if (propagation_.empty()) {
LOG(W, "No propagation, skipping CFAR detections.");
return;
}

if (imu_queue.back()->header.stamp < radar_msg->header.stamp) {
return;
}

radar_queue.pop_front();

while (!imu_queue.empty()) {
processIMUMeasurements(imu_queue.front());
imu_queue.pop_front();
}

Pose3 B_T_BR;
try {
auto R_T_RB_tf = tf_buffer_.lookupTransform(
msg->header.frame_id,
radar_msg->header.frame_id,
propagation_.back().getLatestState()->imu->header.frame_id,
ros::Time(0), ros::Duration(0.0));
Eigen::Affine3d R_T_RB_eigen = tf2::transformToEigen(R_T_RB_tf.transform);
Expand All @@ -260,17 +282,22 @@ void Rio::cfarDetectionsCallback(const sensor_msgs::PointCloud2Ptr& msg) {
return;
}

auto split_it = splitPropagation(msg->header.stamp);
auto split_it = splitPropagation(radar_msg->header.stamp);
if (split_it == propagation_.end()) {
LOG(W, "Failed to split propagation, skipping CFAR detections.");
LOG(W, "Split time: " << msg->header.stamp);
LOG(W, "Split time: " << radar_msg->header.stamp);
LOG(W, "Last IMU time: "
<< propagation_.back().getLatestState()->imu->header.stamp);
imu_queue.clear();

return;
}

split_it->B_T_BR_ = B_T_BR;
split_it->cfar_detections_ = parseRadarMsg(msg);
split_it->cfar_detections_ = parseRadarMsgRioDataset(radar_msg);

// split_it->cfar_detections_ = parseRadarMsg(msg);

// Track zero velocity detections.
split_it->cfar_tracks_ =
tracker_.addCfarDetections(split_it->cfar_detections_.value());
Expand All @@ -284,14 +311,14 @@ void Rio::cfarDetectionsCallback(const sensor_msgs::PointCloud2Ptr& msg) {
// Find baro measurement closest to radar measurement.
auto baro_it = std::lower_bound(
baro_height_bias_history_.begin(), baro_height_bias_history_.end(),
std::make_pair(msg->header.stamp.toSec(), 0.0),
std::make_pair(radar_msg->header.stamp.toSec(), 0.0),
[](const std::pair<double, double>& a,
const std::pair<double, double>& b) { return a.first < b.first; });
if (baro_it != baro_height_bias_history_.begin() &&
baro_it != baro_height_bias_history_.end()) {
auto baro_it_prev = std::prev(baro_it);
if (std::abs(baro_it_prev->first - msg->header.stamp.toSec()) <
std::abs(baro_it->first - msg->header.stamp.toSec())) {
if (std::abs(baro_it_prev->first - radar_msg->header.stamp.toSec()) <
std::abs(baro_it->first - radar_msg->header.stamp.toSec())) {
baro_it = baro_it_prev;
}
}
Expand All @@ -305,23 +332,34 @@ void Rio::cfarDetectionsCallback(const sensor_msgs::PointCloud2Ptr& msg) {
optimization_.addBaroFactor(*split_it, noise_model_baro_height_,
&baro_residual);
DopplerResidual baro_residual_msg;
baro_residual_msg.header = msg->header;
baro_residual_msg.header = radar_msg->header;
baro_residual_msg.residual = baro_residual[0];
baro_residual_pub_.publish(baro_residual_msg);
} else {
LOG(W, "Failed to find baro measurement with stamp before or at "
<< msg->header.stamp << ". Skipping baro factor.");
<< radar_msg->header.stamp << ". Skipping baro factor.");
}
}

optimization_.solve(propagation_);

for (const auto& residual : doppler_residuals) {
DopplerResidual residual_msg;
residual_msg.header = msg->header;
residual_msg.header = radar_msg->header;
residual_msg.residual = residual[0];
doppler_residual_pub_.publish(residual_msg);
}
imu_queue.clear();
}

void Rio::cfarDetectionsCallback(const sensor_msgs::PointCloud2Ptr& msg) {
radar_queue.push_back(msg);

LOG_FIRST(I, 1, "Received first CFAR detections.");
if (propagation_.empty()) {
LOG(W, "No propagation, skipping CFAR detections.");
return;
}
}

void Rio::pressureCallback(const sensor_msgs::FluidPressurePtr& msg) {
Expand Down
2 changes: 1 addition & 1 deletion src/rio_calibration_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -170,7 +170,7 @@ int main(int argc, char** argv) {
} else if (msg.getTopic() == radar_topic) {
sensor_msgs::PointCloud2Ptr radar_msg =
msg.instantiate<sensor_msgs::PointCloud2>();
auto detections = parseRadarMsg(radar_msg);
auto detections = parseRadarMsgRioDataset(radar_msg);
RadarMeasurement radar_measurement;
radar_measurement.t = radar_msg->header.stamp.toSec();
for (const auto& detection : detections) {
Expand Down