Hello!
I made a few additions to my code trying to fix the problem. I noticed that if I put a 2D pose estimate on the robot before send a nav goal the robot seems to navigate correctly. However when I tried sending the position to the initalpose topic, Rviz seemed to be one randomization behind Isaac Sim. I added markers to the code to show where on the rviz map the robots should be.
class RandomPositionNode(Node):
def __init__(self):
super().__init__('random_position_node')
self.costmap_subscriptions = []
self.costmaps = dict()
self.pose_publisher = dict()
for robot in robots:
topic_costmap = '/' + robot + '/global_costmap/costmap'
self.get_logger().info(f"Subscribing to {topic_costmap}")
self.costmap_subscriptions.append(self.create_subscription(
OccupancyGrid,
topic_costmap,
# lambda msg: self.costmap_callback(msg, robot),
partial(self.costmap_callback, robot=robot),
10
))
self.costmaps[robot] = None
topic_pose = '/' + robot + '/initialpose'
self.pose_publisher[robot] = self.create_publisher(PoseWithCovarianceStamped, topic_pose, 10)
def publish_pose(self,x,y,bot):
msg = PoseWithCovarianceStamped()
msg.header.frame_id = "map"
msg.header.stamp.sec = 1741657725
msg.pose.pose.position.x = x
msg.pose.pose.position.y = y
msg.pose.pose.position.z = 0.0
msg.pose.pose.orientation.x = 0.0
msg.pose.pose.orientation.y = 0.0
msg.pose.pose.orientation.z = 0.0
msg.pose.pose.orientation.w = 1.0
msg.pose.covariance = [
0.25,0.0,0.0,0.0,0.0,0.0,
0.0,0.25,0.0,0.0,0.0,0.0,
0.0,0.0,0.0,0.0,0.0,0.0,
0.0,0.0,0.0,0.0,0.0,0.0,
0.0,0.0,0.0,0.0,0.0,0.0,
0.0,0.0,0.0,0.0,0.0,0.0685389
]
self.pose_publisher[bot].publish(msg)
self.get_logger().info('publishing for "%s": "%s", "%s"' % (bot, msg.pose.pose.position.x, msg.pose.pose.position.y))
def make_marker(self,x,y,bot):
self.marker_pub = self.create_publisher(Marker, f"{bot}/Marker", 10)
marker = Marker()
marker.header.frame_id = "/map"
# set shape, Arrow: 0; Cube: 1 ; Sphere: 2 ; Cylinder: 3
marker.type = 2
marker.id = 0
# Set the scale of the marker
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
# Set the color
if bot == "carter1":
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
if bot == "carter2":
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
if bot == "carter3":
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.color.a = 1.0
# Set the pose of the marker
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = 0.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
self.marker_pub.publish(marker)
def reset_pos2(prims,randomization_node):
i=0
for prim in prims:
pos = random_pos(robots[i],randomization_node)
if pos != None:
x,y,z = pos
print(f"loc of {robots[i]}: {x,y}")
rand_loc_gf = Gf.Vec3f(x, y, 0)
randomization_node.make_marker(x,y,robots[i])
prim.GetAttribute("xformOp:translate").Set(rand_loc_gf)
time.sleep(1)
rclpy.spin_once(randomization_node, timeout_sec=1.0)
randomization_node.publish_pose(x,y,robots[i])
i+= 1
I added my updates randomposition node and reset_pos2 function
