diff --git a/src/grudsby_costmap/config/local_costmap.yaml b/src/grudsby_costmap/config/local_costmap.yaml
index 034dbf0..6a1c4ba 100644
--- a/src/grudsby_costmap/config/local_costmap.yaml
+++ b/src/grudsby_costmap/config/local_costmap.yaml
@@ -7,9 +7,9 @@ local_costmap:
rolling_window: true
track_unknown_space: false
inf_is_valid: true
- height: 6.0
- width: 6.0
+ height: 2.0
+ width: 11
plugins:
- - {name: obstacles_laser_local, type: "costmap_2d::ObstacleLayer"}
- - {name: inflation, type: "costmap_2d::InflationLayer"}
+ - {name: obstacles_laser_local, type: "costmap_2d::ObstacleLayer"}
+ - {name: inflation_dymamic, type: "costmap_2d::InflationLayer"}
diff --git a/src/grudsby_costmap/config/shared_costmap.yaml b/src/grudsby_costmap/config/shared_costmap.yaml
index 47dc4b3..558a6ec 100644
--- a/src/grudsby_costmap/config/shared_costmap.yaml
+++ b/src/grudsby_costmap/config/shared_costmap.yaml
@@ -89,3 +89,7 @@ obstacles_laser_local:
inflation:
inflation_radius: 1.0
cost_scaling_factor: 1.0
+
+inflation_dynamic:
+ inflation_radius: 0.1
+ cost_scaling_factor: 1.0
diff --git a/src/grudsby_launch/launch/dynamic.launch b/src/grudsby_launch/launch/dynamic.launch
index 656059b..f558cd3 100644
--- a/src/grudsby_launch/launch/dynamic.launch
+++ b/src/grudsby_launch/launch/dynamic.launch
@@ -20,7 +20,7 @@ Grudsby will stop if an obstacle is encountered
- -->
+ -->
@@ -68,6 +68,9 @@ Grudsby will stop if an obstacle is encountered
+
+ clip_scan : True
+
diff --git a/src/grudsby_vision/scan_filter/scripts/scan_inf_repeater.py b/src/grudsby_vision/scan_filter/scripts/scan_inf_repeater.py
index 30d6663..d34b955 100755
--- a/src/grudsby_vision/scan_filter/scripts/scan_inf_repeater.py
+++ b/src/grudsby_vision/scan_filter/scripts/scan_inf_repeater.py
@@ -8,11 +8,12 @@
from sensor_msgs.msg import LaserScan
-MAX_RANGE = 8
+MAX_RANGE = 4.1
class DiffTf:
def __init__(self):
rospy.init_node("scan_repeater")
+ self.clip_range = rospy.get_param("clip_scan",False)
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
self.rate = 100
@@ -51,7 +52,11 @@ def laserResponseCallback(self, msg):
for i in range ( window_size , len ( fix_nans )-window_size ):
neighborhood = fix_nans [ i-window_size : i + window_size + 1]
neighborhood.sort ( )
- output [ i ] = neighborhood [ int ( math.floor ( window_size / 2 ) ) ]
+ if self.clip_range:
+ output [ i ] = min ( MAX_RANGE ,
+ neighborhood [ int ( math.floor ( window_size / 2 ) ) ] )
+ else:
+ output [ i ] = neighborhood [ int ( math.floor ( window_size / 2 ) ) ]
cloud_out.ranges = tuple (output)
self.laserPub.publish(cloud_out)