Skip to content

Commit

Permalink
final updates working correctly
Browse files Browse the repository at this point in the history
  • Loading branch information
@JatinPatil2003
JatinPatil2003 committed Aug 20, 2024
1 parent 9a4cb0e commit de6ee19
Show file tree
Hide file tree
Showing 2 changed files with 35 additions and 136 deletions.
158 changes: 35 additions & 123 deletions nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
from geometry_msgs.msg import Quaternion, TransformStamped, Vector3
from nav_msgs.msg import Odometry
from nav_msgs.srv import GetMap
from nav2_simple_commander.line_iterator import LineIterator
import rclpy
from rclpy.duration import Duration
from rclpy.node import Node
Expand All @@ -27,19 +28,14 @@
from tf2_ros import Buffer, TransformBroadcaster, TransformListener
import tf_transformations

# from nav2_simple_commander.line_iterator import LineIterator

from .utils import (
addYawToQuat,
getMapOccupancy,
mapToWorld,
matrixToTransform,
transformStampedToMatrix,
worldToMap,
)

LETHAL_OBSTACLE = 100

"""
This is a loopback simulator that replaces a physics simulator to create a
frictionless, inertialess, and collisionless simulation environment. It
Expand Down Expand Up @@ -77,10 +73,6 @@ def __init__(self):
self.declare_parameter('scan_frame_id', 'base_scan')
self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value

self.declare_parameter('publish_map_odom_tf', True)
self.publish_map_odom_tf = self.get_parameter(
'publish_map_odom_tf').get_parameter_value().bool_value

self.declare_parameter('scan_publish_dur', 0.1)
self.scan_publish_dur = self.get_parameter(
'scan_publish_dur').get_parameter_value().double_value
Expand Down Expand Up @@ -133,7 +125,6 @@ def setupTimerCallback(self):
# Publish initial identity odom transform & laser scan to warm up system
self.tf_broadcaster.sendTransform(self.t_odom_to_base_link)
if self.mat_base_to_laser is None:
self.mat_odom_to_base = transformStampedToMatrix(self.t_odom_to_base_link)
self.getBaseToLaserTf()

def cmdVelCallback(self, msg):
Expand All @@ -156,7 +147,6 @@ def initialPoseCallback(self, msg):
self.t_odom_to_base_link.transform.translation = Vector3()
self.t_odom_to_base_link.transform.rotation = Quaternion()
self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link)
self.mat_map_to_odom = transformStampedToMatrix(self.t_map_to_odom)

# Start republication timer and velocity processing
if self.setupTimer is not None:
Expand Down Expand Up @@ -208,8 +198,6 @@ def timerCallback(self):

self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link)
self.publishOdometry(self.t_odom_to_base_link)
self.mat_map_to_odom = transformStampedToMatrix(self.t_map_to_odom)
self.mat_odom_to_base = transformStampedToMatrix(self.t_odom_to_base_link)

def publishLaserScan(self):
# Publish a bogus laser scan for collision monitor
Expand All @@ -235,8 +223,7 @@ def publishTransforms(self, map_to_odom, odom_to_base_link):
map_to_odom.header.stamp = \
(self.get_clock().now() + Duration(seconds=self.update_dur)).to_msg()
odom_to_base_link.header.stamp = self.get_clock().now().to_msg()
if self.publish_map_odom_tf:
self.tf_broadcaster.sendTransform(map_to_odom)
self.tf_broadcaster.sendTransform(map_to_odom)
self.tf_broadcaster.sendTransform(odom_to_base_link)

def publishOdometry(self, odom_to_base_link):
Expand Down Expand Up @@ -279,9 +266,12 @@ def getMap(self):
)

def getLaserPose(self):
mat_map_to_odom = transformStampedToMatrix(self.t_map_to_odom)
mat_odom_to_base = transformStampedToMatrix(self.t_odom_to_base_link)

mat_map_to_laser = tf_transformations.concatenate_matrices(
self.mat_map_to_odom,
self.mat_odom_to_base,
mat_map_to_odom,
mat_odom_to_base,
self.mat_base_to_laser
)
transform = matrixToTransform(mat_map_to_laser)
Expand All @@ -302,119 +292,41 @@ def getLaserScan(self, num_samples):
self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples
return

x, y, theta = self.getLaserPose()
x0, y0, theta = self.getLaserPose()

mx, my = worldToMap(x, y, self.map)
mx0, my0 = worldToMap(x0, y0, self.map)

if not mx and not my:
if not 0 < mx0 < self.map.info.width or not 0 < my0 < self.map.info.height:
# outside map
self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples
return

for i in range(int(num_samples)):
for i in range(num_samples):
curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment
self.scan_msg.ranges[i] = self.findMapRange(mx, my, x, y, curr_angle)

# def getLaserScan(self, num_samples):
# if self.map is None or self.initial_pose is None or self.mat_base_to_laser is None:
# self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples
# return

# x0, y0 ,theta = self.getLaserPose()

# max_distance = math.sqrt(
# (self.map.info.width * self.map.info.resolution) ** 2 +
# (self.map.info.height * self.map.info.resolution) ** 2)
# max_distance = min(max_distance, self.scan_msg.range_max)
# for i in range(num_samples):
# curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment
# x1 = float(x0) + max_distance * math.cos(curr_angle)
# y1 = float(y0) + max_distance * math.sin(curr_angle)

# line_iterator = LineIterator(float(x0), float(y0), x1, y1, self.map.info.resolution)

# while line_iterator.isValid():
# mx, my = worldToMap(line_iterator.getX(), line_iterator.getY(), self.map)

# if not mx and not my:
# break

# point_cost = getMapOccupancy(mx, my, self.map)

# if point_cost >= 60:
# self.scan_msg.ranges[i] = math.sqrt(
# (line_iterator.getX() - x0) ** 2 + (line_iterator.getY() - y0) ** 2)
# break

# line_iterator.advance()

def findMapRange(self, mx, my, x, y, theta):
# Using "A Faster Voxel Traversal Algorithm for Ray Tracing" by Amanatides & Woo
# ======== Initialization Phase ========
origin = [x, y] # u
direction = [math.cos(theta), math.sin(theta)] # v

current = [mx, my] # X, Y
step = [0, 0] # stepX, stepY
tMax = [0.0, 0.0] # tMaxX, tMaxY
tDelta = [0.0, 0.0] # tDeltaX, tDeltaY

voxel_border = [0.0, 0.0]
voxel_border[0], voxel_border[1] = mapToWorld(
current[0], current[1], self.map)

for i in range(2): # For each dimension (x, y)
# Determine step direction
if direction[i] > 0.0:
step[i] = 1
elif direction[i] < 0.0:
step[i] = -1
else:
step[i] = 0

# Determine tMax, tDelta
if step[i] != 0:
if step[i] == 1:
voxel_border[i] += step[i] * self.map.info.resolution

# tMax - voxel boundary crossing
tMax[i] = (voxel_border[i] - origin[i]) / direction[i]
# tDelta - distance along ray
# so that vertical/horizontal component equals one voxel
tDelta[i] = self.map.info.resolution / abs(direction[i])
else:
tMax[i] = float('inf')
tDelta[i] = float('inf')

# ======== Incremental Traversal ========
while True:
# Advance
dim = 0 if tMax[0] < tMax[1] else 1

# Advance one voxel
current[dim] += step[dim]
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):
return self.scan_msg.range_max - 0.1

# Determine current range
current_range = math.sqrt(
(current[0] - mx) ** 2 + (current[1] - my) ** 2
) * self.map.info.resolution

# Are we at max range?
if current_range > self.scan_msg.range_max:
return self.scan_msg.range_max - 0.1
else:
occ = getMapOccupancy(current[0], current[1], self.map)
if occ >= 60: # Current cell is occupied
# Is range less than min range
if current_range < self.scan_msg.range_min:
return 0.0
x1 = x0 + self.scan_msg.range_max * math.cos(curr_angle)
y1 = y0 + self.scan_msg.range_max * math.sin(curr_angle)

mx1, my1 = worldToMap(x1, y1, self.map)

line_iterator = LineIterator(mx0, my0, mx1, my1, 0.5)

while line_iterator.isValid():
mx, my = int(line_iterator.getX()), int(line_iterator.getY())

if not 0 < mx < self.map.info.width or not 0 < my < self.map.info.height:
# if outside map then check next ray
break

point_cost = getMapOccupancy(mx, my, self.map)

if point_cost >= 60:
self.scan_msg.ranges[i] = math.sqrt(
(int(line_iterator.getX()) - mx0) ** 2 +
(int(line_iterator.getY()) - my0) ** 2
) * self.map.info.resolution
break

return current_range
line_iterator.advance()


def main():
Expand Down
13 changes: 0 additions & 13 deletions nav2_loopback_sim/nav2_loopback_sim/utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,23 +68,10 @@ def matrixToTransform(matrix):


def worldToMap(world_x, world_y, map_msg):
# Check if world coordinates are out of bounds
if (world_x < map_msg.info.origin.position.x or world_y < map_msg.info.origin.position.y):
return None, None # Coordinates are out of bounds

map_x = int(math.floor((world_x - map_msg.info.origin.position.x) / map_msg.info.resolution))
map_y = int(math.floor((world_y - map_msg.info.origin.position.y) / map_msg.info.resolution))

if not map_x < map_msg.info.width or not map_y < map_msg.info.height:
return None, None # Coordinates are out of bounds
return map_x, map_y


def mapToWorld(map_x, map_y, map_msg):
world_x = map_msg.info.origin.position.x + ((map_x + 0.5) * map_msg.info.resolution)
world_y = map_msg.info.origin.position.y + ((map_y + 0.5) * map_msg.info.resolution)
return world_x, world_y


def getMapOccupancy(x, y, map_msg):
return map_msg.data[y * map_msg.info.width + x]

0 comments on commit de6ee19

Please sign in to comment.