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)