Compare commits
4 Commits
084ae634d0
...
436f10896b
| Author | SHA1 | Date | |
|---|---|---|---|
| 436f10896b | |||
| 8972af238d | |||
| 21d105a0fc | |||
| 88c5e2b6fb |
BIN
lankuang.pcd
Normal file
BIN
lankuang.pcd
Normal file
Binary file not shown.
5
nav.sh
5
nav.sh
@ -11,7 +11,10 @@ commands=(
|
||||
nav_rviz:=true"
|
||||
"ros2 launch rm_simpal_move simple_move.launch.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map"
|
||||
# "/usr/bin/python3 /home/robofish/RC2025/src/rc_lidar/juxing.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rc_lidar/pcd2pgm.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_goal.py"
|
||||
|
||||
)
|
||||
|
||||
|
||||
4
nav1.sh
4
nav1.sh
@ -11,7 +11,9 @@ commands=(
|
||||
nav_rviz:=true"
|
||||
"ros2 launch rm_simpal_move simple_move.launch.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map1"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rc_lidar/pcd2pgm.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_goal.py"
|
||||
|
||||
)
|
||||
|
||||
|
||||
4
nav2.sh
4
nav2.sh
@ -11,7 +11,9 @@ commands=(
|
||||
nav_rviz:=true"
|
||||
"ros2 launch rm_simpal_move simple_move.launch.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map2"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rc_lidar/pcd2pgm.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rc_lidar/caijian.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_goal.py"
|
||||
|
||||
)
|
||||
|
||||
|
||||
@ -6,6 +6,7 @@ Panels:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /TF1/Tree1
|
||||
- /Map2
|
||||
Splitter Ratio: 0.5833333134651184
|
||||
Tree Height: 462
|
||||
- Class: rviz_common/Selection
|
||||
@ -66,19 +67,11 @@ Visualization Manager:
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
dummy:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
imu_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
livox_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheel_1:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
@ -116,9 +109,7 @@ Visualization Manager:
|
||||
Value: true
|
||||
base_link_fake:
|
||||
Value: true
|
||||
dummy:
|
||||
Value: true
|
||||
imu_link:
|
||||
goal_pose:
|
||||
Value: true
|
||||
lidar_odom:
|
||||
Value: true
|
||||
@ -143,13 +134,13 @@ Visualization Manager:
|
||||
Show Names: false
|
||||
Tree:
|
||||
map:
|
||||
goal_pose:
|
||||
{}
|
||||
odom:
|
||||
lidar_odom:
|
||||
base_link:
|
||||
base_link_fake:
|
||||
{}
|
||||
imu_link:
|
||||
{}
|
||||
livox_frame:
|
||||
{}
|
||||
wheel_1:
|
||||
@ -208,7 +199,7 @@ Visualization Manager:
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
@ -229,7 +220,7 @@ Visualization Manager:
|
||||
Value: /global_costmap/voxel_grid
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
@ -242,7 +233,7 @@ Visualization Manager:
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: ""
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
@ -263,7 +254,7 @@ Visualization Manager:
|
||||
Value: /mobile_base/sensors/bumper_pointcloud
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
Value: false
|
||||
- Alpha: 0.6000000238418579
|
||||
Class: rviz_default_plugins/Map
|
||||
Color Scheme: map
|
||||
@ -288,7 +279,7 @@ Visualization Manager:
|
||||
- Alpha: 1
|
||||
Class: nav2_rviz_plugins/ParticleCloud
|
||||
Color: 0; 180; 0
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Max Arrow Length: 0.30000001192092896
|
||||
Min Arrow Length: 0.019999999552965164
|
||||
Name: Amcl Particle Swarm
|
||||
@ -300,7 +291,7 @@ Visualization Manager:
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Best Effort
|
||||
Value: /particle_cloud
|
||||
Value: true
|
||||
Value: false
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Alpha: 0.30000001192092896
|
||||
@ -420,7 +411,7 @@ Visualization Manager:
|
||||
Reliability Policy: Reliable
|
||||
Value: /global_costmap/published_footprint
|
||||
Value: false
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Name: Global Planner
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
@ -532,10 +523,10 @@ Visualization Manager:
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Name: Controller
|
||||
- Class: rviz_default_plugins/MarkerArray
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Name: MarkerArray
|
||||
Namespaces:
|
||||
{}
|
||||
@ -545,14 +536,14 @@ Visualization Manager:
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /waypoints
|
||||
Value: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Arrow Length: 0.30000001192092896
|
||||
Axes Length: 0.30000001192092896
|
||||
Axes Radius: 0.009999999776482582
|
||||
Class: rviz_default_plugins/PoseArray
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Head Length: 0.07000000029802322
|
||||
Head Radius: 0.029999999329447746
|
||||
Name: PoseArray
|
||||
@ -566,9 +557,9 @@ Visualization Manager:
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /teb_poses
|
||||
Value: true
|
||||
Value: false
|
||||
- Class: rviz_default_plugins/Marker
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Name: Marker
|
||||
Namespaces:
|
||||
{}
|
||||
@ -579,13 +570,34 @@ Visualization Manager:
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /teb_markers
|
||||
Value: true
|
||||
Value: false
|
||||
- Class: rviz_default_plugins/Axes
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Length: 0.5
|
||||
Name: base_link_fake
|
||||
Radius: 0.07999999821186066
|
||||
Reference Frame: base_link_fake
|
||||
Value: false
|
||||
- Alpha: 0.699999988079071
|
||||
Class: rviz_default_plugins/Map
|
||||
Color Scheme: map
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /lidar_grid
|
||||
Update Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /lidar_grid_updates
|
||||
Use Timestamp: false
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
@ -625,16 +637,16 @@ Visualization Manager:
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 14.485743522644043
|
||||
Distance: 2.10038423538208
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 3.056168556213379
|
||||
Y: 0.4070683419704437
|
||||
Z: 0.4124468266963959
|
||||
X: -0.5876622200012207
|
||||
Y: 1.5225260257720947
|
||||
Z: 0.4079643487930298
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
@ -643,7 +655,7 @@ Visualization Manager:
|
||||
Pitch: 1.5697963237762451
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz_default_plugins)
|
||||
Yaw: 3.1423678398132324
|
||||
Yaw: 5.280543804168701
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
@ -661,5 +673,5 @@ Window Geometry:
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1545
|
||||
X: 1585
|
||||
Y: 228
|
||||
X: 375
|
||||
Y: 74
|
||||
BIN
output.pcd
Normal file
BIN
output.pcd
Normal file
Binary file not shown.
BIN
src/rc_lidar.zip
Normal file
BIN
src/rc_lidar.zip
Normal file
Binary file not shown.
63
src/rc_lidar/caijian.py
Normal file
63
src/rc_lidar/caijian.py
Normal file
@ -0,0 +1,63 @@
|
||||
#!/usr/bin/env python3
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import PointCloud2, PointField
|
||||
import numpy as np
|
||||
import struct
|
||||
from sklearn.cluster import DBSCAN
|
||||
|
||||
class LidarFilterNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('caijian_node')
|
||||
self.publisher_ = self.create_publisher(PointCloud2, '/livox/lidar_filtered', 10)
|
||||
self.subscription = self.create_subscription(
|
||||
PointCloud2,
|
||||
'/livox/lidar/pointcloud',
|
||||
self.filter_callback,
|
||||
10)
|
||||
self.get_logger().info('caijian_node started, numpy filtering z in [1.5,3]m, distance<=12m, remove isolated points')
|
||||
|
||||
def filter_callback(self, msg):
|
||||
num_points = msg.width * msg.height
|
||||
data = np.frombuffer(msg.data, dtype=np.uint8)
|
||||
points = np.zeros((num_points, 4), dtype=np.float32) # x, y, z, intensity
|
||||
|
||||
for i in range(num_points):
|
||||
offset = i * msg.point_step
|
||||
x = struct.unpack_from('f', data, offset)[0]
|
||||
y = struct.unpack_from('f', data, offset + 4)[0]
|
||||
z = struct.unpack_from('f', data, offset + 8)[0]
|
||||
intensity = struct.unpack_from('f', data, offset + 12)[0]
|
||||
points[i] = [x, y, z, intensity]
|
||||
|
||||
z_mask = (points[:,2] >= 1.5) & (points[:,2] <= 3.0)
|
||||
dist_mask = np.linalg.norm(points[:,:3], axis=1) <= 16.0
|
||||
mask = z_mask & dist_mask
|
||||
filtered_points = points[mask]
|
||||
|
||||
# 使用DBSCAN去除孤立点
|
||||
if filtered_points.shape[0] > 0:
|
||||
clustering = DBSCAN(eps=0.3, min_samples=5).fit(filtered_points[:,:3])
|
||||
core_mask = clustering.labels_ != -1
|
||||
filtered_points = filtered_points[core_mask]
|
||||
|
||||
fields = [
|
||||
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
|
||||
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
|
||||
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
|
||||
PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1),
|
||||
]
|
||||
filtered_points_list = filtered_points.tolist()
|
||||
import sensor_msgs_py.point_cloud2 as pc2
|
||||
filtered_msg = pc2.create_cloud(msg.header, fields, filtered_points_list)
|
||||
self.publisher_.publish(filtered_msg)
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = LidarFilterNode()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
90
src/rc_lidar/circlr.py
Normal file
90
src/rc_lidar/circlr.py
Normal file
@ -0,0 +1,90 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
from geometry_msgs.msg import PointStamped
|
||||
from sensor_msgs_py import point_cloud2 as pc2
|
||||
import numpy as np
|
||||
from sklearn.cluster import DBSCAN
|
||||
import time
|
||||
|
||||
def statistical_outlier_removal(points, k=20, std_ratio=2.0):
|
||||
from sklearn.neighbors import NearestNeighbors
|
||||
nbrs = NearestNeighbors(n_neighbors=k+1).fit(points)
|
||||
distances, _ = nbrs.kneighbors(points)
|
||||
mean_dist = np.mean(distances[:, 1:], axis=1)
|
||||
threshold = np.mean(mean_dist) + std_ratio * np.std(mean_dist)
|
||||
mask = mean_dist < threshold
|
||||
return points[mask]
|
||||
|
||||
class HoopFinder(Node):
|
||||
def __init__(self):
|
||||
super().__init__('find_hoop')
|
||||
self.sub = self.create_subscription(
|
||||
PointCloud2,
|
||||
'/livox/lidar_filtered',
|
||||
self.callback,
|
||||
10)
|
||||
self.pub = self.create_publisher(PointStamped, '/hoop_position', 10)
|
||||
self.buffer = []
|
||||
self.start_time = None
|
||||
self.hoop_history = []
|
||||
|
||||
def callback(self, msg):
|
||||
# 采集0.4秒内的点云
|
||||
if self.start_time is None:
|
||||
self.start_time = time.time()
|
||||
for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
|
||||
self.buffer.append([p[0], p[1], p[2], p[3]])
|
||||
if time.time() - self.start_time < 0.4:
|
||||
return
|
||||
|
||||
points = np.array(self.buffer)
|
||||
self.buffer = []
|
||||
self.start_time = None
|
||||
|
||||
# 高度滤波
|
||||
filtered = points[(points[:,2] > 1.0) & (points[:,2] < 3.0)]
|
||||
if len(filtered) == 0:
|
||||
return
|
||||
|
||||
# 统计离群点滤波
|
||||
filtered = statistical_outlier_removal(filtered[:,:3], k=20, std_ratio=2.0)
|
||||
|
||||
# DBSCAN聚类
|
||||
clustering = DBSCAN(eps=0.3, min_samples=10).fit(filtered)
|
||||
labels = clustering.labels_
|
||||
unique_labels = set(labels)
|
||||
hoop_pos = None
|
||||
max_cluster_size = 0
|
||||
|
||||
for label in unique_labels:
|
||||
if label == -1:
|
||||
continue
|
||||
cluster = filtered[labels == label]
|
||||
if len(cluster) > max_cluster_size:
|
||||
max_cluster_size = len(cluster)
|
||||
hoop_pos = np.mean(cluster, axis=0)
|
||||
|
||||
# 均值滤波输出
|
||||
if hoop_pos is not None:
|
||||
self.hoop_history.append(hoop_pos)
|
||||
if len(self.hoop_history) > 5:
|
||||
self.hoop_history.pop(0)
|
||||
smooth_pos = np.mean(self.hoop_history, axis=0)
|
||||
pt = PointStamped()
|
||||
pt.header = msg.header
|
||||
pt.point.x = float(smooth_pos[0])
|
||||
pt.point.y = float(smooth_pos[1])
|
||||
pt.point.z = float(smooth_pos[2])
|
||||
self.pub.publish(pt)
|
||||
self.get_logger().info(f"Hoop position (smoothed): {smooth_pos}")
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = HoopFinder()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
59
src/rc_lidar/find.py
Normal file
59
src/rc_lidar/find.py
Normal file
@ -0,0 +1,59 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
from geometry_msgs.msg import PointStamped
|
||||
from sensor_msgs_py import point_cloud2 as pc2
|
||||
import numpy as np
|
||||
from sklearn.cluster import DBSCAN
|
||||
|
||||
class HoopFinder(Node):
|
||||
def __init__(self):
|
||||
super().__init__('find_hoop')
|
||||
self.sub = self.create_subscription(
|
||||
PointCloud2,
|
||||
'/livox/lidar',
|
||||
self.callback,
|
||||
10)
|
||||
self.pub = self.create_publisher(PointStamped, '/hoop_position', 10)
|
||||
|
||||
def callback(self, msg):
|
||||
points = []
|
||||
for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
|
||||
points.append([p[0], p[1], p[2], p[3]])
|
||||
points = np.array(points)
|
||||
filtered = points[(points[:,2] > 1.0) & (points[:,2] < 3.0)]
|
||||
if len(filtered) == 0:
|
||||
return
|
||||
|
||||
clustering = DBSCAN(eps=0.3, min_samples=10).fit(filtered[:,:3])
|
||||
labels = clustering.labels_
|
||||
unique_labels = set(labels)
|
||||
hoop_pos = None
|
||||
max_cluster_size = 0
|
||||
|
||||
for label in unique_labels:
|
||||
if label == -1:
|
||||
continue
|
||||
cluster = filtered[labels == label]
|
||||
if len(cluster) > max_cluster_size:
|
||||
max_cluster_size = len(cluster)
|
||||
hoop_pos = np.mean(cluster[:,:3], axis=0)
|
||||
|
||||
if hoop_pos is not None:
|
||||
pt = PointStamped()
|
||||
pt.header = msg.header
|
||||
pt.point.x = float(hoop_pos[0])
|
||||
pt.point.y = float(hoop_pos[1])
|
||||
pt.point.z = float(hoop_pos[2])
|
||||
self.pub.publish(pt)
|
||||
self.get_logger().info(f"Hoop position: {hoop_pos}")
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = HoopFinder()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
63
src/rc_lidar/fliter.py
Normal file
63
src/rc_lidar/fliter.py
Normal file
@ -0,0 +1,63 @@
|
||||
#!/usr/bin/env python3
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import PointCloud2, PointField
|
||||
import numpy as np
|
||||
import struct
|
||||
from sklearn.cluster import DBSCAN
|
||||
|
||||
class LidarFilterNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('caijian_node')
|
||||
self.publisher_ = self.create_publisher(PointCloud2, '/livox/lidar_filtered', 10)
|
||||
self.subscription = self.create_subscription(
|
||||
PointCloud2,
|
||||
'/livox/lidar/pointcloud',
|
||||
self.filter_callback,
|
||||
10)
|
||||
self.get_logger().info('caijian_node started, numpy filtering z in [1.5,3]m, distance<=12m, remove isolated points')
|
||||
|
||||
def filter_callback(self, msg):
|
||||
num_points = msg.width * msg.height
|
||||
data = np.frombuffer(msg.data, dtype=np.uint8)
|
||||
points = np.zeros((num_points, 4), dtype=np.float32) # x, y, z, intensity
|
||||
|
||||
for i in range(num_points):
|
||||
offset = i * msg.point_step
|
||||
x = struct.unpack_from('f', data, offset)[0]
|
||||
y = struct.unpack_from('f', data, offset + 4)[0]
|
||||
z = struct.unpack_from('f', data, offset + 8)[0]
|
||||
intensity = struct.unpack_from('f', data, offset + 12)[0]
|
||||
points[i] = [x, y, z, intensity]
|
||||
|
||||
z_mask = (points[:,2] >= 1.5) & (points[:,2] <= 3.0)
|
||||
dist_mask = np.linalg.norm(points[:,:3], axis=1) <= 12.0
|
||||
mask = z_mask & dist_mask
|
||||
filtered_points = points[mask]
|
||||
|
||||
# 使用DBSCAN去除孤立点
|
||||
if filtered_points.shape[0] > 0:
|
||||
clustering = DBSCAN(eps=0.3, min_samples=5).fit(filtered_points[:,:3])
|
||||
core_mask = clustering.labels_ != -1
|
||||
filtered_points = filtered_points[core_mask]
|
||||
|
||||
fields = [
|
||||
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
|
||||
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
|
||||
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
|
||||
PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1),
|
||||
]
|
||||
filtered_points_list = filtered_points.tolist()
|
||||
import sensor_msgs_py.point_cloud2 as pc2
|
||||
filtered_msg = pc2.create_cloud(msg.header, fields, filtered_points_list)
|
||||
self.publisher_.publish(filtered_msg)
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = LidarFilterNode()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
340
src/rc_lidar/juxing.py
Normal file
340
src/rc_lidar/juxing.py
Normal file
@ -0,0 +1,340 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import PointCloud2, PointField
|
||||
import numpy as np
|
||||
import struct
|
||||
from sklearn.cluster import DBSCAN
|
||||
import cv2
|
||||
from visualization_msgs.msg import Marker
|
||||
from sklearn.linear_model import RANSACRegressor
|
||||
|
||||
|
||||
def ransac_line_3d(points, threshold=0.05, min_inliers=20):
|
||||
best_inliers = []
|
||||
best_line = None
|
||||
N = len(points)
|
||||
if N < min_inliers:
|
||||
return None, []
|
||||
for _ in range(100):
|
||||
idx = np.random.choice(N, 2, replace=False)
|
||||
p1, p2 = points[idx]
|
||||
v = p2 - p1
|
||||
v = v / np.linalg.norm(v)
|
||||
dists = np.linalg.norm(np.cross(points - p1, v), axis=1)
|
||||
inliers = np.where(dists < threshold)[0]
|
||||
if len(inliers) > len(best_inliers):
|
||||
best_inliers = inliers
|
||||
best_line = (p1, p2)
|
||||
if len(best_inliers) > N * 0.5:
|
||||
break
|
||||
return best_line, best_inliers
|
||||
|
||||
def fit_rectangle_pca(cluster):
|
||||
# 用PCA找主方向和边界点
|
||||
pts = cluster[:, :3]
|
||||
mean = np.mean(pts, axis=0)
|
||||
cov = np.cov(pts.T)
|
||||
eigvals, eigvecs = np.linalg.eigh(cov)
|
||||
order = np.argsort(eigvals)[::-1]
|
||||
main_dir = eigvecs[:, order[0]]
|
||||
second_dir = eigvecs[:, order[1]]
|
||||
# 投影到主方向和次方向
|
||||
proj_main = np.dot(pts - mean, main_dir)
|
||||
proj_second = np.dot(pts - mean, second_dir)
|
||||
# 找四个角点
|
||||
corners = []
|
||||
for xm in [np.min(proj_main), np.max(proj_main)]:
|
||||
for xs in [np.min(proj_second), np.max(proj_second)]:
|
||||
idx = np.argmin((proj_main - xm)**2 + (proj_second - xs)**2)
|
||||
corners.append(pts[idx])
|
||||
return corners
|
||||
|
||||
def rectangle_score(pts):
|
||||
# 评估4个点是否接近矩形
|
||||
d = [np.linalg.norm(pts[i] - pts[(i+1)%4]) for i in range(4)]
|
||||
diag1 = np.linalg.norm(pts[0] - pts[2])
|
||||
diag2 = np.linalg.norm(pts[1] - pts[3])
|
||||
w = max(d)
|
||||
h = min(d)
|
||||
ratio = w / h if h > 0 else 0
|
||||
ideal_ratio = 1.8 / 1.05
|
||||
score = abs(ratio - ideal_ratio) + abs(diag1 - diag2) / max(diag1, diag2)
|
||||
return score
|
||||
|
||||
def classify_lines(lines):
|
||||
# lines: 每条线是 [x, y, z, intensity]
|
||||
# 假设 lines 是8个端点，两两为一条线
|
||||
vertical_lines = []
|
||||
horizontal_lines = []
|
||||
for i in range(0, len(lines), 2):
|
||||
p1 = np.array(lines[i][:3])
|
||||
p2 = np.array(lines[i+1][:3])
|
||||
vec = p2 - p1
|
||||
length = np.linalg.norm(vec)
|
||||
# 计算与地面的夹角（假设地面为z轴为0，垂直为z方向）
|
||||
dz = abs(vec[2])
|
||||
dxy = np.linalg.norm(vec[:2])
|
||||
# 垂直线：z方向分量大，长度约1.05m
|
||||
if dz > dxy and 0.95 < length < 1.15:
|
||||
vertical_lines.append((i, i+1, length))
|
||||
# 水平线：z方向分量小，长度约1.8m
|
||||
elif dz < dxy and 1.7 < length < 1.9:
|
||||
horizontal_lines.append((i, i+1, length))
|
||||
return vertical_lines, horizontal_lines
|
||||
|
||||
def find_best_rectangle_from_lines(lines):
|
||||
vertical_lines, horizontal_lines = classify_lines(lines)
|
||||
# 只选出2条垂直线和2条水平线
|
||||
if len(vertical_lines) < 2 or len(horizontal_lines) < 2:
|
||||
return None
|
||||
# 取长度最接近目标的两条
|
||||
vertical_lines = sorted(vertical_lines, key=lambda x: abs(x[2]-1.05))[:2]
|
||||
horizontal_lines = sorted(horizontal_lines, key=lambda x: abs(x[2]-1.8))[:2]
|
||||
# 组合4个端点
|
||||
indices = [vertical_lines[0][0], vertical_lines[0][1],
|
||||
vertical_lines[1][0], vertical_lines[1][1],
|
||||
horizontal_lines[0][0], horizontal_lines[0][1],
|
||||
horizontal_lines[1][0], horizontal_lines[1][1]]
|
||||
# 去重，只保留4个顶点
|
||||
unique_indices = list(set(indices))
|
||||
if len(unique_indices) < 4:
|
||||
return None
|
||||
pts = [lines[idx] for idx in unique_indices[:4]]
|
||||
# 按矩形评分排序
|
||||
from itertools import permutations
|
||||
best_score = float('inf')
|
||||
best_rect = None
|
||||
for order in permutations(range(4)):
|
||||
ordered = [pts[i] for i in order]
|
||||
score = rectangle_score(np.array([p[:3] for p in ordered]))
|
||||
if score < best_score:
|
||||
best_score = score
|
||||
best_rect = ordered
|
||||
return best_rect
|
||||
|
||||
|
||||
class BasketballFrameDetector(Node):
|
||||
def __init__(self):
|
||||
super().__init__('basketball_frame_detector')
|
||||
self.subscription = self.create_subscription(
|
||||
PointCloud2,
|
||||
'/livox/lidar_filtered',
|
||||
self.pointcloud_callback,
|
||||
10
|
||||
)
|
||||
self.publisher = self.create_publisher(
|
||||
PointCloud2,
|
||||
'/basketball_frame_cloud',
|
||||
10
|
||||
)
|
||||
self.marker_pub = self.create_publisher(
|
||||
Marker,
|
||||
'/basketball_frame_lines',
|
||||
10
|
||||
)
|
||||
self.pointcloud_buffer = []
|
||||
self.max_buffer_size = 10 # 减少缓冲帧数，加快响应
|
||||
self.center_buffer = []
|
||||
self.center_buffer_size = 5
|
||||
|
||||
def pointcloud_callback(self, msg):
|
||||
points = self.pointcloud2_to_xyz(msg)
|
||||
if points.shape[0] == 0:
|
||||
return # 跳过空点云
|
||||
self.pointcloud_buffer.append(points)
|
||||
# 只保留非空点云
|
||||
self.pointcloud_buffer = [arr for arr in self.pointcloud_buffer if arr.shape[0] > 0]
|
||||
if len(self.pointcloud_buffer) > self.max_buffer_size:
|
||||
self.pointcloud_buffer.pop(0)
|
||||
all_points = np.vstack(self.pointcloud_buffer)
|
||||
xy_points = all_points[:, :2]
|
||||
|
||||
if len(xy_points) < 10:
|
||||
self.get_logger().info('点数太少，跳过')
|
||||
return
|
||||
clustering = DBSCAN(eps=0.3, min_samples=10).fit(xy_points)
|
||||
labels = clustering.labels_
|
||||
unique_labels = set(labels)
|
||||
found = False
|
||||
for label in unique_labels:
|
||||
if label == -1:
|
||||
continue
|
||||
cluster = all_points[labels == label]
|
||||
if len(cluster) < 30:
|
||||
continue
|
||||
min_x, min_y = np.min(cluster[:, :2], axis=0)
|
||||
max_x, max_y = np.max(cluster[:, :2], axis=0)
|
||||
width = abs(max_x - min_x)
|
||||
height = abs(max_y - min_y)
|
||||
if 1.5 < width < 2.1 and 0.7 < height < 1.3:
|
||||
cloud_msg = self.xyz_array_to_pointcloud2(cluster, msg.header)
|
||||
self.publisher.publish(cloud_msg)
|
||||
|
||||
# 用PCA直接找矩形四角
|
||||
corners = fit_rectangle_pca(cluster)
|
||||
from itertools import permutations
|
||||
best_score = float('inf')
|
||||
best_rect = None
|
||||
for order in permutations(range(4)):
|
||||
ordered = [corners[i] for i in order]
|
||||
score = rectangle_score(np.array(ordered))
|
||||
if score < best_score:
|
||||
best_score = score
|
||||
best_rect = ordered
|
||||
rect_lines = best_rect
|
||||
if rect_lines is None or len(rect_lines) < 4:
|
||||
self.get_logger().info('未找到合适矩形')
|
||||
continue
|
||||
|
||||
# 发布最新的矩形
|
||||
marker = Marker()
|
||||
marker.header = msg.header
|
||||
marker.ns = "basketball_frame"
|
||||
marker.id = 0
|
||||
marker.type = Marker.LINE_LIST
|
||||
marker.action = Marker.ADD
|
||||
marker.scale.x = 0.08
|
||||
marker.color.r = 0.0
|
||||
marker.color.g = 1.0
|
||||
marker.color.b = 0.0
|
||||
marker.color.a = 1.0
|
||||
marker.points = []
|
||||
from geometry_msgs.msg import Point
|
||||
for i in range(4):
|
||||
p1 = rect_lines[i]
|
||||
p2 = rect_lines[(i+1)%4]
|
||||
pt1 = Point(x=float(p1[0]), y=float(p1[1]), z=float(p1[2]))
|
||||
pt2 = Point(x=float(p2[0]), y=float(p2[1]), z=float(p2[2]))
|
||||
marker.points.append(pt1)
|
||||
marker.points.append(pt2)
|
||||
self.marker_pub.publish(marker)
|
||||
|
||||
# 分别发布4条最优边线
|
||||
for i in range(4):
|
||||
edge_marker = Marker()
|
||||
edge_marker.header = msg.header
|
||||
edge_marker.ns = "basketball_frame_edges"
|
||||
edge_marker.id = i
|
||||
edge_marker.type = Marker.LINE_STRIP
|
||||
edge_marker.action = Marker.ADD
|
||||
edge_marker.scale.x = 0.12
|
||||
colors = [
|
||||
(1.0, 0.0, 0.0),
|
||||
(0.0, 1.0, 0.0),
|
||||
(0.0, 0.0, 1.0),
|
||||
(1.0, 1.0, 0.0)
|
||||
]
|
||||
edge_marker.color.r = colors[i][0]
|
||||
edge_marker.color.g = colors[i][1]
|
||||
edge_marker.color.b = colors[i][2]
|
||||
edge_marker.color.a = 1.0
|
||||
pt1 = Point(x=float(rect_lines[i][0]), y=float(rect_lines[i][1]), z=float(rect_lines[i][2]))
|
||||
pt2 = Point(x=float(rect_lines[(i+1)%4][0]), y=float(rect_lines[(i+1)%4][1]), z=float(rect_lines[(i+1)%4][2]))
|
||||
edge_marker.points = [pt1, pt2]
|
||||
self.marker_pub.publish(edge_marker)
|
||||
|
||||
# 中心点滑动平均
|
||||
center = np.mean(np.array(rect_lines), axis=0)
|
||||
self.center_buffer.append(center)
|
||||
if len(self.center_buffer) > self.center_buffer_size:
|
||||
self.center_buffer.pop(0)
|
||||
stable_center = np.mean(self.center_buffer, axis=0)
|
||||
|
||||
# 发布中心点Marker
|
||||
center_marker = Marker()
|
||||
center_marker.header = msg.header
|
||||
center_marker.ns = "basketball_frame"
|
||||
center_marker.id = 1
|
||||
center_marker.type = Marker.SPHERE
|
||||
center_marker.action = Marker.ADD
|
||||
center_marker.scale.x = 0.15
|
||||
center_marker.scale.y = 0.15
|
||||
center_marker.scale.z = 0.15
|
||||
center_marker.color.r = 1.0
|
||||
center_marker.color.g = 0.0
|
||||
center_marker.color.b = 0.0
|
||||
center_marker.color.a = 1.0
|
||||
center_marker.pose.position.x = float(stable_center[0])
|
||||
center_marker.pose.position.y = float(stable_center[1])
|
||||
center_marker.pose.position.z = float(stable_center[2])
|
||||
center_marker.pose.orientation.x = 0.0
|
||||
center_marker.pose.orientation.y = 0.0
|
||||
center_marker.pose.orientation.z = 0.0
|
||||
center_marker.pose.orientation.w = 1.0
|
||||
self.marker_pub.publish(center_marker)
|
||||
|
||||
# 计算法向量（篮板主方向，PCA最小特征值方向）
|
||||
pts = np.array(rect_lines)
|
||||
mean = np.mean(pts, axis=0)
|
||||
cov = np.cov(pts.T)
|
||||
eigvals, eigvecs = np.linalg.eigh(cov)
|
||||
order = np.argsort(eigvals)[::-1]
|
||||
normal_vec = eigvecs[:, order[2]]
|
||||
normal_vec = normal_vec / np.linalg.norm(normal_vec)
|
||||
# 向内侧偏移30cm
|
||||
offset_point = stable_center + 0.3 * normal_vec
|
||||
|
||||
# 发布偏移点Marker
|
||||
offset_marker = Marker()
|
||||
offset_marker.header = msg.header
|
||||
offset_marker.ns = "basketball_frame"
|
||||
offset_marker.id = 2
|
||||
offset_marker.type = Marker.SPHERE
|
||||
offset_marker.action = Marker.ADD
|
||||
offset_marker.scale.x = 0.12
|
||||
offset_marker.scale.y = 0.12
|
||||
offset_marker.scale.z = 0.12
|
||||
offset_marker.color.r = 0.0
|
||||
offset_marker.color.g = 0.0
|
||||
offset_marker.color.b = 1.0
|
||||
offset_marker.color.a = 1.0
|
||||
offset_marker.pose.position.x = float(offset_point[0])
|
||||
offset_marker.pose.position.y = float(offset_point[1])
|
||||
offset_marker.pose.position.z = float(offset_point[2])
|
||||
offset_marker.pose.orientation.x = 0.0
|
||||
offset_marker.pose.orientation.y = 0.0
|
||||
offset_marker.pose.orientation.z = 0.0
|
||||
offset_marker.pose.orientation.w = 1.0
|
||||
self.marker_pub.publish(offset_marker)
|
||||
|
||||
found = True
|
||||
break
|
||||
if not found:
|
||||
self.get_logger().info('本帧未找到矩形')
|
||||
|
||||
def pointcloud2_to_xyz(self, cloud_msg):
|
||||
fmt = 'ffff'
|
||||
points = []
|
||||
for i in range(cloud_msg.width):
|
||||
offset = i * cloud_msg.point_step
|
||||
x, y, z, intensity = struct.unpack_from(fmt, cloud_msg.data, offset)
|
||||
points.append([x, y, z, intensity])
|
||||
return np.array(points)
|
||||
|
||||
def xyz_array_to_pointcloud2(self, points, header):
|
||||
msg = PointCloud2()
|
||||
msg.header = header
|
||||
msg.height = 1
|
||||
msg.width = len(points)
|
||||
msg.is_dense = True
|
||||
msg.is_bigendian = False
|
||||
msg.point_step = 16
|
||||
msg.row_step = msg.point_step * msg.width
|
||||
msg.fields = [
|
||||
PointField(name='x', offset=0, datatype=7, count=1),
|
||||
PointField(name='y', offset=4, datatype=7, count=1),
|
||||
PointField(name='z', offset=8, datatype=7, count=1),
|
||||
PointField(name='intensity', offset=12, datatype=7, count=1),
|
||||
]
|
||||
msg.data = b''.join([struct.pack('ffff', *p) for p in points])
|
||||
return msg
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = BasketballFrameDetector()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
0
src/rc_lidar/line.py
Normal file
0
src/rc_lidar/line.py
Normal file
75
src/rc_lidar/pcd2pgm.py
Normal file
75
src/rc_lidar/pcd2pgm.py
Normal file
@ -0,0 +1,75 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
from nav_msgs.msg import OccupancyGrid
|
||||
import numpy as np
|
||||
import struct
|
||||
import time
|
||||
|
||||
class PointCloudToGrid(Node):
|
||||
def __init__(self):
|
||||
super().__init__('pointcloud_to_grid')
|
||||
self.subscription = self.create_subscription(
|
||||
PointCloud2,
|
||||
'/livox/lidar_filtered',
|
||||
self.pointcloud_callback,
|
||||
10)
|
||||
self.publisher = self.create_publisher(OccupancyGrid, '/lidar_grid', 10)
|
||||
self.grid_size = 2000
|
||||
self.resolution = 0.02
|
||||
self.origin_x = -20.0
|
||||
self.origin_y = -20.0
|
||||
self.points_buffer = []
|
||||
self.last_header = None
|
||||
# 定时器每0.5秒触发一次
|
||||
self.timer = self.create_timer(0.5, self.publish_grid)
|
||||
|
||||
def pointcloud_callback(self, msg):
|
||||
points = self.pointcloud2_to_xyz_array(msg)
|
||||
self.points_buffer.append(points)
|
||||
self.last_header = msg.header # 保存最新header用于地图消息
|
||||
|
||||
def publish_grid(self):
|
||||
if not self.points_buffer:
|
||||
return
|
||||
# 合并0.5秒内所有点
|
||||
all_points = np.concatenate(self.points_buffer, axis=0)
|
||||
grid = np.zeros((self.grid_size, self.grid_size), dtype=np.int8)
|
||||
for x, y, z in all_points:
|
||||
if z < 2.0:
|
||||
ix = int((x - self.origin_x) / self.resolution)
|
||||
iy = int((y - self.origin_y) / self.resolution)
|
||||
if 0 <= ix < self.grid_size and 0 <= iy < self.grid_size:
|
||||
grid[iy, ix] = 100
|
||||
grid_msg = OccupancyGrid()
|
||||
if self.last_header:
|
||||
grid_msg.header = self.last_header
|
||||
grid_msg.info.resolution = self.resolution
|
||||
grid_msg.info.width = self.grid_size
|
||||
grid_msg.info.height = self.grid_size
|
||||
grid_msg.info.origin.position.x = self.origin_x
|
||||
grid_msg.info.origin.position.y = self.origin_y
|
||||
grid_msg.data = grid.flatten().tolist()
|
||||
self.publisher.publish(grid_msg)
|
||||
self.points_buffer.clear() # 清空缓存
|
||||
|
||||
def pointcloud2_to_xyz_array(self, cloud_msg):
|
||||
# 解析 PointCloud2 数据为 numpy 数组
|
||||
fmt = 'fff' # x, y, z
|
||||
point_step = cloud_msg.point_step
|
||||
data = cloud_msg.data
|
||||
points = []
|
||||
for i in range(0, len(data), point_step):
|
||||
x, y, z = struct.unpack_from(fmt, data, i)
|
||||
points.append([x, y, z])
|
||||
return np.array(points)
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = PointCloudToGrid()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
55
src/rc_lidar/save_pcd.py
Normal file
55
src/rc_lidar/save_pcd.py
Normal file
@ -0,0 +1,55 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
import numpy as np
|
||||
import open3d as o3d
|
||||
import time
|
||||
|
||||
class PointCloudSaver(Node):
|
||||
def __init__(self):
|
||||
super().__init__('pcd_saver')
|
||||
self.subscription = self.create_subscription(
|
||||
PointCloud2,
|
||||
'/livox/lidar_filtered',
|
||||
self.listener_callback,
|
||||
10)
|
||||
self.point_clouds = []
|
||||
self.start_time = time.time()
|
||||
self.timer = self.create_timer(3.0, self.save_and_exit)
|
||||
self.saving = False
|
||||
|
||||
def listener_callback(self, msg):
|
||||
if not self.saving:
|
||||
pc = self.pointcloud2_to_xyz_array(msg)
|
||||
if pc is not None:
|
||||
self.point_clouds.append(pc)
|
||||
|
||||
def pointcloud2_to_xyz_array(self, cloud_msg):
|
||||
# 仅支持 x, y, z 均为 float32 且无 padding 的点云
|
||||
import struct
|
||||
points = []
|
||||
point_step = cloud_msg.point_step
|
||||
for i in range(cloud_msg.width * cloud_msg.height):
|
||||
offset = i * point_step
|
||||
x, y, z = struct.unpack_from('fff', cloud_msg.data, offset)
|
||||
points.append([x, y, z])
|
||||
return np.array(points)
|
||||
|
||||
def save_and_exit(self):
|
||||
if not self.saving:
|
||||
self.saving = True
|
||||
if self.point_clouds:
|
||||
all_points = np.vstack(self.point_clouds)
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(all_points)
|
||||
o3d.io.write_point_cloud("output.pcd", pcd)
|
||||
self.get_logger().info("Saved output.pcd")
|
||||
rclpy.shutdown()
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
saver = PointCloudSaver()
|
||||
rclpy.spin(saver)
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
102
src/rc_lidar/simple.py
Normal file
102
src/rc_lidar/simple.py
Normal file
@ -0,0 +1,102 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import PointCloud2, PointField
|
||||
import numpy as np
|
||||
import struct
|
||||
from sklearn.cluster import DBSCAN
|
||||
|
||||
class BasketballFrameDetector(Node):
|
||||
def __init__(self):
|
||||
super().__init__('basketball_frame_detector')
|
||||
self.subscription = self.create_subscription(
|
||||
PointCloud2,
|
||||
'/livox/lidar_filtered',
|
||||
self.pointcloud_callback,
|
||||
10
|
||||
)
|
||||
self.publisher = self.create_publisher(
|
||||
PointCloud2,
|
||||
'/basketball_frame_cloud',
|
||||
10
|
||||
)
|
||||
self.pointcloud_buffer = []
|
||||
|
||||
def pointcloud_callback(self, msg):
|
||||
points = self.pointcloud2_to_xyz(msg)
|
||||
self.pointcloud_buffer.append(points)
|
||||
self.get_logger().info(f'已保存点云组数: {len(self.pointcloud_buffer)}')
|
||||
if len(self.pointcloud_buffer) < 10:
|
||||
return
|
||||
|
||||
# 合并10组点云
|
||||
all_points = np.vstack(self.pointcloud_buffer)
|
||||
xy_points = all_points[:, :2]
|
||||
self.get_logger().info(f'合并后点数: {xy_points.shape[0]}')
|
||||
|
||||
# 清空缓存，准备下一批
|
||||
self.pointcloud_buffer = []
|
||||
|
||||
# 聚类识别
|
||||
if len(xy_points) < 10:
|
||||
return
|
||||
clustering = DBSCAN(eps=0.3, min_samples=10).fit(xy_points)
|
||||
labels = clustering.labels_
|
||||
unique_labels = set(labels)
|
||||
for label in unique_labels:
|
||||
if label == -1:
|
||||
continue
|
||||
cluster = all_points[labels == label]
|
||||
if len(cluster) < 30:
|
||||
continue
|
||||
min_x, min_y = np.min(cluster[:, :2], axis=0)
|
||||
max_x, max_y = np.max(cluster[:, :2], axis=0)
|
||||
width = abs(max_x - min_x)
|
||||
height = abs(max_y - min_y)
|
||||
self.get_logger().info(
|
||||
f'聚类: label={label}, width={width:.2f}, height={height:.2f}, 点数={len(cluster)}'
|
||||
)
|
||||
if 1.6 < width < 2.0 and 0.8 < height < 1.2:
|
||||
self.get_logger().info(
|
||||
f'可能是篮球框: label={label}, width={width:.2f}, height={height:.2f}, 点数={len(cluster)}'
|
||||
)
|
||||
# 发布识别到的篮球框点云
|
||||
cloud_msg = self.xyz_array_to_pointcloud2(cluster, msg.header)
|
||||
self.publisher.publish(cloud_msg)
|
||||
|
||||
def pointcloud2_to_xyz(self, cloud_msg):
|
||||
fmt = 'ffff' # x, y, z, intensity
|
||||
points = []
|
||||
for i in range(cloud_msg.width):
|
||||
offset = i * cloud_msg.point_step
|
||||
x, y, z, intensity = struct.unpack_from(fmt, cloud_msg.data, offset)
|
||||
points.append([x, y, z, intensity])
|
||||
return np.array(points)
|
||||
|
||||
def xyz_array_to_pointcloud2(self, points, header):
|
||||
# 构造 PointCloud2 消息
|
||||
msg = PointCloud2()
|
||||
msg.header = header
|
||||
msg.height = 1
|
||||
msg.width = len(points)
|
||||
msg.is_dense = True
|
||||
msg.is_bigendian = False
|
||||
msg.point_step = 16
|
||||
msg.row_step = msg.point_step * msg.width
|
||||
msg.fields = [
|
||||
PointField(name='x', offset=0, datatype=7, count=1),
|
||||
PointField(name='y', offset=4, datatype=7, count=1),
|
||||
PointField(name='z', offset=8, datatype=7, count=1),
|
||||
PointField(name='intensity', offset=12, datatype=7, count=1),
|
||||
]
|
||||
msg.data = b''.join([struct.pack('ffff', *p) for p in points])
|
||||
return msg
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = BasketballFrameDetector()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
126
src/rc_lidar/simple_icp.py
Normal file
126
src/rc_lidar/simple_icp.py
Normal file
@ -0,0 +1,126 @@
|
||||
#!/usr/bin/env python3
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
from sensor_msgs_py import point_cloud2
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
import open3d as o3d
|
||||
import numpy as np
|
||||
from transforms3d.quaternions import mat2quat
|
||||
|
||||
class PointCloudLocalization(Node):
|
||||
def __init__(self):
|
||||
super().__init__('point_cloud_localizer')
|
||||
|
||||
# 加载参考点云地图 (PCD文件)
|
||||
self.reference_map = o3d.io.read_point_cloud("/home/robofish/RC2025/lankuang.pcd") # 替换为你的PCD文件路径
|
||||
if not self.reference_map.has_points():
|
||||
self.get_logger().error("Failed to load reference map!")
|
||||
rclpy.shutdown()
|
||||
|
||||
# 预处理参考地图
|
||||
self.reference_map = self.reference_map.voxel_down_sample(voxel_size=0.05)
|
||||
self.reference_map.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)[0]
|
||||
|
||||
# 创建ICP对象
|
||||
self.icp = o3d.pipelines.registration.registration_icp
|
||||
self.threshold = 0.5 # 匹配距离阈值 (米)
|
||||
self.trans_init = np.identity(4) # 初始变换矩阵
|
||||
|
||||
# 订阅激光雷达点云
|
||||
self.subscription = self.create_subscription(
|
||||
PointCloud2,
|
||||
'/livox/lidar_filtered',
|
||||
self.lidar_callback,
|
||||
10)
|
||||
|
||||
# 发布估计位置
|
||||
self.pose_pub = self.create_publisher(PoseStamped, '/estimated_pose', 10)
|
||||
|
||||
self.get_logger().info("Point Cloud Localization Node Initialized!")
|
||||
|
||||
def ros_pc2_to_o3d(self, ros_cloud):
|
||||
"""将ROS PointCloud2转换为Open3D点云"""
|
||||
# 提取xyz坐标
|
||||
points = point_cloud2.read_points(ros_cloud, field_names=("x", "y", "z"), skip_nans=True)
|
||||
xyz = np.array([ [p[0], p[1], p[2]] for p in points ], dtype=np.float32)
|
||||
|
||||
if xyz.shape[0] == 0:
|
||||
return None
|
||||
|
||||
# 创建Open3D点云
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(xyz)
|
||||
return pcd
|
||||
|
||||
def preprocess_pointcloud(self, pcd):
|
||||
"""点云预处理"""
|
||||
# 降采样
|
||||
pcd = pcd.voxel_down_sample(voxel_size=0.03)
|
||||
|
||||
# 移除离群点
|
||||
pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=1.0)
|
||||
|
||||
# 移除地面 (可选)
|
||||
# plane_model, inliers = pcd.segment_plane(distance_threshold=0.1, ransac_n=3, num_iterations=100)
|
||||
# pcd = pcd.select_by_index(inliers, invert=True)
|
||||
|
||||
return pcd
|
||||
|
||||
def lidar_callback(self, msg):
|
||||
"""处理新的激光雷达数据"""
|
||||
# 转换为Open3D格式
|
||||
current_pcd = self.ros_pc2_to_o3d(msg)
|
||||
if current_pcd is None:
|
||||
self.get_logger().warn("Received empty point cloud!")
|
||||
return
|
||||
|
||||
# 预处理当前点云
|
||||
current_pcd = self.preprocess_pointcloud(current_pcd)
|
||||
|
||||
# 执行ICP配准
|
||||
reg_result = self.icp(
|
||||
current_pcd, self.reference_map, self.threshold,
|
||||
self.trans_init,
|
||||
o3d.pipelines.registration.TransformationEstimationPointToPoint(),
|
||||
o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50)
|
||||
)
|
||||
|
||||
# 更新变换矩阵
|
||||
self.trans_init = reg_result.transformation
|
||||
|
||||
# 提取位置和方向
|
||||
translation = reg_result.transformation[:3, 3]
|
||||
rotation_matrix = reg_result.transformation[:3, :3]
|
||||
|
||||
# 转换为四元数
|
||||
quaternion = mat2quat(reg_result.transformation[:3, :3]) # 注意返回顺序为 [w, x, y, z]
|
||||
|
||||
# 发布位姿
|
||||
pose_msg = PoseStamped()
|
||||
pose_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
pose_msg.header.frame_id = "livox_frame"
|
||||
pose_msg.pose.position.x = translation[0]
|
||||
pose_msg.pose.position.y = translation[1]
|
||||
pose_msg.pose.position.z = translation[2]
|
||||
pose_msg.pose.orientation.x = quaternion[1] # x
|
||||
pose_msg.pose.orientation.y = quaternion[2] # y
|
||||
pose_msg.pose.orientation.z = quaternion[3] # z
|
||||
pose_msg.pose.orientation.w = quaternion[0] # w
|
||||
|
||||
self.pose_pub.publish(pose_msg)
|
||||
self.get_logger().info(f"Estimated Position: x={translation[0]:.2f}, y={translation[1]:.2f}, z={translation[2]:.2f}")
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = PointCloudLocalization()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
75
src/rc_lidar/xiamian.py
Normal file
75
src/rc_lidar/xiamian.py
Normal file
@ -0,0 +1,75 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
from nav_msgs.msg import OccupancyGrid
|
||||
import numpy as np
|
||||
import struct
|
||||
import time
|
||||
|
||||
class PointCloudToGrid(Node):
|
||||
def __init__(self):
|
||||
super().__init__('pointcloud_to_grid')
|
||||
self.subscription = self.create_subscription(
|
||||
PointCloud2,
|
||||
'/livox/lidar_filtered',
|
||||
self.pointcloud_callback,
|
||||
10)
|
||||
self.publisher = self.create_publisher(OccupancyGrid, '/lidar_grid', 10)
|
||||
self.grid_size = 2000
|
||||
self.resolution = 0.02
|
||||
self.origin_x = -20.0
|
||||
self.origin_y = -20.0
|
||||
self.points_buffer = []
|
||||
self.last_header = None
|
||||
# 定时器每0.5秒触发一次
|
||||
self.timer = self.create_timer(0.5, self.publish_grid)
|
||||
|
||||
def pointcloud_callback(self, msg):
|
||||
points = self.pointcloud2_to_xyz_array(msg)
|
||||
self.points_buffer.append(points)
|
||||
self.last_header = msg.header # 保存最新header用于地图消息
|
||||
|
||||
def publish_grid(self):
|
||||
if not self.points_buffer:
|
||||
return
|
||||
# 合并0.5秒内所有点
|
||||
all_points = np.concatenate(self.points_buffer, axis=0)
|
||||
grid = np.zeros((self.grid_size, self.grid_size), dtype=np.int8)
|
||||
for x, y, z in all_points:
|
||||
if z < 2.0:
|
||||
ix = int((x - self.origin_x) / self.resolution)
|
||||
iy = int((y - self.origin_y) / self.resolution)
|
||||
if 0 <= ix < self.grid_size and 0 <= iy < self.grid_size:
|
||||
grid[iy, ix] = 100
|
||||
grid_msg = OccupancyGrid()
|
||||
if self.last_header:
|
||||
grid_msg.header = self.last_header
|
||||
grid_msg.info.resolution = self.resolution
|
||||
grid_msg.info.width = self.grid_size
|
||||
grid_msg.info.height = self.grid_size
|
||||
grid_msg.info.origin.position.x = self.origin_x
|
||||
grid_msg.info.origin.position.y = self.origin_y
|
||||
grid_msg.data = grid.flatten().tolist()
|
||||
self.publisher.publish(grid_msg)
|
||||
self.points_buffer.clear() # 清空缓存
|
||||
|
||||
def pointcloud2_to_xyz_array(self, cloud_msg):
|
||||
# 解析 PointCloud2 数据为 numpy 数组
|
||||
fmt = 'fff' # x, y, z
|
||||
point_step = cloud_msg.point_step
|
||||
data = cloud_msg.data
|
||||
points = []
|
||||
for i in range(0, len(data), point_step):
|
||||
x, y, z = struct.unpack_from(fmt, data, i)
|
||||
points.append([x, y, z])
|
||||
return np.array(points)
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = PointCloudToGrid()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
51
src/rc_lidar/zhaoban.py
Normal file
51
src/rc_lidar/zhaoban.py
Normal file
@ -0,0 +1,51 @@
|
||||
import numpy as np
|
||||
from sklearn.cluster import DBSCAN
|
||||
from scipy.optimize import leastsq
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
def fit_circle(x, y):
|
||||
# 拟合圆的函数
|
||||
def calc_R(xc, yc):
|
||||
return np.sqrt((x - xc)**2 + (y - yc)**2)
|
||||
def f(c):
|
||||
Ri = calc_R(*c)
|
||||
return Ri - Ri.mean()
|
||||
center_estimate = np.mean(x), np.mean(y)
|
||||
center, _ = leastsq(f, center_estimate)
|
||||
radius = calc_R(*center).mean()
|
||||
return center[0], center[1], radius
|
||||
|
||||
def find_circle(points, eps=0.5, min_samples=10):
|
||||
# 聚类
|
||||
clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(points)
|
||||
labels = clustering.labels_
|
||||
# 只取最大簇
|
||||
unique, counts = np.unique(labels[labels != -1], return_counts=True)
|
||||
if len(unique) == 0:
|
||||
raise ValueError("未找到有效聚类")
|
||||
main_cluster = unique[np.argmax(counts)]
|
||||
cluster_points = points[labels == main_cluster]
|
||||
x, y = cluster_points[:, 0], cluster_points[:, 1]
|
||||
# 拟合圆
|
||||
xc, yc, r = fit_circle(x, y)
|
||||
return xc, yc, r, cluster_points
|
||||
|
||||
if __name__ == "__main__":
|
||||
# 示例数据
|
||||
np.random.seed(0)
|
||||
angle = np.linspace(0, 2 * np.pi, 100)
|
||||
x = 5 + 3 * np.cos(angle) + np.random.normal(0, 0.1, 100)
|
||||
y = 2 + 3 * np.sin(angle) + np.random.normal(0, 0.1, 100)
|
||||
points = np.vstack((x, y)).T
|
||||
|
||||
xc, yc, r, cluster_points = find_circle(points)
|
||||
print(f"圆心: ({xc:.2f}, {yc:.2f}), 半径: {r:.2f}")
|
||||
|
||||
# 可视化
|
||||
plt.scatter(points[:, 0], points[:, 1], label='所有点')
|
||||
plt.scatter(cluster_points[:, 0], cluster_points[:, 1], label='聚类点')
|
||||
circle = plt.Circle((xc, yc), r, color='r', fill=False, label='拟合圆')
|
||||
plt.gca().add_patch(circle)
|
||||
plt.legend()
|
||||
plt.axis('equal')
|
||||
plt.show()
|
||||
0
src/rc_lidar/zhaoyuan.py
Normal file
0
src/rc_lidar/zhaoyuan.py
Normal file
@ -135,6 +135,9 @@ class AimDataSerial(Node):
|
||||
packet.extend(struct.pack('<f', distance)) # distance (小端序float)
|
||||
packet.append(0xFE) # 包尾
|
||||
|
||||
# 打印发送的两个数据
|
||||
self.get_logger().info(f'发送到R2的数据: yaw={yaw}, distance={distance}')
|
||||
|
||||
# 发送数据到r2串口
|
||||
if self.safe_serial_write(self.serial_conn_r2, packet, "r2"):
|
||||
self.get_logger().info(f'Sent aim packet to r2: {packet.hex()}')
|
||||
@ -159,10 +162,8 @@ class AimDataSerial(Node):
|
||||
y = transform.transform.translation.y
|
||||
z = transform.transform.translation.z
|
||||
|
||||
# 减少日志输出频率
|
||||
self.position_counter += 1
|
||||
if self.position_counter % 50 == 0: # 每5秒打印一次
|
||||
self.get_logger().info(f'Position - x: {x:.3f}, y: {y:.3f}, z: {z:.3f}')
|
||||
# 打印发送的三个数据
|
||||
self.get_logger().info(f'发送到R1的三个坐标数据: x={x:.3f}, y={y:.3f}, z={z:.3f}')
|
||||
|
||||
# 构建坐标数据包
|
||||
# 格式: 包头(0xAA) + x(4字节float) + y(4字节float) + z(4字节float) + 包尾(0xBB)
|
||||
@ -172,11 +173,10 @@ class AimDataSerial(Node):
|
||||
packet.extend(struct.pack('<f', y)) # y坐标 (小端序float)
|
||||
packet.extend(struct.pack('<f', z)) # z坐标 (小端序float)
|
||||
packet.append(0xBB) # 包尾
|
||||
|
||||
|
||||
# 发送数据到r1串口
|
||||
if self.safe_serial_write(self.serial_conn_r1, packet, "r1"):
|
||||
if self.position_counter % 50 == 0: # 只在打印位置时也打印发送信息
|
||||
self.get_logger().info(f'Sent position packet to r1: {packet.hex()}')
|
||||
self.get_logger().info(f'Sent position packet to r1: {packet.hex()}')
|
||||
|
||||
except tf2_ros.TransformException as e:
|
||||
# TF变换异常，可能是坐标系不存在或时间不同步
|
||||
@ -184,6 +184,48 @@ class AimDataSerial(Node):
|
||||
self.get_logger().warn(f'Transform exception: {e}')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Error in position_timer_callback: {e}')
|
||||
# def position_timer_callback(self):
|
||||
# if not self.serial_conn_r1:
|
||||
# return
|
||||
|
||||
# try:
|
||||
# # 获取baselink相对于map的变换
|
||||
# transform = self.tf_buffer.lookup_transform(
|
||||
# 'map', # 目标坐标系
|
||||
# 'base_link', # 源坐标系
|
||||
# rclpy.time.Time() # 最新时间
|
||||
# )
|
||||
|
||||
# # 提取位置坐标
|
||||
# x = transform.transform.translation.x
|
||||
# y = transform.transform.translation.y
|
||||
# z = transform.transform.translation.z
|
||||
|
||||
# # 减少日志输出频率
|
||||
# self.position_counter += 1
|
||||
# if self.position_counter % 50 == 0: # 每5秒打印一次
|
||||
# self.get_logger().info(f'Position - x: {x:.3f}, y: {y:.3f}, z: {z:.3f}')
|
||||
|
||||
# # 构建坐标数据包
|
||||
# # 格式: 包头(0xAA) + x(4字节float) + y(4字节float) + z(4字节float) + 包尾(0xBB)
|
||||
# packet = bytearray()
|
||||
# packet.append(0xAA) # 包头
|
||||
# packet.extend(struct.pack('<f', x)) # x坐标 (小端序float)
|
||||
# packet.extend(struct.pack('<f', y)) # y坐标 (小端序float)
|
||||
# packet.extend(struct.pack('<f', z)) # z坐标 (小端序float)
|
||||
# packet.append(0xBB) # 包尾
|
||||
|
||||
# # 发送数据到r1串口
|
||||
# if self.safe_serial_write(self.serial_conn_r1, packet, "r1"):
|
||||
# if self.position_counter % 50 == 0: # 只在打印位置时也打印发送信息
|
||||
# self.get_logger().info(f'Sent position packet to r1: {packet.hex()}')
|
||||
|
||||
# except tf2_ros.TransformException as e:
|
||||
# # TF变换异常，可能是坐标系不存在或时间不同步
|
||||
# if self.position_counter % 100 == 0: # 减少TF异常日志频率
|
||||
# self.get_logger().warn(f'Transform exception: {e}')
|
||||
# except Exception as e:
|
||||
# self.get_logger().error(f'Error in position_timer_callback: {e}')
|
||||
|
||||
def __del__(self):
|
||||
if hasattr(self, 'serial_conn_r2') and self.serial_conn_r2 and self.serial_conn_r2.is_open:
|
||||
|
||||
36
src/rm_driver/rm_serial_driver/script/pub_goal.py
Normal file
36
src/rm_driver/rm_serial_driver/script/pub_goal.py
Normal file
@ -0,0 +1,36 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import PointStamped
|
||||
from rm_msgs.msg import MoveGoal
|
||||
|
||||
class ClickedGoalPublisher(Node):
|
||||
def __init__(self):
|
||||
super().__init__('clicked_goal_publisher')
|
||||
self.subscription = self.create_subscription(
|
||||
PointStamped,
|
||||
'/clicked_point',
|
||||
self.clicked_callback,
|
||||
10
|
||||
)
|
||||
self.publisher = self.create_publisher(MoveGoal, '/move_goal', 10)
|
||||
|
||||
def clicked_callback(self, msg):
|
||||
goal = MoveGoal()
|
||||
goal.x = msg.point.x
|
||||
goal.y = msg.point.y
|
||||
goal.angle = 0.0
|
||||
goal.max_speed = 0.0
|
||||
goal.tolerance = 0.1
|
||||
goal.rotor = False
|
||||
self.publisher.publish(goal)
|
||||
self.get_logger().info(f'发布目标: x={goal.x:.2f}, y={goal.y:.2f}')
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = ClickedGoalPublisher()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -8,7 +8,7 @@ import sys
|
||||
# 预设多组参数
|
||||
MAP_CONFIGS = {
|
||||
"map": {"split_x": 7.155, "goal1": (0.65, 3.95), "goal2": (13.66, 3.73)},
|
||||
"map1": {"split_x": 7.085, "goal1": (0.54, -3.28), "goal2": (13.63, -3.37)},
|
||||
"map1": {"split_x": 7.085, "goal1": (0.63, -3.40), "goal2": (13.60, -3.42)},
|
||||
"map2": {"split_x": 7.075, "goal1": (0.57, -3.34), "goal2": (13.58, -3.35)},
|
||||
}
|
||||
|
||||
|
||||
@ -8,7 +8,7 @@
|
||||
odom_frame_id: "odom"
|
||||
laser_frame_id: "livox_frame"
|
||||
pointcloud_topic: "/livox/lidar/pointcloud"
|
||||
thresh: 0.1
|
||||
thresh: 0.1
|
||||
xy_offset: 0.2
|
||||
yaw_offset: 30.0
|
||||
yaw_resolution: 10.0
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
slam_toolbox:
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# Plugin params
|
||||
|
||||
@ -1,3 +1,3 @@
|
||||
base_link2livox_frame:
|
||||
xyz: "\"0.01906 -0.33 0.41\""
|
||||
xyz: "\"0.00906 -0.33 0.41\""
|
||||
rpy: "\"0.0 0.0 0.0\""
|
||||
@ -1,23 +0,0 @@
|
||||
build-kinetic:
|
||||
variables:
|
||||
ROSDISTRO: "kinetic"
|
||||
CATKIN_WS: "/root/catkin_ws"
|
||||
stage: build
|
||||
image: osrf/ros:$ROSDISTRO-desktop-full
|
||||
before_script:
|
||||
- apt-get -qq update
|
||||
- apt-get -qq install git-core python-catkin-tools curl
|
||||
- mkdir -p $CATKIN_WS/src
|
||||
- cp -a . $CATKIN_WS/src/
|
||||
- cd $CATKIN_WS
|
||||
- rosdep update
|
||||
- rosdep install -y --from-paths src --ignore-src --rosdistro $ROSDISTRO --as-root=apt:false
|
||||
script:
|
||||
- source /ros_entrypoint.sh
|
||||
- cd $CATKIN_WS
|
||||
- catkin build -i -s --no-status
|
||||
only:
|
||||
- master
|
||||
tags:
|
||||
- ubuntu
|
||||
- docker
|
||||
@ -1,109 +0,0 @@
|
||||
# Generic .travis.yml file for running continuous integration on Travis-CI with
|
||||
# any ROS package.
|
||||
#
|
||||
# This installs ROS on a clean Travis-CI virtual machine, creates a ROS
|
||||
# workspace, resolves all listed dependencies, and sets environment variables
|
||||
# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are
|
||||
# no compilation errors), and runs all the tests. If any of the compilation/test
|
||||
# phases fail, the build is marked as a failure.
|
||||
#
|
||||
# We handle two types of package dependencies:
|
||||
# - packages (ros and otherwise) available through apt-get. These are installed
|
||||
# using rosdep, based on the information in the ROS package.xml.
|
||||
# - dependencies that must be checked out from source. These are handled by
|
||||
# 'wstool', and should be listed in a file named dependencies.rosinstall.
|
||||
#
|
||||
# There are two variables you may want to change:
|
||||
# - ROS_DISTRO (default is indigo). Note that packages must be available for
|
||||
# ubuntu 14.04 trusty.
|
||||
# - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo
|
||||
# root). This should list all necessary repositories in wstool format (see
|
||||
# the ros wiki). If the file does not exists then nothing happens.
|
||||
#
|
||||
# See the README.md for more information.
|
||||
#
|
||||
# Author: Felix Duvallet <felixd@gmail.com>
|
||||
|
||||
# NOTE: The build lifecycle on Travis.ci is something like this:
|
||||
# before_install
|
||||
# install
|
||||
# before_script
|
||||
# script
|
||||
# after_success or after_failure
|
||||
# after_script
|
||||
# OPTIONAL before_deploy
|
||||
# OPTIONAL deploy
|
||||
# OPTIONAL after_deploy
|
||||
|
||||
################################################################################
|
||||
|
||||
# Use ubuntu trusty (14.04) with sudo privileges.
|
||||
dist: trusty
|
||||
sudo: required
|
||||
language:
|
||||
- generic
|
||||
cache:
|
||||
- apt
|
||||
|
||||
# Configuration variables. All variables are global now, but this can be used to
|
||||
# trigger a build matrix for different ROS distributions if desired.
|
||||
env:
|
||||
global:
|
||||
- ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [precise|trusty|...]
|
||||
- CI_SOURCE_PATH=$(pwd)
|
||||
- ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall
|
||||
- CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options
|
||||
- ROS_PARALLEL_JOBS='-j8 -l6'
|
||||
matrix:
|
||||
- ROS_DISTRO=indigo
|
||||
- ROS_DISTRO=jade
|
||||
|
||||
|
||||
################################################################################
|
||||
|
||||
# Install system dependencies, namely a very barebones ROS setup.
|
||||
before_install:
|
||||
- sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
|
||||
- wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
|
||||
- sudo apt-get update -qq
|
||||
- sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin
|
||||
- source /opt/ros/$ROS_DISTRO/setup.bash
|
||||
# Prepare rosdep to install dependencies.
|
||||
- sudo rosdep init
|
||||
- rosdep update
|
||||
|
||||
# Create a catkin workspace with the package under integration.
|
||||
install:
|
||||
- mkdir -p ~/catkin_ws/src
|
||||
- cd ~/catkin_ws/src
|
||||
- catkin_init_workspace
|
||||
# Create the devel/setup.bash (run catkin_make with an empty workspace) and
|
||||
# source it to set the path variables.
|
||||
- cd ~/catkin_ws
|
||||
- catkin_make
|
||||
- source devel/setup.bash
|
||||
# Add the package under integration to the workspace using a symlink.
|
||||
- cd ~/catkin_ws/src
|
||||
- ln -s $CI_SOURCE_PATH .
|
||||
|
||||
# Install all dependencies, using wstool and rosdep.
|
||||
# wstool looks for a ROSINSTALL_FILE defined in the environment variables.
|
||||
before_script:
|
||||
# source dependencies: install using wstool.
|
||||
- cd ~/catkin_ws/src
|
||||
- wstool init
|
||||
- if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi
|
||||
- wstool up
|
||||
# package depdencies: install using rosdep.
|
||||
- cd ~/catkin_ws
|
||||
- rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
|
||||
|
||||
# Compile and test. If the CATKIN_OPTIONS file exists, use it as an argument to
|
||||
# catkin_make.
|
||||
script:
|
||||
- cd ~/catkin_ws
|
||||
- catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS )
|
||||
# Testing: Use both run_tests (to see the output) and test (to error out).
|
||||
- catkin_make run_tests # This always returns 0, but looks pretty.
|
||||
- catkin_make test # This will return non-zero if a test fails.
|
||||
|
||||
@ -1,30 +0,0 @@
|
||||
costmap_converter ROS Package
|
||||
=============================
|
||||
|
||||
A ros package that includes plugins and nodes to convert occupied costmap2d cells to primitive types
|
||||
|
||||
Build status of the *master* branch:
|
||||
- ROS Buildfarm Kinetic: [](http://build.ros.org/job/Kdev__costmap_converter__ubuntu_xenial_amd64/)
|
||||
- ROS Buildfarm Indigo: [](http://build.ros.org/job/Idev__costmap_converter__ubuntu_trusty_amd64/)
|
||||
|
||||
|
||||
### Contributors
|
||||
|
||||
- Christoph Rösmann
|
||||
- Franz Albers (*CostmapToDynamicObstacles* plugin)
|
||||
- Otniel Rinaldo
|
||||
|
||||
|
||||
### License
|
||||
|
||||
The *costmap_converter* package is licensed under the BSD license.
|
||||
It depends on other ROS packages, which are listed in the package.xml. They are also BSD licensed.
|
||||
|
||||
Some third-party dependencies are included that are licensed under different terms:
|
||||
- *MultitargetTracker*, GNU GPLv3, https://github.com/Smorodov/Multitarget-tracker
|
||||
(partially required for the *CostmapToDynamicObstacles* plugin)
|
||||
|
||||
All packages included are distributed in the hope that they will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details.
|
||||
|
||||
|
||||
|
||||
@ -1,105 +0,0 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package costmap_converter
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.1.2 (2020-06-22)
|
||||
------------------
|
||||
* OpenCV 4 compatibility fix
|
||||
* Contributors: daviddudas
|
||||
|
||||
0.1.1 (2020-01-25)
|
||||
------------------
|
||||
* Fixed ament plugin export
|
||||
* Revert release-mode for cmake build
|
||||
* Contributors: Christoph Rösmann
|
||||
|
||||
0.1.0 (2020-01-23)
|
||||
------------------
|
||||
* Port to ROS2 (thanks to Vinnam Kim and stevemacenski)
|
||||
* Messages moved to a separate package
|
||||
* Contributors: Christoph Rösmann, Vinnam Kim, stevemacenski
|
||||
|
||||
0.0.12 (2019-12-02)
|
||||
-------------------
|
||||
* CostmapToPolygons: Simplification of the polygon by Douglas-Peucker algorithm (reduces the density of points in the polygon).
|
||||
* Bugfixes
|
||||
* Contributors: Rainer Kümmerle
|
||||
|
||||
0.0.11 (2019-10-26)
|
||||
-------------------
|
||||
* rostest integration to avoid running a roscore separately for unit testing
|
||||
* Contributors: Christoph Rösmann
|
||||
|
||||
0.0.10 (2019-10-26)
|
||||
-------------------
|
||||
* Runtime improvements for CostmapToPolygonsDBSMCCH (`#12 <https://github.com/rst-tu-dortmund/costmap_converter/issues/12>`_)
|
||||
* Grid lookup for regionQuery
|
||||
* use a grid structure for looking up nearest neighbors
|
||||
* parameters in a struct
|
||||
* guard the parameters by drawing a copy from dynamic reconfigure
|
||||
* Adding some test cases for regionQuery and dbScan
|
||||
* Avoid computing sqrt at the end of convexHull2
|
||||
* Add doxygen comments for the neighbor lookup
|
||||
* Change the param read to one liners
|
||||
* Add test on empty map for dbScan
|
||||
* Contributors: Rainer Kümmerle
|
||||
|
||||
0.0.9 (2018-05-28)
|
||||
------------------
|
||||
* Moved plugin loader for static costmap conversion to BaseCostmapToDynamicObstacles.
|
||||
The corresponding ROS parameter `static_converter_plugin` is now defined in the CostmapToDynamicObstacles namespace.
|
||||
* Contributors: Christoph Rösmann
|
||||
|
||||
0.0.8 (2018-05-17)
|
||||
------------------
|
||||
* Standalone converter subscribes now to costmap updates. Fixes `#1 <https://github.com/rst-tu-dortmund/costmap_converter/issues/1>`_
|
||||
* Adds radius field for circular obstacles to ObstacleMsg
|
||||
* Stacked costmap conversion (`#7 <https://github.com/rst-tu-dortmund/costmap_converter/issues/7>`_).
|
||||
E.g., it is now possible combine a dynamic obstacle and static obstacle converter plugin.
|
||||
* Contributors: Christoph Rösmann, Franz Albers
|
||||
|
||||
0.0.7 (2017-09-20)
|
||||
------------------
|
||||
* Fixed some compilation issues (C++11 compiler flags and opencv2 on indigo/jade).
|
||||
* Dynamic obstacle plugin: obstacle velocity is now published for both x and y coordinates rather than the absolute value
|
||||
|
||||
0.0.6 (2017-09-18)
|
||||
------------------
|
||||
* This pull request adds the costmap to dynamic obstacles plugin (written by Franz Albers).
|
||||
It detects the dynamic foreground of the costmap (based on the temporal evolution of the costmap)
|
||||
including blobs representing the obstacles. Furthermore, Kalman-based tracking is applied to estimate
|
||||
the current velocity for each obstacle.
|
||||
**Note, this plugin is still experimental.**
|
||||
* New message types are introduced: costmap\_converter::ObstacleMsg and costmap\_converter::ObstacleArrayMsg.
|
||||
These types extend the previous polygon representation by additional velocity, orientation and id information.
|
||||
* The API has been extended to provide obstacles via the new ObstacleArrayMsg type instead of vector of polygons.
|
||||
* Contributors: Franz Albers, Christoph Rösmann
|
||||
|
||||
0.0.5 (2016-02-01)
|
||||
------------------
|
||||
* Major changes regarding the line detection based on the convex hull
|
||||
(it should be much more robust now).
|
||||
* Concave hull plugin added.
|
||||
* The cluster size can now be limited from above using a specific parameter.
|
||||
This implicitly avoids large clusters forming a 'L' or 'U'.
|
||||
* All parameters can now be adjusted using dynamic_reconfigure (rqt_reconfigure).
|
||||
* Some parameter names changed.
|
||||
* Line plugin based on ransac: line inliers must now be placed inbetween the start and end of a line.
|
||||
|
||||
0.0.4 (2016-01-11)
|
||||
------------------
|
||||
* Fixed conversion from map to world coordinates if the costmap is not quadratic.
|
||||
|
||||
0.0.3 (2015-12-23)
|
||||
------------------
|
||||
* The argument list of the initialize method requires a nodehandle from now on. This facilitates the handling of parameter namespaces for multiple instantiations of the plugin.
|
||||
* This change is pushed immediately as a single release to avoid API breaks (since version 0.0.2 is not on the official repos up to now).
|
||||
|
||||
0.0.2 (2015-12-22)
|
||||
------------------
|
||||
* Added a plugin for converting the costmap to lines using ransac
|
||||
|
||||
0.0.1 (2015-12-21)
|
||||
------------------
|
||||
* First release of the package including a pluginlib interface, two plugins (costmap to polygons and costmap to lines) and a standalone conversion node.
|
||||
|
||||
@ -1,97 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(costmap_converter)
|
||||
|
||||
# Set to Release in order to speed up the program significantly
|
||||
set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
|
||||
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(class_loader REQUIRED)
|
||||
find_package(costmap_converter_msgs REQUIRED)
|
||||
find_package(cv_bridge REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav2_costmap_2d REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
|
||||
set(dependencies
|
||||
class_loader
|
||||
costmap_converter_msgs
|
||||
cv_bridge
|
||||
geometry_msgs
|
||||
nav2_costmap_2d
|
||||
tf2
|
||||
tf2_geometry_msgs
|
||||
tf2_ros
|
||||
OpenCV
|
||||
pluginlib
|
||||
rclcpp
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
add_library(costmap_converter SHARED
|
||||
src/costmap_to_polygons.cpp
|
||||
src/costmap_to_polygons_concave.cpp
|
||||
src/costmap_to_lines_convex_hull.cpp
|
||||
src/costmap_to_lines_ransac.cpp
|
||||
src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp
|
||||
src/costmap_to_dynamic_obstacles/background_subtractor.cpp
|
||||
src/costmap_to_dynamic_obstacles/blob_detector.cpp
|
||||
src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp
|
||||
src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp
|
||||
src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(costmap_converter
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
add_executable(standalone_converter src/costmap_converter_node.cpp)
|
||||
|
||||
target_link_libraries(standalone_converter
|
||||
costmap_converter
|
||||
)
|
||||
ament_target_dependencies(standalone_converter
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
install(TARGETS costmap_converter
|
||||
DESTINATION lib
|
||||
)
|
||||
install(TARGETS standalone_converter
|
||||
DESTINATION bin
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(costmap_converter)
|
||||
ament_export_dependencies(${dependencies})
|
||||
pluginlib_export_plugin_description_file(costmap_converter costmap_converter_plugins.xml)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
ament_add_gtest(test_costmap_polygons test/costmap_polygons.cpp)
|
||||
if(TARGET test_costmap_polygons)
|
||||
target_link_libraries(test_costmap_polygons costmap_converter)
|
||||
endif()
|
||||
ament_target_dependencies(test_costmap_polygons ${dependencies})
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@ -1,136 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
# For integers and doubles:
|
||||
# Name Type Reconfiguration level
|
||||
# Description
|
||||
# Default Min Max
|
||||
|
||||
##################################################################
|
||||
###################### Foreground detection ######################
|
||||
gen.add("alpha_slow", double_t, 0,
|
||||
"Foreground detection: Learning rate of the slow filter",
|
||||
0.3, 0.0, 1.0)
|
||||
|
||||
gen.add("alpha_fast", double_t, 0,
|
||||
"Foreground detection: Learning rate of the fast filter",
|
||||
0.85, 0.0, 1.0)
|
||||
|
||||
gen.add("beta", double_t, 0,
|
||||
"Foreground detection: Weighting coefficient between a pixels value and the mean of its nearest neighbors",
|
||||
0.85, 0.0, 1.0)
|
||||
|
||||
gen.add("min_sep_between_slow_and_fast_filter", int_t, 0,
|
||||
"Foreground detection: Minimal difference between the fast and the slow filter to recognize a obstacle as dynamic",
|
||||
80, 0, 255)
|
||||
|
||||
gen.add("min_occupancy_probability", int_t, 0,
|
||||
"Foreground detection: Minimal value of the fast filter to recognize a obstacle as dynamic",
|
||||
180, 0, 255)
|
||||
|
||||
gen.add("max_occupancy_neighbors", int_t, 0,
|
||||
"Foreground detection: Maximal mean value of the nearest neighbors of a pixel in the slow filter",
|
||||
80, 0, 255)
|
||||
|
||||
gen.add("morph_size", int_t, 0,
|
||||
"Foreground detection: Size of the structuring element for the closing operation",
|
||||
1, 0, 10)
|
||||
|
||||
gen.add("publish_static_obstacles", bool_t, 0,
|
||||
"Include static obstacles as single-point polygons",
|
||||
True)
|
||||
|
||||
############################################################
|
||||
###################### Blob detection ######################
|
||||
|
||||
# These parameters are commented out, because the input image for the blob detection is already binary -> irrelevant
|
||||
#gen.add("threshold_step", double_t, 0,
|
||||
# "Blob detection: Distance between neighboring thresholds",
|
||||
# 256.0, 0.0, 256.0)
|
||||
#
|
||||
#gen.add("min_threshold", double_t, 0,
|
||||
# "Blob detection: Convert the source image to binary images by applying several thresholds, starting at min_threshold",
|
||||
# 1, 0, 255)
|
||||
#
|
||||
#gen.add("max_threshold", double_t, 0,
|
||||
# "Blob detection: Convert the source image to binary images by applying several thresholds, ending at max_threshold",
|
||||
# 255, 0, 255)
|
||||
#
|
||||
#gen.add("min_repeatability", int_t, 0,
|
||||
# "Blob detection: Minimal number of detections of a blob in the several thresholds to be considered as real blob",
|
||||
# 1, 1, 10)
|
||||
#
|
||||
gen.add("min_distance_between_blobs", double_t, 0,
|
||||
"Blob detection: Minimal distance between centers of two blobs to be considered as seperate blobs",
|
||||
10, 0.0, 300.0)
|
||||
|
||||
gen.add("filter_by_area", bool_t, 0,
|
||||
"Blob detection: Filter blobs based on number of pixels",
|
||||
True)
|
||||
|
||||
gen.add("min_area", int_t, 0,
|
||||
"Blob detection: Minimal number of pixels a blob consists of",
|
||||
3, 0, 300)
|
||||
|
||||
gen.add("max_area", int_t, 0,
|
||||
"Blob detection: Maximal number of pixels a blob consists of",
|
||||
300, 0, 300)
|
||||
|
||||
gen.add("filter_by_circularity", bool_t, 0,
|
||||
"Blob detection: Filter blobs based on their circularity",
|
||||
True)
|
||||
|
||||
gen.add("min_circularity", double_t, 0,
|
||||
"Blob detection: Minimal circularity value (0 in case of a line)",
|
||||
0.2, 0.0, 1.0)
|
||||
|
||||
gen.add("max_circularity", double_t, 0,
|
||||
"Blob detection: Maximal circularity value (1 in case of a circle)",
|
||||
1.0, 0.0, 1.0)
|
||||
|
||||
gen.add("filter_by_inertia", bool_t, 0,
|
||||
"Blob detection: Filter blobs based on their inertia ratio",
|
||||
True)
|
||||
|
||||
gen.add("min_inertia_ratio", double_t, 0,
|
||||
"Blob detection: Minimal inertia ratio",
|
||||
0.2, 0.0, 1.0)
|
||||
|
||||
gen.add("max_inertia_ratio", double_t, 0,
|
||||
"Blob detection: Maximal inertia ratio",
|
||||
1.0, 0.0, 1.0)
|
||||
|
||||
gen.add("filter_by_convexity", bool_t, 0,
|
||||
"Blob detection: Filter blobs based on their convexity (Blob area / area of its convex hull)",
|
||||
False)
|
||||
|
||||
gen.add("min_convexity", double_t, 0,
|
||||
"Blob detection: Minimum convexity ratio",
|
||||
0.0, 0.0, 1.0)
|
||||
|
||||
gen.add("max_convexity", double_t, 0,
|
||||
"Blob detection: Maximal convexity ratio",
|
||||
1.0, 0.0, 1.0)
|
||||
|
||||
################################################################
|
||||
#################### Tracking ##################################
|
||||
gen.add("dt", double_t, 0,
|
||||
"Tracking: Time for one timestep of the kalman filter",
|
||||
0.2, 0.1, 3.0)
|
||||
|
||||
gen.add("dist_thresh", double_t, 0,
|
||||
"Tracking: Maximum distance between two points to be considered in the assignment problem",
|
||||
20.0, 0.0, 150.0)
|
||||
|
||||
gen.add("max_allowed_skipped_frames", int_t, 0,
|
||||
"Tracking: Maximum number of frames a object is tracked while it is not seen",
|
||||
3, 0, 10)
|
||||
|
||||
gen.add("max_trace_length", int_t, 0,
|
||||
"Tracking: Maximum number of Points in a objects trace",
|
||||
10, 1, 100)
|
||||
|
||||
exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToDynamicObstacles"))
|
||||
@ -1,41 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
# For integers and doubles:
|
||||
# Name Type Reconfiguration level
|
||||
# Description
|
||||
# Default Min Max
|
||||
|
||||
|
||||
gen.add("cluster_max_distance", double_t, 0,
|
||||
"Parameter for DB_Scan, maximum distance to neighbors [m]",
|
||||
0.4, 0.0, 10.0)
|
||||
|
||||
gen.add("cluster_min_pts", int_t, 0,
|
||||
"Parameter for DB_Scan: minimum number of points that define a cluster",
|
||||
2, 1, 20)
|
||||
|
||||
gen.add("cluster_max_pts", int_t, 0,
|
||||
"Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
|
||||
30, 2, 200)
|
||||
|
||||
gen.add("convex_hull_min_pt_separation", double_t, 0,
|
||||
"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
|
||||
0.1, 0.0, 10.0)
|
||||
|
||||
gen.add("support_pts_max_dist", double_t, 0,
|
||||
"Minimum distance from a point to the line to be counted as support point",
|
||||
0.3, 0.0, 10.0)
|
||||
|
||||
gen.add("support_pts_max_dist_inbetween", double_t, 0,
|
||||
"A line is only defined, if the distance between two consecutive support points is less than this treshold. Set to 0 in order to deactivate this check.",
|
||||
1.0, 0.0, 10.0)
|
||||
|
||||
gen.add("min_support_pts", int_t, 0,
|
||||
"Minimum number of support points to represent a line",
|
||||
2, 0, 50)
|
||||
|
||||
exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToLinesDBSMCCH"))
|
||||
@ -1,54 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
# For integers and doubles:
|
||||
# Name Type Reconfiguration level
|
||||
# Description
|
||||
# Default Min Max
|
||||
|
||||
|
||||
gen.add("cluster_max_distance", double_t, 0,
|
||||
"Parameter for DB_Scan, maximum distance to neighbors [m]",
|
||||
0.4, 0.0, 10.0)
|
||||
|
||||
gen.add("cluster_min_pts", int_t, 0,
|
||||
"Parameter for DB_Scan: minimum number of points that define a cluster",
|
||||
2, 1, 20)
|
||||
|
||||
gen.add("cluster_max_pts", int_t, 0,
|
||||
"Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
|
||||
30, 2, 200)
|
||||
|
||||
gen.add("ransac_inlier_distance", double_t, 0,
|
||||
"Maximum distance to the line segment for inliers",
|
||||
0.2, 0.0, 10.0)
|
||||
|
||||
gen.add("ransac_min_inliers", int_t, 0,
|
||||
"Minimum numer of inliers required to form a line",
|
||||
10, 0, 100)
|
||||
|
||||
gen.add("ransac_no_iterations", int_t, 0,
|
||||
"Number of ransac iterations",
|
||||
2000, 1, 10000)
|
||||
|
||||
gen.add("ransac_remainig_outliers", int_t, 0,
|
||||
"Repeat ransac until the number of outliers is as specified here",
|
||||
3, 0, 50)
|
||||
|
||||
gen.add("ransac_convert_outlier_pts", bool_t, 0,
|
||||
"Convert remaining outliers to single points.",
|
||||
True)
|
||||
|
||||
gen.add("ransac_filter_remaining_outlier_pts", bool_t, 0,
|
||||
"Filter the interior of remaining outliers and keep only keypoints of their convex hull",
|
||||
False)
|
||||
|
||||
gen.add("convex_hull_min_pt_separation", double_t, 0,
|
||||
"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
|
||||
0.1, 0.0, 10.0)
|
||||
|
||||
|
||||
exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToLinesDBSRANSAC"))
|
||||
@ -1,33 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
# For integers and doubles:
|
||||
# Name Type Reconfiguration level
|
||||
# Description
|
||||
# Default Min Max
|
||||
|
||||
|
||||
gen.add("cluster_max_distance", double_t, 0,
|
||||
"Parameter for DB_Scan, maximum distance to neighbors [m]",
|
||||
0.4, 0.0, 10.0)
|
||||
|
||||
gen.add("cluster_min_pts", int_t, 0,
|
||||
"Parameter for DB_Scan: minimum number of points that define a cluster",
|
||||
2, 1, 20)
|
||||
|
||||
gen.add("cluster_max_pts", int_t, 0,
|
||||
"Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
|
||||
30, 2, 200)
|
||||
|
||||
gen.add("convex_hull_min_pt_separation", double_t, 0,
|
||||
"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
|
||||
0.1, 0.0, 10.0)
|
||||
|
||||
gen.add("concave_hull_depth", double_t, 0,
|
||||
"Smaller depth: sharper surface, depth -> high value: convex hull",
|
||||
2.0, 0.0, 100.0)
|
||||
|
||||
exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToPolygonsDBSConcaveHull"))
|
||||
@ -1,29 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
# For integers and doubles:
|
||||
# Name Type Reconfiguration level
|
||||
# Description
|
||||
# Default Min Max
|
||||
|
||||
|
||||
gen.add("cluster_max_distance", double_t, 0,
|
||||
"Parameter for DB_Scan, maximum distance to neighbors [m]",
|
||||
0.4, 0.0, 10.0)
|
||||
|
||||
gen.add("cluster_min_pts", int_t, 0,
|
||||
"Parameter for DB_Scan: minimum number of points that define a cluster",
|
||||
2, 1, 20)
|
||||
|
||||
gen.add("cluster_max_pts", int_t, 0,
|
||||
"Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
|
||||
30, 2, 200)
|
||||
|
||||
gen.add("convex_hull_min_pt_separation", double_t, 0,
|
||||
"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
|
||||
0.1, 0.0, 10.0)
|
||||
|
||||
exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToPolygonsDBSMCCH"))
|
||||
@ -1,38 +0,0 @@
|
||||
<library path="costmap_converter">
|
||||
|
||||
<class type="costmap_converter::CostmapToPolygonsDBSMCCH" base_class_type="costmap_converter::BaseCostmapToPolygons">
|
||||
<description>
|
||||
This class converts costmap2d obstacles into a vector of convex polygons.
|
||||
Clustering is performed with DBScan. Convex polygons are detected using the Monotone Chain Convex Hull algorithm.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
<class type="costmap_converter::CostmapToLinesDBSMCCH" base_class_type="costmap_converter::BaseCostmapToPolygons">
|
||||
<description>
|
||||
This class converts costmap2d obstacles into a vector of lines (represented as polygon msg).
|
||||
Clustering is performed with DBScan. Clusters are transformed into convex polygons using the Monotone Chain Convex Hull algorithm.
|
||||
The resulting keypoints are used for possible line candidates.
|
||||
A line is only defined if there exist a specified number of support points between each keypoint pair.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
<class type="costmap_converter::CostmapToLinesDBSRANSAC" base_class_type="costmap_converter::BaseCostmapToPolygons">
|
||||
<description>
|
||||
This class converts costmap2d obstacles into a vector of lines (represented as polygon msg).
|
||||
Clustering is performed with DBScan. For each cluster RANSAC is applied sequentally to fit multiple line models.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
<class type="costmap_converter::CostmapToPolygonsDBSConcaveHull" base_class_type="costmap_converter::BaseCostmapToPolygons">
|
||||
<description>
|
||||
This class converts costmap2d obstacles into a vector of non-convex (concave) polygons.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
<class type="costmap_converter::CostmapToDynamicObstacles" base_class_type="costmap_converter::BaseCostmapToPolygons">
|
||||
<description>
|
||||
This class detects and tracks obstacles from a sequence of costmaps.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
</library>
|
||||
@ -1,389 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann, Otniel Rinaldo
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef COSTMAP_CONVERTER_INTERFACE_H_
|
||||
#define COSTMAP_CONVERTER_INTERFACE_H_
|
||||
|
||||
//#include <costmap_2d/costmap_2d_ros.h>
|
||||
#include <mutex>
|
||||
#include <memory>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_lifecycle/lifecycle_node.hpp>
|
||||
#include <nav2_costmap_2d/costmap_2d.hpp>
|
||||
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
|
||||
#include <geometry_msgs/msg/polygon.hpp>
|
||||
#include <costmap_converter_msgs/msg/obstacle_array_msg.hpp>
|
||||
|
||||
namespace costmap_converter
|
||||
{
|
||||
|
||||
//! Typedef for a shared dynamic obstacle container
|
||||
typedef costmap_converter_msgs::msg::ObstacleArrayMsg::SharedPtr ObstacleArrayPtr;
|
||||
//! Typedef for a shared dynamic obstacle container (read-only access)
|
||||
typedef costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr ObstacleArrayConstPtr;
|
||||
|
||||
//! Typedef for a shared polygon container
|
||||
typedef std::shared_ptr<std::vector<geometry_msgs::msg::Polygon>> PolygonContainerPtr;
|
||||
//! Typedef for a shared polygon container (read-only access)
|
||||
typedef std::shared_ptr<const std::vector<geometry_msgs::msg::Polygon>> PolygonContainerConstPtr;
|
||||
|
||||
|
||||
/**
|
||||
* @class BaseCostmapToPolygons
|
||||
* @brief This abstract class defines the interface for plugins that convert the costmap into polygon types
|
||||
*
|
||||
* Plugins must accept a nav2_costmap_2d::Costmap2D datatype as information source.
|
||||
* The interface provides two different use cases:
|
||||
* 1. Manual call to conversion routines: setCostmap2D(), compute() and getPolygons()
|
||||
* (in subsequent calls setCostmap2D() can be substituted by updateCostmap2D())
|
||||
* 2. Repeatedly process costmap with a specific rate (startWorker() and stopWorker()):
|
||||
* Make sure that the costmap is valid while the worker is active (you can specify your own spinner or activate a threaded spinner).
|
||||
* Costmaps can be obtained by invoking getPolygons().
|
||||
*/
|
||||
class BaseCostmapToPolygons
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* @brief Initialize the plugin
|
||||
* @param nh Nodehandle that defines the namespace for parameters
|
||||
*/
|
||||
virtual void initialize(rclcpp::Node::SharedPtr nh) {
|
||||
nh_ = nh;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Destructor
|
||||
*/
|
||||
virtual ~BaseCostmapToPolygons()
|
||||
{
|
||||
stopWorker();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Pass a pointer to the costap to the plugin.
|
||||
* @warning The plugin should handle the costmap's mutex locking.
|
||||
* @sa updateCostmap2D
|
||||
* @param costmap Pointer to the costmap2d source
|
||||
*/
|
||||
virtual void setCostmap2D(nav2_costmap_2d::Costmap2D* costmap) = 0;
|
||||
|
||||
/**
|
||||
* @brief Get updated data from the previously set Costmap2D
|
||||
* @warning The plugin should handle the costmap's mutex locking.
|
||||
* @sa setCostmap2D
|
||||
*/
|
||||
virtual void updateCostmap2D() = 0;
|
||||
|
||||
/**
|
||||
* @brief This method performs the actual work (conversion of the costmap to polygons)
|
||||
*/
|
||||
virtual void compute() = 0;
|
||||
|
||||
/**
|
||||
* @brief Get a shared instance of the current polygon container
|
||||
*
|
||||
* If this method is not implemented by any subclass (plugin) the returned shared
|
||||
* pointer is empty.
|
||||
* @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
|
||||
* @warning The underlying plugin must ensure that this method is thread safe.
|
||||
* @return Shared instance of the current polygon container
|
||||
*/
|
||||
virtual PolygonContainerConstPtr getPolygons(){return PolygonContainerConstPtr();}
|
||||
|
||||
/**
|
||||
* @brief Get a shared instance of the current obstacle container
|
||||
* If this method is not overwritten by the underlying plugin, the obstacle container just imports getPolygons().
|
||||
* @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
|
||||
* @warning The underlying plugin must ensure that this method is thread safe.
|
||||
* @return Shared instance of the current obstacle container
|
||||
* @sa getPolygons
|
||||
*/
|
||||
virtual ObstacleArrayConstPtr getObstacles()
|
||||
{
|
||||
ObstacleArrayPtr obstacles = std::make_shared<costmap_converter_msgs::msg::ObstacleArrayMsg>();
|
||||
PolygonContainerConstPtr polygons = getPolygons();
|
||||
if (polygons)
|
||||
{
|
||||
for (const geometry_msgs::msg::Polygon& polygon : *polygons)
|
||||
{
|
||||
obstacles->obstacles.emplace_back();
|
||||
obstacles->obstacles.back().polygon = polygon;
|
||||
}
|
||||
}
|
||||
return obstacles;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Set name of robot's odometry topic
|
||||
*
|
||||
* Some plugins might require odometry information
|
||||
* to compensate the robot's ego motion
|
||||
* @param odom_topic topic name
|
||||
*/
|
||||
virtual void setOdomTopic(const std::string& odom_topic) { (void)odom_topic; }
|
||||
|
||||
/**
|
||||
* @brief Determines whether an additional plugin for subsequent costmap conversion is specified
|
||||
*
|
||||
* @return false, since all plugins for static costmap conversion are independent
|
||||
*/
|
||||
virtual bool stackedCostmapConversion() {return false;}
|
||||
|
||||
/**
|
||||
* @brief Instantiate a worker that repeatedly coverts the most recent costmap to polygons.
|
||||
* The worker is implemented as a timer event that is invoked at a specific \c rate.
|
||||
* The passed \c costmap pointer must be valid at the complete time and must be lockable.
|
||||
* By specifying the argument \c spin_thread the timer event is invoked in a separate
|
||||
* thread and callback queue or otherwise it is called from the global callback queue (of the
|
||||
* node in which the plugin is used).
|
||||
* @param rate The rate that specifies how often the costmap should be updated
|
||||
* @param costmap Pointer to the underlying costmap (must be valid and lockable as long as the worker is active
|
||||
* @param spin_thread if \c true,the timer is invoked in a separate thread, otherwise in the default callback queue)
|
||||
*/
|
||||
void startWorker(rclcpp::Rate::SharedPtr rate, nav2_costmap_2d::Costmap2D* costmap, bool spin_thread = false)
|
||||
{
|
||||
setCostmap2D(costmap);
|
||||
|
||||
if (spin_thread_)
|
||||
{
|
||||
{
|
||||
std::lock_guard<std::mutex> terminate_lock(terminate_mutex_);
|
||||
need_to_terminate_ = true;
|
||||
}
|
||||
spin_thread_->join();
|
||||
delete spin_thread_;
|
||||
}
|
||||
|
||||
if (spin_thread)
|
||||
{
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "Spinning up a thread for the CostmapToPolygons plugin");
|
||||
need_to_terminate_ = false;
|
||||
|
||||
worker_timer_ = nh_->create_wall_timer(
|
||||
rate->period(),
|
||||
std::bind(&BaseCostmapToPolygons::workerCallback, this));
|
||||
spin_thread_ = new std::thread(std::bind(&BaseCostmapToPolygons::spinThread, this));
|
||||
}
|
||||
else
|
||||
{
|
||||
worker_timer_ = nh_->create_wall_timer(
|
||||
rate->period(),
|
||||
std::bind(&BaseCostmapToPolygons::workerCallback, this));
|
||||
spin_thread_ = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Stop the worker that repeatedly converts the costmap to polygons
|
||||
*/
|
||||
void stopWorker()
|
||||
{
|
||||
if (worker_timer_) worker_timer_->cancel();
|
||||
if (spin_thread_)
|
||||
{
|
||||
{
|
||||
std::lock_guard<std::mutex> terminate_lock(terminate_mutex_);
|
||||
need_to_terminate_ = true;
|
||||
}
|
||||
spin_thread_->join();
|
||||
delete spin_thread_;
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
* @brief Protected constructor that should be called by subclasses
|
||||
*/
|
||||
BaseCostmapToPolygons() : //nh_("~costmap_to_polygons"),
|
||||
nh_(nullptr),
|
||||
spin_thread_(nullptr), need_to_terminate_(false) {}
|
||||
|
||||
/**
|
||||
* @brief Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled)
|
||||
*/
|
||||
void spinThread()
|
||||
{
|
||||
rclcpp::WallRate r(10);
|
||||
while (rclcpp::ok())
|
||||
{
|
||||
{
|
||||
std::lock_guard<std::mutex> terminate_lock(terminate_mutex_);
|
||||
if (need_to_terminate_)
|
||||
break;
|
||||
rclcpp::spin_some(nh_);
|
||||
r.sleep();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The callback of the worker that performs the actual work (updating the costmap and converting it to polygons)
|
||||
*/
|
||||
void workerCallback()
|
||||
{
|
||||
updateCostmap2D();
|
||||
compute();
|
||||
}
|
||||
|
||||
rclcpp::Logger getLogger() const
|
||||
{
|
||||
return nh_->get_logger();
|
||||
}
|
||||
|
||||
rclcpp::Time now() const
|
||||
{
|
||||
return nh_->now();
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::TimerBase::SharedPtr worker_timer_;
|
||||
rclcpp::Node::SharedPtr nh_;
|
||||
std::thread* spin_thread_;
|
||||
std::mutex terminate_mutex_;
|
||||
bool need_to_terminate_;
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @class BaseCostmapToDynamicPolygons
|
||||
* @brief This class extends the BaseCostmapToPolygongs class for dynamic costmap conversion plugins in order to incorporate a subsequent costmap converter plugin for static obstacles
|
||||
*
|
||||
* This class should not be instantiated.
|
||||
*/
|
||||
class BaseCostmapToDynamicObstacles : public BaseCostmapToPolygons
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* @brief Load underlying static costmap conversion plugin via plugin-loader
|
||||
* @param plugin_name Exact class name of the plugin to be loaded, e.g.
|
||||
* costmap_converter::CostmapToPolygonsDBSMCCH
|
||||
* @param nh_parent NodeHandle which is extended by the namespace of the static conversion plugin
|
||||
*/
|
||||
void loadStaticCostmapConverterPlugin(const std::string& plugin_name, rclcpp::Node::SharedPtr nh_parent)
|
||||
{
|
||||
try
|
||||
{
|
||||
static_costmap_converter_ = static_converter_loader_.createSharedInstance(plugin_name);
|
||||
|
||||
if(std::dynamic_pointer_cast<BaseCostmapToDynamicObstacles>(static_costmap_converter_))
|
||||
{
|
||||
throw pluginlib::PluginlibException("The specified plugin for static costmap conversion is a dynamic plugin. Specify a static plugin.");
|
||||
}
|
||||
// std::string raw_plugin_name = static_converter_loader_.getName(plugin_name);
|
||||
static_costmap_converter_->initialize(nh_parent);
|
||||
setStaticCostmapConverterPlugin(static_costmap_converter_);
|
||||
RCLCPP_INFO(getLogger(), "CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles %s loaded.", plugin_name.c_str());
|
||||
}
|
||||
catch(const pluginlib::PluginlibException& ex)
|
||||
{
|
||||
RCLCPP_WARN(getLogger(), "CostmapToDynamicObstacles: The specified costmap converter plugin cannot be loaded. "
|
||||
"Continuing without subsequent conversion of static obstacles. Error message: %s", ex.what());
|
||||
static_costmap_converter_.reset();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Set the underlying plugin for subsequent costmap conversion of the static background of the costmap
|
||||
* @param static_costmap_converter shared pointer to the static costmap conversion plugin
|
||||
*/
|
||||
void setStaticCostmapConverterPlugin(std::shared_ptr<BaseCostmapToPolygons> static_costmap_converter)
|
||||
{
|
||||
static_costmap_converter_ = static_costmap_converter;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Set the costmap for the underlying plugin
|
||||
* @param static_costmap Costmap2D, which contains the static part of the original costmap
|
||||
*/
|
||||
void setStaticCostmap(std::shared_ptr<nav2_costmap_2d::Costmap2D> static_costmap)
|
||||
{
|
||||
static_costmap_converter_->setCostmap2D(static_costmap.get());
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Apply the underlying plugin for static costmap conversion
|
||||
*/
|
||||
void convertStaticObstacles()
|
||||
{
|
||||
static_costmap_converter_->compute();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the converted polygons from the underlying plugin
|
||||
* @return Shared instance of the underlying plugins polygon container
|
||||
*/
|
||||
PolygonContainerConstPtr getStaticPolygons()
|
||||
{
|
||||
return static_costmap_converter_->getPolygons();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Determines whether an additional plugin for subsequent costmap conversion is specified
|
||||
*
|
||||
* @return true, if a plugin for subsequent costmap conversion is specified
|
||||
*/
|
||||
bool stackedCostmapConversion()
|
||||
{
|
||||
if(static_costmap_converter_)
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Protected constructor that should be called by subclasses
|
||||
*/
|
||||
BaseCostmapToDynamicObstacles() : static_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), static_costmap_converter_() {}
|
||||
|
||||
private:
|
||||
pluginlib::ClassLoader<BaseCostmapToPolygons> static_converter_loader_;
|
||||
std::shared_ptr<BaseCostmapToPolygons> static_costmap_converter_;
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif // end COSTMAP_CONVERTER_INTERFACE_H_
|
||||
@ -1,115 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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.
|
||||
*
|
||||
* Notes:
|
||||
* The following code makes use of the OpenCV library.
|
||||
* OpenCV is licensed under the terms of the 3-clause BSD License.
|
||||
*
|
||||
* Authors: Franz Albers, Christoph Rösmann
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef BACKGROUNDSUBTRACTOR_H_
|
||||
#define BACKGROUNDSUBTRACTOR_H_
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
/**
|
||||
* @class BackgroundSubtractor
|
||||
* @brief Perform background subtraction to extract the "moving" foreground
|
||||
*
|
||||
* This class is based on OpenCV's background subtraction class cv::BackgroundSubtractor.
|
||||
* It has been modified in order to incorporate a specialized bandpass filter.
|
||||
*
|
||||
* See http://docs.opencv.org/3.2.0/d7/df6/classcv_1_1BackgroundSubtractor.html for the original class.
|
||||
*/
|
||||
class BackgroundSubtractor
|
||||
{
|
||||
public:
|
||||
struct Params
|
||||
{
|
||||
double alpha_slow; //!< Filter constant (learning rate) of the slow filter part
|
||||
double alpha_fast; //!< Filter constant (learning rate) of the fast filter part
|
||||
double beta;
|
||||
double min_sep_between_fast_and_slow_filter;
|
||||
double min_occupancy_probability;
|
||||
double max_occupancy_neighbors;
|
||||
int morph_size;
|
||||
};
|
||||
|
||||
//! Constructor that accepts a specific parameter configuration
|
||||
BackgroundSubtractor(const Params& parameters);
|
||||
|
||||
/**
|
||||
* @brief Computes a foreground mask
|
||||
* @param[in] image Next video frame
|
||||
* @param[out] fg_mask Foreground mask as an 8-bit binary image
|
||||
* @param[in] shift_x Translation along the x axis between the current and previous image
|
||||
* @param[in] shift_y Translation along the y axis between the current and previous image
|
||||
*/
|
||||
void apply(const cv::Mat& image, cv::Mat& fg_mask, int shift_x = 0, int shift_y = 0);
|
||||
|
||||
/**
|
||||
* @brief OpenCV Visualization
|
||||
* @param name Id/name of the opencv window
|
||||
* @param image Image to be visualized
|
||||
*/
|
||||
void visualize(const std::string& name, const cv::Mat& image);
|
||||
|
||||
/**
|
||||
* @brief Export vector of matrices to yaml file
|
||||
* @remarks This method is intended for debugging purposes
|
||||
* @param filename Desired filename including path and excluding file suffix
|
||||
* @param mat_vec Vector of cv::Mat to be exported
|
||||
*/
|
||||
void writeMatToYAML(const std::string& filename, const std::vector<cv::Mat>& mat_vec);
|
||||
|
||||
//! Update internal parameters
|
||||
void updateParameters(const Params& parameters);
|
||||
|
||||
private:
|
||||
//! Transform/shift all internal matrices/grids according to a given translation vector
|
||||
void transformToCurrentFrame(int shift_x, int shift_y);
|
||||
|
||||
cv::Mat occupancy_grid_fast_;
|
||||
cv::Mat occupancy_grid_slow_;
|
||||
cv::Mat current_frame_;
|
||||
|
||||
int previous_shift_x_;
|
||||
int previous_shift_y_;
|
||||
|
||||
Params params_;
|
||||
};
|
||||
|
||||
#endif // BACKGROUNDSUBTRACTOR_H_
|
||||
@ -1,111 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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.
|
||||
*
|
||||
* Notes:
|
||||
* The following code makes use of the OpenCV library.
|
||||
* OpenCV is licensed under the terms of the 3-clause BSD License.
|
||||
*
|
||||
* Authors: Franz Albers, Christoph Rösmann
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef BLOBDETECTOR_H_
|
||||
#define BLOBDETECTOR_H_
|
||||
|
||||
// Basically the OpenCV SimpleBlobDetector, extended with getContours()
|
||||
|
||||
#include <opencv2/features2d/features2d.hpp>
|
||||
|
||||
/**
|
||||
* @class BlobDetector
|
||||
* @brief Detect blobs in image (specialized for dynamic obstacles in the costmap)
|
||||
*
|
||||
* This class is based on OpenCV's blob detector cv::SimpleBlobDetector.
|
||||
* It has been modified and specialized for dynamic obstacle tracking in the costmap:
|
||||
* -> The modified version also returns contours of the blob.
|
||||
*
|
||||
* See http://docs.opencv.org/trunk/d0/d7a/classcv_1_1SimpleBlobDetector.html for the original class.
|
||||
*/
|
||||
class BlobDetector : public cv::SimpleBlobDetector
|
||||
{
|
||||
public:
|
||||
//! Default constructor which optionally accepts custom parameters
|
||||
BlobDetector(const cv::SimpleBlobDetector::Params& parameters = cv::SimpleBlobDetector::Params());
|
||||
|
||||
//! Create shared instance of the blob detector with given parameters
|
||||
static cv::Ptr<BlobDetector> create(const BlobDetector::Params& params);
|
||||
|
||||
/**
|
||||
* @brief Detects keypoints in an image and extracts contours
|
||||
*
|
||||
* In contrast to the original detect method, this extended version
|
||||
* also extracts contours. Contours can be accessed by getContours()
|
||||
* after invoking this method.
|
||||
*
|
||||
* @todo The mask option is currently not implemented.
|
||||
*
|
||||
* @param image image
|
||||
* @param keypoints The detected keypoints.
|
||||
* @param mask Mask specifying where to look for keypoints (optional). It must be a 8-bit integer
|
||||
* matrix with non-zero values in the region of interest.
|
||||
*/
|
||||
virtual void detect(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints,
|
||||
const cv::Mat& mask = cv::Mat());
|
||||
|
||||
/**
|
||||
* @brief Access contours extracted during detection stage
|
||||
* @return Read-only reference to the contours set of the previous detect() run
|
||||
*/
|
||||
const std::vector<std::vector<cv::Point>>& getContours() { return contours_; }
|
||||
|
||||
//! Update internal parameters
|
||||
void updateParameters(const cv::SimpleBlobDetector::Params& parameters);
|
||||
|
||||
protected:
|
||||
struct Center
|
||||
{
|
||||
cv::Point2d location;
|
||||
double radius;
|
||||
double confidence;
|
||||
};
|
||||
|
||||
virtual void findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector<Center>& centers,
|
||||
std::vector<std::vector<cv::Point>>& cur_contours) const;
|
||||
|
||||
std::vector<std::vector<cv::Point>> contours_;
|
||||
|
||||
Params params_;
|
||||
};
|
||||
|
||||
#endif // BLOBDETECTOR_H_
|
||||
@ -1,212 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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.
|
||||
*
|
||||
* Notes:
|
||||
* The following code makes use of the OpenCV library.
|
||||
* OpenCV is licensed under the terms of the 3-clause BSD License.
|
||||
*
|
||||
* Authors: Franz Albers, Christoph Rösmann
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef COSTMAP_TO_DYNAMIC_OBSTACLES_H_
|
||||
#define COSTMAP_TO_DYNAMIC_OBSTACLES_H_
|
||||
|
||||
// ROS
|
||||
#include <costmap_converter/costmap_converter_interface.h>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <pluginlib/class_loader.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
// OpenCV
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <opencv2/features2d/features2d.hpp>
|
||||
#include <opencv2/video/tracking.hpp>
|
||||
|
||||
// dynamic reconfigure
|
||||
//#include <costmap_converter/CostmapToDynamicObstaclesConfig.h>
|
||||
//#include <dynamic_reconfigure/server.h>
|
||||
|
||||
// Own includes
|
||||
#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h>
|
||||
#include <costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h>
|
||||
#include <costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h>
|
||||
|
||||
// STL
|
||||
#include <memory>
|
||||
|
||||
namespace costmap_converter {
|
||||
|
||||
/**
|
||||
* @class CostmapToDynamicObstacles
|
||||
* @brief This class converts the costmap_2d into dynamic obstacles.
|
||||
*
|
||||
* Static obstacles are treated as point obstacles.
|
||||
*/
|
||||
class CostmapToDynamicObstacles : public BaseCostmapToDynamicObstacles
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor
|
||||
*/
|
||||
CostmapToDynamicObstacles();
|
||||
|
||||
/**
|
||||
* @brief Destructor
|
||||
*/
|
||||
virtual ~CostmapToDynamicObstacles();
|
||||
|
||||
/**
|
||||
* @brief Initialize the plugin
|
||||
* @param nh Nodehandle that defines the namespace for parameters
|
||||
*/
|
||||
virtual void initialize(rclcpp::Node::SharedPtr nh);
|
||||
|
||||
/**
|
||||
* @brief This method performs the actual work (conversion of the costmap to
|
||||
* obstacles)
|
||||
*/
|
||||
virtual void compute();
|
||||
|
||||
/**
|
||||
* @brief Pass a pointer to the costmap to the plugin.
|
||||
* @sa updateCostmap2D
|
||||
* @param costmap Pointer to the costmap2d source
|
||||
*/
|
||||
virtual void setCostmap2D(nav2_costmap_2d::Costmap2D* costmap);
|
||||
|
||||
/**
|
||||
* @brief Get updated data from the previously set Costmap2D
|
||||
* @sa setCostmap2D
|
||||
*/
|
||||
virtual void updateCostmap2D();
|
||||
|
||||
/**
|
||||
* @brief Get a shared instance of the current obstacle container
|
||||
* @remarks If compute() or startWorker() has not been called before, this
|
||||
* method returns an empty instance!
|
||||
* @return Shared instance of the current obstacle container
|
||||
*/
|
||||
ObstacleArrayConstPtr getObstacles();
|
||||
|
||||
/**
|
||||
* @brief Set name of robot's odometry topic
|
||||
*
|
||||
* @warning The method must be called before initialize()
|
||||
*
|
||||
* Some plugins might require odometry information
|
||||
* to compensate the robot's ego motion
|
||||
* @param odom_topic topic name
|
||||
*/
|
||||
virtual void setOdomTopic(const std::string& odom_topic)
|
||||
{
|
||||
odom_topic_ = odom_topic;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief OpenCV Visualization
|
||||
* @param name Id/name of the opencv window
|
||||
* @param image Image to be visualized
|
||||
*/
|
||||
void visualize(const std::string& name, const cv::Mat& image);
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Converts the estimated velocity of a tracked object to m/s and
|
||||
* returns it
|
||||
* @param idx Index of the tracked object in the tracker
|
||||
* @return Point_t, which represents velocity in [m/s] of object(idx) in x,y,z
|
||||
* coordinates
|
||||
*/
|
||||
Point_t getEstimatedVelocityOfObject(unsigned int idx);
|
||||
|
||||
/**
|
||||
* @brief Gets the last observed contour of a object and converts it from [px]
|
||||
* to [m]
|
||||
* @param[in] idx Index of the tracked object in the tracker
|
||||
* @param[out] contour vector of Point_t, which represents the last observed contour in [m]
|
||||
* in x,y,z coordinates
|
||||
*/
|
||||
void getContour(unsigned int idx, std::vector<Point_t>& contour);
|
||||
|
||||
/**
|
||||
* @brief Thread-safe update of the internal obstacle container (that is
|
||||
* shared using getObstacles() from outside this
|
||||
* class)
|
||||
* @param obstacles Updated obstacle container
|
||||
*/
|
||||
void updateObstacleContainer(ObstacleArrayPtr obstacles);
|
||||
|
||||
private:
|
||||
std::mutex mutex_;
|
||||
nav2_costmap_2d::Costmap2D* costmap_;
|
||||
cv::Mat costmap_mat_;
|
||||
ObstacleArrayPtr obstacles_;
|
||||
cv::Mat fg_mask_;
|
||||
std::unique_ptr<BackgroundSubtractor> bg_sub_;
|
||||
cv::Ptr<BlobDetector> blob_det_;
|
||||
std::vector<cv::KeyPoint> keypoints_;
|
||||
std::unique_ptr<CTracker> tracker_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||
Point_t ego_vel_;
|
||||
|
||||
std::string odom_topic_ = "/odom";
|
||||
bool publish_static_obstacles_ = true;
|
||||
|
||||
// dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>*
|
||||
// dynamic_recfg_; //!< Dynamic reconfigure server to allow config
|
||||
// //! modifications at runtime
|
||||
|
||||
/**
|
||||
* @brief Callback for the odometry messages of the observing robot.
|
||||
*
|
||||
* Used to convert the velocity of obstacles to the /map frame.
|
||||
* @param msg The Pointer to the nav_msgs::Odometry of the observing robot
|
||||
*/
|
||||
void odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
|
||||
|
||||
/**
|
||||
* @brief Callback for the dynamic_reconfigure node.
|
||||
*
|
||||
* This callback allows to modify parameters dynamically at runtime without
|
||||
* restarting the node
|
||||
* @param config Reference to the dynamic reconfigure config
|
||||
* @param level Dynamic reconfigure level
|
||||
*/
|
||||
// void reconfigureCB(CostmapToDynamicObstaclesConfig &config, uint32_t level);
|
||||
};
|
||||
|
||||
} // end namespace costmap_converter
|
||||
|
||||
#endif /* COSTMAP_TO_DYNAMIC_OBSTACLES_H_ */
|
||||
@ -1,96 +0,0 @@
|
||||
// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
|
||||
// Refer to README.md in this directory.
|
||||
|
||||
#pragma once
|
||||
#include "Kalman.h"
|
||||
#include "HungarianAlg.h"
|
||||
#include "defines.h"
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <array>
|
||||
|
||||
// --------------------------------------------------------------------------
|
||||
class CTrack
|
||||
{
|
||||
public:
|
||||
CTrack(const Point_t& p, const std::vector<cv::Point>& contour, track_t dt, size_t trackID)
|
||||
: track_id(trackID),
|
||||
skipped_frames(0),
|
||||
prediction(p),
|
||||
lastContour(contour),
|
||||
KF(p, dt)
|
||||
{
|
||||
}
|
||||
|
||||
track_t CalcDist(const Point_t& p)
|
||||
{
|
||||
Point_t diff = prediction - p;
|
||||
return std::sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z);
|
||||
}
|
||||
|
||||
void Update(const Point_t& p, const std::vector<cv::Point>& contour, bool dataCorrect, size_t max_trace_length)
|
||||
{
|
||||
KF.Prediction();
|
||||
prediction = KF.Update(p, dataCorrect);
|
||||
|
||||
if (dataCorrect)
|
||||
{
|
||||
lastContour = contour;
|
||||
}
|
||||
|
||||
if (trace.size() > max_trace_length)
|
||||
{
|
||||
trace.erase(trace.begin(), trace.end() - max_trace_length);
|
||||
}
|
||||
|
||||
trace.push_back(prediction);
|
||||
}
|
||||
|
||||
// Returns contour in [px], not in [m]!
|
||||
std::vector<cv::Point> getLastContour() const
|
||||
{
|
||||
return lastContour;
|
||||
}
|
||||
|
||||
// Returns velocity in [px/s], not in [m/s]!
|
||||
Point_t getEstimatedVelocity() const
|
||||
{
|
||||
return KF.LastVelocity;
|
||||
}
|
||||
|
||||
std::vector<Point_t> trace;
|
||||
size_t track_id;
|
||||
size_t skipped_frames;
|
||||
|
||||
private:
|
||||
Point_t prediction;
|
||||
std::vector<cv::Point> lastContour; // Contour liegt immer auf ganzen Pixeln -> Integer Punkt
|
||||
TKalmanFilter KF;
|
||||
};
|
||||
|
||||
// --------------------------------------------------------------------------
|
||||
class CTracker
|
||||
{
|
||||
public:
|
||||
struct Params{
|
||||
track_t dt; // time for one step of the filter
|
||||
track_t dist_thresh;// distance threshold. Pairs of points with higher distance are not considered in the assignment problem
|
||||
int max_allowed_skipped_frames; // Maximum number of frames the track is maintained without seeing the object in the measurement data
|
||||
int max_trace_length; // Maximum trace length
|
||||
};
|
||||
|
||||
CTracker(const Params& parameters);
|
||||
~CTracker(void);
|
||||
|
||||
std::vector<std::unique_ptr<CTrack>> tracks;
|
||||
void Update(const std::vector<Point_t>& detectedCentroid, const std::vector<std::vector<cv::Point> > &contour);
|
||||
|
||||
void updateParameters(const Params ¶meters);
|
||||
|
||||
private:
|
||||
// Aggregated parameters for the tracker object
|
||||
Params params;
|
||||
// ID for the upcoming CTrack object
|
||||
size_t NextTrackID;
|
||||
};
|
||||
@ -1,60 +0,0 @@
|
||||
// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
|
||||
// Refer to README.md in this directory.
|
||||
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <time.h>
|
||||
#include "defines.h"
|
||||
|
||||
// http://community.topcoder.com/tc?module=Static&d1=tutorials&d2=hungarianAlgorithm
|
||||
|
||||
typedef std::vector<int> assignments_t;
|
||||
typedef std::vector<track_t> distMatrix_t;
|
||||
|
||||
class AssignmentProblemSolver
|
||||
{
|
||||
private:
|
||||
// --------------------------------------------------------------------------
|
||||
// Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
|
||||
// --------------------------------------------------------------------------
|
||||
void assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,
|
||||
size_t nOfColumns);
|
||||
void buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows, size_t nOfColumns);
|
||||
void computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn,
|
||||
size_t nOfRows);
|
||||
void step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
|
||||
bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
|
||||
void step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
|
||||
bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
|
||||
void step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
|
||||
bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
|
||||
void step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
|
||||
bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row,
|
||||
size_t col);
|
||||
void step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
|
||||
bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
|
||||
// --------------------------------------------------------------------------
|
||||
// Computes a suboptimal solution. Good for cases with many forbidden assignments.
|
||||
// --------------------------------------------------------------------------
|
||||
void assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,
|
||||
size_t nOfColumns);
|
||||
// --------------------------------------------------------------------------
|
||||
// Computes a suboptimal solution. Good for cases with many forbidden assignments.
|
||||
// --------------------------------------------------------------------------
|
||||
void assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,
|
||||
size_t nOfColumns);
|
||||
|
||||
public:
|
||||
enum TMethod
|
||||
{
|
||||
optimal,
|
||||
many_forbidden_assignments,
|
||||
without_forbidden_assignments
|
||||
};
|
||||
|
||||
AssignmentProblemSolver();
|
||||
~AssignmentProblemSolver();
|
||||
track_t Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns, assignments_t& assignment,
|
||||
TMethod Method = optimal);
|
||||
};
|
||||
@ -1,20 +0,0 @@
|
||||
// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
|
||||
// Refer to README.md in this directory.
|
||||
|
||||
#pragma once
|
||||
#include "defines.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
// http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/
|
||||
class TKalmanFilter
|
||||
{
|
||||
public:
|
||||
TKalmanFilter(Point_t p, track_t deltatime = 0.2);
|
||||
~TKalmanFilter();
|
||||
void Prediction();
|
||||
Point_t Update(Point_t p, bool DataCorrect);
|
||||
cv::KalmanFilter* kalman;
|
||||
track_t dt;
|
||||
Point_t LastPosition; // contour in [px]
|
||||
Point_t LastVelocity; // velocity in [px/s]
|
||||
};
|
||||
@ -1,13 +0,0 @@
|
||||
Multitarget Tracker
|
||||
===================
|
||||
|
||||
This code is mostly copied from the (MultitargetTracker)[https://github.com/Smorodov/Multitarget-tracker].
|
||||
It is utilized for the *CostmapToDynamicObstacles* plugin.
|
||||
|
||||
The *MultitargetTracker* is licensed under (GNU GPLv3)[https://www.gnu.org/licenses/gpl-3.0.txt].
|
||||
The package itself depends on other third party packages with different licenses (refer to the package repository).
|
||||
|
||||
TODO: Include the whole package as external CMake project.
|
||||
|
||||
|
||||
|
||||
@ -1,9 +0,0 @@
|
||||
// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
|
||||
// Refer to README.md in this directory.
|
||||
|
||||
#pragma once
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
typedef float track_t;
|
||||
typedef cv::Point3_<track_t> Point_t;
|
||||
#define Mat_t CV_32FC
|
||||
@ -1,137 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann, Otniel Rinaldo
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef COSTMAP_TO_LINES_CONVEX_HULL_H_
|
||||
#define COSTMAP_TO_LINES_CONVEX_HULL_H_
|
||||
|
||||
#include <costmap_converter/costmap_converter_interface.h>
|
||||
#include <costmap_converter/costmap_to_polygons.h>
|
||||
|
||||
// dynamic reconfigure
|
||||
//#include <costmap_converter/CostmapToLinesDBSMCCHConfig.h>
|
||||
|
||||
namespace costmap_converter
|
||||
{
|
||||
|
||||
/**
|
||||
* @class CostmapToLinesDBSMCCH
|
||||
* @brief This class converts the costmap_2d into a set of lines (and points)
|
||||
*
|
||||
* The conversion is performed in three stages:
|
||||
* 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
|
||||
* Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
|
||||
* A density-based algorithm for discovering clusters in large spatial databases with noise.
|
||||
* Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9.
|
||||
*
|
||||
* 2. In the subsequent stage, the convex hull of each cluster is determined using the monotone chain algorithm aka Andrew's algorithm:
|
||||
* C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )
|
||||
* Reference: A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).
|
||||
*
|
||||
* 3. In the third step extract lines from each polygon (convex hull) if there exist at least a predefined number of support points.
|
||||
*
|
||||
* The output is a container of polygons (but each polygon does only contain a single vertex (point) or two vertices (line)
|
||||
*/
|
||||
class CostmapToLinesDBSMCCH : public CostmapToPolygonsDBSMCCH
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
*/
|
||||
CostmapToLinesDBSMCCH();
|
||||
|
||||
/**
|
||||
* @brief Destructor
|
||||
*/
|
||||
virtual ~CostmapToLinesDBSMCCH();
|
||||
|
||||
/**
|
||||
* @brief Initialize the plugin
|
||||
* @param nh Nodehandle that defines the namespace for parameters
|
||||
*/
|
||||
virtual void initialize(rclcpp::Node::SharedPtr nh);
|
||||
|
||||
/**
|
||||
* @brief This method performs the actual work (conversion of the costmap to polygons)
|
||||
*/
|
||||
virtual void compute();
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
* @brief Extract points and lines from a given cluster by checking all keypoint pairs of the convex hull for a minimum number of support points
|
||||
* @param cluster list of points in the cluster
|
||||
* @param polygon convex hull of the cluster \c cluster
|
||||
* @param[out] lines back_inserter object to a sequence of polygon msgs (new lines will be pushed back)
|
||||
*/
|
||||
void extractPointsAndLines(std::vector<KeyPoint>& cluster, const geometry_msgs::msg::Polygon& polygon, std::back_insert_iterator< std::vector<geometry_msgs::msg::Polygon> > lines);
|
||||
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
double support_pts_max_dist_inbetween_;
|
||||
double support_pts_max_dist_;
|
||||
int min_support_pts_;
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* @brief Callback for the dynamic_reconfigure node.
|
||||
*
|
||||
* This callback allows to modify parameters dynamically at runtime without restarting the node
|
||||
* @param config Reference to the dynamic reconfigure config
|
||||
* @param level Dynamic reconfigure level
|
||||
*/
|
||||
// void reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level);
|
||||
|
||||
// dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
} //end namespace teb_local_planner
|
||||
|
||||
#endif /* COSTMAP_TO_LINES_CONVEX_HULL_H_ */
|
||||
@ -1,187 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef COSTMAP_TO_LINES_RANSAC_H_
|
||||
#define COSTMAP_TO_LINES_RANSAC_H_
|
||||
|
||||
#include <costmap_converter/costmap_converter_interface.h>
|
||||
#include <costmap_converter/costmap_to_polygons.h>
|
||||
#include <costmap_converter/misc.h>
|
||||
|
||||
#include <random>
|
||||
|
||||
//#include <costmap_converter/CostmapToLinesDBSRANSACConfig.h>
|
||||
|
||||
namespace costmap_converter
|
||||
{
|
||||
|
||||
/**
|
||||
* @class CostmapToLinesDBSRANSAC
|
||||
* @brief This class converts the costmap_2d into a set of lines (and points)
|
||||
*
|
||||
* The conversion is performed in two stages:
|
||||
* 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
|
||||
* Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
|
||||
* A density-based algorithm for discovering clusters in large spatial databases with noise.
|
||||
* Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9.
|
||||
*
|
||||
* 2. The RANSAC algorithm is used to find straight line segment models (https://en.wikipedia.org/wiki/RANSAC)
|
||||
* RANSAC is called repeatedly to find multiple lines per cluster until the number of inliners is below a specific threshold.
|
||||
* In that case the remaining outliers are used as points or keypoints of their convex hull are used as points (depending on a paramter).
|
||||
* The latter one applies as a filter. The convex assumption is not strict in practice, since the remaining regions/cluster (line inliers are removed)
|
||||
* should be dense and small. For details about the convex hull algorithm, refer to costmap_converter::CostmapToPolygonsDBSMCCH.
|
||||
* Resulting lines of RANSAC are currently not refined by linear regression.
|
||||
*
|
||||
* The output is a container of polygons (but each polygon does only contain a single vertex (point) or two vertices (line)
|
||||
*/
|
||||
class CostmapToLinesDBSRANSAC : public CostmapToPolygonsDBSMCCH
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
*/
|
||||
CostmapToLinesDBSRANSAC();
|
||||
|
||||
/**
|
||||
* @brief Destructor
|
||||
*/
|
||||
virtual ~CostmapToLinesDBSRANSAC();
|
||||
|
||||
/**
|
||||
* @brief Initialize the plugin
|
||||
* @param nh Nodehandle that defines the namespace for parameters
|
||||
*/
|
||||
virtual void initialize(rclcpp::Node::SharedPtr nh);
|
||||
|
||||
/**
|
||||
* @brief This method performs the actual work (conversion of the costmap to polygons)
|
||||
*/
|
||||
virtual void compute();
|
||||
|
||||
|
||||
/**
|
||||
* @brief Check if the candidate point is an inlier.
|
||||
* @param point generic 2D point type defining the reference point
|
||||
* @param line_start generic 2D point type defining the start of the line
|
||||
* @param line_end generic 2D point type defining the end of the line
|
||||
* @param min_distance minimum distance allowed
|
||||
* @tparam Point generic point type that should provide (writable) x and y member fiels.
|
||||
* @tparam LinePoint generic point type that should provide (writable) x and y member fiels.
|
||||
* @return \c true if inlier, \c false otherwise
|
||||
*/
|
||||
template <typename Point, typename LinePoint>
|
||||
static bool isInlier(const Point& point, const LinePoint& line_start, const LinePoint& line_end, double min_distance);
|
||||
|
||||
protected:
|
||||
|
||||
double ransac_inlier_distance_; //!< Maximum distance to the line segment for inliers
|
||||
int ransac_min_inliers_; //!< Minimum numer of inliers required to form a line
|
||||
int ransac_no_iterations_; //!< Number of ransac iterations
|
||||
int ransac_remainig_outliers_; //!< Repeat ransac until the number of outliers is as specified here
|
||||
bool ransac_convert_outlier_pts_; //!< If \c true, convert remaining outliers to single points.
|
||||
bool ransac_filter_remaining_outlier_pts_; //!< If \c true, filter the interior of remaining outliers and keep only keypoints of their convex hull
|
||||
|
||||
|
||||
std::mt19937 rnd_generator_; //!< Random number generator for ransac with default seed
|
||||
|
||||
|
||||
/**
|
||||
* @brief Find a straight line segment in a point cloud with RANSAC (without linear regression).
|
||||
* @param data set of 2D data points
|
||||
* @param inlier_distance maximum distance that inliers must satisfy
|
||||
* @param no_iterations number of RANSAC iterations
|
||||
* @param min_inliers minimum number of inliers to return true
|
||||
* @param[out] best_model start and end of the best straight line segment
|
||||
* @param[out] inliers inlier keypoints are written to this container [optional]
|
||||
* @param[out] outliers outlier keypoints are written to this container [optional]
|
||||
* @return \c false, if \c min_inliers is not satisfied, \c true otherwise
|
||||
*/
|
||||
bool lineRansac(const std::vector<KeyPoint>& data, double inlier_distance, int no_iterations, int min_inliers,
|
||||
std::pair<KeyPoint, KeyPoint>& best_model, std::vector<KeyPoint>* inliers = NULL,
|
||||
std::vector<KeyPoint>* outliers = NULL);
|
||||
|
||||
/**
|
||||
* @brief Perform a simple linear regression in order to fit a straight line 'y = slope*x + intercept'
|
||||
* @param data set of 2D data points
|
||||
* @param[out] slope The slope of the fitted line
|
||||
* @param[out] intercept The intercept / offset of the line
|
||||
* @param[out] mean_x_out mean of the x-values of the data [optional]
|
||||
* @param[out] mean_y_out mean of the y-values of the data [optional]
|
||||
* @return \c true, if a valid line has been fitted, \c false otherwise.
|
||||
*/
|
||||
bool linearRegression(const std::vector<KeyPoint>& data, double& slope, double& intercept,
|
||||
double* mean_x_out = NULL, double* mean_y_out = NULL);
|
||||
|
||||
|
||||
|
||||
// void adjustLineLength(const std::vector<KeyPoint>& data, const KeyPoint& linept1, const KeyPoint& linept2,
|
||||
// KeyPoint& line_start, KeyPoint& line_end);
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* @brief Callback for the dynamic_reconfigure node.
|
||||
*
|
||||
* This callback allows to modify parameters dynamically at runtime without restarting the node
|
||||
* @param config Reference to the dynamic reconfigure config
|
||||
* @param level Dynamic reconfigure level
|
||||
*/
|
||||
// void reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level);
|
||||
|
||||
// dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
|
||||
};
|
||||
|
||||
|
||||
|
||||
template <typename Point, typename LinePoint>
|
||||
bool CostmapToLinesDBSRANSAC::isInlier(const Point& point, const LinePoint& line_start, const LinePoint& line_end, double min_distance)
|
||||
{
|
||||
bool is_inbetween = false;
|
||||
double distance = computeDistanceToLineSegment(point, line_start, line_end, &is_inbetween);
|
||||
if (!is_inbetween)
|
||||
return false;
|
||||
if (distance <= min_distance)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
} //end namespace teb_local_planner
|
||||
|
||||
#endif /* COSTMAP_TO_LINES_RANSAC_H_ */
|
||||
@ -1,335 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann, Otniel Rinaldo
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef COSTMAP_TO_POLYGONS_H_
|
||||
#define COSTMAP_TO_POLYGONS_H_
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <costmap_converter/costmap_converter_interface.h>
|
||||
#include <nav_msgs/msg/occupancy_grid.hpp>
|
||||
#include <visualization_msgs/msg/marker.hpp>
|
||||
#include <geometry_msgs/msg/point.hpp>
|
||||
#include <geometry_msgs/msg/polygon.hpp>
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
#include <mutex>
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/StdVector>
|
||||
|
||||
// dynamic reconfigure
|
||||
//#include <costmap_converter/CostmapToPolygonsDBSMCCHConfig.h>
|
||||
//#include <dynamic_reconfigure/server.h>
|
||||
|
||||
|
||||
namespace costmap_converter
|
||||
{
|
||||
|
||||
/**
|
||||
* @class CostmapToPolygonsDBSMCCH
|
||||
* @brief This class converts the costmap_2d into a set of convex polygons (and points)
|
||||
*
|
||||
* The conversion is performed in two stages:
|
||||
* 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
|
||||
* Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
|
||||
* A density-based algorithm for discovering clusters in large spatial databases with noise.
|
||||
* Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9.
|
||||
*
|
||||
* 2. In the subsequent stage, clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm:
|
||||
* C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )
|
||||
* Reference: A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).
|
||||
*
|
||||
* All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex)
|
||||
*/
|
||||
class CostmapToPolygonsDBSMCCH : public BaseCostmapToPolygons
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* @struct KeyPoint
|
||||
* @brief Defines a keypoint in metric coordinates of the map
|
||||
*/
|
||||
struct KeyPoint
|
||||
{
|
||||
//! Default constructor
|
||||
KeyPoint() : x(0.0), y(0.0) {}
|
||||
//! Constructor with point initialization
|
||||
KeyPoint(double x_, double y_) : x(x_), y(y_) {}
|
||||
double x; //!< x coordinate [m]
|
||||
double y; //!< y coordinate [m]
|
||||
|
||||
//! Convert keypoint to geometry_msgs::msg::Point message type
|
||||
void toPointMsg(geometry_msgs::msg::Point& point) const {point.x=x; point.y=y; point.z=0;}
|
||||
//! Convert keypoint to geometry_msgs::msg::Point32 message type
|
||||
void toPointMsg(geometry_msgs::msg::Point32& point) const {point.x=x; point.y=y; point.z=0;}
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct Parameters
|
||||
* @brief Defines the parameters of the algorithm
|
||||
*/
|
||||
struct Parameters
|
||||
{
|
||||
Parameters() : max_distance_(0.4), min_pts_(2), max_pts_(30), min_keypoint_separation_(0.1) {}
|
||||
// DBSCAN parameters
|
||||
double max_distance_; //!< Parameter for DB_Scan, maximum distance to neighbors [m]
|
||||
int min_pts_; //!< Parameter for DB_Scan: minimum number of points that define a cluster
|
||||
int max_pts_; //!< Parameter for DB_Scan: maximum number of points that define a cluster (to avoid large L- and U-shapes)
|
||||
|
||||
// convex hull parameters
|
||||
double min_keypoint_separation_; //!< Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
*/
|
||||
CostmapToPolygonsDBSMCCH();
|
||||
|
||||
/**
|
||||
* @brief Destructor
|
||||
*/
|
||||
virtual ~CostmapToPolygonsDBSMCCH();
|
||||
|
||||
/**
|
||||
* @brief Initialize the plugin
|
||||
* @param nh Nodehandle that defines the namespace for parameters
|
||||
*/
|
||||
virtual void initialize(rclcpp::Node::SharedPtr nh) override;
|
||||
|
||||
/**
|
||||
* @brief This method performs the actual work (conversion of the costmap to polygons)
|
||||
*/
|
||||
virtual void compute();
|
||||
|
||||
/**
|
||||
* @brief Pass a pointer to the costap to the plugin.
|
||||
* @sa updateCostmap2D
|
||||
* @param costmap Pointer to the costmap2d source
|
||||
*/
|
||||
virtual void setCostmap2D(nav2_costmap_2d::Costmap2D* costmap);
|
||||
|
||||
/**
|
||||
* @brief Get updated data from the previously set Costmap2D
|
||||
* @sa setCostmap2D
|
||||
*/
|
||||
virtual void updateCostmap2D();
|
||||
|
||||
|
||||
/**
|
||||
* @brief Convert a generi point type to a geometry_msgs::msg::Polygon
|
||||
* @param point generic 2D point type
|
||||
* @param[out] polygon already instantiated polygon that will be filled with a single point
|
||||
* @tparam Point generic point type that should provide (writable) x and y member fiels.
|
||||
*/
|
||||
template< typename Point>
|
||||
static void convertPointToPolygon(const Point& point, geometry_msgs::msg::Polygon& polygon)
|
||||
{
|
||||
polygon.points.resize(1);
|
||||
polygon.points.front().x = point.x;
|
||||
polygon.points.front().y = point.y;
|
||||
polygon.points.front().z = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get a shared instance of the current polygon container
|
||||
* @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
|
||||
* @return Shared instance of the current polygon container
|
||||
*/
|
||||
PolygonContainerConstPtr getPolygons();
|
||||
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
* @brief DBSCAN algorithm for clustering
|
||||
*
|
||||
* Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
|
||||
* Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
|
||||
* A density-based algorithm for discovering clusters in large spatial databases with noise.
|
||||
* Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9.
|
||||
*
|
||||
* @param[out] clusters clusters will added to this output-argument (a sequence of keypoints for each cluster)
|
||||
* the first cluster (clusters[0]) will contain all noise points (that does not fulfil the min_pts condition
|
||||
*/
|
||||
void dbScan(std::vector< std::vector<KeyPoint> >& clusters);
|
||||
|
||||
/**
|
||||
* @brief Helper function for dbScan to search for neighboring points
|
||||
*
|
||||
* @param curr_index index to the current item in \c occupied_cells
|
||||
* @param[out] neighbor_indices list of neighbors (indices of \c occupied cells)
|
||||
*/
|
||||
void regionQuery(int curr_index, std::vector<int>& neighbor_indices);
|
||||
|
||||
/**
|
||||
* @brief helper function for adding a point to the lookup data structures
|
||||
*/
|
||||
void addPoint(double x, double y)
|
||||
{
|
||||
int idx = occupied_cells_.size();
|
||||
occupied_cells_.emplace_back(x, y);
|
||||
int cx, cy;
|
||||
pointToNeighborCells(occupied_cells_.back(), cx, cy);
|
||||
int nidx = neighborCellsToIndex(cx, cy);
|
||||
if (nidx >= 0)
|
||||
neighbor_lookup_[nidx].push_back(idx);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Compute the convex hull for a single cluster (monotone chain algorithm)
|
||||
*
|
||||
* Clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm:
|
||||
* C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )
|
||||
* Reference: A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).
|
||||
* @remarks The algorithm seems to cut edges, thus we give a try to convexHull2().
|
||||
* @todo Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)
|
||||
* @remarks The last point is the same as the first one
|
||||
* @param cluster list of keypoints that should be converted into a polygon
|
||||
* @param[out] polygon the resulting convex polygon
|
||||
*/
|
||||
void convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::msg::Polygon& polygon);
|
||||
|
||||
/**
|
||||
* @brief Compute the convex hull for a single cluster
|
||||
*
|
||||
* Clusters are converted into convex polgons using an algorithm provided here:
|
||||
* https://bitbucket.org/vostreltsov/concave-hull/overview
|
||||
* The convex hull algorithm is part of the concave hull algorithm.
|
||||
* The license is WTFPL 2.0 and permits any usage.
|
||||
*
|
||||
* @remarks The last point is the same as the first one
|
||||
* @param cluster list of keypoints that should be converted into a polygon
|
||||
* @param[out] polygon the resulting convex polygon
|
||||
* @todo Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)
|
||||
*/
|
||||
void convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::msg::Polygon& polygon);
|
||||
|
||||
/**
|
||||
* @brief Simplify a polygon by removing points.
|
||||
*
|
||||
* We apply the Douglas-Peucker Algorithm to simplify the edges of the polygon.
|
||||
* Internally, the parameter min_keypoint_separation is used for deciding whether
|
||||
* a point is considered close to an edge and to be merged into the line.
|
||||
*/
|
||||
void simplifyPolygon(geometry_msgs::msg::Polygon& polygon);
|
||||
|
||||
/**
|
||||
* @brief 2D Cross product of two vectors defined by two points and a common origin
|
||||
* @param O Origin
|
||||
* @param A First point
|
||||
* @param B Second point
|
||||
* @tparam P1 2D Point type with x and y member fields
|
||||
* @tparam P2 2D Point type with x and y member fields
|
||||
* @tparam P3 2D Point type with x and y member fields
|
||||
*/
|
||||
template <typename P1, typename P2, typename P3>
|
||||
long double cross(const P1& O, const P2& A, const P3& B)
|
||||
{
|
||||
return (long double)(A.x - O.x) * (long double)(B.y - O.y) - (long double)(A.y - O.y) * (long double)(B.x - O.x);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Thread-safe update of the internal polygon container (that is shared using getPolygons() from outside this class)
|
||||
* @param polygons Updated polygon container
|
||||
*/
|
||||
void updatePolygonContainer(PolygonContainerPtr polygons);
|
||||
|
||||
|
||||
std::vector<KeyPoint> occupied_cells_; //!< List of occupied cells in the current map (updated by updateCostmap2D())
|
||||
|
||||
std::vector<std::vector<int> > neighbor_lookup_; //! array of cells for neighbor lookup
|
||||
int neighbor_size_x_; //! size of the neighbour lookup in x (number of cells)
|
||||
int neighbor_size_y_; //! size of the neighbour lookup in y (number of cells)
|
||||
double offset_x_; //! offset [meters] in x for the lookup grid
|
||||
double offset_y_; //! offset [meters] in y for the lookup grid
|
||||
|
||||
/**
|
||||
* @brief convert a 2d cell coordinate into the 1D index of the array
|
||||
* @param cx the x index of the cell
|
||||
* @param cy the y index of the cell
|
||||
*/
|
||||
int neighborCellsToIndex(int cx, int cy)
|
||||
{
|
||||
if (cx < 0 || cx >= neighbor_size_x_ || cy < 0 || cy >= neighbor_size_y_)
|
||||
return -1;
|
||||
return cy * neighbor_size_x_ + cx;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief compute the cell indices of a keypoint
|
||||
* @param kp key point given in world coordinates [m, m]
|
||||
* @param cx output cell index in x direction
|
||||
* @param cy output cell index in y direction
|
||||
*/
|
||||
void pointToNeighborCells(const KeyPoint& kp, int& cx, int& cy)
|
||||
{
|
||||
cx = int((kp.x - offset_x_) / parameter_.max_distance_);
|
||||
cy = int((kp.y - offset_y_) / parameter_.max_distance_);
|
||||
}
|
||||
|
||||
|
||||
Parameters parameter_; //< active parameters throughout computation
|
||||
Parameters parameter_buffered_; //< the buffered parameters that are offered to dynamic reconfigure
|
||||
std::mutex parameter_mutex_; //!< Mutex that keeps track about the ownership of the shared polygon instance
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* @brief Callback for the dynamic_reconfigure node.
|
||||
*
|
||||
* This callback allows to modify parameters dynamically at runtime without restarting the node
|
||||
* @param config Reference to the dynamic reconfigure config
|
||||
* @param level Dynamic reconfigure level
|
||||
*/
|
||||
//void reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level);
|
||||
|
||||
|
||||
PolygonContainerPtr polygons_; //!< Current shared container of polygons
|
||||
std::mutex mutex_; //!< Mutex that keeps track about the ownership of the shared polygon instance
|
||||
|
||||
//dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
|
||||
|
||||
nav2_costmap_2d::Costmap2D *costmap_; //!< Pointer to the costmap2d
|
||||
|
||||
};
|
||||
|
||||
|
||||
} //end namespace teb_local_planner
|
||||
|
||||
#endif /* COSTMAP_TO_POLYGONS_H_ */
|
||||
@ -1,202 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef COSTMAP_TO_POLYGONS_CONCAVE_H_
|
||||
#define COSTMAP_TO_POLYGONS_CONCAVE_H_
|
||||
|
||||
#include <costmap_converter/costmap_to_polygons.h>
|
||||
#include <costmap_converter/misc.h>
|
||||
|
||||
// dynamic reconfigure
|
||||
//#include <costmap_converter/CostmapToPolygonsDBSConcaveHullConfig.h>
|
||||
//#include <dynamic_reconfigure/server.h>
|
||||
|
||||
|
||||
namespace costmap_converter
|
||||
{
|
||||
|
||||
/**
|
||||
* @class CostmapToPolygonsDBSConcaveHull
|
||||
* @brief This class converts the costmap_2d into a set of non-convex polygons (and points)
|
||||
*
|
||||
* All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex)
|
||||
* @todo change the class hierarchy to a clearer and more generic one
|
||||
*/
|
||||
class CostmapToPolygonsDBSConcaveHull : public CostmapToPolygonsDBSMCCH
|
||||
{
|
||||
public:
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
*/
|
||||
CostmapToPolygonsDBSConcaveHull();
|
||||
|
||||
/**
|
||||
* @brief Destructor
|
||||
*/
|
||||
virtual ~CostmapToPolygonsDBSConcaveHull();
|
||||
|
||||
/**
|
||||
* @brief Initialize the plugin
|
||||
* @param nh Nodehandle that defines the namespace for parameters
|
||||
*/
|
||||
virtual void initialize(rclcpp::Node::SharedPtr nh);
|
||||
|
||||
|
||||
/**
|
||||
* @brief This method performs the actual work (conversion of the costmap to polygons)
|
||||
*/
|
||||
virtual void compute();
|
||||
|
||||
protected:
|
||||
|
||||
|
||||
/**
|
||||
* @brief Compute the concave hull for a single cluster
|
||||
*
|
||||
* @remarks The last point is the same as the first one
|
||||
* @param cluster list of keypoints that should be converted into a polygon
|
||||
* @param depth Smaller depth: sharper surface, depth -> high value: convex hull
|
||||
* @param[out] polygon the resulting convex polygon
|
||||
*/
|
||||
void concaveHull(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::msg::Polygon& polygon);
|
||||
|
||||
void concaveHullClusterCut(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::msg::Polygon& polygon);
|
||||
|
||||
template <typename PointLine, typename PointCluster, typename PointHull>
|
||||
std::size_t findNearestInnerPoint(PointLine line_start, PointLine line_end, const std::vector<PointCluster>& cluster, const std::vector<PointHull>& hull, bool* found);
|
||||
|
||||
|
||||
template <typename Point1, typename Point2, typename Point3, typename Point4>
|
||||
bool checkLineIntersection(const Point1& line1_start, const Point2& line1_end, const Point3& line2_start, const Point4& line2_end);
|
||||
|
||||
template <typename PointHull, typename Point1, typename Point2, typename Point3, typename Point4>
|
||||
bool checkLineIntersection(const std::vector<PointHull>& hull, const Point1& current_line_start,
|
||||
const Point2& current_line_end, const Point3& test_line_start, const Point4& test_line_end);
|
||||
|
||||
double concave_hull_depth_;
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* @brief Callback for the dynamic_reconfigure node.
|
||||
*
|
||||
* This callback allows to modify parameters dynamically at runtime without restarting the node
|
||||
* @param config Reference to the dynamic reconfigure config
|
||||
* @param level Dynamic reconfigure level
|
||||
*/
|
||||
// void reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level);
|
||||
|
||||
// dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
template <typename PointLine, typename PointCluster, typename PointHull>
|
||||
std::size_t CostmapToPolygonsDBSConcaveHull::findNearestInnerPoint(PointLine line_start, PointLine line_end, const std::vector<PointCluster>& cluster, const std::vector<PointHull>& hull, bool* found)
|
||||
{
|
||||
std::size_t nearsest_idx = 0;
|
||||
double distance = 0;
|
||||
*found = false;
|
||||
|
||||
for (std::size_t i = 0; i < cluster.size(); ++i)
|
||||
{
|
||||
// Skip points that are already in the hull
|
||||
if (std::find_if( hull.begin(), hull.end(), std::bind(isApprox2d<PointHull, PointCluster>, std::placeholders::_1, std::cref(cluster[i]), 1e-5) ) != hull.end() )
|
||||
continue;
|
||||
|
||||
double dist = computeDistanceToLineSegment(cluster[i], line_start, line_end);
|
||||
bool skip = false;
|
||||
for (int j = 0; !skip && j < (int)hull.size() - 1; ++j)
|
||||
{
|
||||
double dist_temp = computeDistanceToLineSegment(cluster[i], hull[j], hull[j + 1]);
|
||||
skip |= dist_temp < dist;
|
||||
}
|
||||
if (skip)
|
||||
continue;
|
||||
|
||||
if (!(*found) || dist < distance)
|
||||
{
|
||||
nearsest_idx = i;
|
||||
distance = dist;
|
||||
*found = true;
|
||||
}
|
||||
}
|
||||
return nearsest_idx;
|
||||
}
|
||||
|
||||
|
||||
template <typename Point1, typename Point2, typename Point3, typename Point4>
|
||||
bool CostmapToPolygonsDBSConcaveHull::checkLineIntersection(const Point1& line1_start, const Point2& line1_end, const Point3& line2_start, const Point4& line2_end)
|
||||
{
|
||||
double dx1 = line1_end.x - line1_start.x;
|
||||
double dy1 = line1_end.y - line1_start.y;
|
||||
double dx2 = line2_end.x - line2_start.x;
|
||||
double dy2 = line2_end.y - line2_start.y;
|
||||
double s = (-dy1 * (line1_start.x - line2_start.x) + dx1 * (line1_start.y - line2_start.y)) / (-dx2 * dy1 + dx1 * dy2);
|
||||
double t = ( dx2 * (line1_start.y - line2_start.y) - dy2 * (line1_start.x - line2_start.x)) / (-dx2 * dy1 + dx1 * dy2);
|
||||
return (s > 0 && s < 1 && t > 0 && t < 1);
|
||||
}
|
||||
|
||||
|
||||
template <typename PointHull, typename Point1, typename Point2, typename Point3, typename Point4>
|
||||
bool CostmapToPolygonsDBSConcaveHull::checkLineIntersection(const std::vector<PointHull>& hull, const Point1& current_line_start,
|
||||
const Point2& current_line_end, const Point3& test_line_start, const Point4& test_line_end)
|
||||
{
|
||||
for (int i = 0; i < (int)hull.size() - 2; i++)
|
||||
{
|
||||
if (isApprox2d(current_line_start, hull[i], 1e-5) && isApprox2d(current_line_end, hull[i+1], 1e-5))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (checkLineIntersection(test_line_start, test_line_end, hull[i], hull[i+1]))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
} //end namespace teb_local_planner
|
||||
|
||||
#endif /* COSTMAP_TO_POLYGONS_CONCAVE_H_ */
|
||||
@ -1,157 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef MISC_H_
|
||||
#define MISC_H_
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
namespace costmap_converter
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Calculate the distance between a point and a straight line (with infinite length)
|
||||
* @param point generic 2D point type defining the reference point
|
||||
* @param line_pt1 generic 2D point as part of the line
|
||||
* @param line_pt2 generic 2D point as part of the line
|
||||
* @tparam Point generic point type that should provide x and y member fiels.
|
||||
* @tparam LinePoint generic point type that should provide x and y member fiels.
|
||||
* @return (minimum) euclidean distance to the line segment
|
||||
*/
|
||||
template <typename Point, typename LinePoint>
|
||||
inline double computeDistanceToLine(const Point& point, const LinePoint& line_pt1, const LinePoint& line_pt2)
|
||||
{
|
||||
double dx = line_pt2.x - line_pt1.x;
|
||||
double dy = line_pt2.y - line_pt1.y;
|
||||
|
||||
double length = std::sqrt(dx*dx + dy*dy);
|
||||
|
||||
if (length>0)
|
||||
return std::abs(dy * point.x - dx * point.y + line_pt2.x * line_pt1.y - line_pt2.y * line_pt1.x) / length;
|
||||
|
||||
return std::sqrt(std::pow(point.x - line_pt1.x, 2) + std::pow(point.y - line_pt1.y, 2));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Calculate the squared distance between a point and a straight line segment
|
||||
* @param point generic 2D point type defining the reference point
|
||||
* @param line_start generic 2D point type defining the start of the line
|
||||
* @param line_end generic 2D point type defining the end of the line
|
||||
* @param[out] is_inbetween write \c true, if the point is placed inbetween start and end [optional]
|
||||
* @tparam Point generic point type that should provide x and y member fiels.
|
||||
* @tparam LinePoint generic point type that should provide x and y member fiels.
|
||||
* @return (minimum) squared euclidean distance to the line segment
|
||||
*/
|
||||
template <typename Point, typename LinePoint>
|
||||
inline double computeSquaredDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)
|
||||
{
|
||||
double dx = line_end.x - line_start.x;
|
||||
double dy = line_end.y - line_start.y;
|
||||
|
||||
double length_sqr = dx*dx + dy*dy;
|
||||
|
||||
double u = 0;
|
||||
|
||||
if (length_sqr > 0)
|
||||
u = ((point.x - line_start.x) * dx + (point.y - line_start.y) * dy) / length_sqr;
|
||||
|
||||
if (is_inbetween)
|
||||
*is_inbetween = (u>=0 && u<=1);
|
||||
|
||||
if (u <= 0)
|
||||
return std::pow(point.x-line_start.x,2) + std::pow(point.y-line_start.y,2);
|
||||
|
||||
if (u >= 1)
|
||||
return std::pow(point.x-line_end.x,2) + std::pow(point.y-line_end.y,2);
|
||||
|
||||
return std::pow(point.x - (line_start.x+u*dx) ,2) + std::pow(point.y - (line_start.y+u*dy),2);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Calculate the distance between a point and a straight line segment
|
||||
* @param point generic 2D point type defining the reference point
|
||||
* @param line_start generic 2D point type defining the start of the line
|
||||
* @param line_end generic 2D point type defining the end of the line
|
||||
* @param[out] is_inbetween write \c true, if the point is placed inbetween start and end [optional]
|
||||
* @tparam Point generic point type that should provide x and y member fiels.
|
||||
* @tparam LinePoint generic point type that should provide x and y member fiels.
|
||||
* @return (minimum) euclidean distance to the line segment
|
||||
*/
|
||||
template <typename Point, typename LinePoint>
|
||||
inline double computeDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)
|
||||
{
|
||||
return std::sqrt(computeSquaredDistanceToLineSegment(point, line_start, line_end, is_inbetween));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Calculate the distance between two 2d points
|
||||
* @param pt1 generic 2D point
|
||||
* @param pt2 generic 2D point
|
||||
* @tparam Point1 generic point type that should provide x and y member fiels.
|
||||
* @tparam Point2 generic point type that should provide x and y member fiels.
|
||||
* @return euclidean distance to the line segment
|
||||
*/
|
||||
template <typename Point1, typename Point2>
|
||||
inline double norm2d(const Point1& pt1, const Point2& pt2)
|
||||
{
|
||||
return std::sqrt( std::pow(pt2.x - pt1.x, 2) + std::pow(pt2.y - pt1.y, 2) );
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check if two points are approximately defining the same one
|
||||
* @param pt1 generic 2D point
|
||||
* @param pt2 generic 2D point
|
||||
* @param threshold define the minimum threshold |pt1.x-pt2.y| < tresh && |pt1.y-pt2.y| < tresh
|
||||
* @tparam Point1 generic point type that should provide x and y member fiels.
|
||||
* @tparam Point2 generic point type that should provide x and y member fiels.
|
||||
* @return \c true, if similar, \c false otherwise
|
||||
*/
|
||||
template <typename Point1, typename Point2>
|
||||
inline bool isApprox2d(const Point1& pt1, const Point2& pt2, double threshold)
|
||||
{
|
||||
return ( std::abs(pt2.x-pt1.x)<threshold && std::abs(pt2.y-pt1.y)<threshold );
|
||||
}
|
||||
|
||||
|
||||
|
||||
} //end namespace teb_local_planner
|
||||
|
||||
#endif /* MISC_H_ */
|
||||
@ -1,38 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>costmap_converter</name>
|
||||
<version>0.1.2</version>
|
||||
<description>
|
||||
A ros package that includes plugins and nodes to convert occupied costmap2d cells to primitive types.
|
||||
</description>
|
||||
|
||||
<maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
|
||||
|
||||
<author email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</author>
|
||||
<author email="franz.albers@tu-dortmund.de">Franz Albers</author>
|
||||
<author email="otniel.rinaldo@tu-dortmund.de">Otniel Rinaldo</author>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">http://wiki.ros.org/costmap_converter</url>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>class_loader</depend>
|
||||
<depend>costmap_converter_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav2_costmap_2d</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<!-- <depend>dynamic_reconfigure</depend> -->
|
||||
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,264 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann, Otniel Rinaldo
|
||||
*********************************************************************/
|
||||
#include <functional>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <geometry_msgs/msg/polygon_stamped.hpp>
|
||||
#include <nav2_costmap_2d/costmap_2d.hpp>
|
||||
#include <nav_msgs/msg/occupancy_grid.hpp>
|
||||
#include <visualization_msgs/msg/marker.hpp>
|
||||
|
||||
#include <costmap_converter/costmap_converter_interface.h>
|
||||
#include <pluginlib/class_loader.hpp>
|
||||
|
||||
class CostmapStandaloneConversion : public rclcpp::Node {
|
||||
public:
|
||||
CostmapStandaloneConversion(const std::string node_name)
|
||||
: rclcpp::Node(node_name),
|
||||
converter_loader_("costmap_converter",
|
||||
"costmap_converter::BaseCostmapToPolygons") {
|
||||
costmap_ros_ =
|
||||
std::make_shared<nav2_costmap_2d::Costmap2DROS>("converter_costmap");
|
||||
costmap_thread_ = std::make_unique<std::thread>(
|
||||
[](rclcpp_lifecycle::LifecycleNode::SharedPtr node) {
|
||||
rclcpp::spin(node->get_node_base_interface());
|
||||
},
|
||||
costmap_ros_);
|
||||
rclcpp_lifecycle::State state;
|
||||
costmap_ros_->on_configure(state);
|
||||
costmap_ros_->on_activate(state);
|
||||
|
||||
n_ = std::shared_ptr<rclcpp::Node>(this, [](rclcpp::Node *) {});
|
||||
// load converter plugin from parameter server, otherwise set default
|
||||
|
||||
std::string converter_plugin =
|
||||
"costmap_converter::CostmapToPolygonsDBSMCCH";
|
||||
|
||||
declare_parameter("converter_plugin",
|
||||
rclcpp::ParameterValue(converter_plugin));
|
||||
|
||||
get_parameter_or<std::string>("converter_plugin", converter_plugin,
|
||||
converter_plugin);
|
||||
|
||||
try {
|
||||
converter_ = converter_loader_.createSharedInstance(converter_plugin);
|
||||
} catch (const pluginlib::PluginlibException &ex) {
|
||||
RCLCPP_ERROR(get_logger(),
|
||||
"The plugin failed to load for some reason. Error: %s",
|
||||
ex.what());
|
||||
rclcpp::shutdown();
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Standalone costmap converter: %s loaded.",
|
||||
converter_plugin.c_str());
|
||||
|
||||
std::string obstacles_topic = "costmap_obstacles";
|
||||
declare_parameter("obstacles_topic",
|
||||
rclcpp::ParameterValue(obstacles_topic));
|
||||
get_parameter_or<std::string>("obstacles_topic", obstacles_topic,
|
||||
obstacles_topic);
|
||||
|
||||
std::string polygon_marker_topic = "costmap_polygon_markers";
|
||||
declare_parameter("polygon_marker_topic",
|
||||
rclcpp::ParameterValue(polygon_marker_topic));
|
||||
get_parameter_or<std::string>("polygon_marker_topic", polygon_marker_topic,
|
||||
polygon_marker_topic);
|
||||
|
||||
obstacle_pub_ =
|
||||
create_publisher<costmap_converter_msgs::msg::ObstacleArrayMsg>(
|
||||
obstacles_topic, 1000);
|
||||
marker_pub_ = create_publisher<visualization_msgs::msg::Marker>(
|
||||
polygon_marker_topic, 10);
|
||||
|
||||
occupied_min_value_ = 100;
|
||||
declare_parameter("occupied_min_value",
|
||||
rclcpp::ParameterValue(occupied_min_value_));
|
||||
get_parameter_or<int>("occupied_min_value", occupied_min_value_,
|
||||
occupied_min_value_);
|
||||
|
||||
std::string odom_topic = "/odom";
|
||||
declare_parameter("odom_topic", rclcpp::ParameterValue(odom_topic));
|
||||
get_parameter_or<std::string>("odom_topic", odom_topic, odom_topic);
|
||||
|
||||
if (converter_) {
|
||||
converter_->setOdomTopic(odom_topic);
|
||||
converter_->initialize(shared_from_this());
|
||||
converter_->startWorker(std::make_shared<rclcpp::Rate>(5),
|
||||
costmap_ros_->getCostmap(), false);
|
||||
}
|
||||
|
||||
pub_timer_ = n_->create_wall_timer(
|
||||
std::chrono::milliseconds(200),
|
||||
std::bind(&CostmapStandaloneConversion::publishCallback, this));
|
||||
}
|
||||
|
||||
void publishCallback() {
|
||||
costmap_converter::ObstacleArrayConstPtr obstacles =
|
||||
converter_->getObstacles();
|
||||
|
||||
if (!obstacles) return;
|
||||
|
||||
obstacle_pub_->publish(*obstacles);
|
||||
|
||||
frame_id_ = costmap_ros_->getGlobalFrameID();
|
||||
|
||||
publishAsMarker(frame_id_, *obstacles);
|
||||
}
|
||||
|
||||
void publishAsMarker(
|
||||
const std::string &frame_id,
|
||||
const std::vector<geometry_msgs::msg::PolygonStamped> &polygonStamped) {
|
||||
visualization_msgs::msg::Marker line_list;
|
||||
line_list.header.frame_id = frame_id;
|
||||
line_list.header.stamp = now();
|
||||
line_list.ns = "Polygons";
|
||||
line_list.action = visualization_msgs::msg::Marker::ADD;
|
||||
line_list.pose.orientation.w = 1.0;
|
||||
|
||||
line_list.id = 0;
|
||||
line_list.type = visualization_msgs::msg::Marker::LINE_LIST;
|
||||
|
||||
line_list.scale.x = 0.1;
|
||||
line_list.color.g = 1.0;
|
||||
line_list.color.a = 1.0;
|
||||
|
||||
for (std::size_t i = 0; i < polygonStamped.size(); ++i) {
|
||||
for (int j = 0; j < (int)polygonStamped[i].polygon.points.size() - 1;
|
||||
++j) {
|
||||
geometry_msgs::msg::Point line_start;
|
||||
line_start.x = polygonStamped[i].polygon.points[j].x;
|
||||
line_start.y = polygonStamped[i].polygon.points[j].y;
|
||||
line_list.points.push_back(line_start);
|
||||
geometry_msgs::msg::Point line_end;
|
||||
line_end.x = polygonStamped[i].polygon.points[j + 1].x;
|
||||
line_end.y = polygonStamped[i].polygon.points[j + 1].y;
|
||||
line_list.points.push_back(line_end);
|
||||
}
|
||||
// close loop for current polygon
|
||||
if (!polygonStamped[i].polygon.points.empty() &&
|
||||
polygonStamped[i].polygon.points.size() != 2) {
|
||||
geometry_msgs::msg::Point line_start;
|
||||
line_start.x = polygonStamped[i].polygon.points.back().x;
|
||||
line_start.y = polygonStamped[i].polygon.points.back().y;
|
||||
line_list.points.push_back(line_start);
|
||||
if (line_list.points.size() % 2 != 0) {
|
||||
geometry_msgs::msg::Point line_end;
|
||||
line_end.x = polygonStamped[i].polygon.points.front().x;
|
||||
line_end.y = polygonStamped[i].polygon.points.front().y;
|
||||
line_list.points.push_back(line_end);
|
||||
}
|
||||
}
|
||||
}
|
||||
marker_pub_->publish(line_list);
|
||||
}
|
||||
|
||||
void publishAsMarker(
|
||||
const std::string &frame_id,
|
||||
const costmap_converter_msgs::msg::ObstacleArrayMsg &obstacles) {
|
||||
visualization_msgs::msg::Marker line_list;
|
||||
line_list.header.frame_id = frame_id;
|
||||
line_list.header.stamp = now();
|
||||
line_list.ns = "Polygons";
|
||||
line_list.action = visualization_msgs::msg::Marker::ADD;
|
||||
line_list.pose.orientation.w = 1.0;
|
||||
|
||||
line_list.id = 0;
|
||||
line_list.type = visualization_msgs::msg::Marker::LINE_LIST;
|
||||
|
||||
line_list.scale.x = 0.1;
|
||||
line_list.color.g = 1.0;
|
||||
line_list.color.a = 1.0;
|
||||
|
||||
for (const auto &obstacle : obstacles.obstacles) {
|
||||
for (int j = 0; j < (int)obstacle.polygon.points.size() - 1; ++j) {
|
||||
geometry_msgs::msg::Point line_start;
|
||||
line_start.x = obstacle.polygon.points[j].x;
|
||||
line_start.y = obstacle.polygon.points[j].y;
|
||||
line_list.points.push_back(line_start);
|
||||
geometry_msgs::msg::Point line_end;
|
||||
line_end.x = obstacle.polygon.points[j + 1].x;
|
||||
line_end.y = obstacle.polygon.points[j + 1].y;
|
||||
line_list.points.push_back(line_end);
|
||||
}
|
||||
// close loop for current polygon
|
||||
if (!obstacle.polygon.points.empty() &&
|
||||
obstacle.polygon.points.size() != 2) {
|
||||
geometry_msgs::msg::Point line_start;
|
||||
line_start.x = obstacle.polygon.points.back().x;
|
||||
line_start.y = obstacle.polygon.points.back().y;
|
||||
line_list.points.push_back(line_start);
|
||||
if (line_list.points.size() % 2 != 0) {
|
||||
geometry_msgs::msg::Point line_end;
|
||||
line_end.x = obstacle.polygon.points.front().x;
|
||||
line_end.y = obstacle.polygon.points.front().y;
|
||||
line_list.points.push_back(line_end);
|
||||
}
|
||||
}
|
||||
}
|
||||
marker_pub_->publish(line_list);
|
||||
}
|
||||
|
||||
private:
|
||||
pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons>
|
||||
converter_loader_;
|
||||
std::shared_ptr<costmap_converter::BaseCostmapToPolygons> converter_;
|
||||
|
||||
rclcpp::Node::SharedPtr n_;
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
|
||||
std::unique_ptr<std::thread> costmap_thread_;
|
||||
rclcpp::Publisher<costmap_converter_msgs::msg::ObstacleArrayMsg>::SharedPtr
|
||||
obstacle_pub_;
|
||||
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub_;
|
||||
rclcpp::TimerBase::SharedPtr pub_timer_;
|
||||
|
||||
std::string frame_id_;
|
||||
int occupied_min_value_;
|
||||
};
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto convert_process =
|
||||
std::make_shared<CostmapStandaloneConversion>("costmap_converter");
|
||||
|
||||
rclcpp::spin(convert_process);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -1,115 +0,0 @@
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
//#include <opencv2/cvv/cvv.hpp>
|
||||
#include <costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h>
|
||||
|
||||
BackgroundSubtractor::BackgroundSubtractor(const Params ¶meters): params_(parameters)
|
||||
{
|
||||
}
|
||||
|
||||
void BackgroundSubtractor::apply(const cv::Mat& image, cv::Mat& fg_mask, int shift_x, int shift_y)
|
||||
{
|
||||
current_frame_ = image;
|
||||
|
||||
// occupancy grids are empty only once in the beginning -> initialize variables
|
||||
if (occupancy_grid_fast_.empty() && occupancy_grid_slow_.empty())
|
||||
{
|
||||
occupancy_grid_fast_ = current_frame_;
|
||||
occupancy_grid_slow_ = current_frame_;
|
||||
previous_shift_x_ = shift_x;
|
||||
previous_shift_y_ = shift_y;
|
||||
return;
|
||||
}
|
||||
|
||||
// Shift previous occupancy grid to new location (match current_frame_)
|
||||
int shift_relative_to_previous_pos_x_ = shift_x - previous_shift_x_;
|
||||
int shift_relative_to_previous_pos_y_ = shift_y - previous_shift_y_;
|
||||
previous_shift_x_ = shift_x;
|
||||
previous_shift_y_ = shift_y;
|
||||
|
||||
// if(shift_relative_to_previous_pos_x_ != 0 || shift_relative_to_previous_pos_y_ != 0)
|
||||
transformToCurrentFrame(shift_relative_to_previous_pos_x_, shift_relative_to_previous_pos_y_);
|
||||
|
||||
// cvv::debugFilter(occupancy_grid_fast_, currentFrame_, CVVISUAL_LOCATION);
|
||||
|
||||
// Calculate normalized sum (mean) of nearest neighbors
|
||||
int size = 3; // 3, 5, 7, ....
|
||||
cv::Mat nearest_neighbor_mean_fast(occupancy_grid_fast_.size(), CV_8UC1);
|
||||
cv::Mat nearest_neighbor_mean_slow(occupancy_grid_slow_.size(), CV_8UC1);
|
||||
cv::boxFilter(occupancy_grid_fast_, nearest_neighbor_mean_fast, -1, cv::Size(size, size), cv::Point(-1, -1), true,
|
||||
cv::BORDER_REPLICATE);
|
||||
cv::boxFilter(occupancy_grid_slow_, nearest_neighbor_mean_slow, -1, cv::Size(size, size), cv::Point(-1, -1), true,
|
||||
cv::BORDER_REPLICATE);
|
||||
// cv::GaussianBlur(occupancy_grid_fast_, nearest_neighbor_mean_fast, cv::Size(size,size), 1, 1, cv::BORDER_REPLICATE);
|
||||
// cv::GaussianBlur(occupancy_grid_fast_, nearest_neighbor_mean_fast, cv::Size(size,size), 1, 1, cv::BORDER_REPLICATE);
|
||||
|
||||
// compute time mean value for each pixel according to learningrate alpha
|
||||
// occupancy_grid_fast_ = beta*(alpha_fast*current_frame_ + (1.0-alpha_fast)*occupancy_grid_fast_) + (1-beta)*nearest_neighbor_mean_fast;
|
||||
cv::addWeighted(current_frame_, params_.alpha_fast, occupancy_grid_fast_, (1 - params_.alpha_fast), 0, occupancy_grid_fast_);
|
||||
cv::addWeighted(occupancy_grid_fast_, params_.beta, nearest_neighbor_mean_fast, (1 - params_.beta), 0, occupancy_grid_fast_);
|
||||
// occupancy_grid_slow_ = beta*(alpha_slow*current_frame_ + (1.0-alpha_slow)*occupancy_grid_slow) + (1-beta)*nearest_neighbor_mean_slow;
|
||||
cv::addWeighted(current_frame_, params_.alpha_slow, occupancy_grid_slow_, (1 - params_.alpha_slow), 0, occupancy_grid_slow_);
|
||||
cv::addWeighted(occupancy_grid_slow_, params_.beta, nearest_neighbor_mean_slow, (1 - params_.beta), 0, occupancy_grid_slow_);
|
||||
|
||||
// 1) Obstacles should be detected after a minimum response of the fast filter
|
||||
// occupancy_grid_fast_ > min_occupancy_probability
|
||||
cv::threshold(occupancy_grid_fast_, occupancy_grid_fast_, params_.min_occupancy_probability, 0 /*unused*/, cv::THRESH_TOZERO);
|
||||
// 2) Moving obstacles have a minimum difference between the responses of the slow and fast filter
|
||||
// occupancy_grid_fast_-occupancy_grid_slow_ > min_sep_between_fast_and_slow_filter
|
||||
cv::threshold(occupancy_grid_fast_ - occupancy_grid_slow_, fg_mask, params_.min_sep_between_fast_and_slow_filter, 255,
|
||||
cv::THRESH_BINARY);
|
||||
// 3) Dismiss static obstacles
|
||||
// nearest_neighbor_mean_slow < max_occupancy_neighbors
|
||||
cv::threshold(nearest_neighbor_mean_slow, nearest_neighbor_mean_slow, params_.max_occupancy_neighbors, 255, cv::THRESH_BINARY_INV);
|
||||
cv::bitwise_and(nearest_neighbor_mean_slow, fg_mask, fg_mask);
|
||||
|
||||
//visualize("Current frame", currentFrame_);
|
||||
cv::Mat setBorderToZero = cv::Mat(current_frame_.size(), CV_8UC1, 0.0);
|
||||
int border = 5;
|
||||
setBorderToZero(cv::Rect(border, border, current_frame_.cols-2*border, current_frame_.rows-2*border)) = 255;
|
||||
|
||||
cv::bitwise_and(setBorderToZero, fg_mask, fg_mask);
|
||||
|
||||
// cv::imwrite("/home/albers/Desktop/currentFrame.png", currentFrame_);
|
||||
// visualize("Foreground mask", fgMask);
|
||||
|
||||
// Closing Operation
|
||||
cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE,
|
||||
cv::Size(2*params_.morph_size + 1, 2*params_.morph_size + 1),
|
||||
cv::Point(params_.morph_size, params_.morph_size));
|
||||
cv::dilate(fg_mask, fg_mask, element);
|
||||
cv::dilate(fg_mask, fg_mask, element);
|
||||
cv::erode(fg_mask, fg_mask, element);
|
||||
}
|
||||
|
||||
void BackgroundSubtractor::transformToCurrentFrame(int shift_x, int shift_y)
|
||||
{
|
||||
// TODO: initialize with current_frame (first observed image) rather than zeros
|
||||
|
||||
// Translate (shift) image in costmap coordinates
|
||||
// in cv::Mat: shift X -> to the left; shift y -> to the top
|
||||
cv::Mat temp_fast, temp_slow;
|
||||
cv::Mat translation_mat = (cv::Mat_<double>(2, 3, CV_64F) << 1, 0, -shift_x, 0, 1, -shift_y);
|
||||
cv::warpAffine(occupancy_grid_fast_, temp_fast, translation_mat, occupancy_grid_fast_.size()); // can't operate in-place
|
||||
cv::warpAffine(occupancy_grid_slow_, temp_slow, translation_mat, occupancy_grid_slow_.size()); // can't operate in-place
|
||||
|
||||
// cvv::debugFilter(occupancy_grid_fast_, temp_fast);
|
||||
|
||||
occupancy_grid_fast_ = temp_fast;
|
||||
occupancy_grid_slow_ = temp_slow;
|
||||
}
|
||||
|
||||
void BackgroundSubtractor::visualize(const std::string& name, const cv::Mat& image)
|
||||
{
|
||||
if (!image.empty())
|
||||
{
|
||||
cv::Mat im = image.clone();
|
||||
cv::flip(im, im, 0);
|
||||
cv::imshow(name, im);
|
||||
cv::waitKey(1);
|
||||
}
|
||||
}
|
||||
|
||||
void BackgroundSubtractor::updateParameters(const Params ¶meters)
|
||||
{
|
||||
params_ = parameters;
|
||||
}
|
||||
@ -1,193 +0,0 @@
|
||||
#include <costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <iostream>
|
||||
|
||||
BlobDetector::BlobDetector(const SimpleBlobDetector::Params& parameters) : params_(parameters) {}
|
||||
|
||||
cv::Ptr<BlobDetector> BlobDetector::create(const cv::SimpleBlobDetector::Params& params)
|
||||
{
|
||||
return cv::Ptr<BlobDetector> (new BlobDetector(params)); // compatibility with older versions
|
||||
//return cv::makePtr<BlobDetector>(params);
|
||||
}
|
||||
|
||||
void BlobDetector::detect(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat&)
|
||||
{
|
||||
// TODO: support mask
|
||||
contours_.clear();
|
||||
|
||||
keypoints.clear();
|
||||
cv::Mat grayscale_image;
|
||||
if (image.channels() == 3)
|
||||
cv::cvtColor(image, grayscale_image, cv::COLOR_BGR2GRAY);
|
||||
else
|
||||
grayscale_image = image;
|
||||
|
||||
if (grayscale_image.type() != CV_8UC1)
|
||||
{
|
||||
//CV_Error(cv::Error::StsUnsupportedFormat, "Blob detector only supports 8-bit images!");
|
||||
std::cerr << "Blob detector only supports 8-bit images!\n";
|
||||
}
|
||||
|
||||
std::vector<std::vector<Center>> centers;
|
||||
std::vector<std::vector<cv::Point>> contours;
|
||||
for (double thresh = params_.minThreshold; thresh < params_.maxThreshold; thresh += params_.thresholdStep)
|
||||
{
|
||||
cv::Mat binarized_image;
|
||||
cv::threshold(grayscale_image, binarized_image, thresh, 255, cv::THRESH_BINARY);
|
||||
|
||||
std::vector<Center> cur_centers;
|
||||
std::vector<std::vector<cv::Point>> cur_contours, new_contours;
|
||||
findBlobs(grayscale_image, binarized_image, cur_centers, cur_contours);
|
||||
std::vector<std::vector<Center>> new_centers;
|
||||
for (std::size_t i = 0; i < cur_centers.size(); ++i)
|
||||
{
|
||||
bool isNew = true;
|
||||
for (std::size_t j = 0; j < centers.size(); ++j)
|
||||
{
|
||||
double dist = cv::norm(centers[j][centers[j].size() / 2].location - cur_centers[i].location);
|
||||
isNew = dist >= params_.minDistBetweenBlobs && dist >= centers[j][centers[j].size() / 2].radius &&
|
||||
dist >= cur_centers[i].radius;
|
||||
if (!isNew)
|
||||
{
|
||||
centers[j].push_back(cur_centers[i]);
|
||||
|
||||
size_t k = centers[j].size() - 1;
|
||||
while (k > 0 && centers[j][k].radius < centers[j][k - 1].radius)
|
||||
{
|
||||
centers[j][k] = centers[j][k - 1];
|
||||
k--;
|
||||
}
|
||||
centers[j][k] = cur_centers[i];
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (isNew)
|
||||
{
|
||||
new_centers.push_back(std::vector<Center>(1, cur_centers[i]));
|
||||
new_contours.push_back(cur_contours[i]);
|
||||
}
|
||||
}
|
||||
std::copy(new_centers.begin(), new_centers.end(), std::back_inserter(centers));
|
||||
std::copy(new_contours.begin(), new_contours.end(), std::back_inserter(contours));
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < centers.size(); ++i)
|
||||
{
|
||||
if (centers[i].size() < params_.minRepeatability)
|
||||
continue;
|
||||
cv::Point2d sum_point(0, 0);
|
||||
double normalizer = 0;
|
||||
for (std::size_t j = 0; j < centers[i].size(); ++j)
|
||||
{
|
||||
sum_point += centers[i][j].confidence * centers[i][j].location;
|
||||
normalizer += centers[i][j].confidence;
|
||||
}
|
||||
sum_point *= (1. / normalizer);
|
||||
cv::KeyPoint kpt(sum_point, (float)(centers[i][centers[i].size() / 2].radius));
|
||||
keypoints.push_back(kpt);
|
||||
contours_.push_back(contours[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void BlobDetector::findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector<Center>& centers,
|
||||
std::vector<std::vector<cv::Point>>& cur_contours) const
|
||||
{
|
||||
(void)image;
|
||||
centers.clear();
|
||||
cur_contours.clear();
|
||||
|
||||
std::vector<std::vector<cv::Point>> contours;
|
||||
cv::Mat tmp_binary_image = binary_image.clone();
|
||||
cv::findContours(tmp_binary_image, contours, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE);
|
||||
|
||||
for (std::size_t contour_idx = 0; contour_idx < contours.size(); ++contour_idx)
|
||||
{
|
||||
Center center;
|
||||
center.confidence = 1;
|
||||
cv::Moments moms = cv::moments(cv::Mat(contours[contour_idx]));
|
||||
if (params_.filterByArea)
|
||||
{
|
||||
double area = moms.m00;
|
||||
if (area < params_.minArea || area >= params_.maxArea)
|
||||
continue;
|
||||
}
|
||||
|
||||
if (params_.filterByCircularity)
|
||||
{
|
||||
double area = moms.m00;
|
||||
double perimeter = cv::arcLength(cv::Mat(contours[contour_idx]), true);
|
||||
double ratio = 4 * CV_PI * area / (perimeter * perimeter);
|
||||
if (ratio < params_.minCircularity || ratio >= params_.maxCircularity)
|
||||
continue;
|
||||
}
|
||||
|
||||
if (params_.filterByInertia)
|
||||
{
|
||||
double denominator = std::sqrt(std::pow(2 * moms.mu11, 2) + std::pow(moms.mu20 - moms.mu02, 2));
|
||||
const double eps = 1e-2;
|
||||
double ratio;
|
||||
if (denominator > eps)
|
||||
{
|
||||
double cosmin = (moms.mu20 - moms.mu02) / denominator;
|
||||
double sinmin = 2 * moms.mu11 / denominator;
|
||||
double cosmax = -cosmin;
|
||||
double sinmax = -sinmin;
|
||||
|
||||
double imin = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmin - moms.mu11 * sinmin;
|
||||
double imax = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmax - moms.mu11 * sinmax;
|
||||
ratio = imin / imax;
|
||||
}
|
||||
else
|
||||
{
|
||||
ratio = 1;
|
||||
}
|
||||
|
||||
if (ratio < params_.minInertiaRatio || ratio >= params_.maxInertiaRatio)
|
||||
continue;
|
||||
|
||||
center.confidence = ratio * ratio;
|
||||
}
|
||||
|
||||
if (params_.filterByConvexity)
|
||||
{
|
||||
std::vector<cv::Point> hull;
|
||||
cv::convexHull(cv::Mat(contours[contour_idx]), hull);
|
||||
double area = cv::contourArea(cv::Mat(contours[contour_idx]));
|
||||
double hullArea = cv::contourArea(cv::Mat(hull));
|
||||
double ratio = area / hullArea;
|
||||
if (ratio < params_.minConvexity || ratio >= params_.maxConvexity)
|
||||
continue;
|
||||
}
|
||||
|
||||
if (moms.m00 == 0.0)
|
||||
continue;
|
||||
center.location = cv::Point2d(moms.m10 / moms.m00, moms.m01 / moms.m00);
|
||||
|
||||
if (params_.filterByColor)
|
||||
{
|
||||
if (binary_image.at<uchar>(cvRound(center.location.y), cvRound(center.location.x)) != params_.blobColor)
|
||||
continue;
|
||||
}
|
||||
|
||||
// compute blob radius
|
||||
{
|
||||
std::vector<double> dists;
|
||||
for (std::size_t point_idx = 0; point_idx < contours[contour_idx].size(); ++point_idx)
|
||||
{
|
||||
cv::Point2d pt = contours[contour_idx][point_idx];
|
||||
dists.push_back(cv::norm(center.location - pt));
|
||||
}
|
||||
std::sort(dists.begin(), dists.end());
|
||||
center.radius = (dists[(dists.size() - 1) / 2] + dists[dists.size() / 2]) / 2.;
|
||||
}
|
||||
|
||||
centers.push_back(center);
|
||||
cur_contours.push_back(contours[contour_idx]);
|
||||
}
|
||||
}
|
||||
|
||||
void BlobDetector::updateParameters(const Params& parameters)
|
||||
{
|
||||
params_ = parameters;
|
||||
}
|
||||
@ -1,489 +0,0 @@
|
||||
#include <costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h>
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <tf2/LinearMath/Vector3.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToDynamicObstacles, costmap_converter::BaseCostmapToPolygons)
|
||||
|
||||
namespace costmap_converter
|
||||
{
|
||||
|
||||
CostmapToDynamicObstacles::CostmapToDynamicObstacles() : BaseCostmapToDynamicObstacles()
|
||||
{
|
||||
ego_vel_.x = ego_vel_.y = ego_vel_.z = 0;
|
||||
costmap_ = nullptr;
|
||||
// dynamic_recfg_ = nullptr;
|
||||
}
|
||||
|
||||
CostmapToDynamicObstacles::~CostmapToDynamicObstacles()
|
||||
{
|
||||
// if(dynamic_recfg_ != nullptr)
|
||||
// delete dynamic_recfg_;
|
||||
}
|
||||
|
||||
void CostmapToDynamicObstacles::initialize(rclcpp::Node::SharedPtr nh)
|
||||
{
|
||||
BaseCostmapToPolygons::initialize(nh);
|
||||
|
||||
costmap_ = nullptr;
|
||||
|
||||
// We need the odometry from the robot to compensate the ego motion
|
||||
// rclcpp::Node::SharedPtr gn; // create odom topic w.r.t. global node handle
|
||||
odom_sub_ = nh->create_subscription<nav_msgs::msg::Odometry>(
|
||||
odom_topic_,
|
||||
rclcpp::SystemDefaultsQoS(),
|
||||
std::bind(&CostmapToDynamicObstacles::odomCallback, this, std::placeholders::_1));
|
||||
|
||||
nh->get_parameter_or<bool>("publish_static_obstacles", publish_static_obstacles_, publish_static_obstacles_);
|
||||
|
||||
//////////////////////////////////
|
||||
// Foreground detection parameters
|
||||
BackgroundSubtractor::Params bg_sub_params;
|
||||
|
||||
bg_sub_params.alpha_slow = 0.3;
|
||||
nh->get_parameter_or<double>("alpha_slow", bg_sub_params.alpha_slow, bg_sub_params.alpha_slow);
|
||||
|
||||
bg_sub_params.alpha_fast = 0.85;
|
||||
nh->get_parameter_or<double>("alpha_fast", bg_sub_params.alpha_fast, bg_sub_params.alpha_fast);
|
||||
|
||||
bg_sub_params.beta = 0.85;
|
||||
nh->get_parameter_or<double>("beta", bg_sub_params.beta, bg_sub_params.beta);
|
||||
|
||||
bg_sub_params.min_occupancy_probability = 180;
|
||||
nh->get_parameter_or<double>("min_occupancy_probability", bg_sub_params.min_occupancy_probability, bg_sub_params.min_occupancy_probability);
|
||||
|
||||
bg_sub_params.min_sep_between_fast_and_slow_filter = 80;
|
||||
nh->get_parameter_or<double>("min_sep_between_slow_and_fast_filter", bg_sub_params.min_sep_between_fast_and_slow_filter, bg_sub_params.min_sep_between_fast_and_slow_filter);
|
||||
|
||||
bg_sub_params.max_occupancy_neighbors = 100;
|
||||
nh->get_parameter_or<double>("max_occupancy_neighbors", bg_sub_params.max_occupancy_neighbors, bg_sub_params.max_occupancy_neighbors);
|
||||
|
||||
bg_sub_params.morph_size = 1;
|
||||
nh->get_parameter_or<int>("morph_size", bg_sub_params.morph_size, bg_sub_params.morph_size);
|
||||
|
||||
bg_sub_ = std::unique_ptr<BackgroundSubtractor>(new BackgroundSubtractor(bg_sub_params));
|
||||
|
||||
////////////////////////////
|
||||
// Blob detection parameters
|
||||
BlobDetector::Params blob_det_params;
|
||||
|
||||
blob_det_params.filterByColor = true; // actually filterByIntensity, always true
|
||||
blob_det_params.blobColor = 255; // Extract light blobs
|
||||
blob_det_params.thresholdStep = 256; // Input for blob detection is already a binary image
|
||||
blob_det_params.minThreshold = 127;
|
||||
blob_det_params.maxThreshold = 255;
|
||||
blob_det_params.minRepeatability = 1;
|
||||
|
||||
blob_det_params.minDistBetweenBlobs = 10;
|
||||
nh->get_parameter_or<float>("min_distance_between_blobs", blob_det_params.minDistBetweenBlobs, blob_det_params.minDistBetweenBlobs);
|
||||
|
||||
blob_det_params.filterByArea = true;
|
||||
nh->get_parameter_or<bool>("filter_by_area", blob_det_params.filterByArea, blob_det_params.filterByArea);
|
||||
|
||||
blob_det_params.minArea = 3; // Filter out blobs with less pixels
|
||||
nh->get_parameter_or<float>("min_area", blob_det_params.minArea, blob_det_params.minArea);
|
||||
|
||||
blob_det_params.maxArea = 300;
|
||||
nh->get_parameter_or<float>("max_area", blob_det_params.maxArea, blob_det_params.maxArea);
|
||||
|
||||
blob_det_params.filterByCircularity = true; // circularity = 4*pi*area/perimeter^2
|
||||
nh->get_parameter_or<bool>("filter_by_circularity", blob_det_params.filterByCircularity, blob_det_params.filterByCircularity);
|
||||
|
||||
blob_det_params.minCircularity = 0.2;
|
||||
nh->get_parameter_or<float>("min_circularity", blob_det_params.minCircularity, blob_det_params.minCircularity);
|
||||
|
||||
blob_det_params.maxCircularity = 1; // maximal 1 (in case of a circle)
|
||||
nh->get_parameter_or<float>("max_circularity", blob_det_params.maxCircularity, blob_det_params.maxCircularity);
|
||||
|
||||
blob_det_params.filterByInertia = true; // Filter blobs based on their elongation
|
||||
nh->get_parameter_or<bool>("filter_by_intertia", blob_det_params.filterByInertia, blob_det_params.filterByInertia);
|
||||
|
||||
blob_det_params.minInertiaRatio = 0.2; // minimal 0 (in case of a line)
|
||||
nh->get_parameter_or<float>("min_inertia_ratio", blob_det_params.minInertiaRatio, blob_det_params.minInertiaRatio);
|
||||
|
||||
blob_det_params.maxInertiaRatio = 1; // maximal 1 (in case of a circle)
|
||||
nh->get_parameter_or<float>("max_intertia_ratio", blob_det_params.maxInertiaRatio, blob_det_params.maxInertiaRatio);
|
||||
|
||||
blob_det_params.filterByConvexity = false; // Area of the Blob / Area of its convex hull
|
||||
nh->get_parameter_or<bool>("filter_by_convexity", blob_det_params.filterByConvexity, blob_det_params.filterByConvexity);
|
||||
|
||||
blob_det_params.minConvexity = 0; // minimal 0
|
||||
nh->get_parameter_or<float>("min_convexity", blob_det_params.minConvexity, blob_det_params.minConvexity);
|
||||
|
||||
blob_det_params.maxConvexity = 1; // maximal 1
|
||||
nh->get_parameter_or<float>("max_convexity", blob_det_params.maxConvexity, blob_det_params.maxConvexity);
|
||||
|
||||
blob_det_ = BlobDetector::create(blob_det_params);
|
||||
|
||||
////////////////////////////////////
|
||||
// Tracking parameters
|
||||
CTracker::Params tracker_params;
|
||||
tracker_params.dt = 0.2;
|
||||
nh->get_parameter_or<float>("dt", tracker_params.dt, tracker_params.dt);
|
||||
|
||||
tracker_params.dist_thresh = 60.0;
|
||||
nh->get_parameter_or<float>("dist_thresh", tracker_params.dist_thresh, tracker_params.dist_thresh);
|
||||
|
||||
tracker_params.max_allowed_skipped_frames = 3;
|
||||
nh->get_parameter_or<int>("max_allowed_skipped_frames", tracker_params.max_allowed_skipped_frames, tracker_params.max_allowed_skipped_frames);
|
||||
|
||||
tracker_params.max_trace_length = 10;
|
||||
nh->get_parameter_or<int>("max_trace_length", tracker_params.max_trace_length, tracker_params.max_trace_length);
|
||||
|
||||
tracker_ = std::unique_ptr<CTracker>(new CTracker(tracker_params));
|
||||
|
||||
|
||||
////////////////////////////////////
|
||||
// Static costmap conversion parameters
|
||||
std::string static_converter_plugin = "costmap_converter::CostmapToPolygonsDBSMCCH";
|
||||
nh->get_parameter_or<std::string>("static_converter_plugin", static_converter_plugin, static_converter_plugin);
|
||||
loadStaticCostmapConverterPlugin(static_converter_plugin, nh);
|
||||
|
||||
|
||||
// setup dynamic reconfigure
|
||||
// dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>(nh);
|
||||
// dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>::CallbackType cb = boost::bind(&CostmapToDynamicObstacles::reconfigureCB, this, _1, _2);
|
||||
// dynamic_recfg_->setCallback(cb);
|
||||
}
|
||||
|
||||
void CostmapToDynamicObstacles::compute()
|
||||
{
|
||||
if (costmap_mat_.empty())
|
||||
return;
|
||||
|
||||
/////////////////////////// Foreground detection ////////////////////////////////////
|
||||
// Dynamic obstacles are separated from static obstacles
|
||||
int origin_x = round(costmap_->getOriginX() / costmap_->getResolution());
|
||||
int origin_y = round(costmap_->getOriginY() / costmap_->getResolution());
|
||||
// ROS_INFO("Origin x [m]: %f Origin_y [m]: %f", costmap_->getOriginX(), costmap_->getOriginY());
|
||||
// ROS_INFO("Origin x [px]: %d \t Origin_y [px]: %d", originX, originY);
|
||||
|
||||
bg_sub_->apply(costmap_mat_, fg_mask_, origin_x, origin_y);
|
||||
|
||||
// if no foreground object is detected, no ObstacleMsgs need to be published
|
||||
if (fg_mask_.empty())
|
||||
return;
|
||||
|
||||
cv::Mat bg_mat;
|
||||
if (publish_static_obstacles_)
|
||||
{
|
||||
// Get static obstacles
|
||||
bg_mat = costmap_mat_ - fg_mask_;
|
||||
// visualize("bg_mat", bg_mat);
|
||||
}
|
||||
|
||||
|
||||
/////////////////////////////// Blob detection /////////////////////////////////////
|
||||
// Centers and contours of Blobs are detected
|
||||
blob_det_->detect(fg_mask_, keypoints_);
|
||||
std::vector<std::vector<cv::Point>> contours = blob_det_->getContours();
|
||||
|
||||
|
||||
////////////////////////////// Tracking ////////////////////////////////////////////
|
||||
// Objects are assigned to objects from previous frame based on Hungarian Algorithm
|
||||
// Object velocities are estimated using a Kalman Filter
|
||||
std::vector<Point_t> detected_centers(keypoints_.size());
|
||||
for (size_t i = 0; i < keypoints_.size(); i++)
|
||||
{
|
||||
detected_centers.at(i).x = keypoints_.at(i).pt.x;
|
||||
detected_centers.at(i).y = keypoints_.at(i).pt.y;
|
||||
detected_centers.at(i).z = 0; // Currently unused!
|
||||
}
|
||||
|
||||
tracker_->Update(detected_centers, contours);
|
||||
|
||||
|
||||
///////////////////////////////////// Output ///////////////////////////////////////
|
||||
/*
|
||||
cv::Mat fg_mask_with_keypoints = cv::Mat::zeros(fg_mask.size(), CV_8UC3);
|
||||
cv::cvtColor(fg_mask, fg_mask_with_keypoints, cv::COLOR_GRAY2BGR);
|
||||
|
||||
for (int i = 0; i < (int)tracker_->tracks.size(); ++i)
|
||||
cv::rectangle(fg_mask_with_keypoints, cv::boundingRect(tracker_->tracks[i]->getLastContour()),
|
||||
cv::Scalar(0, 0, 255), 1);
|
||||
|
||||
//visualize("fgMaskWithKeyPoints", fgMaskWithKeypoints);
|
||||
//cv::imwrite("/home/albers/Desktop/fgMask.png", fgMask);
|
||||
//cv::imwrite("/home/albers/Desktop/fgMaskWithKeyPoints.png", fgMaskWithKeypoints);
|
||||
*/
|
||||
|
||||
//////////////////////////// Fill ObstacleContainerPtr /////////////////////////////
|
||||
ObstacleArrayPtr obstacles(new costmap_converter_msgs::msg::ObstacleArrayMsg);
|
||||
// header.seq is automatically filled
|
||||
obstacles->header.stamp = now();
|
||||
obstacles->header.frame_id = "/map"; //Global frame /map
|
||||
|
||||
// For all tracked objects
|
||||
for (unsigned int i = 0; i < (unsigned int)tracker_->tracks.size(); ++i)
|
||||
{
|
||||
geometry_msgs::msg::Polygon polygon;
|
||||
|
||||
// TODO directly create polygon inside getContour and avoid copy
|
||||
std::vector<Point_t> contour;
|
||||
getContour(i, contour); // this method also transforms map to world coordinates
|
||||
|
||||
// convert contour to polygon
|
||||
for (const Point_t& pt : contour)
|
||||
{
|
||||
polygon.points.emplace_back();
|
||||
polygon.points.back().x = pt.x;
|
||||
polygon.points.back().y = pt.y;
|
||||
polygon.points.back().z = 0;
|
||||
}
|
||||
|
||||
obstacles->obstacles.emplace_back();
|
||||
obstacles->obstacles.back().polygon = polygon;
|
||||
|
||||
// Set obstacle ID
|
||||
obstacles->obstacles.back().id = tracker_->tracks.at(i)->track_id;
|
||||
|
||||
// Set orientation
|
||||
geometry_msgs::msg::QuaternionStamped orientation;
|
||||
|
||||
Point_t vel = getEstimatedVelocityOfObject(i);
|
||||
double yaw = std::atan2(vel.y, vel.x);
|
||||
//ROS_INFO("yaw: %f", yaw);
|
||||
tf2::Quaternion q;
|
||||
q.setRPY(0, 0, yaw);
|
||||
obstacles->obstacles.back().orientation = tf2::toMsg(q);
|
||||
|
||||
// Set velocity
|
||||
geometry_msgs::msg::TwistWithCovariance velocities;
|
||||
//velocities.twist.linear.x = std::sqrt(vel.x*vel.x + vel.y*vel.y);
|
||||
//velocities.twist.linear.y = 0;
|
||||
velocities.twist.linear.x = vel.x;
|
||||
velocities.twist.linear.y = vel.y; // TODO(roesmann): don't we need to consider the transformation between opencv's and costmap's coordinate frames?
|
||||
velocities.twist.linear.z = 0;
|
||||
velocities.twist.angular.x = 0;
|
||||
velocities.twist.angular.y = 0;
|
||||
velocities.twist.angular.z = 0;
|
||||
|
||||
// TODO: use correct covariance matrix
|
||||
velocities.covariance = {1, 0, 0, 0, 0, 0,
|
||||
0, 1, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 0, 0,
|
||||
0, 0, 0, 1, 0, 0,
|
||||
0, 0, 0, 0, 1, 0,
|
||||
0, 0, 0, 0, 0, 1};
|
||||
|
||||
obstacles->obstacles.back().velocities = velocities;
|
||||
}
|
||||
|
||||
////////////////////////// Static obstacles ////////////////////////////
|
||||
if (publish_static_obstacles_)
|
||||
{
|
||||
uchar* img_data = bg_mat.data;
|
||||
int width = bg_mat.cols;
|
||||
int height = bg_mat.rows;
|
||||
int stride = bg_mat.step;
|
||||
|
||||
if (stackedCostmapConversion())
|
||||
{
|
||||
// Create new costmap with static obstacles (background)
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2D> static_costmap(new nav2_costmap_2d::Costmap2D(costmap_->getSizeInCellsX(),
|
||||
costmap_->getSizeInCellsY(),
|
||||
costmap_->getResolution(),
|
||||
costmap_->getOriginX(),
|
||||
costmap_->getOriginY()));
|
||||
for(int i = 0; i < height; i++)
|
||||
{
|
||||
for(int j = 0; j < width; j++)
|
||||
{
|
||||
static_costmap->setCost(j, i, img_data[i * stride + j]);
|
||||
}
|
||||
}
|
||||
|
||||
// Apply static obstacle conversion plugin
|
||||
setStaticCostmap(static_costmap);
|
||||
convertStaticObstacles();
|
||||
|
||||
// Store converted static obstacles for publishing
|
||||
auto static_polygons = getStaticPolygons();
|
||||
for (auto it = static_polygons->begin(); it != static_polygons->end(); ++it)
|
||||
{
|
||||
obstacles->obstacles.emplace_back();
|
||||
obstacles->obstacles.back().polygon = *it;
|
||||
obstacles->obstacles.back().velocities.twist.linear.x = 0;
|
||||
obstacles->obstacles.back().velocities.twist.linear.y = 0;
|
||||
obstacles->obstacles.back().id = -1;
|
||||
}
|
||||
}
|
||||
// Otherwise leave static obstacles point-shaped
|
||||
else
|
||||
{
|
||||
for(int i = 0; i < height; i++)
|
||||
{
|
||||
for(int j = 0; j < width; j++)
|
||||
{
|
||||
uchar value = img_data[i * stride + j];
|
||||
if (value > 0)
|
||||
{
|
||||
// obstacle found
|
||||
obstacles->obstacles.emplace_back();
|
||||
geometry_msgs::msg::Point32 pt;
|
||||
pt.x = (double)j*costmap_->getResolution() + costmap_->getOriginX();
|
||||
pt.y = (double)i*costmap_->getResolution() + costmap_->getOriginY();
|
||||
obstacles->obstacles.back().polygon.points.push_back(pt);
|
||||
obstacles->obstacles.back().velocities.twist.linear.x = 0;
|
||||
obstacles->obstacles.back().velocities.twist.linear.y = 0;
|
||||
obstacles->obstacles.back().id = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
updateObstacleContainer(obstacles);
|
||||
}
|
||||
|
||||
void CostmapToDynamicObstacles::setCostmap2D(nav2_costmap_2d::Costmap2D* costmap)
|
||||
{
|
||||
if (!costmap)
|
||||
return;
|
||||
|
||||
costmap_ = costmap;
|
||||
|
||||
updateCostmap2D();
|
||||
}
|
||||
|
||||
void CostmapToDynamicObstacles::updateCostmap2D()
|
||||
{
|
||||
if (!costmap_->getMutex())
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "Cannot update costmap since the mutex pointer is null");
|
||||
return;
|
||||
}
|
||||
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*costmap_->getMutex());
|
||||
|
||||
// Initialize costmapMat_ directly with the unsigned char array of costmap_
|
||||
//costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1,
|
||||
// costmap_->getCharMap()).clone(); // Deep copy: TODO
|
||||
costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1,
|
||||
costmap_->getCharMap());
|
||||
}
|
||||
|
||||
ObstacleArrayConstPtr CostmapToDynamicObstacles::getObstacles()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return obstacles_;
|
||||
}
|
||||
|
||||
void CostmapToDynamicObstacles::updateObstacleContainer(ObstacleArrayPtr obstacles)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
obstacles_ = obstacles;
|
||||
}
|
||||
|
||||
Point_t CostmapToDynamicObstacles::getEstimatedVelocityOfObject(unsigned int idx)
|
||||
{
|
||||
// vel [px/s] * costmapResolution [m/px] = vel [m/s]
|
||||
Point_t vel = tracker_->tracks.at(idx)->getEstimatedVelocity() * costmap_->getResolution() + ego_vel_;
|
||||
|
||||
//ROS_INFO("vel x: %f, vel y: %f, vel z: %f", vel.x, vel.y, vel.z);
|
||||
// velocity in /map frame
|
||||
return vel;
|
||||
}
|
||||
|
||||
void CostmapToDynamicObstacles::odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
|
||||
{
|
||||
RCLCPP_INFO_ONCE(getLogger(), "CostmapToDynamicObstacles: odom received.");
|
||||
|
||||
tf2::Quaternion pose;
|
||||
tf2::fromMsg(msg->pose.pose.orientation, pose);
|
||||
|
||||
tf2::Vector3 twistLinear;
|
||||
// tf2::fromMsg(msg->twist.twist.linear, twistLinear); // not available in tf2
|
||||
twistLinear.setX(msg->twist.twist.linear.x);
|
||||
twistLinear.setY(msg->twist.twist.linear.y);
|
||||
twistLinear.setZ(msg->twist.twist.linear.z);
|
||||
|
||||
|
||||
// velocity of the robot in x, y and z coordinates
|
||||
tf2::Vector3 vel = tf2::quatRotate(pose, twistLinear);
|
||||
ego_vel_.x = vel.x();
|
||||
ego_vel_.y = vel.y();
|
||||
ego_vel_.z = vel.z();
|
||||
}
|
||||
|
||||
//void CostmapToDynamicObstacles::reconfigureCB(CostmapToDynamicObstaclesConfig& config, uint32_t level)
|
||||
//{
|
||||
// publish_static_obstacles_ = config.publish_static_obstacles;
|
||||
|
||||
// // BackgroundSubtraction Parameters
|
||||
// BackgroundSubtractor::Params bg_sub_params;
|
||||
// bg_sub_params.alpha_slow = config.alpha_slow;
|
||||
// bg_sub_params.alpha_fast = config.alpha_fast;
|
||||
// bg_sub_params.beta = config.beta;
|
||||
// bg_sub_params.min_sep_between_fast_and_slow_filter = config.min_sep_between_slow_and_fast_filter;
|
||||
// bg_sub_params.min_occupancy_probability = config.min_occupancy_probability;
|
||||
// bg_sub_params.max_occupancy_neighbors = config.max_occupancy_neighbors;
|
||||
// bg_sub_params.morph_size = config.morph_size;
|
||||
// bg_sub_->updateParameters(bg_sub_params);
|
||||
|
||||
// // BlobDetector Parameters
|
||||
// BlobDetector::Params blob_det_params;
|
||||
// // necessary, because blobDetParams are otherwise initialized with default values for dark blobs
|
||||
// blob_det_params.filterByColor = true; // actually filterByIntensity, always true
|
||||
// blob_det_params.blobColor = 255; // Extract light blobs
|
||||
// blob_det_params.thresholdStep = 256; // Input for blobDetection is already a binary image
|
||||
// blob_det_params.minThreshold = 127;
|
||||
// blob_det_params.maxThreshold = 255;
|
||||
// blob_det_params.minRepeatability = 1;
|
||||
// blob_det_params.minDistBetweenBlobs = config.min_distance_between_blobs; // TODO: Currently not working as expected
|
||||
// blob_det_params.filterByArea = config.filter_by_area;
|
||||
// blob_det_params.minArea = config.min_area;
|
||||
// blob_det_params.maxArea = config.max_area;
|
||||
// blob_det_params.filterByCircularity = config.filter_by_circularity;
|
||||
// blob_det_params.minCircularity = config.min_circularity;
|
||||
// blob_det_params.maxCircularity = config.max_circularity;
|
||||
// blob_det_params.filterByInertia = config.filter_by_inertia;
|
||||
// blob_det_params.minInertiaRatio = config.min_inertia_ratio;
|
||||
// blob_det_params.maxInertiaRatio = config.max_inertia_ratio;
|
||||
// blob_det_params.filterByConvexity = config.filter_by_convexity;
|
||||
// blob_det_params.minConvexity = config.min_convexity;
|
||||
// blob_det_params.maxConvexity = config.max_convexity;
|
||||
// blob_det_->updateParameters(blob_det_params);
|
||||
|
||||
// // Tracking Parameters
|
||||
// CTracker::Params tracking_params;
|
||||
// tracking_params.dt = config.dt;
|
||||
// tracking_params.dist_thresh = config.dist_thresh;
|
||||
// tracking_params.max_allowed_skipped_frames = config.max_allowed_skipped_frames;
|
||||
// tracking_params.max_trace_length = config.max_trace_length;
|
||||
// tracker_->updateParameters(tracking_params);
|
||||
//}
|
||||
|
||||
void CostmapToDynamicObstacles::getContour(unsigned int idx, std::vector<Point_t>& contour)
|
||||
{
|
||||
assert(!tracker_->tracks.empty() && idx < tracker_->tracks.size());
|
||||
|
||||
contour.clear();
|
||||
|
||||
// contour [px] * costmapResolution [m/px] = contour [m]
|
||||
std::vector<cv::Point> contour2i = tracker_->tracks.at(idx)->getLastContour();
|
||||
|
||||
contour.reserve(contour2i.size());
|
||||
|
||||
Point_t costmap_origin(costmap_->getOriginX(), costmap_->getOriginY(), 0);
|
||||
|
||||
for (std::size_t i = 0; i < contour2i.size(); ++i)
|
||||
{
|
||||
contour.push_back((Point_t(contour2i.at(i).x, contour2i.at(i).y, 0.0)*costmap_->getResolution())
|
||||
+ costmap_origin); // Shift to /map
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void CostmapToDynamicObstacles::visualize(const std::string& name, const cv::Mat& image)
|
||||
{
|
||||
if (!image.empty())
|
||||
{
|
||||
cv::Mat im = image.clone();
|
||||
cv::flip(im, im, 0);
|
||||
cv::imshow(name, im);
|
||||
cv::waitKey(1);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,130 +0,0 @@
|
||||
// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
|
||||
// Refer to README.md in this directory.
|
||||
|
||||
#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h>
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Tracker. Manage tracks. Create, remove, update.
|
||||
// ---------------------------------------------------------------------------
|
||||
CTracker::CTracker(const Params ¶meters)
|
||||
: params(parameters),
|
||||
NextTrackID(0)
|
||||
{
|
||||
}
|
||||
// ---------------------------------------------------------------------------
|
||||
//
|
||||
// ---------------------------------------------------------------------------
|
||||
void CTracker::Update(const std::vector<Point_t>& detectedCentroid, const std::vector< std::vector<cv::Point> >& contours)
|
||||
{
|
||||
// Each contour has a centroid
|
||||
assert(detectedCentroid.size() == contours.size());
|
||||
|
||||
// -----------------------------------
|
||||
// If there is no tracks yet, then every cv::Point begins its own track.
|
||||
// -----------------------------------
|
||||
if (tracks.size() == 0)
|
||||
{
|
||||
// If no tracks yet
|
||||
for (size_t i = 0; i < detectedCentroid.size(); ++i)
|
||||
{
|
||||
tracks.push_back(
|
||||
std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
|
||||
}
|
||||
}
|
||||
|
||||
size_t N = tracks.size();
|
||||
size_t M = detectedCentroid.size();
|
||||
|
||||
assignments_t assignment;
|
||||
|
||||
if (!tracks.empty())
|
||||
{
|
||||
// Distance matrix of N-th Track to the M-th detectedCentroid
|
||||
distMatrix_t Cost(N * M);
|
||||
|
||||
// calculate distance between the blobs centroids
|
||||
for (size_t i = 0; i < tracks.size(); i++)
|
||||
{
|
||||
for (size_t j = 0; j < detectedCentroid.size(); j++)
|
||||
{
|
||||
Cost[i + j * N] = tracks[i]->CalcDist(detectedCentroid[j]);
|
||||
}
|
||||
}
|
||||
|
||||
// -----------------------------------
|
||||
// Solving assignment problem (tracks and predictions of Kalman filter)
|
||||
// -----------------------------------
|
||||
AssignmentProblemSolver APS;
|
||||
APS.Solve(Cost, N, M, assignment, AssignmentProblemSolver::optimal);
|
||||
|
||||
// -----------------------------------
|
||||
// clean assignment from pairs with large distance
|
||||
// -----------------------------------
|
||||
for (size_t i = 0; i < assignment.size(); i++)
|
||||
{
|
||||
if (assignment[i] != -1)
|
||||
{
|
||||
if (Cost[i + assignment[i] * N] > params.dist_thresh)
|
||||
{
|
||||
assignment[i] = -1;
|
||||
tracks[i]->skipped_frames = 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// If track have no assigned detect, then increment skipped frames counter.
|
||||
tracks[i]->skipped_frames++;
|
||||
}
|
||||
}
|
||||
|
||||
// -----------------------------------
|
||||
// If track didn't get detects long time, remove it.
|
||||
// -----------------------------------
|
||||
for (int i = 0; i < static_cast<int>(tracks.size()); i++)
|
||||
{
|
||||
if ((int)tracks[i]->skipped_frames > params.max_allowed_skipped_frames)
|
||||
{
|
||||
tracks.erase(tracks.begin() + i);
|
||||
assignment.erase(assignment.begin() + i);
|
||||
i--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// -----------------------------------
|
||||
// Search for unassigned detects and start new tracks for them.
|
||||
// -----------------------------------
|
||||
for (size_t i = 0; i < detectedCentroid.size(); ++i)
|
||||
{
|
||||
if (find(assignment.begin(), assignment.end(), i) == assignment.end())
|
||||
{
|
||||
tracks.push_back(std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
|
||||
}
|
||||
}
|
||||
|
||||
// Update Kalman Filters state
|
||||
|
||||
for (size_t i = 0; i < assignment.size(); i++)
|
||||
{
|
||||
// If track updated less than one time, than filter state is not correct.
|
||||
|
||||
if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates,
|
||||
{
|
||||
tracks[i]->skipped_frames = 0;
|
||||
tracks[i]->Update(detectedCentroid[assignment[i]], contours[assignment[i]], true, params.max_trace_length);
|
||||
}
|
||||
else // if not continue using predictions
|
||||
{
|
||||
tracks[i]->Update(Point_t(), std::vector<cv::Point>(), false, params.max_trace_length);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CTracker::updateParameters(const Params ¶meters)
|
||||
{
|
||||
params = parameters;
|
||||
}
|
||||
// ---------------------------------------------------------------------------
|
||||
//
|
||||
// ---------------------------------------------------------------------------
|
||||
CTracker::~CTracker(void) {}
|
||||
@ -1,732 +0,0 @@
|
||||
// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
|
||||
// Refer to README.md in this directory.
|
||||
|
||||
#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h>
|
||||
#include <limits>
|
||||
|
||||
AssignmentProblemSolver::AssignmentProblemSolver() {}
|
||||
|
||||
AssignmentProblemSolver::~AssignmentProblemSolver() {}
|
||||
|
||||
track_t AssignmentProblemSolver::Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns,
|
||||
std::vector<int>& assignment, TMethod Method)
|
||||
{
|
||||
assignment.resize(nOfRows, -1);
|
||||
|
||||
track_t cost = 0;
|
||||
|
||||
switch (Method)
|
||||
{
|
||||
case optimal:
|
||||
assignmentoptimal(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
|
||||
break;
|
||||
|
||||
case many_forbidden_assignments:
|
||||
assignmentsuboptimal1(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
|
||||
break;
|
||||
|
||||
case without_forbidden_assignments:
|
||||
assignmentsuboptimal2(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
|
||||
break;
|
||||
}
|
||||
|
||||
return cost;
|
||||
}
|
||||
// --------------------------------------------------------------------------
|
||||
// Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
|
||||
// --------------------------------------------------------------------------
|
||||
void AssignmentProblemSolver::assignmentoptimal(assignments_t& assignment, track_t& cost,
|
||||
const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
|
||||
{
|
||||
// Generate distance cv::Matrix
|
||||
// and check cv::Matrix elements positiveness :)
|
||||
|
||||
// Total elements number
|
||||
size_t nOfElements = nOfRows * nOfColumns;
|
||||
// Memory allocation
|
||||
track_t* distMatrix = (track_t*)malloc(nOfElements * sizeof(track_t));
|
||||
// Pointer to last element
|
||||
track_t* distMatrixEnd = distMatrix + nOfElements;
|
||||
|
||||
for (size_t row = 0; row < nOfElements; row++)
|
||||
{
|
||||
track_t value = distMatrixIn[row];
|
||||
assert(value >= 0);
|
||||
distMatrix[row] = value;
|
||||
}
|
||||
|
||||
// Memory allocation
|
||||
bool* coveredColumns = (bool*)calloc(nOfColumns, sizeof(bool));
|
||||
bool* coveredRows = (bool*)calloc(nOfRows, sizeof(bool));
|
||||
bool* starMatrix = (bool*)calloc(nOfElements, sizeof(bool));
|
||||
bool* primeMatrix = (bool*)calloc(nOfElements, sizeof(bool));
|
||||
bool* newStarMatrix = (bool*)calloc(nOfElements, sizeof(bool)); /* used in step4 */
|
||||
|
||||
/* preliminary steps */
|
||||
if (nOfRows <= nOfColumns)
|
||||
{
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
/* find the smallest element in the row */
|
||||
track_t* distMatrixTemp = distMatrix + row;
|
||||
track_t minValue = *distMatrixTemp;
|
||||
distMatrixTemp += nOfRows;
|
||||
while (distMatrixTemp < distMatrixEnd)
|
||||
{
|
||||
track_t value = *distMatrixTemp;
|
||||
if (value < minValue)
|
||||
{
|
||||
minValue = value;
|
||||
}
|
||||
distMatrixTemp += nOfRows;
|
||||
}
|
||||
/* subtract the smallest element from each element of the row */
|
||||
distMatrixTemp = distMatrix + row;
|
||||
while (distMatrixTemp < distMatrixEnd)
|
||||
{
|
||||
*distMatrixTemp -= minValue;
|
||||
distMatrixTemp += nOfRows;
|
||||
}
|
||||
}
|
||||
/* Steps 1 and 2a */
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
if (distMatrix[row + nOfRows * col] == 0)
|
||||
{
|
||||
if (!coveredColumns[col])
|
||||
{
|
||||
starMatrix[row + nOfRows * col] = true;
|
||||
coveredColumns[col] = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else /* if(nOfRows > nOfColumns) */
|
||||
{
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
/* find the smallest element in the column */
|
||||
track_t* distMatrixTemp = distMatrix + nOfRows * col;
|
||||
track_t* columnEnd = distMatrixTemp + nOfRows;
|
||||
track_t minValue = *distMatrixTemp++;
|
||||
while (distMatrixTemp < columnEnd)
|
||||
{
|
||||
track_t value = *distMatrixTemp++;
|
||||
if (value < minValue)
|
||||
{
|
||||
minValue = value;
|
||||
}
|
||||
}
|
||||
/* subtract the smallest element from each element of the column */
|
||||
distMatrixTemp = distMatrix + nOfRows * col;
|
||||
while (distMatrixTemp < columnEnd)
|
||||
{
|
||||
*distMatrixTemp++ -= minValue;
|
||||
}
|
||||
}
|
||||
/* Steps 1 and 2a */
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
if (distMatrix[row + nOfRows * col] == 0)
|
||||
{
|
||||
if (!coveredRows[row])
|
||||
{
|
||||
starMatrix[row + nOfRows * col] = true;
|
||||
coveredColumns[col] = true;
|
||||
coveredRows[row] = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
coveredRows[row] = false;
|
||||
}
|
||||
}
|
||||
/* move to step 2b */
|
||||
step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
|
||||
nOfColumns, (nOfRows <= nOfColumns) ? nOfRows : nOfColumns);
|
||||
/* compute cost and remove invalid assignments */
|
||||
computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);
|
||||
/* free allocated memory */
|
||||
free(distMatrix);
|
||||
free(coveredColumns);
|
||||
free(coveredRows);
|
||||
free(starMatrix);
|
||||
free(primeMatrix);
|
||||
free(newStarMatrix);
|
||||
return;
|
||||
}
|
||||
// --------------------------------------------------------------------------
|
||||
//
|
||||
// --------------------------------------------------------------------------
|
||||
void AssignmentProblemSolver::buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows,
|
||||
size_t nOfColumns)
|
||||
{
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
if (starMatrix[row + nOfRows * col])
|
||||
{
|
||||
assignment[row] = static_cast<int>(col);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// --------------------------------------------------------------------------
|
||||
//
|
||||
// --------------------------------------------------------------------------
|
||||
void AssignmentProblemSolver::computeassignmentcost(const assignments_t& assignment, track_t& cost,
|
||||
const distMatrix_t& distMatrixIn, size_t nOfRows)
|
||||
{
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
const int col = assignment[row];
|
||||
if (col >= 0)
|
||||
{
|
||||
cost += distMatrixIn[row + nOfRows * col];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// --------------------------------------------------------------------------
|
||||
//
|
||||
// --------------------------------------------------------------------------
|
||||
void AssignmentProblemSolver::step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
|
||||
bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
|
||||
size_t nOfRows, size_t nOfColumns, size_t minDim)
|
||||
{
|
||||
bool* starMatrixTemp, *columnEnd;
|
||||
/* cover every column containing a starred zero */
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
starMatrixTemp = starMatrix + nOfRows * col;
|
||||
columnEnd = starMatrixTemp + nOfRows;
|
||||
while (starMatrixTemp < columnEnd)
|
||||
{
|
||||
if (*starMatrixTemp++)
|
||||
{
|
||||
coveredColumns[col] = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
/* move to step 3 */
|
||||
step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
|
||||
nOfColumns, minDim);
|
||||
}
|
||||
|
||||
// --------------------------------------------------------------------------
|
||||
//
|
||||
// --------------------------------------------------------------------------
|
||||
void AssignmentProblemSolver::step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
|
||||
bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
|
||||
size_t nOfRows, size_t nOfColumns, size_t minDim)
|
||||
{
|
||||
/* count covered columns */
|
||||
size_t nOfCoveredColumns = 0;
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
if (coveredColumns[col])
|
||||
{
|
||||
nOfCoveredColumns++;
|
||||
}
|
||||
}
|
||||
if (nOfCoveredColumns == minDim)
|
||||
{
|
||||
/* algorithm finished */
|
||||
buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
|
||||
}
|
||||
else
|
||||
{
|
||||
/* move to step 3 */
|
||||
step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
|
||||
nOfColumns, minDim);
|
||||
}
|
||||
}
|
||||
|
||||
// --------------------------------------------------------------------------
|
||||
//
|
||||
// --------------------------------------------------------------------------
|
||||
void AssignmentProblemSolver::step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
|
||||
bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
|
||||
size_t nOfRows, size_t nOfColumns, size_t minDim)
|
||||
{
|
||||
bool zerosFound = true;
|
||||
while (zerosFound)
|
||||
{
|
||||
zerosFound = false;
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
if (!coveredColumns[col])
|
||||
{
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
if ((!coveredRows[row]) && (distMatrix[row + nOfRows * col] == 0))
|
||||
{
|
||||
/* prime zero */
|
||||
primeMatrix[row + nOfRows * col] = true;
|
||||
/* find starred zero in current row */
|
||||
size_t starCol = 0;
|
||||
for (; starCol < nOfColumns; starCol++)
|
||||
{
|
||||
if (starMatrix[row + nOfRows * starCol])
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (starCol == nOfColumns) /* no starred zero found */
|
||||
{
|
||||
/* move to step 4 */
|
||||
step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows,
|
||||
nOfRows, nOfColumns, minDim, row, col);
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
coveredRows[row] = true;
|
||||
coveredColumns[starCol] = false;
|
||||
zerosFound = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/* move to step 5 */
|
||||
step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
|
||||
nOfColumns, minDim);
|
||||
}
|
||||
|
||||
// --------------------------------------------------------------------------
|
||||
//
|
||||
// --------------------------------------------------------------------------
|
||||
void AssignmentProblemSolver::step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
|
||||
bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
|
||||
size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col)
|
||||
{
|
||||
const size_t nOfElements = nOfRows * nOfColumns;
|
||||
/* generate temporary copy of starMatrix */
|
||||
for (size_t n = 0; n < nOfElements; n++)
|
||||
{
|
||||
newStarMatrix[n] = starMatrix[n];
|
||||
}
|
||||
/* star current zero */
|
||||
newStarMatrix[row + nOfRows * col] = true;
|
||||
/* find starred zero in current column */
|
||||
size_t starCol = col;
|
||||
size_t starRow = 0;
|
||||
for (; starRow < nOfRows; starRow++)
|
||||
{
|
||||
if (starMatrix[starRow + nOfRows * starCol])
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
while (starRow < nOfRows)
|
||||
{
|
||||
/* unstar the starred zero */
|
||||
newStarMatrix[starRow + nOfRows * starCol] = false;
|
||||
/* find primed zero in current row */
|
||||
size_t primeRow = starRow;
|
||||
size_t primeCol = 0;
|
||||
for (; primeCol < nOfColumns; primeCol++)
|
||||
{
|
||||
if (primeMatrix[primeRow + nOfRows * primeCol])
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
/* star the primed zero */
|
||||
newStarMatrix[primeRow + nOfRows * primeCol] = true;
|
||||
/* find starred zero in current column */
|
||||
starCol = primeCol;
|
||||
for (starRow = 0; starRow < nOfRows; starRow++)
|
||||
{
|
||||
if (starMatrix[starRow + nOfRows * starCol])
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
/* use temporary copy as new starMatrix */
|
||||
/* delete all primes, uncover all rows */
|
||||
for (size_t n = 0; n < nOfElements; n++)
|
||||
{
|
||||
primeMatrix[n] = false;
|
||||
starMatrix[n] = newStarMatrix[n];
|
||||
}
|
||||
for (size_t n = 0; n < nOfRows; n++)
|
||||
{
|
||||
coveredRows[n] = false;
|
||||
}
|
||||
/* move to step 2a */
|
||||
step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
|
||||
nOfColumns, minDim);
|
||||
}
|
||||
|
||||
// --------------------------------------------------------------------------
|
||||
//
|
||||
// --------------------------------------------------------------------------
|
||||
void AssignmentProblemSolver::step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
|
||||
bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
|
||||
size_t nOfRows, size_t nOfColumns, size_t minDim)
|
||||
{
|
||||
/* find smallest uncovered element h */
|
||||
float h = std::numeric_limits<track_t>::max();
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
if (!coveredRows[row])
|
||||
{
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
if (!coveredColumns[col])
|
||||
{
|
||||
const float value = distMatrix[row + nOfRows * col];
|
||||
if (value < h)
|
||||
{
|
||||
h = value;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/* add h to each covered row */
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
if (coveredRows[row])
|
||||
{
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
distMatrix[row + nOfRows * col] += h;
|
||||
}
|
||||
}
|
||||
}
|
||||
/* subtract h from each uncovered column */
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
if (!coveredColumns[col])
|
||||
{
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
distMatrix[row + nOfRows * col] -= h;
|
||||
}
|
||||
}
|
||||
}
|
||||
/* move to step 3 */
|
||||
step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
|
||||
nOfColumns, minDim);
|
||||
}
|
||||
|
||||
// --------------------------------------------------------------------------
|
||||
// Computes a suboptimal solution. Good for cases without forbidden assignments.
|
||||
// --------------------------------------------------------------------------
|
||||
void AssignmentProblemSolver::assignmentsuboptimal2(assignments_t& assignment, track_t& cost,
|
||||
const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
|
||||
{
|
||||
/* make working copy of distance Matrix */
|
||||
const size_t nOfElements = nOfRows * nOfColumns;
|
||||
float* distMatrix = (float*)malloc(nOfElements * sizeof(float));
|
||||
for (size_t n = 0; n < nOfElements; n++)
|
||||
{
|
||||
distMatrix[n] = distMatrixIn[n];
|
||||
}
|
||||
|
||||
/* recursively search for the minimum element and do the assignment */
|
||||
for (;;)
|
||||
{
|
||||
/* find minimum distance observation-to-track pair */
|
||||
float minValue = std::numeric_limits<track_t>::max();
|
||||
size_t tmpRow = 0;
|
||||
size_t tmpCol = 0;
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
const float value = distMatrix[row + nOfRows * col];
|
||||
if (value != std::numeric_limits<track_t>::max() && (value < minValue))
|
||||
{
|
||||
minValue = value;
|
||||
tmpRow = row;
|
||||
tmpCol = col;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (minValue != std::numeric_limits<track_t>::max())
|
||||
{
|
||||
assignment[tmpRow] = static_cast<int>(tmpCol);
|
||||
cost += minValue;
|
||||
for (size_t n = 0; n < nOfRows; n++)
|
||||
{
|
||||
distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
|
||||
}
|
||||
for (size_t n = 0; n < nOfColumns; n++)
|
||||
{
|
||||
distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
free(distMatrix);
|
||||
}
|
||||
// --------------------------------------------------------------------------
|
||||
// Computes a suboptimal solution. Good for cases with many forbidden assignments.
|
||||
// --------------------------------------------------------------------------
|
||||
void AssignmentProblemSolver::assignmentsuboptimal1(assignments_t& assignment, track_t& cost,
|
||||
const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
|
||||
{
|
||||
/* make working copy of distance Matrix */
|
||||
const size_t nOfElements = nOfRows * nOfColumns;
|
||||
float* distMatrix = (float*)malloc(nOfElements * sizeof(float));
|
||||
for (size_t n = 0; n < nOfElements; n++)
|
||||
{
|
||||
distMatrix[n] = distMatrixIn[n];
|
||||
}
|
||||
|
||||
/* allocate memory */
|
||||
int* nOfValidObservations = (int*)calloc(nOfRows, sizeof(int));
|
||||
int* nOfValidTracks = (int*)calloc(nOfColumns, sizeof(int));
|
||||
|
||||
/* compute number of validations */
|
||||
bool infiniteValueFound = false;
|
||||
bool finiteValueFound = false;
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
|
||||
{
|
||||
nOfValidTracks[col] += 1;
|
||||
nOfValidObservations[row] += 1;
|
||||
finiteValueFound = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
infiniteValueFound = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (infiniteValueFound)
|
||||
{
|
||||
if (!finiteValueFound)
|
||||
{
|
||||
free(nOfValidTracks);
|
||||
free(nOfValidObservations);
|
||||
free(distMatrix);
|
||||
return;
|
||||
}
|
||||
bool repeatSteps = true;
|
||||
|
||||
while (repeatSteps)
|
||||
{
|
||||
repeatSteps = false;
|
||||
|
||||
/* step 1: reject assignments of multiply validated tracks to singly validated observations */
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
bool singleValidationFound = false;
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() &&
|
||||
(nOfValidObservations[row] == 1))
|
||||
{
|
||||
singleValidationFound = true;
|
||||
break;
|
||||
}
|
||||
|
||||
if (singleValidationFound)
|
||||
{
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
if ((nOfValidObservations[row] > 1) &&
|
||||
distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
|
||||
{
|
||||
distMatrix[row + nOfRows * col] = std::numeric_limits<track_t>::max();
|
||||
nOfValidObservations[row] -= 1;
|
||||
nOfValidTracks[col] -= 1;
|
||||
repeatSteps = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* step 2: reject assignments of multiply validated observations to singly validated tracks */
|
||||
if (nOfColumns > 1)
|
||||
{
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
bool singleValidationFound = false;
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() && (nOfValidTracks[col] == 1))
|
||||
{
|
||||
singleValidationFound = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (singleValidationFound)
|
||||
{
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
if ((nOfValidTracks[col] > 1) && distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
|
||||
{
|
||||
distMatrix[row + nOfRows * col] = std::numeric_limits<track_t>::max();
|
||||
nOfValidObservations[row] -= 1;
|
||||
nOfValidTracks[col] -= 1;
|
||||
repeatSteps = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} /* while(repeatSteps) */
|
||||
|
||||
/* for each multiply validated track that validates only with singly validated */
|
||||
/* observations, choose the observation with minimum distance */
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
if (nOfValidObservations[row] > 1)
|
||||
{
|
||||
bool allSinglyValidated = true;
|
||||
float minValue = std::numeric_limits<track_t>::max();
|
||||
size_t tmpCol = 0;
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
const float value = distMatrix[row + nOfRows * col];
|
||||
if (value != std::numeric_limits<track_t>::max())
|
||||
{
|
||||
if (nOfValidTracks[col] > 1)
|
||||
{
|
||||
allSinglyValidated = false;
|
||||
break;
|
||||
}
|
||||
else if ((nOfValidTracks[col] == 1) && (value < minValue))
|
||||
{
|
||||
tmpCol = col;
|
||||
minValue = value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (allSinglyValidated)
|
||||
{
|
||||
assignment[row] = static_cast<int>(tmpCol);
|
||||
cost += minValue;
|
||||
for (size_t n = 0; n < nOfRows; n++)
|
||||
{
|
||||
distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
|
||||
}
|
||||
for (size_t n = 0; n < nOfColumns; n++)
|
||||
{
|
||||
distMatrix[row + nOfRows * n] = std::numeric_limits<track_t>::max();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// for each multiply validated observation that validates only with singly validated track, choose the track with
|
||||
// minimum distance
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
if (nOfValidTracks[col] > 1)
|
||||
{
|
||||
bool allSinglyValidated = true;
|
||||
float minValue = std::numeric_limits<track_t>::max();
|
||||
size_t tmpRow = 0;
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
const float value = distMatrix[row + nOfRows * col];
|
||||
if (value != std::numeric_limits<track_t>::max())
|
||||
{
|
||||
if (nOfValidObservations[row] > 1)
|
||||
{
|
||||
allSinglyValidated = false;
|
||||
break;
|
||||
}
|
||||
else if ((nOfValidObservations[row] == 1) && (value < minValue))
|
||||
{
|
||||
tmpRow = row;
|
||||
minValue = value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (allSinglyValidated)
|
||||
{
|
||||
assignment[tmpRow] = static_cast<int>(col);
|
||||
cost += minValue;
|
||||
for (size_t n = 0; n < nOfRows; n++)
|
||||
{
|
||||
distMatrix[n + nOfRows * col] = std::numeric_limits<track_t>::max();
|
||||
}
|
||||
for (size_t n = 0; n < nOfColumns; n++)
|
||||
{
|
||||
distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} /* if(infiniteValueFound) */
|
||||
|
||||
/* now, recursively search for the minimum element and do the assignment */
|
||||
for (;;)
|
||||
{
|
||||
/* find minimum distance observation-to-track pair */
|
||||
float minValue = std::numeric_limits<track_t>::max();
|
||||
size_t tmpRow = 0;
|
||||
size_t tmpCol = 0;
|
||||
for (size_t row = 0; row < nOfRows; row++)
|
||||
{
|
||||
for (size_t col = 0; col < nOfColumns; col++)
|
||||
{
|
||||
const float value = distMatrix[row + nOfRows * col];
|
||||
if (value != std::numeric_limits<track_t>::max() && (value < minValue))
|
||||
{
|
||||
minValue = value;
|
||||
tmpRow = row;
|
||||
tmpCol = col;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (minValue != std::numeric_limits<track_t>::max())
|
||||
{
|
||||
assignment[tmpRow] = static_cast<int>(tmpCol);
|
||||
cost += minValue;
|
||||
for (size_t n = 0; n < nOfRows; n++)
|
||||
{
|
||||
distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
|
||||
}
|
||||
for (size_t n = 0; n < nOfColumns; n++)
|
||||
{
|
||||
distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* free allocated memory */
|
||||
free(nOfValidObservations);
|
||||
free(nOfValidTracks);
|
||||
free(distMatrix);
|
||||
}
|
||||
@ -1,99 +0,0 @@
|
||||
// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
|
||||
// Refer to README.md in this directory.
|
||||
|
||||
#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h>
|
||||
#include "opencv2/opencv.hpp"
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
//---------------------------------------------------------------------------
|
||||
//---------------------------------------------------------------------------
|
||||
TKalmanFilter::TKalmanFilter(Point_t pt, track_t deltatime)
|
||||
{
|
||||
// time increment (lower values makes target more "massive")
|
||||
dt = deltatime;
|
||||
|
||||
// 6 state variables [x y z xdot ydot zdot], 3 measurements [x y z]
|
||||
kalman = new cv::KalmanFilter(6, 3, 0);
|
||||
// Transition cv::Matrix
|
||||
kalman->transitionMatrix = (cv::Mat_<track_t>(6, 6) <<
|
||||
1, 0, 0, dt, 0, 0,
|
||||
0, 1, 0, 0, dt, 0,
|
||||
0, 0, 1, 0, 0, dt,
|
||||
0, 0, 0, 1, 0, 0,
|
||||
0, 0, 0, 0, 1, 0,
|
||||
0, 0, 0, 0, 0, 1);
|
||||
// init...
|
||||
LastPosition = pt;
|
||||
kalman->statePre.at<track_t>(0) = pt.x;
|
||||
kalman->statePre.at<track_t>(1) = pt.y;
|
||||
kalman->statePre.at<track_t>(2) = pt.z;
|
||||
|
||||
kalman->statePre.at<track_t>(3) = 0;
|
||||
kalman->statePre.at<track_t>(4) = 0;
|
||||
kalman->statePre.at<track_t>(5) = 0;
|
||||
|
||||
kalman->statePost.at<track_t>(0) = pt.x;
|
||||
kalman->statePost.at<track_t>(1) = pt.y;
|
||||
kalman->statePost.at<track_t>(2) = pt.z;
|
||||
|
||||
// Nur x, y und z Koordinaten messbar!
|
||||
kalman->measurementMatrix = (cv::Mat_<track_t>(3, 6) <<
|
||||
1, 0, 0, 0, 0, 0,
|
||||
0, 1, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 0, 0);
|
||||
|
||||
float sigma = 0.5; // Assume standard deviation for acceleration, todo: dynamic reconfigure
|
||||
float c1 = pow(dt,4.0)/4.0;
|
||||
float c2 = pow(dt,2.0);
|
||||
float c3 = pow(dt,3.0)/2.0;
|
||||
kalman->processNoiseCov = sigma*sigma*(cv::Mat_<float>(6, 6) <<
|
||||
c1, 0, 0, c3, 0, 0,
|
||||
0, c1, 0, 0, c3, 0,
|
||||
0, 0, c1, 0, 0, c3,
|
||||
c3, 0, 0, c2, 0, 0,
|
||||
0, c3, 0, 0, c2, 0,
|
||||
0, 0, c3, 0, 0, c2);
|
||||
|
||||
cv::setIdentity(kalman->measurementNoiseCov, cv::Scalar::all(5e-2));
|
||||
|
||||
cv::setIdentity(kalman->errorCovPost, cv::Scalar::all(1000000));
|
||||
}
|
||||
//---------------------------------------------------------------------------
|
||||
TKalmanFilter::~TKalmanFilter() { delete kalman; }
|
||||
|
||||
//---------------------------------------------------------------------------
|
||||
void TKalmanFilter::Prediction()
|
||||
{
|
||||
cv::Mat prediction = kalman->predict();
|
||||
LastPosition = Point_t(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2));
|
||||
LastVelocity = Point_t(prediction.at<track_t>(3), prediction.at<track_t>(4), prediction.at<track_t>(5));
|
||||
}
|
||||
|
||||
//---------------------------------------------------------------------------
|
||||
Point_t TKalmanFilter::Update(Point_t p, bool DataCorrect)
|
||||
{
|
||||
cv::Mat measurement(3, 1, Mat_t(1));
|
||||
if (!DataCorrect)
|
||||
{
|
||||
measurement.at<track_t>(0) = LastPosition.x; // update using prediction
|
||||
measurement.at<track_t>(1) = LastPosition.y;
|
||||
measurement.at<track_t>(2) = LastPosition.z;
|
||||
}
|
||||
else
|
||||
{
|
||||
measurement.at<track_t>(0) = p.x; // update using measurements
|
||||
measurement.at<track_t>(1) = p.y;
|
||||
measurement.at<track_t>(2) = p.z;
|
||||
}
|
||||
// Correction
|
||||
cv::Mat estimated = kalman->correct(measurement);
|
||||
LastPosition.x = estimated.at<track_t>(0); // update using measurements
|
||||
LastPosition.y = estimated.at<track_t>(1);
|
||||
LastPosition.z = estimated.at<track_t>(2);
|
||||
LastVelocity.x = estimated.at<track_t>(3);
|
||||
LastVelocity.y = estimated.at<track_t>(4);
|
||||
LastVelocity.z = estimated.at<track_t>(5);
|
||||
return LastPosition;
|
||||
}
|
||||
//---------------------------------------------------------------------------
|
||||
@ -1,13 +0,0 @@
|
||||
Multitarget Tracker
|
||||
===================
|
||||
|
||||
This code is mostly copied from the (MultitargetTracker)[https://github.com/Smorodov/Multitarget-tracker].
|
||||
It is utilized for the *CostmapToDynamicObstacles* plugin.
|
||||
|
||||
The *MultitargetTracker* is licensed under (GNU GPLv3)[https://www.gnu.org/licenses/gpl-3.0.txt].
|
||||
The package itself depends on other third party packages with different licenses (refer to the package repository).
|
||||
|
||||
TODO: Include the whole package as external CMake project.
|
||||
|
||||
|
||||
|
||||
@ -1,303 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann, Otniel Rinaldo
|
||||
*********************************************************************/
|
||||
|
||||
#include <costmap_converter/costmap_to_lines_convex_hull.h>
|
||||
#include <costmap_converter/misc.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSMCCH, costmap_converter::BaseCostmapToPolygons)
|
||||
|
||||
namespace costmap_converter
|
||||
{
|
||||
|
||||
CostmapToLinesDBSMCCH::CostmapToLinesDBSMCCH() : CostmapToPolygonsDBSMCCH()
|
||||
{
|
||||
// dynamic_recfg_ = NULL;
|
||||
}
|
||||
|
||||
CostmapToLinesDBSMCCH::~CostmapToLinesDBSMCCH()
|
||||
{
|
||||
// if (dynamic_recfg_ != NULL)
|
||||
// delete dynamic_recfg_;
|
||||
}
|
||||
|
||||
void CostmapToLinesDBSMCCH::initialize(rclcpp::Node::SharedPtr nh)
|
||||
{
|
||||
BaseCostmapToPolygons::initialize(nh);
|
||||
|
||||
// DB SCAN
|
||||
parameter_.max_distance_ = 0.4;
|
||||
nh->get_parameter_or<double>("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_);
|
||||
|
||||
parameter_.min_pts_ = 2;
|
||||
nh->get_parameter_or<int>("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_);
|
||||
|
||||
parameter_.max_pts_ = 30;
|
||||
nh->get_parameter_or<int>("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_);
|
||||
|
||||
// convex hull
|
||||
parameter_.min_keypoint_separation_ = 0.1;
|
||||
nh->get_parameter_or<double>("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_);
|
||||
|
||||
parameter_buffered_ = parameter_;
|
||||
|
||||
// Line extraction
|
||||
support_pts_max_dist_ = 0.3;
|
||||
nh->get_parameter_or<double>("support_pts_max_dist", support_pts_max_dist_, support_pts_max_dist_);
|
||||
|
||||
support_pts_max_dist_inbetween_ = 1.0;
|
||||
nh->get_parameter_or<double>("support_pts_max_dist_inbetween", support_pts_max_dist_inbetween_, support_pts_max_dist_inbetween_);
|
||||
|
||||
min_support_pts_ = 2;
|
||||
nh->get_parameter_or<int>("min_support_pts", min_support_pts_, min_support_pts_);
|
||||
|
||||
// setup dynamic reconfigure
|
||||
// dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>(nh);
|
||||
// dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSMCCH::reconfigureCB, this, _1, _2);
|
||||
// dynamic_recfg_->setCallback(cb);
|
||||
|
||||
// deprecated
|
||||
rclcpp::Parameter dummy;
|
||||
if (nh->get_parameter("support_pts_min_dist_", dummy) || nh->get_parameter("support_pts_min_dist", dummy))
|
||||
RCLCPP_WARN(nh->get_logger(), "CostmapToLinesDBSMCCH: Parameter 'support_pts_min_dist' is deprecated and not included anymore.");
|
||||
if (nh->get_parameter("min_support_pts_", dummy))
|
||||
RCLCPP_WARN(nh->get_logger(), "CostmapToLinesDBSMCCH: Parameter 'min_support_pts_' is not found. Remove the underscore.");
|
||||
}
|
||||
|
||||
void CostmapToLinesDBSMCCH::compute()
|
||||
{
|
||||
std::vector< std::vector<KeyPoint> > clusters;
|
||||
dbScan(clusters);
|
||||
|
||||
// Create new polygon container
|
||||
PolygonContainerPtr polygons(new std::vector<geometry_msgs::msg::Polygon>());
|
||||
|
||||
|
||||
// add convex hulls to polygon container
|
||||
for (size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
|
||||
{
|
||||
geometry_msgs::msg::Polygon polygon;
|
||||
convexHull2(clusters[i], polygon );
|
||||
|
||||
// now extract lines of the polygon (by searching for support points in the cluster)
|
||||
// and add them to the polygon container
|
||||
extractPointsAndLines(clusters[i], polygon, std::back_inserter(*polygons));
|
||||
}
|
||||
|
||||
// add our non-cluster points to the polygon container (as single points)
|
||||
if (!clusters.empty())
|
||||
{
|
||||
for (size_t i=0; i < clusters.front().size(); ++i)
|
||||
{
|
||||
polygons->push_back( geometry_msgs::msg::Polygon() );
|
||||
convertPointToPolygon(clusters.front()[i], polygons->back());
|
||||
}
|
||||
}
|
||||
|
||||
// replace shared polygon container
|
||||
updatePolygonContainer(polygons);
|
||||
}
|
||||
|
||||
typedef CostmapToLinesDBSMCCH CL;
|
||||
bool sort_keypoint_x(const std::size_t& i, const std::size_t& j, const std::vector<CL::KeyPoint>& cluster)
|
||||
{ return (cluster[i].x<cluster[j].x) || (cluster[i].x == cluster[j].x && cluster[i].y < cluster[j].y); }
|
||||
bool sort_keypoint_y(const std::size_t& i, const std::size_t& j, const std::vector<CL::KeyPoint>& cluster)
|
||||
{ return (cluster[i].y<cluster[j].y) || (cluster[i].y == cluster[j].y && cluster[i].x < cluster[j].x); }
|
||||
|
||||
void CostmapToLinesDBSMCCH::extractPointsAndLines(std::vector<KeyPoint>& cluster, const geometry_msgs::msg::Polygon& polygon,
|
||||
std::back_insert_iterator< std::vector<geometry_msgs::msg::Polygon> > lines)
|
||||
{
|
||||
if (polygon.points.empty())
|
||||
return;
|
||||
|
||||
if (polygon.points.size() < 2)
|
||||
{
|
||||
lines = polygon; // our polygon is just a point, push back onto the output sequence
|
||||
return;
|
||||
}
|
||||
int n = (int)polygon.points.size();
|
||||
|
||||
for (int i=1; i<n; ++i) // this implemenation requires an explicitly closed polygon (start vertex = end vertex)
|
||||
{
|
||||
const geometry_msgs::msg::Point32* vertex1 = &polygon.points[i-1];
|
||||
const geometry_msgs::msg::Point32* vertex2 = &polygon.points[i];
|
||||
|
||||
// check how many vertices belong to the line (sometimes the convex hull algorithm finds multiple vertices on a line,
|
||||
// in case of small coordinate deviations)
|
||||
double dx = vertex2->x - vertex1->x;
|
||||
double dy = vertex2->y - vertex1->y;
|
||||
// double norm = std::sqrt(dx*dx + dy*dy);
|
||||
// dx /= norm;
|
||||
// dy /= norm;
|
||||
// for (int j=i; j<(int)polygon.points.size() - 2; ++j)
|
||||
// {
|
||||
// const geometry_msgs::msg::Point32* vertex_jp2 = &polygon.points[j+2];
|
||||
// double dx2 = vertex_jp2->x - vertex2->x;
|
||||
// double dy2 = vertex_jp2->y - vertex2->y;
|
||||
// double norm2 = std::sqrt(dx2*dx2 + dy2*dy2);
|
||||
// dx2 /= norm2;
|
||||
// dy2 /= norm2;
|
||||
// if (std::abs(dx*dx2 + dy*dy2) < 0.05) //~3 degree
|
||||
// {
|
||||
// vertex2 = &polygon.points[j+2];
|
||||
// i = j; // DO NOT USE "i" afterwards
|
||||
// }
|
||||
// else break;
|
||||
// }
|
||||
|
||||
//Search support points
|
||||
std::vector<std::size_t> support_points; // store indices of cluster
|
||||
|
||||
for (std::size_t k = 0; k < cluster.size(); ++k)
|
||||
{
|
||||
bool is_inbetween = false;
|
||||
double dist_line_to_point = computeDistanceToLineSegment( cluster[k], *vertex1, *vertex2, &is_inbetween );
|
||||
|
||||
if(is_inbetween && dist_line_to_point <= support_pts_max_dist_)
|
||||
{
|
||||
support_points.push_back(k);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// now check if the inlier models a line by checking the minimum distance between all support points (avoid lines over gaps)
|
||||
// and by checking if the minium number of points is reached // deactivate by setting support_pts_max_dist_inbetween_==0
|
||||
bool is_line=true;
|
||||
if (support_pts_max_dist_inbetween_!=0)
|
||||
{
|
||||
if ((int)support_points.size() >= min_support_pts_ + 2) // +2 since start and goal are included
|
||||
{
|
||||
// sort points
|
||||
if (std::abs(dx) >= std::abs(dy))
|
||||
std::sort(support_points.begin(), support_points.end(), std::bind(sort_keypoint_x, std::placeholders::_1, std::placeholders::_2, std::cref(cluster)));
|
||||
else
|
||||
std::sort(support_points.begin(), support_points.end(), std::bind(sort_keypoint_y, std::placeholders::_1, std::placeholders::_2, std::cref(cluster)));
|
||||
|
||||
// now calculate distances
|
||||
for (int k = 1; k < int(support_points.size()); ++k)
|
||||
{
|
||||
double dist_x = cluster[support_points[k]].x - cluster[support_points[k-1]].x;
|
||||
double dist_y = cluster[support_points[k]].y - cluster[support_points[k-1]].y;
|
||||
double dist = std::sqrt( dist_x*dist_x + dist_y*dist_y);
|
||||
if (dist > support_pts_max_dist_inbetween_)
|
||||
{
|
||||
is_line = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
is_line = false;
|
||||
}
|
||||
|
||||
if (is_line)
|
||||
{
|
||||
// line found:
|
||||
geometry_msgs::msg::Polygon line;
|
||||
line.points.push_back(*vertex1);
|
||||
line.points.push_back(*vertex2);
|
||||
lines = line; // back insertion
|
||||
|
||||
// remove inlier from list
|
||||
// iterate in reverse order, otherwise indices are not valid after erasing elements
|
||||
std::vector<std::size_t>::reverse_iterator support_it = support_points.rbegin();
|
||||
for (; support_it != support_points.rend(); ++support_it)
|
||||
{
|
||||
cluster.erase(cluster.begin() + *support_it);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// remove goal, since it will be added in the subsequent iteration
|
||||
//support_points.pop_back();
|
||||
// old:
|
||||
// // add vertex 1 as single point
|
||||
// geometry_msgs::msg::Polygon point;
|
||||
// point.points.push_back(*vertex1);
|
||||
// lines = point; // back insertion
|
||||
|
||||
// add complete inlier set as points
|
||||
// for (int k = 0; k < int(support_points.size()); ++k)
|
||||
// {
|
||||
// geometry_msgs::msg::Polygon polygon;
|
||||
// convertPointToPolygon(cluster[support_points[k]], polygon);
|
||||
// lines = polygon; // back insertion
|
||||
// }
|
||||
|
||||
// remove inlier from list and add them as point obstacles
|
||||
// iterate in reverse order, otherwise indices are not valid after erasing elements
|
||||
std::vector<std::size_t>::reverse_iterator support_it = support_points.rbegin();
|
||||
for (; support_it != support_points.rend(); ++support_it)
|
||||
{
|
||||
geometry_msgs::msg::Polygon polygon;
|
||||
convertPointToPolygon(cluster[*support_it], polygon);
|
||||
lines = polygon; // back insertion
|
||||
|
||||
cluster.erase(cluster.begin() + *support_it);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// add all remaining cluster points, that do not belong to a line
|
||||
for (int i=0; i<(int)cluster.size();++i)
|
||||
{
|
||||
geometry_msgs::msg::Polygon polygon;
|
||||
convertPointToPolygon(cluster[i], polygon);
|
||||
lines = polygon; // back insertion
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//void CostmapToLinesDBSMCCH::reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level)
|
||||
//{
|
||||
// boost::mutex::scoped_lock lock(parameter_mutex_);
|
||||
// parameter_buffered_.max_distance_ = config.cluster_max_distance;
|
||||
// parameter_buffered_.min_pts_ = config.cluster_min_pts;
|
||||
// parameter_buffered_.max_pts_ = config.cluster_max_pts;
|
||||
// parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
|
||||
// support_pts_max_dist_ = config.support_pts_max_dist;
|
||||
// support_pts_max_dist_inbetween_ = config.support_pts_max_dist_inbetween;
|
||||
// min_support_pts_ = config.min_support_pts;
|
||||
//}
|
||||
|
||||
|
||||
|
||||
}//end namespace costmap_converter
|
||||
|
||||
|
||||
@ -1,361 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann
|
||||
*********************************************************************/
|
||||
|
||||
#include <costmap_converter/costmap_to_lines_ransac.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::BaseCostmapToPolygons)
|
||||
|
||||
namespace costmap_converter
|
||||
{
|
||||
|
||||
CostmapToLinesDBSRANSAC::CostmapToLinesDBSRANSAC() : CostmapToPolygonsDBSMCCH()
|
||||
{
|
||||
// dynamic_recfg_ = NULL;
|
||||
}
|
||||
|
||||
CostmapToLinesDBSRANSAC::~CostmapToLinesDBSRANSAC()
|
||||
{
|
||||
// if (dynamic_recfg_ != NULL)
|
||||
// delete dynamic_recfg_;
|
||||
}
|
||||
|
||||
void CostmapToLinesDBSRANSAC::initialize(rclcpp::Node::SharedPtr nh)
|
||||
{
|
||||
BaseCostmapToPolygons::initialize(nh);
|
||||
|
||||
// DB SCAN
|
||||
parameter_.max_distance_ = 0.4;
|
||||
nh->get_parameter_or<double>("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_);
|
||||
|
||||
parameter_.min_pts_ = 2;
|
||||
nh->get_parameter_or<int>("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_);
|
||||
|
||||
parameter_.max_pts_ = 30;
|
||||
nh->get_parameter_or<int>("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_);
|
||||
|
||||
// convex hull (only necessary if outlier filtering is enabled)
|
||||
parameter_.min_keypoint_separation_ = 0.1;
|
||||
nh->get_parameter_or<double>("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_);
|
||||
|
||||
parameter_buffered_ = parameter_;
|
||||
|
||||
// ransac
|
||||
ransac_inlier_distance_ = 0.2;
|
||||
nh->get_parameter_or<double>("ransac_inlier_distance", ransac_inlier_distance_, ransac_inlier_distance_);
|
||||
|
||||
ransac_min_inliers_ = 10;
|
||||
nh->get_parameter_or<int>("ransac_min_inliers", ransac_min_inliers_, ransac_min_inliers_);
|
||||
|
||||
ransac_no_iterations_ = 2000;
|
||||
nh->get_parameter_or<int>("ransac_no_iterations", ransac_no_iterations_, ransac_no_iterations_);
|
||||
|
||||
ransac_remainig_outliers_ = 3;
|
||||
nh->get_parameter_or<int>("ransac_remainig_outliers", ransac_remainig_outliers_, ransac_remainig_outliers_);
|
||||
|
||||
ransac_convert_outlier_pts_ = true;
|
||||
nh->get_parameter_or<bool>("ransac_convert_outlier_pts", ransac_convert_outlier_pts_, ransac_convert_outlier_pts_);
|
||||
|
||||
ransac_filter_remaining_outlier_pts_ = false;
|
||||
nh->get_parameter_or<bool>("ransac_filter_remaining_outlier_pts", ransac_filter_remaining_outlier_pts_, ransac_filter_remaining_outlier_pts_);
|
||||
|
||||
// setup dynamic reconfigure
|
||||
// dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>(nh);
|
||||
// dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSRANSAC::reconfigureCB, this, _1, _2);
|
||||
// dynamic_recfg_->setCallback(cb);
|
||||
}
|
||||
|
||||
void CostmapToLinesDBSRANSAC::compute()
|
||||
{
|
||||
std::vector< std::vector<KeyPoint> > clusters;
|
||||
dbScan(clusters);
|
||||
|
||||
// Create new polygon container
|
||||
PolygonContainerPtr polygons(new std::vector<geometry_msgs::msg::Polygon>());
|
||||
|
||||
|
||||
// fit lines using ransac for each cluster
|
||||
for (size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
|
||||
{
|
||||
|
||||
while ((int)clusters[i].size() > ransac_remainig_outliers_)
|
||||
{
|
||||
// std::vector<KeyPoint> inliers;
|
||||
std::vector<KeyPoint> outliers;
|
||||
std::pair<KeyPoint,KeyPoint> model;
|
||||
if (!lineRansac(clusters[i], ransac_inlier_distance_, ransac_no_iterations_, ransac_min_inliers_, model, NULL, &outliers ) )
|
||||
break;
|
||||
|
||||
// add to polygon container
|
||||
geometry_msgs::msg::Polygon line;
|
||||
line.points.resize(2);
|
||||
model.first.toPointMsg(line.points.front());
|
||||
model.second.toPointMsg(line.points.back());
|
||||
polygons->push_back(line);
|
||||
|
||||
clusters[i] = outliers;
|
||||
}
|
||||
// create point polygons for remaining outliers
|
||||
if (ransac_convert_outlier_pts_)
|
||||
{
|
||||
if (ransac_filter_remaining_outlier_pts_)
|
||||
{
|
||||
// take edge points of a convex polygon
|
||||
// these points define a cluster and since all lines are extracted,
|
||||
// we remove points from the interior...
|
||||
geometry_msgs::msg::Polygon polygon;
|
||||
convexHull2(clusters[i], polygon);
|
||||
for (int j=0; j < (int)polygon.points.size(); ++j)
|
||||
{
|
||||
polygons->push_back(geometry_msgs::msg::Polygon());
|
||||
convertPointToPolygon(polygon.points[j], polygons->back());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int j = 0; j < (int)clusters[i].size(); ++j)
|
||||
{
|
||||
polygons->push_back(geometry_msgs::msg::Polygon());
|
||||
convertPointToPolygon(clusters[i][j], polygons->back());
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
// add our non-cluster points to the polygon container (as single points)
|
||||
if (!clusters.empty())
|
||||
{
|
||||
for (size_t i=0; i < clusters.front().size(); ++i)
|
||||
{
|
||||
polygons->push_back( geometry_msgs::msg::Polygon() );
|
||||
convertPointToPolygon(clusters.front()[i], polygons->back());
|
||||
}
|
||||
}
|
||||
|
||||
// replace shared polygon container
|
||||
updatePolygonContainer(polygons);
|
||||
}
|
||||
|
||||
|
||||
bool CostmapToLinesDBSRANSAC::lineRansac(const std::vector<KeyPoint>& data, double inlier_distance, int no_iterations,
|
||||
int min_inliers, std::pair<KeyPoint, KeyPoint>& best_model,
|
||||
std::vector<KeyPoint>* inliers, std::vector<KeyPoint>* outliers)
|
||||
{
|
||||
if ((int)data.size() < 2 || (int)data.size() < min_inliers)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
std::uniform_int_distribution<> distribution(0, data.size()-1);
|
||||
|
||||
std::pair<int, int> best_model_idx;
|
||||
int best_no_inliers = -1;
|
||||
|
||||
|
||||
for (int i=0; i < no_iterations; ++i)
|
||||
{
|
||||
// choose random points to define a line candidate
|
||||
int start_idx = distribution(rnd_generator_);
|
||||
int end_idx = start_idx;
|
||||
while (end_idx == start_idx)
|
||||
end_idx = distribution(rnd_generator_);
|
||||
|
||||
|
||||
// compute inliers
|
||||
int no_inliers = 0;
|
||||
for (int j=0; j<(int)data.size(); ++j)
|
||||
{
|
||||
if ( isInlier(data[j], data[start_idx], data[end_idx], inlier_distance) )
|
||||
++no_inliers;
|
||||
}
|
||||
|
||||
if (no_inliers > best_no_inliers)
|
||||
{
|
||||
best_no_inliers = no_inliers;
|
||||
best_model_idx.first = start_idx;
|
||||
best_model_idx.second = end_idx;
|
||||
}
|
||||
}
|
||||
|
||||
best_model.first = data[best_model_idx.first];
|
||||
best_model.second = data[best_model_idx.second];
|
||||
|
||||
if (best_no_inliers < min_inliers)
|
||||
return false;
|
||||
|
||||
// Now repeat the calculation for the best model in order to obtain the inliers and outliers set
|
||||
// This might be faster if no_iterations is large, but TEST
|
||||
if (inliers || outliers)
|
||||
{
|
||||
if (inliers)
|
||||
inliers->clear();
|
||||
if (outliers)
|
||||
outliers->clear();
|
||||
|
||||
// int no_inliers = 0;
|
||||
for (int i=0; i<(int)data.size(); ++i)
|
||||
{
|
||||
if ( isInlier( data[i], best_model.first, best_model.second, inlier_distance ) )
|
||||
{
|
||||
if (inliers)
|
||||
inliers->push_back( data[i] );
|
||||
}
|
||||
else
|
||||
{
|
||||
if (outliers)
|
||||
outliers->push_back( data[i] );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CostmapToLinesDBSRANSAC::linearRegression(const std::vector<KeyPoint>& data, double& slope, double& intercept,
|
||||
double* mean_x_out, double* mean_y_out)
|
||||
{
|
||||
if (data.size() < 2)
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "CostmapToLinesDBSRANSAC: at least 2 data points required for linear regression");
|
||||
return false;
|
||||
}
|
||||
|
||||
double mean_x = 0;
|
||||
double mean_y = 0;
|
||||
for (int i=0; i<(int)data.size(); ++i)
|
||||
{
|
||||
mean_x += data[i].x;
|
||||
mean_y += data[i].y;
|
||||
}
|
||||
mean_x /= double(data.size());
|
||||
mean_y /= double(data.size());
|
||||
|
||||
if (mean_x_out)
|
||||
*mean_x_out = mean_x;
|
||||
|
||||
if (mean_y_out)
|
||||
*mean_y_out = mean_y;
|
||||
|
||||
double numerator = 0.0;
|
||||
double denominator = 0.0;
|
||||
|
||||
for(int i=0; i<(int)data.size(); ++i)
|
||||
{
|
||||
double dx = data[i].x - mean_x;
|
||||
numerator += dx * (data[i].y - mean_y);
|
||||
denominator += dx*dx;
|
||||
}
|
||||
|
||||
if (denominator == 0)
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "CostmapToLinesDBSRANSAC: linear regression failed, denominator 0");
|
||||
return false;
|
||||
}
|
||||
else
|
||||
slope = numerator / denominator;
|
||||
|
||||
intercept = mean_y - slope * mean_x;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
//void CostmapToLinesDBSRANSAC::reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level)
|
||||
//{
|
||||
// boost::mutex::scoped_lock lock(parameter_mutex_);
|
||||
// parameter_buffered_.max_distance_ = config.cluster_max_distance;
|
||||
// parameter_buffered_.min_pts_ = config.cluster_min_pts;
|
||||
// parameter_buffered_.max_pts_ = config.cluster_max_pts;
|
||||
// parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
|
||||
// ransac_inlier_distance_ = config.ransac_inlier_distance;
|
||||
// ransac_min_inliers_ = config.ransac_min_inliers;
|
||||
// ransac_no_iterations_ = config.ransac_no_iterations;
|
||||
// ransac_remainig_outliers_ = config.ransac_remainig_outliers;
|
||||
// ransac_convert_outlier_pts_ = config.ransac_convert_outlier_pts;
|
||||
// ransac_filter_remaining_outlier_pts_ = config.ransac_filter_remaining_outlier_pts;
|
||||
//}
|
||||
|
||||
|
||||
/*
|
||||
void CostmapToLinesDBSRANSAC::adjustLineLength(const std::vector<KeyPoint>& data, const KeyPoint& linept1, const KeyPoint& linept2,
|
||||
KeyPoint& line_start, KeyPoint& line_end)
|
||||
{
|
||||
line_start = linept1;
|
||||
line_end = linept2;
|
||||
|
||||
// infinite line is defined by linept1 and linept2
|
||||
double dir_x = line_end.x - line_start.x;
|
||||
double dir_y = line_end.y - line_start.y;
|
||||
double norm = std::sqrt(dir_x*dir_x + dir_y*dir_y);
|
||||
dir_x /= norm;
|
||||
dir_y /= norm;
|
||||
|
||||
// project data onto the line and check if the distance is increased in both directions
|
||||
for (int i=0; i < (int) data.size(); ++i)
|
||||
{
|
||||
double dx = data[i].x - line_start.x;
|
||||
double dy = data[i].y - line_start.y;
|
||||
// check scalar product at start
|
||||
double extension = dx*dir_x + dy*dir_y;
|
||||
if (extension<0)
|
||||
{
|
||||
line_start.x -= dir_x*extension;
|
||||
line_start.y -= dir_y*extension;
|
||||
}
|
||||
else
|
||||
{
|
||||
dx = data[i].x - line_end.x;
|
||||
dy = data[i].y - line_end.y;
|
||||
// check scalar product at end
|
||||
double extension = dx*dir_x + dy*dir_y;
|
||||
if (extension>0)
|
||||
{
|
||||
line_end.x += dir_x*extension;
|
||||
line_end.y += dir_y*extension;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}*/
|
||||
|
||||
|
||||
}//end namespace costmap_converter
|
||||
|
||||
|
||||
@ -1,514 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann, Otniel Rinaldo
|
||||
*********************************************************************/
|
||||
|
||||
#include <costmap_converter/costmap_to_polygons.h>
|
||||
#include <costmap_converter/misc.h>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::BaseCostmapToPolygons)
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Douglas-Peucker Algorithm for fitting lines into ordered set of points
|
||||
*
|
||||
* Douglas-Peucker Algorithm, see https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm
|
||||
*
|
||||
* @param begin iterator pointing to the begin of the range of points
|
||||
* @param end interator pointing to the end of the range of points
|
||||
* @param epsilon distance criteria for removing points if it is closer to the line segment than this
|
||||
* @param result the simplified polygon
|
||||
*/
|
||||
std::vector<geometry_msgs::msg::Point32> douglasPeucker(std::vector<geometry_msgs::msg::Point32>::iterator begin,
|
||||
std::vector<geometry_msgs::msg::Point32>::iterator end, double epsilon)
|
||||
{
|
||||
if (std::distance(begin, end) <= 2)
|
||||
{
|
||||
return std::vector<geometry_msgs::msg::Point32>(begin, end);
|
||||
}
|
||||
|
||||
// Find the point with the maximum distance from the line [begin, end)
|
||||
double dmax = std::numeric_limits<double>::lowest();
|
||||
std::vector<geometry_msgs::msg::Point32>::iterator max_dist_it;
|
||||
std::vector<geometry_msgs::msg::Point32>::iterator last = std::prev(end);
|
||||
for (auto it = std::next(begin); it != last; ++it)
|
||||
{
|
||||
double d = costmap_converter::computeSquaredDistanceToLineSegment(*it, *begin, *last);
|
||||
if (d > dmax)
|
||||
{
|
||||
max_dist_it = it;
|
||||
dmax = d;
|
||||
}
|
||||
}
|
||||
|
||||
if (dmax < epsilon * epsilon)
|
||||
{ // termination criterion reached, line is good enough
|
||||
std::vector<geometry_msgs::msg::Point32> result;
|
||||
result.push_back(*begin);
|
||||
result.push_back(*last);
|
||||
return result;
|
||||
}
|
||||
|
||||
// Recursive calls for the two splitted parts
|
||||
auto firstLineSimplified = douglasPeucker(begin, std::next(max_dist_it), epsilon);
|
||||
auto secondLineSimplified = douglasPeucker(max_dist_it, end, epsilon);
|
||||
|
||||
// Combine the two lines into one line and return the merged line.
|
||||
// Note that we have to skip the first point of the second line, as it is duplicated above.
|
||||
firstLineSimplified.insert(firstLineSimplified.end(),
|
||||
std::make_move_iterator(std::next(secondLineSimplified.begin())),
|
||||
std::make_move_iterator(secondLineSimplified.end()));
|
||||
return firstLineSimplified;
|
||||
}
|
||||
|
||||
} // end namespace
|
||||
|
||||
namespace costmap_converter
|
||||
{
|
||||
|
||||
CostmapToPolygonsDBSMCCH::CostmapToPolygonsDBSMCCH() : BaseCostmapToPolygons()
|
||||
{
|
||||
costmap_ = NULL;
|
||||
// dynamic_recfg_ = NULL;
|
||||
neighbor_size_x_ = neighbor_size_y_ = -1;
|
||||
offset_x_ = offset_y_ = 0.;
|
||||
}
|
||||
|
||||
CostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH()
|
||||
{
|
||||
// if (dynamic_recfg_ != NULL)
|
||||
// delete dynamic_recfg_;
|
||||
}
|
||||
|
||||
void CostmapToPolygonsDBSMCCH::initialize(rclcpp::Node::SharedPtr nh)
|
||||
{
|
||||
BaseCostmapToPolygons::initialize(nh);
|
||||
|
||||
costmap_ = NULL;
|
||||
|
||||
parameter_.max_distance_ = 0.4;
|
||||
nh->get_parameter_or<double>("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_);
|
||||
|
||||
parameter_.min_pts_ = 2;
|
||||
nh->get_parameter_or<int>("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_);
|
||||
|
||||
parameter_.max_pts_ = 30;
|
||||
nh->get_parameter_or<int>("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_);
|
||||
|
||||
parameter_.min_keypoint_separation_ = 0.1;
|
||||
nh->get_parameter_or<double>("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_);
|
||||
|
||||
parameter_buffered_ = parameter_;
|
||||
|
||||
// setup dynamic reconfigure
|
||||
// dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>(nh);
|
||||
// dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSMCCH::reconfigureCB, this, _1, _2);
|
||||
// dynamic_recfg_->setCallback(cb);
|
||||
}
|
||||
|
||||
|
||||
void CostmapToPolygonsDBSMCCH::compute()
|
||||
{
|
||||
std::vector< std::vector<KeyPoint> > clusters;
|
||||
dbScan(clusters);
|
||||
|
||||
// Create new polygon container
|
||||
PolygonContainerPtr polygons(new std::vector<geometry_msgs::msg::Polygon>());
|
||||
|
||||
|
||||
// add convex hulls to polygon container
|
||||
for (std::size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
|
||||
{
|
||||
polygons->push_back( geometry_msgs::msg::Polygon() );
|
||||
convexHull2(clusters[i], polygons->back() );
|
||||
}
|
||||
|
||||
// add our non-cluster points to the polygon container (as single points)
|
||||
if (!clusters.empty())
|
||||
{
|
||||
for (std::size_t i=0; i < clusters.front().size(); ++i)
|
||||
{
|
||||
polygons->push_back( geometry_msgs::msg::Polygon() );
|
||||
convertPointToPolygon(clusters.front()[i], polygons->back());
|
||||
}
|
||||
}
|
||||
|
||||
// replace shared polygon container
|
||||
updatePolygonContainer(polygons);
|
||||
}
|
||||
|
||||
void CostmapToPolygonsDBSMCCH::setCostmap2D(nav2_costmap_2d::Costmap2D *costmap)
|
||||
{
|
||||
if (!costmap)
|
||||
return;
|
||||
|
||||
costmap_ = costmap;
|
||||
|
||||
updateCostmap2D();
|
||||
}
|
||||
|
||||
void CostmapToPolygonsDBSMCCH::updateCostmap2D()
|
||||
{
|
||||
occupied_cells_.clear();
|
||||
|
||||
if (!costmap_->getMutex())
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "Cannot update costmap since the mutex pointer is null");
|
||||
return;
|
||||
}
|
||||
|
||||
// TODO: currently dynamic reconigure is not supported in ros2
|
||||
{ // get a copy of our parameters from dynamic reconfigure
|
||||
std::lock_guard<std::mutex> lock(parameter_mutex_);
|
||||
parameter_ = parameter_buffered_;
|
||||
}
|
||||
|
||||
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*costmap_->getMutex());
|
||||
|
||||
// allocate neighbor lookup
|
||||
int cells_x = int(costmap_->getSizeInMetersX() / parameter_.max_distance_) + 1;
|
||||
int cells_y = int(costmap_->getSizeInMetersY() / parameter_.max_distance_) + 1;
|
||||
|
||||
if (cells_x != neighbor_size_x_ || cells_y != neighbor_size_y_) {
|
||||
neighbor_size_x_ = cells_x;
|
||||
neighbor_size_y_ = cells_y;
|
||||
neighbor_lookup_.resize(neighbor_size_x_ * neighbor_size_y_);
|
||||
}
|
||||
offset_x_ = costmap_->getOriginX();
|
||||
offset_y_ = costmap_->getOriginY();
|
||||
for (auto& n : neighbor_lookup_)
|
||||
n.clear();
|
||||
|
||||
auto size_x = costmap_->getSizeInCellsX();
|
||||
auto size_y = costmap_->getSizeInCellsY();
|
||||
|
||||
// get indices of obstacle cells
|
||||
for(std::size_t i = 0; i < size_x; i++)
|
||||
{
|
||||
for(std::size_t j = 0; j < size_y; j++)
|
||||
{
|
||||
int value = costmap_->getCost(i,j);
|
||||
if(value >= nav2_costmap_2d::LETHAL_OBSTACLE)
|
||||
{
|
||||
double x, y;
|
||||
costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);
|
||||
addPoint(x, y);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void CostmapToPolygonsDBSMCCH::dbScan(std::vector< std::vector<KeyPoint> >& clusters)
|
||||
{
|
||||
std::vector<bool> visited(occupied_cells_.size(), false);
|
||||
|
||||
clusters.clear();
|
||||
|
||||
//DB Scan Algorithm
|
||||
int cluster_id = 0; // current cluster_id
|
||||
clusters.push_back(std::vector<KeyPoint>());
|
||||
for(int i = 0; i< (int)occupied_cells_.size(); i++)
|
||||
{
|
||||
if(!visited[i]) //keypoint has not been visited before
|
||||
{
|
||||
visited[i] = true; // mark as visited
|
||||
std::vector<int> neighbors;
|
||||
regionQuery(i, neighbors); //Find neighbors around the keypoint
|
||||
if((int)neighbors.size() < parameter_.min_pts_) //If not enough neighbors are found, mark as noise
|
||||
{
|
||||
clusters[0].push_back(occupied_cells_[i]);
|
||||
}
|
||||
else
|
||||
{
|
||||
++cluster_id; // increment current cluster_id
|
||||
clusters.push_back(std::vector<KeyPoint>());
|
||||
|
||||
// Expand the cluster
|
||||
clusters[cluster_id].push_back(occupied_cells_[i]);
|
||||
for(int j = 0; j<(int)neighbors.size(); j++)
|
||||
{
|
||||
if ((int)clusters[cluster_id].size() == parameter_.max_pts_)
|
||||
break;
|
||||
|
||||
if(!visited[neighbors[j]]) //keypoint has not been visited before
|
||||
{
|
||||
visited[neighbors[j]] = true; // mark as visited
|
||||
std::vector<int> further_neighbors;
|
||||
regionQuery(neighbors[j], further_neighbors); //Find more neighbors around the new keypoint
|
||||
// if(further_neighbors.size() < min_pts_)
|
||||
// {
|
||||
// clusters[0].push_back(occupied_cells[neighbors[j]]);
|
||||
// }
|
||||
// else
|
||||
if ((int)further_neighbors.size() >= parameter_.min_pts_)
|
||||
{
|
||||
// neighbors found
|
||||
neighbors.insert(neighbors.end(), further_neighbors.begin(), further_neighbors.end()); //Add these newfound P' neighbour to P neighbour vector "nb_indeces"
|
||||
clusters[cluster_id].push_back(occupied_cells_[neighbors[j]]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CostmapToPolygonsDBSMCCH::regionQuery(int curr_index, std::vector<int>& neighbors)
|
||||
{
|
||||
neighbors.clear();
|
||||
|
||||
double dist_sqr_threshold = parameter_.max_distance_ * parameter_.max_distance_;
|
||||
const KeyPoint& kp = occupied_cells_[curr_index];
|
||||
int cx, cy;
|
||||
pointToNeighborCells(kp, cx,cy);
|
||||
|
||||
// loop over the neighboring cells for looking up the points
|
||||
const int offsets[9][2] = {{-1, -1}, {0, -1}, {1, -1},
|
||||
{-1, 0}, {0, 0}, {1, 0},
|
||||
{-1, 1}, {0, 1}, {1, 1}};
|
||||
for (int i = 0; i < 9; ++i)
|
||||
{
|
||||
int idx = neighborCellsToIndex(cx + offsets[i][0], cy + offsets[i][1]);
|
||||
if (idx < 0 || idx >= int(neighbor_lookup_.size()))
|
||||
continue;
|
||||
const std::vector<int>& pointIndicesToCheck = neighbor_lookup_[idx];
|
||||
for (int point_idx : pointIndicesToCheck) {
|
||||
if (point_idx == curr_index) // point is not a neighbor to itself
|
||||
continue;
|
||||
const KeyPoint& other = occupied_cells_[point_idx];
|
||||
double dx = other.x - kp.x;
|
||||
double dy = other.y - kp.y;
|
||||
double dist_sqr = dx*dx + dy*dy;
|
||||
if (dist_sqr <= dist_sqr_threshold)
|
||||
neighbors.push_back(point_idx);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool isXCoordinateSmaller(const CostmapToPolygonsDBSMCCH::KeyPoint& p1, const CostmapToPolygonsDBSMCCH::KeyPoint& p2)
|
||||
{
|
||||
return p1.x < p2.x || (p1.x == p2.x && p1.y < p2.y);
|
||||
}
|
||||
|
||||
void CostmapToPolygonsDBSMCCH::convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::msg::Polygon& polygon)
|
||||
{
|
||||
//Monotone Chain ConvexHull Algorithm source from http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull
|
||||
|
||||
int k = 0;
|
||||
int n = cluster.size();
|
||||
|
||||
// sort points according to x coordinate (TODO. is it already sorted due to the map representation?)
|
||||
std::sort(cluster.begin(), cluster.end(), isXCoordinateSmaller);
|
||||
|
||||
polygon.points.resize(2*n);
|
||||
|
||||
// lower hull
|
||||
for (int i = 0; i < n; ++i)
|
||||
{
|
||||
while (k >= 2 && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)
|
||||
{
|
||||
--k;
|
||||
}
|
||||
cluster[i].toPointMsg(polygon.points[k]);
|
||||
++k;
|
||||
}
|
||||
|
||||
// upper hull
|
||||
for (int i = n-2, t = k+1; i >= 0; --i)
|
||||
{
|
||||
while (k >= t && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)
|
||||
{
|
||||
--k;
|
||||
}
|
||||
cluster[i].toPointMsg(polygon.points[k]);
|
||||
++k;
|
||||
}
|
||||
|
||||
|
||||
polygon.points.resize(k); // original
|
||||
// TEST we skip the last point, since in our definition the polygon vertices do not contain the start/end vertex twice.
|
||||
// polygon.points.resize(k-1); // TODO remove last point from the algorithm above to reduce computational cost
|
||||
|
||||
simplifyPolygon(polygon);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void CostmapToPolygonsDBSMCCH::convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::msg::Polygon& polygon)
|
||||
{
|
||||
std::vector<KeyPoint>& P = cluster;
|
||||
std::vector<geometry_msgs::msg::Point32>& points = polygon.points;
|
||||
|
||||
// Sort P by x and y
|
||||
std::sort(P.begin(), P.end(), isXCoordinateSmaller);
|
||||
|
||||
// the output array H[] will be used as the stack
|
||||
int i; // array scan index
|
||||
|
||||
// Get the indices of points with min x-coord and min|max y-coord
|
||||
int minmin = 0, minmax;
|
||||
double xmin = P[0].x;
|
||||
for (i = 1; i < (int)P.size(); i++)
|
||||
if (P[i].x != xmin) break;
|
||||
minmax = i - 1;
|
||||
if (minmax == (int)P.size() - 1)
|
||||
{ // degenerate case: all x-coords == xmin
|
||||
points.push_back(geometry_msgs::msg::Point32());
|
||||
P[minmin].toPointMsg(points.back());
|
||||
if (P[minmax].y != P[minmin].y) // a nontrivial segment
|
||||
{
|
||||
points.push_back(geometry_msgs::msg::Point32());
|
||||
P[minmax].toPointMsg(points.back());
|
||||
}
|
||||
// add polygon endpoint
|
||||
points.push_back(geometry_msgs::msg::Point32());
|
||||
P[minmin].toPointMsg(points.back());
|
||||
return;
|
||||
}
|
||||
|
||||
// Get the indices of points with max x-coord and min|max y-coord
|
||||
int maxmin, maxmax = (int)P.size() - 1;
|
||||
double xmax = P.back().x;
|
||||
for (i = P.size() - 2; i >= 0; i--)
|
||||
if (P[i].x != xmax) break;
|
||||
maxmin = i+1;
|
||||
|
||||
// Compute the lower hull on the stack H
|
||||
// push minmin point onto stack
|
||||
points.push_back(geometry_msgs::msg::Point32());
|
||||
P[minmin].toPointMsg(points.back());
|
||||
i = minmax;
|
||||
while (++i <= maxmin)
|
||||
{
|
||||
// the lower line joins P[minmin] with P[maxmin]
|
||||
if (cross(P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin)
|
||||
continue; // ignore P[i] above or on the lower line
|
||||
|
||||
while (points.size() > 1) // there are at least 2 points on the stack
|
||||
{
|
||||
// test if P[i] is left of the line at the stack top
|
||||
if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
|
||||
break; // P[i] is a new hull vertex
|
||||
points.pop_back(); // pop top point off stack
|
||||
}
|
||||
// push P[i] onto stack
|
||||
points.push_back(geometry_msgs::msg::Point32());
|
||||
P[i].toPointMsg(points.back());
|
||||
}
|
||||
|
||||
// Next, compute the upper hull on the stack H above the bottom hull
|
||||
if (maxmax != maxmin) // if distinct xmax points
|
||||
{
|
||||
// push maxmax point onto stack
|
||||
points.push_back(geometry_msgs::msg::Point32());
|
||||
P[maxmax].toPointMsg(points.back());
|
||||
}
|
||||
int bot = (int)points.size(); // the bottom point of the upper hull stack
|
||||
i = maxmin;
|
||||
while (--i >= minmax)
|
||||
{
|
||||
// the upper line joins P[maxmax] with P[minmax]
|
||||
if (cross( P[maxmax], P[minmax], P[i]) >= 0 && i > minmax)
|
||||
continue; // ignore P[i] below or on the upper line
|
||||
|
||||
while ((int)points.size() > bot) // at least 2 points on the upper stack
|
||||
{
|
||||
// test if P[i] is left of the line at the stack top
|
||||
if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
|
||||
break; // P[i] is a new hull vertex
|
||||
points.pop_back(); // pop top point off stack
|
||||
}
|
||||
// push P[i] onto stack
|
||||
points.push_back(geometry_msgs::msg::Point32());
|
||||
P[i].toPointMsg(points.back());
|
||||
}
|
||||
if (minmax != minmin)
|
||||
{
|
||||
// push joining endpoint onto stack
|
||||
points.push_back(geometry_msgs::msg::Point32());
|
||||
P[minmin].toPointMsg(points.back());
|
||||
}
|
||||
|
||||
simplifyPolygon(polygon);
|
||||
}
|
||||
|
||||
void CostmapToPolygonsDBSMCCH::simplifyPolygon(geometry_msgs::msg::Polygon& polygon)
|
||||
{
|
||||
size_t triangleThreshold = 3;
|
||||
// check if first and last point are the same. If yes, a triangle has 4 points
|
||||
if (polygon.points.size() > 1
|
||||
&& std::abs(polygon.points.front().x - polygon.points.back().x) < 1e-5
|
||||
&& std::abs(polygon.points.front().y - polygon.points.back().y) < 1e-5)
|
||||
{
|
||||
triangleThreshold = 4;
|
||||
}
|
||||
if (polygon.points.size() <= triangleThreshold) // nothing to do for triangles or lines
|
||||
return;
|
||||
// TODO Reason about better start conditions for splitting lines, e.g., by
|
||||
// https://en.wikipedia.org/wiki/Rotating_calipers
|
||||
polygon.points = douglasPeucker(polygon.points.begin(), polygon.points.end(), parameter_.min_keypoint_separation_);;
|
||||
}
|
||||
|
||||
void CostmapToPolygonsDBSMCCH::updatePolygonContainer(PolygonContainerPtr polygons)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
polygons_ = polygons;
|
||||
}
|
||||
|
||||
|
||||
PolygonContainerConstPtr CostmapToPolygonsDBSMCCH::getPolygons()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
PolygonContainerConstPtr polygons = polygons_;
|
||||
return polygons;
|
||||
}
|
||||
|
||||
//void CostmapToPolygonsDBSMCCH::reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level)
|
||||
//{
|
||||
//boost::mutex::scoped_lock lock(parameter_mutex_);
|
||||
//parameter_buffered_.max_distance_ = config.cluster_max_distance;
|
||||
//parameter_buffered_.min_pts_ = config.cluster_min_pts;
|
||||
//parameter_buffered_.max_pts_ = config.cluster_max_pts;
|
||||
//parameter_buffered_.min_keypoint_separation_ = config.convex_hull_min_pt_separation;
|
||||
//}
|
||||
|
||||
}//end namespace costmap_converter
|
||||
|
||||
|
||||
@ -1,231 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann, Otniel Rinaldo
|
||||
*********************************************************************/
|
||||
|
||||
#include <costmap_converter/costmap_to_polygons_concave.h>
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSConcaveHull, costmap_converter::BaseCostmapToPolygons)
|
||||
|
||||
namespace costmap_converter
|
||||
{
|
||||
|
||||
CostmapToPolygonsDBSConcaveHull::CostmapToPolygonsDBSConcaveHull() : CostmapToPolygonsDBSMCCH()
|
||||
{
|
||||
// dynamic_recfg_ = NULL;
|
||||
}
|
||||
|
||||
CostmapToPolygonsDBSConcaveHull::~CostmapToPolygonsDBSConcaveHull()
|
||||
{
|
||||
// if (dynamic_recfg_ != NULL)
|
||||
// delete dynamic_recfg_;
|
||||
}
|
||||
|
||||
void CostmapToPolygonsDBSConcaveHull::initialize(rclcpp::Node::SharedPtr nh)
|
||||
{
|
||||
BaseCostmapToPolygons::initialize(nh);
|
||||
|
||||
parameter_.max_distance_ = 0.4;
|
||||
nh->get_parameter_or<double>("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_);
|
||||
|
||||
parameter_.min_pts_ = 2;
|
||||
nh->get_parameter_or<int>("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_);
|
||||
|
||||
parameter_.max_pts_ = 30;
|
||||
nh->get_parameter_or<int>("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_);
|
||||
|
||||
parameter_.min_keypoint_separation_ = 0.1;
|
||||
nh->get_parameter_or<double>("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_);
|
||||
|
||||
parameter_buffered_ = parameter_;
|
||||
|
||||
concave_hull_depth_ = 2.0;
|
||||
nh->get_parameter_or<double>("concave_hull_depth", concave_hull_depth_, concave_hull_depth_);
|
||||
|
||||
// setup dynamic reconfigure
|
||||
// dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>(nh);
|
||||
// dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSConcaveHull::reconfigureCB, this, _1, _2);
|
||||
// dynamic_recfg_->setCallback(cb);
|
||||
}
|
||||
|
||||
|
||||
void CostmapToPolygonsDBSConcaveHull::compute()
|
||||
{
|
||||
std::vector< std::vector<KeyPoint> > clusters;
|
||||
dbScan(clusters);
|
||||
|
||||
// Create new polygon container
|
||||
PolygonContainerPtr polygons(new std::vector<geometry_msgs::msg::Polygon>());
|
||||
|
||||
|
||||
// add convex hulls to polygon container
|
||||
for (size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
|
||||
{
|
||||
polygons->push_back( geometry_msgs::msg::Polygon() );
|
||||
concaveHull(clusters[i], concave_hull_depth_, polygons->back() );
|
||||
}
|
||||
|
||||
// add our non-cluster points to the polygon container (as single points)
|
||||
if (!clusters.empty())
|
||||
{
|
||||
for (size_t i=0; i < clusters.front().size(); ++i)
|
||||
{
|
||||
polygons->push_back( geometry_msgs::msg::Polygon() );
|
||||
convertPointToPolygon(clusters.front()[i], polygons->back());
|
||||
}
|
||||
}
|
||||
|
||||
// replace shared polygon container
|
||||
updatePolygonContainer(polygons);
|
||||
}
|
||||
|
||||
|
||||
void CostmapToPolygonsDBSConcaveHull::concaveHull(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::msg::Polygon& polygon)
|
||||
{
|
||||
// start with convex hull
|
||||
convexHull2(cluster, polygon);
|
||||
|
||||
std::vector<geometry_msgs::msg::Point32>& concave_list = polygon.points;
|
||||
|
||||
for (int i = 0; i < (int)concave_list.size() - 1; ++i)
|
||||
{
|
||||
|
||||
// find nearest inner point pk from line (vertex1 -> vertex2)
|
||||
const geometry_msgs::msg::Point32& vertex1 = concave_list[i];
|
||||
const geometry_msgs::msg::Point32& vertex2 = concave_list[i+1];
|
||||
|
||||
bool found;
|
||||
size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found);
|
||||
if (!found)
|
||||
continue;
|
||||
|
||||
double line_length = norm2d(vertex1, vertex2);
|
||||
|
||||
double dst1 = norm2d(cluster[nearest_idx], vertex1);
|
||||
double dst2 = norm2d(cluster[nearest_idx], vertex2);
|
||||
double dd = std::min(dst1, dst2);
|
||||
if (dd<1e-8)
|
||||
continue;
|
||||
|
||||
if (line_length / dd > depth)
|
||||
{
|
||||
// Check that new candidate edge will not intersect existing edges.
|
||||
bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]);
|
||||
intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2);
|
||||
if (!intersects)
|
||||
{
|
||||
geometry_msgs::msg::Point32 new_point;
|
||||
cluster[nearest_idx].toPointMsg(new_point);
|
||||
concave_list.insert(concave_list.begin() + i + 1, new_point);
|
||||
i--;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void CostmapToPolygonsDBSConcaveHull::concaveHullClusterCut(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::msg::Polygon& polygon)
|
||||
{
|
||||
// start with convex hull
|
||||
convexHull2(cluster, polygon);
|
||||
|
||||
std::vector<geometry_msgs::msg::Point32>& concave_list = polygon.points;
|
||||
|
||||
// get line length
|
||||
double mean_length = 0;
|
||||
for (int i = 0; i < (int)concave_list.size() - 1; ++i)
|
||||
{
|
||||
mean_length += norm2d(concave_list[i],concave_list[i+1]);
|
||||
}
|
||||
mean_length /= double(concave_list.size());
|
||||
|
||||
for (int i = 0; i < (int)concave_list.size() - 1; ++i)
|
||||
{
|
||||
|
||||
// find nearest inner point pk from line (vertex1 -> vertex2)
|
||||
const geometry_msgs::msg::Point32& vertex1 = concave_list[i];
|
||||
const geometry_msgs::msg::Point32& vertex2 = concave_list[i+1];
|
||||
|
||||
double line_length = norm2d(vertex1, vertex2);
|
||||
|
||||
bool found;
|
||||
size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found);
|
||||
if (!found)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
|
||||
double dst1 = norm2d(cluster[nearest_idx], vertex1);
|
||||
double dst2 = norm2d(cluster[nearest_idx], vertex2);
|
||||
double dd = std::min(dst1, dst2);
|
||||
if (dd<1e-8)
|
||||
continue;
|
||||
|
||||
if (line_length / dd > depth)
|
||||
{
|
||||
// Check that new candidate edge will not intersect existing edges.
|
||||
bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]);
|
||||
intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2);
|
||||
if (!intersects)
|
||||
{
|
||||
geometry_msgs::msg::Point32 new_point;
|
||||
cluster[nearest_idx].toPointMsg(new_point);
|
||||
concave_list.insert(concave_list.begin() + i + 1, new_point);
|
||||
i--;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//void CostmapToPolygonsDBSConcaveHull::reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level)
|
||||
//{
|
||||
// boost::mutex::scoped_lock lock(parameter_mutex_);
|
||||
// parameter_buffered_.max_distance_ = config.cluster_max_distance;
|
||||
// parameter_buffered_.min_pts_ = config.cluster_min_pts;
|
||||
// parameter_buffered_.max_pts_ = config.cluster_max_pts;
|
||||
// parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
|
||||
// concave_hull_depth_ = config.concave_hull_depth;
|
||||
//}
|
||||
|
||||
}//end namespace costmap_converter
|
||||
|
||||
|
||||
@ -1,230 +0,0 @@
|
||||
#include <random>
|
||||
#include <memory>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <costmap_converter/costmap_to_polygons.h>
|
||||
|
||||
namespace {
|
||||
geometry_msgs::msg::Point32 create_point(double x, double y)
|
||||
{
|
||||
geometry_msgs::msg::Point32 result;
|
||||
result.x = x;
|
||||
result.y = y;
|
||||
result.z = 0.;
|
||||
return result;
|
||||
}
|
||||
} // end namespace
|
||||
|
||||
// make things accesible in the test
|
||||
class CostmapToPolygons : public costmap_converter::CostmapToPolygonsDBSMCCH
|
||||
{
|
||||
public:
|
||||
const std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint>& points() const {return occupied_cells_;}
|
||||
costmap_converter::CostmapToPolygonsDBSMCCH::Parameters& parameters() {return parameter_;}
|
||||
using costmap_converter::CostmapToPolygonsDBSMCCH::addPoint;
|
||||
using costmap_converter::CostmapToPolygonsDBSMCCH::regionQuery;
|
||||
using costmap_converter::CostmapToPolygonsDBSMCCH::dbScan;
|
||||
using costmap_converter::CostmapToPolygonsDBSMCCH::convexHull2;
|
||||
using costmap_converter::CostmapToPolygonsDBSMCCH::simplifyPolygon;
|
||||
};
|
||||
|
||||
class CostmapToPolygonsDBSMCCHTest : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
void SetUp() override {
|
||||
// parameters
|
||||
costmap_to_polygons.parameters().max_distance_ = 0.5;
|
||||
costmap_to_polygons.parameters().max_pts_ = 100;
|
||||
costmap_to_polygons.parameters().min_pts_ = 2;
|
||||
costmap_to_polygons.parameters().min_keypoint_separation_ = 0.1;
|
||||
|
||||
costmap.reset(new nav2_costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.));
|
||||
costmap_to_polygons.setCostmap2D(costmap.get());
|
||||
|
||||
std::random_device rand_dev;
|
||||
std::mt19937 generator(rand_dev());
|
||||
std::uniform_real_distribution<> random_angle(-M_PI, M_PI);
|
||||
std::uniform_real_distribution<> random_dist(0., costmap_to_polygons.parameters().max_distance_);
|
||||
|
||||
costmap_to_polygons.addPoint(0., 0.);
|
||||
costmap_to_polygons.addPoint(1.3, 1.3);
|
||||
|
||||
// adding random points
|
||||
double center_x = costmap_to_polygons.points()[0].x;
|
||||
double center_y = costmap_to_polygons.points()[0].y;
|
||||
for (int i = 0; i < costmap_to_polygons.parameters().max_pts_ - 1; ++i)
|
||||
{
|
||||
double alpha = random_angle(generator);
|
||||
double dist = random_dist(generator);
|
||||
double wx = center_x + std::cos(alpha) * dist;
|
||||
double wy = center_y + std::sin(alpha) * dist;
|
||||
costmap_to_polygons.addPoint(wx, wy);
|
||||
}
|
||||
|
||||
// some noisy points not belonging to a cluster
|
||||
costmap_to_polygons.addPoint(-1, -1);
|
||||
costmap_to_polygons.addPoint(-2, -2);
|
||||
|
||||
// adding random points
|
||||
center_x = costmap_to_polygons.points()[1].x;
|
||||
center_y = costmap_to_polygons.points()[1].y;
|
||||
for (int i = 0; i < costmap_to_polygons.parameters().max_pts_/2; ++i)
|
||||
{
|
||||
double alpha = random_angle(generator);
|
||||
double dist = random_dist(generator);
|
||||
double wx = center_x + std::cos(alpha) * dist;
|
||||
double wy = center_y + std::sin(alpha) * dist;
|
||||
costmap_to_polygons.addPoint(wx, wy);
|
||||
}
|
||||
}
|
||||
|
||||
void regionQueryTrivial(int curr_index, std::vector<int>& neighbor_indices)
|
||||
{
|
||||
neighbor_indices.clear();
|
||||
const auto& query_point = costmap_to_polygons.points()[curr_index];
|
||||
for (int i = 0; i < int(costmap_to_polygons.points().size()); ++i)
|
||||
{
|
||||
if (i == curr_index)
|
||||
continue;
|
||||
const auto& kp = costmap_to_polygons.points()[i];
|
||||
double dx = query_point.x - kp.x;
|
||||
double dy = query_point.y - kp.y;
|
||||
double dist = sqrt(dx*dx + dy*dy);
|
||||
if (dist < costmap_to_polygons.parameters().max_distance_)
|
||||
neighbor_indices.push_back(i);
|
||||
}
|
||||
}
|
||||
|
||||
CostmapToPolygons costmap_to_polygons;
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap;
|
||||
};
|
||||
|
||||
TEST_F(CostmapToPolygonsDBSMCCHTest, regionQuery)
|
||||
{
|
||||
std::vector<int> neighbors, neighbors_trivial;
|
||||
costmap_to_polygons.regionQuery(0, neighbors);
|
||||
regionQueryTrivial(0, neighbors_trivial);
|
||||
ASSERT_EQ(costmap_to_polygons.parameters().max_pts_ - 1, int(neighbors.size()));
|
||||
ASSERT_EQ(neighbors_trivial.size(), neighbors.size());
|
||||
std::sort(neighbors.begin(), neighbors.end());
|
||||
ASSERT_EQ(neighbors_trivial, neighbors);
|
||||
|
||||
costmap_to_polygons.regionQuery(1, neighbors);
|
||||
regionQueryTrivial(1, neighbors_trivial);
|
||||
ASSERT_EQ(costmap_to_polygons.parameters().max_pts_/2, int(neighbors.size()));
|
||||
ASSERT_EQ(neighbors_trivial.size(), neighbors.size());
|
||||
std::sort(neighbors.begin(), neighbors.end());
|
||||
ASSERT_EQ(neighbors_trivial, neighbors);
|
||||
}
|
||||
|
||||
TEST_F(CostmapToPolygonsDBSMCCHTest, dbScan)
|
||||
{
|
||||
std::vector< std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint> > clusters;
|
||||
costmap_to_polygons.dbScan(clusters);
|
||||
|
||||
ASSERT_EQ((unsigned)3, clusters.size());
|
||||
ASSERT_EQ((unsigned)2, clusters[0].size()); // noisy points not belonging to a cluster
|
||||
ASSERT_EQ((unsigned)costmap_to_polygons.parameters().max_pts_, clusters[1].size()); // first cluster at (0,0)
|
||||
ASSERT_EQ((unsigned)costmap_to_polygons.parameters().max_pts_/2 + 1, clusters[2].size()); // second cluster at (1,1)
|
||||
}
|
||||
|
||||
TEST(CostmapToPolygonsDBSMCCH, EmptyMap)
|
||||
{
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap =
|
||||
std::make_shared<nav2_costmap_2d::Costmap2D>(nav2_costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.));
|
||||
CostmapToPolygons costmap_to_polygons;
|
||||
costmap_to_polygons.setCostmap2D(costmap.get());
|
||||
|
||||
std::vector< std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint> > clusters;
|
||||
costmap_to_polygons.dbScan(clusters);
|
||||
ASSERT_EQ((unsigned)1, clusters.size()); // noise cluster exists
|
||||
ASSERT_EQ((unsigned)0, clusters[0].size()); // noise clsuter is empty
|
||||
}
|
||||
|
||||
TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygon)
|
||||
{
|
||||
const double simplification_threshold = 0.1;
|
||||
CostmapToPolygons costmap_to_polygons;
|
||||
costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold;
|
||||
|
||||
geometry_msgs::msg::Polygon polygon;
|
||||
polygon.points.push_back(create_point(0., 0.));
|
||||
polygon.points.push_back(create_point(1., 0.));
|
||||
|
||||
// degenerate case with just two points
|
||||
geometry_msgs::msg::Polygon original_polygon = polygon;
|
||||
costmap_to_polygons.simplifyPolygon(polygon);
|
||||
ASSERT_EQ((unsigned)2, polygon.points.size());
|
||||
for (size_t i = 0; i < polygon.points.size(); ++i)
|
||||
{
|
||||
ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
|
||||
ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);
|
||||
}
|
||||
|
||||
// degenerate case with three points
|
||||
polygon.points.push_back(create_point(1., simplification_threshold / 2.));
|
||||
original_polygon = polygon;
|
||||
costmap_to_polygons.simplifyPolygon(polygon);
|
||||
ASSERT_EQ((unsigned)3, polygon.points.size());
|
||||
for (size_t i = 0; i < polygon.points.size(); ++i)
|
||||
{
|
||||
ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
|
||||
ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);
|
||||
}
|
||||
|
||||
polygon.points.push_back(create_point(1., 1.));
|
||||
original_polygon = polygon;
|
||||
// remove the point that has to be removed by the simplification
|
||||
original_polygon.points.erase(original_polygon.points.begin()+2);
|
||||
costmap_to_polygons.simplifyPolygon(polygon);
|
||||
ASSERT_EQ((unsigned)3, polygon.points.size());
|
||||
for (size_t i = 0; i < polygon.points.size(); ++i)
|
||||
{
|
||||
ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
|
||||
ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygonPerfectLines)
|
||||
{
|
||||
const double simplification_threshold = 0.1;
|
||||
CostmapToPolygons costmap_to_polygons;
|
||||
costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold;
|
||||
|
||||
geometry_msgs::msg::Polygon polygon;
|
||||
for (int i = 0; i <= 100; ++i)
|
||||
polygon.points.push_back(create_point(i*1., 0.));
|
||||
geometry_msgs::msg::Point32 lastPoint = polygon.points.back();
|
||||
for (int i = 0; i <= 100; ++i)
|
||||
polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.));
|
||||
lastPoint = polygon.points.back();
|
||||
for (int i = 0; i <= 100; ++i)
|
||||
polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y));
|
||||
lastPoint = polygon.points.back();
|
||||
for (int i = 0; i <= 100; ++i)
|
||||
polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.));
|
||||
lastPoint = polygon.points.back();
|
||||
for (int i = 0; i <= 100; ++i)
|
||||
polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y));
|
||||
|
||||
costmap_to_polygons.simplifyPolygon(polygon);
|
||||
ASSERT_EQ((unsigned)6, polygon.points.size());
|
||||
ASSERT_FLOAT_EQ( 0., polygon.points[0].x);
|
||||
ASSERT_FLOAT_EQ( 0., polygon.points[0].y);
|
||||
ASSERT_FLOAT_EQ(100., polygon.points[1].x);
|
||||
ASSERT_FLOAT_EQ( 0., polygon.points[1].y);
|
||||
ASSERT_FLOAT_EQ(100., polygon.points[2].x);
|
||||
ASSERT_FLOAT_EQ(100., polygon.points[2].y);
|
||||
ASSERT_FLOAT_EQ(200., polygon.points[3].x);
|
||||
ASSERT_FLOAT_EQ(100., polygon.points[3].y);
|
||||
ASSERT_FLOAT_EQ(200., polygon.points[4].x);
|
||||
ASSERT_FLOAT_EQ(200., polygon.points[4].y);
|
||||
ASSERT_FLOAT_EQ(300., polygon.points[5].x);
|
||||
ASSERT_FLOAT_EQ(200., polygon.points[5].y);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
@ -1,3 +0,0 @@
|
||||
<launch>
|
||||
<test test-name="costmap_polygons" pkg="costmap_converter" type="test_costmap_polygons" />
|
||||
</launch>
|
||||
@ -1,41 +0,0 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package costmap_converter_msgs
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.1.2 (2020-06-22)
|
||||
------------------
|
||||
|
||||
0.1.1 (2020-01-25)
|
||||
------------------
|
||||
|
||||
0.1.0 (2020-01-23)
|
||||
------------------
|
||||
* Messages moved to a separate package
|
||||
* Contributors: Vinnam Kim
|
||||
|
||||
0.0.9 (2018-05-28)
|
||||
------------------
|
||||
|
||||
0.0.8 (2018-05-17)
|
||||
------------------
|
||||
|
||||
0.0.7 (2017-09-20)
|
||||
------------------
|
||||
|
||||
0.0.6 (2017-09-18)
|
||||
------------------
|
||||
|
||||
0.0.5 (2016-02-01)
|
||||
------------------
|
||||
|
||||
0.0.4 (2016-01-11)
|
||||
------------------
|
||||
|
||||
0.0.3 (2015-12-23)
|
||||
------------------
|
||||
|
||||
0.0.2 (2015-12-22)
|
||||
------------------
|
||||
|
||||
0.0.1 (2015-12-21)
|
||||
------------------
|
||||
@ -1,26 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
|
||||
project(costmap_converter_msgs)
|
||||
|
||||
# Default to C++14
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(costmap_converter_msgs
|
||||
"msg/ObstacleMsg.msg"
|
||||
"msg/ObstacleArrayMsg.msg"
|
||||
DEPENDENCIES builtin_interfaces geometry_msgs std_msgs
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@ -1,10 +0,0 @@
|
||||
# Message that contains a list of polygon shaped obstacles.
|
||||
# Special types:
|
||||
# Polygon with 1 vertex: Point obstacle
|
||||
# Polygon with 2 vertices: Line obstacle
|
||||
# Polygon with more than 2 vertices: First and last points are assumed to be connected
|
||||
|
||||
std_msgs/Header header
|
||||
|
||||
costmap_converter_msgs/ObstacleMsg[] obstacles
|
||||
|
||||
@ -1,24 +0,0 @@
|
||||
# Special types:
|
||||
# Polygon with 1 vertex: Point obstacle (you might also specify a non-zero value for radius)
|
||||
# Polygon with 2 vertices: Line obstacle
|
||||
# Polygon with more than 2 vertices: First and last points are assumed to be connected
|
||||
|
||||
std_msgs/Header header
|
||||
|
||||
# Obstacle footprint (polygon descriptions)
|
||||
geometry_msgs/Polygon polygon
|
||||
|
||||
# Specify the radius for circular/point obstacles
|
||||
float64 radius
|
||||
|
||||
# Obstacle ID
|
||||
# Specify IDs in order to provide (temporal) relationships
|
||||
# between obstacles among multiple messages.
|
||||
int64 id
|
||||
|
||||
# Individual orientation (centroid)
|
||||
geometry_msgs/Quaternion orientation
|
||||
|
||||
# Individual velocities (centroid)
|
||||
geometry_msgs/TwistWithCovariance velocities
|
||||
|
||||
@ -1,31 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>costmap_converter_msgs</name>
|
||||
<version>0.1.2</version>
|
||||
<description>Package containing message types for costmap conversion</description>
|
||||
<maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
|
||||
<build_depend>builtin_interfaces</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
|
||||
<exec_depend>builtin_interfaces</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,55 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
project(fake_vel_transform)
|
||||
|
||||
## Use C++14
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
## By adding -Wall and -Werror, the compiler does not ignore warnings anymore,
|
||||
## enforcing cleaner code.
|
||||
add_definitions(-Wall -Werror)
|
||||
|
||||
## Export compile commands for clangd
|
||||
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
||||
|
||||
#######################
|
||||
## Find dependencies ##
|
||||
#######################
|
||||
|
||||
find_package(ament_cmake_auto REQUIRED)
|
||||
ament_auto_find_build_dependencies()
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
ament_auto_add_library(${PROJECT_NAME} SHARED
|
||||
src/fake_vel_transform.cpp
|
||||
)
|
||||
|
||||
rclcpp_components_register_node(${PROJECT_NAME}
|
||||
PLUGIN fake_vel_transform::FakeVelTransform
|
||||
EXECUTABLE ${PROJECT_NAME}_node
|
||||
)
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
list(APPEND AMENT_LINT_AUTO_EXCLUDE
|
||||
ament_cmake_copyright
|
||||
ament_cmake_uncrustify
|
||||
)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
ament_auto_package(
|
||||
INSTALL_TO_SHARE
|
||||
launch
|
||||
)
|
||||
@ -1,29 +0,0 @@
|
||||
# fake_vel_transform
|
||||
|
||||
该包用于实现雷达和云台共用的 yaw 轴时的兼容性，即使云台处于旋转扫描状态，速度变换后，仍然可以实现较稳定的轨迹跟踪效果。
|
||||
|
||||
## 实现方式
|
||||
|
||||
创建 XYZ 轴基于 `base_link`, RPY 轴基于局部路径规划朝向的 `base_link_fake` 坐标系，使得 nav2 局部规划器能够将机器人的方向视为与当前路径规划方向一致。
|
||||
|
||||
nav2 发布的速度也是基于 `base_link_fake` 坐标系的，通过 tf2 将速度转换到 `base_link` 坐标系，使机器人能够正常运动。在仿真中表现即底盘小陀螺时仍能跟随轨迹。
|
||||
|
||||
电控端执行底盘控制命令时，机器人运动正方向为云台枪管朝向。
|
||||
|
||||
## fake_vel_transform_node
|
||||
|
||||
订阅：
|
||||
|
||||
- nav2 发布的基于 base_link_fake 坐标系的速度指令 `/cmd_vel`
|
||||
- nav2 controller 发布的局部路径朝向 `/local_path`
|
||||
- `odom` 到 `base_link` 的 tf 变换
|
||||
|
||||
发布：
|
||||
|
||||
- 转换到 base_link 坐标系的速度 `/cmd_vel_chassis`
|
||||
|
||||
静态参数：
|
||||
|
||||
- 底盘固定旋转速度 `spin_speed`
|
||||
|
||||
搭配电控固定小陀螺速度，将 spin_speed 设为负，可实现移动时小陀螺减慢。
|
||||
@ -1,56 +0,0 @@
|
||||
#ifndef FAKE_VEL_TRANSFORM__FAKE_VEL_TRANSFORM_HPP_
|
||||
#define FAKE_VEL_TRANSFORM__FAKE_VEL_TRANSFORM_HPP_
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <tf2_ros/create_timer_ros.h>
|
||||
#include <tf2_ros/message_filter.h>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
||||
#include <geometry_msgs/msg/pose_array.hpp>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
#include <rclcpp/publisher.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp/subscription.hpp>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||
|
||||
namespace fake_vel_transform
|
||||
{
|
||||
class FakeVelTransform : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
explicit FakeVelTransform(const rclcpp::NodeOptions & options);
|
||||
|
||||
private:
|
||||
void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
|
||||
|
||||
void localPoseCallback(const nav_msgs::msg::Path::SharedPtr msg);
|
||||
|
||||
void publishTransform();
|
||||
|
||||
// Subscriber with tf2 message_filter
|
||||
std::string target_frame_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
|
||||
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr local_pose_sub_;
|
||||
|
||||
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_chassis_pub_;
|
||||
|
||||
// Broadcast tf from base_link to base_link_fake
|
||||
rclcpp::TimerBase::SharedPtr tf_timer_;
|
||||
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||
|
||||
geometry_msgs::msg::PoseStamped planner_local_pose_;
|
||||
double current_angle_;
|
||||
double base_link_angle_;
|
||||
float spin_speed_;
|
||||
};
|
||||
|
||||
} // namespace fake_vel_transform
|
||||
|
||||
#endif // FAKE_VEL_TRANSFORM__FAKE_VEL_TRANSFORM_HPP_
|
||||
@ -1,20 +0,0 @@
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
||||
|
||||
fake_vel_transform_node = Node(
|
||||
package='fake_vel_transform',
|
||||
executable='fake_vel_transform_node',
|
||||
output='screen',
|
||||
parameters=[
|
||||
{'use_sim_time': use_sim_time }
|
||||
]
|
||||
)
|
||||
|
||||
return LaunchDescription([fake_vel_transform_node])
|
||||
@ -1,31 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model
|
||||
href="http://download.ros.org/schema/package_format3.xsd"
|
||||
schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>fake_vel_transform</name>
|
||||
<version>0.1.0</version>
|
||||
<description>A template for ROS packages.</description>
|
||||
<license>MIT</license>
|
||||
<author email="lihanchen2004@163.com">Lihan Chen</author>
|
||||
<maintainer email="lihanchen2004@163.com">Lihan Chen</maintainer>
|
||||
|
||||
<!-- buildtool_depend: dependencies of the build process -->
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<!-- depend: build, export, and execution dependency -->
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_cmake_clang_format</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,103 +0,0 @@
|
||||
#include "fake_vel_transform/fake_vel_transform.hpp"
|
||||
|
||||
#include <tf2/utils.h>
|
||||
|
||||
#include <rclcpp/logging.hpp>
|
||||
#include <rclcpp/qos.hpp>
|
||||
#include <rclcpp/utilities.hpp>
|
||||
|
||||
namespace fake_vel_transform
|
||||
{
|
||||
const std::string CMD_VEL_TOPIC = "/cmd_vel";
|
||||
const std::string AFTER_TF_CMD_VEL = "/cmd_vel_chassis";
|
||||
const std::string TRAJECTORY_TOPIC = "/local_plan";
|
||||
const int TF_PUBLISH_FREQUENCY = 20; // base_link to base_link_fake. Frequency in Hz.
|
||||
|
||||
FakeVelTransform::FakeVelTransform(const rclcpp::NodeOptions & options)
|
||||
: Node("fake_vel_transform", options)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Start FakeVelTransform!");
|
||||
|
||||
// Declare and get the spin speed parameter
|
||||
this->declare_parameter<float>("spin_speed", -6.0);
|
||||
this->get_parameter("spin_speed", spin_speed_);
|
||||
|
||||
// TF broadcaster
|
||||
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
|
||||
tf2_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
|
||||
tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
|
||||
|
||||
// Create Publisher and Subscriber
|
||||
cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
|
||||
CMD_VEL_TOPIC, 1, std::bind(&FakeVelTransform::cmdVelCallback, this, std::placeholders::_1));
|
||||
cmd_vel_chassis_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
|
||||
AFTER_TF_CMD_VEL, rclcpp::QoS(rclcpp::KeepLast(1)));
|
||||
local_pose_sub_ = this->create_subscription<nav_msgs::msg::Path>(
|
||||
TRAJECTORY_TOPIC, 1,
|
||||
std::bind(&FakeVelTransform::localPoseCallback, this, std::placeholders::_1));
|
||||
|
||||
// Create a timer to publish the transform regularly
|
||||
tf_timer_ = this->create_wall_timer(
|
||||
std::chrono::milliseconds(1000 / TF_PUBLISH_FREQUENCY),
|
||||
std::bind(&FakeVelTransform::publishTransform, this));
|
||||
}
|
||||
|
||||
// Get the local pose from planner
|
||||
void FakeVelTransform::localPoseCallback(const nav_msgs::msg::Path::SharedPtr msg)
|
||||
{
|
||||
if (!msg || msg->poses.empty()) {
|
||||
RCLCPP_WARN(get_logger(), "Received empty or invalid PoseArray message");
|
||||
return;
|
||||
}
|
||||
|
||||
// Choose the pose based on the size of the poses array
|
||||
size_t index = std::min(msg->poses.size() / 4, msg->poses.size() - 1);
|
||||
const geometry_msgs::msg::Pose & selected_pose = msg->poses[index].pose;
|
||||
|
||||
// Update current angle based on the difference between teb_angle and base_link_angle_
|
||||
double teb_angle = tf2::getYaw(selected_pose.orientation);
|
||||
current_angle_ = teb_angle - base_link_angle_;
|
||||
}
|
||||
|
||||
// Transform the velocity from base_link to base_link_fake
|
||||
void FakeVelTransform::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
try {
|
||||
geometry_msgs::msg::TransformStamped transform_stamped;
|
||||
transform_stamped = tf2_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero);
|
||||
base_link_angle_ = tf2::getYaw(transform_stamped.transform.rotation);
|
||||
|
||||
double angle_diff = -current_angle_;
|
||||
|
||||
geometry_msgs::msg::Twist aft_tf_vel;
|
||||
aft_tf_vel.angular.z = (msg->angular.z != 0) ? spin_speed_ : 0;
|
||||
aft_tf_vel.linear.x = msg->linear.x * cos(angle_diff) + msg->linear.y * sin(angle_diff);
|
||||
aft_tf_vel.linear.y = -msg->linear.x * sin(angle_diff) + msg->linear.y * cos(angle_diff);
|
||||
|
||||
cmd_vel_chassis_pub_->publish(aft_tf_vel);
|
||||
} catch (tf2::TransformException & ex) {
|
||||
RCLCPP_INFO(this->get_logger(), "Could not transform odom to base_link: %s", ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
// Publish transform from base_link to base_link_fake
|
||||
void FakeVelTransform::publishTransform()
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped t;
|
||||
t.header.stamp = get_clock()->now();
|
||||
t.header.frame_id = "base_link";
|
||||
t.child_frame_id = "base_link_fake";
|
||||
tf2::Quaternion q;
|
||||
q.setRPY(0, 0, current_angle_);
|
||||
t.transform.rotation = tf2::toMsg(q);
|
||||
tf_broadcaster_->sendTransform(t);
|
||||
}
|
||||
|
||||
} // namespace fake_vel_transform
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// Register the component with class_loader.
|
||||
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||
// is being loaded into a running process.
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(fake_vel_transform::FakeVelTransform)
|
||||
@ -1,34 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(rm_navigation)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
if(CMAKE_EXPORT_COMPILE_COMMANDS)
|
||||
set(CMAKE_EXPORT_COMPILE_COMMANDS ${CMAKE_EXPORT_COMPILE_COMMANDS})
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake_auto REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_auto_package(INSTALL_TO_SHARE
|
||||
params
|
||||
rviz
|
||||
launch
|
||||
)
|
||||
@ -1,156 +0,0 @@
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (DeclareLaunchArgument, GroupAction,
|
||||
IncludeLaunchDescription, SetEnvironmentVariable)
|
||||
from launch.conditions import IfCondition
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.actions import PushRosNamespace
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# Get the launch directory
|
||||
bringup_dir = get_package_share_directory('rm_navigation')
|
||||
launch_dir = os.path.join(bringup_dir, 'launch')
|
||||
|
||||
# Create the launch configuration variables
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
use_namespace = LaunchConfiguration('use_namespace')
|
||||
map_yaml_file = LaunchConfiguration('map')
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
params_file = LaunchConfiguration('params_file')
|
||||
autostart = LaunchConfiguration('autostart')
|
||||
use_composition = LaunchConfiguration('use_composition')
|
||||
use_respawn = LaunchConfiguration('use_respawn')
|
||||
log_level = LaunchConfiguration('log_level')
|
||||
use_nav_rviz = LaunchConfiguration('nav_rviz')
|
||||
|
||||
remappings = [('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static')]
|
||||
|
||||
# Create our own temporary YAML files that include substitutions
|
||||
param_substitutions = {
|
||||
'use_sim_time': use_sim_time,
|
||||
'yaml_filename': map_yaml_file}
|
||||
|
||||
configured_params = RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key=namespace,
|
||||
param_rewrites=param_substitutions,
|
||||
convert_types=True)
|
||||
|
||||
stdout_linebuf_envvar = SetEnvironmentVariable(
|
||||
'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
|
||||
|
||||
declare_namespace_cmd = DeclareLaunchArgument(
|
||||
'namespace',
|
||||
default_value='',
|
||||
description='Top-level namespace')
|
||||
|
||||
declare_use_namespace_cmd = DeclareLaunchArgument(
|
||||
'use_namespace',
|
||||
default_value='false',
|
||||
description='Whether to apply a namespace to the navigation stack')
|
||||
|
||||
declare_use_slam_cmd = DeclareLaunchArgument(
|
||||
'use_slam',
|
||||
default_value='True',
|
||||
description='Whether run a SLAM')
|
||||
|
||||
declare_map_yaml_cmd = DeclareLaunchArgument(
|
||||
'map',
|
||||
default_value= os.path.join(bringup_dir,'map', 'RMUL.yaml'),
|
||||
description='Full path to map yaml file to load')
|
||||
|
||||
declare_use_sim_time_cmd = DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='True',
|
||||
description='Use simulation (Gazebo) clock if true')
|
||||
|
||||
declare_params_file_cmd = DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
|
||||
description='Full path to the ROS2 parameters file to use for all launched nodes')
|
||||
|
||||
|
||||
declare_autostart_cmd = DeclareLaunchArgument(
|
||||
'autostart', default_value='True',
|
||||
description='Automatically startup the nav2 stack')
|
||||
|
||||
declare_use_composition_cmd = DeclareLaunchArgument(
|
||||
'use_composition', default_value='True',
|
||||
description='Whether to use composed bringup')
|
||||
|
||||
declare_use_respawn_cmd = DeclareLaunchArgument(
|
||||
'use_respawn', default_value='True',
|
||||
description='Whether to respawn if a node crashes. Applied when composition is disabled.')
|
||||
|
||||
declare_log_level_cmd = DeclareLaunchArgument(
|
||||
'log_level', default_value='info',
|
||||
description='log level')
|
||||
|
||||
declare_nav_rviz_cmd = DeclareLaunchArgument(
|
||||
'nav_rviz',
|
||||
default_value='True',
|
||||
description='Visualize navigation2 if true')
|
||||
|
||||
# Specify the actions
|
||||
bringup_cmd_group = GroupAction([
|
||||
PushRosNamespace(
|
||||
condition=IfCondition(use_namespace),
|
||||
namespace=namespace),
|
||||
|
||||
Node(
|
||||
condition=IfCondition(use_composition),
|
||||
name='nav2_container',
|
||||
package='rclcpp_components',
|
||||
executable='component_container_mt',
|
||||
parameters=[configured_params, {'autostart': autostart}],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
remappings=remappings,
|
||||
output='screen'),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')),
|
||||
launch_arguments={'namespace': namespace,
|
||||
'use_sim_time': use_sim_time,
|
||||
'autostart': autostart,
|
||||
'params_file': params_file,
|
||||
'use_composition': use_composition,
|
||||
'use_respawn': use_respawn,
|
||||
'container_name': 'nav2_container'}.items()),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')),
|
||||
condition=IfCondition(use_nav_rviz)
|
||||
),
|
||||
])
|
||||
|
||||
# Create the launch description and populate
|
||||
ld = LaunchDescription()
|
||||
|
||||
# Set environment variables
|
||||
ld.add_action(stdout_linebuf_envvar)
|
||||
|
||||
# Declare the launch options
|
||||
ld.add_action(declare_namespace_cmd)
|
||||
ld.add_action(declare_use_namespace_cmd)
|
||||
ld.add_action(declare_use_slam_cmd)
|
||||
ld.add_action(declare_map_yaml_cmd)
|
||||
ld.add_action(declare_use_sim_time_cmd)
|
||||
ld.add_action(declare_params_file_cmd)
|
||||
ld.add_action(declare_autostart_cmd)
|
||||
ld.add_action(declare_use_composition_cmd)
|
||||
ld.add_action(declare_use_respawn_cmd)
|
||||
ld.add_action(declare_log_level_cmd)
|
||||
ld.add_action(declare_nav_rviz_cmd)
|
||||
|
||||
# Add the actions to launch all of the navigation nodes
|
||||
ld.add_action(bringup_cmd_group)
|
||||
|
||||
return ld
|
||||
@ -1,149 +0,0 @@
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, SetEnvironmentVariable
|
||||
from launch.conditions import IfCondition
|
||||
from launch.substitutions import LaunchConfiguration, PythonExpression
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import LoadComposableNodes, Node
|
||||
from launch_ros.descriptions import ComposableNode, ParameterFile
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# Get the launch directory
|
||||
bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
autostart = LaunchConfiguration('autostart')
|
||||
params_file = LaunchConfiguration('params_file')
|
||||
use_composition = LaunchConfiguration('use_composition')
|
||||
container_name = LaunchConfiguration('container_name')
|
||||
container_name_full = (namespace, '/', container_name)
|
||||
use_respawn = LaunchConfiguration('use_respawn')
|
||||
log_level = LaunchConfiguration('log_level')
|
||||
|
||||
lifecycle_nodes = ['amcl']
|
||||
|
||||
remappings = [('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static')]
|
||||
|
||||
# Create our own temporary YAML files that include substitutions
|
||||
param_substitutions = {
|
||||
'use_sim_time': use_sim_time}
|
||||
|
||||
configured_params = ParameterFile(
|
||||
RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key=namespace,
|
||||
param_rewrites=param_substitutions,
|
||||
convert_types=True),
|
||||
allow_substs=True)
|
||||
|
||||
stdout_linebuf_envvar = SetEnvironmentVariable(
|
||||
'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
|
||||
|
||||
declare_namespace_cmd = DeclareLaunchArgument(
|
||||
'namespace',
|
||||
default_value='',
|
||||
description='Top-level namespace')
|
||||
|
||||
declare_use_sim_time_cmd = DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true')
|
||||
|
||||
declare_params_file_cmd = DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
|
||||
description='Full path to the ROS2 parameters file to use for all launched nodes')
|
||||
|
||||
declare_autostart_cmd = DeclareLaunchArgument(
|
||||
'autostart', default_value='true',
|
||||
description='Automatically startup the nav2 stack')
|
||||
|
||||
declare_use_composition_cmd = DeclareLaunchArgument(
|
||||
'use_composition', default_value='False',
|
||||
description='Use composed bringup if True')
|
||||
|
||||
declare_container_name_cmd = DeclareLaunchArgument(
|
||||
'container_name', default_value='nav2_container',
|
||||
description='the name of conatiner that nodes will load in if use composition')
|
||||
|
||||
declare_use_respawn_cmd = DeclareLaunchArgument(
|
||||
'use_respawn', default_value='False',
|
||||
description='Whether to respawn if a node crashes. Applied when composition is disabled.')
|
||||
|
||||
declare_log_level_cmd = DeclareLaunchArgument(
|
||||
'log_level', default_value='info',
|
||||
description='log level')
|
||||
|
||||
load_nodes = GroupAction(
|
||||
condition=IfCondition(PythonExpression(['not ', use_composition])),
|
||||
actions=[
|
||||
Node(
|
||||
package='nav2_amcl',
|
||||
executable='amcl',
|
||||
name='amcl',
|
||||
output='screen',
|
||||
respawn=use_respawn,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
remappings=remappings),
|
||||
Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_localization',
|
||||
output='screen',
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
parameters=[{'use_sim_time': use_sim_time},
|
||||
{'autostart': autostart},
|
||||
{'node_names': lifecycle_nodes}])
|
||||
]
|
||||
)
|
||||
|
||||
load_composable_nodes = LoadComposableNodes(
|
||||
condition=IfCondition(use_composition),
|
||||
target_container=container_name_full,
|
||||
composable_node_descriptions=[
|
||||
ComposableNode(
|
||||
package='nav2_amcl',
|
||||
plugin='nav2_amcl::AmclNode',
|
||||
name='amcl',
|
||||
parameters=[configured_params],
|
||||
remappings=remappings),
|
||||
ComposableNode(
|
||||
package='nav2_lifecycle_manager',
|
||||
plugin='nav2_lifecycle_manager::LifecycleManager',
|
||||
name='lifecycle_manager_localization',
|
||||
parameters=[{'use_sim_time': use_sim_time,
|
||||
'autostart': autostart,
|
||||
'node_names': lifecycle_nodes}]),
|
||||
],
|
||||
)
|
||||
|
||||
# Create the launch description and populate
|
||||
ld = LaunchDescription()
|
||||
|
||||
# Set environment variables
|
||||
ld.add_action(stdout_linebuf_envvar)
|
||||
|
||||
# Declare the launch options
|
||||
ld.add_action(declare_namespace_cmd)
|
||||
ld.add_action(declare_use_sim_time_cmd)
|
||||
ld.add_action(declare_params_file_cmd)
|
||||
ld.add_action(declare_autostart_cmd)
|
||||
ld.add_action(declare_use_composition_cmd)
|
||||
ld.add_action(declare_container_name_cmd)
|
||||
ld.add_action(declare_use_respawn_cmd)
|
||||
ld.add_action(declare_log_level_cmd)
|
||||
|
||||
# Add the actions to launch all of the localiztion nodes
|
||||
ld.add_action(load_nodes)
|
||||
ld.add_action(load_composable_nodes)
|
||||
|
||||
return ld
|
||||
@ -1,158 +0,0 @@
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
|
||||
from launch.conditions import IfCondition
|
||||
from launch.substitutions import LaunchConfiguration, PythonExpression
|
||||
from launch_ros.actions import LoadComposableNodes
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.descriptions import ComposableNode, ParameterFile
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# Get the launch directory
|
||||
bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
map_yaml_file = LaunchConfiguration('map')
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
autostart = LaunchConfiguration('autostart')
|
||||
params_file = LaunchConfiguration('params_file')
|
||||
use_composition = LaunchConfiguration('use_composition')
|
||||
container_name = LaunchConfiguration('container_name')
|
||||
container_name_full = (namespace, '/', container_name)
|
||||
use_respawn = LaunchConfiguration('use_respawn')
|
||||
log_level = LaunchConfiguration('log_level')
|
||||
|
||||
lifecycle_nodes = ['map_server']
|
||||
|
||||
remappings = [('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static')]
|
||||
|
||||
# Create our own temporary YAML files that include substitutions
|
||||
param_substitutions = {
|
||||
'use_sim_time': use_sim_time,
|
||||
'yaml_filename': map_yaml_file}
|
||||
|
||||
configured_params = ParameterFile(
|
||||
RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key=namespace,
|
||||
param_rewrites=param_substitutions,
|
||||
convert_types=True),
|
||||
allow_substs=True)
|
||||
|
||||
stdout_linebuf_envvar = SetEnvironmentVariable(
|
||||
'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
|
||||
|
||||
declare_namespace_cmd = DeclareLaunchArgument(
|
||||
'namespace',
|
||||
default_value='',
|
||||
description='Top-level namespace')
|
||||
|
||||
declare_map_yaml_cmd = DeclareLaunchArgument(
|
||||
'map',
|
||||
description='Full path to map yaml file to load')
|
||||
|
||||
declare_use_sim_time_cmd = DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true')
|
||||
|
||||
declare_params_file_cmd = DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
|
||||
description='Full path to the ROS2 parameters file to use for all launched nodes')
|
||||
|
||||
declare_autostart_cmd = DeclareLaunchArgument(
|
||||
'autostart', default_value='true',
|
||||
description='Automatically startup the nav2 stack')
|
||||
|
||||
declare_use_composition_cmd = DeclareLaunchArgument(
|
||||
'use_composition', default_value='False',
|
||||
description='Use composed bringup if True')
|
||||
|
||||
declare_container_name_cmd = DeclareLaunchArgument(
|
||||
'container_name', default_value='nav2_container',
|
||||
description='the name of conatiner that nodes will load in if use composition')
|
||||
|
||||
declare_use_respawn_cmd = DeclareLaunchArgument(
|
||||
'use_respawn', default_value='False',
|
||||
description='Whether to respawn if a node crashes. Applied when composition is disabled.')
|
||||
|
||||
declare_log_level_cmd = DeclareLaunchArgument(
|
||||
'log_level', default_value='info',
|
||||
description='log level')
|
||||
|
||||
load_nodes = GroupAction(
|
||||
condition=IfCondition(PythonExpression(['not ', use_composition])),
|
||||
actions=[
|
||||
Node(
|
||||
package='nav2_map_server',
|
||||
executable='map_server',
|
||||
name='map_server',
|
||||
output='screen',
|
||||
respawn=use_respawn,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_localization',
|
||||
output='screen',
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
parameters=[{'use_sim_time': use_sim_time},
|
||||
{'autostart': autostart},
|
||||
{'node_names': lifecycle_nodes}])
|
||||
]
|
||||
)
|
||||
|
||||
load_composable_nodes = LoadComposableNodes(
|
||||
condition=IfCondition(use_composition),
|
||||
target_container=container_name_full,
|
||||
composable_node_descriptions=[
|
||||
ComposableNode(
|
||||
package='nav2_map_server',
|
||||
plugin='nav2_map_server::MapServer',
|
||||
name='map_server',
|
||||
parameters=[configured_params],
|
||||
remappings=remappings),
|
||||
|
||||
ComposableNode(
|
||||
package='nav2_lifecycle_manager',
|
||||
plugin='nav2_lifecycle_manager::LifecycleManager',
|
||||
name='lifecycle_manager_localization',
|
||||
parameters=[{'use_sim_time': use_sim_time,
|
||||
'autostart': autostart,
|
||||
'node_names': lifecycle_nodes}]),
|
||||
],
|
||||
)
|
||||
|
||||
# Create the launch description and populate
|
||||
ld = LaunchDescription()
|
||||
|
||||
# Set environment variables
|
||||
ld.add_action(stdout_linebuf_envvar)
|
||||
|
||||
# Declare the launch options
|
||||
ld.add_action(declare_namespace_cmd)
|
||||
ld.add_action(declare_map_yaml_cmd)
|
||||
ld.add_action(declare_use_sim_time_cmd)
|
||||
ld.add_action(declare_params_file_cmd)
|
||||
ld.add_action(declare_autostart_cmd)
|
||||
ld.add_action(declare_use_composition_cmd)
|
||||
ld.add_action(declare_container_name_cmd)
|
||||
ld.add_action(declare_use_respawn_cmd)
|
||||
ld.add_action(declare_log_level_cmd)
|
||||
|
||||
# Add the actions to launch all of the localiztion nodes
|
||||
ld.add_action(load_nodes)
|
||||
ld.add_action(load_composable_nodes)
|
||||
|
||||
return ld
|
||||
@ -1,270 +0,0 @@
|
||||
# Copyright (c) 2018 Intel Corporation
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
|
||||
from launch.conditions import IfCondition
|
||||
from launch.substitutions import LaunchConfiguration, PythonExpression
|
||||
from launch_ros.actions import LoadComposableNodes
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# Get the launch directory
|
||||
bringup_dir = get_package_share_directory('rm_navigation')
|
||||
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
autostart = LaunchConfiguration('autostart')
|
||||
params_file = LaunchConfiguration('params_file')
|
||||
use_composition = LaunchConfiguration('use_composition')
|
||||
container_name = LaunchConfiguration('container_name')
|
||||
container_name_full = (namespace, '/', container_name)
|
||||
use_respawn = LaunchConfiguration('use_respawn')
|
||||
log_level = LaunchConfiguration('log_level')
|
||||
|
||||
lifecycle_nodes = ['controller_server',
|
||||
'smoother_server',
|
||||
'planner_server',
|
||||
'behavior_server',
|
||||
'bt_navigator',
|
||||
'waypoint_follower',
|
||||
'velocity_smoother']
|
||||
|
||||
# Map fully qualified names to relative ones so the node's namespace can be prepended.
|
||||
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
|
||||
# https://github.com/ros/geometry2/issues/32
|
||||
# https://github.com/ros/robot_state_publisher/pull/30
|
||||
# TODO(orduno) Substitute with `PushNodeRemapping`
|
||||
# https://github.com/ros2/launch_ros/issues/56
|
||||
remappings = [('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static')]
|
||||
|
||||
# Create our own temporary YAML files that include substitutions
|
||||
param_substitutions = {
|
||||
'use_sim_time': use_sim_time,
|
||||
'autostart': autostart}
|
||||
|
||||
configured_params = RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key=namespace,
|
||||
param_rewrites=param_substitutions,
|
||||
convert_types=True)
|
||||
|
||||
stdout_linebuf_envvar = SetEnvironmentVariable(
|
||||
'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
|
||||
|
||||
declare_namespace_cmd = DeclareLaunchArgument(
|
||||
'namespace',
|
||||
default_value='',
|
||||
description='Top-level namespace')
|
||||
|
||||
declare_use_sim_time_cmd = DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='true',
|
||||
description='Use simulation (Gazebo) clock if true')
|
||||
|
||||
declare_params_file_cmd = DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
|
||||
description='Full path to the ROS2 parameters file to use for all launched nodes')
|
||||
|
||||
declare_autostart_cmd = DeclareLaunchArgument(
|
||||
'autostart', default_value='true',
|
||||
description='Automatically startup the nav2 stack')
|
||||
|
||||
declare_use_composition_cmd = DeclareLaunchArgument(
|
||||
'use_composition', default_value='False',
|
||||
description='Use composed bringup if True')
|
||||
|
||||
declare_container_name_cmd = DeclareLaunchArgument(
|
||||
'container_name', default_value='nav2_container',
|
||||
description='the name of conatiner that nodes will load in if use composition')
|
||||
|
||||
declare_use_respawn_cmd = DeclareLaunchArgument(
|
||||
'use_respawn', default_value='False',
|
||||
description='Whether to respawn if a node crashes. Applied when composition is disabled.')
|
||||
|
||||
declare_log_level_cmd = DeclareLaunchArgument(
|
||||
'log_level', default_value='info',
|
||||
description='log level')
|
||||
|
||||
load_nodes = GroupAction(
|
||||
condition=IfCondition(PythonExpression(['not ', use_composition])),
|
||||
actions=[
|
||||
Node(
|
||||
package='nav2_controller',
|
||||
executable='controller_server',
|
||||
output='screen',
|
||||
respawn=use_respawn,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
|
||||
Node(
|
||||
package='nav2_smoother',
|
||||
executable='smoother_server',
|
||||
name='smoother_server',
|
||||
output='screen',
|
||||
respawn=use_respawn,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
remappings=remappings),
|
||||
Node(
|
||||
package='nav2_planner',
|
||||
executable='planner_server',
|
||||
name='planner_server',
|
||||
output='screen',
|
||||
respawn=use_respawn,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
remappings=remappings),
|
||||
Node(
|
||||
package='nav2_behaviors',
|
||||
executable='behavior_server',
|
||||
name='behavior_server',
|
||||
output='screen',
|
||||
respawn=use_respawn,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
remappings=remappings),
|
||||
Node(
|
||||
package='nav2_bt_navigator',
|
||||
executable='bt_navigator',
|
||||
name='bt_navigator',
|
||||
output='screen',
|
||||
respawn=use_respawn,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
remappings=remappings),
|
||||
Node(
|
||||
package='nav2_waypoint_follower',
|
||||
executable='waypoint_follower',
|
||||
name='waypoint_follower',
|
||||
output='screen',
|
||||
respawn=use_respawn,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
remappings=remappings),
|
||||
Node(
|
||||
package='nav2_velocity_smoother',
|
||||
executable='velocity_smoother',
|
||||
name='velocity_smoother',
|
||||
output='screen',
|
||||
respawn=use_respawn,
|
||||
respawn_delay=2.0,
|
||||
parameters=[configured_params],
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
remappings=remappings +
|
||||
[('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
|
||||
Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_navigation',
|
||||
output='screen',
|
||||
arguments=['--ros-args', '--log-level', log_level],
|
||||
parameters=[{'use_sim_time': use_sim_time},
|
||||
{'autostart': autostart},
|
||||
{'node_names': lifecycle_nodes}]),
|
||||
]
|
||||
)
|
||||
|
||||
load_composable_nodes = LoadComposableNodes(
|
||||
condition=IfCondition(use_composition),
|
||||
target_container=container_name_full,
|
||||
composable_node_descriptions=[
|
||||
ComposableNode(
|
||||
package='nav2_controller',
|
||||
plugin='nav2_controller::ControllerServer',
|
||||
name='controller_server',
|
||||
parameters=[configured_params],
|
||||
remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
|
||||
ComposableNode(
|
||||
package='nav2_smoother',
|
||||
plugin='nav2_smoother::SmootherServer',
|
||||
name='smoother_server',
|
||||
parameters=[configured_params],
|
||||
remappings=remappings),
|
||||
ComposableNode(
|
||||
package='nav2_planner',
|
||||
plugin='nav2_planner::PlannerServer',
|
||||
name='planner_server',
|
||||
parameters=[configured_params],
|
||||
remappings=remappings),
|
||||
ComposableNode(
|
||||
package='nav2_behaviors',
|
||||
plugin='behavior_server::BehaviorServer',
|
||||
name='behavior_server',
|
||||
parameters=[configured_params],
|
||||
remappings=remappings),
|
||||
ComposableNode(
|
||||
package='nav2_bt_navigator',
|
||||
plugin='nav2_bt_navigator::BtNavigator',
|
||||
name='bt_navigator',
|
||||
parameters=[configured_params],
|
||||
remappings=remappings),
|
||||
ComposableNode(
|
||||
package='nav2_waypoint_follower',
|
||||
plugin='nav2_waypoint_follower::WaypointFollower',
|
||||
name='waypoint_follower',
|
||||
parameters=[configured_params],
|
||||
remappings=remappings),
|
||||
ComposableNode(
|
||||
package='nav2_velocity_smoother',
|
||||
plugin='nav2_velocity_smoother::VelocitySmoother',
|
||||
name='velocity_smoother',
|
||||
parameters=[configured_params],
|
||||
remappings=remappings +
|
||||
[('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
|
||||
ComposableNode(
|
||||
package='nav2_lifecycle_manager',
|
||||
plugin='nav2_lifecycle_manager::LifecycleManager',
|
||||
name='lifecycle_manager_navigation',
|
||||
parameters=[{'use_sim_time': use_sim_time,
|
||||
'autostart': autostart,
|
||||
'node_names': lifecycle_nodes}]),
|
||||
],
|
||||
)
|
||||
|
||||
# Create the launch description and populate
|
||||
ld = LaunchDescription()
|
||||
|
||||
# Set environment variables
|
||||
ld.add_action(stdout_linebuf_envvar)
|
||||
|
||||
# Declare the launch options
|
||||
ld.add_action(declare_namespace_cmd)
|
||||
ld.add_action(declare_use_sim_time_cmd)
|
||||
ld.add_action(declare_params_file_cmd)
|
||||
ld.add_action(declare_autostart_cmd)
|
||||
ld.add_action(declare_use_composition_cmd)
|
||||
ld.add_action(declare_container_name_cmd)
|
||||
ld.add_action(declare_use_respawn_cmd)
|
||||
ld.add_action(declare_log_level_cmd)
|
||||
# Add the actions to launch all of the navigation nodes
|
||||
ld.add_action(load_nodes)
|
||||
ld.add_action(load_composable_nodes)
|
||||
|
||||
return ld
|
||||
@ -1,109 +0,0 @@
|
||||
# Copyright (c) 2018 Intel Corporation
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.events import Shutdown
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from nav2_common.launch import ReplaceString
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# Get the launch directory
|
||||
bringup_dir = get_package_share_directory('rm_navigation')
|
||||
|
||||
# Create the launch configuration variables
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
use_namespace = LaunchConfiguration('use_namespace')
|
||||
rviz_config_file = LaunchConfiguration('rviz_config')
|
||||
|
||||
# Declare the launch arguments
|
||||
declare_namespace_cmd = DeclareLaunchArgument(
|
||||
'namespace',
|
||||
default_value='navigation',
|
||||
description=('Top-level namespace. The value will be used to replace the '
|
||||
'<robot_namespace> keyword on the rviz config file.'))
|
||||
|
||||
declare_use_namespace_cmd = DeclareLaunchArgument(
|
||||
'use_namespace',
|
||||
default_value='false',
|
||||
description='Whether to apply a namespace to the navigation stack')
|
||||
|
||||
declare_rviz_config_file_cmd = DeclareLaunchArgument(
|
||||
'rviz_config',
|
||||
default_value=os.path.join(bringup_dir, 'rviz', 'nav2.rviz'),
|
||||
description='Full path to the RVIZ config file to use')
|
||||
|
||||
# Launch rviz
|
||||
start_rviz_cmd = Node(
|
||||
condition=UnlessCondition(use_namespace),
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
arguments=['-d', rviz_config_file],
|
||||
output='screen')
|
||||
|
||||
namespaced_rviz_config_file = ReplaceString(
|
||||
source_file=rviz_config_file,
|
||||
replacements={'<robot_namespace>': ('/', namespace)})
|
||||
|
||||
start_namespaced_rviz_cmd = Node(
|
||||
condition=IfCondition(use_namespace),
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
namespace=namespace,
|
||||
arguments=['-d', namespaced_rviz_config_file],
|
||||
output='screen',
|
||||
remappings=[('/map', 'map'),
|
||||
('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static'),
|
||||
('/goal_pose', 'goal_pose'),
|
||||
('/clicked_point', 'clicked_point'),
|
||||
('/initialpose', 'initialpose')])
|
||||
|
||||
exit_event_handler = RegisterEventHandler(
|
||||
condition=UnlessCondition(use_namespace),
|
||||
event_handler=OnProcessExit(
|
||||
target_action=start_rviz_cmd,
|
||||
on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
|
||||
|
||||
exit_event_handler_namespaced = RegisterEventHandler(
|
||||
condition=IfCondition(use_namespace),
|
||||
event_handler=OnProcessExit(
|
||||
target_action=start_namespaced_rviz_cmd,
|
||||
on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
|
||||
|
||||
# Create the launch description and populate
|
||||
ld = LaunchDescription()
|
||||
|
||||
# Declare the launch options
|
||||
ld.add_action(declare_namespace_cmd)
|
||||
ld.add_action(declare_use_namespace_cmd)
|
||||
ld.add_action(declare_rviz_config_file_cmd)
|
||||
|
||||
# Add any conditioned actions
|
||||
ld.add_action(start_rviz_cmd)
|
||||
ld.add_action(start_namespaced_rviz_cmd)
|
||||
|
||||
# Add other nodes and processes we need
|
||||
ld.add_action(exit_event_handler)
|
||||
ld.add_action(exit_event_handler_namespaced)
|
||||
|
||||
return ld
|
||||
@ -1,20 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rm_navigation</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="fyt@todo.todo">fyt</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>spatio_temporal_voxel_layer</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,73 +0,0 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# Plugin params
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: livox_frame
|
||||
scan_topic: /scan
|
||||
mode: mapping #localization
|
||||
|
||||
# if you'd like to immediately start continuing a map at a given pose
|
||||
# or at the dock, but they are mutually exclusive, if pose is given
|
||||
# will use pose
|
||||
#map_file_name: test_steve
|
||||
# map_start_pose: [0.0, 0.0, 0.0]
|
||||
#map_start_at_dock: true
|
||||
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 5.0
|
||||
resolution: 0.05
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 30.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
enable_interactive_mode: true
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
loop_search_maximum_distance: 3.0
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
@ -1,371 +0,0 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_link"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.05
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::OmniMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 1.0
|
||||
update_min_a: 0.2
|
||||
update_min_d: 0.25
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: scan
|
||||
|
||||
amcl_map_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
amcl_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /Odometry
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
enable_groot_monitoring: True
|
||||
groot_zmq_publisher_port: 1666
|
||||
groot_zmq_server_port: 1667
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
|
||||
bt_navigator_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
controller_frequency: 5.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
|
||||
# Goal checker parameters
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: False
|
||||
min_vel_x: -2.5
|
||||
min_vel_y: -2.5
|
||||
max_vel_x: 2.5
|
||||
max_vel_y: 2.5
|
||||
max_vel_theta: 12.0
|
||||
min_speed_xy: -8.0
|
||||
max_speed_xy: 8.0
|
||||
min_speed_theta: -12.0
|
||||
acc_lim_x: 5.0
|
||||
acc_lim_y: 5.0
|
||||
acc_lim_theta: 15.0
|
||||
decel_lim_x: -5.0
|
||||
decel_lim_y: -5.0
|
||||
decel_lim_theta: -15.0
|
||||
vx_samples: 20
|
||||
vy_samples: 20
|
||||
vtheta_samples: 20
|
||||
sim_time: 0.51
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
controller_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 20.0
|
||||
publish_frequency: 10.0
|
||||
# global_frame: odom
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
rolling_window: true
|
||||
width: 5
|
||||
height: 5
|
||||
resolution: 0.05
|
||||
robot_radius: 0.2
|
||||
plugins: ["voxel2d_layer", "voxel3d_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 8.0
|
||||
inflation_radius: 0.7
|
||||
voxel2d_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: False
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
raytrace_min_range: 0.0
|
||||
raytrace_max_range: 50.0
|
||||
min_obstacle_height: 0.0
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
voxel3d_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: False
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
mark_threshold: 1
|
||||
observation_sources: livox
|
||||
min_obstacle_height: 0.00
|
||||
max_obstacle_height: 2.0
|
||||
livox:
|
||||
topic: /livox/lidar/pointcloud
|
||||
raytrace_min_range: 0.0
|
||||
raytrace_max_range: 50.0
|
||||
min_obstacle_height: 0.0
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "PointCloud2"
|
||||
static_layer:
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
local_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
local_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 20.0
|
||||
publish_frequency: 10.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
robot_radius: 0.2
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "voxel2d_layer", "voxel3d_layer", "inflation_layer"]
|
||||
voxel2d_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
raytrace_min_range: 0.0
|
||||
raytrace_max_range: 50.0
|
||||
min_obstacle_height: 0.0
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
voxel3d_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: False
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
mark_threshold: 1
|
||||
observation_sources: livox
|
||||
min_obstacle_height: 0.00
|
||||
max_obstacle_height: 2.0
|
||||
livox:
|
||||
topic: /livox/lidar/pointcloud
|
||||
raytrace_min_range: 0.0
|
||||
raytrace_max_range: 50.0
|
||||
min_obstacle_height: 0.0
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "PointCloud2"
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 8.0
|
||||
inflation_radius: 0.7
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
yaml_filename: ""
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: False
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
# planner_server_rclcpp_node:
|
||||
# ros__parameters:
|
||||
# use_sim_time: False
|
||||
|
||||
recoveries_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
recovery_plugins: ["spin", "backup", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_recoveries/Spin"
|
||||
backup:
|
||||
plugin: "nav2_recoveries/BackUp"
|
||||
wait:
|
||||
plugin: "nav2_recoveries/Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_timeout: 0.1
|
||||
use_sim_time: False
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 20.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 10.0
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 5.0
|
||||
scale_velocities: False
|
||||
feedback: "CLOSED_LOOP"
|
||||
max_velocity: [2.5, 2.5, 12.0]
|
||||
min_velocity: [-2.5, -2.5, -12.0]
|
||||
max_accel: [5.0, 5.0, 15.0]
|
||||
max_decel: [-5.0, -5.0, -15.0]
|
||||
odom_topic: "Odometry"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
@ -1,3 +0,0 @@
|
||||
.svn
|
||||
.kdev4
|
||||
*.kdev4
|
||||
@ -1,23 +0,0 @@
|
||||
build-kinetic:
|
||||
variables:
|
||||
ROSDISTRO: "kinetic"
|
||||
CATKIN_WS: "/root/catkin_ws"
|
||||
stage: build
|
||||
image: osrf/ros:$ROSDISTRO-desktop-full
|
||||
before_script:
|
||||
- apt-get -qq update
|
||||
- apt-get -qq install git-core python-catkin-tools curl
|
||||
- mkdir -p $CATKIN_WS/src
|
||||
- cp -a . $CATKIN_WS/src/
|
||||
- cd $CATKIN_WS
|
||||
- rosdep update
|
||||
- rosdep install -y --from-paths src --ignore-src --rosdistro $ROSDISTRO --as-root=apt:false
|
||||
script:
|
||||
- source /ros_entrypoint.sh
|
||||
- cd $CATKIN_WS
|
||||
- catkin build -i -s --no-status
|
||||
only:
|
||||
- kinetic-devel
|
||||
tags:
|
||||
- ubuntu
|
||||
- docker
|
||||
@ -1,108 +0,0 @@
|
||||
# Generic .travis.yml file for running continuous integration on Travis-CI with
|
||||
# any ROS package.
|
||||
#
|
||||
# This installs ROS on a clean Travis-CI virtual machine, creates a ROS
|
||||
# workspace, resolves all listed dependencies, and sets environment variables
|
||||
# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are
|
||||
# no compilation errors), and runs all the tests. If any of the compilation/test
|
||||
# phases fail, the build is marked as a failure.
|
||||
#
|
||||
# We handle two types of package dependencies:
|
||||
# - packages (ros and otherwise) available through apt-get. These are installed
|
||||
# using rosdep, based on the information in the ROS package.xml.
|
||||
# - dependencies that must be checked out from source. These are handled by
|
||||
# 'wstool', and should be listed in a file named dependencies.rosinstall.
|
||||
#
|
||||
# There are two variables you may want to change:
|
||||
# - ROS_DISTRO (default is indigo). Note that packages must be available for
|
||||
# ubuntu 14.04 trusty.
|
||||
# - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo
|
||||
# root). This should list all necessary repositories in wstool format (see
|
||||
# the ros wiki). If the file does not exists then nothing happens.
|
||||
#
|
||||
# See the README.md for more information.
|
||||
#
|
||||
# Author: Felix Duvallet <felixd@gmail.com>
|
||||
|
||||
# NOTE: The build lifecycle on Travis.ci is something like this:
|
||||
# before_install
|
||||
# install
|
||||
# before_script
|
||||
# script
|
||||
# after_success or after_failure
|
||||
# after_script
|
||||
# OPTIONAL before_deploy
|
||||
# OPTIONAL deploy
|
||||
# OPTIONAL after_deploy
|
||||
|
||||
################################################################################
|
||||
|
||||
# Use ubuntu trusty (14.04) with sudo privileges.
|
||||
dist: trusty
|
||||
sudo: required
|
||||
language:
|
||||
- generic
|
||||
cache:
|
||||
- apt
|
||||
|
||||
# Configuration variables. All variables are global now, but this can be used to
|
||||
# trigger a build matrix for different ROS distributions if desired.
|
||||
env:
|
||||
global:
|
||||
- ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [precise|trusty|...]
|
||||
- CI_SOURCE_PATH=$(pwd)
|
||||
- ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall
|
||||
- CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options
|
||||
- ROS_PARALLEL_JOBS='-j8 -l6'
|
||||
matrix:
|
||||
- ROS_DISTRO=jade
|
||||
|
||||
|
||||
################################################################################
|
||||
|
||||
# Install system dependencies, namely a very barebones ROS setup.
|
||||
before_install:
|
||||
- sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
|
||||
- wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
|
||||
- sudo apt-get update -qq
|
||||
- sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin
|
||||
- source /opt/ros/$ROS_DISTRO/setup.bash
|
||||
# Prepare rosdep to install dependencies.
|
||||
- sudo rosdep init
|
||||
- rosdep update
|
||||
|
||||
# Create a catkin workspace with the package under integration.
|
||||
install:
|
||||
- mkdir -p ~/catkin_ws/src
|
||||
- cd ~/catkin_ws/src
|
||||
- catkin_init_workspace
|
||||
# Create the devel/setup.bash (run catkin_make with an empty workspace) and
|
||||
# source it to set the path variables.
|
||||
- cd ~/catkin_ws
|
||||
- catkin_make
|
||||
- source devel/setup.bash
|
||||
# Add the package under integration to the workspace using a symlink.
|
||||
- cd ~/catkin_ws/src
|
||||
- ln -s $CI_SOURCE_PATH .
|
||||
|
||||
# Install all dependencies, using wstool and rosdep.
|
||||
# wstool looks for a ROSINSTALL_FILE defined in the environment variables.
|
||||
before_script:
|
||||
# source dependencies: install using wstool.
|
||||
- cd ~/catkin_ws/src
|
||||
- wstool init
|
||||
- if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi
|
||||
- wstool up
|
||||
# package depdencies: install using rosdep.
|
||||
- cd ~/catkin_ws
|
||||
- rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
|
||||
|
||||
# Compile and test. If the CATKIN_OPTIONS file exists, use it as an argument to
|
||||
# catkin_make.
|
||||
script:
|
||||
- cd ~/catkin_ws
|
||||
- catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS )
|
||||
# Testing: Use both run_tests (to see the output) and test (to error out).
|
||||
- catkin_make run_tests # This always returns 0, but looks pretty.
|
||||
- catkin_make test # This will return non-zero if a test fails.
|
||||
|
||||
@ -1,61 +0,0 @@
|
||||
teb_local_planner ROS Package
|
||||
=============================
|
||||
|
||||
The teb_local_planner package implements a plugin to the base_local_planner of the 2D navigation stack.
|
||||
The underlying method called Timed Elastic Band locally optimizes the robot's trajectory with respect to trajectory execution time,
|
||||
separation from obstacles and compliance with kinodynamic constraints at runtime.
|
||||
|
||||
Refer to http://wiki.ros.org/teb_local_planner for more information and tutorials.
|
||||
|
||||
Build status of the *melodic-devel* branch:
|
||||
- ROS Buildfarm (Melodic): [](http://build.ros.org/job/Mdev__teb_local_planner__ubuntu_bionic_amd64/)
|
||||
|
||||
### Port to ROS2
|
||||
This branch is the teb_local_planner package ported to ROS2(Dashing Diademata). Currently, it is currently compatible with [Navigation2(master branch)](https://github.com/ros-planning/navigation2/tree/master)([226f06c](https://github.com/ros-planning/navigation2/commit/226f06ce282c727ca240ce8be0cb4b093e26343b)). You can test teb_local_planner with Navigation2 and TurtleBot3 simulation by launching the following command.
|
||||
```
|
||||
ros2 launch teb_local_planner teb_tb3_simulation_launch.py
|
||||
```
|
||||
|
||||
## Citing the Software
|
||||
|
||||
*Since a lot of time and effort has gone into the development, please cite at least one of the following publications if you are using the planner for your own research:*
|
||||
|
||||
- C. Rösmann, F. Hoffmann and T. Bertram: Integrated online trajectory planning and optimization in distinctive topologies, Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153.
|
||||
- C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Trajectory modification considering dynamic constraints of autonomous robots. Proc. 7th German Conference on Robotics, Germany, Munich, May 2012, pp 74–79.
|
||||
- C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Efficient trajectory optimization using a sparse model. Proc. IEEE European Conference on Mobile Robots, Spain, Barcelona, Sept. 2013, pp. 138–143.
|
||||
- C. Rösmann, F. Hoffmann and T. Bertram: Planning of Multiple Robot Trajectories in Distinctive Topologies, Proc. IEEE European Conference on Mobile Robots, UK, Lincoln, Sept. 2015.
|
||||
- C. Rösmann, F. Hoffmann and T. Bertram: Kinodynamic Trajectory Optimization and Control for Car-Like Robots, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, Sept. 2017.
|
||||
|
||||
<a href="https://www.buymeacoffee.com/croesmann" target="_blank"><img src="https://cdn.buymeacoffee.com/buttons/lato-black.png" alt="Buy Me A Coffee" height="31px" width="132px" ></a>
|
||||
|
||||
## Videos
|
||||
|
||||
The left of the following videos presents features of the package and shows examples from simulation and real robot situations.
|
||||
Some spoken explanations are included in the audio track of the video.
|
||||
The right one demonstrates features introduced in version 0.2 (supporting car-like robots and costmap conversion). Please watch the left one first.
|
||||
|
||||
<a href="http://www.youtube.com/watch?feature=player_embedded&v=e1Bw6JOgHME" target="_blank"><img src="http://img.youtube.com/vi/e1Bw6JOgHME/0.jpg"
|
||||
alt="teb_local_planner - An Optimal Trajectory Planner for Mobile Robots" width="240" height="180" border="10" /></a>
|
||||
<a href="http://www.youtube.com/watch?feature=player_embedded&v=o5wnRCzdUMo" target="_blank"><img src="http://img.youtube.com/vi/o5wnRCzdUMo/0.jpg"
|
||||
alt="teb_local_planner - Car-like Robots and Costmap Conversion" width="240" height="180" border="10" /></a>
|
||||
|
||||
## License
|
||||
|
||||
The *teb_local_planner* package is licensed under the BSD license.
|
||||
It depends on other ROS packages, which are listed in the package.xml. They are also BSD licensed.
|
||||
|
||||
Some third-party dependencies are included that are licensed under different terms:
|
||||
- *Eigen*, MPL2 license, http://eigen.tuxfamily.org
|
||||
- *libg2o* / *g2o* itself is licensed under BSD, but the enabled *csparse_extension* is licensed under LGPL3+,
|
||||
https://github.com/RainerKuemmerle/g2o. [*CSparse*](http://www.cise.ufl.edu/research/sparse/CSparse/) is included as part of the *SuiteSparse* collection, http://www.suitesparse.com.
|
||||
- *Boost*, Boost Software License, http://www.boost.org
|
||||
|
||||
All packages included are distributed in the hope that they will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details.
|
||||
|
||||
## Requirements
|
||||
|
||||
Install dependencies (listed in the *package.xml* and *CMakeLists.txt* file) using *rosdep*:
|
||||
|
||||
rosdep install teb_local_planner
|
||||
|
||||
|
||||
@ -1,407 +0,0 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package teb_local_planner
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.9.1 (2020-05-29)
|
||||
------------------
|
||||
* Fixed RobotFootprintModel visualization bug (thanks to Anson Wang)
|
||||
* Reserve the size of the relevant obstacles vector to avoid excessive memory allocations (thanks to João Monteiro)
|
||||
* CMake: Removed system include to avoid compiling issues on some platforms
|
||||
* Contributors: Anson Wang, Christoph Rösmann, João Carlos Espiúca Monteiro
|
||||
|
||||
0.9.0 (2020-05-26)
|
||||
------------------
|
||||
* Added pill resp. stadium-shaped obstacle
|
||||
* Changed minimum CMake version to 3.1
|
||||
* Improved efficiency of 3d h-signature computation
|
||||
* Changed default value for parameter penalty_epsilon to 0.05
|
||||
* Improved efficiency of findClosedTrajectoryPose()
|
||||
* Removed obsolete method isHorizonReductionAppropriate()
|
||||
* Contributors: Christoph Rösmann, XinyuKhan
|
||||
|
||||
0.8.4 (2019-12-02)
|
||||
------------------
|
||||
* Fixed TEB autoResize if last TimeDiff is small
|
||||
* Add a rotational threshold for identifying a warm start goal
|
||||
* Contributors: Rainer Kümmerle
|
||||
|
||||
0.8.3 (2019-10-25)
|
||||
------------------
|
||||
* Limiting the control look-ahead pose to the first that execeeds the expected look-ahead time (thanks to Marco Bassa)
|
||||
* test_optim_node fix circular obstacles (thanks to dtaranta)
|
||||
* Fix shadow variable warning (thanks to Victor Lopez)
|
||||
* Use SYSTEM when including external dependencies headers (thanks to Victor Lopez)
|
||||
* Robustify initTrajectoryToGoal if a plan is given (thanks to Rainer Kuemmerle)
|
||||
* Adding the option to shift ahead the target pose used to extract the velocity command (thanks to Marco Bassa)
|
||||
* Fixed segfault in optimal_planner.cpp when clearing graph with unallocated optimizer.
|
||||
Fixes `#158 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/158>`_.
|
||||
* On footprintCost, fail only if footprint is in collision, not outside the map or on unknown space (thanks to corot)
|
||||
* Native MoveBaseFlex support added: Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract interfaces (thanks to corot)
|
||||
* added warning if parameter optimal_time is <= 0
|
||||
* Nonlinear obstacle cost from EdgeInflatedObstacle also added to EdgeObstacle.
|
||||
See `#140 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/140>`_.
|
||||
* Fixed proper initialization of parameter obstacle_cost_exponent in case it is not loaded from the parameter server
|
||||
* Contributors: Christoph Rösmann, Marco Bassa, Rainer Kuemmerle, Victor Lopez, corot, dtaranta
|
||||
|
||||
0.8.2 (2019-07-02)
|
||||
------------------
|
||||
* Allow scripts to be executable and usable by rosrun after catkin_make install and through the catkin release process (thanks to Devon Ash)
|
||||
* Add nonlinear part to obstacle cost to improve narrow gap behavior.
|
||||
Parameter `obstacle_cost_exponent` defines the exponent of the nonlinear cost term.
|
||||
The default linear behavior is achieved by setting this parameter to 1 (default).
|
||||
A value of 4 performed well in some tests and experiments (thanks to Howard Cochran).
|
||||
* Parameter `global_plan_prune_distance` added via ros parameter server.
|
||||
* Fixed SIGSEGV in optimizeAllTEBs() if main thread is interrupted by boost (thanks to Howard Cochran)
|
||||
* Fixed SIGSEGV crash in deleteTebDetours() (thanks to Howard Cochran)
|
||||
* On footprint visualization, avoid overshadowing by obstacles (thanks to corot)
|
||||
* Do not ignore robot model on the association stage.
|
||||
Important mostly for polygon footprint model (thanks to corot).
|
||||
* Adjustable color for footprint visualization
|
||||
* Showing (detected) infeasible robot poses in a separate marker namespace and color
|
||||
* Added edge for minimizing Euclidean path length (parameter: `weight_shortest_path`)
|
||||
* Ackermann steering conversion (python script): fixed direction inversion in backwards mode when `cmd_angle_instead_rotvel` is true (thanks to Tobi Loew)
|
||||
* Fixed wrong skipping condition in AddEdgesKinematicsCarlike() (thanks to ShiquLIU)
|
||||
* Never discarding the previous best teb in renewAndAnalyzeOldTebs (thanks to Marco Bassa)
|
||||
* Allowing for the fallback to a different trajectory when the costmap check fails. This prevents the switch to unfeasible trajectories (thanks to Marco Bassa).
|
||||
* Skipping the generation of the homotopy exploration graph in case the maximum number of allowed classes is reached (thanks to Marco Bassa)
|
||||
* Changed isTrajectoryFeasible function to allow for a more accurate linear and angular discretization (thanks to Marco Bassa)
|
||||
* Function TebOptimalPlanner::computeError() considers now the actual optimizer weights.
|
||||
As a result, the default value of `selection_obst_cost_scale` is reduced (thanks to Howard Cochran).
|
||||
* update to use non deprecated pluginlib macro (thanks to Mikael Arguedas)
|
||||
* Avoiding h signature interpolation between coincident poses (thanks to Marco Bassa)
|
||||
* New strategy for the deletion of detours: Detours are now determined w.r.t. the least-cost alternative and not w.r.t. just the goal heading.
|
||||
Deletion of additional alternatives applies if either an initial backward motion is detected, if the transition time is much bigger than the duration of the best teb
|
||||
and if a teb cannot be optimized (thanks to Marco Bassa).
|
||||
Optionally allowing the usage of the initial plan orientation when initializing new tebs.
|
||||
* Contributors: Christoph Rösmann, Mikael Arguedas, Devon Ash, Howard Cochran, Marco Bassa, ShiquLIU, Tobi Loew, corot
|
||||
|
||||
0.8.1 (2018-08-14)
|
||||
------------------
|
||||
* bugfix in calculateHSignature. Fixes `#90 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/90>`_.
|
||||
* fixed centroid computation in a special case of polygon-obstacles
|
||||
* Contributors: Christoph Rösmann
|
||||
|
||||
0.8.0 (2018-08-06)
|
||||
------------------
|
||||
* First melodic release
|
||||
* Updated to new g2o API
|
||||
* Migration to tf2
|
||||
* Contributors: Christoph Rösmann
|
||||
|
||||
0.7.3 (2018-07-05)
|
||||
------------------
|
||||
* Parameter `switching_blocking_period` added to homotopy class planner parameter group.
|
||||
Values greater than zero enforce the homotopy class planner to only switch to new equivalence classes as soon
|
||||
as the given period is expired (this might reduce oscillations in some scenarios). The value is set to zero seconds
|
||||
by default in order to not change the behavior of existing configurations.
|
||||
* Fixed unconsistent naming of parameter `global_plan_viapoint_sep`.
|
||||
The parameter retrieved at startup was `global_plan_via_point_sep` and via dynamic_reconfigure it was `global_plan_viapoint_sep`.
|
||||
`global_plan_via_point_sep` has now been replaced by `global_plan_viapoint_sep` since this is more consistent with the variable name
|
||||
in the code as well as `weight_viapoint` and the ros wiki description.
|
||||
In order to not break things, the old parameter name can still be used. However, a deprecated warning is printed.
|
||||
* transformGlobalPlan searches now for the closest point within the complete subset of the global plan in the local costmap:
|
||||
In every sampling interval, the global plan is processed in order to find the closest pose to the robot (as reference start)
|
||||
and the current end pose (either local at costmap boundary or max_global_plan_lookahead_dist).
|
||||
Previously, the search algorithm stopped as soon as the distance to the robot increased once.
|
||||
This caused troubles with more complex global plans, hence the new strategy checks the complete subset
|
||||
of the global plan in the local costmap for the closest distance to the robot.
|
||||
* via-points that are very close to the current robot pose or behind the robot are now skipped (in non-ordered mode)
|
||||
* Edge creation: minor performance improvement for dynamic obstacle edges
|
||||
* dynamic_reconfigure: parameter visualize_with_time_as_z_axis_scale moved to group trajectory
|
||||
* Contributors: Christoph Rösmann
|
||||
|
||||
0.7.2 (2018-06-08)
|
||||
------------------
|
||||
* Adds the possibility to provide via-points via a topic.
|
||||
Currently, the user needs to decide whether to receive via-points from topic or to obtain them from the global reference plan
|
||||
(e.g., activate the latter by setting global_plan_viapoint_sep>0 as before).
|
||||
A small test script publish_viapoints.py is provided to demonstrate the feature within test_optim_node.
|
||||
* Contributors: Christoph Rösmann
|
||||
|
||||
0.7.1 (2018-06-05)
|
||||
------------------
|
||||
* Fixed a crucial bug (from 0.6.6): A cost function for prefering a clockwise resp. anti-clockwise turn was enabled by default.
|
||||
This cost function was only intended to be active only for recovering from an oscillating robot.
|
||||
This cost led to a penalty for one of the turning directions and hence the maximum turning rate for the penalized direction could not be reached.
|
||||
Furthermore, which is more crucial: since the penalty applied only to a small (initial) subset of the trajectory, the overall control performance was poor
|
||||
(huge gap between planned motion and closed-loop trajectories led to frequent corrections of the robot pose and hence many motion reversals).
|
||||
* Adds support for circular obstacle types. This includes support for the radius field in costmap_converter::ObstacleMsg
|
||||
* rqt reconfigure: parameters are now grouped in tabs (robot, trajectory, viapoints, ...)
|
||||
* Update to use non deprecated pluginlib macro
|
||||
* Python scripts updated to new obstacle message definition.
|
||||
* Fixed issue when start and end are at the same location (PR #43)
|
||||
* Normalize marker quaternions in *test_optim_node*
|
||||
* Contributors: Christoph Rösmann, Alexander Reimann, Mikael Arguedas, wollip
|
||||
|
||||
0.7.0 (2017-09-23)
|
||||
------------------
|
||||
* This update introduces support for dynamic obstacles (thanks to Franz Albers, who implemented and tested the code).
|
||||
Dynamic obstacle support requires parameter *include\_dynamic\_obstacles* to be activated.
|
||||
Note, this feature is still experimental and subject to testing.
|
||||
Motion prediction is performed using a constant velocity model.
|
||||
Dynamic obstacles might be incorporated as follows:
|
||||
* via a custom message provided on topic ~/obstacles (warning: we changed the message type from teb_local_planner/ObstacleMsg to costmap_converter/ObstacleArrayMsg).
|
||||
* via the CostmapToDynamicObstacles plugin as part of the costmap\_converter package (still experimental).
|
||||
A tutorial is going to be provided soon.
|
||||
* FeedbackMsg includes a ObstacleMsg instead of a polygon
|
||||
* ObstacleMsg removed from package since it is now part of the costmap\_converter package.
|
||||
* Homotopy class planer code update: graph search methods and equivalence classes (h-signatures) are now
|
||||
implemented as subclasses of more general interfaces.
|
||||
* TEB trajectory initialization now uses a max\_vel\_x argument instead of the desired time difference in order to give the optimizer a better warm start.
|
||||
Old methods are marked as deprecated. This change does not affect users settings.
|
||||
* Inplace rotations removed from trajectory initialization to improve convergence speed of the optimizer
|
||||
* teb\_local\_planner::ObstacleMsg removed in favor of costmap\_converter::ObstacleArrayMsg. This also requires custom obstacle publishers to update to the new format
|
||||
* the "new" trajectory resizing method is only activated, if "include_dynamic_obstacles" is set to true.
|
||||
We introduced the non-fast mode with the support of dynamic obstacles
|
||||
(which leads to better results in terms of x-y-t homotopy planning).
|
||||
However, we have not yet tested this mode intensively, so we keep
|
||||
the previous mode as default until we finish our tests.
|
||||
* added parameter and code to update costmap footprint if it is dynamic (#49)
|
||||
* Contributors: Franz Albers, Christoph Rösmann, procopiostein
|
||||
|
||||
0.6.6 (2016-12-23)
|
||||
------------------
|
||||
* Strategy for recovering from oscillating local plans added (see new parameters)
|
||||
* Horizon reduction for resolving infeasible trajectories is not activated anymore if the global goal is already selected
|
||||
(to avoid oscillations due to changing final orientations)
|
||||
* Global plan orientations are now taken for TEB initialization if lobal_plan_overwrite_orientation==true
|
||||
* Parameter max_samples added
|
||||
* Further fixes (thanks to Matthias Füller and Daniel Neumann for providing patches)
|
||||
|
||||
0.6.5 (2016-11-15)
|
||||
------------------
|
||||
* The trajectory is now initialized backwards for goals close to and behind the robot.
|
||||
Parameter 'allow_init_with_backwards_motion' added.
|
||||
* Updated the TEB selection in the HomotopyClassPlanner.
|
||||
* A new parameter is introduced to prefer the equivalence class of the initial plan
|
||||
* Fixed some bugs related to the deletion of candidates and for keeping the equivalence class of the initial plan.
|
||||
* Weight adaptation added for obstacles edges.
|
||||
Added parameter 'weight_adapt_factor'.
|
||||
Obstacle weights are repeatedly scaled by this factor in each outer TEB iteration.
|
||||
Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions.
|
||||
* Added a warning if the optim footprint + min_obstacle_dist is smaller than the costmap footprint.
|
||||
Validation is performed by only comparing the inscribed radii of the footprints.
|
||||
* Revision/extension of the reduced-horizon backup mode which is triggered in case infeasible trajectories are detected.
|
||||
* Changed HSignature to a generic equivalence class
|
||||
* Minor changes
|
||||
|
||||
0.6.4 (2016-10-23)
|
||||
------------------
|
||||
* New default obstacle association strategy:
|
||||
During optimization graph creation, for each pose of the trajectory a
|
||||
relevance detection is performed before considering the obstacle
|
||||
during optimization. New parameters are introduced. The
|
||||
old strategy is kept as 'legacy' strategy (see parameters).
|
||||
* Computation of velocities, acceleration and turning radii extended:
|
||||
Added an option to compute the actual arc length
|
||||
instead of using the Euclidean distance approximation (see parameter `exact_arc_length`.
|
||||
* Added intermediate edge layer for unary, binary and multi edges in order to reduce code redundancy.
|
||||
* Script for visualizing velocity profile updated to accept the feedback topic name via rosparam server
|
||||
* Removed TebConfig dependency in TebVisualization
|
||||
* PolygonObstacle can now be constructed using a vertices container
|
||||
* HomotopyClassPlanner public interface extended
|
||||
* Changed H-Signature computation to work 'again' with few obstacles such like 1 or 2
|
||||
* Removed inline flags in visualization.cpp
|
||||
* Removed inline flags in timed_elastic_band.cpp.
|
||||
Fixes `#15 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/15>`_.
|
||||
* Increased bounds of many variables in dynamic_reconfigure.
|
||||
Resolves `#14 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/14>`_.
|
||||
The particular variables are maximum velocities, maximum accelerations,
|
||||
minimum turning radius,...
|
||||
Note: optimization weights and dt_ref as well as dt_hyst are not
|
||||
tuned for velocities and accelerations beyond
|
||||
the default values (e.g. >1 m/s). Just increasing the maximum velocity
|
||||
bounds without adjusting the other parameters leads to an insufficient behavior.
|
||||
* Default parameter value update: 'costmap_obstacles_behind_robot_dist'
|
||||
* Additional minor fixes.
|
||||
|
||||
0.6.3 (2016-08-17)
|
||||
------------------
|
||||
* Changed the f0 function for calculating the H-Signature.
|
||||
The new one seems to be more robust for a much larger number of obstacles
|
||||
after some testing.
|
||||
* HomotopyClassPlanner: vertex collision check removed since collisions will be determined in the edge collision check again
|
||||
* Fixed distance calculation polygon-to-polygon-obstacle
|
||||
* cmake config exports now *include directories* of external packages for dependent projects
|
||||
* Enlarged upper bounds on goal position and orientation tolerances in *dynamic_reconfigure*. Fixes #13.
|
||||
|
||||
|
||||
0.6.2 (2016-06-15)
|
||||
------------------
|
||||
* Fixed bug causing the goal to disappear in case the robot arrives with non-zero orientation error.
|
||||
* Inflation mode for obstacles added.
|
||||
* The homotopy class of the global plan is now always forced to be initialized as trajectory.
|
||||
* The initial velocity of the robot is now taken into account correctly for
|
||||
all candidate trajectories.
|
||||
* Removed a check in which the last remaining candidate trajectory was rejected if it was close to an obstacle.
|
||||
This fix addresses issue `#7 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/7>`_
|
||||
|
||||
0.6.1 (2016-05-23)
|
||||
------------------
|
||||
* Debian ARM64 library path added to SuiteSparse cmake find-script (resolves ARM compilation issue)
|
||||
|
||||
|
||||
0.6.0 (2016-05-22)
|
||||
------------------
|
||||
* Extended support to holonomic robots
|
||||
* Wrong parameter namespace for *costmap_converter* plugins fixed
|
||||
* Added the option to scale the length of the hcp sampling area
|
||||
* Compiler warnings fixed.
|
||||
* Workaround for compilation issues that are caused by a bug in boost 1.58
|
||||
concerning the graph library (missing move constructor/assignment operator
|
||||
in boost source).
|
||||
* Using *tf_listener* from *move_base* now.
|
||||
* Via-point support improved.
|
||||
Added the possibility to take the actual order of via-points into account.
|
||||
Additionally, via-points beyond start and goal are now included.
|
||||
* Obsolete include of the angles package header removed
|
||||
* Update to package.xml version 2
|
||||
* Some other minor fixes.
|
||||
|
||||
|
||||
0.4.0 (2016-04-19)
|
||||
------------------
|
||||
* The teb_local_planner supports a path-following mode (w.r.t. the global plan) and via-points now.
|
||||
This allows the user to adapt the tradeoff between time-optimality and path-following.
|
||||
Check out the new tutorial: "Following the Global Plan (Via-Points)".
|
||||
* All external configuration and launch files are removed, since they are part
|
||||
of the new teb_local_planner_tutorials package.
|
||||
|
||||
|
||||
0.3.1 (2016-04-14)
|
||||
------------------
|
||||
* Fixed wrong coordinate transformation in 'line' and 'polygon' footprint models.
|
||||
* Trajectory selection strategy in case of multiple topologies updated:
|
||||
* The obstacle costs for selection can now be scaling separately.
|
||||
* The cost regarding time optimality can now be replaced by the actual transition time.
|
||||
* Added a hysteresis to cost comparison between a new and the previously selected trajectory.
|
||||
* In the default parameter setting the strategy is similar to release 0.3.0.
|
||||
* Warning message removed that occured if an odom message with only zeros was received.
|
||||
|
||||
|
||||
0.3.0 (2016-04-08)
|
||||
------------------
|
||||
* Different/custom robot footprints are now supported and subject to optimization (refer to the new tutorial!).
|
||||
* The new robot footprint is also visualized using the common marker topic.
|
||||
* The strategy of taking occupied costmap cells behind the robot into account has been improved.
|
||||
These changes significantly improve navigation close to walls.
|
||||
* Parameter 'max_global_plan_lookahead_dist' added.
|
||||
Previously, the complete subset of the global plan contained in the local costmap
|
||||
was taken into account for choosing the current intermediate goal point. With this parameter, the maximum
|
||||
length of the reference global plan can be limited. The actual global plan subset
|
||||
is now computed using the logical conjunction of both local costmap size and 'max_global_plan_lookahead_dist'.
|
||||
* Bug fixes:
|
||||
* Fixed a compilation issue on ARM architectures
|
||||
* If custom obstacles are used, the container with old obstacles is now cleared properly.
|
||||
* Parameter cleanup:
|
||||
* "weight_X_obstacle" parameters combined to single parameter "weight_obstacle".
|
||||
* "X_obstacle_poses_affected" parameters combined to single parameter "obstacle_poses_affected".
|
||||
* Deprecated parameter 'costmap_emergency_stop_dist' removed.
|
||||
* Code cleanup
|
||||
|
||||
|
||||
0.2.3 (2016-02-01)
|
||||
------------------
|
||||
* Marker lifetime changed
|
||||
* In case the local planner detects an infeasible trajectory it does now try to
|
||||
reduce the horizon to 50 percent of the length. The trajectory is only reduced
|
||||
if some predefined cases are detected.
|
||||
This mechanism constitutes a backup behavior.
|
||||
* Improved carlike robot support.
|
||||
Instead of commanding the robot using translational and rotational velocities,
|
||||
the robot might also be commanded using the transl. velocity and steering angle.
|
||||
Appropriate parameters are added to the config.
|
||||
* Changed default parameter for 'h_signature_threshold' from 0.01 to 0.1 to better match the actual precision.
|
||||
* Some python scripts for data conversion added
|
||||
* Minor other changes
|
||||
|
||||
0.2.2 (2016-01-11)
|
||||
------------------
|
||||
* Carlike robots (ackermann steering) are supported from now on (at least experimentally)
|
||||
by specifying a minimum bound on the turning radius.
|
||||
Currently, the output of the planner in carlike mode is still (v,omega).
|
||||
Since I don't have any real carlike robot, I would be really happy if someone could provide me with
|
||||
some feedback to further improve/extend the support.
|
||||
* Obstacle cost function modified to avoid undesired jerks in the trajectory.
|
||||
* Added a feedback message that contains current trajectory information (poses, velocities and temporal information).
|
||||
This is useful for analyzing and debugging the velocity profile e.g. at runtime.
|
||||
The message will be published only if it's activated (rosparam).
|
||||
A small python script is added to plot the velocity profile (while *test_optim_node* runs).
|
||||
* Cost functions are now taking the direction/sign of the translational velocity into account:
|
||||
Specifying a maximum backwards velocity other than forward velocity works now.
|
||||
Additionally, the change in acceleration is now computed correctly if the robot switches directions.
|
||||
* The global plan is now pruned such that already passed posses are cut off
|
||||
(relevant for global planners with *planning_rate=0*).
|
||||
* Fixed issue#1: If a global planner with *planning_rate=0* was used,
|
||||
a TF timing/extrapolation issue appeared after some time.
|
||||
* The planner resets now properly if the velocity command cannot be computed due to invalid optimization results.
|
||||
|
||||
|
||||
0.2.1 (2015-12-30)
|
||||
------------------
|
||||
* This is an important bugfix release.
|
||||
* Fixed a major issue concerning the stability and performance of the optimization process. Each time the global planner was updating the global plan, the local planner was resetted completely even if
|
||||
the updated global plan did not differ from the previous one. This led to stupid reinitializations and a slighly jerky behavior if the update rate of the global planner was high (each 0.5-2s).
|
||||
From now on the local planner is able to utilize the global plan as a warm start and determine automatically whether to reinitialize or not.
|
||||
* Support for polygon obstacles extended and improved (e.g. the homotopy class planner does now compute actual distances to the polygon rather than utilizing the distance to the centroid).
|
||||
|
||||
0.2.0 (2015-12-23)
|
||||
------------------
|
||||
* The teb_local_planner supports costmap_converter plugins (pluginlib) from now on. Those plugins convert occupied costmap2d cells into polygon shapes.
|
||||
The costmap_converter is disabled by default, since the extension still needs to be tested (parameter choices, computation time advantages, etc.).
|
||||
A tutorial will explain how to activate the converter using the ros-param server.
|
||||
|
||||
0.1.11 (2015-12-12)
|
||||
-------------------
|
||||
* This is a bugfix release (it fixes a lot of issues which occured frequently when the robot was close to the goal)
|
||||
|
||||
0.1.10 (2015-08-13)
|
||||
-------------------
|
||||
* The optimizer copies the global plan as initialization now instead of using a simple straight line approximation.
|
||||
* Some bugfixes and improvements
|
||||
|
||||
0.1.9 (2015-06-24)
|
||||
------------------
|
||||
* Fixed a segmentation fault issue. This minor update is crucial for stability.
|
||||
|
||||
0.1.8 (2015-06-08)
|
||||
------------------
|
||||
* Custom obstacles can be included via publishing dedicated messages
|
||||
* Goal-reached-condition also checks orientation error (desired yaw) now
|
||||
* Numerical improvements of the h-signature calculation
|
||||
* Minor bugfixes
|
||||
|
||||
0.1.7 (2015-05-22)
|
||||
------------------
|
||||
* Finally fixed saucy compilation issue by retaining compatiblity to newer distros
|
||||
(my "new" 13.10 VM helps me to stop spamming new releases for testing).
|
||||
|
||||
0.1.6 (2015-05-22)
|
||||
------------------
|
||||
* Fixed compilation errors on ubuntu saucy caused by different FindEigen.cmake scripts.
|
||||
I am not able to test releasing on saucy, forcing me to release again and again. Sorry.
|
||||
|
||||
0.1.5 (2015-05-21)
|
||||
------------------
|
||||
* Added possibility to dynamically change parameters of test_optim_node using dynamic reconfigure.
|
||||
* Fixed a wrong default-min-max tuple in the dynamic reconfigure config.
|
||||
* Useful config and launch files are now added to cmake install.
|
||||
* Added install target for the test_optim_node executable.
|
||||
|
||||
0.1.4 (2015-05-20)
|
||||
------------------
|
||||
* Fixed compilation errors on ROS Jade
|
||||
|
||||
0.1.3 (2015-05-20)
|
||||
------------------
|
||||
* Fixed compilation errors on ubuntu saucy
|
||||
|
||||
0.1.2 (2015-05-19)
|
||||
------------------
|
||||
* Removed unused include that could break compilation.
|
||||
|
||||
0.1.1 (2015-05-19)
|
||||
------------------
|
||||
* All files added to the indigo-devel branch
|
||||
* Initial commit
|
||||
* Contributors: Christoph Rösmann
|
||||
@ -1,162 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(teb_local_planner)
|
||||
|
||||
# Set to Release in order to speed up the program significantly
|
||||
set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
|
||||
|
||||
# Default to C++14
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
#if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
# add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
#endif()
|
||||
|
||||
## Find catkin macros and libraries
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(costmap_converter REQUIRED)
|
||||
find_package(dwb_critics REQUIRED)
|
||||
find_package(nav_2d_utils REQUIRED)
|
||||
find_package(nav2_core REQUIRED)
|
||||
find_package(nav2_costmap_2d REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
#find_package(interactive_markers REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(rclcpp_lifecycle REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(teb_msgs REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_eigen REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
|
||||
message(STATUS "System: ${CMAKE_SYSTEM}")
|
||||
## System dependencies are found with CMake's conventions
|
||||
SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake_modules)
|
||||
message(STATUS "${CMAKE_MODULE_PATH}")
|
||||
find_package(SUITESPARSE REQUIRED)
|
||||
find_package(G2O REQUIRED)
|
||||
find_package(Boost REQUIRED)
|
||||
|
||||
# Eigen3 FindScript Backward compatibility (ubuntu saucy)
|
||||
# Since FindEigen.cmake is deprecated starting from jade.
|
||||
if (EXISTS "FindEigen3.cmake")
|
||||
find_package(Eigen3 REQUIRED)
|
||||
set(Eigen_INCLUDE_DIRS ${Eigen3_INCLUDE_DIRS})
|
||||
elseif (EXISTS "FindEigen.cmake")
|
||||
find_package(Eigen REQUIRED)
|
||||
elseif (EXISTS "FindEigen.cmake")
|
||||
message(WARNING "No findEigen cmake script found. You must provde one of them,
|
||||
e.g. by adding it to ${PROJECT_SOURCE_DIR}/cmake_modules.")
|
||||
endif (EXISTS "FindEigen3.cmake")
|
||||
|
||||
set(EXTERNAL_INCLUDE_DIRS ${Eigen_INCLUDE_DIRS} ${SUITESPARSE_INCLUDE_DIRS} ${G2O_INCLUDE_DIR})
|
||||
set(EXTERNAL_LIBS ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${G2O_LIBRARIES})
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${Boost_INCLUDE_DIRS}
|
||||
${EXTERNAL_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Build the teb_local_planner library
|
||||
|
||||
add_library(teb_local_planner SHARED
|
||||
src/timed_elastic_band.cpp
|
||||
src/optimal_planner.cpp
|
||||
src/obstacles.cpp
|
||||
src/visualization.cpp
|
||||
src/recovery_behaviors.cpp
|
||||
src/teb_config.cpp
|
||||
src/homotopy_class_planner.cpp
|
||||
src/teb_local_planner_ros.cpp
|
||||
src/graph_search.cpp
|
||||
)
|
||||
|
||||
set(ament_dependencies
|
||||
costmap_converter
|
||||
dwb_critics
|
||||
nav_2d_utils
|
||||
nav2_core
|
||||
nav2_costmap_2d
|
||||
nav2_util
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
rclcpp_lifecycle
|
||||
std_msgs
|
||||
pluginlib
|
||||
teb_msgs
|
||||
tf2
|
||||
tf2_eigen
|
||||
tf2_geometry_msgs
|
||||
tf2_ros
|
||||
visualization_msgs
|
||||
builtin_interfaces
|
||||
)
|
||||
ament_target_dependencies(teb_local_planner
|
||||
${ament_dependencies}
|
||||
)
|
||||
target_link_libraries(teb_local_planner
|
||||
${EXTERNAL_LIBS}
|
||||
)
|
||||
|
||||
target_compile_definitions(teb_local_planner PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
|
||||
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
install(TARGETS teb_local_planner
|
||||
DESTINATION lib
|
||||
)
|
||||
|
||||
## Mark cpp header files for installation
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
install(FILES
|
||||
teb_local_planner_plugin.xml
|
||||
DESTINATION share
|
||||
)
|
||||
|
||||
install(PROGRAMS scripts/cmd_vel_to_ackermann_drive.py DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(teb_local_planner)
|
||||
ament_export_dependencies(${ament_dependencies})
|
||||
pluginlib_export_plugin_description_file(nav2_core teb_local_planner_plugin.xml)
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
#ament_add_gtest(homotopy_class_planner_test
|
||||
# test/homotopy_class_planner_test.cpp
|
||||
#)
|
||||
#target_link_libraries(homotopy_class_planner_test
|
||||
# teb_local_planner
|
||||
#)
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@ -1,28 +0,0 @@
|
||||
Copyright (c) 2016, TU Dortmund - Lehrstuhl RST
|
||||
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 teb_local_planner 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 HOLDER 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.
|
||||
|
||||
@ -1,420 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
#from local_planner_limits import add_generic_localplanner_params
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
# This unusual line allows to reuse existing parameter definitions
|
||||
# that concern all localplanners
|
||||
#add_generic_localplanner_params(gen)
|
||||
|
||||
# For integers and doubles:
|
||||
# Name Type Reconfiguration level
|
||||
# Description
|
||||
# Default Min Max
|
||||
|
||||
|
||||
grp_trajectory = gen.add_group("Trajectory", type="tab")
|
||||
|
||||
# Trajectory
|
||||
grp_trajectory.add("teb_autosize", bool_t, 0,
|
||||
"Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)",
|
||||
True)
|
||||
|
||||
grp_trajectory.add("dt_ref", double_t, 0,
|
||||
"Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)",
|
||||
0.3, 0.01, 1)
|
||||
|
||||
grp_trajectory.add("dt_hysteresis", double_t, 0,
|
||||
"Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref",
|
||||
0.1, 0.002, 0.5)
|
||||
|
||||
grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0,
|
||||
"Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically",
|
||||
True)
|
||||
|
||||
grp_trajectory.add("allow_init_with_backwards_motion", bool_t, 0,
|
||||
"If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)",
|
||||
False)
|
||||
|
||||
grp_trajectory.add("max_global_plan_lookahead_dist", double_t, 0,
|
||||
"Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]",
|
||||
3.0, 0, 50.0)
|
||||
|
||||
grp_trajectory.add("force_reinit_new_goal_dist", double_t, 0,
|
||||
"Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)",
|
||||
1.0, 0.0, 10.0)
|
||||
|
||||
grp_trajectory.add("force_reinit_new_goal_angular", double_t, 0,
|
||||
"Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)",
|
||||
0.78, 0.0, 4.0)
|
||||
|
||||
grp_trajectory.add("feasibility_check_no_poses", int_t, 0,
|
||||
"Specify up to which pose (under the feasibility_check_lookahead_distance) on the predicted plan the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_lookahead_distance are checked.",
|
||||
5, -1, 50)
|
||||
|
||||
grp_trajectory.add("feasibility_check_lookahead_distance", double_t, 0,
|
||||
"Specify up to which distance (and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_no_poses are checked.",
|
||||
-1, -1, 20)
|
||||
|
||||
grp_trajectory.add("exact_arc_length", bool_t, 0,
|
||||
"If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.",
|
||||
False)
|
||||
|
||||
grp_trajectory.add("publish_feedback", bool_t, 0,
|
||||
"Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)",
|
||||
False)
|
||||
|
||||
grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0,
|
||||
"If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.",
|
||||
0, 0, 1)
|
||||
|
||||
# ViaPoints
|
||||
grp_viapoints = gen.add_group("ViaPoints", type="tab")
|
||||
|
||||
grp_viapoints.add("global_plan_viapoint_sep", double_t, 0,
|
||||
"Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]",
|
||||
-0.1, -0.1, 5.0)
|
||||
|
||||
grp_viapoints.add("via_points_ordered", bool_t, 0,
|
||||
"If true, the planner adheres to the order of via-points in the storage container",
|
||||
False)
|
||||
|
||||
# Robot
|
||||
grp_robot = gen.add_group("Robot", type="tab")
|
||||
|
||||
grp_robot.add("max_vel_x", double_t, 0,
|
||||
"Maximum translational velocity of the robot",
|
||||
0.4, 0.01, 100)
|
||||
|
||||
grp_robot.add("max_vel_x_backwards", double_t, 0,
|
||||
"Maximum translational velocity of the robot for driving backwards",
|
||||
0.2, 0.01, 100)
|
||||
|
||||
grp_robot.add("max_vel_theta", double_t, 0,
|
||||
"Maximum angular velocity of the robot",
|
||||
0.3, 0.01, 100)
|
||||
|
||||
grp_robot.add("acc_lim_x", double_t, 0,
|
||||
"Maximum translational acceleration of the robot",
|
||||
0.5, 0.01, 100)
|
||||
|
||||
grp_robot.add("acc_lim_theta", double_t, 0,
|
||||
"Maximum angular acceleration of the robot",
|
||||
0.5, 0.01, 100)
|
||||
|
||||
grp_robot.add("is_footprint_dynamic", bool_t, 0,
|
||||
"If true, updated the footprint before checking trajectory feasibility",
|
||||
False)
|
||||
|
||||
grp_robot.add("use_proportional_saturation", bool_t, 0,
|
||||
"If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually",
|
||||
False)
|
||||
grp_robot.add("transform_tolerance", double_t, 0,
|
||||
"Tolerance when querying the TF Tree for a transformation (seconds)",
|
||||
0.5, 0.001, 20)
|
||||
|
||||
# Robot/Carlike
|
||||
|
||||
grp_robot_carlike = grp_robot.add_group("Carlike", type="hide")
|
||||
|
||||
grp_robot_carlike.add("min_turning_radius", double_t, 0,
|
||||
"Minimum turning radius of a carlike robot (diff-drive robot: zero)",
|
||||
0.0, 0.0, 50.0)
|
||||
|
||||
grp_robot_carlike.add("wheelbase", double_t, 0,
|
||||
"The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!",
|
||||
1.0, -10.0, 10.0)
|
||||
|
||||
grp_robot_carlike.add("cmd_angle_instead_rotvel", bool_t, 0,
|
||||
"Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')",
|
||||
False)
|
||||
|
||||
# Robot/Omni
|
||||
|
||||
grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide")
|
||||
|
||||
grp_robot_omni.add("max_vel_y", double_t, 0,
|
||||
"Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)",
|
||||
0.0, 0.0, 100)
|
||||
|
||||
grp_robot_omni.add("acc_lim_y", double_t, 0,
|
||||
"Maximum strafing acceleration of the robot",
|
||||
0.5, 0.01, 100)
|
||||
|
||||
# GoalTolerance
|
||||
grp_goal = gen.add_group("GoalTolerance", type="tab")
|
||||
|
||||
grp_goal.add("free_goal_vel", bool_t, 0,
|
||||
"Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)",
|
||||
False)
|
||||
|
||||
# Obstacles
|
||||
grp_obstacles = gen.add_group("Obstacles", type="tab")
|
||||
|
||||
grp_obstacles.add("min_obstacle_dist", double_t, 0,
|
||||
"Minimum desired separation from obstacles",
|
||||
0.5, 0, 10)
|
||||
|
||||
grp_obstacles.add("inflation_dist", double_t, 0,
|
||||
"Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)",
|
||||
0.6, 0, 15)
|
||||
|
||||
grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0,
|
||||
"Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)",
|
||||
0.6, 0, 15)
|
||||
|
||||
grp_obstacles.add("include_dynamic_obstacles", bool_t, 0,
|
||||
"Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.",
|
||||
False)
|
||||
|
||||
grp_obstacles.add("include_costmap_obstacles", bool_t, 0,
|
||||
"Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)",
|
||||
True)
|
||||
|
||||
grp_obstacles.add("legacy_obstacle_association", bool_t, 0,
|
||||
"If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only 'relevant' obstacles).",
|
||||
False)
|
||||
|
||||
grp_obstacles.add("obstacle_association_force_inclusion_factor", double_t, 0,
|
||||
"The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.",
|
||||
1.5, 0.0, 100.0)
|
||||
|
||||
grp_obstacles.add("obstacle_association_cutoff_factor", double_t, 0,
|
||||
"See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.",
|
||||
5.0, 1.0, 100.0)
|
||||
|
||||
grp_obstacles.add("costmap_obstacles_behind_robot_dist", double_t, 0,
|
||||
"Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)",
|
||||
1.5, 0.0, 20.0)
|
||||
|
||||
grp_obstacles.add("obstacle_poses_affected", int_t, 0,
|
||||
"The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well",
|
||||
30, 0, 200)
|
||||
|
||||
# Obstacle - Velocity ratio parameters
|
||||
grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles")
|
||||
|
||||
grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0,
|
||||
"Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles",
|
||||
1, 0, 1)
|
||||
|
||||
grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0,
|
||||
"Distance to a static obstacle for which the velocity should be lower",
|
||||
0, 0, 10)
|
||||
|
||||
grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0,
|
||||
"Distance to a static obstacle for which the velocity should be higher",
|
||||
0.5, 0, 10)
|
||||
|
||||
# Optimization
|
||||
grp_optimization = gen.add_group("Optimization", type="tab")
|
||||
|
||||
grp_optimization.add("no_inner_iterations", int_t, 0,
|
||||
"Number of solver iterations called in each outerloop iteration",
|
||||
5, 1, 100)
|
||||
|
||||
grp_optimization.add("no_outer_iterations", int_t, 0,
|
||||
"Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations",
|
||||
4, 1, 100)
|
||||
|
||||
grp_optimization.add("optimization_activate", bool_t, 0,
|
||||
"Activate the optimization",
|
||||
True)
|
||||
|
||||
grp_optimization.add("optimization_verbose", bool_t, 0,
|
||||
"Print verbose information",
|
||||
False)
|
||||
|
||||
grp_optimization.add("penalty_epsilon", double_t, 0,
|
||||
"Add a small safty margin to penalty functions for hard-constraint approximations",
|
||||
0.05, 0, 1.0)
|
||||
|
||||
grp_optimization.add("weight_max_vel_x", double_t, 0,
|
||||
"Optimization weight for satisfying the maximum allowed translational velocity",
|
||||
2, 0, 1000)
|
||||
|
||||
grp_optimization.add("weight_max_vel_y", double_t, 0,
|
||||
"Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)",
|
||||
2, 0, 1000)
|
||||
|
||||
grp_optimization.add("weight_max_vel_theta", double_t, 0,
|
||||
"Optimization weight for satisfying the maximum allowed angular velocity",
|
||||
1, 0, 1000)
|
||||
|
||||
grp_optimization.add("weight_acc_lim_x", double_t, 0,
|
||||
"Optimization weight for satisfying the maximum allowed translational acceleration",
|
||||
1, 0, 1000)
|
||||
|
||||
grp_optimization.add("weight_acc_lim_y", double_t, 0,
|
||||
"Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)",
|
||||
1, 0, 1000)
|
||||
|
||||
grp_optimization.add("weight_acc_lim_theta", double_t, 0,
|
||||
"Optimization weight for satisfying the maximum allowed angular acceleration",
|
||||
1, 0, 1000)
|
||||
|
||||
grp_optimization.add("weight_kinematics_nh", double_t, 0,
|
||||
"Optimization weight for satisfying the non-holonomic kinematics",
|
||||
1000 , 0, 10000)
|
||||
|
||||
grp_optimization.add("weight_kinematics_forward_drive", double_t, 0,
|
||||
"Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)",
|
||||
1, 0, 10000)
|
||||
|
||||
grp_optimization.add("weight_kinematics_turning_radius", double_t, 0,
|
||||
"Optimization weight for enforcing a minimum turning radius (carlike robots)",
|
||||
1, 0, 1000)
|
||||
|
||||
grp_optimization.add("weight_optimaltime", double_t, 0,
|
||||
"Optimization weight for contracting the trajectory w.r.t. transition time",
|
||||
1, 0, 1000)
|
||||
|
||||
grp_optimization.add("weight_shortest_path", double_t, 0,
|
||||
"Optimization weight for contracting the trajectory w.r.t. path length",
|
||||
0, 0, 100)
|
||||
|
||||
grp_optimization.add("weight_obstacle", double_t, 0,
|
||||
"Optimization weight for satisfying a minimum seperation from obstacles",
|
||||
50, 0, 1000)
|
||||
|
||||
grp_optimization.add("weight_inflation", double_t, 0,
|
||||
"Optimization weight for the inflation penalty (should be small)",
|
||||
0.1, 0, 10)
|
||||
|
||||
grp_optimization.add("weight_dynamic_obstacle", double_t, 0,
|
||||
"Optimization weight for satisfying a minimum seperation from dynamic obstacles",
|
||||
50, 0, 1000)
|
||||
|
||||
grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0,
|
||||
"Optimization weight for the inflation penalty of dynamic obstacles (should be small)",
|
||||
0.1, 0, 10)
|
||||
|
||||
grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0,
|
||||
"Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle",
|
||||
0, 0, 1000)
|
||||
|
||||
grp_optimization.add("weight_viapoint", double_t, 0,
|
||||
"Optimization weight for minimizing the distance to via-points",
|
||||
1, 0, 1000)
|
||||
|
||||
grp_optimization.add("weight_adapt_factor", double_t, 0,
|
||||
"Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.",
|
||||
2, 1, 100)
|
||||
|
||||
grp_optimization.add("obstacle_cost_exponent", double_t, 0,
|
||||
"Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)",
|
||||
1, 0.01, 100)
|
||||
|
||||
|
||||
|
||||
# Homotopy Class Planner
|
||||
grp_hcp = gen.add_group("HCPlanning", type="tab")
|
||||
|
||||
grp_hcp.add("enable_multithreading", bool_t, 0,
|
||||
"Activate multiple threading for planning multiple trajectories in parallel",
|
||||
True)
|
||||
|
||||
grp_hcp.add("max_number_classes", int_t, 0,
|
||||
"Specify the maximum number of allowed alternative homotopy classes (limits computational effort)",
|
||||
5, 1, 100)
|
||||
|
||||
grp_hcp.add("max_number_plans_in_current_class", int_t, 0,
|
||||
"Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes",
|
||||
1, 1, 10)
|
||||
|
||||
grp_hcp.add("selection_cost_hysteresis", double_t, 0,
|
||||
"Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor)",
|
||||
1.0, 0, 2)
|
||||
|
||||
|
||||
grp_hcp.add("selection_prefer_initial_plan", double_t, 0,
|
||||
"Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)",
|
||||
0.95, 0, 1)
|
||||
|
||||
grp_hcp.add("selection_obst_cost_scale", double_t, 0,
|
||||
"Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)",
|
||||
2.0, 0, 1000)
|
||||
|
||||
grp_hcp.add("selection_viapoint_cost_scale", double_t, 0,
|
||||
"Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)",
|
||||
1.0, 0, 100)
|
||||
|
||||
grp_hcp.add("selection_alternative_time_cost", bool_t, 0,
|
||||
"If true, time cost is replaced by the total transition time.",
|
||||
False)
|
||||
|
||||
grp_hcp.add("selection_dropping_probability", double_t, 0,
|
||||
"At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.",
|
||||
0.0, 0.0, 1.0)
|
||||
|
||||
grp_hcp.add("switching_blocking_period", double_t, 0,
|
||||
"Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed",
|
||||
0.0, 0.0, 60)
|
||||
|
||||
grp_hcp.add("roadmap_graph_no_samples", int_t, 0,
|
||||
"Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off",
|
||||
15, 1, 100)
|
||||
|
||||
grp_hcp.add("roadmap_graph_area_width", double_t, 0,
|
||||
"Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)",
|
||||
5, 0.1, 20)
|
||||
|
||||
grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0,
|
||||
"The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!)",
|
||||
1.0, 0.5, 2)
|
||||
|
||||
grp_hcp.add("h_signature_prescaler", double_t, 0,
|
||||
"Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<H<=1)",
|
||||
1, 0.2, 1)
|
||||
|
||||
grp_hcp.add("h_signature_threshold", double_t, 0,
|
||||
"Two h-signuteres are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold",
|
||||
0.1, 0, 1)
|
||||
|
||||
grp_hcp.add("obstacle_heading_threshold", double_t, 0,
|
||||
"Specify the value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration)",
|
||||
0.45, 0, 1)
|
||||
|
||||
grp_hcp.add("viapoints_all_candidates", bool_t, 0,
|
||||
"If true, all trajectories of different topologies are attached to the set of via-points, otherwise only the trajectory sharing the same one as the initial/global plan is attached (no effect in test_optim_node).",
|
||||
True)
|
||||
|
||||
grp_hcp.add("visualize_hc_graph", bool_t, 0,
|
||||
"Visualize the graph that is created for exploring new homotopy classes",
|
||||
False)
|
||||
|
||||
|
||||
# Recovery
|
||||
grp_recovery = gen.add_group("Recovery", type="tab")
|
||||
|
||||
grp_recovery.add("shrink_horizon_backup", bool_t, 0,
|
||||
"Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues.",
|
||||
True)
|
||||
|
||||
grp_recovery.add("oscillation_recovery", bool_t, 0,
|
||||
"Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards).",
|
||||
True)
|
||||
|
||||
grp_recovery_divergence = grp_recovery.add_group("Divergence Detection", type="hide")
|
||||
|
||||
grp_recovery_divergence.add(
|
||||
"divergence_detection_enable",
|
||||
bool_t,
|
||||
0,
|
||||
"True to enable divergence detection.",
|
||||
False
|
||||
)
|
||||
|
||||
grp_recovery_divergence.add(
|
||||
"divergence_detection_max_chi_squared",
|
||||
double_t,
|
||||
0,
|
||||
"Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged.",
|
||||
10,
|
||||
0,
|
||||
100
|
||||
)
|
||||
|
||||
exit(gen.generate("teb_local_planner", "teb_local_planner", "TebLocalPlannerReconfigure"))
|
||||
@ -1,183 +0,0 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 81
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 562
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.588679016
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.0299999993
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 0.100000001
|
||||
Cell Size: 0.100000001
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.0299999993
|
||||
Value: Lines
|
||||
Name: Fine Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 100
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 25; 255; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.300000012
|
||||
Head Length: 0.200000003
|
||||
Length: 0.300000012
|
||||
Line Style: Lines
|
||||
Line Width: 0.0299999993
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Style: None
|
||||
Radius: 0.0299999993
|
||||
Shaft Diameter: 0.100000001
|
||||
Shaft Length: 0.100000001
|
||||
Topic: /test_optim_node/local_plan
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Arrow Length: 0.300000012
|
||||
Axes Length: 0.300000012
|
||||
Axes Radius: 0.00999999978
|
||||
Class: rviz/PoseArray
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.0700000003
|
||||
Head Radius: 0.0299999993
|
||||
Name: PoseArray
|
||||
Shaft Length: 0.230000004
|
||||
Shaft Radius: 0.00999999978
|
||||
Shape: Arrow (Flat)
|
||||
Topic: /test_optim_node/teb_poses
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: /test_optim_node/teb_markers
|
||||
Name: Marker
|
||||
Namespaces:
|
||||
PointObstacles: true
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
- Class: rviz/InteractiveMarkers
|
||||
Enable Transparency: true
|
||||
Enabled: true
|
||||
Name: InteractiveMarkers
|
||||
Show Axes: false
|
||||
Show Descriptions: true
|
||||
Show Visual Aids: false
|
||||
Update Topic: /marker_obstacles/update
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: odom
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Topic: /initialpose
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 7.77247
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.0599999987
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.0500000007
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.00999999978
|
||||
Pitch: 1.56979632
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 4.71043873
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003da000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1354
|
||||
X: 137
|
||||
Y: 50
|
||||
@ -1,97 +0,0 @@
|
||||
# Locate the g2o libraries
|
||||
# A general framework for graph optimization.
|
||||
#
|
||||
# This module defines
|
||||
# G2O_FOUND, if false, do not try to link against g2o
|
||||
# G2O_LIBRARIES, path to the libg2o
|
||||
# G2O_INCLUDE_DIR, where to find the g2o header files
|
||||
#
|
||||
# Niko Suenderhauf <niko@etit.tu-chemnitz.de>
|
||||
# Adapted by Felix Endres <endres@informatik.uni-freiburg.de>
|
||||
|
||||
IF(UNIX)
|
||||
|
||||
#IF(G2O_INCLUDE_DIR AND G2O_LIBRARIES)
|
||||
# in cache already
|
||||
# SET(G2O_FIND_QUIETLY TRUE)
|
||||
#ENDIF(G2O_INCLUDE_DIR AND G2O_LIBRARIES)
|
||||
|
||||
MESSAGE(STATUS "Searching for g2o ...")
|
||||
FIND_PATH(G2O_INCLUDE_DIR
|
||||
NAMES core math_groups types
|
||||
PATHS /usr/local /usr
|
||||
PATH_SUFFIXES include/g2o include)
|
||||
|
||||
IF (G2O_INCLUDE_DIR)
|
||||
MESSAGE(STATUS "Found g2o headers in: ${G2O_INCLUDE_DIR}")
|
||||
ENDIF (G2O_INCLUDE_DIR)
|
||||
|
||||
FIND_LIBRARY(G2O_CORE_LIB
|
||||
NAMES g2o_core g2o_core_rd
|
||||
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
|
||||
PATH_SUFFIXES lib)
|
||||
FIND_LIBRARY(G2O_STUFF_LIB
|
||||
NAMES g2o_stuff g2o_stuff_rd
|
||||
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
|
||||
PATH_SUFFIXES lib)
|
||||
FIND_LIBRARY(G2O_TYPES_SLAM2D_LIB
|
||||
NAMES g2o_types_slam2d g2o_types_slam2d_rd
|
||||
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
|
||||
PATH_SUFFIXES lib)
|
||||
FIND_LIBRARY(G2O_TYPES_SLAM3D_LIB
|
||||
NAMES g2o_types_slam3d g2o_types_slam3d_rd
|
||||
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
|
||||
PATH_SUFFIXES lib)
|
||||
FIND_LIBRARY(G2O_SOLVER_CHOLMOD_LIB
|
||||
NAMES g2o_solver_cholmod g2o_solver_cholmod_rd
|
||||
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
|
||||
PATH_SUFFIXES lib)
|
||||
FIND_LIBRARY(G2O_SOLVER_PCG_LIB
|
||||
NAMES g2o_solver_pcg g2o_solver_pcg_rd
|
||||
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
|
||||
PATH_SUFFIXES lib)
|
||||
FIND_LIBRARY(G2O_SOLVER_CSPARSE_LIB
|
||||
NAMES g2o_solver_csparse g2o_solver_csparse_rd
|
||||
PATHS /usr/local /usr
|
||||
PATH_SUFFIXES lib)
|
||||
FIND_LIBRARY(G2O_INCREMENTAL_LIB
|
||||
NAMES g2o_incremental g2o_incremental_rd
|
||||
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
|
||||
PATH_SUFFIXES lib)
|
||||
FIND_LIBRARY(G2O_CSPARSE_EXTENSION_LIB
|
||||
NAMES g2o_csparse_extension g2o_csparse_extension_rd
|
||||
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
|
||||
PATH_SUFFIXES lib)
|
||||
|
||||
SET(G2O_LIBRARIES ${G2O_CSPARSE_EXTENSION_LIB}
|
||||
${G2O_CORE_LIB}
|
||||
${G2O_STUFF_LIB}
|
||||
${G2O_TYPES_SLAM2D_LIB}
|
||||
${G2O_TYPES_SLAM3D_LIB}
|
||||
${G2O_SOLVER_CHOLMOD_LIB}
|
||||
${G2O_SOLVER_PCG_LIB}
|
||||
${G2O_SOLVER_CSPARSE_LIB}
|
||||
${G2O_INCREMENTAL_LIB}
|
||||
)
|
||||
|
||||
IF(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
|
||||
SET(G2O_FOUND "YES")
|
||||
IF(NOT G2O_FIND_QUIETLY)
|
||||
MESSAGE(STATUS "Found libg2o: ${G2O_LIBRARIES}")
|
||||
ENDIF(NOT G2O_FIND_QUIETLY)
|
||||
ELSE(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
|
||||
IF(NOT G2O_LIBRARIES)
|
||||
IF(G2O_FIND_REQUIRED)
|
||||
message(FATAL_ERROR "Could not find libg2o!")
|
||||
ENDIF(G2O_FIND_REQUIRED)
|
||||
ENDIF(NOT G2O_LIBRARIES)
|
||||
|
||||
IF(NOT G2O_INCLUDE_DIR)
|
||||
IF(G2O_FIND_REQUIRED)
|
||||
message(FATAL_ERROR "Could not find g2o include directory!")
|
||||
ENDIF(G2O_FIND_REQUIRED)
|
||||
ENDIF(NOT G2O_INCLUDE_DIR)
|
||||
ENDIF(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
|
||||
|
||||
ENDIF(UNIX)
|
||||
|
||||
@ -1,133 +0,0 @@
|
||||
# - Try to find SUITESPARSE
|
||||
# Once done this will define
|
||||
#
|
||||
# SUITESPARSE_FOUND - system has SUITESPARSE
|
||||
# SUITESPARSE_INCLUDE_DIRS - the SUITESPARSE include directory
|
||||
# SUITESPARSE_LIBRARIES - Link these to use SUITESPARSE
|
||||
# SUITESPARSE_SPQR_LIBRARY - name of spqr library (necessary due to error in debian package)
|
||||
# SUITESPARSE_SPQR_LIBRARY_DIR - name of spqr library (necessary due to error in debian package)
|
||||
# SUITESPARSE_LIBRARY_DIR - Library main directory containing suitesparse libs
|
||||
# SUITESPARSE_LIBRARY_DIRS - all Library directories containing suitesparse libs
|
||||
# SUITESPARSE_SPQR_VALID - automatic identification whether or not spqr package is installed correctly
|
||||
|
||||
IF (SUITESPARSE_INCLUDE_DIRS)
|
||||
# Already in cache, be silent
|
||||
SET(SUITESPARSE_FIND_QUIETLY TRUE)
|
||||
ENDIF (SUITESPARSE_INCLUDE_DIRS)
|
||||
|
||||
if( WIN32 )
|
||||
# Find cholmod part of the suitesparse library collection
|
||||
|
||||
FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
|
||||
PATHS "C:\\libs\\win32\\SuiteSparse\\Include" )
|
||||
|
||||
# Add cholmod include directory to collection include directories
|
||||
IF ( CHOLMOD_INCLUDE_DIR )
|
||||
list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} )
|
||||
ENDIF( CHOLMOD_INCLUDE_DIR )
|
||||
|
||||
|
||||
# find path suitesparse library
|
||||
FIND_PATH( SUITESPARSE_LIBRARY_DIRS
|
||||
amd.lib
|
||||
PATHS "C:\\libs\\win32\\SuiteSparse\\libs" )
|
||||
|
||||
# if we found the library, add it to the defined libraries
|
||||
IF ( SUITESPARSE_LIBRARY_DIRS )
|
||||
list ( APPEND SUITESPARSE_LIBRARIES optimized;amd;optimized;camd;optimized;ccolamd;optimized;cholmod;optimized;colamd;optimized;metis;optimized;spqr;optimized;umfpack;debug;amdd;debug;camdd;debug;ccolamdd;debug;cholmodd;debug;spqrd;debug;umfpackd;debug;colamdd;debug;metisd;optimized;blas;optimized;libf2c;optimized;lapack;debug;blasd;debug;libf2cd;debug;lapackd )
|
||||
ENDIF( SUITESPARSE_LIBRARY_DIRS )
|
||||
|
||||
else( WIN32 )
|
||||
IF(APPLE)
|
||||
FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
|
||||
PATHS /opt/local/include/ufsparse
|
||||
/usr/local/include )
|
||||
|
||||
FIND_PATH( SUITESPARSE_LIBRARY_DIR
|
||||
NAMES libcholmod.a
|
||||
PATHS /opt/local/lib
|
||||
/usr/local/lib )
|
||||
ELSE(APPLE)
|
||||
FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
|
||||
PATHS /usr/local/include
|
||||
/usr/include
|
||||
/usr/include/suitesparse/
|
||||
${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod
|
||||
PATH_SUFFIXES cholmod/ CHOLMOD/ )
|
||||
|
||||
FIND_PATH( SUITESPARSE_LIBRARY_DIR
|
||||
NAMES libcholmod.so libcholmod.a
|
||||
PATHS /usr/lib
|
||||
/usr/lib64
|
||||
/usr/lib/x86_64-linux-gnu
|
||||
/usr/lib/i386-linux-gnu
|
||||
/usr/local/lib
|
||||
/usr/lib/arm-linux-gnueabihf/
|
||||
/usr/lib/aarch64-linux-gnu/
|
||||
/usr/lib/arm-linux-gnueabi/
|
||||
/usr/lib/arm-linux-gnu)
|
||||
ENDIF(APPLE)
|
||||
|
||||
# Add cholmod include directory to collection include directories
|
||||
IF ( CHOLMOD_INCLUDE_DIR )
|
||||
list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} )
|
||||
ENDIF( CHOLMOD_INCLUDE_DIR )
|
||||
|
||||
# if we found the library, add it to the defined libraries
|
||||
IF ( SUITESPARSE_LIBRARY_DIR )
|
||||
|
||||
list ( APPEND SUITESPARSE_LIBRARIES amd)
|
||||
list ( APPEND SUITESPARSE_LIBRARIES btf)
|
||||
list ( APPEND SUITESPARSE_LIBRARIES camd)
|
||||
list ( APPEND SUITESPARSE_LIBRARIES ccolamd)
|
||||
list ( APPEND SUITESPARSE_LIBRARIES cholmod)
|
||||
list ( APPEND SUITESPARSE_LIBRARIES colamd)
|
||||
# list ( APPEND SUITESPARSE_LIBRARIES csparse)
|
||||
list ( APPEND SUITESPARSE_LIBRARIES cxsparse)
|
||||
list ( APPEND SUITESPARSE_LIBRARIES klu)
|
||||
# list ( APPEND SUITESPARSE_LIBRARIES spqr)
|
||||
list ( APPEND SUITESPARSE_LIBRARIES umfpack)
|
||||
|
||||
IF (APPLE)
|
||||
list ( APPEND SUITESPARSE_LIBRARIES suitesparseconfig)
|
||||
ENDIF (APPLE)
|
||||
|
||||
# Metis and spqr are optional
|
||||
FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY
|
||||
NAMES metis
|
||||
PATHS ${SUITESPARSE_LIBRARY_DIR} )
|
||||
IF (SUITESPARSE_METIS_LIBRARY)
|
||||
list ( APPEND SUITESPARSE_LIBRARIES metis)
|
||||
ENDIF(SUITESPARSE_METIS_LIBRARY)
|
||||
|
||||
if(EXISTS "${CHOLMOD_INCLUDE_DIR}/SuiteSparseQR.hpp")
|
||||
SET(SUITESPARSE_SPQR_VALID TRUE CACHE BOOL "SuiteSparseSPQR valid")
|
||||
else()
|
||||
SET(SUITESPARSE_SPQR_VALID false CACHE BOOL "SuiteSparseSPQR valid")
|
||||
endif()
|
||||
|
||||
if(SUITESPARSE_SPQR_VALID)
|
||||
FIND_LIBRARY( SUITESPARSE_SPQR_LIBRARY
|
||||
NAMES spqr
|
||||
PATHS ${SUITESPARSE_LIBRARY_DIR} )
|
||||
IF (SUITESPARSE_SPQR_LIBRARY)
|
||||
list ( APPEND SUITESPARSE_LIBRARIES spqr)
|
||||
ENDIF (SUITESPARSE_SPQR_LIBRARY)
|
||||
endif()
|
||||
|
||||
ENDIF( SUITESPARSE_LIBRARY_DIR )
|
||||
|
||||
endif( WIN32 )
|
||||
|
||||
|
||||
IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
|
||||
IF(WIN32)
|
||||
list (APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR}/../../UFconfig )
|
||||
ENDIF(WIN32)
|
||||
SET(SUITESPARSE_FOUND TRUE)
|
||||
MESSAGE(STATUS "Found SuiteSparse")
|
||||
ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
|
||||
SET( SUITESPARSE_FOUND FALSE )
|
||||
MESSAGE(FATAL_ERROR "Unable to find SuiteSparse")
|
||||
ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
|
||||
|
||||
@ -1,464 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef DISTANCE_CALCULATIONS_H
|
||||
#define DISTANCE_CALCULATIONS_H
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include "teb_local_planner/misc.h"
|
||||
|
||||
|
||||
namespace teb_local_planner
|
||||
{
|
||||
|
||||
//! Abbrev. for a container storing 2d points
|
||||
typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > Point2dContainer;
|
||||
|
||||
|
||||
/**
|
||||
* @brief Helper function to obtain the closest point on a line segment w.r.t. a reference point
|
||||
* @param point 2D point
|
||||
* @param line_start 2D point representing the start of the line segment
|
||||
* @param line_end 2D point representing the end of the line segment
|
||||
* @return Closest point on the line segment
|
||||
*/
|
||||
inline Eigen::Vector2d closest_point_on_line_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& point, const Eigen::Ref<const Eigen::Vector2d>& line_start, const Eigen::Ref<const Eigen::Vector2d>& line_end)
|
||||
{
|
||||
Eigen::Vector2d diff = line_end - line_start;
|
||||
double sq_norm = diff.squaredNorm();
|
||||
|
||||
if (sq_norm == 0)
|
||||
return line_start;
|
||||
|
||||
double u = ((point.x() - line_start.x()) * diff.x() + (point.y() - line_start.y())*diff.y()) / sq_norm;
|
||||
|
||||
if (u <= 0) return line_start;
|
||||
else if (u >= 1) return line_end;
|
||||
|
||||
return line_start + u*diff;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Helper function to calculate the distance between a line segment and a point
|
||||
* @param point 2D point
|
||||
* @param line_start 2D point representing the start of the line segment
|
||||
* @param line_end 2D point representing the end of the line segment
|
||||
* @return minimum distance to a given line segment
|
||||
*/
|
||||
inline double distance_point_to_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& point, const Eigen::Ref<const Eigen::Vector2d>& line_start, const Eigen::Ref<const Eigen::Vector2d>& line_end)
|
||||
{
|
||||
return (point - closest_point_on_line_segment_2d(point, line_start, line_end)).norm();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Helper function to check whether two line segments intersects
|
||||
* @param line1_start 2D point representing the start of the first line segment
|
||||
* @param line1_end 2D point representing the end of the first line segment
|
||||
* @param line2_start 2D point representing the start of the second line segment
|
||||
* @param line2_end 2D point representing the end of the second line segment
|
||||
* @param[out] intersection [optional] Write intersection point to destination (the value is only written, if both lines intersect, e.g. if the function returns \c true)
|
||||
* @return \c true if both line segments intersect
|
||||
*/
|
||||
inline bool check_line_segments_intersection_2d(const Eigen::Ref<const Eigen::Vector2d>& line1_start, const Eigen::Ref<const Eigen::Vector2d>& line1_end,
|
||||
const Eigen::Ref<const Eigen::Vector2d>& line2_start, const Eigen::Ref<const Eigen::Vector2d>& line2_end, Eigen::Vector2d* intersection = NULL)
|
||||
{
|
||||
// http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
|
||||
double s_numer, t_numer, denom, t;
|
||||
Eigen::Vector2d line1 = line1_end - line1_start;
|
||||
Eigen::Vector2d line2 = line2_end - line2_start;
|
||||
|
||||
denom = line1.x() * line2.y() - line2.x() * line1.y();
|
||||
if (denom == 0) return false; // Collinear
|
||||
bool denomPositive = denom > 0;
|
||||
|
||||
Eigen::Vector2d aux = line1_start - line2_start;
|
||||
|
||||
s_numer = line1.x() * aux.y() - line1.y() * aux.x();
|
||||
if ((s_numer < 0) == denomPositive) return false; // No collision
|
||||
|
||||
t_numer = line2.x() * aux.y() - line2.y() * aux.x();
|
||||
if ((t_numer < 0) == denomPositive) return false; // No collision
|
||||
|
||||
if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive)) return false; // No collision
|
||||
|
||||
// Otherwise collision detected
|
||||
t = t_numer / denom;
|
||||
if (intersection)
|
||||
{
|
||||
*intersection = line1_start + t * line1;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Helper function to calculate the smallest distance between two line segments
|
||||
* @param line1_start 2D point representing the start of the first line segment
|
||||
* @param line1_end 2D point representing the end of the first line segment
|
||||
* @param line2_start 2D point representing the start of the second line segment
|
||||
* @param line2_end 2D point representing the end of the second line segment
|
||||
* @return smallest distance between both segments
|
||||
*/
|
||||
inline double distance_segment_to_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& line1_start, const Eigen::Ref<const Eigen::Vector2d>& line1_end,
|
||||
const Eigen::Ref<const Eigen::Vector2d>& line2_start, const Eigen::Ref<const Eigen::Vector2d>& line2_end)
|
||||
{
|
||||
// TODO more efficient implementation
|
||||
|
||||
// check if segments intersect
|
||||
if (check_line_segments_intersection_2d(line1_start, line1_end, line2_start, line2_end))
|
||||
return 0;
|
||||
|
||||
// check all 4 combinations
|
||||
std::array<double,4> distances;
|
||||
|
||||
distances[0] = distance_point_to_segment_2d(line1_start, line2_start, line2_end);
|
||||
distances[1] = distance_point_to_segment_2d(line1_end, line2_start, line2_end);
|
||||
distances[2] = distance_point_to_segment_2d(line2_start, line1_start, line1_end);
|
||||
distances[3] = distance_point_to_segment_2d(line2_end, line1_start, line1_end);
|
||||
|
||||
return *std::min_element(distances.begin(), distances.end());
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Helper function to calculate the smallest distance between a point and a closed polygon
|
||||
* @param point 2D point
|
||||
* @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end)
|
||||
* @return smallest distance between point and polygon
|
||||
*/
|
||||
inline double distance_point_to_polygon_2d(const Eigen::Vector2d& point, const Point2dContainer& vertices)
|
||||
{
|
||||
double dist = HUGE_VAL;
|
||||
|
||||
// the polygon is a point
|
||||
if (vertices.size() == 1)
|
||||
{
|
||||
return (point - vertices.front()).norm();
|
||||
}
|
||||
|
||||
// check each polygon edge
|
||||
for (int i=0; i<(int)vertices.size()-1; ++i)
|
||||
{
|
||||
double new_dist = distance_point_to_segment_2d(point, vertices.at(i), vertices.at(i+1));
|
||||
// double new_dist = calc_distance_point_to_segment( position, vertices.at(i), vertices.at(i+1));
|
||||
if (new_dist < dist)
|
||||
dist = new_dist;
|
||||
}
|
||||
|
||||
if (vertices.size()>2) // if not a line close polygon
|
||||
{
|
||||
double new_dist = distance_point_to_segment_2d(point, vertices.back(), vertices.front()); // check last edge
|
||||
if (new_dist < dist)
|
||||
return new_dist;
|
||||
}
|
||||
|
||||
return dist;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Helper function to calculate the smallest distance between a line segment and a closed polygon
|
||||
* @param line_start 2D point representing the start of the line segment
|
||||
* @param line_end 2D point representing the end of the line segment
|
||||
* @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end)
|
||||
* @return smallest distance between point and polygon
|
||||
*/
|
||||
inline double distance_segment_to_polygon_2d(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, const Point2dContainer& vertices)
|
||||
{
|
||||
double dist = HUGE_VAL;
|
||||
|
||||
// the polygon is a point
|
||||
if (vertices.size() == 1)
|
||||
{
|
||||
return distance_point_to_segment_2d(vertices.front(), line_start, line_end);
|
||||
}
|
||||
|
||||
// check each polygon edge
|
||||
for (int i=0; i<(int)vertices.size()-1; ++i)
|
||||
{
|
||||
double new_dist = distance_segment_to_segment_2d(line_start, line_end, vertices.at(i), vertices.at(i+1));
|
||||
// double new_dist = calc_distance_point_to_segment( position, vertices.at(i), vertices.at(i+1));
|
||||
if (new_dist < dist)
|
||||
dist = new_dist;
|
||||
}
|
||||
|
||||
if (vertices.size()>2) // if not a line close polygon
|
||||
{
|
||||
double new_dist = distance_segment_to_segment_2d(line_start, line_end, vertices.back(), vertices.front()); // check last edge
|
||||
if (new_dist < dist)
|
||||
return new_dist;
|
||||
}
|
||||
|
||||
return dist;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Helper function to calculate the smallest distance between two closed polygons
|
||||
* @param vertices1 Vertices describing the first closed polygon (the first vertex is not repeated at the end)
|
||||
* @param vertices2 Vertices describing the second closed polygon (the first vertex is not repeated at the end)
|
||||
* @return smallest distance between point and polygon
|
||||
*/
|
||||
inline double distance_polygon_to_polygon_2d(const Point2dContainer& vertices1, const Point2dContainer& vertices2)
|
||||
{
|
||||
double dist = HUGE_VAL;
|
||||
|
||||
// the polygon1 is a point
|
||||
if (vertices1.size() == 1)
|
||||
{
|
||||
return distance_point_to_polygon_2d(vertices1.front(), vertices2);
|
||||
}
|
||||
|
||||
// check each edge of polygon1
|
||||
for (int i=0; i<(int)vertices1.size()-1; ++i)
|
||||
{
|
||||
double new_dist = distance_segment_to_polygon_2d(vertices1[i], vertices1[i+1], vertices2);
|
||||
if (new_dist < dist)
|
||||
dist = new_dist;
|
||||
}
|
||||
|
||||
if (vertices1.size()>2) // if not a line close polygon1
|
||||
{
|
||||
double new_dist = distance_segment_to_polygon_2d(vertices1.back(), vertices1.front(), vertices2); // check last edge
|
||||
if (new_dist < dist)
|
||||
return new_dist;
|
||||
}
|
||||
|
||||
return dist;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// Further distance calculations:
|
||||
|
||||
|
||||
// The Distance Calculations are mainly copied from http://geomalgorithms.com/a07-_distance.html
|
||||
// Copyright 2001 softSurfer, 2012 Dan Sunday
|
||||
// This code may be freely used and modified for any purpose
|
||||
// providing that this copyright notice is included with it.
|
||||
// SoftSurfer makes no warranty for this code, and cannot be held
|
||||
// liable for any real or imagined damage resulting from its use.
|
||||
// Users of this code must verify correctness for their application.
|
||||
|
||||
inline double calc_distance_line_to_line_3d(const Eigen::Ref<const Eigen::Vector3d>& x1, Eigen::Ref<const Eigen::Vector3d>& u,
|
||||
const Eigen::Ref<const Eigen::Vector3d>& x2, Eigen::Ref<const Eigen::Vector3d>& v)
|
||||
{
|
||||
Eigen::Vector3d w = x2 - x1;
|
||||
double a = u.squaredNorm(); // dot(u,u) always >= 0
|
||||
double b = u.dot(v);
|
||||
double c = v.squaredNorm(); // dot(v,v) always >= 0
|
||||
double d = u.dot(w);
|
||||
double e = v.dot(w);
|
||||
double D = a*c - b*b; // always >= 0
|
||||
double sc, tc;
|
||||
|
||||
// compute the line parameters of the two closest points
|
||||
if (D < SMALL_NUM) { // the lines are almost parallel
|
||||
sc = 0.0;
|
||||
tc = (b>c ? d/b : e/c); // use the largest denominator
|
||||
}
|
||||
else {
|
||||
sc = (b*e - c*d) / D;
|
||||
tc = (a*e - b*d) / D;
|
||||
}
|
||||
|
||||
// get the difference of the two closest points
|
||||
Eigen::Vector3d dP = w + (sc * u) - (tc * v); // = L1(sc) - L2(tc)
|
||||
|
||||
return dP.norm(); // return the closest distance
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
inline double calc_distance_segment_to_segment3D(const Eigen::Ref<const Eigen::Vector3d>& line1_start, Eigen::Ref<const Eigen::Vector3d>& line1_end,
|
||||
const Eigen::Ref<const Eigen::Vector3d>& line2_start, Eigen::Ref<const Eigen::Vector3d>& line2_end)
|
||||
{
|
||||
Eigen::Vector3d u = line1_end - line1_start;
|
||||
Eigen::Vector3d v = line2_end - line2_start;
|
||||
Eigen::Vector3d w = line2_start - line1_start;
|
||||
double a = u.squaredNorm(); // dot(u,u) always >= 0
|
||||
double b = u.dot(v);
|
||||
double c = v.squaredNorm(); // dot(v,v) always >= 0
|
||||
double d = u.dot(w);
|
||||
double e = v.dot(w);
|
||||
double D = a*c - b*b; // always >= 0
|
||||
double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
|
||||
double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
|
||||
|
||||
// compute the line parameters of the two closest points
|
||||
if (D < SMALL_NUM)
|
||||
{ // the lines are almost parallel
|
||||
sN = 0.0; // force using point P0 on segment S1
|
||||
sD = 1.0; // to prevent possible division by 0.0 later
|
||||
tN = e;
|
||||
tD = c;
|
||||
}
|
||||
else
|
||||
{ // get the closest points on the infinite lines
|
||||
sN = (b*e - c*d);
|
||||
tN = (a*e - b*d);
|
||||
if (sN < 0.0)
|
||||
{ // sc < 0 => the s=0 edge is visible
|
||||
sN = 0.0;
|
||||
tN = e;
|
||||
tD = c;
|
||||
}
|
||||
else if (sN > sD)
|
||||
{ // sc > 1 => the s=1 edge is visible
|
||||
sN = sD;
|
||||
tN = e + b;
|
||||
tD = c;
|
||||
}
|
||||
}
|
||||
|
||||
if (tN < 0.0)
|
||||
{ // tc < 0 => the t=0 edge is visible
|
||||
tN = 0.0;
|
||||
// recompute sc for this edge
|
||||
if (-d < 0.0)
|
||||
sN = 0.0;
|
||||
else if (-d > a)
|
||||
sN = sD;
|
||||
else
|
||||
{
|
||||
sN = -d;
|
||||
sD = a;
|
||||
}
|
||||
}
|
||||
else if (tN > tD)
|
||||
{ // tc > 1 => the t=1 edge is visible
|
||||
tN = tD;
|
||||
// recompute sc for this edge
|
||||
if ((-d + b) < 0.0)
|
||||
sN = 0;
|
||||
else if ((-d + b) > a)
|
||||
sN = sD;
|
||||
else
|
||||
{
|
||||
sN = (-d + b);
|
||||
sD = a;
|
||||
}
|
||||
}
|
||||
// finally do the division to get sc and tc
|
||||
sc = (abs(sN) < SMALL_NUM ? 0.0 : sN / sD);
|
||||
tc = (abs(tN) < SMALL_NUM ? 0.0 : tN / tD);
|
||||
|
||||
// get the difference of the two closest points
|
||||
Eigen::Vector3d dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
|
||||
|
||||
return dP.norm(); // return the closest distance
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
template <typename VectorType>
|
||||
double calc_closest_point_to_approach_time(const VectorType& x1, const VectorType& vel1, const VectorType& x2, const VectorType& vel2)
|
||||
{
|
||||
VectorType dv = vel1 - vel2;
|
||||
|
||||
double dv2 = dv.squaredNorm(); // dot(v,v)
|
||||
if (dv2 < SMALL_NUM) // the tracks are almost parallel
|
||||
return 0.0; // any time is ok. Use time 0.
|
||||
|
||||
VectorType w0 = x1 - x2;
|
||||
double cpatime = -w0.dot(dv) / dv2;
|
||||
|
||||
return cpatime; // time of CPA
|
||||
}
|
||||
|
||||
template <typename VectorType>
|
||||
double calc_closest_point_to_approach_distance(const VectorType& x1, const VectorType& vel1, const VectorType& x2, const VectorType& vel2, double bound_cpa_time = 0)
|
||||
{
|
||||
double ctime = calc_closest_point_to_approach_time<VectorType>(x1, vel1, x2, vel2);
|
||||
if (bound_cpa_time!=0 && ctime > bound_cpa_time) ctime = bound_cpa_time;
|
||||
VectorType P1 = x1 + (ctime * vel1);
|
||||
VectorType P2 = x2 + (ctime * vel2);
|
||||
|
||||
return (P2-P1).norm(); // distance at CPA
|
||||
}
|
||||
|
||||
|
||||
|
||||
// dist_Point_to_Line(): get the distance of a point to a line
|
||||
// Input: a Point P and a Line L (in any dimension)
|
||||
// Return: the shortest distance from P to L
|
||||
template <typename VectorType>
|
||||
double calc_distance_point_to_line( const VectorType& point, const VectorType& line_base, const VectorType& line_dir)
|
||||
{
|
||||
VectorType w = point - line_base;
|
||||
|
||||
double c1 = w.dot(line_dir);
|
||||
double c2 = line_dir.dot(line_dir);
|
||||
double b = c1 / c2;
|
||||
|
||||
VectorType Pb = line_base + b * line_dir;
|
||||
return (point-Pb).norm();
|
||||
}
|
||||
//===================================================================
|
||||
|
||||
|
||||
// dist_Point_to_Segment(): get the distance of a point to a segment
|
||||
// Input: a Point P and a Segment S (in any dimension)
|
||||
// Return: the shortest distance from P to S
|
||||
template <typename VectorType>
|
||||
double calc_distance_point_to_segment( const VectorType& point, const VectorType& line_start, const VectorType& line_end)
|
||||
{
|
||||
VectorType v = line_end - line_start;
|
||||
VectorType w = point - line_start;
|
||||
|
||||
double c1 = w.dot(v);
|
||||
if ( c1 <= 0 )
|
||||
return w.norm();
|
||||
|
||||
double c2 = v.dot(v);
|
||||
if ( c2 <= c1 )
|
||||
return (point-line_end).norm();
|
||||
|
||||
double b = c1 / c2;
|
||||
VectorType Pb = line_start + b * v;
|
||||
return (point-Pb).norm();
|
||||
}
|
||||
|
||||
|
||||
|
||||
} // namespace teb_local_planner
|
||||
|
||||
#endif /* DISTANCE_CALCULATIONS_H */
|
||||
@ -1,103 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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: Christoph Rösmann
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef EQUIVALENCE_RELATIONS_H_
|
||||
#define EQUIVALENCE_RELATIONS_H_
|
||||
|
||||
#include <memory>
|
||||
|
||||
namespace teb_local_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @class EquivalenceClass
|
||||
* @brief Abstract class that defines an interface for computing and comparing equivalence classes
|
||||
*
|
||||
* Equivalence relations are utilized in order to test if two trajectories are belonging to the same
|
||||
* equivalence class w.r.t. the current obstacle configurations. A common equivalence relation is
|
||||
* the concept of homotopy classes. All trajectories belonging to the same homotopy class
|
||||
* can CONTINUOUSLY be deformed into each other without intersecting any obstacle. Hence they likely
|
||||
* share the same local minimum after invoking (local) trajectory optimization. A weaker equivalence relation
|
||||
* is defined by the concept of homology classes (e.g. refer to HSignature).
|
||||
*
|
||||
* Each EquivalenceClass object (or subclass) stores a candidate value which might be compared to another EquivalenceClass object.
|
||||
*
|
||||
* @remarks Currently, the computeEquivalenceClass method is not available in the generic interface EquivalenceClass.
|
||||
* Call the "compute"-methods directly on the subclass.
|
||||
*/
|
||||
class EquivalenceClass
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* @brief Default constructor
|
||||
*/
|
||||
EquivalenceClass() {}
|
||||
|
||||
/**
|
||||
* @brief virtual destructor
|
||||
*/
|
||||
virtual ~EquivalenceClass() {}
|
||||
|
||||
/**
|
||||
* @brief Check if two candidate classes are equivalent
|
||||
* @param other The other equivalence class to test with
|
||||
*/
|
||||
virtual bool isEqual(const EquivalenceClass& other) const = 0;
|
||||
|
||||
/**
|
||||
* @brief Check if the equivalence value is detected correctly
|
||||
* @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
|
||||
*/
|
||||
virtual bool isValid() const = 0;
|
||||
|
||||
/**
|
||||
* @brief Check if the trajectory is non-looping around an obstacle
|
||||
* @return Returns false, if the trajectory loops around an obstacle
|
||||
*/
|
||||
virtual bool isReasonable() const = 0;
|
||||
|
||||
};
|
||||
|
||||
using EquivalenceClassPtr = std::shared_ptr<EquivalenceClass>;
|
||||
|
||||
|
||||
} // namespace teb_local_planner
|
||||
|
||||
|
||||
#endif /* EQUIVALENCE_RELATIONS_H_ */
|
||||
@ -1,278 +0,0 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2016,
|
||||
* TU Dortmund - Institute of Control Theory and Systems Engineering.
|
||||
* 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 institute 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.
|
||||
*
|
||||
* Notes:
|
||||
* The following class is derived from a class defined by the
|
||||
* g2o-framework. g2o is licensed under the terms of the BSD License.
|
||||
* Refer to the base class source for detailed licensing information.
|
||||
*
|
||||
* Author: Christoph Rösmann
|
||||
*********************************************************************/
|
||||
|
||||
#ifndef _BASE_TEB_EDGES_H_
|
||||
#define _BASE_TEB_EDGES_H_
|
||||
|
||||
#include "teb_local_planner/teb_config.h"
|
||||
|
||||
#include <g2o/core/base_binary_edge.h>
|
||||
#include <g2o/core/base_unary_edge.h>
|
||||
#include <g2o/core/base_multi_edge.h>
|
||||
|
||||
namespace teb_local_planner
|
||||
{
|
||||
|
||||
|
||||
/**
|
||||
* @class BaseTebUnaryEdge
|
||||
* @brief Base edge connecting a single vertex in the TEB optimization problem
|
||||
*
|
||||
* This edge defines a base edge type for the TEB optimization problem.
|
||||
* It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
|
||||
* The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
|
||||
* Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
|
||||
* @see BaseTebMultiEdge, BaseTebBinaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
|
||||
*/
|
||||
template <int D, typename E, typename VertexXi>
|
||||
class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>
|
||||
{
|
||||
public:
|
||||
|
||||
using typename g2o::BaseUnaryEdge<D, E, VertexXi>::ErrorVector;
|
||||
using g2o::BaseUnaryEdge<D, E, VertexXi>::computeError;
|
||||
|
||||
/**
|
||||
* @brief Compute and return error / cost value.
|
||||
*
|
||||
* This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
|
||||
* @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
|
||||
*/
|
||||
ErrorVector& getError()
|
||||
{
|
||||
computeError();
|
||||
return _error;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read values from input stream
|
||||
*/
|
||||
virtual bool read(std::istream& is)
|
||||
{
|
||||
// TODO generic read
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Write values to an output stream
|
||||
*/
|
||||
virtual bool write(std::ostream& os) const
|
||||
{
|
||||
// TODO generic write
|
||||
return os.good();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Assign the TebConfig class for parameters.
|
||||
* @param cfg TebConfig class
|
||||
*/
|
||||
void setTebConfig(const TebConfig& cfg)
|
||||
{
|
||||
cfg_ = &cfg;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
using g2o::BaseUnaryEdge<D, E, VertexXi>::_error;
|
||||
using g2o::BaseUnaryEdge<D, E, VertexXi>::_vertices;
|
||||
|
||||
const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
/**
|
||||
* @class BaseTebBinaryEdge
|
||||
* @brief Base edge connecting two vertices in the TEB optimization problem
|
||||
*
|
||||
* This edge defines a base edge type for the TEB optimization problem.
|
||||
* It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
|
||||
* The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
|
||||
* Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
|
||||
* @see BaseTebMultiEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
|
||||
*/
|
||||
template <int D, typename E, typename VertexXi, typename VertexXj>
|
||||
class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
|
||||
{
|
||||
public:
|
||||
|
||||
using typename g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::ErrorVector;
|
||||
using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::computeError;
|
||||
|
||||
/**
|
||||
* @brief Compute and return error / cost value.
|
||||
*
|
||||
* This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
|
||||
* @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
|
||||
*/
|
||||
ErrorVector& getError()
|
||||
{
|
||||
computeError();
|
||||
return _error;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read values from input stream
|
||||
*/
|
||||
virtual bool read(std::istream& is)
|
||||
{
|
||||
// TODO generic read
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Write values to an output stream
|
||||
*/
|
||||
virtual bool write(std::ostream& os) const
|
||||
{
|
||||
// TODO generic write
|
||||
return os.good();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Assign the TebConfig class for parameters.
|
||||
* @param cfg TebConfig class
|
||||
*/
|
||||
void setTebConfig(const TebConfig& cfg)
|
||||
{
|
||||
cfg_ = &cfg;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_error;
|
||||
using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_vertices;
|
||||
|
||||
const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @class BaseTebMultiEdge
|
||||
* @brief Base edge connecting multiple vertices in the TEB optimization problem
|
||||
*
|
||||
* This edge defines a base edge type for the TEB optimization problem.
|
||||
* It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
|
||||
* The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
|
||||
* Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
|
||||
* @see BaseTebBinaryEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
|
||||
*/
|
||||
template <int D, typename E>
|
||||
class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>
|
||||
{
|
||||
public:
|
||||
|
||||
using typename g2o::BaseMultiEdge<D, E>::ErrorVector;
|
||||
using g2o::BaseMultiEdge<D, E>::computeError;
|
||||
|
||||
// Overwrites resize() from the parent class
|
||||
virtual void resize(size_t size)
|
||||
{
|
||||
g2o::BaseMultiEdge<D, E>::resize(size);
|
||||
|
||||
for(std::size_t i=0; i<_vertices.size(); ++i)
|
||||
_vertices[i] = NULL;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Compute and return error / cost value.
|
||||
*
|
||||
* This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
|
||||
* @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
|
||||
*/
|
||||
ErrorVector& getError()
|
||||
{
|
||||
computeError();
|
||||
return _error;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read values from input stream
|
||||
*/
|
||||
virtual bool read(std::istream& is)
|
||||
{
|
||||
// TODO generic read
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Write values to an output stream
|
||||
*/
|
||||
virtual bool write(std::ostream& os) const
|
||||
{
|
||||
// TODO generic write
|
||||
return os.good();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Assign the TebConfig class for parameters.
|
||||
* @param cfg TebConfig class
|
||||
*/
|
||||
void setTebConfig(const TebConfig& cfg)
|
||||
{
|
||||
cfg_ = &cfg;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
using g2o::BaseMultiEdge<D, E>::_error;
|
||||
using g2o::BaseMultiEdge<D, E>::_vertices;
|
||||
|
||||
const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
} // end namespace
|
||||
|
||||
#endif
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user