diff --git a/build/CMakeFiles/costmap_2d.dir/CXX.includecache b/build/CMakeFiles/costmap_2d.dir/CXX.includecache index cc41033..81711a5 100644 --- a/build/CMakeFiles/costmap_2d.dir/CXX.includecache +++ b/build/CMakeFiles/costmap_2d.dir/CXX.includecache @@ -6,55 +6,103 @@ #IncludeRegexTransform: -../include/costmap_2d/cost_values.h +../include/costmap_2d/observation.h +sensor_msgs/PointCloud2.h +- +geometry_msgs/Point.h +- -../include/costmap_2d/costmap_2d.h +../include/costmap_2d/observation_buffer.h vector - -queue -- -boost/thread.hpp -- -costmap_2d/msg.h -- - -../include/costmap_2d/costmap_layer.h -costmap_2d/layer.h -- -costmap_2d/layered_costmap.h -- - -../include/costmap_2d/layer.h -costmap_2d/costmap_2d.h -- -costmap_2d/layered_costmap.h -- -string -- -tf2/buffer_core.h -../include/costmap_2d/tf2/buffer_core.h - -../include/costmap_2d/layered_costmap.h -costmap_2d/cost_values.h -- -costmap_2d/layer.h -- -costmap_2d/costmap_2d.h -- -vector -- -string -- - -../include/costmap_2d/msg.h -vector +list - string - chrono - - -/home/duongtd/robotics_core/costmap_2d/src/costmap_layer.cpp -costmap_2d/costmap_layer.h +costmap_2d/observation.h +- +tf2/buffer_core.h +- +sensor_msgs/PointCloud2.h +- +geometry_msgs/PointStamped.h +- +geometry_msgs/Point.h +- +boost/thread.hpp +- + +/home/duongtd/robotics_core/costmap_2d/src/observation_buffer.cpp +costmap_2d/observation_buffer.h +- +tf2/convert.h +- +sensor_msgs/point_cloud2_iterator.h +- +cstdint +- + +/home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h +cmath +- +iostream +- + +/home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PointStamped.h +std_msgs/Header.h +/home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/std_msgs/Header.h +geometry_msgs/Point.h +/home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/geometry_msgs/Point.h + +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointCloud2.h +cstdint +- +string +- +vector +- +std_msgs/Header.h +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/std_msgs/Header.h +sensor_msgs/PointField.h +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/sensor_msgs/PointField.h + +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointField.h +cstdint +- +string +- + +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h +sensor_msgs/PointCloud2.h +- +cstdarg +- +sstream +- +string +- +vector +- + +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h +sensor_msgs/PointCloud2.h +- +cstdarg +- +string +- +vector +- +sensor_msgs/impl/point_cloud2_iterator.h +- + +/home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h +string +- +chrono +- +cstdint - diff --git a/build/CMakeFiles/costmap_2d.dir/depend.internal b/build/CMakeFiles/costmap_2d.dir/depend.internal index 7ed9eb3..9f3fb55 100644 --- a/build/CMakeFiles/costmap_2d.dir/depend.internal +++ b/build/CMakeFiles/costmap_2d.dir/depend.internal @@ -5,50 +5,57 @@ CMakeFiles/costmap_2d.dir/src/array_parser.cpp.o /home/duongtd/robotics_core/costmap_2d/src/array_parser.cpp CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o ../include/costmap_2d/costmap_2d.h - ../include/costmap_2d/msg.h /home/duongtd/robotics_core/costmap_2d/src/costmap_2d.cpp + /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o ../include/costmap_2d/cost_values.h ../include/costmap_2d/costmap_2d.h ../include/costmap_2d/costmap_layer.h ../include/costmap_2d/layer.h ../include/costmap_2d/layered_costmap.h - ../include/costmap_2d/msg.h /home/duongtd/robotics_core/costmap_2d/src/costmap_layer.cpp + /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o ../include/costmap_2d/costmap_math.h - ../include/costmap_2d/msg.h /home/duongtd/robotics_core/costmap_2d/src/costmap_math.cpp /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/footprint.cpp.o ../include/costmap_2d/array_parser.h ../include/costmap_2d/costmap_math.h ../include/costmap_2d/footprint.h - ../include/costmap_2d/msg.h /home/duongtd/robotics_core/costmap_2d/src/footprint.cpp + /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h + /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point32.h + /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Polygon.h + /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PolygonStamped.h + /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h CMakeFiles/costmap_2d.dir/src/layer.cpp.o ../include/costmap_2d/cost_values.h ../include/costmap_2d/costmap_2d.h ../include/costmap_2d/layer.h ../include/costmap_2d/layered_costmap.h - ../include/costmap_2d/msg.h /home/duongtd/robotics_core/costmap_2d/src/layer.cpp + /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o ../include/costmap_2d/cost_values.h ../include/costmap_2d/costmap_2d.h ../include/costmap_2d/footprint.h ../include/costmap_2d/layer.h ../include/costmap_2d/layered_costmap.h - ../include/costmap_2d/msg.h /home/duongtd/robotics_core/costmap_2d/src/layered_costmap.cpp + /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h + /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point32.h + /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Polygon.h + /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PolygonStamped.h + /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o ../include/costmap_2d/observation.h ../include/costmap_2d/observation_buffer.h /home/duongtd/robotics_core/costmap_2d/src/observation_buffer.cpp /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PointStamped.h - /home/duongtd/robotics_core/sensor_msgs/include/msg/PointCloud2.h - /home/duongtd/robotics_core/sensor_msgs/include/msg/PointField.h + /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointCloud2.h + /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointField.h /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h - /home/duongtd/robotics_core/std_msgs/include/msg/Header.h + /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h diff --git a/build/CMakeFiles/costmap_2d.dir/depend.make b/build/CMakeFiles/costmap_2d.dir/depend.make index 6ca0626..04a1640 100644 --- a/build/CMakeFiles/costmap_2d.dir/depend.make +++ b/build/CMakeFiles/costmap_2d.dir/depend.make @@ -4,51 +4,58 @@ CMakeFiles/costmap_2d.dir/src/array_parser.cpp.o: ../src/array_parser.cpp CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o: ../include/costmap_2d/costmap_2d.h -CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o: ../include/costmap_2d/msg.h CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o: ../src/costmap_2d.cpp +CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../include/costmap_2d/cost_values.h CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../include/costmap_2d/costmap_2d.h CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../include/costmap_2d/costmap_layer.h CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../include/costmap_2d/layer.h CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../include/costmap_2d/layered_costmap.h -CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../include/costmap_2d/msg.h CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: ../src/costmap_layer.cpp +CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o: ../include/costmap_2d/costmap_math.h -CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o: ../include/costmap_2d/msg.h CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o: ../src/costmap_math.cpp CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: ../include/costmap_2d/array_parser.h CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: ../include/costmap_2d/costmap_math.h CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: ../include/costmap_2d/footprint.h -CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: ../include/costmap_2d/msg.h CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: ../src/footprint.cpp +CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h +CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point32.h +CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Polygon.h +CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PolygonStamped.h +CMakeFiles/costmap_2d.dir/src/footprint.cpp.o: /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h CMakeFiles/costmap_2d.dir/src/layer.cpp.o: ../include/costmap_2d/cost_values.h CMakeFiles/costmap_2d.dir/src/layer.cpp.o: ../include/costmap_2d/costmap_2d.h CMakeFiles/costmap_2d.dir/src/layer.cpp.o: ../include/costmap_2d/layer.h CMakeFiles/costmap_2d.dir/src/layer.cpp.o: ../include/costmap_2d/layered_costmap.h -CMakeFiles/costmap_2d.dir/src/layer.cpp.o: ../include/costmap_2d/msg.h CMakeFiles/costmap_2d.dir/src/layer.cpp.o: ../src/layer.cpp +CMakeFiles/costmap_2d.dir/src/layer.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/cost_values.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/costmap_2d.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/footprint.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/layer.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/layered_costmap.h -CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../include/costmap_2d/msg.h CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: ../src/layered_costmap.cpp +CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h +CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point32.h +CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Polygon.h +CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PolygonStamped.h +CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o: /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: ../include/costmap_2d/observation.h CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: ../include/costmap_2d/observation_buffer.h CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: ../src/observation_buffer.cpp CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/Point.h CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/geometry_msgs/include/geometry_msgs/PointStamped.h -CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/PointCloud2.h -CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/PointField.h +CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointCloud2.h +CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointField.h CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h -CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/std_msgs/include/msg/Header.h +CMakeFiles/costmap_2d.dir/src/observation_buffer.cpp.o: /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h diff --git a/build/CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o b/build/CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o index 1156f3c..9336b19 100644 Binary files a/build/CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o and b/build/CMakeFiles/costmap_2d.dir/src/costmap_2d.cpp.o differ diff --git a/build/CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o b/build/CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o index 27c21e6..226fe67 100644 Binary files a/build/CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o and b/build/CMakeFiles/costmap_2d.dir/src/costmap_layer.cpp.o differ diff --git a/build/CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o b/build/CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o index f7e14ba..dba4ce4 100644 Binary files a/build/CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o and b/build/CMakeFiles/costmap_2d.dir/src/costmap_math.cpp.o differ diff --git a/build/CMakeFiles/costmap_2d.dir/src/footprint.cpp.o b/build/CMakeFiles/costmap_2d.dir/src/footprint.cpp.o index ccb5104..5de16a0 100644 Binary files a/build/CMakeFiles/costmap_2d.dir/src/footprint.cpp.o and b/build/CMakeFiles/costmap_2d.dir/src/footprint.cpp.o differ diff --git a/build/CMakeFiles/costmap_2d.dir/src/layer.cpp.o b/build/CMakeFiles/costmap_2d.dir/src/layer.cpp.o index 75b428d..4693ff1 100644 Binary files a/build/CMakeFiles/costmap_2d.dir/src/layer.cpp.o and b/build/CMakeFiles/costmap_2d.dir/src/layer.cpp.o differ diff --git a/build/CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o b/build/CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o index d0463f1..11670f9 100644 Binary files a/build/CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o and b/build/CMakeFiles/costmap_2d.dir/src/layered_costmap.cpp.o differ diff --git a/build/CMakeFiles/layers.dir/CXX.includecache b/build/CMakeFiles/layers.dir/CXX.includecache index 8e54ca9..cee5a8c 100644 --- a/build/CMakeFiles/layers.dir/CXX.includecache +++ b/build/CMakeFiles/layers.dir/CXX.includecache @@ -6,3 +6,5 @@ #IncludeRegexTransform: +/home/duongtd/robotics_core/costmap_2d/plugins/inflation_layer.cpp + diff --git a/build/libcostmap_2d.a b/build/libcostmap_2d.a index 9251447..6fa58eb 100644 Binary files a/build/libcostmap_2d.a and b/build/libcostmap_2d.a differ diff --git a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/CXX.includecache b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/CXX.includecache index adbfce0..2b5b9a7 100644 --- a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/CXX.includecache +++ b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/CXX.includecache @@ -6,9 +6,9 @@ #IncludeRegexTransform: -/home/duongtd/robotics_core/sensor_msgs/include/msg/BatteryState.h -msg/Header.h -/home/duongtd/robotics_core/sensor_msgs/include/msg/msg/Header.h +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/BatteryState.h +std_msgs/Header.h +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/std_msgs/Header.h cstdint - string @@ -18,45 +18,45 @@ vector limits - -/home/duongtd/robotics_core/sensor_msgs/include/msg/JoyFeedback.h +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/JoyFeedback.h cstdint - -/home/duongtd/robotics_core/sensor_msgs/include/msg/JoyFeedbackArray.h +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/JoyFeedbackArray.h vector - -msg/JoyFeedback.h -/home/duongtd/robotics_core/sensor_msgs/include/msg/msg/JoyFeedback.h +sensor_msgs/JoyFeedback.h +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/sensor_msgs/JoyFeedback.h -/home/duongtd/robotics_core/sensor_msgs/include/msg/PointCloud2.h +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointCloud2.h cstdint - string - vector - -msg/Header.h -/home/duongtd/robotics_core/sensor_msgs/include/msg/msg/Header.h -msg/PointField.h -/home/duongtd/robotics_core/sensor_msgs/include/msg/msg/PointField.h +std_msgs/Header.h +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/std_msgs/Header.h +sensor_msgs/PointField.h +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/sensor_msgs/PointField.h -/home/duongtd/robotics_core/sensor_msgs/include/msg/PointField.h +/home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointField.h cstdint - string - /home/duongtd/robotics_core/sensor_msgs/test/main.cpp -msg/BatteryState.h -/home/duongtd/robotics_core/sensor_msgs/test/msg/BatteryState.h -msg/JoyFeedbackArray.h -/home/duongtd/robotics_core/sensor_msgs/test/msg/JoyFeedbackArray.h -msg/PointCloud2.h -/home/duongtd/robotics_core/sensor_msgs/test/msg/PointCloud2.h +sensor_msgs/BatteryState.h +/home/duongtd/robotics_core/sensor_msgs/test/sensor_msgs/BatteryState.h +sensor_msgs/JoyFeedbackArray.h +/home/duongtd/robotics_core/sensor_msgs/test/sensor_msgs/JoyFeedbackArray.h +sensor_msgs/PointCloud2.h +/home/duongtd/robotics_core/sensor_msgs/test/sensor_msgs/PointCloud2.h iostream - -/home/duongtd/robotics_core/std_msgs/include/msg/Header.h +/home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h string - chrono diff --git a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.internal b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.internal index 17307a5..218ff48 100644 --- a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.internal +++ b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.internal @@ -2,10 +2,10 @@ # Generated by "Unix Makefiles" Generator, CMake Version 3.16 sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o - /home/duongtd/robotics_core/sensor_msgs/include/msg/BatteryState.h - /home/duongtd/robotics_core/sensor_msgs/include/msg/JoyFeedback.h - /home/duongtd/robotics_core/sensor_msgs/include/msg/JoyFeedbackArray.h - /home/duongtd/robotics_core/sensor_msgs/include/msg/PointCloud2.h - /home/duongtd/robotics_core/sensor_msgs/include/msg/PointField.h + /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/BatteryState.h + /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/JoyFeedback.h + /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/JoyFeedbackArray.h + /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointCloud2.h + /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointField.h /home/duongtd/robotics_core/sensor_msgs/test/main.cpp - /home/duongtd/robotics_core/std_msgs/include/msg/Header.h + /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h diff --git a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.make b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.make index 8e4e740..f9f4c96 100644 --- a/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.make +++ b/build/sensor_msgs_build/CMakeFiles/test_battery_state.dir/depend.make @@ -1,11 +1,11 @@ # CMAKE generated file: DO NOT EDIT! # Generated by "Unix Makefiles" Generator, CMake Version 3.16 -sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/BatteryState.h -sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/JoyFeedback.h -sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/JoyFeedbackArray.h -sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/PointCloud2.h -sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/msg/PointField.h +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/BatteryState.h +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/JoyFeedback.h +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/JoyFeedbackArray.h +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointCloud2.h +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/include/sensor_msgs/PointField.h sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/sensor_msgs/test/main.cpp -sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/std_msgs/include/msg/Header.h +sensor_msgs_build/CMakeFiles/test_battery_state.dir/test/main.cpp.o: /home/duongtd/robotics_core/std_msgs/include/std_msgs/Header.h diff --git a/include/costmap_2d/costmap_2d.h b/include/costmap_2d/costmap_2d.h index 2849ace..2a6442f 100644 --- a/include/costmap_2d/costmap_2d.h +++ b/include/costmap_2d/costmap_2d.h @@ -4,7 +4,7 @@ #include // Dùng cho std::vector #include // Dùng trong các thuật toán quét ô (flood fill, BFS) #include // Dùng mutex để đồng bộ truy cập giữa các thread (multi-thread safe) -#include +#include namespace costmap_2d // Mọi thứ nằm trong namespace costmap_2d { @@ -169,7 +169,7 @@ public: * Đặt cost cho một vùng đa giác lồi (convex polygon) * Dùng để đánh dấu vùng chướng ngại vật hoặc vùng forbidden. */ - bool setConvexPolygonCost(const std::vector& polygon, unsigned char cost_value); + bool setConvexPolygonCost(const std::vector& polygon, unsigned char cost_value); /** * Lấy danh sách các ô nằm trên viền (outline) của một polygon. diff --git a/include/costmap_2d/costmap_math.h b/include/costmap_2d/costmap_math.h index 46872b3..71fe96c 100644 --- a/include/costmap_2d/costmap_math.h +++ b/include/costmap_2d/costmap_math.h @@ -41,7 +41,7 @@ #include #include #include -#include +#include /** @brief Return -1 if x < 0, +1 otherwise. */ inline double sign(double x) @@ -62,8 +62,8 @@ inline double distance(double x0, double y0, double x1, double y1) double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1); -bool intersects(std::vector& polygon, float testx, float testy); +bool intersects(std::vector& polygon, float testx, float testy); -bool intersects(std::vector& polygon1, std::vector& polygon2); +bool intersects(std::vector& polygon1, std::vector& polygon2); #endif // COSTMAP_2D_COSTMAP_MATH_H_ diff --git a/include/costmap_2d/footprint.h b/include/costmap_2d/footprint.h index 8fce107..b57d0cc 100644 --- a/include/costmap_2d/footprint.h +++ b/include/costmap_2d/footprint.h @@ -37,8 +37,11 @@ *********************************************************************/ #ifndef COSTMAP_2D_FOOTPRINT_H #define COSTMAP_2D_FOOTPRINT_H +#include +#include +#include -#include +// #include #include #include @@ -54,28 +57,28 @@ namespace costmap_2d * @param min_dist Output parameter of the minimum distance * @param max_dist Output parameter of the maximum distance */ -void calculateMinAndMaxDistances(const std::vector& footprint, +void calculateMinAndMaxDistances(const std::vector& footprint, double& min_dist, double& max_dist); /** - * @brief Convert Point32 to Point + * @brief Convert geometry_msgs::Point32 to geometry_msgs::Point */ -Point toPoint(Point32 pt); +geometry_msgs::Point toPoint(geometry_msgs::Point32 pt); /** - * @brief Convert Point to Point32 + * @brief Convert geometry_msgs::Point to geometry_msgs::Point32 */ -Point32 toPoint32(Point pt); +geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt); /** - * @brief Convert vector of Points to Polygon msg + * @brief Convert vector of Points to geometry_msgs::Polygon msg */ -Polygon toPolygon(std::vector pts); +geometry_msgs::Polygon toPolygon(std::vector pts); /** - * @brief Convert Polygon msg to vector of Points. + * @brief Convert geometry_msgs::Polygon msg to vector of Points. */ -std::vector toPointVector(Polygon polygon); +std::vector toPointVector(geometry_msgs::Polygon polygon); /** * @brief Given a pose and base footprint, build the oriented footprint of the robot (list of Points) @@ -85,29 +88,29 @@ std::vector toPointVector(Polygon polygon); * @param footprint_spec Basic shape of the footprint * @param oriented_footprint Will be filled with the points in the oriented footprint of the robot */ -void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, - std::vector& oriented_footprint); +void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + std::vector& oriented_footprint); /** - * @brief Given a pose and base footprint, build the oriented footprint of the robot (PolygonStamped) + * @brief Given a pose and base footprint, build the oriented footprint of the robot (geometry_msgs::PolygonStamped) * @param x The x position of the robot * @param y The y position of the robot * @param theta The orientation of the robot * @param footprint_spec Basic shape of the footprint * @param oriented_footprint Will be filled with the points in the oriented footprint of the robot */ -void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, - PolygonStamped & oriented_footprint); +void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + geometry_msgs::PolygonStamped & oriented_footprint); /** * @brief Adds the specified amount of padding to the footprint (in place) */ -void padFootprint(std::vector& footprint, double padding); +void padFootprint(std::vector& footprint, double padding); /** * @brief Create a circular footprint from a given radius */ -std::vector makeFootprintFromRadius(double radius); +std::vector makeFootprintFromRadius(double radius); /** * @brief Make the footprint from the given string. @@ -115,32 +118,47 @@ std::vector makeFootprintFromRadius(double radius); * Format should be bracketed array of arrays of floats, like so: [[1.0, 2.2], [3.3, 4.2], ...] * */ -bool makeFootprintFromString(const std::string& footprint_string, std::vector& footprint); +bool makeFootprintFromString(const std::string& footprint_string, std::vector& footprint); -/** - * @brief Read the ros-params "footprint" and/or "robot_radius" from - * the given NodeHandle using searchParam() to go up the tree. - */ -std::vector makeFootprintFromParams(std::string& nh); -/** - * @brief Create the footprint from the given XmlRpcValue. - * - * @param footprint_xmlrpc should be an array of arrays, where the - * top-level array should have 3 or more elements, and the - * sub-arrays should all have exactly 2 elements (x and y - * coordinates). - * - * @param full_param_name this is the full name of the rosparam from - * which the footprint_xmlrpc value came. It is used only for - * reporting errors. */ -std::vector makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc, - const std::string& full_param_name); -/** @brief Write the current unpadded_footprint_ to the "footprint" - * parameter of the given NodeHandle so that dynamic_reconfigure - * will see the new value. */ -void writeFootprintToParam(std::string& nh, const std::vector& footprint); +////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////// +//////////////////CẦN THƯ VIỆN XmlRpcValue//////////////////// +////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////// +//#include +// https://github.com/ros/ros_comm/tree/d54e9be3421af71b70fc6b60a3bf916e779b43dc/utilities/xmlrpcpp/src +////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////// + +// /** +// * @brief Read the ros-params "footprint" and/or "robot_radius" from +// * the given NodeHandle using searchParam() to go up the tree. +// */ +// std::vector makeFootprintFromParams(std::string& nh); + +// /** +// * @brief Create the footprint from the given XmlRpcValue. +// * +// * @param footprint_xmlrpc should be an array of arrays, where the +// * top-level array should have 3 or more elements, and the +// * sub-arrays should all have exactly 2 elements (x and y +// * coordinates). +// * +// * @param full_param_name this is the full name of the rosparam from +// * which the footprint_xmlrpc value came. It is used only for +// * reporting errors. */ +// std::vector makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc, +// const std::string& full_param_name); + +// /** @brief Write the current unpadded_footprint_ to the "footprint" +// * parameter of the given NodeHandle so that dynamic_reconfigure +// * will see the new value. */ +// void writeFootprintToParam(std::string& nh, const std::vector& footprint); } // end namespace costmap_2d diff --git a/include/costmap_2d/inflation_layer.h b/include/costmap_2d/inflation_layer.h index ff6c7ff..4a24d30 100644 --- a/include/costmap_2d/inflation_layer.h +++ b/include/costmap_2d/inflation_layer.h @@ -161,4 +161,107 @@ // } // namespace costmap_2d -#endif // COSTMAP_2D_INFLATION_LAYER_H_ +// #endif // COSTMAP_2D_INFLATION_LAYER_H_ + + + + + + + + + + + + + +// ///////////////////////////////////////// + + + + + + + + + +// #ifndef INFLATION_LAYER_H_ +// #define INFLATION_LAYER_H_ + +// #include "share.h" +// #include +// #include +// #include +// #include + +// /** +// * @brief Lưu thông tin 1 cell khi thực hiện inflation +// */ +// class CellData +// { +// public: +// CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) +// : index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) {} + +// unsigned int index_; +// unsigned int x_, y_; +// unsigned int src_x_, src_y_; +// }; + +// /** +// * @brief Lớp thực hiện "inflation" cho costmap (mở rộng vùng vật cản) +// */ +// class InflationLayer : public Layer +// { +// public: +// InflationLayer(); +// virtual ~InflationLayer(); + +// virtual void onInitialize(); +// virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, +// double* min_x, double* min_y, double* max_x, double* max_y); +// virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); +// virtual bool isDiscretized() { return true; } +// virtual void matchSize(); +// virtual void reset() { onInitialize(); } + +// void setInflationParameters(double inflation_radius, double cost_scaling_factor); + +// protected: +// virtual void onFootprintChanged(); + +// boost::recursive_mutex inflation_access_; + +// double resolution_; +// double inflation_radius_; +// double inscribed_radius_; +// double weight_; +// bool inflate_unknown_; + +// private: +// inline double distanceLookup(int mx, int my, int src_x, int src_y); +// inline unsigned char costLookup(int mx, int my, int src_x, int src_y); +// inline unsigned char computeCost(double distance) const; + +// void computeCaches(); +// void deleteKernels(); +// void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid); +// inline void enqueue(unsigned int index, unsigned int mx, unsigned int my, +// unsigned int src_x, unsigned int src_y); + +// unsigned int cell_inflation_radius_; +// unsigned int cached_cell_inflation_radius_; +// std::map> inflation_cells_; + +// bool* seen_; +// int seen_size_; + +// unsigned char** cached_costs_; +// double** cached_distances_; +// double last_min_x_, last_min_y_, last_max_x_, last_max_y_; + +// bool need_reinflation_; +// }; + +#endif // INFLATION_LAYER_H_ + diff --git a/include/costmap_2d/layer.h b/include/costmap_2d/layer.h index d5940ff..abff2c8 100644 --- a/include/costmap_2d/layer.h +++ b/include/costmap_2d/layer.h @@ -122,7 +122,7 @@ public: } /** @brief Convenience function for layered_costmap_->getFootprint(). */ - const std::vector& getFootprint() const; + const std::vector& getFootprint() const; /** @brief LayeredCostmap calls this whenever the footprint there * changes (via LayeredCostmap::setFootprint()). Override to be @@ -143,7 +143,7 @@ protected: std::shared_ptr *tf_; private: - std::vector footprint_spec_; + std::vector footprint_spec_; }; } // namespace costmap_2d diff --git a/include/costmap_2d/layered_costmap.h b/include/costmap_2d/layered_costmap.h index 6227106..1e8a0eb 100644 --- a/include/costmap_2d/layered_costmap.h +++ b/include/costmap_2d/layered_costmap.h @@ -148,14 +148,14 @@ public: * @brief Cập nhật footprint (hình dạng chiếm chỗ của robot). * Đồng thời tính lại bán kính bao quanh và nội tiếp. * Gọi hàm onFootprintChanged() của tất cả layer. - * @param footprint_spec: vector các điểm (geometry_msgs::Point) mô tả đa giác footprint. + * @param footprint_spec: vector các điểm (geometry_msgs::geometry_msgs::Point) mô tả đa giác footprint. */ - void setFootprint(const std::vector& footprint_spec); + void setFootprint(const std::vector& footprint_spec); /** * @brief Trả về footprint hiện tại của robot. */ - const std::vector& getFootprint() { return footprint_; } + const std::vector& getFootprint() { return footprint_; } /** * @brief Bán kính đường tròn bao ngoài (circumscribed radius): @@ -185,7 +185,7 @@ private: bool size_locked_; ///< Có cho phép thay đổi kích thước bản đồ sau này không double circumscribed_radius_, inscribed_radius_; ///< Hai bán kính của footprint robot (bao ngoài và nội tiếp) - std::vector footprint_; ///< Đa giác mô tả footprint robot + std::vector footprint_; ///< Đa giác mô tả footprint robot }; } // namespace costmap_2d diff --git a/include/costmap_2d/observation.h b/include/costmap_2d/observation.h index 19ee4a1..e6aee65 100644 --- a/include/costmap_2d/observation.h +++ b/include/costmap_2d/observation.h @@ -32,7 +32,7 @@ #ifndef COSTMAP_2D_OBSERVATION_H_ #define COSTMAP_2D_OBSERVATION_H_ -#include +#include #include namespace costmap_2d diff --git a/include/costmap_2d/observation_buffer.h b/include/costmap_2d/observation_buffer.h index 697b2e7..60ff251 100644 --- a/include/costmap_2d/observation_buffer.h +++ b/include/costmap_2d/observation_buffer.h @@ -11,7 +11,7 @@ #include #include -#include +#include #include #include diff --git a/include/costmap_2d/static_layer.h b/include/costmap_2d/static_layer.h index 893431d..da76095 100644 --- a/include/costmap_2d/static_layer.h +++ b/include/costmap_2d/static_layer.h @@ -1,40 +1,3 @@ -/********************************************************************* - * - * 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 - * David V. Lu!! - *********************************************************************/ #ifndef COSTMAP_2D_STATIC_LAYER_H_ #define COSTMAP_2D_STATIC_LAYER_H_ diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp index e69de29..538beee 100644 --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -0,0 +1,347 @@ +// #include +// #include +// #include +// #include +// #include + +// PLUGINLIB_EXPORT_CLASS(costmap_2d::InflationLayer, costmap_2d::Layer) + +// using costmap_2d::LETHAL_OBSTACLE; +// using costmap_2d::INSCRIBED_INFLATED_OBSTACLE; +// using costmap_2d::NO_INFORMATION; + +// namespace costmap_2d +// { + +// InflationLayer::InflationLayer() +// : resolution_(0) +// , inflation_radius_(0) +// , inscribed_radius_(0) +// , weight_(0) +// , inflate_unknown_(false) +// , cell_inflation_radius_(0) +// , cached_cell_inflation_radius_(0) +// , dsrv_(NULL) +// , seen_(NULL) +// , cached_costs_(NULL) +// , cached_distances_(NULL) +// , last_min_x_(-std::numeric_limits::max()) +// , last_min_y_(-std::numeric_limits::max()) +// , last_max_x_(std::numeric_limits::max()) +// , last_max_y_(std::numeric_limits::max()) +// { +// inflation_access_ = new boost::recursive_mutex(); +// } + +// void InflationLayer::onInitialize() +// { +// { +// boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); +// ros::NodeHandle nh("~/" + name_), g_nh; +// current_ = true; +// if (seen_) +// delete[] seen_; +// seen_ = NULL; +// seen_size_ = 0; +// need_reinflation_ = false; + +// dynamic_reconfigure::Server::CallbackType cb = +// [this](auto& config, auto level){ reconfigureCB(config, level); }; + +// if (dsrv_ != NULL){ +// dsrv_->clearCallback(); +// dsrv_->setCallback(cb); +// } +// else +// { +// dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~/" + name_)); +// dsrv_->setCallback(cb); +// } +// } + +// matchSize(); +// } + +// void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level) +// { +// setInflationParameters(config.inflation_radius, config.cost_scaling_factor); + +// if (enabled_ != config.enabled || inflate_unknown_ != config.inflate_unknown) { +// enabled_ = config.enabled; +// inflate_unknown_ = config.inflate_unknown; +// need_reinflation_ = true; +// } +// } + +// void InflationLayer::matchSize() +// { +// boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); +// costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap(); +// resolution_ = costmap->getResolution(); +// cell_inflation_radius_ = cellDistance(inflation_radius_); +// computeCaches(); + +// unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY(); +// if (seen_) +// delete[] seen_; +// seen_size_ = size_x * size_y; +// seen_ = new bool[seen_size_]; +// } + +// void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, +// double* min_y, double* max_x, double* max_y) +// { +// if (need_reinflation_) +// { +// last_min_x_ = *min_x; +// last_min_y_ = *min_y; +// last_max_x_ = *max_x; +// last_max_y_ = *max_y; +// // For some reason when I make these -::max() it does not +// // work with Costmap2D::worldToMapEnforceBounds(), so I'm using +// // -::max() instead. +// *min_x = -std::numeric_limits::max(); +// *min_y = -std::numeric_limits::max(); +// *max_x = std::numeric_limits::max(); +// *max_y = std::numeric_limits::max(); +// need_reinflation_ = false; +// } +// else +// { +// double tmp_min_x = last_min_x_; +// double tmp_min_y = last_min_y_; +// double tmp_max_x = last_max_x_; +// double tmp_max_y = last_max_y_; +// last_min_x_ = *min_x; +// last_min_y_ = *min_y; +// last_max_x_ = *max_x; +// last_max_y_ = *max_y; +// *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_; +// *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_; +// *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_; +// *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_; +// } +// } + +// void InflationLayer::onFootprintChanged() +// { +// inscribed_radius_ = layered_costmap_->getInscribedRadius(); +// cell_inflation_radius_ = cellDistance(inflation_radius_); +// computeCaches(); +// need_reinflation_ = true; + +// ROS_DEBUG("InflationLayer::onFootprintChanged(): num footprint points: %lu," +// " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", +// layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_); +// } + +// void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) +// { +// boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); +// if (cell_inflation_radius_ == 0) +// return; + +// // make sure the inflation list is empty at the beginning of the cycle (should always be true) +// ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation"); + +// unsigned char* master_array = master_grid.getCharMap(); +// unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); + +// if (seen_ == NULL) { +// ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL"); +// seen_size_ = size_x * size_y; +// seen_ = new bool[seen_size_]; +// } +// else if (seen_size_ != size_x * size_y) +// { +// ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong"); +// delete[] seen_; +// seen_size_ = size_x * size_y; +// seen_ = new bool[seen_size_]; +// } +// memset(seen_, false, size_x * size_y * sizeof(bool)); + +// // We need to include in the inflation cells outside the bounding +// // box min_i...max_j, by the amount cell_inflation_radius_. Cells +// // up to that distance outside the box can still influence the costs +// // stored in cells inside the box. +// min_i -= cell_inflation_radius_; +// min_j -= cell_inflation_radius_; +// max_i += cell_inflation_radius_; +// max_j += cell_inflation_radius_; + +// min_i = std::max(0, min_i); +// min_j = std::max(0, min_j); +// max_i = std::min(int(size_x), max_i); +// max_j = std::min(int(size_y), max_j); + +// // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle +// // We use a map to emulate the priority queue used before, with a notable performance boost + +// // Start with lethal obstacles: by definition distance is 0.0 +// std::vector& obs_bin = inflation_cells_[0.0]; +// for (int j = min_j; j < max_j; j++) +// { +// for (int i = min_i; i < max_i; i++) +// { +// int index = master_grid.getIndex(i, j); +// unsigned char cost = master_array[index]; +// if (cost == LETHAL_OBSTACLE) +// { +// obs_bin.push_back(CellData(index, i, j, i, j)); +// } +// } +// } + +// // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they +// // can overtake previously inserted but farther away cells +// std::map >::iterator bin; +// for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) +// { +// for (int i = 0; i < bin->second.size(); ++i) +// { +// // process all cells at distance dist_bin.first +// const CellData& cell = bin->second[i]; + +// unsigned int index = cell.index_; + +// // ignore if already visited +// if (seen_[index]) +// { +// continue; +// } + +// seen_[index] = true; + +// unsigned int mx = cell.x_; +// unsigned int my = cell.y_; +// unsigned int sx = cell.src_x_; +// unsigned int sy = cell.src_y_; + +// // assign the cost associated with the distance from an obstacle to the cell +// unsigned char cost = costLookup(mx, my, sx, sy); +// unsigned char old_cost = master_array[index]; +// if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE))) +// master_array[index] = cost; +// else +// master_array[index] = std::max(old_cost, cost); + +// // attempt to put the neighbors of the current cell onto the inflation list +// if (mx > 0) +// enqueue(index - 1, mx - 1, my, sx, sy); +// if (my > 0) +// enqueue(index - size_x, mx, my - 1, sx, sy); +// if (mx < size_x - 1) +// enqueue(index + 1, mx + 1, my, sx, sy); +// if (my < size_y - 1) +// enqueue(index + size_x, mx, my + 1, sx, sy); +// } +// } + +// inflation_cells_.clear(); +// } + +// /** +// * @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation +// * @param grid The costmap +// * @param index The index of the cell +// * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it) +// * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it) +// * @param src_x The x index of the obstacle point inflation started at +// * @param src_y The y index of the obstacle point inflation started at +// */ +// inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my, +// unsigned int src_x, unsigned int src_y) +// { +// if (!seen_[index]) +// { +// // we compute our distance table one cell further than the inflation radius dictates so we can make the check below +// double distance = distanceLookup(mx, my, src_x, src_y); + +// // we only want to put the cell in the list if it is within the inflation radius of the obstacle point +// if (distance > cell_inflation_radius_) +// return; + +// // push the cell data onto the inflation list and mark +// inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y)); +// } +// } + +// void InflationLayer::computeCaches() +// { +// if (cell_inflation_radius_ == 0) +// return; + +// // based on the inflation radius... compute distance and cost caches +// if (cell_inflation_radius_ != cached_cell_inflation_radius_) +// { +// deleteKernels(); + +// cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2]; +// cached_distances_ = new double*[cell_inflation_radius_ + 2]; + +// for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) +// { +// cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2]; +// cached_distances_[i] = new double[cell_inflation_radius_ + 2]; +// for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) +// { +// cached_distances_[i][j] = hypot(i, j); +// } +// } + +// cached_cell_inflation_radius_ = cell_inflation_radius_; +// } + +// for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) +// { +// for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) +// { +// cached_costs_[i][j] = computeCost(cached_distances_[i][j]); +// } +// } +// } + +// void InflationLayer::deleteKernels() +// { +// if (cached_distances_ != NULL) +// { +// for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) +// { +// if (cached_distances_[i]) +// delete[] cached_distances_[i]; +// } +// if (cached_distances_) +// delete[] cached_distances_; +// cached_distances_ = NULL; +// } + +// if (cached_costs_ != NULL) +// { +// for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) +// { +// if (cached_costs_[i]) +// delete[] cached_costs_[i]; +// } +// delete[] cached_costs_; +// cached_costs_ = NULL; +// } +// } + +// void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor) +// { +// if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius) +// { +// // Lock here so that reconfiguring the inflation radius doesn't cause segfaults +// // when accessing the cached arrays +// boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); + +// inflation_radius_ = inflation_radius; +// cell_inflation_radius_ = cellDistance(inflation_radius_); +// weight_ = cost_scaling_factor; +// need_reinflation_ = true; +// computeCaches(); +// } +// } + +// } // namespace costmap_2d diff --git a/src/costmap_2d.cpp b/src/costmap_2d.cpp index 3212208..7494e57 100644 --- a/src/costmap_2d.cpp +++ b/src/costmap_2d.cpp @@ -306,7 +306,7 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y) * Đặt giá trị cost cho vùng đa giác lồi * (ví dụ: để đánh dấu vùng cấm hoặc vùng obstacle) *****************************************************/ -bool Costmap2D::setConvexPolygonCost(const std::vector& polygon, unsigned char cost_value) +bool Costmap2D::setConvexPolygonCost(const std::vector& polygon, unsigned char cost_value) { std::vector map_polygon; for (unsigned int i = 0; i < polygon.size(); ++i) diff --git a/src/costmap_math.cpp b/src/costmap_math.cpp index 11c008f..97f7f50 100644 --- a/src/costmap_math.cpp +++ b/src/costmap_math.cpp @@ -28,7 +28,6 @@ */ #include -#include double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1) { diff --git a/src/footprint.cpp b/src/footprint.cpp index 4767c79..742e43c 100644 --- a/src/footprint.cpp +++ b/src/footprint.cpp @@ -27,18 +27,18 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include #include #include #include #include #include -#include +// #include namespace costmap_2d { -void calculateMinAndMaxDistances(const std::vector& footprint, double& min_dist, double& max_dist) +void calculateMinAndMaxDistances(const std::vector& footprint, double& min_dist, double& max_dist) { min_dist = std::numeric_limits::max(); max_dist = 0.0; @@ -66,36 +66,36 @@ void calculateMinAndMaxDistances(const std::vector& footprint, double& mi max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); } -Point32 toPoint32(Point pt) +geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt) { - Point32 point32; + geometry_msgs::Point32 point32; point32.x = pt.x; point32.y = pt.y; point32.z = pt.z; return point32; } -Point toPoint(Point32 pt) +geometry_msgs::Point toPoint(geometry_msgs::Point32 pt) { - Point point; + geometry_msgs::Point point; point.x = pt.x; point.y = pt.y; point.z = pt.z; return point; } -Polygon toPolygon(std::vector pts) +geometry_msgs::Polygon toPolygon(std::vector pts) { - Polygon polygon; + geometry_msgs::Polygon polygon; for (int i = 0; i < pts.size(); i++){ polygon.points.push_back(toPoint32(pts[i])); } return polygon; } -std::vector toPointVector(Polygon polygon) +std::vector toPointVector(geometry_msgs::Polygon polygon) { - std::vector pts; + std::vector pts; for (int i = 0; i < polygon.points.size(); i++) { pts.push_back(toPoint(polygon.points[i])); @@ -103,8 +103,8 @@ std::vector toPointVector(Polygon polygon) return pts; } -void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, - std::vector& oriented_footprint) +void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + std::vector& oriented_footprint) { // build the oriented footprint at a given location oriented_footprint.clear(); @@ -112,15 +112,15 @@ void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, - PolygonStamped& oriented_footprint) +void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + geometry_msgs::PolygonStamped& oriented_footprint) { // build the oriented footprint at a given location oriented_footprint.polygon.points.clear(); @@ -128,32 +128,32 @@ void transformFootprint(double x, double y, double theta, const std::vector& footprint, double padding) +void padFootprint(std::vector& footprint, double padding) { // pad footprint in place for (unsigned int i = 0; i < footprint.size(); i++) { - Point& pt = footprint[ i ]; + geometry_msgs::Point& pt = footprint[ i ]; pt.x += sign0(pt.x) * padding; pt.y += sign0(pt.y) * padding; } } -std::vector makeFootprintFromRadius(double radius) +std::vector makeFootprintFromRadius(double radius) { - std::vector points; + std::vector points; // Loop over 16 angles around a circle making a point each time int N = 16; - Point pt; + geometry_msgs::Point pt; for (int i = 0; i < N; ++i) { double angle = i * 2 * M_PI / N; @@ -167,7 +167,7 @@ std::vector makeFootprintFromRadius(double radius) } -bool makeFootprintFromString(const std::string& footprint_string, std::vector& footprint) +bool makeFootprintFromString(const std::string& footprint_string, std::vector& footprint) { std::string error; std::vector > vvf = parseVVF(footprint_string, error); @@ -190,7 +190,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector makeFootprintFromParams(ros::NodeHandle& nh) +// std::vector makeFootprintFromParams(ros::NodeHandle& nh) // { // std::string full_param_name; // std::string full_radius_param_name; -// std::vector points; +// std::vector points; // if (nh.searchParam("footprint", full_param_name)) // { @@ -249,13 +249,13 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector& footprint) +// void writeFootprintToParam(std::string& nh, const std::vector& footprint) // { // std::ostringstream oss; // bool first = true; // for (unsigned int i = 0; i < footprint.size(); i++) // { -// Point p = footprint[ i ]; +// geometry_msgs::Point p = footprint[ i ]; // if (first) // { // oss << "[[" << p.x << "," << p.y << "]"; @@ -284,7 +284,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc, +// std::vector makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc, // const std::string& full_param_name) // { // // Make sure we have an array of at least 3 elements. @@ -297,8 +297,8 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector footprint; -// Point pt; +// std::vector footprint; +// geometry_msgs::Point pt; // for (int i = 0; i < footprint_xmlrpc.size(); ++i) // { diff --git a/src/layer.cpp b/src/layer.cpp index 60e5afd..06ea46b 100644 --- a/src/layer.cpp +++ b/src/layer.cpp @@ -19,7 +19,7 @@ void Layer::initialize(LayeredCostmap* parent, std::string name, std::shared_ptr onInitialize(); } -const std::vector& Layer::getFootprint() const +const std::vector& Layer::getFootprint() const { return layered_costmap_->getFootprint(); } diff --git a/src/layered_costmap.cpp b/src/layered_costmap.cpp index ba77b33..258d30a 100644 --- a/src/layered_costmap.cpp +++ b/src/layered_costmap.cpp @@ -138,7 +138,7 @@ namespace costmap_2d return current_; } - void LayeredCostmap::setFootprint(const std::vector &footprint_spec) + void LayeredCostmap::setFootprint(const std::vector &footprint_spec) { footprint_ = footprint_spec; costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_); diff --git a/test/array_parser_test.cpp b/test/array_parser_test.cpp new file mode 100644 index 0000000..7ac2a39 --- /dev/null +++ b/test/array_parser_test.cpp @@ -0,0 +1,80 @@ +/* + * Copyright (c) 2012, 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 the 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. + */ + +#include + +#include "costmap_2d/array_parser.h" + +using namespace costmap_2d; + +TEST(array_parser, basic_operation) +{ + std::string error; + std::vector > vvf; + vvf = parseVVF( "[[1, 2.2], [.3, -4e4]]", error ); + EXPECT_EQ( 2, vvf.size() ); + EXPECT_EQ( 2, vvf[0].size() ); + EXPECT_EQ( 2, vvf[1].size() ); + EXPECT_EQ( 1.0f, vvf[0][0] ); + EXPECT_EQ( 2.2f, vvf[0][1] ); + EXPECT_EQ( 0.3f, vvf[1][0] ); + EXPECT_EQ( -40000.0f, vvf[1][1] ); + EXPECT_EQ( "", error ); +} + +TEST(array_parser, missing_open) +{ + std::string error; + std::vector > vvf; + vvf = parseVVF( "[1, 2.2], [.3, -4e4]]", error ); + EXPECT_FALSE( error == "" ); +} + +TEST(array_parser, missing_close) +{ + std::string error; + std::vector > vvf; + vvf = parseVVF( "[[1, 2.2], [.3, -4e4]", error ); + EXPECT_FALSE( error == "" ); +} + +TEST(array_parser, wrong_depth) +{ + std::string error; + std::vector > vvf; + vvf = parseVVF( "[1, 2.2], [.3, -4e4]", error ); + EXPECT_FALSE( error == "" ); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest( &argc, argv ); + return RUN_ALL_TESTS(); +} +