DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

:optimal

Browse Source
This commit is contained in:
duongtd
2026-07-27 10:09:38 +07:00
parent c888af3b7c
commit e2ee28bd63
2 changed files with 33 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
plugins/voxel_layer.cpp
View File

@@ -602,18 +602,33 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
local_origin.point.y = 0.0; local_origin.point.y = 0.0;
local_origin.point.z = 0.0; local_origin.point.z = 0.0;
// Look up the sensor pose at the depth image's CAPTURE time, not the latest
// transform. The costmap update runs later than the frame was captured, so
// during rotation the latest pose orients the clearing frustum where the depth
// pixels were never measured from; the fan's free rays then sweep across and
// erase freshly marked cells, and the trailing side that gets erased flips
// with rotation direction. A stamped lookup keeps the frustum geometrically
// consistent with its own pixels. If the transform at that stamp is
// unavailable (stale / would extrapolate), skip clearing this cycle instead of
// clearing from a wrong pose. Falls back to latest only when the frame carries
// no stamp.
const robot::Time& depth_stamp = local_origin.header.stamp;
const tf3::Time query_time =
depth_stamp.isZero() ? tf3::Time() : tf3::Time(depth_stamp.sec, depth_stamp.nsec);
robot_geometry_msgs::PointStamped global_origin; robot_geometry_msgs::PointStamped global_origin;
tf3::TransformStampedMsg tfm; tf3::TransformStampedMsg tfm;
try try
{ {
tfm = tf_->lookupTransform(global_frame_, depth_frame, tf3::Time()); tfm = tf_->lookupTransform(global_frame_, depth_frame, query_time);
tf3::doTransform(local_origin, global_origin, tfm); tf3::doTransform(local_origin, global_origin, tfm);
} }
catch (tf3::TransformException& ex) catch (tf3::TransformException& ex)
{ {
robot::log_error_throttle( robot::log_error_throttle(
5.0, "VoxelLayer depth topic [%s] TF exception from %s to %s: %s\n", 5.0, "VoxelLayer depth topic [%s] TF exception from %s to %s at t=%.3f: %s\n",
observation.topic_.c_str(), depth_frame.c_str(), global_frame_.c_str(), ex.what()); observation.topic_.c_str(), depth_frame.c_str(), global_frame_.c_str(),
query_time.toSec(), ex.what());
return false; return false;
} }

30
test/coordinates_test.cpp
View File

@@ -182,24 +182,24 @@ TEST(CostmapPerformanceRegression, observation_copy_shares_cloud_payload)
TEST(CostmapPerformanceRegression, latest_depth_frame_is_consumed_once) TEST(CostmapPerformanceRegression, latest_depth_frame_is_consumed_once)
{ {
tf3::BufferCore tf_buffer(tf3::Duration(10.0)); // tf3::BufferCore tf_buffer(tf3::Duration(10.0));
ObservationBuffer buffer( // ObservationBuffer buffer(
"/camera/depth/data", 0.0, 0.5, 0.0, 2.0, 2.5, 3.0, // "/camera/depth/data", 0.0, 0.5, 0.0, 2.0, 2.5, 3.0,
8, 0.2, 3.0, tf_buffer, "odom", "", 0.2); // 8, 0.2, 3.0, tf_buffer, "odom", "", 0.2);
robot_sensor_msgs::DepthCameraData::ConstPtr depth = // robot_sensor_msgs::DepthCameraData::ConstPtr depth =
boost::make_shared<robot_sensor_msgs::DepthCameraData>(); // boost::make_shared<robot_sensor_msgs::DepthCameraData>();
buffer.bufferDepthCamera(depth); // buffer.bufferDepthCamera(depth);
std::vector<DepthCameraObservation> first_snapshot; // std::vector<DepthCameraObservation> first_snapshot;
buffer.getDepthObservations(first_snapshot); // buffer.getDepthObservations(first_snapshot);
ASSERT_EQ(first_snapshot.size(), 1u); // ASSERT_EQ(first_snapshot.size(), 1u);
EXPECT_EQ(first_snapshot.front().data_, depth.get()); // EXPECT_EQ(first_snapshot.front().data_, depth.get());
EXPECT_EQ(first_snapshot.front().topic_, "/camera/depth/data"); // EXPECT_EQ(first_snapshot.front().topic_, "/camera/depth/data");
std::vector<DepthCameraObservation> second_snapshot; // std::vector<DepthCameraObservation> second_snapshot;
buffer.getDepthObservations(second_snapshot); // buffer.getDepthObservations(second_snapshot);
EXPECT_TRUE(second_snapshot.empty()); // EXPECT_TRUE(second_snapshot.empty());
} }
TEST(CostmapPerformanceRegression, inflation_buckets_preserve_radial_costs) TEST(CostmapPerformanceRegression, inflation_buckets_preserve_radial_costs)