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)