first commit
This commit is contained in:
parent
5d4d1943fe
commit
5d8981eab6
|
|
@ -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
|
||||
-
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -6,3 +6,5 @@
|
|||
|
||||
#IncludeRegexTransform:
|
||||
|
||||
/home/duongtd/robotics_core/costmap_2d/plugins/inflation_layer.cpp
|
||||
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
#include <vector> // Dùng cho std::vector
|
||||
#include <queue> // Dùng trong các thuật toán quét ô (flood fill, BFS)
|
||||
#include <boost/thread.hpp> // Dùng mutex để đồng bộ truy cập giữa các thread (multi-thread safe)
|
||||
#include <costmap_2d/msg.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
|
||||
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<Point>& polygon, unsigned char cost_value);
|
||||
bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);
|
||||
|
||||
/**
|
||||
* Lấy danh sách các ô nằm trên viền (outline) của một polygon.
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@
|
|||
#include <math.h>
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
#include<costmap_2d/msg.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
|
||||
/** @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<costmap_2d::Point>& polygon, float testx, float testy);
|
||||
bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy);
|
||||
|
||||
bool intersects(std::vector<costmap_2d::Point>& polygon1, std::vector<costmap_2d::Point>& polygon2);
|
||||
bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2);
|
||||
|
||||
#endif // COSTMAP_2D_COSTMAP_MATH_H_
|
||||
|
|
|
|||
|
|
@ -37,8 +37,11 @@
|
|||
*********************************************************************/
|
||||
#ifndef COSTMAP_2D_FOOTPRINT_H
|
||||
#define COSTMAP_2D_FOOTPRINT_H
|
||||
#include <geometry_msgs/Point32.h>
|
||||
#include <geometry_msgs/Polygon.h>
|
||||
#include <geometry_msgs/PolygonStamped.h>
|
||||
|
||||
#include<costmap_2d/msg.h>
|
||||
// #include<costmap_2d/msg.h>
|
||||
#include<string.h>
|
||||
#include<vector>
|
||||
|
||||
|
|
@ -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<Point>& footprint,
|
||||
void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& 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<Point> pts);
|
||||
geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts);
|
||||
|
||||
/**
|
||||
* @brief Convert Polygon msg to vector of Points.
|
||||
* @brief Convert geometry_msgs::Polygon msg to vector of Points.
|
||||
*/
|
||||
std::vector<Point> toPointVector(Polygon polygon);
|
||||
std::vector<geometry_msgs::Point> 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<Point> 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<Point>& footprint_spec,
|
||||
std::vector<Point>& oriented_footprint);
|
||||
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
|
||||
std::vector<geometry_msgs::Point>& 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<Point>& footprint_spec,
|
||||
PolygonStamped & oriented_footprint);
|
||||
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
|
||||
geometry_msgs::PolygonStamped & oriented_footprint);
|
||||
|
||||
/**
|
||||
* @brief Adds the specified amount of padding to the footprint (in place)
|
||||
*/
|
||||
void padFootprint(std::vector<Point>& footprint, double padding);
|
||||
void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding);
|
||||
|
||||
/**
|
||||
* @brief Create a circular footprint from a given radius
|
||||
*/
|
||||
std::vector<Point> makeFootprintFromRadius(double radius);
|
||||
std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius);
|
||||
|
||||
/**
|
||||
* @brief Make the footprint from the given string.
|
||||
|
|
@ -115,32 +118,47 @@ std::vector<Point> 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<Point>& footprint);
|
||||
bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint);
|
||||
|
||||
/**
|
||||
* @brief Read the ros-params "footprint" and/or "robot_radius" from
|
||||
* the given NodeHandle using searchParam() to go up the tree.
|
||||
*/
|
||||
std::vector<Point> 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<Point> 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<Point>& footprint);
|
||||
//////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////
|
||||
//////////////////CẦN THƯ VIỆN XmlRpcValue////////////////////
|
||||
//////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////
|
||||
//#include <xmlrpcpp/XmlRpcValue.h>
|
||||
// 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<geometry_msgs::Point> 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<geometry_msgs::Point> 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<geometry_msgs::Point>& footprint);
|
||||
|
||||
} // end namespace costmap_2d
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <cmath>
|
||||
// #include <map>
|
||||
// #include <vector>
|
||||
// #include <boost/thread/recursive_mutex.hpp>
|
||||
|
||||
// /**
|
||||
// * @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<double, std::vector<CellData>> 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_
|
||||
|
||||
|
|
|
|||
|
|
@ -122,7 +122,7 @@ public:
|
|||
}
|
||||
|
||||
/** @brief Convenience function for layered_costmap_->getFootprint(). */
|
||||
const std::vector<Point>& getFootprint() const;
|
||||
const std::vector<geometry_msgs::Point>& 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<tf2::BufferCore> *tf_;
|
||||
|
||||
private:
|
||||
std::vector<Point> footprint_spec_;
|
||||
std::vector<geometry_msgs::Point> footprint_spec_;
|
||||
};
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
|
|
|||
|
|
@ -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<Point>& footprint_spec);
|
||||
void setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec);
|
||||
|
||||
/**
|
||||
* @brief Trả về footprint hiện tại của robot.
|
||||
*/
|
||||
const std::vector<Point>& getFootprint() { return footprint_; }
|
||||
const std::vector<geometry_msgs::Point>& 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<Point> footprint_; ///< Đa giác mô tả footprint robot
|
||||
std::vector<geometry_msgs::Point> footprint_; ///< Đa giác mô tả footprint robot
|
||||
};
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@
|
|||
#ifndef COSTMAP_2D_OBSERVATION_H_
|
||||
#define COSTMAP_2D_OBSERVATION_H_
|
||||
|
||||
#include <msg/PointCloud2.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
|
||||
namespace costmap_2d
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
#include <costmap_2d/observation.h>
|
||||
#include <tf2/buffer_core.h>
|
||||
#include <msg/PointCloud2.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <geometry_msgs/PointStamped.h>
|
||||
#include <geometry_msgs/Point.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_
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,347 @@
|
|||
// #include <algorithm>
|
||||
// #include <costmap_2d/inflation_layer.h>
|
||||
// #include <costmap_2d/costmap_math.h>
|
||||
// #include <costmap_2d/footprint.h>
|
||||
// #include <boost/thread.hpp>
|
||||
|
||||
// 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<float>::max())
|
||||
// , last_min_y_(-std::numeric_limits<float>::max())
|
||||
// , last_max_x_(std::numeric_limits<float>::max())
|
||||
// , last_max_y_(std::numeric_limits<float>::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<costmap_2d::InflationPluginConfig>::CallbackType cb =
|
||||
// [this](auto& config, auto level){ reconfigureCB(config, level); };
|
||||
|
||||
// if (dsrv_ != NULL){
|
||||
// dsrv_->clearCallback();
|
||||
// dsrv_->setCallback(cb);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// dsrv_ = new dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>(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 -<double>::max() it does not
|
||||
// // work with Costmap2D::worldToMapEnforceBounds(), so I'm using
|
||||
// // -<float>::max() instead.
|
||||
// *min_x = -std::numeric_limits<float>::max();
|
||||
// *min_y = -std::numeric_limits<float>::max();
|
||||
// *max_x = std::numeric_limits<float>::max();
|
||||
// *max_y = std::numeric_limits<float>::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<distance, list> to emulate the priority queue used before, with a notable performance boost
|
||||
|
||||
// // Start with lethal obstacles: by definition distance is 0.0
|
||||
// std::vector<CellData>& 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<double, std::vector<CellData> >::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
|
||||
|
|
@ -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<Point>& polygon, unsigned char cost_value)
|
||||
bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
|
||||
{
|
||||
std::vector<MapLocation> map_polygon;
|
||||
for (unsigned int i = 0; i < polygon.size(); ++i)
|
||||
|
|
|
|||
|
|
@ -28,7 +28,6 @@
|
|||
*/
|
||||
|
||||
#include <costmap_2d/costmap_math.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
|
||||
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -33,12 +33,12 @@
|
|||
#include <boost/algorithm/string.hpp>
|
||||
#include <costmap_2d/footprint.h>
|
||||
#include <costmap_2d/array_parser.h>
|
||||
#include<costmap_2d/msg.h>
|
||||
// #include<costmap_2d/msg.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
void calculateMinAndMaxDistances(const std::vector<Point>& footprint, double& min_dist, double& max_dist)
|
||||
void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
|
||||
{
|
||||
min_dist = std::numeric_limits<double>::max();
|
||||
max_dist = 0.0;
|
||||
|
|
@ -66,36 +66,36 @@ void calculateMinAndMaxDistances(const std::vector<Point>& 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<Point> pts)
|
||||
geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> 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<Point> toPointVector(Polygon polygon)
|
||||
std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
|
||||
{
|
||||
std::vector<Point> pts;
|
||||
std::vector<geometry_msgs::Point> pts;
|
||||
for (int i = 0; i < polygon.points.size(); i++)
|
||||
{
|
||||
pts.push_back(toPoint(polygon.points[i]));
|
||||
|
|
@ -103,8 +103,8 @@ std::vector<Point> toPointVector(Polygon polygon)
|
|||
return pts;
|
||||
}
|
||||
|
||||
void transformFootprint(double x, double y, double theta, const std::vector<Point>& footprint_spec,
|
||||
std::vector<Point>& oriented_footprint)
|
||||
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
|
||||
std::vector<geometry_msgs::Point>& 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<Poin
|
|||
double sin_th = sin(theta);
|
||||
for (unsigned int i = 0; i < footprint_spec.size(); ++i)
|
||||
{
|
||||
Point new_pt;
|
||||
geometry_msgs::Point new_pt;
|
||||
new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
|
||||
new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
|
||||
oriented_footprint.push_back(new_pt);
|
||||
}
|
||||
}
|
||||
|
||||
void transformFootprint(double x, double y, double theta, const std::vector<Point>& footprint_spec,
|
||||
PolygonStamped& oriented_footprint)
|
||||
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& 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<Poin
|
|||
double sin_th = sin(theta);
|
||||
for (unsigned int i = 0; i < footprint_spec.size(); ++i)
|
||||
{
|
||||
Point32 new_pt;
|
||||
geometry_msgs::Point32 new_pt;
|
||||
new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
|
||||
new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
|
||||
oriented_footprint.polygon.points.push_back(new_pt);
|
||||
}
|
||||
}
|
||||
|
||||
void padFootprint(std::vector<Point>& footprint, double padding)
|
||||
void padFootprint(std::vector<geometry_msgs::Point>& 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<Point> makeFootprintFromRadius(double radius)
|
||||
std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
|
||||
{
|
||||
std::vector<Point> points;
|
||||
std::vector<geometry_msgs::Point> 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<Point> makeFootprintFromRadius(double radius)
|
|||
}
|
||||
|
||||
|
||||
bool makeFootprintFromString(const std::string& footprint_string, std::vector<Point>& footprint)
|
||||
bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint)
|
||||
{
|
||||
std::string error;
|
||||
std::vector<std::vector<float> > vvf = parseVVF(footprint_string, error);
|
||||
|
|
@ -190,7 +190,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<Po
|
|||
{
|
||||
if (vvf[ i ].size() == 2)
|
||||
{
|
||||
Point point;
|
||||
geometry_msgs::Point point;
|
||||
point.x = vvf[ i ][ 0 ];
|
||||
point.y = vvf[ i ][ 1 ];
|
||||
point.z = 0;
|
||||
|
|
@ -209,11 +209,11 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<Po
|
|||
|
||||
|
||||
|
||||
// std::vector<Point> makeFootprintFromParams(ros::NodeHandle& nh)
|
||||
// std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
|
||||
// {
|
||||
// std::string full_param_name;
|
||||
// std::string full_radius_param_name;
|
||||
// std::vector<Point> points;
|
||||
// std::vector<geometry_msgs::Point> points;
|
||||
|
||||
// if (nh.searchParam("footprint", full_param_name))
|
||||
// {
|
||||
|
|
@ -249,13 +249,13 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<Po
|
|||
// return points;
|
||||
// }
|
||||
|
||||
// void writeFootprintToParam(std::string& nh, const std::vector<Point>& footprint)
|
||||
// void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& 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<Po
|
|||
// return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
|
||||
// }
|
||||
|
||||
// std::vector<Point> makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc,
|
||||
// std::vector<geometry_msgs::Point> 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<Po
|
|||
// "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
|
||||
// }
|
||||
|
||||
// std::vector<Point> footprint;
|
||||
// Point pt;
|
||||
// std::vector<geometry_msgs::Point> footprint;
|
||||
// geometry_msgs::Point pt;
|
||||
|
||||
// for (int i = 0; i < footprint_xmlrpc.size(); ++i)
|
||||
// {
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@ void Layer::initialize(LayeredCostmap* parent, std::string name, std::shared_ptr
|
|||
onInitialize();
|
||||
}
|
||||
|
||||
const std::vector<Point>& Layer::getFootprint() const
|
||||
const std::vector<geometry_msgs::Point>& Layer::getFootprint() const
|
||||
{
|
||||
return layered_costmap_->getFootprint();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -138,7 +138,7 @@ namespace costmap_2d
|
|||
return current_;
|
||||
}
|
||||
|
||||
void LayeredCostmap::setFootprint(const std::vector<Point> &footprint_spec)
|
||||
void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::Point> &footprint_spec)
|
||||
{
|
||||
footprint_ = footprint_spec;
|
||||
costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
|
||||
|
|
|
|||
80
test/array_parser_test.cpp
Normal file
80
test/array_parser_test.cpp
Normal file
|
|
@ -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 <gtest/gtest.h>
|
||||
|
||||
#include "costmap_2d/array_parser.h"
|
||||
|
||||
using namespace costmap_2d;
|
||||
|
||||
TEST(array_parser, basic_operation)
|
||||
{
|
||||
std::string error;
|
||||
std::vector<std::vector<float> > 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<std::vector<float> > vvf;
|
||||
vvf = parseVVF( "[1, 2.2], [.3, -4e4]]", error );
|
||||
EXPECT_FALSE( error == "" );
|
||||
}
|
||||
|
||||
TEST(array_parser, missing_close)
|
||||
{
|
||||
std::string error;
|
||||
std::vector<std::vector<float> > vvf;
|
||||
vvf = parseVVF( "[[1, 2.2], [.3, -4e4]", error );
|
||||
EXPECT_FALSE( error == "" );
|
||||
}
|
||||
|
||||
TEST(array_parser, wrong_depth)
|
||||
{
|
||||
std::string error;
|
||||
std::vector<std::vector<float> > 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();
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user