From 44a64165f1d6a612c618cebb7fb0eadc71d7266a Mon Sep 17 00:00:00 2001 From: Ruben Garcia <84038639+rgarciaruiz@users.noreply.github.com> Date: Thu, 18 Jul 2024 17:07:23 -0700 Subject: [PATCH 1/2] Fix dds_ros_bridge shutdown so it exits cleanly (#802) Nodelet caused segfault on shutdown which prevented its reload (manager died with it). DDS objects were attempting read/close operations after factory was destroyed leading to segfault inside soraCore. Order of declaration matters, this change lets the factory be the last soraCore reference to be destroyed. Co-authored-by: Ruben Garcia --- .../dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h b/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h index a488d1a988..d51c2c9590 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h @@ -229,8 +229,8 @@ class DdsRosBridge : public ff_util::FreeFlyerNodelet { ros::Publisher robot_name_pub_; ros::ServiceServer srv_set_telem_rate_; - std::map ros_sub_rapid_pubs_; std::shared_ptr dds_entities_factory_; + std::map ros_sub_rapid_pubs_; std::string agent_name_, participant_name_; std::vector rapid_pubs_; std::vector rapid_sub_ros_pubs_; From c4b509c2ba15a7bb4d609ee6e76044d68cc34147 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Tue, 23 Jul 2024 13:47:48 -0700 Subject: [PATCH 2/2] Fixes from ISAAC16 (#800) * updated registration timeout * updated gl duration * updated brisk params * added vl runtime to msg * fixed loc nodelet read params bug * Release 0.19.1 * don't publish haz cam in sim by default * added scaling when changing threshold ratios for teblid * added rounding when casting interest point dynamic thresholds * updated teblid default thres * added toomany/toofew ratios as params * adding adjusting num similar images * fixed loc header * remove else for adjust num similar * visual landmarks adding field bmr rule * updated loc min success rate config * set valid to true * localizer parameter in test changed * fixing release spaces and # * fixed recording toomany and toofew ratios during mapping * avoid adjusting thresholds if success history size is 0 * Removed essential matrix check and adjusted hamming distances * updated loc configs * Avoid storing two maps in memory at once * added check for feature descriptor during a map switch * added protection against switching to an invalid map * added set start pose tool * enable loc after revert to last map * added missing read params after revert from map switch * added sleep for subscribing to clock * added time delay to set start pose * upgraded loc analysis scripts to python3 * upgraded sparse map scripts to python3 * updated min faetuers in make teblid map * updated make map script * added loc coverage display tool for rviz --------- Co-authored-by: Marina Moreira Co-authored-by: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com> --- astrobee/config/localization.config | 32 +++---- .../include/localization_node/localization.h | 2 +- .../localization_node/localization_nodelet.h | 6 +- .../localization_node/src/localization.cc | 22 ++++- .../src/localization_nodelet.cc | 52 ++++++++-- .../localization_node/tools/set_start_pose.py | 95 ++++++++++++++++++ .../include/sparse_mapping/sparse_mapping.h | 2 +- .../sparse_mapping/scripts/make_surf_map.py | 2 +- .../sparse_mapping/scripts/make_surf_maps.py | 2 +- .../scripts/make_teblid512_map.py | 4 +- .../scripts/process_sequential_images.py | 2 +- .../scripts/prune_partitioned_directories.py | 2 +- .../sparse_mapping/src/sparse_mapping.cc | 14 ++- .../scripts/depth_odometry_parameter_sweep.py | 2 +- .../scripts/groundtruth_sweep.py | 2 +- .../scripts/imu_analyzer.py | 2 +- .../scripts/make_groundtruth.py | 2 +- .../localization_analysis/scripts/make_map.py | 4 +- .../scripts/plot_helpers.py | 2 +- .../localization_rviz_plugins/CMakeLists.txt | 17 ++-- .../nodelet_plugins.xml | 7 ++ tools/localization_rviz_plugins/readme.md | 3 + .../src/localization_coverage_display.cc | 96 +++++++++++++++++++ .../src/localization_coverage_display.h | 61 ++++++++++++ 24 files changed, 378 insertions(+), 57 deletions(-) create mode 100755 localization/localization_node/tools/set_start_pose.py create mode 100644 tools/localization_rviz_plugins/src/localization_coverage_display.cc create mode 100644 tools/localization_rviz_plugins/src/localization_coverage_display.h diff --git a/astrobee/config/localization.config b/astrobee/config/localization.config index 7bfe466c07..e5ef8e651a 100644 --- a/astrobee/config/localization.config +++ b/astrobee/config/localization.config @@ -54,69 +54,69 @@ brisk_min_num_similar = 20 brisk_max_num_similar = 20 -- TEBLID512 -teblid512_num_similar = 20 +teblid512_num_similar = 18 teblid512_min_query_score_ratio = 0.45 teblid512_ransac_inlier_tolerance = 3 teblid512_num_ransac_iterations = 1000 teblid512_early_break_landmarks = 100 teblid512_histogram_equalization = 3 -teblid512_check_essential_matrix = true +teblid512_check_essential_matrix = false teblid512_essential_ransac_iterations = 2000 teblid512_add_similar_images = true teblid512_add_best_previous_image = true -teblid512_hamming_distance = 85 +teblid512_hamming_distance = 90 teblid512_goodness_ratio = 0.8 teblid512_use_clahe = true teblid512_num_extra_localization_db_images = 0 -- Detection Params -teblid512_min_threshold = 20.0 +teblid512_min_threshold = 30.0 teblid512_default_threshold = 72.0 teblid512_max_threshold = 110.0 teblid512_min_features = 1000 -teblid512_max_features = 3000 -teblid512_too_many_ratio = 1.05 +teblid512_max_features = 2750 +teblid512_too_many_ratio = 1.07 teblid512_too_few_ratio = 0.95 teblid512_detection_retries = 1 -- Localizer Threshold Params teblid512_success_history_size = 10 -teblid512_min_success_rate = 0.5 +teblid512_min_success_rate = 0.4 teblid512_max_success_rate = 0.9 teblid512_adjust_num_similar = true teblid512_min_num_similar = 15 -teblid512_max_num_similar = 20 +teblid512_max_num_similar = 18 -- TEBLID256 -teblid256_num_similar = 20 +teblid256_num_similar = 18 teblid256_min_query_score_ratio = 0.45 teblid256_ransac_inlier_tolerance = 3 teblid256_num_ransac_iterations = 1000 teblid256_early_break_landmarks = 100 teblid256_histogram_equalization = 3 -teblid256_check_essential_matrix = true +teblid256_check_essential_matrix = false teblid256_essential_ransac_iterations = 2000 teblid256_add_similar_images = true teblid256_add_best_previous_image = true -teblid256_hamming_distance = 85 +teblid256_hamming_distance = 90 teblid256_goodness_ratio = 0.8 teblid256_use_clahe = true teblid256_num_extra_localization_db_images = 0 -- Detection Params -teblid256_min_threshold = 20.0 +teblid256_min_threshold = 30.0 teblid256_default_threshold = 72.0 teblid256_max_threshold = 110.0 teblid256_min_features = 1000 -teblid256_max_features = 3000 -teblid256_too_many_ratio = 1.05 +teblid256_max_features = 2750 +teblid256_too_many_ratio = 1.07 teblid256_too_few_ratio = 0.95 teblid256_detection_retries = 1 -- Localizer Threshold Params teblid256_success_history_size = 10 -teblid256_min_success_rate = 0.5 +teblid256_min_success_rate = 0.4 teblid256_max_success_rate = 0.9 teblid256_adjust_num_similar = true teblid256_min_num_similar = 15 -teblid256_max_num_similar = 20 +teblid256_max_num_similar = 18 diff --git a/localization/localization_node/include/localization_node/localization.h b/localization/localization_node/include/localization_node/localization.h index 7a22665ad3..9278f3aa00 100644 --- a/localization/localization_node/include/localization_node/localization.h +++ b/localization/localization_node/include/localization_node/localization.h @@ -45,7 +45,7 @@ struct ThresholdParams { class Localizer { public: explicit Localizer(sparse_mapping::SparseMap* map); - void ReadParams(config_reader::ConfigReader& config); + bool ReadParams(config_reader::ConfigReader& config, bool fatal_failure = true); bool Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLandmarks* vl, Eigen::Matrix2Xd* image_keypoints = NULL); private: diff --git a/localization/localization_node/include/localization_node/localization_nodelet.h b/localization/localization_node/include/localization_node/localization_nodelet.h index 6a13e1a467..4593a72bb5 100644 --- a/localization/localization_node/include/localization_node/localization_nodelet.h +++ b/localization/localization_node/include/localization_node/localization_nodelet.h @@ -44,7 +44,10 @@ class LocalizationNodelet : public ff_util::FreeFlyerNodelet { virtual void Initialize(ros::NodeHandle* nh); private: - void ReadParams(void); + // Wrapper function that calls ReadParams but does not take a param, + // required for configuring with a config timer. + void ReadParamsWrapper(); + bool ReadParams(bool fatal_failure = true); bool ResetMap(const std::string& map_file); void Run(void); void Localize(void); @@ -56,6 +59,7 @@ class LocalizationNodelet : public ff_util::FreeFlyerNodelet { std::shared_ptr map_; std::shared_ptr thread_; config_reader::ConfigReader config_; + std::string last_valid_map_file_; ros::Timer config_timer_; std::shared_ptr it_; diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index a4eba47b28..7ba01f1265 100644 --- a/localization/localization_node/src/localization.cc +++ b/localization/localization_node/src/localization.cc @@ -29,7 +29,7 @@ namespace localization_node { Localizer::Localizer(sparse_mapping::SparseMap* map): map_(map) {} -void Localizer::ReadParams(config_reader::ConfigReader& config) { +bool Localizer::ReadParams(config_reader::ConfigReader& config, bool fatal_failure) { camera::CameraParameters cam_params(&config, "nav_cam"); std::string prefix; const auto detector_name = map_->GetDetectorName(); @@ -40,7 +40,12 @@ void Localizer::ReadParams(config_reader::ConfigReader& config) { } else if (detector_name == "TEBLID256") { prefix = "teblid256_"; } else { - ROS_FATAL_STREAM("Invalid detector: " << detector_name); + if (fatal_failure) { + ROS_FATAL_STREAM("Invalid detector: " << detector_name); + } else { + ROS_ERROR_STREAM("Invalid detector: " << detector_name); + } + return false; } // Loc params @@ -86,17 +91,24 @@ void Localizer::ReadParams(config_reader::ConfigReader& config) { // This check must happen before the histogram_equalization flag is set into the map // to compare with what is there already. - sparse_mapping::HistogramEqualizationCheck(map_->GetHistogramEqualization(), - loc_params.histogram_equalization); + if (!sparse_mapping::HistogramEqualizationCheck(map_->GetHistogramEqualization(), + loc_params.histogram_equalization, fatal_failure)) return false; + // Check consistency between clahe params if (loc_params.use_clahe && (loc_params.histogram_equalization != 3 || map_->GetHistogramEqualization() != 3)) { - ROS_FATAL("Invalid clahe and histogram equalization settings."); + if (fatal_failure) { + ROS_FATAL("Invalid clahe and histogram equalization settings."); + } else { + ROS_ERROR("Invalid clahe and histogram equalization settings."); + } + return false; } map_->SetCameraParameters(cam_params); map_->SetLocParams(loc_params); map_->SetDetectorParams(min_features, max_features, detection_retries, min_threshold, default_threshold, max_threshold, too_many_ratio, too_few_ratio); + return true; } bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLandmarks* vl, diff --git a/localization/localization_node/src/localization_nodelet.cc b/localization/localization_node/src/localization_nodelet.cc index c2db553023..5363bafadb 100644 --- a/localization/localization_node/src/localization_nodelet.cc +++ b/localization/localization_node/src/localization_nodelet.cc @@ -59,10 +59,33 @@ bool LocalizationNodelet::ResetMap(const std::string& map_file) { while (processing_image_) { usleep(100000); } - map_.reset(new sparse_mapping::SparseMap(map_file, true)); + // Reset map before creating new one to avoid storing two maps in memory at the same time + map_.reset(); + map_ = std::make_shared(map_file, true); inst_.reset(new Localizer(map_.get())); - // Check to see if any params were changed when map was reset - ReadParams(); + + // Check to see if any params were changed when map was reset. + // Make sure they are valid. + if (!ReadParams(false)) { + ROS_ERROR_STREAM("Failed to switch to map file " << map_file + << " due to invalid params."); + if (!last_valid_map_file_.empty()) { + ROS_ERROR_STREAM("Reverting to last map file " << last_valid_map_file_); + map_.reset(); + map_ = std::make_shared(last_valid_map_file_, true); + inst_.reset(new Localizer(map_.get())); + ReadParams(); + enabled_ = true; + } else { + ROS_ERROR_STREAM("No last valid map, please reset with a valid map. Not localizing."); + map_.reset(); + inst_.reset(); + } + return false; + } else { + last_valid_map_file_ = map_file; + } + enabled_ = true; return true; } @@ -83,7 +106,11 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) { // Reset all internal shared pointers it_.reset(new image_transport::ImageTransport(*nh)); - map_.reset(new sparse_mapping::SparseMap(map_file, true)); + map_.reset(); + map_ = std::make_shared(map_file, true); + last_valid_map_file_ = map_file; + + inst_.reset(new Localizer(map_.get())); registration_publisher_ = nh->advertise( @@ -118,7 +145,7 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) { cv::setNumThreads(num_threads); config_timer_ = nh->createTimer(ros::Duration(1), [this](ros::TimerEvent e) { - config_.CheckFilesUpdated(std::bind(&LocalizationNodelet::ReadParams, this));}, false, true); + config_.CheckFilesUpdated(std::bind(&LocalizationNodelet::ReadParamsWrapper, this));}, false, true); enable_srv_ = nh->advertiseService(SERVICE_LOCALIZATION_ML_ENABLE, &LocalizationNodelet::EnableService, this); reset_map_srv_ = nh->advertiseService(SERVICE_LOCALIZATION_RESET_MAP, &LocalizationNodelet::ResetMapService, this); @@ -126,12 +153,20 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) { SERVICE_LOCALIZATION_RESET_MAP_LOC); } -void LocalizationNodelet::ReadParams(void) { + +void LocalizationNodelet::ReadParamsWrapper() { + ReadParams(true); +} + +bool LocalizationNodelet::ReadParams(bool fatal_failure) { if (!config_.ReadFiles()) { ROS_ERROR("Failed to read config files."); - return; + return false; } - if (inst_) inst_->ReadParams(config_); + if (inst_) { + return inst_->ReadParams(config_, fatal_failure); + } + return true; } bool LocalizationNodelet::EnableService(ff_msgs::SetBool::Request & req, ff_msgs::SetBool::Response & res) { @@ -189,6 +224,7 @@ void LocalizationNodelet::ImageCallback(const sensor_msgs::ImageConstPtr& msg) { } void LocalizationNodelet::Localize(void) { + if (!inst_) return; ff_msgs::VisualLandmarks vl; Eigen::Matrix2Xd image_keypoints; diff --git a/localization/localization_node/tools/set_start_pose.py b/localization/localization_node/tools/set_start_pose.py new file mode 100755 index 0000000000..97f915507c --- /dev/null +++ b/localization/localization_node/tools/set_start_pose.py @@ -0,0 +1,95 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Initializes the localizer with a defined pose by sending a ff_msgs VisualLandmarks message +with the pose value. Note the pose is expected in the robot nav camera frame. +""" + +import argparse +import os +import sys + +import rospy +import std_msgs.msg +from ff_msgs.msg import VisualLandmark, VisualLandmarks + + +def publish_pose(pose): + # Init publisher + pub = rospy.Publisher("/loc/ml/features", VisualLandmarks, queue_size=1) + rospy.init_node("PosePublisher") + # Sleep so node can subscribe to /clock topic and produce a valid header time + rospy.sleep(5) + + msg = VisualLandmarks() + msg.header = std_msgs.msg.Header() + msg.header.stamp = rospy.Time.now() + # Subtract 2 seconds from stamp to simulate localization time, + # allows VIO messages to accrue that span before and after the vl message + msg.header.stamp.secs = msg.header.stamp.secs - 2 + msg.header.frame_id = "world" + msg.camera_id = 0 + # Set pose values + msg.pose.position.x = pose[0] + msg.pose.position.y = pose[1] + msg.pose.position.z = pose[2] + msg.pose.orientation.x = pose[3] + msg.pose.orientation.y = pose[4] + msg.pose.orientation.z = pose[5] + msg.pose.orientation.w = pose[6] + # Set empty landmark features, make sure to have > 5 so msg is considered valid + for i in range(0, 5): + landmark = VisualLandmark() + landmark.x = 0 + landmark.y = 0 + landmark.z = 0 + landmark.u = 0 + landmark.v = 0 + msg.landmarks.append(landmark) + + pub.publish(msg) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter + ) + parser.add_argument( + "pose", + nargs="+", + type=float, + help="Start pose in the format: x y z qx qy qz qw. If --position-only used, only x y z are required.", + ) + parser.add_argument( + "-p", + "--position-only", + dest="initialize_position_only", + action="store_true", + help="Initialize a pose using only the position, the orientation will be set to identity.", + ) + args = parser.parse_args() + if not args.initialize_position_only and len(args.pose) != 7: + print("Pose requires 7 fields.") + sys.exit() + if args.initialize_position_only and len(args.pose) != 3: + print("Position only pose requires 3 fields.") + sys.exit() + if args.initialize_position_only: + args.pose.extend([0, 0, 0, 1]) + publish_pose(args.pose) diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h b/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h index f573acc273..c9ec62ae19 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h @@ -76,7 +76,7 @@ Eigen::Quaternion slerp_n(std::vector const& W, std::vector const& cid_to_keypoint_map, std::vector const& cid_to_filename, diff --git a/localization/sparse_mapping/scripts/make_surf_map.py b/localization/sparse_mapping/scripts/make_surf_map.py index f5deb5f33a..338e8c512e 100755 --- a/localization/sparse_mapping/scripts/make_surf_map.py +++ b/localization/sparse_mapping/scripts/make_surf_map.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/localization/sparse_mapping/scripts/make_surf_maps.py b/localization/sparse_mapping/scripts/make_surf_maps.py index 1e6d011451..28cc50d4c5 100755 --- a/localization/sparse_mapping/scripts/make_surf_maps.py +++ b/localization/sparse_mapping/scripts/make_surf_maps.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/localization/sparse_mapping/scripts/make_teblid512_map.py b/localization/sparse_mapping/scripts/make_teblid512_map.py index 75766ab10f..a26b3a5b52 100755 --- a/localization/sparse_mapping/scripts/make_teblid512_map.py +++ b/localization/sparse_mapping/scripts/make_teblid512_map.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. @@ -53,7 +53,7 @@ def make_teblid512_map(surf_map, world, robot_name, map_directory=None): if map_directory: os.chdir(map_directory) build_teblid512_map_command = ( - "rosrun sparse_mapping build_map -rebuild -histogram_equalization -use_clahe -output_map " + "rosrun sparse_mapping build_map -rebuild -histogram_equalization -use_clahe -min_brisk_features 3000 -output_map " + teblid512_map_full_path ) lu.run_command_and_save_output( diff --git a/localization/sparse_mapping/scripts/process_sequential_images.py b/localization/sparse_mapping/scripts/process_sequential_images.py index aafe1c1586..dbe5c05b03 100755 --- a/localization/sparse_mapping/scripts/process_sequential_images.py +++ b/localization/sparse_mapping/scripts/process_sequential_images.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/localization/sparse_mapping/scripts/prune_partitioned_directories.py b/localization/sparse_mapping/scripts/prune_partitioned_directories.py index f986d57697..ff5efd6889 100755 --- a/localization/sparse_mapping/scripts/prune_partitioned_directories.py +++ b/localization/sparse_mapping/scripts/prune_partitioned_directories.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/localization/sparse_mapping/src/sparse_mapping.cc b/localization/sparse_mapping/src/sparse_mapping.cc index 94642e2150..8cfda8af8b 100644 --- a/localization/sparse_mapping/src/sparse_mapping.cc +++ b/localization/sparse_mapping/src/sparse_mapping.cc @@ -115,15 +115,21 @@ namespace sparse_mapping { // Logic for implementing if two histogram equalization flags are compatible. // This flag can be either 0 (false), 1 (true), or 2 (unknown). Be tolerant // of unknown values, but intolerant when true and false are mixed. -void sparse_mapping::HistogramEqualizationCheck(int histogram_equalization1, - int histogram_equalization2) { +bool sparse_mapping::HistogramEqualizationCheck(int histogram_equalization1, + int histogram_equalization2, bool fatal_failure) { // Ignore if either has unknown equalization value if (histogram_equalization1 == HistogramEqualizationType::kUnknown || histogram_equalization2 == HistogramEqualizationType::kUnknown) { - return; + return true; } else if (histogram_equalization1 != histogram_equalization2) { - LOG(FATAL) << "Incompatible values of histogram equalization detected."; + if (fatal_failure) { + LOG(FATAL) << "Incompatible values of histogram equalization detected."; + } else { + LOG(ERROR) << "Incompatible values of histogram equalization detected."; + } + return false; } + return true; } bool sparse_mapping::IsBinaryDescriptor(std::string const& descriptor) { diff --git a/tools/localization_analysis/scripts/depth_odometry_parameter_sweep.py b/tools/localization_analysis/scripts/depth_odometry_parameter_sweep.py index faa4474eb9..5029e64f3e 100755 --- a/tools/localization_analysis/scripts/depth_odometry_parameter_sweep.py +++ b/tools/localization_analysis/scripts/depth_odometry_parameter_sweep.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/tools/localization_analysis/scripts/groundtruth_sweep.py b/tools/localization_analysis/scripts/groundtruth_sweep.py index f0479a58f7..ebc440022e 100755 --- a/tools/localization_analysis/scripts/groundtruth_sweep.py +++ b/tools/localization_analysis/scripts/groundtruth_sweep.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/tools/localization_analysis/scripts/imu_analyzer.py b/tools/localization_analysis/scripts/imu_analyzer.py index 499fbcbd13..b66959cb3e 100755 --- a/tools/localization_analysis/scripts/imu_analyzer.py +++ b/tools/localization_analysis/scripts/imu_analyzer.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/tools/localization_analysis/scripts/make_groundtruth.py b/tools/localization_analysis/scripts/make_groundtruth.py index 5d2274f71b..576dfc1e9f 100755 --- a/tools/localization_analysis/scripts/make_groundtruth.py +++ b/tools/localization_analysis/scripts/make_groundtruth.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/tools/localization_analysis/scripts/make_map.py b/tools/localization_analysis/scripts/make_map.py index ede8dd16f7..5f248e7c77 100755 --- a/tools/localization_analysis/scripts/make_map.py +++ b/tools/localization_analysis/scripts/make_map.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. @@ -133,7 +133,7 @@ def make_map( if merge_with_base_map: os.chdir("maps") rebuild_map_command = ( - "rosrun sparse_mapping build_map -rebuild -rebuild_detector TEBLID512 -output_map " + "rosrun sparse_mapping build_map -rebuild -rebuild_detector TEBLID512 -min_brisk_features 3000 -output_map " + bag_teblid512_map_full_path ) if histogram_equalization: diff --git a/tools/localization_analysis/scripts/plot_helpers.py b/tools/localization_analysis/scripts/plot_helpers.py index 6d879b2c0c..faf22c3276 100644 --- a/tools/localization_analysis/scripts/plot_helpers.py +++ b/tools/localization_analysis/scripts/plot_helpers.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/tools/localization_rviz_plugins/CMakeLists.txt b/tools/localization_rviz_plugins/CMakeLists.txt index d93d1ff85b..715bf644d5 100644 --- a/tools/localization_rviz_plugins/CMakeLists.txt +++ b/tools/localization_rviz_plugins/CMakeLists.txt @@ -18,7 +18,7 @@ cmake_minimum_required(VERSION 3.0) project(localization_rviz_plugins) -if (FALSE AND BUILD_LOC_RVIZ_PLUGINS) +if (BUILD_LOC_RVIZ_PLUGINS) ## Compile as C++14, supported in ROS Kinetic and newer add_compile_options(-std=c++14) @@ -61,13 +61,14 @@ if (FALSE AND BUILD_LOC_RVIZ_PLUGINS) # Declare C++ libraries add_library(${PROJECT_NAME} - src/depth_odometry_display.cc - src/localization_graph_display.cc - src/localization_graph_panel.cc - src/pose_display.cc - src/utilities.cc - src/imu_augmentor_display.cc - src/slider_property.cc + #src/depth_odometry_display.cc + src/localization_coverage_display.cc + #src/localization_graph_display.cc + #src/localization_graph_panel.cc + #src/pose_display.cc + #src/utilities.cc + #src/imu_augmentor_display.cc + #src/slider_property.cc src/sparse_mapping_display.cc ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) diff --git a/tools/localization_rviz_plugins/nodelet_plugins.xml b/tools/localization_rviz_plugins/nodelet_plugins.xml index c4b3f276d7..7636b79bca 100644 --- a/tools/localization_rviz_plugins/nodelet_plugins.xml +++ b/tools/localization_rviz_plugins/nodelet_plugins.xml @@ -6,6 +6,13 @@ Depth Odometry Display + + + Localization Coverage Display + + diff --git a/tools/localization_rviz_plugins/readme.md b/tools/localization_rviz_plugins/readme.md index f749a9e6b5..8a444fa848 100644 --- a/tools/localization_rviz_plugins/readme.md +++ b/tools/localization_rviz_plugins/readme.md @@ -16,6 +16,9 @@ The depth odometry display publishes source and target point clouds and correspo ## Imu Augmentor Display The imu augmentor display draws imu augmentor poses. This is useful when comparing with graph localizer poses and sparse mapping poses, as ideally these are all alligned. +## Localization Coverage Display +The localization coverage display draws map pose positions from a bagfile. To use this, use: export COVERAGE_BAG=/path/to/bagfile before launching rviz to set the bagfile to use for plotting. The bag file should have poses stored using the /sparse_mapping/pose topic, which is the output for the localization_analysis map matcher and offline replay tools for map matched poses. Subscribe to the output topic called /localization_coverage/cloud using the rviz PointCloud2 visualization plugin to view the pose positions. + ## Localization Graph Display The localization graph display draws the full history of poses in the latest graph localization message. It also draws imu estimates between poses as arrows, and publishes optical flow feature track images using the feature tracks in the localizer. diff --git a/tools/localization_rviz_plugins/src/localization_coverage_display.cc b/tools/localization_rviz_plugins/src/localization_coverage_display.cc new file mode 100644 index 0000000000..027ee7e92a --- /dev/null +++ b/tools/localization_rviz_plugins/src/localization_coverage_display.cc @@ -0,0 +1,96 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include + +#include +#include + +#include +#include +#include + +#include + +#include "localization_coverage_display.h" // NOLINT + +namespace localization_rviz_plugins { +LocalizationCoverageDisplay::LocalizationCoverageDisplay() { + int ff_argc = 1; + char argv[] = "localization_coverage_display"; + char* argv_ptr = &argv[0]; + char** argv_ptr_ptr = &argv_ptr; + cloud_publisher_ = nh_.advertise("localization_coverage/cloud", 1); + // Load positions from bagfile + const std::string coverage_bag = std::getenv("COVERAGE_BAG"); + rosbag::Bag bag(coverage_bag, rosbag::bagmode::Read); + rosbag::View view(bag, rosbag::TopicQuery("/sparse_mapping/pose")); + for (const rosbag::MessageInstance msg : view) { + geometry_msgs::PoseStampedPtr pose_msg = msg.instantiate(); + if (pose_msg) { + positions_.emplace_back(localization_common::PoseFromMsg(*pose_msg).translation().cast()); + } + } +} + +void LocalizationCoverageDisplay::onInitialize() { drawMap(); } + +void LocalizationCoverageDisplay::drawMap() { + // TODO(rsoussan): Use pcl point cloud when pcl_ros dependency added + sensor_msgs::PointCloud2 cloud; + cloud.header = std_msgs::Header(); + cloud.header.frame_id = "world"; + cloud.height = 1; + cloud.width = positions_.size(); + cloud.fields.resize(3); + cloud.fields[0].name = "x"; + cloud.fields[0].offset = 0; + cloud.fields[0].datatype = 7; + cloud.fields[0].count = 1; + cloud.fields[1].name = "y"; + cloud.fields[1].offset = 4; + cloud.fields[1].datatype = 7; + cloud.fields[1].count = 1; + cloud.fields[2].name = "z"; + cloud.fields[2].offset = 8; + cloud.fields[2].datatype = 7; + cloud.fields[2].count = 1; + cloud.is_bigendian = false; + cloud.point_step = 12; + cloud.row_step = cloud.point_step * cloud.width; + cloud.is_dense = true; + cloud.data.resize(cloud.row_step); + + for (int i = 0; i < static_cast(cloud.width); ++i) { + const Eigen::Vector3f& point = positions_[i]; + memcpy(&cloud.data[cloud.point_step * i + 0], &point.x(), 4); + memcpy(&cloud.data[cloud.point_step * i + 4], &point.y(), 4); + memcpy(&cloud.data[cloud.point_step * i + 8], &point.z(), 4); + } + + cloud.header.stamp = ros::Time::now(); + // TODO(rsoussan): Use ros point_cloud_common instead of publishing when rviz_default_plugin linker issue fixed + cloud_publisher_.publish(cloud); +} + +void LocalizationCoverageDisplay::reset() {} +} // namespace localization_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(localization_rviz_plugins::LocalizationCoverageDisplay, rviz::Display) diff --git a/tools/localization_rviz_plugins/src/localization_coverage_display.h b/tools/localization_rviz_plugins/src/localization_coverage_display.h new file mode 100644 index 0000000000..21a0d1bad8 --- /dev/null +++ b/tools/localization_rviz_plugins/src/localization_coverage_display.h @@ -0,0 +1,61 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// Header file must go in src directory for Qt/Rviz plugin +#ifndef LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_COVERAGE_DISPLAY_H_ // NOLINT +#define LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_COVERAGE_DISPLAY_H_ // NOLINT + +// Required for Qt +#ifndef Q_MOC_RUN +#include +#include +#include +#endif + +#include +#include +#include + +// Forward declarations for ogre and rviz +namespace Ogre { +class SceneNode; +} + +namespace localization_rviz_plugins { + +class LocalizationCoverageDisplay : public rviz::Display { + Q_OBJECT // NOLINT + public : // NOLINT + LocalizationCoverageDisplay(); + ~LocalizationCoverageDisplay() = default; + + protected: + void onInitialize() final; + void reset() final; + + private: + void drawMap(); + + // TODO(rosussan): Remove publishing and use point_cloud_common from rviz/default_plugins + // when linking error is fixed + std::vector positions_; + ros::Publisher cloud_publisher_; + ros::NodeHandle nh_; +}; +} // namespace localization_rviz_plugins +#endif // LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_COVERAGE_DISPLAY_H_ NOLINT