otimal deep coppy obj

test/coordinates_test.cpp

@@ -36,6 +36,14 @@
 
 #include <gtest/gtest.h>
 #include <robot_costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/cost_values.h>
+#include <robot_costmap_2d/inflation_layer.h>
+#include <robot_costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/observation_buffer.h>
+#include <robot_costmap_2d/voxel_layer.h>
+
+#include <boost/make_shared.hpp>
+#include <cstdlib>
 
 using namespace robot_costmap_2d;
 
@@ -124,9 +132,100 @@ TEST(CostmapCoordinates, hard_coordinates_test)
   EXPECT_EQ(my, 2);
 }
 
+TEST(CostmapPerformanceRegression, rolling_origin_preserves_overlap)
+{
+  Costmap2D costmap(4, 3, 1.0, 0.0, 0.0, FREE_SPACE);
+  costmap.setCost(1, 1, LETHAL_OBSTACLE);
+  costmap.setCost(3, 2, INSCRIBED_INFLATED_OBSTACLE);
+
+  costmap.updateOrigin(0.25, 0.25);
+  EXPECT_DOUBLE_EQ(costmap.getOriginX(), 0.0);
+  EXPECT_DOUBLE_EQ(costmap.getOriginY(), 0.0);
+  EXPECT_EQ(costmap.getCost(1, 1), LETHAL_OBSTACLE);
+
+  costmap.updateOrigin(1.0, 0.0);
+  EXPECT_DOUBLE_EQ(costmap.getOriginX(), 1.0);
+  EXPECT_EQ(costmap.getCost(0, 1), LETHAL_OBSTACLE);
+  EXPECT_EQ(costmap.getCost(3, 2), FREE_SPACE);
+}
+
+TEST(CostmapPerformanceRegression, voxel_origin_subcell_shift_is_noop)
+{
+  VoxelLayer layer;
+  layer.resizeMap(4, 3, 1.0, 0.0, 0.0);
+  layer.setCost(1, 1, LETHAL_OBSTACLE);
+
+  layer.updateOrigin(0.25, 0.25);
+
+  EXPECT_DOUBLE_EQ(layer.getOriginX(), 0.0);
+  EXPECT_DOUBLE_EQ(layer.getOriginY(), 0.0);
+  EXPECT_EQ(layer.getCost(1, 1), LETHAL_OBSTACLE);
+}
+
+TEST(CostmapPerformanceRegression, observation_copy_shares_cloud_payload)
+{
+  robot_geometry_msgs::Point origin;
+  robot_sensor_msgs::PointCloud2 cloud;
+  cloud.height = 1;
+  cloud.width = 1;
+  cloud.point_step = 4;
+  cloud.row_step = 4;
+  cloud.data = {1, 2, 3, 4};
+
+  Observation observation(origin, cloud, 2.5, 3.0);
+  Observation copied = observation;
+
+  EXPECT_EQ(copied.cloud_, observation.cloud_);
+  EXPECT_EQ(copied.cloud_handle_.use_count(), 2);
+  EXPECT_EQ(copied.cloud_->data, cloud.data);
+}
+
+TEST(CostmapPerformanceRegression, latest_depth_frame_is_consumed_once)
+{
+  tf3::BufferCore tf_buffer(tf3::Duration(10.0));
+  ObservationBuffer buffer(
+      "/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);
+
+  robot_sensor_msgs::DepthCameraData::ConstPtr depth =
+      boost::make_shared<robot_sensor_msgs::DepthCameraData>();
+  buffer.bufferDepthCamera(depth);
+
+  std::vector<DepthCameraObservation> first_snapshot;
+  buffer.getDepthObservations(first_snapshot);
+  ASSERT_EQ(first_snapshot.size(), 1u);
+  EXPECT_EQ(first_snapshot.front().data_, depth.get());
+  EXPECT_EQ(first_snapshot.front().topic_, "/camera/depth/data");
+
+  std::vector<DepthCameraObservation> second_snapshot;
+  buffer.getDepthObservations(second_snapshot);
+  EXPECT_TRUE(second_snapshot.empty());
+}
+
+TEST(CostmapPerformanceRegression, inflation_buckets_preserve_radial_costs)
+{
+  ASSERT_EQ(setenv("PNKX_NAV_CORE_CONFIG_DIR", ROBOT_COSTMAP_2D_DIR, 1), 0);
+
+  LayeredCostmap layered_costmap("map", false, false);
+  layered_costmap.resizeMap(7, 7, 1.0, 0.0, 0.0, true);
+  tf3::BufferCore tf_buffer(tf3::Duration(10.0));
+  InflationLayer inflation;
+  inflation.initialize(&layered_costmap, "inflation", &tf_buffer);
+  inflation.setInflationParameters(2.0, 1.0);
+
+  Costmap2D& master = *layered_costmap.getCostmap();
+  master.setCost(3, 3, LETHAL_OBSTACLE);
+  inflation.updateCosts(master, 0, 0, 7, 7);
+
+  EXPECT_EQ(master.getCost(3, 3), LETHAL_OBSTACLE);
+  EXPECT_EQ(master.getCost(2, 3), master.getCost(4, 3));
+  EXPECT_EQ(master.getCost(3, 2), master.getCost(3, 4));
+  EXPECT_GT(master.getCost(4, 3), master.getCost(5, 3));
+  EXPECT_EQ(master.getCost(6, 3), FREE_SPACE);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest( &argc, argv );
   return RUN_ALL_TESTS();
 }
-