Skip to content

Commit

Permalink
Fixed Linting
Browse files Browse the repository at this point in the history
Signed-off-by: Jatin Patil <[email protected]>
  • Loading branch information
JatinPatil2003 committed Aug 13, 2024
1 parent e0313b7 commit 7db9b1f
Show file tree
Hide file tree
Showing 2 changed files with 9 additions and 11 deletions.
16 changes: 7 additions & 9 deletions nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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:
Expand All @@ -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
Expand All @@ -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)
Expand Down
4 changes: 2 additions & 2 deletions nav2_loopback_sim/nav2_loopback_sim/utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down

0 comments on commit 7db9b1f

Please sign in to comment.