Skip to content

Commit

Permalink
Refactoring similar to lidarslam_ros2
Browse files Browse the repository at this point in the history
  • Loading branch information
@rsasaki0109
rsasaki0109 committed Nov 26, 2023
1 parent e623a0d commit 31951c3
Show file tree
Hide file tree
Showing 5 changed files with 94 additions and 87 deletions.
2 changes: 1 addition & 1 deletion graph_based_slam/include/graph_based_slam/graph_based_slam_component.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -142,6 +142,6 @@ namespace graphslam
std::vector < LoopEdge > loop_edges_;

};
}
} // namespace graphslam

#endif //GS_GBS_COMPONENT_H_INCLUDED
21 changes: 9 additions & 12 deletions graph_based_slam/src/graph_based_slam_component.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ GraphBasedSlamComponent::GraphBasedSlamComponent(const rclcpp::NodeOptions & opt
ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
if (ndt_num_threads > 0) {ndt->setNumThreads(ndt_num_threads);}
registration_ = ndt;
} else {
} else if (registration_method == "GICP") {
boost::shared_ptr<pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI>>
gicp(new pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI>());
gicp->setMaxCorrespondenceDistance(30);
Expand All @@ -77,6 +77,9 @@ GraphBasedSlamComponent::GraphBasedSlamComponent(const rclcpp::NodeOptions & opt
gicp->setEuclideanFitnessEpsilon(1e-6);
gicp->setRANSACIterations(0);
registration_ = gicp;
} else {
RCLCPP_ERROR(get_logger(), "invalid registration method");
exit(1);
}

initializePubSub();
Expand Down Expand Up @@ -149,8 +152,7 @@ void GraphBasedSlamComponent::searchLoop()
lidarslam_msgs::msg::MapArray map_array_msg = map_array_msg_;
std::lock_guard<std::mutex> lock(mtx_);
int num_submaps = map_array_msg.submaps.size();
std::cout << "----------------------------" << std::endl;
std::cout << "searching Loop, num_submaps:" << num_submaps << std::endl;
RCLCPP_INFO(get_logger(), "searching Loop, num_submaps:%d", num_submaps);

double min_fitness_score = std::numeric_limits<double>::max();
double distance_min_fitness_score = 0;
Expand Down Expand Up @@ -231,21 +233,16 @@ void GraphBasedSlamComponent::searchLoop()
loop_edge.relative_pose = Eigen::Isometry3d(from.inverse() * to);
loop_edges_.push_back(loop_edge);

std::cout << "---" << std::endl;
std::cout << "PoseAdjustment" << std::endl;
std::cout << "distance:" << min_submap.distance << ", score:" << fitness_score << std::endl;
std::cout << "id_loop_point 1:" << id_min << std::endl;
std::cout << "id_loop_point 2:" << num_submaps - 1 << std::endl;
std::cout << "PoseAdjustment" << " distance:" << min_submap.distance << ", score:" << fitness_score << std::endl;
std::cout << "id_loop_point 1:" << id_min << " id_loop_point 2:" << num_submaps - 1 << std::endl;
std::cout << "final transformation:" << std::endl;
std::cout << registration_->getFinalTransformation() << std::endl;
doPoseAdjustment(map_array_msg, use_save_map_in_loop_);

return;
}

std::cout << "-" << std::endl;
std::cout << "min_submap_distance:" << min_submap.distance << std::endl;
std::cout << "min_fitness_score:" << fitness_score << std::endl;
std::cout << "min_submap_distance:" << min_submap.distance << " min_fitness_score:" << fitness_score << std::endl;
}
}

Expand Down Expand Up @@ -358,7 +355,7 @@ void GraphBasedSlamComponent::doPoseAdjustment(

}

}
} // namespace graphslam

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(graphslam::GraphBasedSlamComponent)
3 changes: 2 additions & 1 deletion scanmatcher/include/scanmatcher/scanmatcher_component.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,7 @@ namespace graphslam
rclcpp::Publisher < nav_msgs::msg::Odometry > ::SharedPtr odom_pub_;

void initializePubSub();
void initializeMap(const pcl::PointCloud <pcl::PointXYZI>::Ptr & cloud_ptr, const std_msgs::msg::Header & header);
void receiveCloud(
const pcl::PointCloud < PointType > ::ConstPtr & input_cloud_ptr,
const rclcpp::Time stamp);
Expand All @@ -135,7 +136,7 @@ namespace graphslam
const rclcpp::Time stamp
);
Eigen::Matrix4f getTransformation(const geometry_msgs::msg::Pose pose);
void publishMap();
void publishMap(const lidarslam_msgs::msg::MapArray & map_array_msg , const std::string & map_frame_id);
void updateMap(
const pcl::PointCloud < PointType > ::ConstPtr cloud_ptr,
const Eigen::Matrix4f final_transformation,
Expand Down
6 changes: 3 additions & 3 deletions scanmatcher/param/lio_bigloop.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,9 @@
ros__parameters:
# LiDAR setting
pointCloudTopic: "points_raw"
sensor: velodyne
N_SCAN: 16
Horizon_SCAN: 1800
sensor: velodyne # lidar sensor type, 'velodyne' or 'ouster' or 'livox'
N_SCAN: 16 # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
# IMU setting
imuTopic: "imu_correct"
imuAccNoise: 3.9939570888238808e-03
Expand Down
149 changes: 79 additions & 70 deletions scanmatcher/src/scanmatcher_component.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,13 +107,16 @@ ScanMatcherComponent::ScanMatcherComponent(const rclcpp::NodeOptions & options)

registration_ = ndt;

} else {
} else if (registration_method == "GICP") {
boost::shared_ptr<pclomp::GeneralizedIterativeClosestPoint<PointType, PointType>>
gicp(new pclomp::GeneralizedIterativeClosestPoint<PointType, PointType>());
gicp->setMaxCorrespondenceDistance(gicp_corr_dist_threshold);
//gicp->setCorrespondenceRandomness(20);
gicp->setTransformationEpsilon(1e-8);
registration_ = gicp;
} else {
RCLCPP_ERROR(get_logger(), "invalid registration method");
exit(1);
}

map_array_msg_.header.frame_id = global_frame_id_;
Expand Down Expand Up @@ -169,73 +172,45 @@ void ScanMatcherComponent::initializePubSub()
auto cloud_callback =
[this](const typename sensor_msgs::msg::PointCloud2::SharedPtr msg) -> void
{
if (initial_pose_received_) {
sensor_msgs::msg::PointCloud2 transformed_msg;
try {
tf2::TimePoint time_point = tf2::TimePoint(
std::chrono::seconds(msg->header.stamp.sec) +
std::chrono::nanoseconds(msg->header.stamp.nanosec));
const geometry_msgs::msg::TransformStamped transform = tfbuffer_.lookupTransform(
robot_frame_id_, msg->header.frame_id, time_point);
tf2::doTransform(*msg, transformed_msg, transform); // TODO:slow now(https://github.com/ros/geometry2/pull/432)
} catch (tf2::TransformException & e) {
RCLCPP_ERROR(this->get_logger(), "%s", e.what());
return;
}

pcl::PointCloud<PointType>::Ptr tmp_ptr(new pcl::PointCloud<PointType>());
pcl::fromROSMsg(transformed_msg, *tmp_ptr);

if (use_min_max_filter_) {
double r;
pcl::PointCloud<PointType> tmp;
for (const auto & p : tmp_ptr->points) {
r = sqrt(pow(p.x, 2.0) + pow(p.y, 2.0));
if (scan_min_range_ < r && r < scan_max_range_) {tmp.push_back(p);}
}
}
if (!initial_pose_received_)
{
RCLCPP_WARN(get_logger(), "initial_pose is not received");
return;
}
sensor_msgs::msg::PointCloud2 transformed_msg;
try {
tf2::TimePoint time_point = tf2::TimePoint(
std::chrono::seconds(msg->header.stamp.sec) +
std::chrono::nanoseconds(msg->header.stamp.nanosec));
const geometry_msgs::msg::TransformStamped transform = tfbuffer_.lookupTransform(
robot_frame_id_, msg->header.frame_id, time_point);
tf2::doTransform(*msg, transformed_msg, transform); // TODO:slow now(https://github.com/ros/geometry2/pull/432)
} catch (tf2::TransformException & e) {
RCLCPP_ERROR(this->get_logger(), "%s", e.what());
return;
}

if (!initial_cloud_received_) {
RCLCPP_INFO(get_logger(), "create a first map");
pcl::PointCloud<PointType>::Ptr cloud_ptr(new pcl::PointCloud<PointType>());
pcl::VoxelGrid<PointType> voxel_grid;
voxel_grid.setLeafSize(vg_size_for_map_, vg_size_for_map_, vg_size_for_map_);
voxel_grid.setInputCloud(tmp_ptr);
voxel_grid.filter(*cloud_ptr);

initial_cloud_received_ = true;

Eigen::Matrix4f sim_trans = getTransformation(corrent_pose_stamped_.pose);
pcl::PointCloud<PointType>::Ptr transformed_cloud_ptr(
new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*cloud_ptr, *transformed_cloud_ptr, sim_trans);
registration_->setInputTarget(transformed_cloud_ptr);

// map
sensor_msgs::msg::PointCloud2::SharedPtr map_msg_ptr(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*transformed_cloud_ptr, *map_msg_ptr);

//map array
sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg_ptr(
new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*cloud_ptr, *cloud_msg_ptr);
lidarslam_msgs::msg::SubMap submap;
submap.header = msg->header;
submap.distance = 0;
submap.pose = corrent_pose_stamped_.pose;
submap.cloud = *cloud_msg_ptr;
map_array_msg_.header = msg->header;
map_array_msg_.submaps.push_back(submap);

map_pub_->publish(submap.cloud);

last_map_time_ = clock_.now();
pcl::PointCloud<PointType>::Ptr tmp_ptr(new pcl::PointCloud<PointType>());
pcl::fromROSMsg(transformed_msg, *tmp_ptr);

if (use_min_max_filter_) {
double r;
pcl::PointCloud<PointType> tmp;
for (const auto & p : tmp_ptr->points) {
r = sqrt(pow(p.x, 2.0) + pow(p.y, 2.0));
if (scan_min_range_ < r && r < scan_max_range_) {tmp.push_back(p);}
}
}

if (initial_cloud_received_) {receiveCloud(tmp_ptr, msg->header.stamp);}
if (!initial_cloud_received_) {
RCLCPP_INFO(get_logger(), "initial_cloud is received");
initial_cloud_received_ = true;
initializeMap(tmp_ptr, msg->header);
last_map_time_ = clock_.now();
}

if (initial_cloud_received_) {receiveCloud(tmp_ptr, msg->header.stamp);}

};


Expand Down Expand Up @@ -278,6 +253,40 @@ void ScanMatcherComponent::initializePubSub()
1)).reliable());
}

void ScanMatcherComponent::initializeMap(const pcl::PointCloud <pcl::PointXYZI>::Ptr & tmp_ptr, const std_msgs::msg::Header & header)
{
RCLCPP_INFO(get_logger(), "create a first map");
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>());
pcl::VoxelGrid<pcl::PointXYZI> voxel_grid;
voxel_grid.setLeafSize(vg_size_for_map_, vg_size_for_map_, vg_size_for_map_);
voxel_grid.setInputCloud(tmp_ptr);
voxel_grid.filter(*cloud_ptr);

Eigen::Matrix4f sim_trans = getTransformation(corrent_pose_stamped_.pose);
pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud_ptr(
new pcl::PointCloud<pcl::PointXYZI>());
pcl::transformPointCloud(*cloud_ptr, *transformed_cloud_ptr, sim_trans);
registration_->setInputTarget(transformed_cloud_ptr);

// map
sensor_msgs::msg::PointCloud2::SharedPtr map_msg_ptr(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*transformed_cloud_ptr, *map_msg_ptr);

// map array
sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg_ptr(
new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*cloud_ptr, *cloud_msg_ptr);
lidarslam_msgs::msg::SubMap submap;
submap.header = header;
submap.distance = 0;
submap.pose = corrent_pose_stamped_.pose;
submap.cloud = *cloud_msg_ptr;
map_array_msg_.header = header;
map_array_msg_.submaps.push_back(submap);

map_pub_->publish(submap.cloud);
}

void ScanMatcherComponent::receiveCloud(
const pcl::PointCloud<PointType>::ConstPtr & cloud_ptr,
const rclcpp::Time stamp)
Expand Down Expand Up @@ -476,7 +485,7 @@ void ScanMatcherComponent::updateMap(
rclcpp::Time map_time = clock_.now();
double dt = map_time.seconds() - last_map_time_.seconds();
if (dt > map_publish_period_) {
publishMap();
publishMap(map_array_msg_, global_frame_id_);
last_map_time_ = map_time;
}
}
Expand All @@ -500,32 +509,32 @@ void ScanMatcherComponent::receiveOdom(const nav_msgs::msg::Odometry odom_msg)
}
}

void ScanMatcherComponent::publishMap()
void ScanMatcherComponent::publishMap(const lidarslam_msgs::msg::MapArray & map_array_msg , const std::string & map_frame_id)
{
RCLCPP_INFO(get_logger(), "publish a map");

pcl::PointCloud<pcl::PointXYZI>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZI>);
for (auto & submap : map_array_msg_.submaps) {
for (auto & submap : map_array_msg.submaps) {
pcl::PointCloud<pcl::PointXYZI>::Ptr submap_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_submap_cloud_ptr(
new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(submap.cloud, *submap_cloud_ptr);

Eigen::Affine3d affine;
tf2::fromMsg(submap.pose, affine);
pcl::transformPointCloud(
*submap_cloud_ptr, *transformed_submap_cloud_ptr,
affine.matrix().cast<float>());

*map_ptr += *transformed_submap_cloud_ptr;
}
std::cout << "number of map　points: " << map_ptr->size() << std::endl;
RCLCPP_INFO(get_logger(), "publish a map, number of points in the map : %ld", map_ptr->size());

sensor_msgs::msg::PointCloud2::SharedPtr map_msg_ptr(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*map_ptr, *map_msg_ptr);
map_msg_ptr->header.frame_id = global_frame_id_;
map_msg_ptr->header.frame_id = map_frame_id;
map_pub_->publish(*map_msg_ptr);
}

}
} // namespace graphslam

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(graphslam::ScanMatcherComponent)

0 comments on commit 31951c3

Please sign in to comment.