def collision(self):
# Reset if there is a collision
if self._collision_msg is not None:
while not self.reset_sim.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'/reset_simulation service not available, waiting again...'
)
reset_future = self.reset_sim.call_async(Empty.Request())
rclpy.spin_until_future_complete(self.node, reset_future)
self._collision_msg = None
self.collided += 1
return True
else:
return False
def _reset(self):
# Create an empty request to reset the simulation (emtpy because we don't specify any arguments for the request)
reset_request = Empty.Request()
# Wait for the reset service to appear
while not self.reset_client.wait_for_service(timeout_sec=1.0):
self.ros_node.get_logger().info('simulation reset service not available, waiting...')
# Send the request to reset the simulation
self.reset_client.call(reset_request)
# Reset the observation - otherwise, the bot will lose immediately because the old observation
# (where we drove against the wall) is still there
self.latest_observation = np.array([0.2] * 360, dtype=np.dtype('float64'))
self._episode_ended = False
return time_step.restart(self.latest_observation)
def reset(self):
"""
Reset the agent for a particular experiment condition.
"""
self.iterator = 0
if self.reset_jnts is True:
# reset simulation
while not self.reset_sim.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info('service not available, waiting again...')
reset_future = self.reset_sim.call_async(Empty.Request())
rclpy.spin_until_future_complete(self.node, reset_future)
# Take an observation
self.ob = self.take_observation()
# Return the corresponding observation
return self.ob
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node('unpause_simulation',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
timeout = 0.0
if node.has_parameter('timeout'):
timeout = node.get_parameter('timeout').get_parameter_value().double_value
if timeout <= 0:
raise RuntimeError('Unpause time must be a positive floating point value')
print('Unpause simulation - Time = {} s'.format(timeout))
if(timeout > 0):
time.sleep(timeout)
# Handle for retrieving model properties
unpause = node.create_client(Empty, '/gazebo/unpause_physics')
unpause.wait_for_service(timeout_sec=100)
if(not ready):
raise Exception('Service %s is unavailable' % unpause.srv_name)
sys.exit()
node.get_logger().info(
'The Gazebo "unpause_physics" service was available {} s after the timeout'.format(timeout))
req = Empty.Request()
future = unpause.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
prop = future.result()
if prop.succes:
print('Simulation unpaused...')
else
node.get_logger().error('Failed to unpaused the simulation')
def __init__(self):
super().__init__("spawn_node")
self.turtle_counter = 2
self.turtle_spawnServer = self.create_service(TurtleService, "turtle_spawn", self.turtleServer_callBack)
self.turtle_killingClient = self.create_client(Kill, "kill")
self.turtle_spawnClient = self.create_client(Spawn, "spawn")
self.turtle_clearClient = self.create_client(Empty, "clear")
while not( (self.turtle_killingClient.wait_for_service(timeout_sec=1.0)) and (self.turtle_spawnClient.wait_for_service(timeout_sec=1.0)) ):
self.get_logger().info("Waiting for turtle services")
self.killReq = Kill.Request()
self.spawnReq = Spawn.Request()
self.clearReq = Empty.Request()
self.spawnReq.x = float(rand.randint(1, 10))
self.spawnReq.y = float(rand.randint(1, 10))
self.spawnReq.theta = 0.0
self.spawnReq.name = "turtle{}".format(self.turtle_counter)
self.turtle_spawnClient.call_async(self.spawnReq)
def reset(self):
"""
Reset the agent for a particular experiment condition.
"""
self.iterator = 0
if self.reset_jnts:
# reset simulation
while not self.reset_sim.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info('/reset_simulation service not available, waiting again...')
reset_future = self.reset_sim.call_async(Empty.Request())
rclpy.spin_until_future_complete(self.node, reset_future)
self.ros_clock = rclpy.clock.Clock().now().nanoseconds
# delete entity
delete_entity_cli = self.node.create_client(DeleteEntity, '/delete_entity')
while not delete_entity_cli.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info('/reset_simulation service not available, waiting again...')
req = DeleteEntity.Request()
req.name = "target"
delete_future = delete_entity_cli.call_async(req)
rclpy.spin_until_future_complete(self.node, delete_future)
self.spawn_target()
# Take an observation
obs = self.take_observation()
# Return the corresponding observation
return obs
def pause_sim(self) -> None:
while not self._physics_pause_client.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'/pause_physics service not available, waiting again...')
pause_future = self._physics_pause_client.call_async(Empty.Request())
rclpy.spin_until_future_complete(self.node, pause_future)
def reset():
global reset_service
while not reset_service.wait_for_service(timeout_sec=1.0):
print('service not available, trying again...')
reset_service.call_async(Empty.Request())
def loop(self):
try:
while True:
key = self.getKey()
if key in moveBindings.keys():
self.x += moveBindings[key][0] * self.x_speed_step
self.x = round(self.x, 2)
self.y += moveBindings[key][1] * self.y_speed_step
self.y = round(self.y, 2)
self.th += moveBindings[key][2] * self.turn_speed_step
self.th = round(self.th, 2)
self.a_x = 0
elif key in headBindings.keys():
self.head_msg.positions[0] = self.head_pan_pos + headBindings[key][1] * self.head_pan_step
self.head_msg.positions[1] = self.head_tilt_pos + headBindings[key][0] * self.head_tilt_step
self.head_pub.publish(self.head_msg)
elif key == 'k' or key == 'K':
# put head back in init
self.head_msg.positions[0] = 0
self.head_msg.positions[1] = 0
self.head_pub.publish(self.head_msg)
elif key == 'y':
# kick left forward
self.client.send_goal_async(self.generate_kick_goal(0.2, 0.1, 0))
elif key == '<':
# kick left side ball left
self.client.send_goal_async(self.generate_kick_goal(0.2, 0.1, -1.57))
elif key == '>':
# kick left side ball center
self.client.send_goal_async(self.generate_kick_goal(0.2, 0, -1.57))
elif key == 'c':
# kick right forward
self.client.send_goal_async(self.generate_kick_goal(0.2, -0.1, 0))
elif key == 'v':
# kick right side ball right
self.client.send_goal_async(self.generate_kick_goal(0.2, -0.1, 1.57))
elif key == 'V':
# kick right side ball center
self.client.send_goal_async(self.generate_kick_goal(0.2, 0, 1.57))
elif key == "x":
# kick center forward
self.client.send_goal_async(self.generate_kick_goal(0.2, 0, 0))
elif key == "X":
# kick center backwards
self.client.send_goal_async(self.generate_kick_goal(-0.2, 0, 0))
elif key == "b":
# kick left backwards
self.client.send_goal_async(self.generate_kick_goal(-0.2, 0.1, 0))
elif key == "n":
# kick right backwards
self.client.send_goal_async(self.generate_kick_goal(-0.2, -0.1, 0))
elif key == "B":
# kick left backwards
self.client.send_goal_async(self.generate_kick_goal(0, 0.14, -1.57))
elif key == "N":
# kick right backwards
self.client.send_goal_async(self.generate_kick_goal(0, -0.14, 1.57))
elif key == 'Y':
# kick left walk
self.walk_kick_pub.publish(False)
elif key == 'C':
# kick right walk
self.walk_kick_pub.publish(True)
elif key == 'F':
# play walkready animation
self.walkready.header.stamp = self.get_clock().now().to_msg()
self.joint_pub.publish(self.walkready)
elif key == 'r':
# reset robot in sim
try:
self.reset_robot.call_async(Empty.Request())
except:
pass
elif key == 'R':
# reset ball in sim
try:
self.reset_ball.call_async(Empty.Request())
except:
pass
elif key == 'f':
# complete walk stop
self.x = 0
self.y = 0
self.z = 0
self.a_x = -1
self.th = 0
else:
self.x = 0
self.y = 0
self.z = 0
self.a_x = 0
self.th = 0
if (key == '\x03'):
self.a_x = -1
break
twist = Twist()
twist.linear.x = float(self.x)
twist.linear.y = float(self.y)
twist.linear.z = float(0)
twist.angular.x = float(self.a_x)
twist.angular.y = float(0)
twist.angular.z = float(self.th)
self.pub.publish(twist)
sys.stdout.write("\x1b[A")
sys.stdout.write("\x1b[A")
sys.stdout.write("\x1b[A")
sys.stdout.write("\x1b[A")
sys.stdout.write("\x1b[A")
print(
f"x: {self.x} \ny: {self.y} \nturn: {self.th} \n\n")
except Exception as e:
print(e)
finally:
print("\n")
twist = Twist()
twist.linear.x = float(0)
twist.linear.y = float(0)
twist.linear.z = float(0)
twist.angular.x = float(0)
twist.angular.y = float(0)
twist.angular.z = float(0)
self.pub.publish(twist)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
def run(self):
"""
Run node, spawning entity and doing other actions as configured in program arguments.
Returns exit code, 1 for failure, 0 for success
"""
# Wait for entity to exist if wait flag is enabled
if self.args.wait:
self.entity_exists = False
def entity_cb(entity):
self.entity_exists = self.args.wait in entity.name
self.subscription = self.create_subscription(
ModelStates, '%s/model_states' % self.args.gazebo_namespace,
entity_cb, 10)
self.get_logger().info(
'Waiting for entity {} before proceeding.'.format(
self.args.wait))
while rclpy.ok() and not self.entity_exists:
rclpy.spin_once(self)
pass
# Load entity XML from file
if self.args.file:
self.get_logger().info('Loading entity XML from file %s' %
self.args.file)
if not os.path.exists(self.args.file):
self.get_logger().error(
'Error: specified file %s does not exist', self.args.file)
return 1
if not os.path.isfile(self.args.file):
self.get_logger().error(
'Error: specified file %s is not a file', self.args.file)
return 1
# load file
try:
f = open(self.args.file, 'r')
entity_xml = f.read()
except IOError as e:
self.get_logger().error('Error reading file {}: {}'.format(
self.args.file, e))
return 1
if entity_xml == '':
self.get_logger().error('Error: file %s is empty',
self.args.file)
return 1
# Load entity XML published on topic specified
elif self.args.topic:
self.get_logger().info('Loading entity published on topic %s' %
self.args.topic)
entity_xml = ''
def entity_xml_cb(msg):
nonlocal entity_xml
entity_xml = msg.data
self.subscription = self.create_subscription(
String, self.args.topic, entity_xml_cb,
QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
while rclpy.ok() and entity_xml == '':
self.get_logger().info('Waiting for entity xml on %s' %
self.args.topic)
rclpy.spin_once(self)
pass
# Generate entity XML by putting requested entity name into request template
elif self.args.database:
self.get_logger().info(
'Loading entity XML from Gazebo Model Database')
entity_xml = self.MODEL_DATABASE_TEMPLATE.format(
self.args.database)
elif self.args.stdin:
self.get_logger().info('Loading entity XML from stdin')
entity_xml = sys.stdin.read()
if entity_xml == '':
self.get_logger().error('Error: stdin buffer was empty')
return 1
# Parse xml to detect invalid xml before sending to gazebo
try:
xml_parsed = ElementTree.fromstring(entity_xml)
except ElementTree.ParseError as e:
self.get_logger().error('Invalid XML: {}'.format(e))
return 1
# Replace package:// with model:// for mesh tags if flag is set
if self.args.package_to_model:
for element in xml_parsed.iterfind('.//mesh'):
filename_tag = element.get('filename')
if filename_tag is None:
continue
url = urlsplit(filename_tag)
if url.scheme == 'package':
url = SplitResult('model', *url[1:])
element.set('filename', url.geturl())
# Encode xml object back into string for service call
entity_xml = ElementTree.tostring(xml_parsed)
# Form requested Pose from arguments
initial_pose = Pose()
initial_pose.position.x = float(self.args.x)
initial_pose.position.y = float(self.args.y)
initial_pose.position.z = float(self.args.z)
q = quaternion_from_euler(self.args.R, self.args.P, self.args.Y)
initial_pose.orientation.w = q[0]
initial_pose.orientation.x = q[1]
initial_pose.orientation.y = q[2]
initial_pose.orientation.z = q[3]
success = self._spawn_entity(entity_xml, initial_pose)
if not success:
self.get_logger().error('Spawn service failed. Exiting.')
return 1
# TODO(shivesh): Wait for /set_model_configuration
# (https://github.com/ros-simulation/gazebo_ros_pkgs/issues/779)
# Apply joint positions if any specified
# if len(self.args.joints) != 0:
# joint_names = [joint[0] for joint in self.args.joints]
# joint_positions = [joint[1] for joint in self.args.joints]
# success = _set_model_configuration(joint_names, joint_positions)
# if not success:
# self.get_logger().error('SetModelConfiguration service failed. Exiting.')
# return 1
# Unpause physics if user requested
if self.args.unpause:
client = self.create_client(
Empty, '%s/unpause_physics' % self.args.gazebo_namespace)
if client.wait_for_service(timeout_sec=self.args.timeout):
self.get_logger().info('Calling service %s/unpause_physics' %
self.args.gazebo_namespace)
client.call_async(Empty.Request())
else:
self.get_logger().error(
'Service %s/unpause_physics unavailable. \
Was Gazebo started with GazeboRosInit?'
)
# If bond enabled, setup shutdown callback and wait for shutdown
if self.args.bond:
self.get_logger().info(
'Waiting for shutdown to delete entity [{}]'.format(
self.args.entity))
try:
rclpy.spin(self)
except KeyboardInterrupt:
self.get_logger().info('Ctrl-C detected')
self._delete_entity()
return 0
def process(self):
#environment init
while not self.make_environment_client.wait_for_service(
timeout_sec=1.0):
print('Environment client ...')
self.make_environment_client.call_async(Empty.Request())
time.sleep(1.0)
episode_num = 0
for episode in range(self.load_episode + 1,
self.max_training_episodes):
episode_num += 1
local_step = 0
score = 0
# Reset DQN environment
state = self.reset_environment()
time.sleep(1.0)
while True:
local_step += 1
action = int(self.get_action(state))
next_state, reward, done = self.step(action)
#print('reward ',reward, ' score ',score)
score += reward
if self.train_mode:
self.append_sample(
(state, action, reward, next_state, done))
self.train_model(done)
state = next_state
if done:
if LOGGING:
self.dqn_reward_metric.update_state(score)
with self.dqn_reward_writer.as_default():
tf.summary.scalar('dqn_reward',
self.dqn_reward_metric.result(),
step=episode_num)
self.dqn_reward_metric.reset_states()
print("Episode:", episode,
"score:", score, "memory length:",
len(self.replay_memory), "epsilon:", self.epsilon)
param_keys = ['epsilon']
param_values = [self.epsilon]
param_dictionary = dict(zip(param_keys, param_values))
break
# While loop rate
time.sleep(0.01)
# Update result and save model every 100 episodes
if self.train_mode:
if episode % 100 == 0:
self.model_path = 'stage' + str(
self.stage) + '_episode' + str(episode) + '.h5'
self.json_path = self.model_path.replace('.h5', '.json')
self.model.save(self.model_path)
with open(self.json_path, 'w') as outfile:
json.dump(param_dictionary, outfile)
#EPSILON CALCULATION
#self.epsilon = self.epsilon_min + (1.0 - self.epsilon_min) * math.exp(-1.0 * self.step_counter / self.epsilon_decay)
self.epsilon *= self.epsilon_decay
if (self.epsilon < self.epsilon_min):
self.epsilon = self.epsilon_min
def start_run(self):
print_info("preparing to start run")
# wait to receive sensor data from the environment (e.g., a simulator may need time to startup)
waiting_time = 0.0
waiting_period = 0.5
while not self.received_first_scan and rclpy.ok():
time.sleep(waiting_period)
rclpy.spin_once(self)
waiting_time += waiting_period
if waiting_time > 5.0:
self.get_logger().warning(
'still waiting to receive first sensor message from environment'
)
waiting_time = 0.0
# get the parameter robot_radius from the global costmap
parameters_request = GetParameters.Request(names=['robot_radius'])
parameters_response = self.call_service(
self.global_costmap_get_parameters_service_client,
parameters_request)
self.robot_radius = parameters_response.values[0].double_value
print_info("got robot radius")
# get deleaved reduced Voronoi graph from ground truth map
voronoi_graph = self.ground_truth_map.deleaved_reduced_voronoi_graph(
minimum_radius=2 * self.robot_radius).copy()
minimum_length_paths = nx.all_pairs_dijkstra_path(
voronoi_graph, weight='voronoi_path_distance')
minimum_length_costs = dict(
nx.all_pairs_dijkstra_path_length(voronoi_graph,
weight='voronoi_path_distance'))
costs = defaultdict(dict)
for i, paths_dict in minimum_length_paths:
for j in paths_dict.keys():
if i != j:
costs[i][j] = minimum_length_costs[i][j]
# in case the graph has multiple unconnected components, remove the components with less than two nodes
too_small_voronoi_graph_components = list(
filter(lambda component: len(component) < 2,
nx.connected_components(voronoi_graph)))
for graph_component in too_small_voronoi_graph_components:
voronoi_graph.remove_nodes_from(graph_component)
if len(voronoi_graph.nodes) < 2:
self.write_event(
self.get_clock().now(),
'insufficient_number_of_nodes_in_deleaved_reduced_voronoi_graph'
)
raise RunFailException(
"insufficient number of nodes in deleaved_reduced_voronoi_graph, can not generate traversal path"
)
# get greedy path traversing the whole graph starting from a random node
traversal_path_indices = list()
current_node = random.choice(list(voronoi_graph.nodes))
nodes_queue = set(
nx.node_connected_component(voronoi_graph, current_node))
while len(nodes_queue):
candidates = list(
filter(lambda node_cost: node_cost[0] in nodes_queue,
costs[current_node].items()))
candidate_nodes, candidate_costs = zip(*candidates)
next_node = candidate_nodes[int(np.argmin(candidate_costs))]
traversal_path_indices.append(next_node)
current_node = next_node
nodes_queue.remove(next_node)
# convert path of nodes to list of poses (as deque so they can be popped)
self.traversal_path_poses = deque()
for node_index in traversal_path_indices:
pose = Pose()
pose.position.x, pose.position.y = voronoi_graph.nodes[node_index][
'vertex']
q = pyquaternion.Quaternion(axis=[0, 0, 1],
radians=np.random.uniform(
-np.pi, np.pi))
pose.orientation = Quaternion(w=q.w, x=q.x, y=q.y, z=q.z)
self.traversal_path_poses.append(pose)
# publish the traversal path for visualization
traversal_path_msg = Path()
traversal_path_msg.header.frame_id = self.fixed_frame
traversal_path_msg.header.stamp = self.get_clock().now().to_msg()
for traversal_pose in self.traversal_path_poses:
traversal_pose_stamped = PoseStamped()
traversal_pose_stamped.header = traversal_path_msg.header
traversal_pose_stamped.pose = traversal_pose
traversal_path_msg.poses.append(traversal_pose_stamped)
self.traversal_path_publisher.publish(traversal_path_msg)
# pop the first pose from traversal_path_poses and set it as initial pose
self.initial_pose = PoseWithCovarianceStamped()
self.initial_pose.header.frame_id = self.fixed_frame
self.initial_pose.header.stamp = self.get_clock().now().to_msg()
self.initial_pose.pose.pose = self.traversal_path_poses.popleft()
self.initial_pose.pose.covariance = list(
self.initial_pose_covariance_matrix.flat)
self.num_goals = len(self.traversal_path_poses)
# set the position of the robot in the simulator
self.call_service(self.pause_physics_service_client, Empty.Request())
print_info("called pause_physics_service")
time.sleep(1.0)
robot_entity_state = EntityState(name=self.robot_entity_name,
pose=self.initial_pose.pose.pose)
set_entity_state_response = self.call_service(
self.set_entity_state_service_client,
SetEntityState.Request(state=robot_entity_state))
if not set_entity_state_response.success:
self.write_event(self.get_clock().now(),
'failed_to_set_entity_state')
raise RunFailException("could not set robot entity state")
print_info("called set_entity_state_service")
time.sleep(1.0)
self.call_service(self.unpause_physics_service_client, Empty.Request())
print_info("called unpause_physics_service")
time.sleep(1.0)
# ask lifecycle_manager to startup all its managed nodes
startup_request = ManageLifecycleNodes.Request(
command=ManageLifecycleNodes.Request.STARTUP)
startup_response: ManageLifecycleNodes.Response = self.call_service(
self.lifecycle_manager_service_client, startup_request)
if not startup_response.success:
self.write_event(self.get_clock().now(), 'failed_to_startup_nodes')
raise RunFailException("lifecycle manager could not startup nodes")
self.write_event(self.get_clock().now(), 'run_start')
self.run_started = True
self.send_goal()
def unpause_physics(self):
pause_future = self.unpause_sim.call_async(Empty.Request())
rclpy.spin_until_future_complete(self.node, pause_future)