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 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