diff --git a/CMakeLists.txt b/CMakeLists.txt index e6a5f19..59326c4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -210,6 +210,10 @@ endif() "nav_msgs" "sensor_msgs" "geometry_msgs") + + install( + TARGETS ldrp_node + DESTINATION lib/${PROJECT_NAME}/) # ament_export_include_directories() # ament_export_dependencies() ament_package() diff --git a/src/ldrp/grid.hpp b/src/ldrp/grid.hpp index a456767..97d3fb2 100644 --- a/src/ldrp/grid.hpp +++ b/src/ldrp/grid.hpp @@ -481,7 +481,8 @@ class QuantizedRatioGrid : public RatioGrid< Ratio_t, Int_t, Float_t, X_Major, M _min = this->boundingLoc(min.x(), min.y()), // grid cell locations containing min and max, aligned with current offsets _max = this->boundingLoc(max.x(), max.y()); - if (_min.cwiseLess(_zero).any() || _max.cwiseGreater(this->grid_size).any()) { + // if (_min.cwiseLess(_zero).any() || _max.cwiseGreater(this->grid_size).any()) { + if( (_min.x() < _zero.x() || _min.y() < _zero.y()) || (_max.x() > this->grid_size.x() || _max.y() > this->grid_size.y()) ) { const Vec2i _low = _min.cwiseMin(_zero), // new high and low bounds for the map _high = _max.cwiseMax(this->grid_size) + _one, // need to add an additional row + col since indexing happens in the corner, thus by default the difference between corners will not cover the entire size diff --git a/src/ldrp/perception.cpp b/src/ldrp/perception.cpp index f81f5f9..1f1b0a9 100644 --- a/src/ldrp/perception.cpp +++ b/src/ldrp/perception.cpp @@ -1,28 +1,96 @@ +/******************************************************************************* +* Copyright (C) 2024 Cardinal Space Mining Club * +* * +* 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. * +* * +* ##**# * +* #%%#%%%@ * +* %%%#%%@@@@ * +* %%*%%%@@@ * +* +*%%@@@ * +* *%@@@%%%*=+%%% * +* +##++*++++*%@*++#%%% * +* +++#++*+++==*%%@#+++#%% * +* +++*#*++++++++*%%@@%*+*#*+* * +* *+*#%%@*+*+++*#%%%%@@@%++++*## * +* +++==*%#####*##%%%%%#***++*#%%% * +* #%%%###*+****=*%%%%%%%#%##****###%%%%%%%% * +* -----========+*#%%#%%%%%%@@@@%%%%%%%%%%%%%% * +* ---===++++++++**+=--==++++++++*##*++***+++**#%@@@ * +* --=+*##%%##%%###%%%*-----===++*#%###*#%%%%%%%%%%@@ * +* ++#%%%%%%%%%%%%%@@@#=--===++*#%%%@@@#*#%@@@@@@@@ * +* *#%@@%%%%%@@@@@@@#=--=-::.....-+#%%@@*#%@@@@@@@ * +* %@@@@@@@@@@@@@@*--=-............::-+@@@@%%@@@@ * +* * +*******************************************************************************/ /** FOR REFERENCE * pub/sub example: https://docs.ros.org/en/iron/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html * params example: https://docs.ros.org/en/iron/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-CPP.html */ #include "./perception.hpp" +#include "./filtering.hpp" #include +#include -#include -#include +#include #include -PerceptionNode::PerceptionNode() : rclcpp::Node("perception_node") { +namespace util { + + template + struct identity { typedef T type; }; + + template + void declare_param(rclcpp::Node* node, const std::string param_name, T& param, const typename identity::type& default_value) { + node->declare_parameter(param_name, default_value); + node->get_parameter(param_name, param); + } + + + template + static inline int64_t floatSecondsToIntMicros(const FT v) { + static_assert(std::is_floating_point_v, ""); + return static_cast(v * static_cast(1e6)); + } + + template + static inline int64_t constructTimestampMicros(const T seconds, const T nanoseconds) { + return (static_cast(seconds) * 1000000L) + (static_cast(nanoseconds) / 1000L); + } +}; + + +PerceptionNode::PerceptionNode() : rclcpp::Node("perception_node") { + RCLCPP_INFO(this->get_logger(), "Perception Node Initialization!\n"); // setup subs/pubs - this->scan_sub = this->create_subscription("input scan", 1, + this->scan_sub = this->create_subscription("input_scan", 1, std::bind(&PerceptionNode::scan_cb, this, std::placeholders::_1)); - this->trfm_sub = this->create_subscription("input trfm", 1, + this->trfm_sub = this->create_subscription("input_trfm", 1, std::bind(&PerceptionNode::trfm_cb, this, std::placeholders::_1)); - this->grid_pub = this->create_publisher("obstacle grid", 1); - // declare params - // this->declare_parameter("", /* default value*/); - // this->get_parameter("", /* param */); + this->grid_pub = this->create_publisher("obstacle_grid", 1); + +//get parameters + util::declare_param(this, "voxel_size_cm", this->_config.voxel_size_cm, this->_config.voxel_size_cm); + util::declare_param(this, "map_resolution_cm", this->_config.map_resolution_cm, this->_config.map_resolution_cm); + util::declare_param(this, "max_pmf_range_cm", this->_config.max_pmf_range_cm, this->_config.max_pmf_range_cm); + util::declare_param(this, "max_z_thresh_cm", this->_config.max_z_thresh_cm, this->_config.max_z_thresh_cm); + util::declare_param(this, "min_z_thresh_cm", this->_config.min_z_thresh_cm, this->_config.min_z_thresh_cm); + util::declare_param(this, "pmf_window_base", this->_config.pmf_window_base, this->_config.pmf_window_base); + util::declare_param(this, "pmf_max_window_size_cm", this->_config.pmf_max_window_size_cm, this->_config.pmf_max_window_size_cm); + util::declare_param(this, "pmf_cell_size_cm", this->_config.pmf_cell_size_cm, this->_config.pmf_cell_size_cm); + util::declare_param(this, "pmf_init_distance_cm", this->_config.pmf_init_distance_cm, this->_config.pmf_init_distance_cm); + util::declare_param(this, "pmf_max_distance_cm", this->_config.pmf_max_distance_cm, this->_config.pmf_max_distance_cm); + util::declare_param(this, "pmf_slope", this->_config.pmf_slope, this->_config.pmf_slope); + util::declare_param(this, "scan_matching_history_range_s", this->_config.scan_matching_history_range_s, this->_config.scan_matching_history_range_s); + // util::declare_param(this, "TODO", this->_config.scan_matching_skip_invalid, this->_config.scan_matching_skip_invalid); this->accumulator.reset(1.f); // use params @@ -32,29 +100,68 @@ PerceptionNode::~PerceptionNode() {} void PerceptionNode::scan_cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan) { + RCLCPP_INFO(this->get_logger(), "Scan callback called!\n"); - static const pcl::Indices DEFAULT_NO_SELECTION = pcl::Indices{}; + pcl::PointCloud::Ptr point_cloud = std::make_shared>(); + pcl::fromROSMsg(*scan, *point_cloud); - // TODO: add back all these params! - const float // all "unitted" parameters are normalized to be in meters and radians! - _max_scan_theta = this->_config.fpipeline.max_scan_theta_deg * (std::numbers::pi_v / 180.f), - _min_scan_theta = this->_config.fpipeline.min_scan_theta_deg * (std::numbers::pi_v / 180.f), - _min_scan_range = this->_config.fpipeline.min_scan_range_cm * 1e-2f, - _voxel_size = this->_config.fpipeline.voxel_size_cm * 1e-2f, - _max_pmf_range = this->_config.fpipeline.max_pmf_range_cm * 1e-2f, - _max_z_thresh = this->_config.fpipeline.max_z_thresh_cm * 1e-2f, - _min_z_thresh = this->_config.fpipeline.min_z_thresh_cm * 1e-2f, - _pmf_window_base = this->_config.fpipeline.pmf_window_base, - _pmf_max_window_size = this->_config.fpipeline.pmf_max_window_size_cm * 1e-2f, - _pmf_cell_size = this->_config.fpipeline.pmf_cell_size_cm * 1e-2f, - _pmf_init_distance = this->_config.fpipeline.pmf_init_distance_cm * 1e-2f, - _pmf_max_distance = this->_config.fpipeline.pmf_max_distance_cm * 1e-2f, - _pmf_slope = this->_config.fpipeline.pmf_slope - ; + const int64_t ts = util::constructTimestampMicros(scan->header.stamp.sec, scan->header.stamp.nanosec); + this->scan_sampler.insert( ts, point_cloud ); + this->scan_sampler.updateMin(ts - util::floatSecondsToIntMicros(this->_config.scan_matching_history_range_s)); + +} + + +void PerceptionNode::trfm_cb(const geometry_msgs::msg::TransformStamped::ConstSharedPtr& trfm) { + RCLCPP_INFO(this->get_logger(), "Transform callback called!\n"); + + const int64_t trfm_target_ts = util::constructTimestampMicros(trfm->header.stamp.sec, trfm->header.stamp.nanosec); + const typename decltype(this->scan_sampler)::ElemT* sample = this->scan_sampler.sampleTimestamped(trfm_target_ts); + + // if scan doesn't exist + if(!sample) return; + + // transform age - scan age + const int64_t ts_diff = trfm_target_ts - sample->first; + + // if scan came after the transform do not use it + if(ts_diff < 0) return; + + // valid case to use transform with scan + else if(ts_diff < this->_config.scan_matching_history_range_s) { + pcl::PointCloud::Ptr scan = sample->second; + this->scan_sampler.updateMin(sample->first + 1); // don't use 'sample' after here since it is invalid + + const Eigen::Quaternionf quat{ + static_cast(trfm->transform.rotation.w), + static_cast(trfm->transform.rotation.x), + static_cast(trfm->transform.rotation.y), + static_cast(trfm->transform.rotation.z) + }; + const Eigen::Translation3f pos{ + static_cast(trfm->transform.translation.x), + static_cast(trfm->transform.translation.y), + static_cast(trfm->transform.translation.z) + }; + + pcl::transformPointCloud( + *scan, *scan, // :O + pos * quat // be very careful with the order of these uWu + ); + + this->process_and_export(*scan, *reinterpret_cast(&pos)); + } + +} + + + +void PerceptionNode::process(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin) { + RCLCPP_INFO(this->get_logger(), "Processing...\n"); + + static const pcl::Indices DEFAULT_NO_SELECTION = pcl::Indices{}; - // pcl::PointCloud::Ptr cloud = std::make_shared>(); pcl::PointCloud - point_cloud, voxel_cloud; pcl::Indices z_high_filtered{}, @@ -65,30 +172,54 @@ void PerceptionNode::scan_cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr pmf_filtered_obstacles{}, combined_obstacles{}; - pcl::fromROSMsg(*scan, cloud); - - // transform somehow - - // NOTE: probably make the pipline a static method that gets called +#if LDRP_ENABLE_TUNING + float + max_pmf_range_m = (float) this->get_parameter("max_pmf_range_m").as_double() * 1e-2f, + max_z_thresh_m = (float) this->get_parameter("max_z_thresh_m").as_double() * 1e-2f, + min_z_thresh_m = (float) this->get_parameter("min_z_thresh_m").as_double() * 1e-2f, + voxel_size_m = (float) this->get_parameter("voxel_size_m").as_double() * 1e-2f, + pmf_window_base = (float) this->get_parameter("pmf_window_base").as_double(), + pmf_max_window_size_m = (float) this->get_parameter("pmf_max_window_size_m").as_double() * 1e-2f, + pmf_cell_size_m = (float) this->get_parameter("pmf_cell_size_m").as_double() * 1e-2f, + pmf_init_distance_m = (float) this->get_parameter("pmf_init_distance_m").as_double() * 1e-2f, + pmf_max_distance_m = (float) this->get_parameter("pmf_max_distance_m").as_double() * 1e-2f, + pmf_slope = (float) this->get_parameter("pmf_slope").as_double() + ; +#else + float + max_pmf_range_m = (float) this->_config.max_pmf_range_cm * 1e-2f, + max_z_thresh_m = (float) this->_config.max_z_thresh_cm * 1e-2f, + min_z_thresh_m = (float) this->_config.min_z_thresh_cm * 1e-2f, + voxel_size_m = (float) this->_config.voxel_size_cm * 1e-2f, + pmf_window_base = (float) this->_config.pmf_window_base, + pmf_max_window_size_m = (float) this->_config.pmf_max_window_size_cm * 1e-2f, + pmf_cell_size_m = (float) this->_config.pmf_cell_size_cm * 1e-2f, + pmf_init_distance_m = (float) this->_config.pmf_init_distance_cm * 1e-2f, + pmf_max_distance_m = (float) this->_config.pmf_max_distance_cm * 1e-2f, + pmf_slope = (float) this->_config.pmf_slope; + ; +#endif // voxelize points voxel_filter( - point_cloud, DEFAULT_NO_SELECTION, voxelized_points, - _voxel_size, _voxel_size, _voxel_size + cloud, DEFAULT_NO_SELECTION, voxel_cloud, + voxel_size_m, voxel_size_m, voxel_size_m ); // filter points under "high cut" thresh carteZ_filter( - voxelized_points, DEFAULT_NO_SELECTION, z_high_filtered, + voxel_cloud, DEFAULT_NO_SELECTION, z_high_filtered, -std::numeric_limits::infinity(), - _max_z_thresh + max_z_thresh_m ); + // further filter points below "low cut" thresh carteZ_filter( - voxelized_points, z_high_filtered, z_low_subset_filtered, + voxel_cloud, z_high_filtered, z_low_subset_filtered, -std::numeric_limits::infinity(), - _min_z_thresh + min_z_thresh_m ); + // get the points inbetween high and low thresholds --> treated as wall obstacles pc_negate_selection( z_high_filtered, @@ -98,24 +229,25 @@ void PerceptionNode::scan_cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr // filter close enough points for PMF pc_filter_distance( - voxelized_points.points, + voxel_cloud.points, z_low_subset_filtered, pre_pmf_range_filtered, - 0.f, _max_pmf_range, - avg_sample_origin + 0.f, max_pmf_range_m, + origin ); // apply pmf to selected points progressive_morph_filter( - voxelized_points, pre_pmf_range_filtered, pmf_filtered_ground, - _pmf_window_base, - _pmf_max_window_size, - _pmf_cell_size, - _pmf_init_distance, - _pmf_max_distance, - _pmf_slope, + voxel_cloud, pre_pmf_range_filtered, pmf_filtered_ground, + pmf_window_base, + pmf_max_window_size_m, + pmf_cell_size_m, + pmf_init_distance_m, + pmf_max_distance_m, + pmf_slope, false ); + // obstacles = (base - ground) pc_negate_selection( pre_pmf_range_filtered, @@ -123,28 +255,39 @@ void PerceptionNode::scan_cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr pmf_filtered_obstacles ); - this->accumulator.incrementRatio( // insert PMF obstacles - voxelized_points, + voxel_cloud, pre_pmf_range_filtered, // base pmf_filtered_obstacles // subset ); this->accumulator.incrementRatio( // insert z-thresh obstacles - voxelized_points, + voxel_cloud, z_mid_filtered_obstacles, // base DEFAULT_NO_SELECTION // use all of base ); + // trim ratios + +} - // run a pass over the grid to trim ratio magnitudes +void PerceptionNode::process_and_export(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin) { - // convert grid to occupancy map - // publish occupancy map + this->process(cloud, origin); + nav_msgs::msg::OccupancyGrid out_grid; -} + const size_t _area = static_cast(this->accumulator.area()); + const Eigen::Vector2f& _origin = this->accumulator.origin(); + const Eigen::Vector2i& _grid_size = this->accumulator.size(); -void PerceptionNode::trfm_cb(const geometry_msgs::msg::TransformStamped::ConstSharedPtr& trfm) { + out_grid.info.resolution = this->accumulator.cellRes(); + out_grid.info.origin.position.x = _origin.x(); + out_grid.info.origin.position.y = _origin.y(); + out_grid.info.width = _grid_size.x(); + out_grid.info.height = _grid_size.y(); - // do something! + out_grid.data.resize(_area); + memcpy(out_grid.data.data(), this->accumulator.buffData(), _area); -} \ No newline at end of file + this->grid_pub->publish(out_grid); + +} diff --git a/src/ldrp/perception.hpp b/src/ldrp/perception.hpp index 9444140..1f854aa 100644 --- a/src/ldrp/perception.hpp +++ b/src/ldrp/perception.hpp @@ -1,7 +1,13 @@ #pragma once -#include "./filtering.hpp" #include "./grid.hpp" +#include "./timestamp_sampler.hpp" + +#include + +#include +#include +#include #include #include @@ -19,7 +25,8 @@ class PerceptionNode : public rclcpp::Node { void scan_cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan); void trfm_cb(const geometry_msgs::msg::TransformStamped::ConstSharedPtr& trfm); - void process(); + void process(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin); + void process_and_export(const pcl::PointCloud& cloud, const Eigen::Vector3f& origin); protected: @@ -28,10 +35,27 @@ class PerceptionNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr grid_pub; // last pointcloud and transform? + TimestampSampler::Ptr, int64_t> scan_sampler{}; QuantizedRatioGrid accumulator{}; struct { - // defaults -- copy from original + + // bool + // scan_matching_skip_invalid = false; + double + scan_matching_history_range_s = 0.25, + map_resolution_cm = 5., + max_pmf_range_cm = 250., + max_z_thresh_cm = 100., + min_z_thresh_cm = 25., + voxel_size_cm = 3., + pmf_window_base = 2., + pmf_max_window_size_cm = 48., + pmf_cell_size_cm = 5., + pmf_init_distance_cm = 5., + pmf_max_distance_cm = 12., + pmf_slope = 2.; + } _config; diff --git a/src/ldrp/timestamp_sampler.hpp b/src/ldrp/timestamp_sampler.hpp new file mode 100644 index 0000000..dc3d9c6 --- /dev/null +++ b/src/ldrp/timestamp_sampler.hpp @@ -0,0 +1,96 @@ +#pragma once + +#include +#include +#include + + +/** TimestampSampler stores an ordered list of time-element pairs, and can efficiently look up the closes element given a target timestamp. */ +template +class TimestampSampler { +public: + using This_T = TimestampSampler; + using Type = T; + using TimeT = Time_t; + using ElemT = std::pair; + +private: + static bool t_less__(TimeT t, const ElemT& elem) { + return t < elem.first; + } + static bool t_greater__(const ElemT& elem, TimeT t) { + return t > elem.first; + } + +public: + TimestampSampler() = default; + ~TimestampSampler() = default; + + + /** Update the minimum bound timestamp such as to limit the number of entities stored in the buffer */ + void updateMin(TimeT min) { + this->absolute_bound = min; + this->enforceBound(); + } + + /** Add a new sample to the buffer */ + inline void insert(TimeT time, const Type& sample) + { this->insert(time, sample); } + /** Add a new sample to the buffer */ + void insert(TimeT time, Type&& sample) { + if(this->samples.size() <= 0 || time > this->samples.back().first) { + this->samples.emplace_back(time, std::forward(sample)); + } else { + auto after = std::upper_bound(this->samples.begin(), this->samples.end(), time, &This_T::t_less__); + auto before = after - 1; + if(after == this->samples.begin()) { + this->samples.insert(after, std::pair{time, std::forward(sample)}); + } else { + if(before == this->samples.begin() || before->first < time) { + this->samples.insert(after, std::pair{time, std::forward(sample)}); + } else { + before->second = sample; + } + } + } + } + + /** Erase all the elements */ + inline void clear() { this->samples.clear(); } + + /** Sample the element with a timestamp closest to that given as a parameter */ + inline const Type* sample(TimeT time) const { + const ElemT* elem = this->sampleTimestamped(time); + return elem ? elem->second : nullptr; + } + /** Sample the closest element, but returns the full pair so that the element's timestamp can be accessed */ + const ElemT* sampleTimestamped(TimeT time) const { + if( this->samples.empty() ) return nullptr; + if( time <= this->samples.front().first ) return &this->samples.front(); + if( time >= this->samples.back().first ) return &this->samples.back(); + if( this->samples.size() == 1 ) return &this->samples[0]; + + auto greater = std::lower_bound(this->samples.begin(), this->samples.end(), time, &This_T::t_greater__); // "lower bound" of times greater than t + if( greater == this->samples.begin() ) return &*greater; + + auto less = greater - 1; + return abs(time - less->first) < abs(greater->first - time) ? &*less : &*greater; // return closer sample + } + + /** Return a const reference to the internal element buffer */ + inline const std::vector& getSamples() const { return this->samples; } + + +protected: + void enforceBound() { + auto last = this->samples.begin(); + for(; last < this->samples.end() && last->first < this->absolute_bound; last++); + this->samples.erase(this->samples.begin(), last); + } + +protected: + TimeT absolute_bound = static_cast(0); // the absolute minimum bounding time + std::vector samples{}; // use a tree for better search performance + + +}; diff --git a/src/main.cpp b/src/main.cpp index 7066749..df86eff 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,14 +1,16 @@ #include "./ldrp/perception.hpp" +#include + #include int main(int argc, char** argv) { rclcpp::init(argc, argv); - - + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); + return 0; }