diff --git a/src/environments/environments/CarWallEnvironment.py b/src/environments/environments/CarWallEnvironment.py index 81a62244..d2336654 100644 --- a/src/environments/environments/CarWallEnvironment.py +++ b/src/environments/environments/CarWallEnvironment.py @@ -90,13 +90,16 @@ def __init__(self, car_name, reward_range=0.2, max_steps=50, collision_range=0.2 while not self.reset_client.wait_for_service(timeout_sec=1.0): self.get_logger().info('reset service not available, waiting again...') - time.sleep(2) - - # TODO: generate goal self.goal_position = [10, 10] # x and y - time.sleep(5) - + self.timer = self.create_timer(step_length, self.timer_cb) + self.timer_future = Future() + + self.get_logger().info('Environment Setup Complete') + + def timer_cb(self): + self.timer_future.set_result(True) + def reset(self): self.step_counter = 0 @@ -105,11 +108,12 @@ def reset(self): #TODO: Remove Hard coded-ness of 10x10 self.goal_position = self.generate_goal() - time.sleep(self.STEP_LENGTH) + while not self.timer_future.done(): + rclpy.spin_once(self) + + self.timer_future = Future() self.call_reset_service() - - time.sleep(self.STEP_LENGTH) observation = self.get_observation() @@ -136,7 +140,10 @@ def step(self, action): lin_vel, ang_vel = action self.set_velocity(lin_vel, ang_vel) - time.sleep(self.STEP_LENGTH) + while not self.timer_future.done(): + rclpy.spin_once(self) + + self.timer_future = Future() next_state = self.get_observation() reward = self.compute_reward(state, next_state) diff --git a/src/environments/launch/carwall.launch.py b/src/environments/launch/carwall.launch.py index cd3391e1..3de0ddc3 100644 --- a/src/environments/launch/carwall.launch.py +++ b/src/environments/launch/carwall.launch.py @@ -18,7 +18,11 @@ def generate_launch_description(): f'/world/empty/create@ros_gz_interfaces/srv/SpawnEntity', f'/world/empty/remove@ros_gz_interfaces/srv/DeleteEntity', f'/world/empty/set_pose@ros_gz_interfaces/srv/SetEntityPose', - ] + f'/world/empty/clock@rosgraph_msgs/msg/Clock@gz.msgs.Clock' + ], + remappings=[ + (f'/world/empty/clock', f'/clock'), + ], ) gz_sim = IncludeLaunchDescription( diff --git a/src/f1tenth b/src/f1tenth index 012327dc..c5eba9a4 160000 --- a/src/f1tenth +++ b/src/f1tenth @@ -1 +1 @@ -Subproject commit 012327dca3e647f4791f7f8545b5f69fdafe98d8 +Subproject commit c5eba9a4cf0e5ccd3fbf01d10a4656d200167ecc diff --git a/src/reinforcement_learning/launch/car_wall.launch.py b/src/reinforcement_learning/launch/car_wall.launch.py index f06a6b72..3e65d746 100644 --- a/src/reinforcement_learning/launch/car_wall.launch.py +++ b/src/reinforcement_learning/launch/car_wall.launch.py @@ -1,6 +1,6 @@ import os from ament_index_python import get_package_share_directory -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter from launch import LaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable @@ -47,6 +47,7 @@ def generate_launch_description(): return LaunchDescription([ #TODO: Find a way to remove this SetEnvironmentVariable(name='GZ_SIM_RESOURCE_PATH', value=pkg_f1tenth_description[:-19]), + SetParameter(name='use_sim_time', value=True), environment, f1tenth, main