diff --git a/CMakeLists.txt b/CMakeLists.txt index 15427651..4e995e12 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,42 +1,19 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(hdl_localization) if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64") - add_definitions(-std=c++11) - set(CMAKE_CXX_FLAGS "-std=c++11") + add_definitions(-std=c++17) + set(CMAKE_CXX_FLAGS "-std=c++17") else() # -mavx causes a lot of errors!! - if("$ENV{ROS_DISTRO}" STRGREATER "melodic") - add_definitions(-std=c++17 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) - set(CMAKE_CXX_FLAGS "-std=c++17 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") - else() - add_definitions(-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) - set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") - endif() + add_definitions(-std=c++17 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) + set(CMAKE_CXX_FLAGS "-std=c++17 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") endif() -# pcl 1.7 causes a segfault when it is built with debug mode set(CMAKE_BUILD_TYPE "RELEASE") -find_package(catkin REQUIRED COMPONENTS - nodelet - tf2 - tf2_ros - tf2_eigen - tf2_geometry_msgs - eigen_conversions - pcl_ros - roscpp - rospy - sensor_msgs - geometry_msgs - message_generation - ndt_omp - fast_gicp - hdl_global_localization -) +find_package(PCL REQUIRED) -find_package(PCL 1.7 REQUIRED) include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) @@ -51,47 +28,125 @@ if (OPENMP_FOUND) set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") endif() -######################## -## message generation ## -######################## -add_message_files(FILES - ScanMatchingStatus.msg +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(rclpy REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(ndt_omp REQUIRED) +find_package(fast_gicp REQUIRED) +find_package(hdl_global_localization REQUIRED) +find_package(rclcpp_components REQUIRED) + +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ScanMatchingStatus.msg" + DEPENDENCIES std_msgs geometry_msgs +) + +ament_export_dependencies(rosidl_default_runtime) + +rosidl_get_typesupport_target(cpp_typesupport_target + ${PROJECT_NAME} rosidl_typesupport_cpp) + +add_library(hdl_localization_component + src/hdl_localization/pose_estimator.cpp + apps/hdl_localization.cpp +) +target_include_directories(hdl_localization_component PUBLIC + $ + $) +ament_target_dependencies(hdl_localization_component + tf2_ros + tf2_eigen + pcl_ros + rclcpp + sensor_msgs + geometry_msgs + ndt_omp + fast_gicp + hdl_global_localization + std_srvs + nav_msgs + rclcpp_components + std_msgs ) -generate_messages(DEPENDENCIES std_msgs geometry_msgs) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS include -# LIBRARIES hdl_scan_matching_odometry -# CATKIN_DEPENDS pcl_ros roscpp sensor_msgs -# DEPENDS system_lib +target_link_libraries(hdl_localization_component + ${PCL_LIBRARIES} "${cpp_typesupport_target}" ) -########### -## Build ## -########### -include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} +rclcpp_components_register_node( + hdl_localization_component + PLUGIN "hdl_localization::HdlLocalization" + EXECUTABLE hdl_localization_node ) -# nodelets -add_library(hdl_localization_nodelet - src/hdl_localization/pose_estimator.cpp - apps/hdl_localization_nodelet.cpp + +add_library(globalmap_server_component + apps/globalmap_server.cpp +) +target_include_directories(globalmap_server_component PUBLIC + $ + $) +ament_target_dependencies(globalmap_server_component + tf2_ros + tf2_eigen + pcl_ros + rclcpp + sensor_msgs + geometry_msgs + ndt_omp + fast_gicp + hdl_global_localization + std_srvs + nav_msgs + rclcpp_components + std_msgs ) -target_link_libraries(hdl_localization_nodelet - ${catkin_LIBRARIES} - ${PCL_LIBRARIES} +target_link_libraries(globalmap_server_component + ${PCL_LIBRARIES} "${cpp_typesupport_target}" ) -add_dependencies(hdl_localization_nodelet ${PROJECT_NAME}_gencpp) +rclcpp_components_register_node( + globalmap_server_component + PLUGIN "hdl_localization::GlobalmapServer" + EXECUTABLE globalmap_server +) + + +install(DIRECTORY launch rviz param + DESTINATION share/${PROJECT_NAME}/ +) + +ament_export_targets(export_hdl_localization_component) +install(TARGETS hdl_localization_component + EXPORT export_hdl_localization_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +ament_export_targets(export_globalmap_server_component) +install(TARGETS globalmap_server_component + EXPORT export_globalmap_server_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) -add_library(globalmap_server_nodelet apps/globalmap_server_nodelet.cpp) -target_link_libraries(globalmap_server_nodelet - ${catkin_LIBRARIES} - ${PCL_LIBRARIES} +install(PROGRAMS + scripts/plot_status.py + DESTINATION lib/${PROJECT_NAME} ) +ament_package() \ No newline at end of file diff --git a/README.md b/README.md index 57a5e42b..01d60e1a 100644 --- a/README.md +++ b/README.md @@ -25,17 +25,17 @@ The following ros packages are required: ## Installation ```bash -cd /your/catkin_ws/src +cd /your/ros2_ws/src git clone https://github.com/koide3/ndt_omp git clone https://github.com/SMRT-AIST/fast_gicp --recursive -git clone https://github.com/koide3/hdl_localization -git clone https://github.com/koide3/hdl_global_localization +git clone https://github.com/koide3/hdl_localization -b humble +git clone https://github.com/koide3/hdl_global_localization -b humble --recursive -cd /your/catkin_ws -catkin_make -DCMAKE_BUILD_TYPE=Release +cd /your/ros2_ws +colcon build --cmake-args "-DCMAKE_BUILD_TYPE=Release" # if you want to enable CUDA-accelerated NDT -# catkin_make -DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON +# colcon build --cmake-args "-DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON" ``` ### Support docker :whale: @@ -46,19 +46,19 @@ Please refer to the repository below and use the docker easily. - [Taeyoung96/hdl_localization_tutorial](https://github.com/Taeyoung96/hdl_localization_tutorial) ## Parameters -All configurable parameters are listed in *launch/hdl_localization.launch* as ros params. +All configurable parameters are listed in *param/hdl_localization.yaml* and *launch/hdl_localization.launch.py* as ros parameters. The estimated pose can be reset using using "2D Pose Estimate" on rviz ## Topics -- ***/odom*** (nav_msgs/Odometry) +- ***/odom*** (nav_msgs/msg/Odometry) - Estimated sensor pose in the map frame - ***/aligned_points*** - Input point cloud aligned with the map -- ***/status*** (hdl_localization/ScanMatchingStatus) +- ***/status*** (hdl_localization/msg/ScanMatchingStatus) - Scan matching result information (e.g., convergence, matching error, and inlier fraction) ## Services -- ***/relocalize*** (std_srvs/Empty) +- ***/relocalize*** (std_srvs/srv/Empty) - Reset the sensor pose with the global localization result - For details of the global localization method, see [hdl_global_localization](https://github.com/koide3/hdl_global_localization) @@ -67,22 +67,24 @@ The estimated pose can be reset using using "2D Pose Estimate" on rviz Example bag file (recorded in an outdoor environment): [hdl_400.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_400.bag.tar.gz) (933MB) ```bash -rosparam set use_sim_time true -roslaunch hdl_localization hdl_localization.launch +ros2 launch hdl_localization hdl_localization.launch.py +ros2 param set use_sim_time true ``` ```bash -roscd hdl_localization/rviz -rviz -d hdl_localization.rviz +colcon_cd hdl_localization +cd rviz +rviz2 -d hdl_localization.rviz ``` ```bash -rosbag play --clock hdl_400.bag +# need rosbag_v2 plugin +ros2 bag play -s rosbag_v2 --clock hdl_400.bag ``` ```bash # perform global localization -rosservice call /relocalize +ros2 service call /relocalize std_srvs/srv/Empty ``` diff --git a/apps/globalmap_server.cpp b/apps/globalmap_server.cpp new file mode 100644 index 00000000..8fa2a206 --- /dev/null +++ b/apps/globalmap_server.cpp @@ -0,0 +1,115 @@ +#include +#include +#include +#include + + +#include +#include +#include + +#include +#include + +#include + +using namespace std::chrono_literals; +using std::placeholders::_1; + +namespace hdl_localization { + +class GlobalmapServer : public rclcpp::Node { +public: + using PointT = pcl::PointXYZI; + + + GlobalmapServer(const rclcpp::NodeOptions & options) : Node("globalmap_server", options) { + initialize_params(); + + // publish globalmap with "latched" publisher + globalmap_pub = this->create_publisher("/globalmap", 5); + map_update_sub = this->create_subscription("/map_request/pcd", 10, std::bind(&GlobalmapServer::map_update_callback, this, _1)); + + globalmap_pub_timer = this->create_wall_timer(1s, std::bind(&GlobalmapServer::pub_once_cb, this)); + } + virtual ~GlobalmapServer() { + } + +private: + void initialize_params() { + // read globalmap from a pcd file + this->declare_parameter("globalmap_pcd", ""); + this->declare_parameter("convert_utm_to_local", true); + this->declare_parameter("downsample_resolution", 0.1); + + std::string globalmap_pcd = this->get_parameter("globalmap_pcd").as_string(); + globalmap.reset(new pcl::PointCloud()); + pcl::io::loadPCDFile(globalmap_pcd, *globalmap); + globalmap->header.frame_id = "map"; + + std::ifstream utm_file(globalmap_pcd + ".utm"); + if (utm_file.is_open() && this->get_parameter("convert_utm_to_local").as_bool()) { + double utm_easting; + double utm_northing; + double altitude; + utm_file >> utm_easting >> utm_northing >> altitude; + for(auto& pt : globalmap->points) { + pt.getVector3fMap() -= Eigen::Vector3f(utm_easting, utm_northing, altitude); + } + RCLCPP_INFO_STREAM(this->get_logger(), "Global map offset by UTM reference coordinates (x = " + << utm_easting << ", y = " << utm_northing << ") and altitude (z = " << altitude << ")"); + } + + // downsample globalmap + double downsample_resolution = this->get_parameter("downsample_resolution").as_double(); + std::shared_ptr> voxelgrid(new pcl::VoxelGrid()); + voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); + voxelgrid->setInputCloud(globalmap); + + pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); + voxelgrid->filter(*filtered); + + globalmap = filtered; + pcl::toROSMsg(*globalmap, globalmap_msg); + } + + void pub_once_cb() { + globalmap_pub->publish(globalmap_msg); + globalmap_pub_timer->cancel(); + } + + void map_update_callback(const std_msgs::msg::String &msg) { + RCLCPP_INFO_STREAM(this->get_logger(), "Received map request, map path : "<< msg.data); + std::string globalmap_pcd = msg.data; + globalmap.reset(new pcl::PointCloud()); + pcl::io::loadPCDFile(globalmap_pcd, *globalmap); + globalmap->header.frame_id = "map"; + + // downsample globalmap + double downsample_resolution = this->get_parameter("downsample_resolution").as_double(); + std::shared_ptr> voxelgrid(new pcl::VoxelGrid()); + voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); + voxelgrid->setInputCloud(globalmap); + + pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); + voxelgrid->filter(*filtered); + + globalmap = filtered; + pcl::toROSMsg(*globalmap, globalmap_msg); + globalmap_pub->publish(globalmap_msg); + } + +private: + // ROS2 + rclcpp::Publisher::SharedPtr globalmap_pub; + rclcpp::Subscription::SharedPtr map_update_sub; + + rclcpp::TimerBase::SharedPtr globalmap_pub_timer; + pcl::PointCloud::Ptr globalmap; + sensor_msgs::msg::PointCloud2 globalmap_msg; +}; + +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(hdl_localization::GlobalmapServer) diff --git a/apps/globalmap_server_nodelet.cpp b/apps/globalmap_server_nodelet.cpp deleted file mode 100644 index 3f33f657..00000000 --- a/apps/globalmap_server_nodelet.cpp +++ /dev/null @@ -1,115 +0,0 @@ -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include - -#include - -namespace hdl_localization { - -class GlobalmapServerNodelet : public nodelet::Nodelet { -public: - using PointT = pcl::PointXYZI; - - GlobalmapServerNodelet() { - } - virtual ~GlobalmapServerNodelet() { - } - - void onInit() override { - nh = getNodeHandle(); - mt_nh = getMTNodeHandle(); - private_nh = getPrivateNodeHandle(); - - initialize_params(); - - // publish globalmap with "latched" publisher - globalmap_pub = nh.advertise("/globalmap", 5, true); - map_update_sub = nh.subscribe("/map_request/pcd", 10, &GlobalmapServerNodelet::map_update_callback, this); - - globalmap_pub_timer = nh.createWallTimer(ros::WallDuration(1.0), &GlobalmapServerNodelet::pub_once_cb, this, true, true); - } - -private: - void initialize_params() { - // read globalmap from a pcd file - std::string globalmap_pcd = private_nh.param("globalmap_pcd", ""); - globalmap.reset(new pcl::PointCloud()); - pcl::io::loadPCDFile(globalmap_pcd, *globalmap); - globalmap->header.frame_id = "map"; - - std::ifstream utm_file(globalmap_pcd + ".utm"); - if (utm_file.is_open() && private_nh.param("convert_utm_to_local", true)) { - double utm_easting; - double utm_northing; - double altitude; - utm_file >> utm_easting >> utm_northing >> altitude; - for(auto& pt : globalmap->points) { - pt.getVector3fMap() -= Eigen::Vector3f(utm_easting, utm_northing, altitude); - } - ROS_INFO_STREAM("Global map offset by UTM reference coordinates (x = " - << utm_easting << ", y = " << utm_northing << ") and altitude (z = " << altitude << ")"); - } - - // downsample globalmap - double downsample_resolution = private_nh.param("downsample_resolution", 0.1); - boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); - voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); - voxelgrid->setInputCloud(globalmap); - - pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); - voxelgrid->filter(*filtered); - - globalmap = filtered; - } - - void pub_once_cb(const ros::WallTimerEvent& event) { - globalmap_pub.publish(globalmap); - } - - void map_update_callback(const std_msgs::String &msg){ - ROS_INFO_STREAM("Received map request, map path : "<< msg.data); - std::string globalmap_pcd = msg.data; - globalmap.reset(new pcl::PointCloud()); - pcl::io::loadPCDFile(globalmap_pcd, *globalmap); - globalmap->header.frame_id = "map"; - - // downsample globalmap - double downsample_resolution = private_nh.param("downsample_resolution", 0.1); - boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); - voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); - voxelgrid->setInputCloud(globalmap); - - pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); - voxelgrid->filter(*filtered); - - globalmap = filtered; - globalmap_pub.publish(globalmap); - } - -private: - // ROS - ros::NodeHandle nh; - ros::NodeHandle mt_nh; - ros::NodeHandle private_nh; - - ros::Publisher globalmap_pub; - ros::Subscriber map_update_sub; - - ros::WallTimer globalmap_pub_timer; - pcl::PointCloud::Ptr globalmap; -}; - -} - - -PLUGINLIB_EXPORT_CLASS(hdl_localization::GlobalmapServerNodelet, nodelet::Nodelet) diff --git a/apps/hdl_localization.cpp b/apps/hdl_localization.cpp new file mode 100644 index 00000000..712d8e6e --- /dev/null +++ b/apps/hdl_localization.cpp @@ -0,0 +1,608 @@ +#include +#include +#include +#include + +#include +#include +#include + + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +#include "hdl_localization/msg/scan_matching_status.hpp" +#include +#include + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace hdl_localization { + +class HdlLocalization : public rclcpp::Node { +public: + using PointT = pcl::PointXYZI; + + HdlLocalization(const rclcpp::NodeOptions & options) : Node("hdl_localization", options){ + tf_buffer = std::make_unique(this->get_clock()); + tf_listener = std::make_shared(*tf_buffer); + tf_broadcaster = std::make_unique(*this); + + initialize_params(); + + this->declare_parameter("robot_odom_frame_id", "robot_odom"); + this->declare_parameter("odom_child_frame_id", "base_link"); + this->declare_parameter("use_imu", true); + this->declare_parameter("invert_acc", false); + this->declare_parameter("invert_gyro", false); + this->declare_parameter("use_global_localization", true); + this->declare_parameter("enable_robot_odometry_prediction", false); + this->declare_parameter("status_max_correspondence_dist", 0.5); + this->declare_parameter("status_max_valid_point_dist", 25.0); + + robot_odom_frame_id = this->get_parameter("robot_odom_frame_id").as_string(); + odom_child_frame_id = this->get_parameter("odom_child_frame_id").as_string(); + + use_imu = this->get_parameter("use_imu").as_bool(); + invert_acc = this->get_parameter("invert_acc").as_bool(); + invert_gyro = this->get_parameter("invert_gyro").as_bool(); + if (use_imu) { + RCLCPP_INFO(this->get_logger(), "enable imu-based prediction"); + imu_sub = this->create_subscription("/gpsimu_driver/imu_data", 256, std::bind(&HdlLocalization::imu_callback, this, _1)); + } + points_sub = this->create_subscription("/velodyne_points", 5, std::bind(&HdlLocalization::points_callback, this, _1)); + globalmap_sub = this->create_subscription("/globalmap", 1, std::bind(&HdlLocalization::globalmap_callback, this, _1)); + initialpose_sub = this->create_subscription("/initialpose", 8, std::bind(&HdlLocalization::initialpose_callback, this, _1)); + + pose_pub = this->create_publisher("/odom", 5); + aligned_pub = this->create_publisher("/aligned_points", 5); + status_pub = this->create_publisher("/status", 5); + + // global localization + use_global_localization = this->get_parameter("use_global_localization").as_bool(); + if(use_global_localization) { + RCLCPP_INFO_STREAM(this->get_logger(), "wait for global localization services"); + set_global_map_service = this->create_client("/hdl_global_localization/set_global_map"); + query_global_localization_service = this->create_client("/hdl_global_localization/query"); + while (!set_global_map_service->wait_for_service(1s)); + while (!query_global_localization_service->wait_for_service(1s)); + + relocalize_server = this->create_service("/relocalize", std::bind(&HdlLocalization::relocalize, this, _1, _2)); + } + } + virtual ~HdlLocalization() { + } + +private: + pcl::Registration::Ptr create_registration() { + std::string reg_method = this->get_parameter("reg_method").as_string(); + std::string ndt_neighbor_search_method = this->get_parameter("ndt_neighbor_search_method").as_string(); + double ndt_neighbor_search_radius = this->get_parameter("ndt_neighbor_search_radius").as_double(); + double ndt_resolution = this->get_parameter("ndt_resolution").as_double(); + + if(reg_method == "NDT_OMP") { + RCLCPP_INFO(this->get_logger(), "NDT_OMP is selected"); + pclomp::NormalDistributionsTransform::Ptr ndt(new pclomp::NormalDistributionsTransform()); + ndt->setTransformationEpsilon(0.01); + ndt->setResolution(ndt_resolution); + if (ndt_neighbor_search_method == "DIRECT1") { + RCLCPP_INFO(this->get_logger(), "search_method DIRECT1 is selected"); + ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1); + } else if (ndt_neighbor_search_method == "DIRECT7") { + RCLCPP_INFO(this->get_logger(), "search_method DIRECT7 is selected"); + ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7); + } else { + if (ndt_neighbor_search_method == "KDTREE") { + RCLCPP_INFO(this->get_logger(), "search_method KDTREE is selected"); + } else { + RCLCPP_WARN(this->get_logger(), "invalid search method was given"); + RCLCPP_WARN(this->get_logger(), "default method is selected (KDTREE)"); + } + ndt->setNeighborhoodSearchMethod(pclomp::KDTREE); + } + return ndt; + } else if(reg_method.find("NDT_CUDA") != std::string::npos) { + RCLCPP_INFO(this->get_logger(), "NDT_CUDA is selected"); + std::shared_ptr> ndt(new fast_gicp::NDTCuda); + ndt->setResolution(ndt_resolution); + + if(reg_method.find("D2D") != std::string::npos) { + ndt->setDistanceMode(fast_gicp::NDTDistanceMode::D2D); + } else if (reg_method.find("P2D") != std::string::npos) { + ndt->setDistanceMode(fast_gicp::NDTDistanceMode::P2D); + } + + if (ndt_neighbor_search_method == "DIRECT1") { + RCLCPP_INFO(this->get_logger(), "search_method DIRECT1 is selected"); + ndt->setNeighborSearchMethod(fast_gicp::NeighborSearchMethod::DIRECT1); + } else if (ndt_neighbor_search_method == "DIRECT7") { + RCLCPP_INFO(this->get_logger(), "search_method DIRECT7 is selected"); + ndt->setNeighborSearchMethod(fast_gicp::NeighborSearchMethod::DIRECT7); + } else if (ndt_neighbor_search_method == "DIRECT_RADIUS") { + RCLCPP_INFO_STREAM(this->get_logger(), "search_method DIRECT_RADIUS is selected : " << ndt_neighbor_search_radius); + ndt->setNeighborSearchMethod(fast_gicp::NeighborSearchMethod::DIRECT_RADIUS, ndt_neighbor_search_radius); + } else { + RCLCPP_WARN(this->get_logger(), "invalid search method was given"); + } + return ndt; + } + + RCLCPP_ERROR_STREAM(this->get_logger(), "unknown registration method:" << reg_method); + return nullptr; + } + + void initialize_params() { + // intialize scan matching method + this->declare_parameter("reg_method", "NDT_OMP"); + this->declare_parameter("ndt_neighbor_search_method", "DIRECT7"); + this->declare_parameter("ndt_neighbor_search_radius", 2.0); + this->declare_parameter("ndt_resolution", 1.0); + this->declare_parameter("downsample_resolution", 0.1); + this->declare_parameter("specify_init_pose", true); + this->declare_parameter("init_pos_x", 0.0); + this->declare_parameter("init_pos_y", 0.0); + this->declare_parameter("init_pos_z", 0.0); + this->declare_parameter("init_ori_w", 1.0); + this->declare_parameter("init_ori_x", 0.0); + this->declare_parameter("init_ori_y", 0.0); + this->declare_parameter("init_ori_z", 0.0); + this->declare_parameter("cool_time_duration", 0.5); + + double downsample_resolution = this->get_parameter("downsample_resolution").as_double(); + std::shared_ptr> voxelgrid(new pcl::VoxelGrid()); + voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); + downsample_filter = voxelgrid; + + RCLCPP_INFO(this->get_logger(), "create registration method for localization"); + registration = create_registration(); + + // global localization + RCLCPP_INFO(this->get_logger(), "create registration method for fallback during relocalization"); + relocalizing = false; + delta_estimater.reset(new DeltaEstimater(create_registration())); + + // initialize pose estimator + if(this->get_parameter("specify_init_pose").as_bool()) { + RCLCPP_INFO(this->get_logger(), "initialize pose estimator with specified parameters!!"); + pose_estimator.reset(new hdl_localization::PoseEstimator(registration, + Eigen::Vector3f(this->get_parameter("init_pos_x").as_double(), this->get_parameter("init_pos_y").as_double(), this->get_parameter("init_pos_z").as_double()), + Eigen::Quaternionf(this->get_parameter("init_ori_w").as_double(), this->get_parameter("init_ori_x").as_double(), this->get_parameter("init_ori_y").as_double(), this->get_parameter("init_ori_z").as_double()), + this->get_parameter("cool_time_duration").as_double() + )); + } + } + +private: + /** + * @brief callback for imu data + * @param imu_msg + */ + void imu_callback(const sensor_msgs::msg::Imu & imu_msg) { + std::lock_guard lock(imu_data_mutex); + imu_data.push_back(imu_msg); + } + + /** + * @brief callback for point cloud data + * @param points_msg + */ + void points_callback(const sensor_msgs::msg::PointCloud2 & points_msg) { + if(!globalmap) { + RCLCPP_ERROR(this->get_logger(), "globalmap has not been received!!"); + return; + } + + const auto& stamp = rclcpp::Time(points_msg.header.stamp); + pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud()); + pcl::fromROSMsg(points_msg, *pcl_cloud); + + if(pcl_cloud->empty()) { + RCLCPP_ERROR(this->get_logger(), "cloud is empty!!"); + return; + } + + // transform pointcloud into odom_child_frame_id + std::string tfError; + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + if(tf_buffer->canTransform(odom_child_frame_id, pcl_cloud->header.frame_id, stamp, rclcpp::Duration::from_seconds(0.1), &tfError)) + { + if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, *tf_buffer)) { + RCLCPP_ERROR(this->get_logger(), "point cloud cannot be transformed into target frame!!"); + return; + } + }else + { + RCLCPP_ERROR(this->get_logger(), tfError.c_str()); + return; + } + + auto filtered = downsample(cloud); + last_scan = filtered; + + if(relocalizing) { + delta_estimater->add_frame(filtered); + } + + std::lock_guard estimator_lock(pose_estimator_mutex); + if(!pose_estimator) { + RCLCPP_ERROR(this->get_logger(), "waiting for initial pose input!!"); + return; + } + Eigen::Matrix4f before = pose_estimator->matrix(); + + // predict + if(!use_imu) { + pose_estimator->predict(stamp); + } else { + std::lock_guard lock(imu_data_mutex); + auto imu_iter = imu_data.begin(); + for(imu_iter; imu_iter != imu_data.end(); imu_iter++) { + rclcpp::Time imu_stamp = rclcpp::Time(imu_iter->header.stamp); + if(stamp < imu_stamp) { + break; + } + const auto& acc = imu_iter->linear_acceleration; + const auto& gyro = imu_iter->angular_velocity; + double acc_sign = invert_acc ? -1.0 : 1.0; + double gyro_sign = invert_gyro ? -1.0 : 1.0; + pose_estimator->predict(imu_stamp, acc_sign * Eigen::Vector3f(acc.x, acc.y, acc.z), gyro_sign * Eigen::Vector3f(gyro.x, gyro.y, gyro.z)); + } + imu_data.erase(imu_data.begin(), imu_iter); + } + + // odometry-based prediction + rclcpp::Time last_correction_time = pose_estimator->last_correction_time(); + if(this->get_parameter("enable_robot_odometry_prediction").as_bool() && last_correction_time.nanoseconds() != 0) { + geometry_msgs::msg::TransformStamped odom_delta; + if(tf_buffer->canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, rclcpp::Duration::from_seconds(0.1))) { + odom_delta = tf_buffer->lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, rclcpp::Duration::from_seconds(0.0)); + } else if(tf_buffer->canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, rclcpp::Time(0), robot_odom_frame_id, rclcpp::Duration::from_seconds(0.0))) { + odom_delta = tf_buffer->lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, rclcpp::Time(0), robot_odom_frame_id, rclcpp::Duration::from_seconds(0.0)); + } + + if(rclcpp::Time(odom_delta.header.stamp).nanoseconds() == 0) { + RCLCPP_WARN_STREAM(this->get_logger(), "failed to look up transform between " << cloud->header.frame_id << " and " << robot_odom_frame_id); + } else { + Eigen::Isometry3d delta = tf2::transformToEigen(odom_delta); + pose_estimator->predict_odom(delta.cast().matrix()); + } + } + + // correct + auto aligned = pose_estimator->correct(stamp, filtered); + + if(aligned_pub->get_subscription_count()) { + sensor_msgs::msg::PointCloud2 aligned_msg; + pcl::toROSMsg(*aligned, aligned_msg); + aligned_msg.header.frame_id = "map"; + aligned_msg.header.stamp = pcl_conversions::fromPCL(cloud->header.stamp); + aligned_pub->publish(aligned_msg); + } + + if(status_pub->get_subscription_count()) { + publish_scan_matching_status(points_msg.header, aligned); + } + + publish_odometry(points_msg.header.stamp, pose_estimator->matrix()); + } + + /** + * @brief callback for globalmap input + * @param points_msg + */ + void globalmap_callback(const sensor_msgs::msg::PointCloud2 & points_msg) { + RCLCPP_INFO(this->get_logger(), "globalmap received!"); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + pcl::fromROSMsg(points_msg, *cloud); + globalmap = cloud; + + registration->setInputTarget(globalmap); + + if(use_global_localization) { + RCLCPP_INFO(this->get_logger(), "set globalmap for global localization!"); + auto request = std::make_shared(); + pcl::toROSMsg(*globalmap, request->global_map); + + while (!set_global_map_service->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + } + + auto result = set_global_map_service->async_send_request(request, std::bind(&HdlLocalization::set_global_map_response_callback, this, _1)); + } + } + + void set_global_map_response_callback(rclcpp::Client::SharedFuture future) { + if (future.wait_for(std::chrono::seconds(0)) == std::future_status::ready){ + RCLCPP_INFO(this->get_logger(), "done"); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to set global map"); + } + } + + /** + * @brief perform global localization to relocalize the sensor position + * @param + */ + void relocalize(const std::shared_ptr request, std::shared_ptr response) { + if(last_scan == nullptr) { + RCLCPP_INFO_STREAM(this->get_logger(), "no scan has been received"); + return; + } + + relocalizing = true; + delta_estimater->reset(); + pcl::PointCloud::ConstPtr scan = last_scan; + + auto srv = std::make_shared(); + pcl::toROSMsg(*scan, srv->cloud); + srv->max_num_candidates = 1; + + while (!query_global_localization_service->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + } + + auto resp = query_global_localization_service->async_send_request(srv, std::bind(&HdlLocalization::query_global_localization_callback, this, _1)); + } + + void query_global_localization_callback(rclcpp::Client::SharedFuture future) { + if (future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) { + if (future.get()->poses.empty()) { + relocalizing = false; + RCLCPP_INFO_STREAM(this->get_logger(), "global localization failed"); + return; + } + + const auto& result = future.get()->poses[0]; + + RCLCPP_INFO_STREAM(this->get_logger(), "--- Global localization result ---"); + RCLCPP_INFO_STREAM(this->get_logger(), "Trans :" << result.position.x << " " << result.position.y << " " << result.position.z); + RCLCPP_INFO_STREAM(this->get_logger(), "Quat :" << result.orientation.x << " " << result.orientation.y << " " << result.orientation.z << " " << result.orientation.w); + RCLCPP_INFO_STREAM(this->get_logger(), "Error :" << future.get()->errors[0]); + RCLCPP_INFO_STREAM(this->get_logger(), "Inlier:" << future.get()->inlier_fractions[0]); + + Eigen::Isometry3f pose = Eigen::Isometry3f::Identity(); + pose.linear() = Eigen::Quaternionf(result.orientation.w, result.orientation.x, result.orientation.y, result.orientation.z).toRotationMatrix(); + pose.translation() = Eigen::Vector3f(result.position.x, result.position.y, result.position.z); + pose = pose * delta_estimater->estimated_delta(); + + std::lock_guard lock(pose_estimator_mutex); + pose_estimator.reset(new hdl_localization::PoseEstimator( + registration, + pose.translation(), + Eigen::Quaternionf(pose.linear()), + this->get_parameter("cool_time_duration").as_double())); + + relocalizing = false; + } else { + relocalizing = false; + RCLCPP_INFO_STREAM(this->get_logger(), "global localization failed"); + return; + } + + } + + /** + * @brief callback for initial pose input ("2D Pose Estimate" on rviz) + * @param pose_msg + */ + void initialpose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped & pose_msg) { + RCLCPP_INFO(this->get_logger(), "initial pose received!!"); + std::lock_guard lock(pose_estimator_mutex); + const auto& p = pose_msg.pose.pose.position; + const auto& q = pose_msg.pose.pose.orientation; + pose_estimator.reset( + new hdl_localization::PoseEstimator( + registration, + Eigen::Vector3f(p.x, p.y, p.z), + Eigen::Quaternionf(q.w, q.x, q.y, q.z), + this->get_parameter("cool_time_duration").as_double()) + ); + } + + /** + * @brief downsampling + * @param cloud input cloud + * @return downsampled cloud + */ + pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& cloud) const { + if(!downsample_filter) { + return cloud; + } + + pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); + downsample_filter->setInputCloud(cloud); + downsample_filter->filter(*filtered); + filtered->header = cloud->header; + + return filtered; + } + + /** + * @brief publish odometry + * @param stamp timestamp + * @param pose odometry pose to be published + */ + void publish_odometry(const rclcpp::Time& stamp, const Eigen::Matrix4f& pose) { + // broadcast the transform over tf + if(tf_buffer->canTransform(robot_odom_frame_id, odom_child_frame_id, rclcpp::Time(0))) { + geometry_msgs::msg::TransformStamped map_wrt_frame = tf2::eigenToTransform(Eigen::Isometry3d(pose.inverse().cast())); + map_wrt_frame.header.stamp = stamp; + map_wrt_frame.header.frame_id = odom_child_frame_id; + map_wrt_frame.child_frame_id = "map"; + + geometry_msgs::msg::TransformStamped frame_wrt_odom = tf_buffer->lookupTransform(robot_odom_frame_id, odom_child_frame_id, rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); + Eigen::Matrix4f frame2odom = tf2::transformToEigen(frame_wrt_odom).cast().matrix(); + + geometry_msgs::msg::TransformStamped map_wrt_odom; + tf2::doTransform(map_wrt_frame, map_wrt_odom, frame_wrt_odom); + + tf2::Transform odom_wrt_map; + tf2::fromMsg(map_wrt_odom.transform, odom_wrt_map); + odom_wrt_map = odom_wrt_map.inverse(); + + geometry_msgs::msg::TransformStamped odom_trans; + odom_trans.transform = tf2::toMsg(odom_wrt_map); + odom_trans.header.stamp = stamp; + odom_trans.header.frame_id = "map"; + odom_trans.child_frame_id = robot_odom_frame_id; + + tf_broadcaster->sendTransform(odom_trans); + } else { + geometry_msgs::msg::TransformStamped odom_trans = tf2::eigenToTransform(Eigen::Isometry3d(pose.cast())); + odom_trans.header.stamp = stamp; + odom_trans.header.frame_id = "map"; + odom_trans.child_frame_id = odom_child_frame_id; + tf_broadcaster->sendTransform(odom_trans); + } + + // publish the transform + nav_msgs::msg::Odometry odom; + odom.header.stamp = stamp; + odom.header.frame_id = "map"; + + odom.pose.pose = tf2::toMsg(Eigen::Isometry3d(pose.cast())); + odom.child_frame_id = odom_child_frame_id; + odom.twist.twist.linear.x = 0.0; + odom.twist.twist.linear.y = 0.0; + odom.twist.twist.angular.z = 0.0; + + pose_pub->publish(odom); + } + + /** + * @brief publish scan matching status information + */ + void publish_scan_matching_status(const std_msgs::msg::Header& header, pcl::PointCloud::ConstPtr aligned) { + hdl_localization::msg::ScanMatchingStatus status; + status.header = header; + + status.has_converged = registration->hasConverged(); + status.matching_error = 0.0; + + const double max_correspondence_dist = this->get_parameter("status_max_correspondence_dist").as_double(); + const double max_valid_point_dist = this->get_parameter("status_max_valid_point_dist").as_double(); + + int num_inliers = 0; + int num_valid_points = 0; + std::vector k_indices; + std::vector k_sq_dists; + for(int i = 0; i < aligned->size(); i++) { + const auto& pt = aligned->at(i); + if (pt.getVector3fMap().norm() > max_valid_point_dist) { + continue; + } + num_valid_points++; + + registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists); + if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) { + status.matching_error += k_sq_dists[0]; + num_inliers++; + } + } + + status.matching_error /= num_inliers; + status.inlier_fraction = static_cast(num_inliers) / std::max(1, num_valid_points); + status.relative_pose = tf2::eigenToTransform(Eigen::Isometry3d(registration->getFinalTransformation().cast())).transform; + + status.prediction_labels.reserve(2); + status.prediction_errors.reserve(2); + + std::vector errors(6, 0.0); + + if(pose_estimator->wo_prediction_error()) { + status.prediction_labels.push_back(std_msgs::msg::String()); + status.prediction_labels.back().data = "without_pred"; + status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->wo_prediction_error().get().cast())).transform); + } + + if(pose_estimator->imu_prediction_error()) { + status.prediction_labels.push_back(std_msgs::msg::String()); + status.prediction_labels.back().data = use_imu ? "imu" : "motion_model"; + status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->imu_prediction_error().get().cast())).transform); + } + + if(pose_estimator->odom_prediction_error()) { + status.prediction_labels.push_back(std_msgs::msg::String()); + status.prediction_labels.back().data = "odom"; + status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->odom_prediction_error().get().cast())).transform); + } + + status_pub->publish(status); + } + +private: + std::string robot_odom_frame_id; + std::string odom_child_frame_id; + + bool use_imu; + bool invert_acc; + bool invert_gyro; + + // ROS2 + rclcpp::Subscription::SharedPtr imu_sub; + rclcpp::Subscription::SharedPtr points_sub; + rclcpp::Subscription::SharedPtr globalmap_sub; + rclcpp::Subscription::SharedPtr initialpose_sub; + + rclcpp::Publisher::SharedPtr pose_pub; + rclcpp::Publisher::SharedPtr aligned_pub; + rclcpp::Publisher::SharedPtr status_pub; + + + std::shared_ptr tf_listener{nullptr}; + std::unique_ptr tf_buffer; + std::unique_ptr tf_broadcaster; + + // imu input buffer + std::mutex imu_data_mutex; + std::vector imu_data; + + // globalmap and registration method + pcl::PointCloud::Ptr globalmap; + pcl::Filter::Ptr downsample_filter; + pcl::Registration::Ptr registration; + + // pose estimator + std::mutex pose_estimator_mutex; + std::unique_ptr pose_estimator; + + // global localization + bool use_global_localization; + std::atomic_bool relocalizing; + std::unique_ptr delta_estimater; + + pcl::PointCloud::ConstPtr last_scan; + rclcpp::Service::SharedPtr relocalize_server; + rclcpp::Client::SharedPtr set_global_map_service; + rclcpp::Client::SharedPtr query_global_localization_service; +}; +} + + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(hdl_localization::HdlLocalization) diff --git a/apps/hdl_localization_nodelet.cpp b/apps/hdl_localization_nodelet.cpp deleted file mode 100644 index 210f6481..00000000 --- a/apps/hdl_localization_nodelet.cpp +++ /dev/null @@ -1,554 +0,0 @@ -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include - -#include -#include -#include - -namespace hdl_localization { - -class HdlLocalizationNodelet : public nodelet::Nodelet { -public: - using PointT = pcl::PointXYZI; - - HdlLocalizationNodelet() : tf_buffer(), tf_listener(tf_buffer) { - } - virtual ~HdlLocalizationNodelet() { - } - - void onInit() override { - nh = getNodeHandle(); - mt_nh = getMTNodeHandle(); - private_nh = getPrivateNodeHandle(); - - initialize_params(); - - robot_odom_frame_id = private_nh.param("robot_odom_frame_id", "robot_odom"); - odom_child_frame_id = private_nh.param("odom_child_frame_id", "base_link"); - - use_imu = private_nh.param("use_imu", true); - invert_acc = private_nh.param("invert_acc", false); - invert_gyro = private_nh.param("invert_gyro", false); - if (use_imu) { - NODELET_INFO("enable imu-based prediction"); - imu_sub = mt_nh.subscribe("/gpsimu_driver/imu_data", 256, &HdlLocalizationNodelet::imu_callback, this); - } - points_sub = mt_nh.subscribe("/velodyne_points", 5, &HdlLocalizationNodelet::points_callback, this); - globalmap_sub = nh.subscribe("/globalmap", 1, &HdlLocalizationNodelet::globalmap_callback, this); - initialpose_sub = nh.subscribe("/initialpose", 8, &HdlLocalizationNodelet::initialpose_callback, this); - - pose_pub = nh.advertise("/odom", 5, false); - aligned_pub = nh.advertise("/aligned_points", 5, false); - status_pub = nh.advertise("/status", 5, false); - - // global localization - use_global_localization = private_nh.param("use_global_localization", true); - if(use_global_localization) { - NODELET_INFO_STREAM("wait for global localization services"); - ros::service::waitForService("/hdl_global_localization/set_global_map"); - ros::service::waitForService("/hdl_global_localization/query"); - - set_global_map_service = nh.serviceClient("/hdl_global_localization/set_global_map"); - query_global_localization_service = nh.serviceClient("/hdl_global_localization/query"); - - relocalize_server = nh.advertiseService("/relocalize", &HdlLocalizationNodelet::relocalize, this); - } - } - -private: - pcl::Registration::Ptr create_registration() const { - std::string reg_method = private_nh.param("reg_method", "NDT_OMP"); - std::string ndt_neighbor_search_method = private_nh.param("ndt_neighbor_search_method", "DIRECT7"); - double ndt_neighbor_search_radius = private_nh.param("ndt_neighbor_search_radius", 2.0); - double ndt_resolution = private_nh.param("ndt_resolution", 1.0); - - if(reg_method == "NDT_OMP") { - NODELET_INFO("NDT_OMP is selected"); - pclomp::NormalDistributionsTransform::Ptr ndt(new pclomp::NormalDistributionsTransform()); - ndt->setTransformationEpsilon(0.01); - ndt->setResolution(ndt_resolution); - if (ndt_neighbor_search_method == "DIRECT1") { - NODELET_INFO("search_method DIRECT1 is selected"); - ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1); - } else if (ndt_neighbor_search_method == "DIRECT7") { - NODELET_INFO("search_method DIRECT7 is selected"); - ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7); - } else { - if (ndt_neighbor_search_method == "KDTREE") { - NODELET_INFO("search_method KDTREE is selected"); - } else { - NODELET_WARN("invalid search method was given"); - NODELET_WARN("default method is selected (KDTREE)"); - } - ndt->setNeighborhoodSearchMethod(pclomp::KDTREE); - } - return ndt; - } else if(reg_method.find("NDT_CUDA") != std::string::npos) { - NODELET_INFO("NDT_CUDA is selected"); - boost::shared_ptr> ndt(new fast_gicp::NDTCuda); - ndt->setResolution(ndt_resolution); - - if(reg_method.find("D2D") != std::string::npos) { - ndt->setDistanceMode(fast_gicp::NDTDistanceMode::D2D); - } else if (reg_method.find("P2D") != std::string::npos) { - ndt->setDistanceMode(fast_gicp::NDTDistanceMode::P2D); - } - - if (ndt_neighbor_search_method == "DIRECT1") { - NODELET_INFO("search_method DIRECT1 is selected"); - ndt->setNeighborSearchMethod(fast_gicp::NeighborSearchMethod::DIRECT1); - } else if (ndt_neighbor_search_method == "DIRECT7") { - NODELET_INFO("search_method DIRECT7 is selected"); - ndt->setNeighborSearchMethod(fast_gicp::NeighborSearchMethod::DIRECT7); - } else if (ndt_neighbor_search_method == "DIRECT_RADIUS") { - NODELET_INFO_STREAM("search_method DIRECT_RADIUS is selected : " << ndt_neighbor_search_radius); - ndt->setNeighborSearchMethod(fast_gicp::NeighborSearchMethod::DIRECT_RADIUS, ndt_neighbor_search_radius); - } else { - NODELET_WARN("invalid search method was given"); - } - return ndt; - } - - NODELET_ERROR_STREAM("unknown registration method:" << reg_method); - return nullptr; - } - - void initialize_params() { - // intialize scan matching method - double downsample_resolution = private_nh.param("downsample_resolution", 0.1); - boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); - voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); - downsample_filter = voxelgrid; - - NODELET_INFO("create registration method for localization"); - registration = create_registration(); - - // global localization - NODELET_INFO("create registration method for fallback during relocalization"); - relocalizing = false; - delta_estimater.reset(new DeltaEstimater(create_registration())); - - // initialize pose estimator - if(private_nh.param("specify_init_pose", true)) { - NODELET_INFO("initialize pose estimator with specified parameters!!"); - pose_estimator.reset(new hdl_localization::PoseEstimator(registration, - Eigen::Vector3f(private_nh.param("init_pos_x", 0.0), private_nh.param("init_pos_y", 0.0), private_nh.param("init_pos_z", 0.0)), - Eigen::Quaternionf(private_nh.param("init_ori_w", 1.0), private_nh.param("init_ori_x", 0.0), private_nh.param("init_ori_y", 0.0), private_nh.param("init_ori_z", 0.0)), - private_nh.param("cool_time_duration", 0.5) - )); - } - } - -private: - /** - * @brief callback for imu data - * @param imu_msg - */ - void imu_callback(const sensor_msgs::ImuConstPtr& imu_msg) { - std::lock_guard lock(imu_data_mutex); - imu_data.push_back(imu_msg); - } - - /** - * @brief callback for point cloud data - * @param points_msg - */ - void points_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) { - if(!globalmap) { - NODELET_ERROR("globalmap has not been received!!"); - return; - } - - const auto& stamp = points_msg->header.stamp; - pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud()); - pcl::fromROSMsg(*points_msg, *pcl_cloud); - - if(pcl_cloud->empty()) { - NODELET_ERROR("cloud is empty!!"); - return; - } - - // transform pointcloud into odom_child_frame_id - std::string tfError; - pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); - if(this->tf_buffer.canTransform(odom_child_frame_id, pcl_cloud->header.frame_id, stamp, ros::Duration(0.1), &tfError)) - { - if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_buffer)) { - NODELET_ERROR("point cloud cannot be transformed into target frame!!"); - return; - } - }else - { - NODELET_ERROR(tfError.c_str()); - return; - } - - auto filtered = downsample(cloud); - last_scan = filtered; - - if(relocalizing) { - delta_estimater->add_frame(filtered); - } - - std::lock_guard estimator_lock(pose_estimator_mutex); - if(!pose_estimator) { - NODELET_ERROR("waiting for initial pose input!!"); - return; - } - Eigen::Matrix4f before = pose_estimator->matrix(); - - // predict - if(!use_imu) { - pose_estimator->predict(stamp); - } else { - std::lock_guard lock(imu_data_mutex); - auto imu_iter = imu_data.begin(); - for(imu_iter; imu_iter != imu_data.end(); imu_iter++) { - if(stamp < (*imu_iter)->header.stamp) { - break; - } - const auto& acc = (*imu_iter)->linear_acceleration; - const auto& gyro = (*imu_iter)->angular_velocity; - double acc_sign = invert_acc ? -1.0 : 1.0; - double gyro_sign = invert_gyro ? -1.0 : 1.0; - pose_estimator->predict((*imu_iter)->header.stamp, acc_sign * Eigen::Vector3f(acc.x, acc.y, acc.z), gyro_sign * Eigen::Vector3f(gyro.x, gyro.y, gyro.z)); - } - imu_data.erase(imu_data.begin(), imu_iter); - } - - // odometry-based prediction - ros::Time last_correction_time = pose_estimator->last_correction_time(); - if(private_nh.param("enable_robot_odometry_prediction", false) && !last_correction_time.isZero()) { - geometry_msgs::TransformStamped odom_delta; - if(tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0.1))) { - odom_delta = tf_buffer.lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0)); - } else if(tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0))) { - odom_delta = tf_buffer.lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0)); - } - - if(odom_delta.header.stamp.isZero()) { - NODELET_WARN_STREAM("failed to look up transform between " << cloud->header.frame_id << " and " << robot_odom_frame_id); - } else { - Eigen::Isometry3d delta = tf2::transformToEigen(odom_delta); - pose_estimator->predict_odom(delta.cast().matrix()); - } - } - - // correct - auto aligned = pose_estimator->correct(stamp, filtered); - - if(aligned_pub.getNumSubscribers()) { - aligned->header.frame_id = "map"; - aligned->header.stamp = cloud->header.stamp; - aligned_pub.publish(aligned); - } - - if(status_pub.getNumSubscribers()) { - publish_scan_matching_status(points_msg->header, aligned); - } - - publish_odometry(points_msg->header.stamp, pose_estimator->matrix()); - } - - /** - * @brief callback for globalmap input - * @param points_msg - */ - void globalmap_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) { - NODELET_INFO("globalmap received!"); - pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); - pcl::fromROSMsg(*points_msg, *cloud); - globalmap = cloud; - - registration->setInputTarget(globalmap); - - if(use_global_localization) { - NODELET_INFO("set globalmap for global localization!"); - hdl_global_localization::SetGlobalMap srv; - pcl::toROSMsg(*globalmap, srv.request.global_map); - - if(!set_global_map_service.call(srv)) { - NODELET_INFO("failed to set global map"); - } else { - NODELET_INFO("done"); - } - } - } - - /** - * @brief perform global localization to relocalize the sensor position - * @param - */ - bool relocalize(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& res) { - if(last_scan == nullptr) { - NODELET_INFO_STREAM("no scan has been received"); - return false; - } - - relocalizing = true; - delta_estimater->reset(); - pcl::PointCloud::ConstPtr scan = last_scan; - - hdl_global_localization::QueryGlobalLocalization srv; - pcl::toROSMsg(*scan, srv.request.cloud); - srv.request.max_num_candidates = 1; - - if(!query_global_localization_service.call(srv) || srv.response.poses.empty()) { - relocalizing = false; - NODELET_INFO_STREAM("global localization failed"); - return false; - } - - const auto& result = srv.response.poses[0]; - - NODELET_INFO_STREAM("--- Global localization result ---"); - NODELET_INFO_STREAM("Trans :" << result.position.x << " " << result.position.y << " " << result.position.z); - NODELET_INFO_STREAM("Quat :" << result.orientation.x << " " << result.orientation.y << " " << result.orientation.z << " " << result.orientation.w); - NODELET_INFO_STREAM("Error :" << srv.response.errors[0]); - NODELET_INFO_STREAM("Inlier:" << srv.response.inlier_fractions[0]); - - Eigen::Isometry3f pose = Eigen::Isometry3f::Identity(); - pose.linear() = Eigen::Quaternionf(result.orientation.w, result.orientation.x, result.orientation.y, result.orientation.z).toRotationMatrix(); - pose.translation() = Eigen::Vector3f(result.position.x, result.position.y, result.position.z); - pose = pose * delta_estimater->estimated_delta(); - - std::lock_guard lock(pose_estimator_mutex); - pose_estimator.reset(new hdl_localization::PoseEstimator( - registration, - pose.translation(), - Eigen::Quaternionf(pose.linear()), - private_nh.param("cool_time_duration", 0.5))); - - relocalizing = false; - - return true; - } - - /** - * @brief callback for initial pose input ("2D Pose Estimate" on rviz) - * @param pose_msg - */ - void initialpose_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg) { - NODELET_INFO("initial pose received!!"); - std::lock_guard lock(pose_estimator_mutex); - const auto& p = pose_msg->pose.pose.position; - const auto& q = pose_msg->pose.pose.orientation; - pose_estimator.reset( - new hdl_localization::PoseEstimator( - registration, - Eigen::Vector3f(p.x, p.y, p.z), - Eigen::Quaternionf(q.w, q.x, q.y, q.z), - private_nh.param("cool_time_duration", 0.5)) - ); - } - - /** - * @brief downsampling - * @param cloud input cloud - * @return downsampled cloud - */ - pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& cloud) const { - if(!downsample_filter) { - return cloud; - } - - pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); - downsample_filter->setInputCloud(cloud); - downsample_filter->filter(*filtered); - filtered->header = cloud->header; - - return filtered; - } - - /** - * @brief publish odometry - * @param stamp timestamp - * @param pose odometry pose to be published - */ - void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose) { - // broadcast the transform over tf - if(tf_buffer.canTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0))) { - geometry_msgs::TransformStamped map_wrt_frame = tf2::eigenToTransform(Eigen::Isometry3d(pose.inverse().cast())); - map_wrt_frame.header.stamp = stamp; - map_wrt_frame.header.frame_id = odom_child_frame_id; - map_wrt_frame.child_frame_id = "map"; - - geometry_msgs::TransformStamped frame_wrt_odom = tf_buffer.lookupTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0), ros::Duration(0.1)); - Eigen::Matrix4f frame2odom = tf2::transformToEigen(frame_wrt_odom).cast().matrix(); - - geometry_msgs::TransformStamped map_wrt_odom; - tf2::doTransform(map_wrt_frame, map_wrt_odom, frame_wrt_odom); - - tf2::Transform odom_wrt_map; - tf2::fromMsg(map_wrt_odom.transform, odom_wrt_map); - odom_wrt_map = odom_wrt_map.inverse(); - - geometry_msgs::TransformStamped odom_trans; - odom_trans.transform = tf2::toMsg(odom_wrt_map); - odom_trans.header.stamp = stamp; - odom_trans.header.frame_id = "map"; - odom_trans.child_frame_id = robot_odom_frame_id; - - tf_broadcaster.sendTransform(odom_trans); - } else { - geometry_msgs::TransformStamped odom_trans = tf2::eigenToTransform(Eigen::Isometry3d(pose.cast())); - odom_trans.header.stamp = stamp; - odom_trans.header.frame_id = "map"; - odom_trans.child_frame_id = odom_child_frame_id; - tf_broadcaster.sendTransform(odom_trans); - } - - // publish the transform - nav_msgs::Odometry odom; - odom.header.stamp = stamp; - odom.header.frame_id = "map"; - - tf::poseEigenToMsg(Eigen::Isometry3d(pose.cast()), odom.pose.pose); - odom.child_frame_id = odom_child_frame_id; - odom.twist.twist.linear.x = 0.0; - odom.twist.twist.linear.y = 0.0; - odom.twist.twist.angular.z = 0.0; - - pose_pub.publish(odom); - } - - /** - * @brief publish scan matching status information - */ - void publish_scan_matching_status(const std_msgs::Header& header, pcl::PointCloud::ConstPtr aligned) { - ScanMatchingStatus status; - status.header = header; - - status.has_converged = registration->hasConverged(); - status.matching_error = 0.0; - - const double max_correspondence_dist = private_nh.param("status_max_correspondence_dist", 0.5); - const double max_valid_point_dist = private_nh.param("status_max_valid_point_dist", 25.0); - - int num_inliers = 0; - int num_valid_points = 0; - std::vector k_indices; - std::vector k_sq_dists; - for(int i = 0; i < aligned->size(); i++) { - const auto& pt = aligned->at(i); - if (pt.getVector3fMap().norm() > max_valid_point_dist) { - continue; - } - num_valid_points++; - - registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists); - if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) { - status.matching_error += k_sq_dists[0]; - num_inliers++; - } - } - - status.matching_error /= num_inliers; - status.inlier_fraction = static_cast(num_inliers) / std::max(1, num_valid_points); - status.relative_pose = tf2::eigenToTransform(Eigen::Isometry3d(registration->getFinalTransformation().cast())).transform; - - status.prediction_labels.reserve(2); - status.prediction_errors.reserve(2); - - std::vector errors(6, 0.0); - - if(pose_estimator->wo_prediction_error()) { - status.prediction_labels.push_back(std_msgs::String()); - status.prediction_labels.back().data = "without_pred"; - status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->wo_prediction_error().get().cast())).transform); - } - - if(pose_estimator->imu_prediction_error()) { - status.prediction_labels.push_back(std_msgs::String()); - status.prediction_labels.back().data = use_imu ? "imu" : "motion_model"; - status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->imu_prediction_error().get().cast())).transform); - } - - if(pose_estimator->odom_prediction_error()) { - status.prediction_labels.push_back(std_msgs::String()); - status.prediction_labels.back().data = "odom"; - status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->odom_prediction_error().get().cast())).transform); - } - - status_pub.publish(status); - } - -private: - // ROS - ros::NodeHandle nh; - ros::NodeHandle mt_nh; - ros::NodeHandle private_nh; - - std::string robot_odom_frame_id; - std::string odom_child_frame_id; - - bool use_imu; - bool invert_acc; - bool invert_gyro; - ros::Subscriber imu_sub; - ros::Subscriber points_sub; - ros::Subscriber globalmap_sub; - ros::Subscriber initialpose_sub; - - ros::Publisher pose_pub; - ros::Publisher aligned_pub; - ros::Publisher status_pub; - - tf2_ros::Buffer tf_buffer; - tf2_ros::TransformListener tf_listener; - tf2_ros::TransformBroadcaster tf_broadcaster; - - // imu input buffer - std::mutex imu_data_mutex; - std::vector imu_data; - - // globalmap and registration method - pcl::PointCloud::Ptr globalmap; - pcl::Filter::Ptr downsample_filter; - pcl::Registration::Ptr registration; - - // pose estimator - std::mutex pose_estimator_mutex; - std::unique_ptr pose_estimator; - - // global localization - bool use_global_localization; - std::atomic_bool relocalizing; - std::unique_ptr delta_estimater; - - pcl::PointCloud::ConstPtr last_scan; - ros::ServiceServer relocalize_server; - ros::ServiceClient set_global_map_service; - ros::ServiceClient query_global_localization_service; -}; -} - - -PLUGINLIB_EXPORT_CLASS(hdl_localization::HdlLocalizationNodelet, nodelet::Nodelet) diff --git a/hdl_localization/__init__.py b/hdl_localization/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/include/hdl_localization/pose_estimator.hpp b/include/hdl_localization/pose_estimator.hpp index 1dcc53ec..bbdd43ff 100644 --- a/include/hdl_localization/pose_estimator.hpp +++ b/include/hdl_localization/pose_estimator.hpp @@ -4,7 +4,7 @@ #include #include -#include +#include #include #include #include @@ -41,7 +41,7 @@ class PoseEstimator { * @brief predict * @param stamp timestamp */ - void predict(const ros::Time& stamp); + void predict(const rclcpp::Time& stamp); /** * @brief predict @@ -49,7 +49,7 @@ class PoseEstimator { * @param acc acceleration * @param gyro angular velocity */ - void predict(const ros::Time& stamp, const Eigen::Vector3f& acc, const Eigen::Vector3f& gyro); + void predict(const rclcpp::Time& stamp, const Eigen::Vector3f& acc, const Eigen::Vector3f& gyro); /** * @brief update the state of the odomety-based pose estimation @@ -61,10 +61,10 @@ class PoseEstimator { * @param cloud input cloud * @return cloud aligned to the globalmap */ - pcl::PointCloud::Ptr correct(const ros::Time& stamp, const pcl::PointCloud::ConstPtr& cloud); + pcl::PointCloud::Ptr correct(const rclcpp::Time& stamp, const pcl::PointCloud::ConstPtr& cloud); /* getters */ - ros::Time last_correction_time() const; + rclcpp::Time last_correction_time() const; Eigen::Vector3f pos() const; Eigen::Vector3f vel() const; @@ -80,9 +80,9 @@ class PoseEstimator { const boost::optional& odom_prediction_error() const; private: - ros::Time init_stamp; // when the estimator was initialized - ros::Time prev_stamp; // when the estimator was updated last time - ros::Time last_correction_stamp; // when the estimator performed the correction step + rclcpp::Time init_stamp; // when the estimator was initialized + rclcpp::Time prev_stamp; // when the estimator was updated last time + rclcpp::Time last_correction_stamp; // when the estimator performed the correction step double cool_time_duration; // Eigen::MatrixXf process_noise; diff --git a/launch/hdl_localization.launch b/launch/hdl_localization.launch deleted file mode 100644 index fbf06606..00000000 --- a/launch/hdl_localization.launch +++ /dev/null @@ -1,70 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/hdl_localization.launch.py b/launch/hdl_localization.launch.py new file mode 100644 index 00000000..e8e87a4a --- /dev/null +++ b/launch/hdl_localization.launch.py @@ -0,0 +1,106 @@ +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.conditions import IfCondition +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ThisLaunchFileDir +from launch_ros.descriptions import ComposableNode, ParameterFile + + +def generate_launch_description(): + component_manager = LaunchConfiguration( + "component_manager", default="hdl_localization_component_manager" + ) + points_topic = LaunchConfiguration("points_topic", default="/velodyne_points") + globalmap_pcd = LaunchConfiguration( + "globalmap_pcd", default=os.path.join(os.path.expanduser("~"), "map.pcd") + ) + imu_topic = LaunchConfiguration("imu_topic", default="/imu/data") + use_global_localization = LaunchConfiguration( + "use_global_localization", default=True + ) + plot_estimation_errors = LaunchConfiguration( + "plot_estimation_errors", default=False + ) + + params_file = LaunchConfiguration( + "params_file", + default=os.path.join( + get_package_share_directory("hdl_localization"), + "param", + "hdl_localization.yaml", + ), + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + "component_manager", + default_value=component_manager, + description="name of component manager", + ), + DeclareLaunchArgument("imu_topic", default_value=imu_topic), + DeclareLaunchArgument( + "points_topic", + default_value=points_topic, + description="topic of point cloud", + ), + DeclareLaunchArgument("globalmap_pcd", default_value=globalmap_pcd), + DeclareLaunchArgument( + "use_global_localization", default_value=use_global_localization + ), + DeclareLaunchArgument( + "plot_estimation_errors", default_value=plot_estimation_errors + ), + DeclareLaunchArgument("params_file", default_value=params_file), + Node( + package="hdl_global_localization", + executable="hdl_global_localization_node", + name="hdl_global_localization_node", + namespace="hdl_global_localization", + condition=IfCondition(use_global_localization), + ), + ComposableNodeContainer( + name=component_manager, + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="hdl_localization", + plugin="hdl_localization::GlobalmapServer", + name="global_map_server", + parameters=[params_file, {"globalmap_pcd": globalmap_pcd}], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="hdl_localization", + plugin="hdl_localization::HdlLocalization", + name="hdl_localization", + remappings=[ + ("/velodyne_points", points_topic), + ("/gpsimu_driver/imu_data", imu_topic), + ], + parameters=[ + params_file, + {"use_global_localization": use_global_localization}, + ], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + ), + Node( + package="hdl_localization", + executable="plot_status.py", + name="plot_estimation_errors", + condition=IfCondition(plot_estimation_errors), + ), + ] + ) diff --git a/msg/ScanMatchingStatus.msg b/msg/ScanMatchingStatus.msg index 1b2a0152..480defba 100644 --- a/msg/ScanMatchingStatus.msg +++ b/msg/ScanMatchingStatus.msg @@ -1,4 +1,4 @@ -Header header +std_msgs/Header header bool has_converged float32 matching_error diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml deleted file mode 100644 index a25c0da8..00000000 --- a/nodelet_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/package.xml b/package.xml index 6b8293a9..1997fce1 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,6 @@ - + + hdl_localization 0.0.0 The hdl_localization package @@ -8,40 +9,47 @@ BSD - catkin - nodelet + ament_cmake + ament_cmake_python + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + rclcpp + rclpy + rclcpp_action + rclcpp_components + geometry_msgs + sensor_msgs + tf2_geometry_msgs + std_srvs + nav_msgs + std_msgs + + tf2 tf2_ros tf2_eigen - tf2_geometry_msgs - eigen_conversions pcl_ros roscpp - rospy - sensor_msgs - geometry_msgs - message_generation ndt_omp fast_gicp hdl_global_localization - nodelet - tf2 - tf2_ros - tf2_eigen - tf2_geometry_msgs - eigen_conversions - pcl_ros - roscpp - rospy - sensor_msgs - geometry_msgs - message_generation - ndt_omp - fast_gicp - hdl_global_localization + tf2 + tf2_ros + tf2_eigen + pcl_ros + ndt_omp + fast_gicp + hdl_global_localization + + ament_lint_auto + ament_lint_common - + ament_cmake - + \ No newline at end of file diff --git a/param/hdl_localization.yaml b/param/hdl_localization.yaml new file mode 100644 index 00000000..d8ea71b7 --- /dev/null +++ b/param/hdl_localization.yaml @@ -0,0 +1,32 @@ +hdl_localization: + ros__parameters: + # input clouds are transformed in odom_child_frame, and then localization is performed in that frame. + # this is useful to match the LIDAR and IMU coodinate systems. + odom_child_frame_id: velodyne + use_imu: false + invert_imu_acc: false + invert_imu_gyro: false + enable_robot_odometry_prediction: false + robot_odom_frame_id: odom + cool_time_duration: 2.0 # during "cool_time", imu inputs are ignored + reg_method: NDT_OMP # ndt settings. available reg_methods: NDT_OMP, NDT_CUDA_P2D, NDT_CUDA_D2D + # if NDT is slow for your PC, try DIRECT1 serach method, which is a bit unstable but extremely fast. + ndt_neighbor_search_method: DIRECT7 + ndt_neighbor_search_radius: 2.0 + ndt_resolution: 1.0 + downsample_resolution: 0.1 + # if "specify_init_pose" is true, pose estimator will be initialized with the following params. + # otherwise, you need to input an initial pose with "2D Pose Estimate" on rviz" + specify_init_pose: true + init_pos_x: 0.0 + init_pos_y: 0.0 + init_pos_z: 0.0 + init_ori_w: 1.0 + init_ori_x: 0.0 + init_ori_y: 0.0 + init_ori_z: 0.0 + +global_map_server: + ros__parameters: + convert_utm_to_local: true + downsample_resolution: 0.1 \ No newline at end of file diff --git a/scripts/plot_status.py b/scripts/plot_status.py index c8ab91c9..1149495f 100755 --- a/scripts/plot_status.py +++ b/scripts/plot_status.py @@ -1,71 +1,91 @@ -#!/usr/bin/python3 -import rospy +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile + import numpy import scipy.spatial from matplotlib import pyplot -from hdl_localization.msg import * +from hdl_localization.msg import ScanMatchingStatus + + +class Plotter(Node): + def __init__(self): + super().__init__("status_plotter") -class Plotter(object): - def __init__(self): - pyplot.ion() - pyplot.show(block=False) + pyplot.ion() + pyplot.show(block=False) - self.status_buffer = [] - self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback) - self.status_sub = rospy.Subscriber('/status', ScanMatchingStatus, self.status_callback) + self.status_buffer = [] + self.timer = self.create_timer(0.1, self.timer_callback) + self.status_sub = self.create_subscription( + ScanMatchingStatus, "/status", self.status_callback, QoSProfile(history=2) + ) - def status_callback(self, status_msg): - self.status_buffer.append(status_msg) + def status_callback(self, status_msg): + self.status_buffer.append(status_msg) - if len(self.status_buffer) > 50: - self.status_buffer = self.status_buffer[-50:] + if len(self.status_buffer) > 50: + self.status_buffer = self.status_buffer[-50:] - def timer_callback(self, event): - if len(self.status_buffer) < 2: - return + def timer_callback(self): + if len(self.status_buffer) < 2: + return - errors = {} - for status in self.status_buffer: - for label, error in zip(status.prediction_labels, status.prediction_errors): - if label.data not in errors: - errors[label.data] = [] + errors = {} + for status in self.status_buffer: + for label, error in zip(status.prediction_labels, status.prediction_errors): + if label.data not in errors: + errors[label.data] = [] - quat = [error.rotation.x, error.rotation.y, error.rotation.z, error.rotation.w] - trans = [error.translation.x, error.translation.y, error.translation.z] + quat = [ + error.rotation.x, + error.rotation.y, + error.rotation.z, + error.rotation.w, + ] + trans = [error.translation.x, error.translation.y, error.translation.z] - t = status.header.stamp.secs + status.header.stamp.nsecs / 1e9 - t_error = numpy.linalg.norm(trans) - r_error = numpy.linalg.norm(scipy.spatial.transform.Rotation.from_quat(quat).as_rotvec()) + t = status.header.stamp.sec + status.header.stamp.nanosec / 1e9 + t_error = numpy.linalg.norm(trans) + r_error = numpy.linalg.norm( + scipy.spatial.transform.Rotation.from_quat(quat).as_rotvec() + ) - if len(errors[label.data]) and abs(errors[label.data][-1][0] - t) > 1.0: - errors[label.data] = [] + if len(errors[label.data]) and abs(errors[label.data][-1][0] - t) > 1.0: + errors[label.data] = [] - errors[label.data].append((t, t_error, r_error)) + errors[label.data].append((t, t_error, r_error)) - pyplot.clf() - for label in errors: - errs = numpy.float64(errors[label]) - pyplot.subplot('211') - pyplot.plot(errs[:, 0], errs[:, 1], label=label) + pyplot.clf() + for label in errors: + errs = numpy.float64(errors[label]) + pyplot.subplot(211) + pyplot.plot(errs[:, 0], errs[:, 1], label=label) - pyplot.subplot('212') - pyplot.plot(errs[:, 0], errs[:, 2], label=label) + pyplot.subplot(212) + pyplot.plot(errs[:, 0], errs[:, 2], label=label) - pyplot.subplot('211') - pyplot.ylabel('trans error') - pyplot.subplot('212') - pyplot.ylabel('rot error') + pyplot.subplot(211) + pyplot.ylabel("trans error") + pyplot.subplot(212) + pyplot.ylabel("rot error") - pyplot.legend(loc='upper center', bbox_to_anchor=(0.5, -0.05), ncol=len(errors)) - pyplot.gcf().canvas.flush_events() - # pyplot.pause(0.0001) + pyplot.legend(loc="upper center", bbox_to_anchor=(0.5, -0.05), ncol=len(errors)) + pyplot.gcf().canvas.flush_events() + # pyplot.pause(0.0001) def main(): - rospy.init_node('status_plotter') - node = Plotter() - rospy.spin() + rclpy.init() + + node = Plotter() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + -if __name__ == '__main__': - main() +if __name__ == "__main__": + main() diff --git a/src/hdl_localization/pose_estimator.cpp b/src/hdl_localization/pose_estimator.cpp index d3135028..8a265e4d 100644 --- a/src/hdl_localization/pose_estimator.cpp +++ b/src/hdl_localization/pose_estimator.cpp @@ -52,17 +52,17 @@ PoseEstimator::~PoseEstimator() {} * @param acc acceleration * @param gyro angular velocity */ -void PoseEstimator::predict(const ros::Time& stamp) { - if (init_stamp.is_zero()) { +void PoseEstimator::predict(const rclcpp::Time& stamp) { + if (init_stamp.nanoseconds() == 0) { init_stamp = stamp; } - if ((stamp - init_stamp).toSec() < cool_time_duration || prev_stamp.is_zero() || prev_stamp == stamp) { + if ((stamp - init_stamp).seconds() < cool_time_duration || prev_stamp.nanoseconds() == 0 || prev_stamp == stamp) { prev_stamp = stamp; return; } - double dt = (stamp - prev_stamp).toSec(); + double dt = (stamp - prev_stamp).seconds(); prev_stamp = stamp; ukf->setProcessNoiseCov(process_noise * dt); @@ -77,17 +77,17 @@ void PoseEstimator::predict(const ros::Time& stamp) { * @param acc acceleration * @param gyro angular velocity */ -void PoseEstimator::predict(const ros::Time& stamp, const Eigen::Vector3f& acc, const Eigen::Vector3f& gyro) { - if (init_stamp.is_zero()) { +void PoseEstimator::predict(const rclcpp::Time& stamp, const Eigen::Vector3f& acc, const Eigen::Vector3f& gyro) { + if (init_stamp.nanoseconds() == 0) { init_stamp = stamp; } - if ((stamp - init_stamp).toSec() < cool_time_duration || prev_stamp.is_zero() || prev_stamp == stamp) { + if ((stamp - init_stamp).seconds() < cool_time_duration || prev_stamp.nanoseconds() == 0 || prev_stamp == stamp) { prev_stamp = stamp; return; } - double dt = (stamp - prev_stamp).toSec(); + double dt = (stamp - prev_stamp).seconds(); prev_stamp = stamp; ukf->setProcessNoiseCov(process_noise * dt); @@ -140,8 +140,8 @@ void PoseEstimator::predict_odom(const Eigen::Matrix4f& odom_delta) { * @param cloud input cloud * @return cloud aligned to the globalmap */ -pcl::PointCloud::Ptr PoseEstimator::correct(const ros::Time& stamp, const pcl::PointCloud::ConstPtr& cloud) { - if (init_stamp.is_zero()) { +pcl::PointCloud::Ptr PoseEstimator::correct(const rclcpp::Time& stamp, const pcl::PointCloud::ConstPtr& cloud) { + if (init_stamp.nanoseconds() == 0) { init_stamp = stamp; } @@ -220,7 +220,7 @@ pcl::PointCloud::Ptr PoseEstimator::correct(const ros::Ti } /* getters */ -ros::Time PoseEstimator::last_correction_time() const { +rclcpp::Time PoseEstimator::last_correction_time() const { return last_correction_stamp; }