class World: def __init__(self, world_id, world_init_prop): self.name = Namespace(namespace='world/', name=world_id) self.__init_properties = world_init_prop self.launch = None self.__init_launch() self.__env = Environment(owner=self, properties=self.__init_properties) self.__env.launch() def __init_launch(self): self.launch = ROSLaunch() self.launch.start() def destroy(self): self.__env.stop()
class BaseVehicle(WheeledVehicle): """ Base class for all vehicles. Other vehicles inherit from this class and can use/overwrite its functions. It provides the following basic services: - /set_state: Set the vehicle state. - /set_speed_kph: Set the vehicles speed in kilometers per hour. - /set_loop: Set a closed loop trajectory from a certain node. - /set_destination: Set a trajectory to a certain destination node. - /toggle_simulation: Toggle the simulation of the vehicle on/off. The launch_sensor function can be called from child classes to launch the sensor nodes that are listed in their class variable self.sensors. """ def __init__(self, namespace, vehicle_id, simulation_rate, x=0., y=0., yaw=0., v=0.): """ Initialize class BaseVehicle. @param namespace: I{(string)} Namespace in which the vehicle node is started. @param vehicle_id: I{(int)} ID of the vehicle that is created. @param simulation_rate: I{(int)} Rate at which the vehicle is simulated (hz) @param x: I{(float)} x-coordinate at which the vehicle starts. @param y: I{(float)} y-coordinate at which the vehicle starts. @param yaw: I{(float)} Initial yaw of the vehicle. @param v: I{(float)} Initial velocity of the vehicle. """ self.launcher = ROSLaunch() self.launcher.start() self.namespace = namespace self.vehicle_id = int(vehicle_id) self.class_name = self.__class__.__name__ self.simulation_rate = simulation_rate # Set parameters of base vehicle to default values. self.simulate = False self.sensors = [] self.coms = [] self.x = x self.y = y self.yaw = yaw self.v = v self.cruising_speed = v self.axles_distance = 1.9 self.np_trajectory = [] self.commands = {} self.loop = False self.at_dest = False # Start the simulation loop in a separate thread. sim_thread = threading.Thread(target=self.simulation_loop) sim_thread.daemon = True sim_thread.start() # Register all services, pubs and subs last to prevent attempts to use # the services before the initialization of the vehicle is finished. self.pub_state = rospy.Publisher('/current_vehicle_state', VehicleState, queue_size=10) rospy.Service(self.namespace + 'set_state', SetVehicleState, self.handle_set_state) rospy.Service(self.namespace + 'set_speed_kph', SetSpeed, self.handle_set_speed_kph) rospy.Service(self.namespace + 'set_loop', SetLoop, self.handle_set_loop) rospy.Service(self.namespace + 'set_destination', SetDestination, self.handle_set_destination) rospy.Service(self.namespace + 'toggle_simulation', SetBool, self.handle_toggle_simulation) # rospy.wait_for_service(self.namespace + '/publish_com') # self.publish_com = rospy.ServiceProxy(self.namespace + 'publish_com', # PublishCom) def simulation_loop(self): """The simulation loop of the car.""" rate = rospy.Rate(self.simulation_rate) while not rospy.is_shutdown(): # print self.x, self.y, self.yaw, self.v # Simulate only if the simulate flat is set. if self.simulate: self.simulation_step() # Check if simulatio rate could be achieved or not. if rate.remaining() < rospy.Duration(0): rospy.logwarn("Simulation rate of vehicle " + "#%i " % self.vehicle_id + "could not be achieved.") rate.last_time = rospy.get_rostime() else: rate.sleep() def simulation_step(self): """Simulate one timestep of the car.""" # Find closest trajectory point, then set reference point some indices # ahead of the closest trajecctory point to imporove lateral controller # performance. Use this trajectory pose as reference pose. closest_ind = self.find_closest_trajectory_pose() ref_ind = closest_ind + numpy.round(self.v / 4) traj_len = len(self.np_trajectory[0]) print traj_len - 1, closest_ind if self.loop is True: ref_ind = ref_ind % traj_len else: if ref_ind > traj_len - 1: ref_ind = traj_len - 1 if closest_ind == traj_len - 1: self.at_dest = True else: ref_ind = closest_ind ref_state = self.np_trajectory[:, ref_ind] # set controll commands. self.set_control_commands(ref_state) # update vehicle state. self.update_vehicle_state() # publish vehicle state. vehicle_state = VehicleState(self.vehicle_id, self.class_name, self.x, self.y, self.yaw, self.v) self.pub_state.publish(vehicle_state) def find_closest_trajectory_pose(self): """ Find closest point to the current vehicle position in the trajectory. @return: The index of the closest trajectory point to the current vehicle position. """ np_state = numpy.array([[self.x], [self.y]]) temp_distance = numpy.sum((self.np_trajectory[0:2, :] - np_state)**2, axis=0) best_idx = numpy.argmin(temp_distance) return best_idx def set_control_commands(self, ref_state): """ Set the control commands, depending on the vehicles controler. @param ref_state: I{(numpy array)} Reference state [x, y, yaw] that the vehicle tries to reach. """ if not self.at_dest: self.commands['speed'] = self.cruising_speed else: self.commands['speed'] = 0.0 dx = ref_state[0] - self.x dy = ref_state[1] - self.y dx_v = numpy.cos(self.yaw) * dx + numpy.sin(self.yaw) * dy dy_v = -numpy.sin(self.yaw) * dx + numpy.cos(self.yaw) * dy dyaw_v = ref_state[2] - self.yaw # Correct yaw difference. dyaw_v 0..pi while dyaw_v > numpy.pi: dyaw_v -= 2 * numpy.pi while dyaw_v < -numpy.pi: dyaw_v += 2 * numpy.pi # Calculate steering command from dy_v, dx_v and dyaw_v steering_command = dy_v + dyaw_v * 1.5 / (1 + dx_v) # Compare with max steering angle if steering_command > 0.5: steering_command = 0.5 elif steering_command < -0.5: steering_command = -0.5 self.commands['steering_angle'] = steering_command def update_vehicle_state(self): """Update the vehicle state.""" sim_timestep = 1. / self.simulation_rate # Decompose v into x and y component. self.v = self.commands['speed'] vx = numpy.cos(self.yaw) * self.v vy = numpy.sin(self.yaw) * self.v # Update vehicles position self.x += vx * sim_timestep self.y += vy * sim_timestep self.yaw += ((self.v / self.axles_distance) * numpy.tan(self.commands['steering_angle']) * sim_timestep) # Make sure self.yaw is never negative. # self.yaw 0..2pi if self.yaw > 2 * numpy.pi: self.yaw = 0. elif self.yaw < 0.: self.yaw += 2 * numpy.pi def launch_sensors(self): """Launch and register the sensors used by the vehicle.""" # Go through sensor list. for sensor in self.sensors: # Launch sensor node. sensor_name = sensor.partition(' ')[0] subpub_name = sensor_name.lower() + '_readings' args = str(self.vehicle_id) + ' ' + sensor node = Node('sml_world', 'sensor.py', namespace=self.namespace, args=args, name=sensor_name.lower()) self.launcher.launch(node) # Register subscriptions for each of them. rospy.Subscriber(self.namespace + subpub_name, getattr(msgs, sensor_name + 'Readings'), getattr(self, 'process_' + subpub_name)) def launch_coms(self): """Launch and register the communications used by the vehicle.""" # Go through list of comunication. for com in self.coms: com_name = com.partition(' ')[0] subpub_name = com_name.lower() + '_com' args = str(self.vehicle_id) + ' ' + com node = Node('sml_world', 'communication.py', namespace=self.namespace, args=args, name=com_name.lower()) self.launcher.launch(node) # Register subscriptions for each of them. rospy.Subscriber(self.namespace + subpub_name, getattr(msgs, com_name + 'Com'), getattr(self, 'process_' + subpub_name)) def handle_set_state(self, req): """ Handle the set state request. @param req: I{(SetState)} Request of the service that sets the vehicle state. """ self.x = req.x self.y = req.y self.yaw = req.yaw self.v = req.v msg = "State of vehicle #%i successfully set." % self.vehicle_id return SetVehicleStateResponse(True, msg) def handle_set_speed_kph(self, req): """ Handle the set speed request. @param req: I{(SetSpeed)} Request of the service that sets the vehicles cruising speed in kmh. """ self.cruising_speed = req.speed / 3.6 msg = "Speed of vehicle #%i successfully set." % self.vehicle_id return SetSpeedResponse(True, msg) def handle_set_loop(self, req): """ Handle the set closed loop request. @param req: I{(SetLoop)} Request of the service that sets the vehicles closed loop trajectory. """ rospy.wait_for_service('/get_trajectory') try: get_traj = rospy.ServiceProxy('/get_trajectory', GetTrajectory) trajectory = get_traj(True, req.node_id, 0).trajectory except rospy.ServiceException, e: raise "Service call failed: %s" % e self.np_trajectory = to_numpy_trajectory(trajectory) self.loop = True self.at_dest = False msg = ("Closed loop trajectory of vehicle #%i " % self.vehicle_id + "successfully set.") return SetLoopResponse(True, msg)
class SmartWifi(BaseCom): ''' Communication class that acts through wifi, but has the limitation that it only connects to vehicles directly in front of or behind it. Thus, we will have two radars, one in each direction, which will sense when a vehicle is close by, then send a request to connect. We will stay connected to this vehicle so long as we retain this connection. ''' def __init__(self, vehicle_id, name, com_range): super(SmartWifi, self).__init__(vehicle_id) self.name = name self.yaw = 0. #unused self.com_range = float(com_range) self.launcher = ROSLaunch() self.launcher.start() self.front_con = DirectWifi(vehicle_id, 'front', speed_cb=self.light_turned_green, overpass_cb=self.reset_op_flag, abort_cb=self.stop_overtaking) self.back_con = DirectWifi(vehicle_id, 'back', speed_cb=self.max_speed_request, overpass_cb=self.handle_overpass) self.desired_speed = -1 # Will be updated on the first update_traj # Tracks all vehicles within range of the vehicle, useful for telling # safety of overtaking self.vehicles_within_range = [] self.trajectory = [] self.sub = rospy.Subscriber('/smart_wifi_request_' + str(vehicle_id), msgs.SmartWifiRequest, self.update_traj) self.commander = rospy.Publisher('/smart_wifi_commands_' + str(vehicle_id), msgs.SmartWifiCommand, queue_size=10) self.overtaking = False self.overtake_in_progress = False self.is_head_in_overtake = False def update_vehicle_state(self, ws): ''' Update vehicle state and publish results to smart_wifi ''' super(SmartWifi, self).update_vehicle_state(ws) self.handle_send_smartwifi_com() def set_speed(self, speed=None, reason='None'): if speed == None: speed = self.desired_speed self.send_command(2, speed, reason) def max_speed_request(self): ''' Returns max speed that this vehicle and all vehicles behind this vehicle can go ''' if self.overtaking: speed = self.desired_speed * 1.2 self.set_speed(speed, 'overtaking') return speed speed = min(self.desired_speed, self.front_con.get_max_speed()) self.set_speed(speed, 'traffic') return speed def light_turned_green(self, new_speed): speed_we_go = min(new_speed, self.desired_speed) self.back_con.speed_up_request(speed_we_go) self.set_speed(speed_we_go, 'green light') def update_traj(self, vehicle_request): ''' Obtain the trajectory from the vehicle, as well as process any requests. ''' # Convert the trajectory back to its original form from Pose2d self.trajectory = to_array_traj(vehicle_request.trajectory) self.desired_speed = vehicle_request.desired_speed if vehicle_request.request == 1: #Returns int traffic_head = self.handle_overpass( 0 ) # Traffic head is the leading vehicle that you need to overtake before if traffic_head != -1: # getting back to safety. self.head_of_traffic_id = traffic_head self.send_command(1, traffic_head, 'Safe to overtake') # Max speed will be the maximum of all of the vehicles in # front of this one if vehicle_request.request == 2: self.max_speed_request() # Start an overtake if vehicle_request.request == 3: self.start_overpass() # Vehicle has begun overtaking. # We will set overtake pass once we have actually passed the vehicle if vehicle_request.request == 4: self.overtaking = True self.overtake_pass = False self.max_speed_request() #Vehicle has finished overtaking. Unused for now. if vehicle_request.request == 5: self.stop_overtaking() # Light changed to green, let cars behind us know if vehicle_request.request == 6: self.light_turned_green(self.desired_speed) def launch_direct_wifis(self): ''' Launch the wifi modules which will speak directly to the to other vehicles. They will start unconnected to other vehicles. ''' self.front_con = DirectWifi(self.vehicle_id, 'front') self.back_con = DirectWifi(self.vehicle_id, 'back') def predict_trajectory(self, target, yaw, their_yaw): ''' After filtering, we want to tell if the other car is on our trajectory, meaning that it is somewhere in front of us. If it is we will launch our front connection, and it in turn will launch its back connection to us (DirectComs are a two way connection) Returns 2 if the vehicle is on our trajectory and is in front of us Returns 1 if the vehicle is on our trajectory and is probably behind us (less accurate than the forward looking one) Returns 0 otherwise ''' if abs(yaw - their_yaw) > 1: return 0 #Find the closest point on our path to our target's position min_index_target, min_traj_value = min( enumerate(self.trajectory), key=lambda x: ptdist(self.trajectory[x[0]], (target.x, target.y))) min_traj_diff = ptdist(min_traj_value, (target.x, target.y)) # Find our index on our own trajectory. The purpose of doing this # is that we only want to return true if there is a car in front # of us. So our index must be before the targets index of our # trajectory for us to return true. min_index_self = min( range(len(self.trajectory)), key=lambda i: ptdist(self.trajectory[i], self.pos)) if min_traj_diff < MAX_ERROR: if min_index_self < min_index_target and min_index_target - min_index_self < len( self.trajectory) / 2: return 2 my_x = self.trajectory[min_index_self][0] my_y = self.trajectory[min_index_self][1] back_traj = math.fabs( tan(yaw) - ((target.y - my_y) / (target.x - my_x))) if back_traj < MAX_ERROR: return 1 return 0 def stop_overtaking(self): self.overtaking = False self.overtake_pass = False self.head_of_traffic_id = -1 self.set_speed() self.send_command(3, -1, 'Stop overtaking') def reset_op_flag(self): rospy.logwarn('Resetting op flag') self.overtake_in_progress = False self.back_con.reset_overpass_flag() def filter_all_smartwifi_com(self, wc): ''' Filter to find vehicles within a pretermined range. If we find such a vehicle, we will test wheather it is on our trajectory. If it is, we will connect to it. ''' # Ensure self.pos is not (None,None) if wc.vehicle_id == self.vehicle_id or not self.pos[0]: #self.pos = (vs.x, vs.y) return dist = ptdist(self.pos, (wc.x, wc.y)) # We want to count the number of vehicles within our range for safety. if dist < CONNECTION_MIN_DIST and not wc.vehicle_id in self.vehicles_within_range: self.vehicles_within_range.append(wc.vehicle_id) if abs(self.yaw - wc.yaw) > 2.: self.back_con.stop_overtake() elif dist > CONNECTION_MIN_DIST and wc.vehicle_id in self.vehicles_within_range: self.vehicles_within_range.remove(wc.vehicle_id) # if we are able to merge back from overtaking, we will set overtake_pass # to true. That is, we have passed the head of traffic, and we can safely # merge back in. if self.overtaking and wc.vehicle_id == self.head_of_traffic_id: if ptdist(self.pos, (wc.x, wc.y)) < PASS_DIST: if self.vehicle_id == 2: rospy.logwarn('--------------- SETTTING TO TRUE') self.overtake_pass = True if ptdist( self.pos, (wc.x, wc.y)) > OVERTAKE_MERGE_DIST and self.overtake_pass: #Merge back into the original lane self.stop_overtaking() self.front_con.disconnect() # If we are connected to a vehicle, disconnect when we get out of range or when # we have overtaken them. If we are connected, we ignore all vehicles that are # not the one we are connected to. if self.front_con.is_connected: # Is this the vehicle we are connected to? if wc.vehicle_id == self.front_con.connect_id: # check_for_disconnect will disconnect the vehicle if it is too # far away. Will return true if vehicle did indeed disconnect self.front_con.check_for_disconnect(self.pos, (wc.x, wc.y)) return if self.back_con.is_connected: if wc.vehicle_id == self.back_con.connect_id: self.back_con.check_for_disconnect(self.pos, (wc.x, wc.y)) elif self.overtaking: self.stop_overtaking() if dist < CONNECTION_MIN_DIST: calc_traj = self.predict_trajectory(wc, self.yaw, wc.yaw) if calc_traj == 2 and not self.front_con.is_connected: self.front_con.establish_connection(wc.vehicle_id, 'back') #We will now update our global smartwifi to let the vehicle in front #of us to connect to us self.handle_send_smartwifi_com(wc.vehicle_id) self.max_speed_request() elif calc_traj == 1 and not self.back_con.is_connected: self.back_con.establish_connection(wc.vehicle_id, 'front') if self.overtake_in_progress: self.reset_op_flag() def handle_send_smartwifi_com(self, back_connection_id=-1): ''' Back connection signals to the vehicle with that id that their back connection should point to our forward connection to allow for two way communication. ''' self.pub_glob.publish(self.vehicle_id, self.pos[0], self.pos[1], self.yaw, back_connection_id) return srvs.SendSmartWifiComResponse(True, "Message sucessfully sent out.") def send_command(self, command, result, msg): self.commander.publish(command, result, msg) def handle_overpass(self, num_cars): ''' Returns whether an overpass is possible. There are two reasons that it is not: There are more than OVERPASS_SAFETY_MAX_CARS in front of the car requesting an overpass. There is another vehicle that the requesting car cannot see but the cars in front of them can. This function returns an integer: -1 means that it is not safe to pass Any other value means is the vehicle_id of the frontmost car. ''' if num_cars == OVERPASS_SAFETY_MAX_CARS or self.overtaking or self.overtake_in_progress: return -1 if self.front_con.connect_id != None: # Three cars would be one behind, one in front and # one approaching in the other lane, thus unsafe. # NOTE: This method does not work on multiple lane # roads. We have to be more clever there..... if len(self.vehicles_within_range) >= 3: return -1 else: ret = self.front_con.request_overpass(num_cars + 1) else: if len(self.vehicles_within_range) >= 2: return -1 else: ret = self.vehicle_id # We are the top head, continously check for incoming vehicles. self.is_head_in_overtake = True if ret != -1: self.overtake_in_progress = True return ret def start_overpass(self): pass
class BaseVehicle(WheeledVehicle): """ Base class for all vehicles. Other vehicles inherit from this class and can use/overwrite its functions. It provides the following basic services: - /set_state: Set the vehicle state. - /set_speed_kph: Set the vehicles speed in kilometers per hour. - /set_loop: Set a closed loop trajectory from a certain node. - /set_destination: Set a trajectory to a certain destination node. - /toggle_simulation: Toggle the simulation of the vehicle on/off. The launch_sensor function can be called from child classes to launch the sensor nodes that are listed in their class variable self.sensors. """ def __init__(self, namespace, vehicle_id, simulation_rate, x=0., y=0., yaw=0., v=0.): """ Initialize class BaseVehicle. @param namespace: I{(string)} Namespace in which the vehicle node is started. @param vehicle_id: I{(int)} ID of the vehicle that is created. @param simulation_rate: I{(int)} Rate at which the vehicle is simulated (hz) @param x: I{(float)} x-coordinate at which the vehicle starts. @param y: I{(float)} y-coordinate at which the vehicle starts. @param yaw: I{(float)} Initial yaw of the vehicle. @param v: I{(float)} Initial velocity of the vehicle. """ self.launcher = ROSLaunch() self.launcher.start() self.namespace = namespace self.vehicle_id = int(vehicle_id) self.class_name = self.__class__.__name__ self.simulation_rate = simulation_rate # Set parameters of base vehicle to default values. self.simulate = False self.sensors = [] self.coms = [] self.x = x self.y = y self.yaw = yaw self.v = v self.cruising_speed = v self.axles_distance = 1.9 self.np_trajectory = [] self.commands = {} self.loop = False self.at_dest = False # Start the simulation loop in a separate thread. sim_thread = threading.Thread(target=self.simulation_loop) sim_thread.daemon = True sim_thread.start() # Register all services, pubs and subs last to prevent attempts to use # the services before the initialization of the vehicle is finished. self.pub_state = rospy.Publisher('/current_vehicle_state', VehicleState, queue_size=10) rospy.Service(self.namespace + 'set_state', SetVehicleState, self.handle_set_state) rospy.Service(self.namespace + 'set_speed_kph', SetSpeed, self.handle_set_speed_kph) rospy.Service(self.namespace + 'set_loop', SetLoop, self.handle_set_loop) rospy.Service(self.namespace + 'set_destination', SetDestination, self.handle_set_destination) rospy.Service(self.namespace + 'toggle_simulation', SetBool, self.handle_toggle_simulation) # rospy.wait_for_service(self.namespace + '/publish_com') # self.publish_com = rospy.ServiceProxy(self.namespace + 'publish_com', # PublishCom) def simulation_loop(self): """The simulation loop of the car.""" rate = rospy.Rate(self.simulation_rate) while not rospy.is_shutdown(): # print self.x, self.y, self.yaw, self.v # Simulate only if the simulate flat is set. if self.simulate: self.simulation_step() # Check if simulatio rate could be achieved or not. if rate.remaining() < rospy.Duration(0): rospy.logwarn("Simulation rate of vehicle " + "#%i " % self.vehicle_id + "could not be achieved.") rate.last_time = rospy.get_rostime() else: rate.sleep() def simulation_step(self): """Simulate one timestep of the car.""" # Find closest trajectory point, then set reference point some indices # ahead of the closest trajecctory point to imporove lateral controller # performance. Use this trajectory pose as reference pose. closest_ind = self.find_closest_trajectory_pose() ref_ind = closest_ind + numpy.round(self.v / 4) traj_len = len(self.np_trajectory[0]) print traj_len-1, closest_ind if self.loop is True: ref_ind = ref_ind % traj_len else: if ref_ind > traj_len-1: ref_ind = traj_len-1 if closest_ind == traj_len-1: self.at_dest = True else: ref_ind = closest_ind ref_state = self.np_trajectory[:, ref_ind] # set controll commands. self.set_control_commands(ref_state) # update vehicle state. self.update_vehicle_state() # publish vehicle state. vehicle_state = VehicleState(self.vehicle_id, self.class_name, self.x, self.y, self.yaw, self.v) self.pub_state.publish(vehicle_state) def find_closest_trajectory_pose(self): """ Find closest point to the current vehicle position in the trajectory. @return: The index of the closest trajectory point to the current vehicle position. """ np_state = numpy.array([[self.x], [self.y]]) temp_distance = numpy.sum( (self.np_trajectory[0:2, :] - np_state) ** 2, axis=0) best_idx = numpy.argmin(temp_distance) return best_idx def set_control_commands(self, ref_state): """ Set the control commands, depending on the vehicles controler. @param ref_state: I{(numpy array)} Reference state [x, y, yaw] that the vehicle tries to reach. """ if not self.at_dest: self.commands['speed'] = self.cruising_speed else: self.commands['speed'] = 0.0 dx = ref_state[0] - self.x dy = ref_state[1] - self.y dx_v = numpy.cos(self.yaw) * dx + numpy.sin(self.yaw) * dy dy_v = -numpy.sin(self.yaw) * dx + numpy.cos(self.yaw) * dy dyaw_v = ref_state[2] - self.yaw # Correct yaw difference. dyaw_v 0..pi while dyaw_v > numpy.pi: dyaw_v -= 2*numpy.pi while dyaw_v < -numpy.pi: dyaw_v += 2*numpy.pi # Calculate steering command from dy_v, dx_v and dyaw_v steering_command = dy_v + dyaw_v * 1.5 / (1 + dx_v) # Compare with max steering angle if steering_command > 0.5: steering_command = 0.5 elif steering_command < -0.5: steering_command = -0.5 self.commands['steering_angle'] = steering_command def update_vehicle_state(self): """Update the vehicle state.""" sim_timestep = 1. / self.simulation_rate # Decompose v into x and y component. self.v = self.commands['speed'] vx = numpy.cos(self.yaw) * self.v vy = numpy.sin(self.yaw) * self.v # Update vehicles position self.x += vx * sim_timestep self.y += vy * sim_timestep self.yaw += ((self.v / self.axles_distance) * numpy.tan(self.commands['steering_angle']) * sim_timestep) # Make sure self.yaw is never negative. # self.yaw 0..2pi if self.yaw > 2*numpy.pi: self.yaw = 0. elif self.yaw < 0.: self.yaw += 2*numpy.pi def launch_sensors(self): """Launch and register the sensors used by the vehicle.""" # Go through sensor list. for sensor in self.sensors: # Launch sensor node. sensor_name = sensor.partition(' ')[0] subpub_name = sensor_name.lower()+'_readings' args = str(self.vehicle_id)+' '+sensor node = Node('sml_world', 'sensor.py', namespace=self.namespace, args=args, name=sensor_name.lower()) self.launcher.launch(node) # Register subscriptions for each of them. rospy.Subscriber(self.namespace + subpub_name, getattr(msgs, sensor_name+'Readings'), getattr(self, 'process_'+subpub_name)) def launch_coms(self): """Launch and register the communications used by the vehicle.""" # Go through list of comunication. for com in self.coms: com_name = com.partition(' ')[0] subpub_name = com_name.lower()+'_com' args = str(self.vehicle_id)+' '+com node = Node('sml_world', 'communication.py', namespace=self.namespace, args=args, name=com_name.lower()) self.launcher.launch(node) # Register subscriptions for each of them. rospy.Subscriber(self.namespace + subpub_name, getattr(msgs, com_name+'Com'), getattr(self, 'process_'+subpub_name)) def handle_set_state(self, req): """ Handle the set state request. @param req: I{(SetState)} Request of the service that sets the vehicle state. """ self.x = req.x self.y = req.y self.yaw = req.yaw self.v = req.v msg = "State of vehicle #%i successfully set." % self.vehicle_id return SetVehicleStateResponse(True, msg) def handle_set_speed_kph(self, req): """ Handle the set speed request. @param req: I{(SetSpeed)} Request of the service that sets the vehicles cruising speed in kmh. """ self.cruising_speed = req.speed / 3.6 msg = "Speed of vehicle #%i successfully set." % self.vehicle_id return SetSpeedResponse(True, msg) def handle_set_loop(self, req): """ Handle the set closed loop request. @param req: I{(SetLoop)} Request of the service that sets the vehicles closed loop trajectory. """ rospy.wait_for_service('/get_trajectory') try: get_traj = rospy.ServiceProxy('/get_trajectory', GetTrajectory) trajectory = get_traj(True, req.node_id, 0).trajectory except rospy.ServiceException, e: raise "Service call failed: %s" % e self.np_trajectory = to_numpy_trajectory(trajectory) self.loop = True self.at_dest = False msg = ("Closed loop trajectory of vehicle #%i " % self.vehicle_id + "successfully set.") return SetLoopResponse(True, msg)
class ScenarioLauncher: def __init__(self): print "I am ScenarioLauncher in python !!" def start_launcher(self): self.launcher = ROSLaunch() self.launcher.start() def stop_launcher(self): self.launcher.stop() time.sleep(60) def runGazeboServer(self, Scenarin_folder): arguments = "-u " + Scenarin_folder + "/scenarioEnv.world" # -u starts the simulation in pause mode node = ROSNode( "gazebo_ros", "gzserver", name="gazebo", args=arguments, output="screen", respawn="false" ) # name="gazebo" in order for replay and grader to work (name space) self.launcher.launch(node) time.sleep(3) return True def runGazeboClient(self): node = ROSNode("gazebo_ros", "gzclient", name="gazebo_client", output="screen", respawn="false") self.launcher.launch(node) time.sleep(3) return True def setScenarioEnv(self, Scenarin_folder): rospy.set_param('/use_sim_time', True) #synchronizing ros to gazebo rospack = rospkg.RosPack() srvss_pkg_path = rospack.get_path('smartest') os.environ[ "GAZEBO_MODEL_PATH"] = srvss_pkg_path + "/world_components_models/:" + Scenarin_folder + "/scenarioSystemModels/" return True def launch_platform_controls_spawner(self): #used by the srvss_dummy return True def launch_platform_controls_unspawner(self): #used by the srvss_dummy return True def launch_WPdriver(self, Scenarin_folder): def run_node(package_name, node_type, node_nickname, params_file, remap_file, arguments): def read_remaps_from_file(filename): from ast import literal_eval with open(filename) as f: mainlist = [list(literal_eval(line)) for line in f] return mainlist def read_params_from_file(filename, namespace): paramlist = rosparam.load_file(filename, default_namespace=namespace, verbose=True) for params, ns in paramlist: rosparam.upload_params(ns, params) if params_file != None: read_params_from_file(params_file, node_nickname) remaps_list = None if remap_file != None: remaps_list = read_remaps_from_file(remap_file) node = ROSNode(package_name, node_type, name=node_nickname, remap_args=remaps_list, args=arguments, respawn=True, output="screen") self.launcher.launch(node) #rospack = rospkg.RosPack() #robil2conf_pkg_path = rospack.get_path('robil2conf') #robil2conf_yaml = robil2conf_pkg_path +"/configuration.yaml" #paramlist=rosparam.load_file(robil2conf_yaml, default_namespace='' ,verbose=True) #for params, ns in paramlist: # rosparam.upload_params(ns,params) #<!-- == GENERAL ===== --> print "Loading GENERAL !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" arguments = "ODOM GPS 0.000 0.000 0.000 0.000 0.000 0.000 \ ODOM INS 0.266 0.155 -1.683 0.000 0.000 0.000 \ ODOM IBEO 0.310 0.170 -0.230 0.000 0.215 0.149 \ ODOM TRACKS_BOTTOM 0.000 0.000 -2.260 0.000 0.000 0.000 \ ODOM ROBOT_CENTER -0.380 0.155 -1.683 0.000 0.000 0.000" node = ROSNode("robil2tf", "robil2TF_node", name="robil2TF_publisher", args=arguments, output="screen", respawn="true") self.launcher.launch(node) node = ROSNode("robil2tf", "world_gazebo_to_WORLD_TF_pub.py", name="world_gazebo_to_WORLD_TF_pub", output="screen", respawn="true") self.launcher.launch(node) # ================ --> #<!-- == SENSORS INTERFACEs == --> node = ROSNode("shiffon2ros", "shiffon2ros_node", name="ipon2ros", args="127.0.0.1", output="screen") self.launcher.launch(node) node = ROSNode("sick_ldmrs", "sickldmrs.py", name="ibeo2ros", args="127.0.0.1", output="screen") self.launcher.launch(node) time.sleep(3) # ================ --> #<!-- == PLATFORM INTERFACE == --> node = ROSNode("lli", "lli_node", name="ros2qinetiq", args="127.0.0.1", output="screen", respawn="true") self.launcher.launch(node) # ================ --> # == MONITORING == --> print "Loading MONITORING !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" node = ROSNode("ssm", "ssm_fsm_states_tracker_node", name="ssm_fsm_states_tracker_node", output="screen") self.launcher.launch(node) node = ROSNode("ssm", "ssm_heartbeat_monitor_node", name="ssm_heartbeat_monitor_node", output="screen") self.launcher.launch(node) node = ROSNode("ssm", "ssm_node", name="ssm_node", output="screen") self.launcher.launch(node) # ================ --> #<!-- == LOCALIZATION == --> print "Loading LOCALIZATION node !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" node = ROSNode("loc", "loc_node", name="loc_node", output="screen", respawn="true") self.launcher.launch(node) time.sleep(1) # ================ --> # == PERCEPTION == --> print "Loading PERCEPTION !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" node = ROSNode("per", "per_node", name="per_node", output="screen", respawn="true") self.launcher.launch(node) #<!-- == OPERATOR CONTROL UNIT == --> node = ROSNode("ocu", "ocu_node", name="ocu_node", output="screen") self.launcher.launch(node) # ================ --> # -- MISSION CONTROL --> print "Loading MISSION CONTRO !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" node = ROSNode("smme", "smme_node", name="smme_node", output="screen", respawn="true") self.launcher.launch(node) node = ROSNode("wsm", "wsm_node", name="wsm_node", output="screen", respawn="true") self.launcher.launch(node) # ================ --> #<!-- == LOW LEVEL CONTROL == --> print "Loading LOW LEVEL CONTROL node !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" node = ROSNode("llc", "llc_node", name="llc_node", output="screen", respawn="true") self.launcher.launch(node) time.sleep(3) #<!-- == PATH PLANING + WAY POINT DRIVER == --> print "Loading PATH PLANING + WAY POINT DRIVER !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" rospack = rospkg.RosPack() pp_pkg_path = rospack.get_path('pp') run_node("navex_navigation", "navex_navigation_node", "navex_navigation", pp_pkg_path + "/launch/navex.yaml", pp_pkg_path + "/launch/navex.remap", '') run_node("pp", "pp_node", "pp_node", pp_pkg_path + "/launch/pp_node.yaml", pp_pkg_path + "/launch/pp_node.remap", '') run_node("wpd", "wpd_node", "wpd_node", None, None, '') # ================ --> #<!-- == Navigation Mission Load == --> arguments = "-d 20 " + Scenarin_folder + "/scenarioMission.bag" node = ROSNode("rosbag", "play", name="rosbag_Mission_node", output="screen", respawn="true", args=arguments) self.launcher.launch(node) # ================ --> def launch_robot_tf_broadcaster(self): node = ROSNode("bobcat_model", "world_to_body_TF_pub.py", name="world_to_body_TF_pub", output="screen", respawn="true") self.launcher.launch(node) time.sleep(3) rospack = rospkg.RosPack() bobcat_pkg_path = rospack.get_path('bobcat_model') urdf_file = bobcat_pkg_path + "/urdf_models/bobcat_tracked_urdf_for_dynamic_arm.URDF" robot_urdf_file = open(urdf_file) robot_urdf = robot_urdf_file.read() rospy.set_param("/robot_description", robot_urdf) remaping = [("/joint_states", "/Sahar/joint_states")] node = ROSNode("robot_state_publisher", "robot_state_publisher", name="robot_state_broadcaster_node", remap_args=remaping, output="screen", respawn="false") self.launcher.launch(node) time.sleep(3) def launch_recorder(self, Scenarin_folder): arguments = "-O " + Scenarin_folder + "/scen_rec.bag --all --exclude /SENSORS/CAM/(.*)|/SENSORS/IBEO/(.*)|/Sahar/(.*)|/PER/Map|/WSM/(.*)|/flea3/(.*)|/right_sick/(.*)|/left_sick/(.*)|/gazebo/model_states|/clock" # arguments = "-O " + Scenarin_folder + "/scen_rec.bag --all --exclude /SENSORS/(.*)|/LOC/Velocity|/Sahar/(.*)|/OCU/(.*)|/LLC/(.*)|/PER/(.*)|/WPD/(.*)|/WSM/(.*)|/flea3/(.*)|/right_sick/(.*)|/left_sick/(.*)|/heartbeat|/gazebo/model_states|/clock" # arguments = "-O " + Scenarin_folder + "/scen_rec.bag --all --exclude /SENSORS/(.*)|/LOC/Pose|/LOC/Velocity|/Sahar/(.*)|/OCU/(.*)|/LLC/(.*)|/PER/(.*)|/WPD/(.*)|/WSM/(.*)|/flea3/(.*)|/right_sick/(.*)|/left_sick/(.*)|/heartbeat|/gazebo/model_states|/clock" node = ROSNode("rosbag", "record", name="rosbag_recorde_node", args=arguments) self.launcher.launch(node) time.sleep(3) def launch_grader(self, Scenarin_folder): arguments = Scenarin_folder + " " + Scenarin_folder + "/scen.SFV" node = ROSNode("smartest", "grader_node", name="grader_node", output="screen", args=arguments) self.launcher.launch(node) time.sleep(3) def Gazebo_UnPause(self): print " i am inside Gazebo_UnPause !!!!!!!!! " name = '/gazebo/unpause_physics' msg_class = std_srvs.srv.Empty() rospy.wait_for_service(name) try: service = rospy.ServiceProxy(name, msg_class) resp1 = service() return True except rospy.ServiceException, e: print "Service call failed: %s" % e return True
class ScenarioLauncher: def __init__(self): print "I am ScenarioLauncher in python !!" def start_launcher(self): self.launcher = ROSLaunch() self.launcher.start() def stop_launcher(self): self.launcher.stop() time.sleep(10) def runGazeboServer(self, Scenarin_folder): arguments = "-u " + Scenarin_folder + "/scenarioEnv.world" # -u starts the simulation in pause mode node = ROSNode( "gazebo_ros", "gzserver", name="gazebo", args=arguments, output="screen", respawn="false" ) # name="gazebo" in order for replay and grader to work (name space) self.launcher.launch(node) time.sleep(3) return True def runGazeboClient(self): node = ROSNode("gazebo_ros", "gzclient", name="gazebo_client", output="screen", respawn="false") self.launcher.launch(node) time.sleep(3) return True def setScenarioEnv(self, Scenarin_folder): rospy.set_param('/use_sim_time', True) #synchronizing ros to gazebo rospack = rospkg.RosPack() smartest_pkg_path = rospack.get_path('smartest') os.environ[ "GAZEBO_MODEL_PATH"] = smartest_pkg_path + "/world_components_models/:" + Scenarin_folder + "/scenarioSystemModels/" srvss_bobcat_pkg_path = rospack.get_path('srvss_bobcat') urdf_file = srvss_bobcat_pkg_path + "/urdf/BOBCAT.URDF" robot_urdf_file = open(urdf_file) robot_urdf = robot_urdf_file.read() rospy.set_param("/robot_description", robot_urdf) return True def launch_platform_controls_spawner(self): rospack = rospkg.RosPack() srvss_bobcat_pkg_path = rospack.get_path('srvss_bobcat') srvss_bobcat_controllers_yaml = srvss_bobcat_pkg_path + "/controllers/srvss_bobcat_controllers.yaml" paramlist = rosparam.load_file(srvss_bobcat_controllers_yaml, default_namespace='', verbose=True) for params, ns in paramlist: rosparam.upload_params(ns, params) # if the controllers do not load it possible to increase the time of waiting for the server in the spawner # it located in /home/userws3/gz_ros_cws/src/ros_control/controller_manager/scripts node = ROSNode("controller_manager", "spawner", name="platform_controllers_spawner", namespace="/srvss_bobcat", output="screen", respawn="false", args="joint_state_controller \ front_right_wheel_velocity_controller \ front_left_wheel_velocity_controller \ back_right_wheel_velocity_controller \ back_left_wheel_velocity_controller") self.launcher.launch(node) time.sleep(10) node = ROSNode("srvss_bobcat", "srvss_bobcat_rate2effort_node", name="srvss_bobcat_RateToEffort_node", cwd="node", output="screen") self.launcher.launch(node) time.sleep(3) def launch_platform_controls_unspawner(self): node = ROSNode("controller_manager", "unspawner", name="platform_controllers_unspawner", namespace="/srvss_bobcat", output="screen", respawn="false", args="joint_state_controller \ front_right_wheel_velocity_controller \ front_left_wheel_velocity_controller \ back_right_wheel_velocity_controller \ back_left_wheel_velocity_controller") self.launcher.launch(node) time.sleep(10) def launch_WPdriver(self, Scenarin_folder): arguments = "-file " + Scenarin_folder + "/scenarioMission.txt" node = ROSNode("srvss_wp_driver", "srvss_wp_driver_node", name="srvss_wp_driver_node", args=arguments, respawn="false") # output="screen" self.launcher.launch(node) time.sleep(3) def launch_robot_tf_broadcaster(self): remaping = [("/joint_states", "/srvss_bobcat/joint_states")] node = ROSNode("robot_state_publisher", "robot_state_publisher", name="robot_state_broadcaster_node", remap_args=remaping, output="screen", respawn="false") self.launcher.launch(node) time.sleep(3) node = ROSNode("srvss_bobcat", "world_to_bobcat_tf_broadcaster_node", name="worltd_to_bobcat_tf_broadcaster_node", output="screen", respawn="false") self.launcher.launch(node) time.sleep(3) node = ROSNode("tf", "static_transform_publisher", name="sick_link_tf_broadcaster_node", args="1 0 0.2 0 0 0 body front_sick 100", output="screen", respawn="false") self.launcher.launch(node) time.sleep(3) def launch_recorder(self, Scenarin_folder): arguments = "-a -O " + Scenarin_folder + "/scen_rec.bag" node = ROSNode("rosbag", "record", name="rosbag_recorde_node", args=arguments) self.launcher.launch(node) time.sleep(3) def launch_grader(self, Scenarin_folder): arguments = Scenarin_folder + " " + Scenarin_folder + "/scen.SFV" print "I am hear arguments = " + arguments node = ROSNode("smartest", "grader_node", name="grader_node", output="screen", args=arguments) self.launcher.launch(node) time.sleep(3) def Gazebo_UnPause(self): name = '/gazebo/unpause_physics' msg_class = std_srvs.srv.Empty() print "wait for service " + name rospy.wait_for_service(name) print "done wating" try: service = rospy.ServiceProxy(name, msg_class) resp1 = service() return True except rospy.ServiceException, e: print "Service call failed: %s" % e return True
class ScenarioLauncher: def __init__(self): print "I am ScenarioLauncher in Python_robil2_bobtank !!" def start_launcher(self): self.launcher = ROSLaunch() self.launcher.start() def stop_launcher(self): self.launcher.stop() time.sleep(60) def runGazeboServer(self, Scenarin_folder): arguments = "-u " + Scenarin_folder + "/scenarioEnv.world" # -u starts the simulation in pause mode node = ROSNode("gazebo_ros","gzserver",name="gazebo" ,args=arguments ,output="screen" , respawn="false") # name="gazebo" in order for replay and grader to work (name space) self.launcher.launch(node) time.sleep(3) return True def runGazeboClient(self): node = ROSNode("gazebo_ros", "gzclient",name="gazebo_client" ,output="screen", respawn="false") self.launcher.launch(node) time.sleep(3) return True def setScenarioEnv(self,Scenarin_folder): rospy.set_param('/use_sim_time',True) #synchronizing ros to gazebo rospack = rospkg.RosPack() srvss_pkg_path = rospack.get_path('SRVSS') os.environ["GAZEBO_MODEL_PATH"] = srvss_pkg_path+"/world_components_models/:" + Scenarin_folder + "/scenarioSystemModels/" bobcat_pkg_path = rospack.get_path('bobtank') urdf_file = bobcat_pkg_path +"/urdf/BOBCAT_sdf_model_new.URDF" robot_urdf_file = open(urdf_file) robot_urdf = robot_urdf_file.read() rospy.set_param("/robot_description", robot_urdf ) return True def launch_platform_controls_spawner(self): rospack = rospkg.RosPack() bobcat_pkg_path = rospack.get_path('bobtank') bobcat_controllers_yaml = bobcat_pkg_path +"/config/bobcat_with_trucks_gazebo_control.yaml" paramlist=rosparam.load_file(bobcat_controllers_yaml, default_namespace='' ,verbose=True) for params, ns in paramlist: rosparam.upload_params(ns,params) # if the controllers do not load it possible to increase the time of waiting for the server in the spawner # it located in /home/userws3/gz_ros_cws/src/ros_control/controller_manager/scripts node = ROSNode("controller_manager", "spawner", name="platform_controllers_spawner" ,namespace="/Sahar", output="screen", respawn="false" , args="joint_state_controller \ right_wheel_velocity_controller \ left_wheel_velocity_controller \ loader_position_controller \ supporter_position_controller \ brackets_position_controller") self.launcher.launch(node) time.sleep(10) node = ROSNode("rate2effort", "rate2effort_for_tracks", name="rate2effort_for_tracks_node", cwd="node", output="screen") self.launcher.launch(node) time.sleep(3) def launch_platform_controls_unspawner(self): node = ROSNode("controller_manager", "unspawner" ,name="platform_controllers_unspawner" ,namespace="/Sahar", output="screen", respawn="false" , args="joint_state_controller \ right_wheel_velocity_controller \ left_wheel_velocity_controller \ loader_position_controller \ supporter_position_controller \ brackets_position_controller") self.launcher.launch(node) time.sleep(10) def launch_WPdriver(self,Scenarin_folder): # == GENERAL == --> rospack = rospkg.RosPack() robil2conf_pkg_path = rospack.get_path('robil2conf') robil2conf_yaml = robil2conf_pkg_path +"/configuration.yaml" paramlist=rosparam.load_file(robil2conf_yaml, default_namespace='' ,verbose=True) for params, ns in paramlist: rosparam.upload_params(ns,params) node = ROSNode("robil2TF", "robil2TF_node",name="robil2TF_publishe",output="screen" args="ODOM GPS 0.000 0.000 0.000 0.000 0.000 0.000 \ ODOM INS 0.266 0.155 -1.683 0.000 0.000 0.000 \ ODOM IBEO 0.256 0.169 -0.231 0.005 0.201 0.107 \ ODOM TRACKS_BOTTOM 0.000 0.000 -2.323 0.000 0.000 0.000 \ ODOM ROBOT_CENTER -0.380 0.155 -1.683 0.000 0.000 0.000 \ ODOM body 0.616 0.155 -2.133 0.000 0.000 0.000") self.launcher.launch(node) time.sleep(3) # == MONITORING == --> print "Loading MONITORING !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" node = ROSNode("ssm", "ssm_fsm_states_tracker_node",name="ssm_fsm_states_tracker_node",output="screen") self.launcher.launch(node) node = ROSNode("ssm", "ssm_heartbeat_monitor_node",name="ssm_heartbeat_monitor_node",output="screen") self.launcher.launch(node) node = ROSNode("ssm", "ssm_node",name="ssm_node",output="screen") self.launcher.launch(node) #time.sleep(3) # ================ --> # == PERCEPTION == --> #print "Loading PERCEPTION-sensors_node !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" -- Not Working therefore stuck the loading process, not critical for robil2 #node = ROSNode("sensors", "sensors_node",name="sensors_node",output="screen",respawn="true") #self.launcher.launch(node) print "Loading PERCEPTION-iedsim_node !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" node = ROSNode("iedsim", "iedsim_node",name="iedsim_node",output="screen",respawn="true") self.launcher.launch(node) print "Loading PERCEPTION-per_node !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" node = ROSNode("per", "per_node",name="per_node",output="screen",respawn="true") self.launcher.launch(node) print "Loading PERCEPTION-loc_node !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" node = ROSNode("loc", "loc_node",name="loc_node",output="screen",respawn="true") self.launcher.launch(node) time.sleep(3) # ================ --> # -- MISSION CONTROL --> print "Loading MISSION CONTRO !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" node = ROSNode("smme", "smme_node",name="smme_node",output="screen",respawn="true") self.launcher.launch(node) node = ROSNode("wsm", "wsm_node",name="wsm_node",output="screen",respawn="true") self.launcher.launch(node) node = ROSNode("ocu", "ocu_node",name="ocu_node",output="screen") self.launcher.launch(node) # ================ --> #-- == NAVIGATION == --> print "Loading NAVIGATION !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" rospack = rospkg.RosPack() pp_pkg_path = rospack.get_path('pp') costmap_common_params = pp_pkg_path +"/params/costmap_common_params.yaml" paramlist=rosparam.load_file(costmap_common_params, default_namespace='move_base/global_costmap' ,verbose=True) for params, ns in paramlist: rosparam.upload_params(ns,params) paramlist=rosparam.load_file(costmap_common_params, default_namespace='move_base/local_costmap' ,verbose=True) for params, ns in paramlist: rosparam.upload_params(ns,params) local_costmap_params = pp_pkg_path +"/params/local_costmap_params.yaml" paramlist=rosparam.load_file(local_costmap_params, default_namespace='move_base' ,verbose=True) for params, ns in paramlist: rosparam.upload_params(ns,params) global_costmap_params = pp_pkg_path +"/params/global_costmap_params.yaml" paramlist=rosparam.load_file(global_costmap_params, default_namespace='move_base' ,verbose=True) for params, ns in paramlist: rosparam.upload_params(ns,params) base_local_planner_params = pp_pkg_path +"/params/base_local_planner_params.yaml" paramlist=rosparam.load_file(base_local_planner_params, default_namespace='move_base' ,verbose=True) for params, ns in paramlist: rosparam.upload_params(ns,params) node = ROSNode("move_base", "move_base",name="move_base",output="screen") self.launcher.launch(node) node = ROSNode("pp", "pp_node",name="pp_node",output="screen") self.launcher.launch(node) node = ROSNode("wpd", "wpd_node",name="wpd_node",output="screen") self.launcher.launch(node) node = ROSNode("llc", "llc_node",name="llc_node",output="screen",respawn="true") self.launcher.launch(node) time.sleep(3) arguments = "-d 40 "+ Scenarin_folder + "/scenarioMission.bag" node = ROSNode("rosbag", "play",name="rosbag_Mission_node",output="screen",respawn="true", args=arguments) self.launcher.launch(node) # ================ --> def launch_robot_tf_broadcaster(self): remaping = [ ("/joint_states" , "/Sahar/joint_states") ] node = ROSNode("robot_state_publisher", "robot_state_publisher",name="robot_state_broadcaster_node", remap_args=remaping ,output="screen", respawn="false") self.launcher.launch(node) time.sleep(3) def launch_recorder(self, Scenarin_folder): arguments = "-O " + Scenarin_folder + "/scen_rec.bag --all --exclude /SENSORS/CAM/(.*)|/SENSORS/IBEO/(.*)|/Sahar/(.*)|/PER/Map|/WSM/(.*)|/flea3/(.*)|/right_sick/(.*)|/left_sick/(.*)|/gazebo/model_states|/clock" # arguments = "-O " + Scenarin_folder + "/scen_rec.bag --all --exclude /SENSORS/(.*)|/LOC/Velocity|/Sahar/(.*)|/OCU/(.*)|/LLC/(.*)|/PER/(.*)|/WPD/(.*)|/WSM/(.*)|/flea3/(.*)|/right_sick/(.*)|/left_sick/(.*)|/heartbeat|/gazebo/model_states|/clock" # arguments = "-O " + Scenarin_folder + "/scen_rec.bag --all --exclude /SENSORS/(.*)|/LOC/Pose|/LOC/Velocity|/Sahar/(.*)|/OCU/(.*)|/LLC/(.*)|/PER/(.*)|/WPD/(.*)|/WSM/(.*)|/flea3/(.*)|/right_sick/(.*)|/left_sick/(.*)|/heartbeat|/gazebo/model_states|/clock" node = ROSNode("rosbag", "record", name="rosbag_recorde_node", args=arguments) self.launcher.launch(node) time.sleep(3) def launch_grader(self, Scenarin_folder): arguments = Scenarin_folder + " " + Scenarin_folder+"/scen.SFV" node = ROSNode("SRVSS", "grader_node", name="grader_node", output="screen", args=arguments) self.launcher.launch(node) time.sleep(3) def Gazebo_UnPause(self): name = '/gazebo/unpause_physics' msg_class = std_srvs.srv.Empty() print "wait for service " + name rospy.wait_for_service(name) print "done wating" try: service = rospy.ServiceProxy(name, msg_class) resp1 = service() return True except rospy.ServiceException, e: print "Service call failed: %s"%e return True
class BITE: def __init__(self, robotName, plan, team, packageName): self.robotName = robotName self.plan = plan self.team = team self.packageName = packageName self.runningBehaviorsStack = [] self.behaviorsRunning = [] self.nodesToTerminate = [] self.readyTeam = [] self.roslauncher = ROSLaunch() self.roslauncher.start() # Initiate publishers # broadcast is the topic used for broadcast messages self.broadcastPublisher = rospy.Publisher('/bite/broadcast', InformMsg) self.terminateBehaviorPublisher = rospy.Publisher(str.format('/bite/{0}/terminate',self.robotName), String) # Initiate subscribers # broadcast is the topic used for broadcast messages rospy.Subscriber('/bite/broadcast', InformMsg, self.receiveCallback) print 'Waiting for knowledgebase...' knowledgeServiceName = str.format('/bite/{0}/get_knowledge', self.robotName) rospy.wait_for_service(knowledgeServiceName) self.getKnowledgeClient = rospy.ServiceProxy(knowledgeServiceName, GetKnowledge) print 'Ready' rospy.sleep(0.5) ####################################################### # Expands the running behaviors stack hierarchichally # ####################################################### def expandStackHierarchically(self): # Initiates the current node currentNode = self.plan.nodes[0] currentTeam = self.team if self.runningBehaviorsStack == []: # Adds the first node of the plan print str.format('Adding node "{0}" to the stack', currentNode.nodeName) self.runningBehaviorsStack.append((currentNode, currentTeam, '')) else: # If the stack is not empty starting from the last behavior in the stack currentNode, currentTeam = self.runningBehaviorsStack[-1][0:2] print str.format('Starting from node "{0}"', currentNode.nodeName) print str.format('Hierarchical children of node "{0}" are: {1}', currentNode.nodeName, [node.nodeName for node in currentNode.hierarchicalChildren]) while not currentNode.hierarchicalChildren == []: # Allocating next level of hierarchy result = self.allocateNextNode(currentNode, currentTeam) if result.node == '': break currentNode = self.plan.getNode(result.node) currentTeam = result.newTeam # Adds the node to the stack print str.format('Adding node "{0}" to the stack', currentNode.nodeName) self.runningBehaviorsStack.append((currentNode, currentTeam, result.params)) print str.format('Hierarchical children of node "{0}" are: {1}', currentNode.nodeName, [node.nodeName for node in currentNode.hierarchicalChildren]) self.terminateBehaviorsNotInStack() rospy.sleep(1) print 'Starting behaviors in stack' for (node, team, params) in self.runningBehaviorsStack: if not node in self.behaviorsRunning: print str.format('Starting behavior: {0}', node.behaviorName) self.startBehavior(node, params) # Notify other robots what behavior its starting self.broadcastPublisher.publish(self.robotName, self.team, str.format('STRATING {0}', node.behaviorName)) self.nodesToTerminate = [] #################################################################### # Allocate procedure for next node. Including team synchronization # #################################################################### def allocateNextNode(self, currentNode, currentTeam): filteredChildren = self.filterByPreConds(currentNode.hierarchicalChildren) print str.format('Next behaviors that their preconditions are true: {0}', [node.nodeName for node in filteredChildren]) # Set the right allocate method for the current node print str.format('Node\'s allocate method is: "{0}"', currentNode.allocateMethod) allocateServiceName = str.format('/bite/{0}/{1}', self.robotName, currentNode.allocateMethod) rospy.wait_for_service(allocateServiceName) allocateClient = rospy.ServiceProxy(allocateServiceName, Allocate) if len(currentTeam) > 1: # Inform the rest of the team that preconditions of these behaviors are true print str.format('Informing the rest of the team: {0}', currentTeam) for node in filteredChildren: for preCond in node.preConds: self.broadcastPublisher.publish(self.robotName, currentTeam, str.format('INFORM {0} {1}', preCond, True)) # Sync with team print str.format('Waiting for team: {0}', currentTeam) self.waitForTeam(currentTeam) # Checks again after received information from other robots filteredChildren = self.filterByPreConds(currentNode.hierarchicalChildren) # Sleeps to make sure all the team gets here rospy.sleep(1) if (filteredChildren == []): # None of the nodes fit their preconditions return AllocateResponse('', [], '') # Calls allocate return allocateClient([child.nodeName for child in filteredChildren], currentTeam) ############################################## # Filters given nodes by their preconditions # ############################################## def filterByPreConds(self, nodes): filteredNodes = [] # Iterates over all the given nodes for node in nodes: shouldAddNode = True for preCond in node.preConds: if not self.getKnowledgeClient(preCond).value == 'True': # One of the preconds isn't true - not adding the node shouldAddNode = False break if shouldAddNode: filteredNodes.append(node) return filteredNodes ############################## # Waits for rest of the team # ############################## def waitForTeam(self, currentTeam): allTeamReady = False self.readyTeam = [] while not allTeamReady: allTeamReady = True self.broadcastPublisher.publish(self.robotName, currentTeam, 'READY') rospy.sleep(1) for robot in currentTeam: if not robot in self.readyTeam: allTeamReady = False break self.broadcastPublisher.publish(self.robotName, currentTeam, 'READY') print 'All team is ready' ############################################## # Terminates behaviors not in current branch # ############################################## def terminateBehaviorsNotInStack(self): print 'Terminating behaviors not in current branch' for (node, team, params) in self.nodesToTerminate: if not (node, team, params) in self.runningBehaviorsStack: print str.format('Terminating behavior: {0}', node.behaviorName) self.terminateBehavior(node, team) ############################################################ # Terminates a behavior and informing the rest of the team # ############################################################ def terminateBehavior(self, node, team): # Inform the rest of the robots self.broadcastPublisher.publish(self.robotName, team, str.format('TERMINATING {0}', node.behaviorName)) # Terminate the behavior self.terminateBehaviorPublisher.publish(node.behaviorName) self.behaviorsRunning.remove(node) ##################### # Starts a behavior # ##################### def startBehavior(self, node, params): # Node name with the robot name appended for namespace reasons fullNodeName = str.format('bite_{0}_{1}', self.robotName.lower(), node.behaviorName.lower()) # args var is for command line input, and by default is given the robot name, each argument is # is seperetd by space char args = str.format('{0} {1} {2}', self.robotName, node.behaviorName, params) # Creates a node, name argument is adding robotName and _ for namespace reasons runNode = Node(package = self.packageName, node_type = "BehaviorLauncher.py", name = fullNodeName, args = args, output = "screen") # Launch node self.roslauncher.launch(runNode) self.behaviorsRunning.append(node) ################################################### # Receives information from the broadcast channel # ################################################### def receiveCallback(self, msg): if self.robotName in msg.to: if msg.data == 'READY' and not msg.from_ in self.readyTeam: print str.format('Received that {0} is ready', msg.from_) self.readyTeam.append(msg.from_) ########################################### # Monitors running behaviors in the stack # ########################################### def monitorBehaviors(self): behaviorsEnded = [] # Monitors behaviors in running stack while behaviorsEnded == []: rospy.sleep(1) # Test all nodes in the running stack for their termination conditions for (node, team, params) in self.runningBehaviorsStack: for termCond in node.termConds: if self.getKnowledgeClient(termCond).value == 'True': print str.format('I think that node {0} has to be terminated', node.nodeName) behaviorsEnded.append(node) # Informs the rest of the team print str.format('Informing: {0}', team) self.broadcastPublisher.publish(self.robotName, team, str.format('INFORM {0} {1}', termCond, True)) break # Adds nodes pending for termination while not behaviorsEnded == []: (node, team, params) = self.runningBehaviorsStack.pop() self.nodesToTerminate.append((node, team, params)) if node in behaviorsEnded: behaviorsEnded.remove(node) # Returns the last node terminated return (node, team) ####################################################### # Expands the stack sequentially # # Returns whether the stack was expanded sequentially # ####################################################### def expandStackSequentially(self, currentNode, currentTeam): if currentNode.sequentialChildren == []: return False # Get all sequential children of current node and filter them by preconditions print str.format('Sequential children of node "{0}" are: {1}', currentNode.nodeName, [node.nodeName for node in currentNode.sequentialChildren]) filteredChildren = self.filterByPreConds(currentNode.sequentialChildren) print str.format('Next behaviors that their preconditions are true: {0}', [node.nodeName for node in filteredChildren]) # Set the right vote method for the last node went out of the stack print str.format('Node\'s vote method is: "{0}"', currentNode.voteMethod) voteServiceName = str.format('/bite/{0}/{1}', self.robotName, currentNode.voteMethod) rospy.wait_for_service(voteServiceName) voteClient = rospy.ServiceProxy(voteServiceName, Vote) if len(currentTeam) > 1: # Inform the rest of the team that preconditions of these behaviors are true print str.format('Informing the rest of the team: {0}', currentTeam) for node in filteredChildren: for preCond in node.preConds: self.broadcastPublisher.publish(self.robotName, currentTeam, str.format('INFORM {0} {1}', preCond, True)) # Sync with team print str.format('Waiting for team: {0}', currentTeam) self.waitForTeam(currentTeam) # Checks again after received information from other robots filteredChildren = self.filterByPreConds(currentNode.sequentialChildren) if filteredChildren == []: # None of the nodes fit their preconditions return False # Sleeps to make sure all the team gets here rospy.sleep(1) # Calls Vote result = voteClient([child.nodeName for child in filteredChildren], currentTeam) # Adds the node to the stack nextNode = self.plan.getNode(result.node) print str.format('Adding node "{0}" to the stack', nextNode.nodeName) self.runningBehaviorsStack.append((nextNode, currentTeam, result.params)) return True
class BaseVehicle(WheeledVehicle): """ Base class for all vehicles. Other vehicles inherit from this class and can use/overwrite its functions. It provides the following basic services: - /set_state: Set the vehicle state. - /set_speed_kph: Set the vehicles speed in kilometers per hour. - /set_loop: Set a closed loop trajectory from a certain node. - /set_destination: Set a trajectory to a certain destination node. - /toggle_simulation: Toggle the simulation of the vehicle on/off. The launch_sensor function can be called from child classes to launch the sensor nodes that are listed in their class variable self.sensors. """ def __init__(self, namespace, vehicle_id, simulation_rate, x=0., y=0., yaw=0., v=0., weight='light'): """ Initialize class BaseVehicle. @param namespace: I{(string)} Namespace in which the vehicle node is started. @param vehicle_id: I{(int)} ID of the vehicle that is created. @param simulation_rate: I{(int)} Rate at which the vehicle is simulated (hz) @param x: I{(float)} x-coordinate at which the vehicle starts. @param y: I{(float)} y-coordinate at which the vehicle starts. @param yaw: I{(float)} Initial yaw of the vehicle in radians. @param v: I{(float)} Initial velocity of the vehicle. """ self.launcher = ROSLaunch() self.launcher.start() self.namespace = namespace self.vehicle_id = int(vehicle_id) self.class_name = self.__class__.__name__ self.simulation_rate = simulation_rate # Set parameters of base vehicle to default values. self.simulate = False self.sensors = [] self.coms = [] self.x = x self.y = y self.yaw = yaw self.goal_speed = v self.v = v self.cruising_speed = v self.axles_distance = 1.9 self.np_trajectory = numpy.zeros((0, 0)) self.commands = {} self.weight = weight self.radar_vis = True self.loop = False self.at_dest = False self.traffic_level = 5 #Default value, no traffic self.traffic_lights = [] self.lights_status = [] # Start the simulation loop in a separate thread. sim_thread = threading.Thread(target=self.simulation_loop) sim_thread.daemon = True sim_thread.start() self.overtake = False self.waiting_at_stop = False # Register all services, pubs and subs last to prevent attempts to use # the services before the initialization of the vehicle is finished. self.pub_state = rospy.Publisher('/current_vehicle_state', msgs.VehicleState, queue_size=10) self.pub_demand = rospy.Publisher('/current_demand', msgs.TrafficDemand, queue_size=10) self.traffic_pub = rospy.Publisher('/update_traffic', msgs.VehicleTrafficUpdate, queue_size=10) rospy.Service(self.namespace + 'set_state', srvs.SetVehicleState, self.handle_set_state) rospy.Service(self.namespace + 'set_speed_kph', srvs.SetSpeed, self.handle_set_speed_kph) rospy.Service(self.namespace + 'set_loop', srvs.SetLoop, self.handle_set_loop) rospy.Service(self.namespace + 'set_destination', srvs.SetDestination, self.handle_set_destination) rospy.Service(self.namespace + 'toggle_simulation', srvs.SetBool, self.handle_toggle_simulation) rospy.Service(self.namespace + 'set_radar_vis', srvs.SetRadarVis, self.handle_set_radar_vis) rospy.Service(self.namespace + 'set_demand', srvs.SetDemand, self.handle_set_demand) # rospy.wait_for_service(self.namespace + '/publish_com') # self.publish_com = rospy.ServiceProxy(self.namespace + 'publish_com', # PublishCom) rospy.Subscriber('/world_state', WorldState, self.update_traffic_light_status) def get_nearest_node(self, dest_id, search_all=False): rospy.wait_for_service('/get_nearest_nodeid') try: get_nodeid = rospy.ServiceProxy('/get_nearest_nodeid', srvs.GetNearestNodeId) self.current_node = get_nodeid(self.x, self.y, dest_id, search_all).node_id except rospy.ServiceException, e: raise NameError("Service call failed: %s" % e)