add function computeCost file layer.h
This commit is contained in:
@@ -378,15 +378,25 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
|
||||
bool clearing;
|
||||
bool marking;
|
||||
bool inf_is_valid;
|
||||
std::string sensor_frame;
|
||||
double observation_persistence;
|
||||
double expected_update_rate;
|
||||
double min_obstacle_height;
|
||||
double max_obstacle_height;
|
||||
double obstacle_range;
|
||||
double raytrace_range;
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "expected_update_rate", expected_update_rate);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "obstacle_range", obstacle_range);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "raytrace_range", raytrace_range);
|
||||
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
|
||||
}
|
||||
}
|
||||
@@ -417,15 +427,25 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
|
||||
bool clearing;
|
||||
bool marking;
|
||||
bool inf_is_valid;
|
||||
std::string sensor_frame;
|
||||
double observation_persistence;
|
||||
double expected_update_rate;
|
||||
double min_obstacle_height;
|
||||
double max_obstacle_height;
|
||||
double obstacle_range;
|
||||
double raytrace_range;
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "expected_update_rate", expected_update_rate);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "obstacle_range", obstacle_range);
|
||||
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "raytrace_range", raytrace_range);
|
||||
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,3 +1,906 @@
|
||||
// // /*********************************************************************
|
||||
// // *
|
||||
// // * Software License Agreement (BSD License)
|
||||
// // *
|
||||
// // * Copyright (c) 2008, 2013, Willow Garage, Inc.
|
||||
// // * All rights reserved.
|
||||
// // *
|
||||
// // * Redistribution and use in source and binary forms, with or without
|
||||
// // * modification, are permitted provided that the following conditions
|
||||
// // * are met:
|
||||
// // *
|
||||
// // * * Redistributions of source code must retain the above copyright
|
||||
// // * notice, this list of conditions and the following disclaimer.
|
||||
// // * * Redistributions in binary form must reproduce the above
|
||||
// // * copyright notice, this list of conditions and the following
|
||||
// // * disclaimer in the documentation and/or other materials provided
|
||||
// // * with the distribution.
|
||||
// // * * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
// // * contributors may be used to endorse or promote products derived
|
||||
// // * from this software without specific prior written permission.
|
||||
// // *
|
||||
// // * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
// // * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
// // * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
// // * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
// // * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
// // * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
// // * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
// // * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
// // * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
// // * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
// // * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
// // * POSSIBILITY OF SUCH DAMAGE.
|
||||
// // *
|
||||
// // * Author: Eitan Marder-Eppstein
|
||||
// // *********************************************************************/
|
||||
// // #include <robot_costmap_2d/observation_buffer.h>
|
||||
|
||||
// // #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
// // #include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
|
||||
// // #include <robot_sensor_msgs/point_cloud2_iterator.h>
|
||||
|
||||
// // using namespace std;
|
||||
// // using namespace tf3;
|
||||
|
||||
// // namespace robot_costmap_2d
|
||||
// // {
|
||||
// // ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
|
||||
// // double min_obstacle_height, double max_obstacle_height, double obstacle_range,
|
||||
// // double raytrace_range, tf3::BufferCore& tf3_buffer, string global_frame,
|
||||
// // string sensor_frame, double tf_tolerance) :
|
||||
// // tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
|
||||
// // last_updated_(robot::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
|
||||
// // min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
|
||||
// // obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance), voxel_size_(0.05)
|
||||
// // {
|
||||
// // }
|
||||
|
||||
// // ObservationBuffer::~ObservationBuffer()
|
||||
// // {
|
||||
// // }
|
||||
|
||||
// // bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
|
||||
// // {
|
||||
// // tf3::Time transform_time = tf3::Time::now();
|
||||
// // std::string tf_error;
|
||||
|
||||
// // robot_geometry_msgs::TransformStamped transformStamped;
|
||||
// // if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
|
||||
// // {
|
||||
// // robot::log_error("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
|
||||
// // global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
|
||||
// // return false;
|
||||
// // }
|
||||
|
||||
// // list<Observation>::iterator obs_it;
|
||||
// // for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
|
||||
// // {
|
||||
// // try
|
||||
// // {
|
||||
// // Observation& obs = *obs_it;
|
||||
|
||||
// // robot_geometry_msgs::PointStamped origin;
|
||||
// // origin.header.frame_id = global_frame_;
|
||||
// // origin.header.stamp = data_convert::convertTime(transform_time);
|
||||
// // origin.point = obs.origin_;
|
||||
|
||||
// // // we need to transform the origin of the observation to the new global frame
|
||||
// // // tf3_buffer_.transform(origin, origin, new_global_frame);
|
||||
// // tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
|
||||
// // new_global_frame, // frame đích
|
||||
// // origin.header.frame_id, // frame nguồn
|
||||
// // transform_time
|
||||
// // );
|
||||
// // tf3::doTransform(origin, origin, tfm_1);
|
||||
// // obs.origin_ = origin.point;
|
||||
|
||||
// // // we also need to transform the cloud of the observation to the new global frame
|
||||
// // // tf3_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
|
||||
// // tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
|
||||
// // new_global_frame, // frame đích
|
||||
// // obs.cloud_->header.frame_id, // frame nguồn
|
||||
// // transform_time
|
||||
// // );
|
||||
// // tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2);
|
||||
// // }
|
||||
// // catch (TransformException& ex)
|
||||
// // {
|
||||
// // robot::log_error("TF Error attempting to transform an observation from %s to %s: %s\n", global_frame_.c_str(),
|
||||
// // new_global_frame.c_str(), ex.what());
|
||||
// // return false;
|
||||
// // }
|
||||
// // }
|
||||
|
||||
// // // now we need to update our global_frame member
|
||||
// // global_frame_ = new_global_frame;
|
||||
// // return true;
|
||||
// // }
|
||||
|
||||
// // // void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
|
||||
// // // {
|
||||
// // // robot_geometry_msgs::PointStamped global_origin;
|
||||
|
||||
// // // // create a new observation on the list to be populated
|
||||
// // // observation_list_.push_front(Observation());
|
||||
|
||||
// // // // check whether the origin frame has been set explicitly or whether we should get it from the cloud
|
||||
// // // string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
|
||||
|
||||
// // // try
|
||||
// // // {
|
||||
// // // // given these observations come from sensors... we'll need to store the origin pt of the sensor
|
||||
// // // robot_geometry_msgs::PointStamped local_origin;
|
||||
// // // local_origin.header.stamp = cloud.header.stamp;
|
||||
// // // local_origin.header.frame_id = origin_frame;
|
||||
// // // local_origin.point.x = 0;
|
||||
// // // local_origin.point.y = 0;
|
||||
// // // local_origin.point.z = 0;
|
||||
// // // // tf3_buffer_.transform(local_origin, global_origin, global_frame_);
|
||||
// // // tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
|
||||
// // // global_frame_, // frame đích
|
||||
// // // local_origin.header.frame_id, // frame nguồn
|
||||
// // // tf3::Time()
|
||||
// // // // data_convert::convertTime(local_origin.header.stamp)
|
||||
// // // );
|
||||
// // // tf3::doTransform(local_origin, global_origin, tfm_1);
|
||||
// // // tf3::convert(global_origin.point, observation_list_.front().origin_);
|
||||
|
||||
// // // // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
|
||||
// // // observation_list_.front().raytrace_range_ = raytrace_range_;
|
||||
// // // observation_list_.front().obstacle_range_ = obstacle_range_;
|
||||
|
||||
// // // robot_sensor_msgs::PointCloud2 global_frame_cloud;
|
||||
|
||||
// // // // transform the point cloud
|
||||
// // // // tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_);
|
||||
// // // tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
|
||||
// // // global_frame_, // frame đích
|
||||
// // // cloud.header.frame_id, // frame nguồn
|
||||
// // // tf3::Time()
|
||||
// // // // data_convert::convertTime(cloud.header.stamp)
|
||||
// // // );
|
||||
// // // tf3::doTransform(cloud, global_frame_cloud, tfm_2);
|
||||
// // // global_frame_cloud.header.stamp = cloud.header.stamp;
|
||||
|
||||
// // // // now we need to remove observations from the cloud that are below or above our height thresholds
|
||||
// // // robot_sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
|
||||
// // // observation_cloud.height = global_frame_cloud.height;
|
||||
// // // observation_cloud.width = global_frame_cloud.width;
|
||||
// // // observation_cloud.fields = global_frame_cloud.fields;
|
||||
// // // observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
|
||||
// // // observation_cloud.point_step = global_frame_cloud.point_step;
|
||||
// // // observation_cloud.row_step = global_frame_cloud.row_step;
|
||||
// // // observation_cloud.is_dense = global_frame_cloud.is_dense;
|
||||
|
||||
// // // unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
|
||||
// // // robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
|
||||
// // // modifier.resize(cloud_size);
|
||||
// // // unsigned int point_count = 0;
|
||||
|
||||
// // // // copy over the points that are within our height bounds
|
||||
// // // robot_sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
|
||||
// // // std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
|
||||
// // // std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
|
||||
// // // for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
|
||||
// // // {
|
||||
// // // if ((*iter_z) <= max_obstacle_height_
|
||||
// // // && (*iter_z) >= min_obstacle_height_)
|
||||
// // // {
|
||||
// // // std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
|
||||
// // // iter_obs += global_frame_cloud.point_step;
|
||||
// // // ++point_count;
|
||||
// // // }
|
||||
// // // }
|
||||
|
||||
// // // // resize the cloud for the number of legal points
|
||||
// // // modifier.resize(point_count);
|
||||
// // // observation_cloud.header.stamp = cloud.header.stamp;
|
||||
// // // observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
|
||||
// // // }
|
||||
// // // catch (TransformException& ex)
|
||||
// // // {
|
||||
// // // // if an exception occurs, we need to remove the empty observation from the list
|
||||
// // // observation_list_.pop_front();
|
||||
// // // robot::log_error("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
|
||||
// // // cloud.header.frame_id.c_str(), ex.what());
|
||||
// // // return;
|
||||
// // // }
|
||||
|
||||
// // // // if the update was successful, we want to update the last updated time
|
||||
// // // last_updated_ = robot::Time::now();
|
||||
|
||||
// // // // we'll also remove any stale observations from the list
|
||||
// // // purgeStaleObservations();
|
||||
// // // }
|
||||
|
||||
|
||||
|
||||
// // // void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
|
||||
// // // {
|
||||
// // // robot_geometry_msgs::PointStamped global_origin;
|
||||
|
||||
// // // observation_list_.push_front(Observation());
|
||||
|
||||
// // // string origin_frame = sensor_frame_.empty() ? cloud.header.frame_id : sensor_frame_;
|
||||
|
||||
// // // try
|
||||
// // // {
|
||||
// // // // --- [1] TF lookups: giữ nguyên, không thể tránh ---
|
||||
// // // robot_geometry_msgs::PointStamped local_origin;
|
||||
// // // local_origin.header.stamp = cloud.header.stamp;
|
||||
// // // local_origin.header.frame_id = origin_frame;
|
||||
// // // local_origin.point.x = local_origin.point.y = local_origin.point.z = 0;
|
||||
|
||||
// // // tf3::TransformStampedMsg tfm_origin = tf3_buffer_.lookupTransform(
|
||||
// // // global_frame_, local_origin.header.frame_id, tf3::Time());
|
||||
// // // tf3::doTransform(local_origin, global_origin, tfm_origin);
|
||||
// // // // origin là 1 điểm duy nhất → doTransform ổn
|
||||
// // // tf3::convert(global_origin.point, observation_list_.front().origin_);
|
||||
|
||||
// // // observation_list_.front().raytrace_range_ = raytrace_range_;
|
||||
// // // observation_list_.front().obstacle_range_ = obstacle_range_;
|
||||
|
||||
// // // // --- [2] Trích ma trận transform 1 lần cho toàn bộ cloud ---
|
||||
// // // tf3::TransformStampedMsg tfm_cloud = tf3_buffer_.lookupTransform(
|
||||
// // // global_frame_, cloud.header.frame_id, tf3::Time());
|
||||
// // // const Transform4x4 M = extractMatrix(tfm_cloud);
|
||||
|
||||
// // // // --- [3] Tìm offset của x, y, z trong point layout ---
|
||||
// // // int x_off = -1, y_off = -1, z_off = -1;
|
||||
// // // for (const auto& field : cloud.fields) {
|
||||
// // // if (field.name == "x") x_off = (int)field.offset;
|
||||
// // // else if (field.name == "y") y_off = (int)field.offset;
|
||||
// // // else if (field.name == "z") z_off = (int)field.offset;
|
||||
// // // }
|
||||
// // // // Fallback nếu không tìm thấy (không nên xảy ra với PointCloud2 hợp lệ)
|
||||
// // // if (x_off < 0 || y_off < 0 || z_off < 0) {
|
||||
// // // observation_list_.pop_front();
|
||||
// // // robot::log_error("PointCloud2 thiếu field x/y/z\n");
|
||||
// // // return;
|
||||
// // // }
|
||||
|
||||
// // // // --- [4] Setup output cloud (copy metadata, không copy data) ---
|
||||
// // // robot_sensor_msgs::PointCloud2& obs_cloud = *(observation_list_.front().cloud_);
|
||||
// // // obs_cloud.fields = cloud.fields; // shallow copy, thường nhỏ
|
||||
// // // obs_cloud.is_bigendian = cloud.is_bigendian;
|
||||
// // // obs_cloud.point_step = cloud.point_step;
|
||||
// // // obs_cloud.is_dense = cloud.is_dense;
|
||||
// // // obs_cloud.height = 1; // output luôn là unordered
|
||||
|
||||
// // // const uint32_t point_step = cloud.point_step;
|
||||
// // // const uint32_t cloud_size = cloud.height * cloud.width;
|
||||
|
||||
// // // // [KEY] Reserve trước toàn bộ capacity → tránh realloc nhiều lần
|
||||
// // // // Worst case: tất cả points đều pass filter
|
||||
// // // obs_cloud.data.reserve(static_cast<size_t>(cloud_size) * point_step);
|
||||
|
||||
// // // // --- [5] SINGLE PASS: transform + filter + copy ---
|
||||
// // // const uint8_t* src_ptr = cloud.data.data();
|
||||
// // // uint32_t point_count = 0;
|
||||
|
||||
// // // for (uint32_t i = 0; i < cloud_size; ++i, src_ptr += point_step)
|
||||
// // // {
|
||||
// // // // Đọc x, y, z gốc (float32)
|
||||
// // // float lx, ly, lz;
|
||||
// // // std::memcpy(&lx, src_ptr + x_off, sizeof(float));
|
||||
// // // std::memcpy(&ly, src_ptr + y_off, sizeof(float));
|
||||
// // // std::memcpy(&lz, src_ptr + z_off, sizeof(float));
|
||||
|
||||
// // // // Áp dụng transform (matrix multiply inline, không có virtual call)
|
||||
// // // // Với PointCloud2 thường dùng float32, cast double→float ở cuối
|
||||
// // // const double gx = M.m[0][0]*lx + M.m[0][1]*ly + M.m[0][2]*lz + M.m[0][3];
|
||||
// // // const double gy = M.m[1][0]*lx + M.m[1][1]*ly + M.m[1][2]*lz + M.m[1][3];
|
||||
// // // const double gz = M.m[2][0]*lx + M.m[2][1]*ly + M.m[2][2]*lz + M.m[2][3];
|
||||
|
||||
// // // // Filter height (dùng gz vừa tính, không cần PointCloud2Iterator)
|
||||
// // // if (gz < min_obstacle_height_ || gz > max_obstacle_height_)
|
||||
// // // continue;
|
||||
|
||||
// // // // Copy toàn bộ point (giữ nguyên các field khác: intensity, ring, …)
|
||||
// // // // rồi patch lại x, y, z bằng giá trị đã transform
|
||||
// // // const size_t insert_pos = obs_cloud.data.size();
|
||||
// // // obs_cloud.data.resize(insert_pos + point_step);
|
||||
// // // uint8_t* dst_ptr = obs_cloud.data.data() + insert_pos;
|
||||
|
||||
// // // std::memcpy(dst_ptr, src_ptr, point_step);
|
||||
|
||||
// // // // Ghi lại x, y, z đã transform (float32)
|
||||
// // // const float gxf = static_cast<float>(gx);
|
||||
// // // const float gyf = static_cast<float>(gy);
|
||||
// // // const float gzf = static_cast<float>(gz);
|
||||
// // // std::memcpy(dst_ptr + x_off, &gxf, sizeof(float));
|
||||
// // // std::memcpy(dst_ptr + y_off, &gyf, sizeof(float));
|
||||
// // // std::memcpy(dst_ptr + z_off, &gzf, sizeof(float));
|
||||
|
||||
// // // ++point_count;
|
||||
// // // }
|
||||
|
||||
// // // // --- [6] Finalize output ---
|
||||
// // // obs_cloud.width = point_count;
|
||||
// // // obs_cloud.row_step = point_count * point_step;
|
||||
// // // obs_cloud.header.stamp = cloud.header.stamp;
|
||||
// // // obs_cloud.header.frame_id = global_frame_;
|
||||
|
||||
// // // // Giải phóng capacity dư (optional, tùy memory pressure)
|
||||
// // // // obs_cloud.data.shrink_to_fit();
|
||||
// // // }
|
||||
// // // catch (TransformException& ex)
|
||||
// // // {
|
||||
// // // observation_list_.pop_front();
|
||||
// // // robot::log_error("TF Exception: sensor_frame=%s, cloud_frame=%s: %s\n",
|
||||
// // // sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what());
|
||||
// // // return;
|
||||
// // // }
|
||||
|
||||
// // // last_updated_ = robot::Time::now();
|
||||
// // // purgeStaleObservations();
|
||||
// // // }
|
||||
|
||||
// // void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
|
||||
// // {
|
||||
// // robot_geometry_msgs::PointStamped global_origin;
|
||||
// // observation_list_.push_front(Observation());
|
||||
// // string origin_frame = sensor_frame_.empty() ? cloud.header.frame_id : sensor_frame_;
|
||||
|
||||
// // try
|
||||
// // {
|
||||
// // // [1] Transform origin (giữ nguyên)
|
||||
// // robot_geometry_msgs::PointStamped local_origin;
|
||||
// // local_origin.header.stamp = cloud.header.stamp;
|
||||
// // local_origin.header.frame_id = origin_frame;
|
||||
// // local_origin.point.x = local_origin.point.y = local_origin.point.z = 0;
|
||||
|
||||
// // tf3::TransformStampedMsg tfm_origin = tf3_buffer_.lookupTransform(
|
||||
// // global_frame_, local_origin.header.frame_id, tf3::Time());
|
||||
// // tf3::doTransform(local_origin, global_origin, tfm_origin);
|
||||
// // tf3::convert(global_origin.point, observation_list_.front().origin_);
|
||||
|
||||
// // observation_list_.front().raytrace_range_ = raytrace_range_;
|
||||
// // observation_list_.front().obstacle_range_ = obstacle_range_;
|
||||
|
||||
// // // [2] Lấy transform matrix 1 lần
|
||||
// // tf3::TransformStampedMsg tfm_cloud = tf3_buffer_.lookupTransform(
|
||||
// // global_frame_, cloud.header.frame_id, tf3::Time());
|
||||
// // const Transform4x4 M = extractMatrix(tfm_cloud);
|
||||
|
||||
// // // [3] Tìm offset x/y/z
|
||||
// // int x_off = -1, y_off = -1, z_off = -1;
|
||||
// // for (const auto& field : cloud.fields) {
|
||||
// // if (field.name == "x") x_off = (int)field.offset;
|
||||
// // else if (field.name == "y") y_off = (int)field.offset;
|
||||
// // else if (field.name == "z") z_off = (int)field.offset;
|
||||
// // }
|
||||
// // if (x_off < 0 || y_off < 0 || z_off < 0) {
|
||||
// // observation_list_.pop_front();
|
||||
// // robot::log_error("PointCloud2 thiếu field x/y/z\n");
|
||||
// // return;
|
||||
// // }
|
||||
|
||||
// // // [4] Setup output cloud
|
||||
// // robot_sensor_msgs::PointCloud2& obs_cloud = *(observation_list_.front().cloud_);
|
||||
// // obs_cloud.fields = cloud.fields;
|
||||
// // obs_cloud.is_bigendian = cloud.is_bigendian;
|
||||
// // obs_cloud.point_step = cloud.point_step;
|
||||
// // obs_cloud.is_dense = cloud.is_dense;
|
||||
// // obs_cloud.height = 1;
|
||||
|
||||
// // const uint32_t point_step = cloud.point_step;
|
||||
// // const uint32_t cloud_size = cloud.height * cloud.width;
|
||||
|
||||
// // // ─────────────────────────────────────────────────────────────────
|
||||
// // // [5] VOXEL FILTER theo cell costmap
|
||||
// // //
|
||||
// // // Thay vì giữ 6.5M points, ta hash mỗi point về (voxel_x, voxel_y)
|
||||
// // // theo voxel_size = costmap resolution (thường 0.05m).
|
||||
// // // Mỗi voxel cell chỉ giữ 1 point đại diện (first hit).
|
||||
// // //
|
||||
// // // Kết quả: 6.5M → số lượng ô costmap thực sự có obstacle
|
||||
// // // (~vài nghìn đến vài chục nghìn, tùy scene)
|
||||
// // // ─────────────────────────────────────────────────────────────────
|
||||
// // const double voxel_size = voxel_size_; // khớp với costmap resolution
|
||||
// // const double inv_voxel_size = 1.0 / voxel_size;
|
||||
|
||||
// // // unordered_map: key = packed (ix, iy) → value = raw point bytes
|
||||
// // // Dùng int64 pack để tránh custom hash
|
||||
// // struct VoxelData {
|
||||
// // float x, y, z;
|
||||
// // std::vector<uint8_t> raw; // toàn bộ point_step bytes gốc
|
||||
// // };
|
||||
|
||||
// // // Ước lượng số voxel thực tế: reserve để tránh rehash
|
||||
// // // Với scene thực tế thường << 100K cells có obstacle
|
||||
// // std::unordered_map<int64_t, VoxelData> voxel_map;
|
||||
// // voxel_map.reserve(65536); // 64K slots ban đầu
|
||||
|
||||
// // const uint8_t* src_ptr = cloud.data.data();
|
||||
|
||||
// // for (uint32_t i = 0; i < cloud_size; ++i, src_ptr += point_step)
|
||||
// // {
|
||||
// // float lx, ly, lz;
|
||||
// // std::memcpy(&lx, src_ptr + x_off, sizeof(float));
|
||||
// // std::memcpy(&ly, src_ptr + y_off, sizeof(float));
|
||||
// // std::memcpy(&lz, src_ptr + z_off, sizeof(float));
|
||||
|
||||
// // // Bỏ qua NaN (thường xuất hiện trong depth camera cloud)
|
||||
// // if (!std::isfinite(lx) || !std::isfinite(ly) || !std::isfinite(lz))
|
||||
// // continue;
|
||||
|
||||
// // // Transform sang global frame
|
||||
// // const double gx = M.m[0][0]*lx + M.m[0][1]*ly + M.m[0][2]*lz + M.m[0][3];
|
||||
// // const double gy = M.m[1][0]*lx + M.m[1][1]*ly + M.m[1][2]*lz + M.m[1][3];
|
||||
// // const double gz = M.m[2][0]*lx + M.m[2][1]*ly + M.m[2][2]*lz + M.m[2][3];
|
||||
|
||||
// // // Height filter
|
||||
// // if (gz < min_obstacle_height_ || gz > max_obstacle_height_)
|
||||
// // continue;
|
||||
|
||||
// // // Tính voxel index (floor division, handle negative coords)
|
||||
// // const int32_t ix = static_cast<int32_t>(std::floor(gx * inv_voxel_size));
|
||||
// // const int32_t iy = static_cast<int32_t>(std::floor(gy * inv_voxel_size));
|
||||
|
||||
// // // Pack 2×int32 thành 1×int64 làm key
|
||||
// // const int64_t key = (static_cast<int64_t>(ix) << 32) |
|
||||
// // static_cast<int64_t>(static_cast<uint32_t>(iy));
|
||||
|
||||
// // // Chỉ insert nếu voxel này chưa có điểm nào (first-hit policy)
|
||||
// // auto result = voxel_map.emplace(key, VoxelData{});
|
||||
// // if (result.second) // true = voxel mới, chưa có data
|
||||
// // {
|
||||
// // VoxelData& vd = result.first->second;
|
||||
// // vd.x = static_cast<float>(gx);
|
||||
// // vd.y = static_cast<float>(gy);
|
||||
// // vd.z = static_cast<float>(gz);
|
||||
// // vd.raw.assign(src_ptr, src_ptr + point_step);
|
||||
// // // Patch x/y/z trong raw bytes ngay tại đây
|
||||
// // std::memcpy(vd.raw.data() + x_off, &vd.x, sizeof(float));
|
||||
// // std::memcpy(vd.raw.data() + y_off, &vd.y, sizeof(float));
|
||||
// // std::memcpy(vd.raw.data() + z_off, &vd.z, sizeof(float));
|
||||
// // }
|
||||
// // // Nếu voxel đã có → bỏ qua (không cần xử lý thêm)
|
||||
// // }
|
||||
|
||||
// // // [6] Ghi kết quả voxel filter vào obs_cloud
|
||||
// // const uint32_t point_count = static_cast<uint32_t>(voxel_map.size());
|
||||
// // obs_cloud.data.resize(static_cast<size_t>(point_count) * point_step);
|
||||
|
||||
// // uint8_t* dst = obs_cloud.data.data();
|
||||
// // for (const auto& kv : voxel_map)
|
||||
// // {
|
||||
// // std::memcpy(dst, kv.second.raw.data(), point_step);
|
||||
// // dst += point_step;
|
||||
// // }
|
||||
|
||||
// // obs_cloud.width = point_count;
|
||||
// // obs_cloud.row_step = point_count * point_step;
|
||||
// // obs_cloud.header.stamp = cloud.header.stamp;
|
||||
// // obs_cloud.header.frame_id = global_frame_;
|
||||
// // }
|
||||
// // catch (TransformException& ex)
|
||||
// // {
|
||||
// // observation_list_.pop_front();
|
||||
// // robot::log_error("TF Exception: sensor_frame=%s, cloud_frame=%s: %s\n",
|
||||
// // sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what());
|
||||
// // return;
|
||||
// // }
|
||||
|
||||
// // last_updated_ = robot::Time::now();
|
||||
// // purgeStaleObservations();
|
||||
// // }
|
||||
|
||||
// // // returns a copy of the observations
|
||||
// // void ObservationBuffer::getObservations(vector<Observation>& observations)
|
||||
// // {
|
||||
// // // first... let's make sure that we don't have any stale observations
|
||||
// // purgeStaleObservations();
|
||||
|
||||
// // // now we'll just copy the observations for the caller
|
||||
// // list<Observation>::iterator obs_it;
|
||||
// // for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
|
||||
// // {
|
||||
// // observations.push_back(*obs_it);
|
||||
// // }
|
||||
// // }
|
||||
|
||||
// // void ObservationBuffer::purgeStaleObservations()
|
||||
// // {
|
||||
// // if (!observation_list_.empty())
|
||||
// // {
|
||||
// // list<Observation>::iterator obs_it = observation_list_.begin();
|
||||
// // // if we're keeping observations for no time... then we'll only keep one observation
|
||||
// // if (observation_keep_time_ == robot::Duration(0.0))
|
||||
// // {
|
||||
// // observation_list_.erase(++obs_it, observation_list_.end());
|
||||
// // return;
|
||||
// // }
|
||||
|
||||
// // // otherwise... we'll have to loop through the observations to see which ones are stale
|
||||
// // for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
|
||||
// // {
|
||||
// // Observation& obs = *obs_it;
|
||||
// // // check if the observation is out of date... and if it is, remove it and those that follow from the list
|
||||
// // if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
|
||||
// // {
|
||||
// // observation_list_.erase(obs_it, observation_list_.end());
|
||||
// // return;
|
||||
// // }
|
||||
// // }
|
||||
// // }
|
||||
// // }
|
||||
|
||||
// // bool ObservationBuffer::isCurrent() const
|
||||
// // {
|
||||
// // if (expected_update_rate_ == robot::Duration(0.0))
|
||||
// // return true;
|
||||
|
||||
// // bool current = (robot::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
|
||||
// // if (!current)
|
||||
// // {
|
||||
// // robot::log_error("The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
|
||||
// // topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
|
||||
// // }
|
||||
// // return current;
|
||||
// // }
|
||||
|
||||
// // void ObservationBuffer::resetLastUpdated()
|
||||
// // {
|
||||
// // last_updated_ = robot::Time::now();
|
||||
// // }
|
||||
// // } // namespace robot_costmap_2d
|
||||
|
||||
// /*********************************************************************
|
||||
// *
|
||||
// * Software License Agreement (BSD License)
|
||||
// *
|
||||
// * Copyright (c) 2008, 2013, Willow Garage, Inc.
|
||||
// * All rights reserved.
|
||||
// * (License text omitted for brevity – same as original)
|
||||
// *
|
||||
// * Author: Eitan Marder-Eppstein
|
||||
// *
|
||||
// * ── Optimization notes ──────────────────────────────────────────────
|
||||
// *
|
||||
// * bufferCloud() – single-pass voxel-downsampling transform
|
||||
// * ─────────────────────────────────────────────────────────────────────
|
||||
// * Original pipeline (3 passes, 6.5 M × point_step bytes each):
|
||||
// * Pass 1 – tf3::doTransform → global_frame_cloud (new allocation)
|
||||
// * Pass 2 – height filter → observation_cloud (byte-by-byte copy)
|
||||
// * Pass 3 – updateBounds/raytrace iter the result again
|
||||
// *
|
||||
// * Optimised pipeline (1 pass, result << 6.5 M points):
|
||||
// * Single loop:
|
||||
// * memcpy x/y/z → inline matrix-multiply → height filter →
|
||||
// * voxel-hash (int64 key) → first-hit insert into flat output buffer
|
||||
// *
|
||||
// * Voxel size = costmap resolution (default 0.05 m).
|
||||
// * With a 10 m × 10 m map → at most 40 000 output points instead of
|
||||
// * 6 500 000. Every downstream consumer (updateBounds marking loop,
|
||||
// * raytraceFreespace, frustum-clearing FOV scan) benefits equally.
|
||||
// *
|
||||
// * Key micro-optimisations
|
||||
// * ───────────────────────
|
||||
// * • extractMatrix() – quaternion → 4×4 double once per cloud; avoids
|
||||
// * virtual-dispatch / exception-guard overhead of doTransform per point.
|
||||
// * • std::memcpy for unaligned float reads (safe on all platforms).
|
||||
// * • unordered_map::emplace with int64 packed key – O(1) amortised.
|
||||
// * • reserve(65536) on the map to avoid rehash for typical scenes.
|
||||
// * • Output data written directly into obs_cloud.data (no intermediate
|
||||
// * vector of VoxelData structs on heap).
|
||||
// * • NaN guard before the hash (depth cameras emit NaN for invalid pixels).
|
||||
// *********************************************************************/
|
||||
// #include <robot_costmap_2d/observation_buffer.h>
|
||||
|
||||
// #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
// #include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
|
||||
// #include <robot_sensor_msgs/point_cloud2_iterator.h>
|
||||
|
||||
// #include <unordered_map>
|
||||
// #include <cmath>
|
||||
// #include <cstring>
|
||||
|
||||
// using namespace std;
|
||||
// using namespace tf3;
|
||||
|
||||
// namespace robot_costmap_2d
|
||||
// {
|
||||
|
||||
// // ── Constructor / Destructor ─────────────────────────────────────────────────
|
||||
|
||||
// ObservationBuffer::ObservationBuffer(string topic_name,
|
||||
// double observation_keep_time,
|
||||
// double expected_update_rate,
|
||||
// double min_obstacle_height,
|
||||
// double max_obstacle_height,
|
||||
// double obstacle_range,
|
||||
// double raytrace_range,
|
||||
// tf3::BufferCore& tf3_buffer,
|
||||
// string global_frame,
|
||||
// string sensor_frame,
|
||||
// double tf_tolerance)
|
||||
// : tf3_buffer_(tf3_buffer)
|
||||
// , observation_keep_time_(observation_keep_time)
|
||||
// , expected_update_rate_(expected_update_rate)
|
||||
// , last_updated_(robot::Time::now())
|
||||
// , global_frame_(global_frame)
|
||||
// , sensor_frame_(sensor_frame)
|
||||
// , topic_name_(topic_name)
|
||||
// , min_obstacle_height_(min_obstacle_height)
|
||||
// , max_obstacle_height_(max_obstacle_height)
|
||||
// , obstacle_range_(obstacle_range)
|
||||
// , raytrace_range_(raytrace_range)
|
||||
// , tf_tolerance_(tf_tolerance)
|
||||
// , voxel_size_(0.05)
|
||||
// , inv_voxel_size_(20.0)
|
||||
// {}
|
||||
|
||||
// ObservationBuffer::~ObservationBuffer() {}
|
||||
|
||||
// // ── setGlobalFrame ───────────────────────────────────────────────────────────
|
||||
|
||||
// bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
|
||||
// {
|
||||
// tf3::Time transform_time = tf3::Time::now();
|
||||
// std::string tf_error;
|
||||
|
||||
// if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
|
||||
// {
|
||||
// robot::log_error("Transform between %s and %s with tolerance %.2f failed: %s.\n",
|
||||
// new_global_frame.c_str(), global_frame_.c_str(),
|
||||
// tf_tolerance_, tf_error.c_str());
|
||||
// return false;
|
||||
// }
|
||||
|
||||
// for (auto& obs : observation_list_)
|
||||
// {
|
||||
// try
|
||||
// {
|
||||
// robot_geometry_msgs::PointStamped origin;
|
||||
// origin.header.frame_id = global_frame_;
|
||||
// origin.header.stamp = data_convert::convertTime(transform_time);
|
||||
// origin.point = obs.origin_;
|
||||
|
||||
// tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
|
||||
// new_global_frame, origin.header.frame_id, transform_time);
|
||||
// tf3::doTransform(origin, origin, tfm_1);
|
||||
// obs.origin_ = origin.point;
|
||||
|
||||
// tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
|
||||
// new_global_frame, obs.cloud_->header.frame_id, transform_time);
|
||||
// tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2);
|
||||
// }
|
||||
// catch (TransformException& ex)
|
||||
// {
|
||||
// robot::log_error("TF Error attempting to transform an observation from %s to %s: %s\n",
|
||||
// global_frame_.c_str(), new_global_frame.c_str(), ex.what());
|
||||
// return false;
|
||||
// }
|
||||
// }
|
||||
|
||||
// global_frame_ = new_global_frame;
|
||||
// return true;
|
||||
// }
|
||||
|
||||
// // ── bufferCloud ──────────────────────────────────────────────────────────────
|
||||
// //
|
||||
// // Single-pass: transform → height-filter → voxel-downsample → write output.
|
||||
// // No intermediate global_frame_cloud allocation.
|
||||
|
||||
// void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
|
||||
// {
|
||||
// robot_geometry_msgs::PointStamped global_origin;
|
||||
// observation_list_.push_front(Observation());
|
||||
|
||||
// const string origin_frame = sensor_frame_.empty() ? cloud.header.frame_id : sensor_frame_;
|
||||
|
||||
// try
|
||||
// {
|
||||
// // ── [1] Transform sensor origin (single point – doTransform is fine) ──
|
||||
// robot_geometry_msgs::PointStamped local_origin;
|
||||
// local_origin.header.stamp = cloud.header.stamp;
|
||||
// local_origin.header.frame_id = origin_frame;
|
||||
// local_origin.point.x = local_origin.point.y = local_origin.point.z = 0.0;
|
||||
|
||||
// tf3::TransformStampedMsg tfm_origin =
|
||||
// tf3_buffer_.lookupTransform(global_frame_, origin_frame, tf3::Time());
|
||||
// tf3::doTransform(local_origin, global_origin, tfm_origin);
|
||||
// tf3::convert(global_origin.point, observation_list_.front().origin_);
|
||||
|
||||
// observation_list_.front().raytrace_range_ = raytrace_range_;
|
||||
// observation_list_.front().obstacle_range_ = obstacle_range_;
|
||||
|
||||
// // ── [2] Extract 4×4 rotation+translation matrix once ─────────────────
|
||||
// tf3::TransformStampedMsg tfm_cloud =
|
||||
// tf3_buffer_.lookupTransform(global_frame_, cloud.header.frame_id, tf3::Time());
|
||||
// const Transform4x4 M = extractMatrix(tfm_cloud);
|
||||
|
||||
// // ── [3] Find byte offsets of x, y, z fields ───────────────────────────
|
||||
// int x_off = -1, y_off = -1, z_off = -1;
|
||||
// for (const auto& f : cloud.fields)
|
||||
// {
|
||||
// if (f.name == "x") x_off = static_cast<int>(f.offset);
|
||||
// else if (f.name == "y") y_off = static_cast<int>(f.offset);
|
||||
// else if (f.name == "z") z_off = static_cast<int>(f.offset);
|
||||
// }
|
||||
// if (x_off < 0 || y_off < 0 || z_off < 0)
|
||||
// {
|
||||
// observation_list_.pop_front();
|
||||
// robot::log_error("ObservationBuffer::bufferCloud – PointCloud2 missing x/y/z fields\n");
|
||||
// return;
|
||||
// }
|
||||
|
||||
// // ── [4] Prepare output cloud metadata (no data copy yet) ─────────────
|
||||
// robot_sensor_msgs::PointCloud2& obs_cloud = *(observation_list_.front().cloud_);
|
||||
// obs_cloud.fields = cloud.fields;
|
||||
// obs_cloud.is_bigendian = cloud.is_bigendian;
|
||||
// obs_cloud.point_step = cloud.point_step;
|
||||
// obs_cloud.is_dense = cloud.is_dense;
|
||||
// obs_cloud.height = 1; // unordered output
|
||||
|
||||
// const uint32_t point_step = cloud.point_step;
|
||||
// const uint32_t cloud_size = cloud.height * cloud.width;
|
||||
|
||||
// // ── [5] Voxel-grid downsampling + transform + height filter (1 pass) ──
|
||||
// //
|
||||
// // Key insight for 2-D costmap:
|
||||
// // Two points that fall in the same (ix, iy) voxel cell will mark the
|
||||
// // same costmap cell, so we only need one representative per voxel.
|
||||
// // We use first-hit policy: whichever point is encountered first wins.
|
||||
// //
|
||||
// // Hash: pack (int32_t ix, int32_t iy) → int64_t key.
|
||||
// // • No custom hasher needed (default hash<int64_t> is fast).
|
||||
// // • Negative world coordinates are handled correctly by casting
|
||||
// // int32 → uint32 before the shift.
|
||||
|
||||
// // Preallocate output buffer worst-case (all points pass filter).
|
||||
// // In practice the voxel map will be much smaller; we'll resize at end.
|
||||
// obs_cloud.data.reserve(static_cast<size_t>(cloud_size) * point_step);
|
||||
|
||||
// // Voxel map: key → byte offset in obs_cloud.data (first-hit written directly)
|
||||
// std::unordered_map<int64_t, uint32_t> voxel_map;
|
||||
// voxel_map.reserve(65536); // 64 K buckets – covers typical indoor scenes
|
||||
|
||||
// const double inv_vs = inv_voxel_size_;
|
||||
// const double min_h = min_obstacle_height_;
|
||||
// const double max_h = max_obstacle_height_;
|
||||
// const uint8_t* src = cloud.data.data();
|
||||
// uint32_t point_count = 0;
|
||||
|
||||
// for (uint32_t i = 0; i < cloud_size; ++i, src += point_step)
|
||||
// {
|
||||
// // Read local x/y/z (float32, potentially unaligned)
|
||||
// float lx, ly, lz;
|
||||
// std::memcpy(&lx, src + x_off, sizeof(float));
|
||||
// std::memcpy(&ly, src + y_off, sizeof(float));
|
||||
// std::memcpy(&lz, src + z_off, sizeof(float));
|
||||
|
||||
// // Skip NaN / Inf (common in depth camera output)
|
||||
// if (!std::isfinite(lx) || !std::isfinite(ly) || !std::isfinite(lz))
|
||||
// continue;
|
||||
|
||||
// // Inline 3-D transform: gp = M * [lx, ly, lz, 1]^T
|
||||
// const double gx = M.m[0][0]*lx + M.m[0][1]*ly + M.m[0][2]*lz + M.m[0][3];
|
||||
// const double gy = M.m[1][0]*lx + M.m[1][1]*ly + M.m[1][2]*lz + M.m[1][3];
|
||||
// const double gz = M.m[2][0]*lx + M.m[2][1]*ly + M.m[2][2]*lz + M.m[2][3];
|
||||
|
||||
// // Height filter (in global frame)
|
||||
// if (gz < min_h || gz > max_h)
|
||||
// continue;
|
||||
|
||||
// // Voxel index (floor division – correct for negative coordinates)
|
||||
// const int32_t ix = static_cast<int32_t>(std::floor(gx * inv_vs));
|
||||
// const int32_t iy = static_cast<int32_t>(std::floor(gy * inv_vs));
|
||||
|
||||
// // Pack into single int64 key
|
||||
// const int64_t key =
|
||||
// (static_cast<int64_t>(ix) << 32) |
|
||||
// static_cast<int64_t>(static_cast<uint32_t>(iy));
|
||||
|
||||
// // Try to insert; skip if this voxel already has a representative
|
||||
// if (!voxel_map.emplace(key, point_count).second)
|
||||
// continue;
|
||||
|
||||
// // Write point into output buffer
|
||||
// const size_t insert_pos = obs_cloud.data.size();
|
||||
// obs_cloud.data.resize(insert_pos + point_step);
|
||||
// uint8_t* dst = obs_cloud.data.data() + insert_pos;
|
||||
|
||||
// std::memcpy(dst, src, point_step);
|
||||
|
||||
// // Patch x/y/z with transformed (global-frame) values
|
||||
// const float gxf = static_cast<float>(gx);
|
||||
// const float gyf = static_cast<float>(gy);
|
||||
// const float gzf = static_cast<float>(gz);
|
||||
// std::memcpy(dst + x_off, &gxf, sizeof(float));
|
||||
// std::memcpy(dst + y_off, &gyf, sizeof(float));
|
||||
// std::memcpy(dst + z_off, &gzf, sizeof(float));
|
||||
|
||||
// ++point_count;
|
||||
// }
|
||||
|
||||
// // ── [6] Finalise output cloud ─────────────────────────────────────────
|
||||
// obs_cloud.width = point_count;
|
||||
// obs_cloud.row_step = point_count * point_step;
|
||||
// obs_cloud.header.stamp = cloud.header.stamp;
|
||||
// obs_cloud.header.frame_id = global_frame_;
|
||||
// }
|
||||
// catch (TransformException& ex)
|
||||
// {
|
||||
// observation_list_.pop_front();
|
||||
// robot::log_error("TF Exception in bufferCloud – sensor_frame=%s, cloud_frame=%s: %s\n",
|
||||
// sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what());
|
||||
// return;
|
||||
// }
|
||||
|
||||
// last_updated_ = robot::Time::now();
|
||||
// purgeStaleObservations();
|
||||
// }
|
||||
|
||||
// // ── getObservations ──────────────────────────────────────────────────────────
|
||||
|
||||
// void ObservationBuffer::getObservations(vector<Observation>& observations)
|
||||
// {
|
||||
// purgeStaleObservations();
|
||||
|
||||
// for (const auto& obs : observation_list_)
|
||||
// observations.push_back(obs);
|
||||
// }
|
||||
|
||||
// // ── purgeStaleObservations ───────────────────────────────────────────────────
|
||||
|
||||
// void ObservationBuffer::purgeStaleObservations()
|
||||
// {
|
||||
// if (observation_list_.empty())
|
||||
// return;
|
||||
|
||||
// // If keep_time == 0 → keep only the most recent observation
|
||||
// if (observation_keep_time_ == robot::Duration(0.0))
|
||||
// {
|
||||
// auto it = observation_list_.begin();
|
||||
// observation_list_.erase(++it, observation_list_.end());
|
||||
// return;
|
||||
// }
|
||||
|
||||
// // Walk forward and erase from first stale entry onward
|
||||
// for (auto it = observation_list_.begin(); it != observation_list_.end(); ++it)
|
||||
// {
|
||||
// if ((last_updated_ - it->cloud_->header.stamp) > observation_keep_time_)
|
||||
// {
|
||||
// observation_list_.erase(it, observation_list_.end());
|
||||
// return;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
// // ── isCurrent ────────────────────────────────────────────────────────────────
|
||||
|
||||
// bool ObservationBuffer::isCurrent() const
|
||||
// {
|
||||
// if (expected_update_rate_ == robot::Duration(0.0))
|
||||
// return true;
|
||||
|
||||
// const bool current =
|
||||
// (robot::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
|
||||
|
||||
// if (!current)
|
||||
// {
|
||||
// robot::log_error(
|
||||
// "The %s observation buffer has not been updated for %.2f seconds, "
|
||||
// "and it should be updated every %.2f seconds.\n",
|
||||
// topic_name_.c_str(),
|
||||
// (robot::Time::now() - last_updated_).toSec(),
|
||||
// expected_update_rate_.toSec());
|
||||
// }
|
||||
// return current;
|
||||
// }
|
||||
|
||||
// // ── resetLastUpdated ─────────────────────────────────────────────────────────
|
||||
|
||||
// void ObservationBuffer::resetLastUpdated()
|
||||
// {
|
||||
// last_updated_ = robot::Time::now();
|
||||
// }
|
||||
|
||||
// } // namespace robot_costmap_2d
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
@@ -141,7 +1044,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
|
||||
global_frame_, // frame đích
|
||||
local_origin.header.frame_id, // frame nguồn
|
||||
tf3::Time()
|
||||
// data_convert::convertTime(local_origin.header.stamp)
|
||||
// data_convert::convertTime(cloud.header.stamp)
|
||||
);
|
||||
tf3::doTransform(local_origin, global_origin, tfm_1);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user