Commit a312113b authored by Maurice Fallon's avatar Maurice Fallon
Browse files

write raw data to file, read raw data from file

parent 44dd6119
......@@ -39,6 +39,9 @@ struct CommandLineConfig
int reference_update_frequency;
float max_correction_magnitude;
bool verbose;
bool write_input_clouds_to_file;
bool process_input_clouds_from_file;
string process_input_clouds_folder;
};
namespace aicp {
......@@ -63,6 +66,10 @@ public:
ClassificationParams class_params_;
int online_results_line_;
void processFromFile(std::string file_path);
void processCloud(AlignedCloudPtr cloud);
// thread function doing actual work
void operator()();
......
......@@ -11,7 +11,7 @@ class TimingUtils{
~TimingUtils(){}
static void tic();
static void toc();
static void toc(std::string message = "");
static std::string currentDateTime();
static void sleepSeconds(clock_t sec);
......
......@@ -241,25 +241,65 @@ void App::runAicpPipeline(pcl::PointCloud<pcl::PointXYZ>::Ptr& reference_prefilt
computeRegistration(*reference_prefiltered, *reading_prefiltered, T);
}
void App::operator()() {
running_ = true;
while (running_) {
std::unique_lock<std::mutex> lock(worker_mutex_);
// Wait for notification from planarLidarHandler
worker_condition_.wait_for(lock, std::chrono::milliseconds(1000));
// Copy current workload from cloud queue to work queue
std::list<AlignedCloudPtr> work_queue;
{
std::unique_lock<std::mutex> lock(data_mutex_);
while (!cloud_queue_.empty()) {
work_queue.push_back(cloud_queue_.front());
cloud_queue_.pop_front();
void App::processFromFile(std::string file_path){
// Reach the input poses file:
std::stringstream ss;
ss << file_path << "/input_poses.csv";
ifstream in( ss.str() );
vector<vector<double>> fields;
if (in) {
string line;
while (getline(in, line)) {
if (line.at(0) == '#'){
continue;
}
stringstream sep(line);
string field;
fields.push_back(vector<double>());
while (getline(sep, field, ',')) {
fields.back().push_back(stod(field));
}
}
}
// Read each point cloud and consecutively feed to AICP
for (auto row : fields) {
int counter = int(row[0]);
int sec = int(row[1]);
int nsec = int(row[2]);
int64_t utime = 0;
Eigen::Isometry3d world_to_body_ = Eigen::Isometry3d::Identity();
world_to_body_.translation() << row[3],row[4],row[5];
world_to_body_.rotate( Eigen::Quaterniond(row[9],row[6],row[7],row[8]) );
std::stringstream ss2;
ss2 << file_path << "/cloud_" << counter << "_" << sec << "_" << nsec << ".pcd";
std::cout << ss2.str() << "\n";
pcl::PointCloud<pcl::PointXYZ>::Ptr accumulated_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (ss2.str(), *accumulated_cloud) == -1){
std::cout << "Couldn't read file " << ss2.str() << "\n";
return;
}
AlignedCloudPtr current_cloud (new AlignedCloud(utime,
accumulated_cloud,
world_to_body_));
processCloud(current_cloud);
}
}
void App::processCloud(AlignedCloudPtr cloud){
// Process workload
for (auto cloud : work_queue) {
// First point cloud (becomes first reference)
if(!cl_cfg_.localize_against_prior_map &&
......@@ -345,7 +385,7 @@ void App::operator()() {
aligned_clouds_graph_->getNbClouds() != 0)
{
cout << "[Main] -----> WRONG ALIGNMENT: DROPPED POINT CLOUD" << endl;
break;
return;
}
pcl::transformPointCloud (*read_prefiltered, *output, correction);
......@@ -493,6 +533,32 @@ void App::operator()() {
cout << "Updates: " << updates_counter_ << endl;
}
cout << "--------------------------------------------------------------------------------------" << endl;
}
void App::operator()() {
running_ = true;
while (running_) {
std::unique_lock<std::mutex> lock(worker_mutex_);
// Wait for notification from planarLidarHandler
worker_condition_.wait_for(lock, std::chrono::milliseconds(1000));
// Copy current workload from cloud queue to work queue
std::list<AlignedCloudPtr> work_queue;
{
std::unique_lock<std::mutex> lock(data_mutex_);
while (!cloud_queue_.empty()) {
work_queue.push_back(cloud_queue_.front());
cloud_queue_.pop_front();
}
}
// Process workload
for (auto cloud : work_queue) {
processCloud(cloud);
}
}
}
......
......@@ -7,10 +7,10 @@ void TimingUtils::tic() {
tictoc_stack.push(clock());
}
void TimingUtils::toc() {
void TimingUtils::toc(std::string message) {
std::cout << "Time elapsed: "
<< ((double)(clock() - tictoc_stack.top())) / CLOCKS_PER_SEC
<< " sec"
<< " sec " << message
<< std::endl;
tictoc_stack.pop();
}
......
......@@ -24,11 +24,17 @@ public:
const ClassificationParams& class_params);
inline ~AppROS() {
if (input_poses_file_.is_open()) {
input_poses_file_.close();
}
delete accu_;
delete vis_ros_;
delete talk_ros_;
}
void writeCloudToFile(AlignedCloudPtr cloud);
// Subscriber callabacks
void velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr& laser_msg_in);
void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg_in);
......@@ -45,6 +51,9 @@ public:
private:
ros::NodeHandle& nh_;
std::ofstream input_poses_file_;
int input_clouds_counter_;
Eigen::Isometry3d world_to_body_;
Eigen::Isometry3d world_to_body_previous_;
tf::Pose temp_tf_pose_;
......
......@@ -35,7 +35,7 @@ public:
uint16_t getCounter() const;
bool getFinished() const;
uint64_t getFinishedTime() const;
int64_t getFinishedTime() const;
const PointCloud& getCloud();
void clearCloud();
......@@ -53,7 +53,7 @@ private:
// implicitly discarding intensities from the clouds
PointCloud point_cloud_;
PointCloud accumulated_point_cloud_;
uint64_t utime_;
int64_t utime_;
ros::NodeHandle& nh_;
ros::Subscriber lidar_sub_;
......
......@@ -20,6 +20,11 @@
<arg name="saveProbs" default="$(find aicp_core)/data/classification/probs_opencv3.txt"/>
<arg name="modelLocation" default="$(find aicp_core)/data/models/svm_opencv3.xml" />
<arg name="write_input_clouds_to_file" default="false"/>
<arg name="process_input_clouds_from_file" default="false"/>
<arg name="process_input_clouds_folder" default="/tmp/aicp_data/"/>
<!-- Run AICP (command line configuration) -->
<node pkg="aicp_ros" type="aicp_ros_node" name="aicp_ros_node" output="screen">
......@@ -61,6 +66,13 @@
<!-- Visualize and store -->
<param name="verbose" value="false" />
<!-- Write out incoming data to file -->
<param name="write_input_clouds_to_file" value="$(arg write_input_clouds_to_file)" />
<!-- Read incoming data from file -->
<param name="process_input_clouds_from_file" value="$(arg process_input_clouds_from_file)" />
<!-- Read incoming data from file -->
<param name="process_input_clouds_folder" value="$(arg process_input_clouds_folder)" />
</node>
</launch>
......@@ -8,10 +8,19 @@
<arg name="localize_against_prior_map" default="false"/> <!-- ! should always be false in this launch file ! -->
<arg name="localize_against_built_map" default="false"/> <!-- implicit loop closure when revisit known place -->
<!-- options for post processing -->
<arg name="write_input_clouds_to_file" default="false"/>
<arg name="process_input_clouds_from_file" default="false"/>
<arg name="process_input_clouds_folder" default="/tmp/aicp_data"/>
<include file="$(find aicp_ros)/launch/aicp.launch">
<arg name="load_map_from_file" value="$(arg load_map_from_file)"/>
<arg name="localize_against_prior_map" value="$(arg localize_against_prior_map)"/>
<arg name="localize_against_built_map" value="$(arg localize_against_built_map)"/>
<arg name="write_input_clouds_to_file" value="$(arg write_input_clouds_to_file)"/>
<arg name="process_input_clouds_from_file" value="$(arg process_input_clouds_from_file)"/>
<arg name="process_input_clouds_folder" value="$(arg process_input_clouds_folder)"/>
</include>
<group if="$(arg load_map_from_file)">
......
......@@ -31,6 +31,9 @@ int main(int argc, char** argv){
cl_cfg.pose_body_channel = "/state_estimator/pose_in_odom";
cl_cfg.output_channel = "/aicp/pose_corrected"; // Create new channel...
cl_cfg.verbose = false; // enable visualization for debug
cl_cfg.write_input_clouds_to_file = false; // write the raw incoming point clouds to a folder, for post processing
cl_cfg.process_input_clouds_from_file = false; // process raw incoming point cloud from a folder
cl_cfg.process_input_clouds_folder = "/tmp/aicp_data";
aicp::VelodyneAccumulatorConfig va_cfg;
va_cfg.batch_size = 80; // 240 is about 1 sweep at 5RPM // 80 is about 1 sweep at 15RPM
......@@ -57,6 +60,10 @@ int main(int argc, char** argv){
nh.getParam("pose_body_channel", cl_cfg.pose_body_channel);
nh.getParam("output_channel", cl_cfg.output_channel);
nh.getParam("verbose", cl_cfg.verbose);
nh.getParam("write_input_clouds_to_file", cl_cfg.write_input_clouds_to_file);
nh.getParam("process_input_clouds_from_file", cl_cfg.process_input_clouds_from_file);
nh.getParam("process_input_clouds_folder", cl_cfg.process_input_clouds_folder);
nh.getParam("batch_size", va_cfg.batch_size);
nh.getParam("min_range", va_cfg.min_range);
......@@ -100,19 +107,25 @@ int main(int argc, char** argv){
overlap_params,
classification_params));
// Subscribers
ros::Subscriber lidar_sub = nh.subscribe(va_cfg.lidar_topic, 100, &aicp::AppROS::velodyneCallBack, app.get());
ros::Subscriber pose_sub = nh.subscribe(cl_cfg.pose_body_channel, 100, &aicp::AppROS::robotPoseCallBack, app.get());
ros::Subscriber marker_sub = nh.subscribe("/interaction_marker/pose", 100, &aicp::AppROS::interactionMarkerCallBack, app.get());
if (!cl_cfg.process_input_clouds_from_file){
// Subscribers
ros::Subscriber lidar_sub = nh.subscribe(va_cfg.lidar_topic, 100, &aicp::AppROS::velodyneCallBack, app.get());
ros::Subscriber pose_sub = nh.subscribe(cl_cfg.pose_body_channel, 100, &aicp::AppROS::robotPoseCallBack, app.get());
ros::Subscriber marker_sub = nh.subscribe("/interaction_marker/pose", 100, &aicp::AppROS::interactionMarkerCallBack, app.get());
// Advertise services (using service published by anybotics icp_tools ui)
ros::ServiceServer load_map_server_ = nh.advertiseService("/icp_tools/load_map_from_file", &aicp::AppROS::loadMapFromFileCallBack, app.get());
ros::ServiceServer go_back_server_ = nh.advertiseService("/aicp/go_back_request", &aicp::AppROS::goBackRequestCallBack, app.get());
// Advertise services (using service published by anybotics icp_tools ui)
ros::ServiceServer load_map_server_ = nh.advertiseService("/icp_tools/load_map_from_file", &aicp::AppROS::loadMapFromFileCallBack, app.get());
ros::ServiceServer go_back_server_ = nh.advertiseService("/aicp/go_back_request", &aicp::AppROS::goBackRequestCallBack, app.get());
ROS_INFO_STREAM("[Aicp] Waiting for input messages...");
ROS_INFO_STREAM("[Aicp] Waiting for input messages...");
app->run();
ros::spin();
app->run();
ros::spin();
}else{
app->processFromFile(cl_cfg.process_input_clouds_folder);
}
return 0;
}
......@@ -49,6 +49,19 @@ AppROS::AppROS(ros::NodeHandle &nh,
alignability_pub_ = nh_.advertise<std_msgs::Float32>("/aicp/alignability",10);
risk_pub_ = nh_.advertise<std_msgs::Float32>("/aicp/alignment_risk",10);
}
// Write the incoming data to file. This should be false when running live
if (cl_cfg_.write_input_clouds_to_file)
{
std::stringstream input_poses_filename;
input_poses_filename << data_directory_path_.str() << "/input_poses.csv";
input_poses_file_.open (input_poses_filename.str().c_str() );
input_poses_file_ << "# counter, sec, nsec, x, y, z, qx, qy, qz, qw\n";
input_poses_file_.flush();
input_clouds_counter_ = 0;
}
}
void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &pose_msg_in)
......@@ -134,6 +147,33 @@ void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedCon
pose_initialized_ = true;
}
void AppROS::writeCloudToFile(AlignedCloudPtr cloud){
// Extract the data from AlignedCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud = cloud->getCloud();
Eigen::Isometry3d pose = cloud->getPriorPose();
Eigen::Quaterniond quat(pose.rotation());
int64_t utime = cloud->getUtime();
int64_t sec = floor(utime * 1E-6);
int64_t nsec = utime - sec* 1E6;
// Write the base-to-odom estimate
std::stringstream ss;
ss << input_clouds_counter_ << ", " << sec << ", " << nsec << ", ";
ss << pose.translation().x() << ", " << pose.translation().y() << ", " << pose.translation().z() << ", ";
ss << quat.x() << ", " << quat.y() << ", " << quat.z() << ", " << quat.w();
input_poses_file_ << ss.str() << "\n";
input_poses_file_.flush();
// Write the point cloud
std::stringstream ss2;
ss2 << data_directory_path_.str() << "/cloud_" << input_clouds_counter_ << "_" << sec << "_" << nsec << ".pcd";
pcd_writer_.write<pcl::PointXYZ> (ss2.str (), *point_cloud, true);
input_clouds_counter_++;
}
void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_msg_in){
if (!pose_initialized_){
ROS_WARN_STREAM("[Aicp] Pose not initialized, waiting for pose prior...");
......@@ -190,8 +230,12 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
world_to_body_));
world_to_body_previous_ = world_to_body_;
if (cl_cfg_.write_input_clouds_to_file)
writeCloudToFile(current_cloud);
// Stack current cloud into queue
cloud_queue_.push_back(current_cloud);
//cout << "[App ROS] cloud_queue_ size: " << cloud_queue_.size() << endl;
if (cloud_queue_.size() > max_queue_size) {
cout << "|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||\n";
......
......@@ -91,7 +91,7 @@ bool VelodyneAccumulatorROS::getFinished() const {
return finished_;
}
uint64_t VelodyneAccumulatorROS::getFinishedTime() const{
int64_t VelodyneAccumulatorROS::getFinishedTime() const{
return utime_;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment