-
Notifications
You must be signed in to change notification settings - Fork 278
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
41 changed files
with
2,237 additions
and
2,274 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,65 +1,129 @@ | ||
| cmake_minimum_required(VERSION 2.8.3) | ||
| cmake_minimum_required(VERSION 3.5) | ||
| project(floam) | ||
|
|
||
| set(CMAKE_BUILD_TYPE "Release") | ||
| set(CMAKE_CXX_FLAGS "-std=c++11") | ||
| set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") | ||
| set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread") | ||
|
|
||
| find_package(catkin REQUIRED COMPONENTS | ||
| geometry_msgs | ||
| nav_msgs | ||
| sensor_msgs | ||
| roscpp | ||
| rospy | ||
| rosbag | ||
| std_msgs | ||
| tf | ||
| eigen_conversions | ||
| ) | ||
| # Default to C++14 | ||
| if(NOT CMAKE_CXX_STANDARD) | ||
| set(CMAKE_CXX_STANDARD 14) | ||
| endif() | ||
|
|
||
| find_package(Eigen3) | ||
| if(NOT EIGEN3_FOUND) | ||
| # Fallback to cmake_modules | ||
| find_package(cmake_modules REQUIRED) | ||
| find_package(Eigen REQUIRED) | ||
| set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) | ||
| set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only | ||
| # Possibly map additional variables to the EIGEN3_ prefix. | ||
| else() | ||
| set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) | ||
| if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") | ||
| add_compile_options(-Wall -Wextra -Wpedantic) | ||
| endif() | ||
|
|
||
| # find dependencies | ||
| find_package(ament_cmake REQUIRED) | ||
| # uncomment the following section in order to fill in | ||
| # further dependencies manually. | ||
| # find_package(<dependency> REQUIRED) | ||
| find_package(rclcpp REQUIRED) | ||
| find_package(sensor_msgs REQUIRED) | ||
| find_package(nav_msgs REQUIRED) | ||
| find_package(geometry_msgs REQUIRED) | ||
| find_package(tf2 REQUIRED) | ||
| find_package(tf2_ros REQUIRED) | ||
| find_package(pcl_conversions REQUIRED) | ||
| find_package(PCL REQUIRED) | ||
| find_package(Eigen3 REQUIRED) | ||
| find_package(Ceres REQUIRED) | ||
|
|
||
| include_directories( | ||
| include | ||
| ${catkin_INCLUDE_DIRS} | ||
| ${PCL_INCLUDE_DIRS} | ||
| ${EIGEN3_INCLUDE_DIR} | ||
| ${CERES_INCLUDE_DIRS} | ||
| ${EIGEN3_INCLUDE_DIRS} | ||
| ) | ||
|
|
||
| link_directories( | ||
| include | ||
| ${PCL_LIBRARY_DIRS} | ||
| ${CERES_LIBRARY_DIRS} | ||
| ) | ||
|
|
||
| add_executable(laser_processing_node | ||
| src/laser_processing_node.cpp | ||
| src/laser_processing.cpp | ||
| src/laser_processing_class.cpp | ||
| src/lidar.cpp | ||
| ) | ||
|
|
||
| catkin_package( | ||
| CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs | ||
| DEPENDS EIGEN3 PCL Ceres | ||
| INCLUDE_DIRS include | ||
| ament_target_dependencies(laser_processing_node | ||
| rclcpp | ||
| sensor_msgs | ||
| pcl_conversions | ||
| ) | ||
|
|
||
| target_link_libraries(laser_processing_node | ||
| ${PCL_LIBRARIES} | ||
| ) | ||
|
|
||
| add_executable(odom_estimation_node | ||
| src/odom_estimation_node.cpp | ||
| src/odom_estimation.cpp | ||
| src/odom_estimation_class.cpp | ||
| src/lidar_optimization.cpp | ||
| src/lidar.cpp | ||
| ) | ||
|
|
||
| add_executable(floam_laser_processing_node src/laserProcessingNode.cpp src/laserProcessingClass.cpp src/lidar.cpp) | ||
| target_link_libraries(floam_laser_processing_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) | ||
| ament_target_dependencies(odom_estimation_node | ||
| rclcpp | ||
| sensor_msgs | ||
| nav_msgs | ||
| geometry_msgs | ||
| tf2 | ||
| tf2_ros | ||
| pcl_conversions | ||
| Eigen3 | ||
| #CERES | ||
| ) | ||
|
|
||
| add_executable(floam_odom_estimation_node src/odomEstimationNode.cpp src/lidarOptimization.cpp src/lidar.cpp src/odomEstimationClass.cpp) | ||
| target_link_libraries(floam_odom_estimation_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) | ||
| target_link_libraries(odom_estimation_node | ||
| ${PCL_LIBRARIES} | ||
| #${EIGEN3_LIBRARIES} | ||
| ${CERES_LIBRARIES} | ||
| ) | ||
|
|
||
| add_executable(floam_laser_mapping_node src/laserMappingNode.cpp src/laserMappingClass.cpp src/lidar.cpp) | ||
| target_link_libraries(floam_laser_mapping_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) | ||
| add_executable(laser_mapping_node | ||
| src/laser_mapping_node.cpp | ||
| src/laser_mapping.cpp | ||
| src/laser_mapping_class.cpp | ||
| src/lidar.cpp | ||
| ) | ||
|
|
||
| ament_target_dependencies(laser_mapping_node | ||
| rclcpp | ||
| sensor_msgs | ||
| nav_msgs | ||
| pcl_conversions | ||
| Eigen3 | ||
| ) | ||
|
|
||
| target_link_libraries(laser_mapping_node | ||
| ${PCL_LIBRARIES} | ||
| ) | ||
|
|
||
| install(TARGETS laser_processing_node odom_estimation_node laser_mapping_node | ||
| DESTINATION lib/${PROJECT_NAME} | ||
| ) | ||
|
|
||
| install(DIRECTORY launch | ||
| DESTINATION share/${PROJECT_NAME} | ||
| ) | ||
|
|
||
| install(DIRECTORY rviz | ||
| DESTINATION share/${PROJECT_NAME} | ||
| ) | ||
|
|
||
| if(BUILD_TESTING) | ||
| find_package(ament_lint_auto REQUIRED) | ||
| # the following line skips the linter which checks for copyrights | ||
| # uncomment the line when a copyright and license is not present in all source files | ||
| #set(ament_cmake_copyright_FOUND TRUE) | ||
| # the following line skips cpplint (only works in a git repo) | ||
| # uncomment the line when this package is not in a git repo | ||
| #set(ament_cmake_cpplint_FOUND TRUE) | ||
| ament_lint_auto_find_test_dependencies() | ||
| endif() | ||
|
|
||
| ament_package() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,49 @@ | ||
| // Author of FLOAM: Wang Han | ||
| // ROS2 Migration: Yi-Chen Zhang | ||
|
|
||
| #pragma once | ||
|
|
||
| // pcl header | ||
| #include <pcl/point_cloud.h> | ||
| #include <pcl/point_types.h> | ||
| #include <pcl/filters/voxel_grid.h> | ||
|
|
||
| // c++ header | ||
| #include <vector> | ||
|
|
||
| #define LASER_CELL_WIDTH 50.0 | ||
| #define LASER_CELL_HEIGHT 50.0 | ||
| #define LASER_CELL_DEPTH 50.0 | ||
|
|
||
| // separate map as many sub point clouds | ||
|
|
||
| #define LASER_CELL_RANGE_HORIZONTAL 2 | ||
| #define LASER_CELL_RANGE_VERTICAL 2 | ||
|
|
||
|
|
||
| class LaserMappingClass | ||
| { | ||
| public: | ||
| LaserMappingClass() = default; | ||
| void init(double map_resolution); | ||
| void updateCurrentPointsToMap(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, const Eigen::Isometry3d& pose_current); | ||
| pcl::PointCloud<pcl::PointXYZI>::Ptr getMap(); | ||
|
|
||
| private: | ||
| int origin_in_map_x; | ||
| int origin_in_map_y; | ||
| int origin_in_map_z; | ||
| int map_width; | ||
| int map_height; | ||
| int map_depth; | ||
| std::vector<std::vector<std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>>> map; | ||
| pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter; | ||
|
|
||
| void addWidthCellNegative(); | ||
| void addWidthCellPositive(); | ||
| void addHeightCellNegative(); | ||
| void addHeightCellPositive(); | ||
| void addDepthCellNegative(); | ||
| void addDepthCellPositive(); | ||
| void checkPoints(int& x, int& y, int& z); | ||
| }; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,40 @@ | ||
| // Author of FLOAM: Wang Han | ||
| // ROS2 Migration: Yi-Chen Zhang | ||
|
|
||
| #pragma once | ||
|
|
||
| // ros header | ||
| #include <rclcpp/rclcpp.hpp> | ||
| #include <sensor_msgs/msg/point_cloud2.hpp> | ||
| #include <nav_msgs/msg/odometry.hpp> | ||
|
|
||
| // c++ header | ||
| #include <mutex> | ||
| #include <queue> | ||
|
|
||
| // local header | ||
| #include "floam/lidar.hpp" | ||
| #include "floam/laser_mapping_class.hpp" | ||
|
|
||
|
|
||
| class LaserMappingNode: public rclcpp::Node | ||
| { | ||
| public: | ||
| LaserMappingNode(); | ||
| void laser_mapping(); | ||
|
|
||
| private: | ||
| LaserMappingClass laserMapping_; | ||
| std::queue<nav_msgs::msg::Odometry::ConstSharedPtr> odometryBuf_; | ||
| std::queue<sensor_msgs::msg::PointCloud2::ConstSharedPtr> pointCloudBuf_; | ||
| std::mutex mutex_lock_; | ||
| lidar::Lidar lidar_param_; | ||
|
|
||
| rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subLaserCloud_; | ||
| rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subOdometry_; | ||
|
|
||
| rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubMap_; | ||
|
|
||
| void odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg); | ||
| void velodyneHandler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr laserCloudMsg); | ||
| }; |
Oops, something went wrong.