diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index f15b88115f..cf34b3f003 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -34,8 +34,8 @@ get_world2map_coordinates, matrixToTransform, transformStampedToMatrix, - ) + """ This is a loopback simulator that replaces a physics simulator to create a frictionless, inertialess, and collisionless simulation environment. It @@ -197,7 +197,7 @@ def publishLaserScan(self): self.scan_msg.range_min = 0.05 self.scan_msg.range_max = 100.0 num_samples = int( - (self.scan_msg.angle_max - self.scan_msg.angle_min) / + (self.scan_msg.angle_max - self.scan_msg.angle_min) / self.scan_msg.angle_increment) self.scan_msg.ranges = [0.0] * num_samples x, y, theta = self.get_laser_pose() @@ -293,8 +293,8 @@ def find_map_range(self, x, y, theta): start_x, start_y = get_world2map_coordinates(x, y, self.map) - if (start_x < 0 or start_y < 0 or - start_x >= self.map.info.width or start_y >= self.map.info.height): + if (start_x < 0 or start_y < 0 or + start_x >= self.map.info.width or start_y >= self.map.info.height): # Outside the map, find entry point delta_x = abs(self.map.info.origin.position.x - x) delta_y = abs(self.map.info.origin.position.y - y) @@ -329,7 +329,7 @@ def find_map_range(self, x, y, theta): # tMax - voxel boundary crossing tMax[i] = (voxel_border[i] - origin[i]) / direction[i] - # tDelta - distance along ray + # tDelta - distance along ray # so that vertical/horizontal component equals one voxel tDelta[i] = self.map.info.resolution / abs(direction[i]) else: @@ -346,9 +346,8 @@ def find_map_range(self, x, y, theta): tMax[dim] += tDelta[dim] # Check map inbounds - if (current[0] < 0 or current[0] >= self.map.info.width or - current[1] < 0 or current[1] >= self.map.info.height): - # self.get_logger().info("Here-1") + if (current[0] < 0 or current[0] >= self.map.info.width or + current[1] < 0 or current[1] >= self.map.info.height): return self.scan_msg.range_max # Determine current range @@ -358,7 +357,6 @@ def find_map_range(self, x, y, theta): # Are we at max range? if current_range > self.scan_msg.range_max: - # self.get_logger().info("Here-1") return self.scan_msg.range_max else: occ = get_map_occupancy(current[0], current[1], self.map) diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index ec44509bb8..e2a513d166 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -74,9 +74,9 @@ def get_world2map_coordinates(world_x, world_y, map_msg): def get_map2world_coordinates(map_x, map_y, map_msg): - world_x = ((map_x * map_msg.info.resolution) + map_msg.info.origin.position.x + + world_x = ((map_x * map_msg.info.resolution) + map_msg.info.origin.position.x + 0.5 * map_msg.info.resolution) - world_y = ((map_y * map_msg.info.resolution) + map_msg.info.origin.position.y + + world_y = ((map_y * map_msg.info.resolution) + map_msg.info.origin.position.y + 0.5 * map_msg.info.resolution) return world_x, world_y