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)