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 !!"
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_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