def generate_test_description():
# Set env
#os.environ['GAZEBO_MASTER_URI'] ='http://localhost:3000'
file_path = pathlib.Path(__file__)
# Here, parent first removes the file name
parent_file_path = pathlib.Path(__file__).parent
# Gazebo
gazebo_launch = os.path.join(get_package_share_directory('gazebo_ros'),
'launch', 'gazebo.launch.py')
gazebo_world = os.path.join(
str(parent_file_path),
'worlds',
'test_empty.world',
)
if not pathlib.Path(gazebo_launch).exists() or not pathlib.Path(
gazebo_world).exists():
exc = 'Launch file ' + gazebo_launch + ' or ' + gazebo_world + ' does not exist'
raise Exception(exc)
launch_args = [('world', gazebo_world), ('paused', 'false'),
('gui', 'false'), ('verbose', 'true')]
gazebo_launch_desc = IncludeLaunchDescription(
AnyLaunchDescriptionSource(gazebo_launch),
launch_arguments=launch_args)
# gazebo_launch_desc = launch.actions.ExecuteProcess(
# cmd=['gzserver', '--verbose', 'world', gazebo_world,
# '-s', 'libgazebo_ros_init.so', 'libgazebo_ros_properties.so', 'libgazebo_ros_factory.so', 'libgazebo_ros_state.so'],
# output='screen'
# )
# Upload vehicle
upload_launch = os.path.join(
str(parent_file_path), 'models', 'default_fossen_vehicle',
'test_upload_default_fossen_vehicle.launch.py')
if not pathlib.Path(upload_launch).exists():
exc = 'Launch file ' + upload_launch + ' does not exist'
raise Exception(exc)
upload_launch_desc = IncludeLaunchDescription(
AnyLaunchDescriptionSource(upload_launch))
# listener_node = launch_ros.actions.Node(
# executable=sys.executable,
# arguments=[os.path.join(path_to_test, 'listener.py')],
# additional_env={'PYTHONUNBUFFERED': '1'},
# remappings=[('chatter', 'listener_chatter')]
# )
return (launch.LaunchDescription([
gazebo_launch_desc,
upload_launch_desc,
# Start tests right away - no need to wait for anything
launch_testing.actions.ReadyToTest(),
]))
def generate_test_description():
# Set env...not sure if the tests actually connect to the right Gazebo...
# os.environ['GAZEBO_MASTER_URI'] ='http://localhost:3001'
file_path = pathlib.Path(__file__)
# Here, parent first removes the file name
parent_file_path = pathlib.Path(__file__).parent
# Gazebo
gazebo_launch = os.path.join(get_package_share_directory('gazebo_ros'),
'launch', 'gazebo.launch.py')
gazebo_world = os.path.join(
str(parent_file_path),
'worlds',
'test_empty.world',
)
if not pathlib.Path(gazebo_launch).exists() or not pathlib.Path(
gazebo_world).exists():
exc = 'Launch file ' + gazebo_launch + ' or ' + gazebo_world + ' does not exist'
raise Exception(exc)
launch_args = [
('world', gazebo_world),
('paused', 'false'),
('gui', 'false'),
('verbose', 'true'),
]
gazebo_launch_desc = IncludeLaunchDescription(
AnyLaunchDescriptionSource(gazebo_launch),
launch_arguments=launch_args)
# Upload vehicle
upload_launch = os.path.join(str(parent_file_path), 'models',
'sphere_vehicle',
'test_upload_sphere_vehicle.launch.py')
if not pathlib.Path(upload_launch).exists():
exc = 'Launch file ' + upload_launch + ' does not exist'
raise Exception(exc)
upload_launch_desc = IncludeLaunchDescription(
AnyLaunchDescriptionSource(upload_launch))
return (launch.LaunchDescription([
gazebo_launch_desc,
upload_launch_desc,
launch_testing.actions.ReadyToTest(),
]))
def generate_test_description():
#path_to_test = os.path.dirname(__file__)
file_path = pathlib.Path(__file__)
# Here, parent first removes the file name
parent_file_path = pathlib.Path(__file__).parent
thruster_manager_launch = os.path.join(
get_package_share_directory('uuv_thruster_manager'), 'launch',
'thruster_manager.launch')
thruster_manager_yaml = os.path.join(
str(parent_file_path),
'test_vehicle_thruster_manager_proportional.yaml')
xacro_file = os.path.join(str(parent_file_path),
'test_vehicle_z_axis.urdf.xacro')
doc = xacro.process(xacro_file)
launch_args = [
('model_name', 'test_vehicle'),
('output_dir', '/tmp'),
('config_file', thruster_manager_yaml),
('reset_tam', 'true'),
]
thruster_manager_launch_desc = IncludeLaunchDescription(
AnyLaunchDescriptionSource(thruster_manager_launch),
launch_arguments=launch_args)
joint_state_publisher = launch_ros.actions.Node(
namespace='test_vehicle',
package="joint_state_publisher",
executable="joint_state_publisher",
name="joint_state_publisher",
output='screen',
parameters=[{
'use_sim_time': False
}, {
'rate': 100
}],
)
robot_state_description = launch_ros.actions.Node(
namespace='test_vehicle',
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[{
'use_sim_time': False
}, {
'robot_description': doc
}], # Use subst here
)
return (launch.LaunchDescription([
joint_state_publisher,
robot_state_description,
thruster_manager_launch_desc,
launch_testing.actions.ReadyToTest(),
]))
def include(self,
package,
launch_file,
launch_dir=None,
launch_arguments=None):
'''
Include another launch file
'''
launch_file = self.find(package, launch_file, launch_dir)
self.entity(
IncludeLaunchDescription(AnyLaunchDescriptionSource(launch_file),
launch_arguments=launch_arguments))
def launch(shutdown_pipe, package_name, launch_file_name, launch_arguments,
debug):
"""Launch a ROS system."""
event_loop = asyncio.new_event_loop()
asyncio.set_event_loop(event_loop)
# based on ros2launch/command/launch.py:main
if package_name is None:
single_file = True
else:
single_file = False
get_package_prefix(package_name)
path = None
launch_arguments = []
if single_file:
if os.path.exists(package_name):
path = package_name
else:
raise ValueError(package_name)
if launch_file_name is not None:
launch_arguments.append(launch_file_name)
else:
try:
path = get_share_file_path_from_package(package_name=package_name,
file_name=launch_file_name)
except PackageNotFoundError as exc:
raise RuntimeError("Package '{}' not found: {}".format(
package_name, exc))
except (FileNotFoundError, MultipleLaunchFilesError) as exc:
raise RuntimeError(str(exc))
launch_arguments.extend(launch_arguments)
launch_service = LaunchService(argv=launch_arguments, debug=debug)
launch_description = LaunchDescription([
IncludeLaunchDescription(
AnyLaunchDescriptionSource(path),
launch_arguments={},
),
])
launch_service.include_launch_description(launch_description)
finished = False
def shutdown():
while not finished and not shutdown_pipe.poll(1):
pass
launch_service.shutdown()
t = threading.Thread(target=shutdown)
t.start()
launch_service.run(shutdown_when_idle=True)
finished = True
t.join()
event_loop.close()
def generate_test_description():
test_fault_injection_launch_file = os.path.join(
get_package_share_directory("fault_injection"),
"launch",
"test_fault_injection.launch.xml",
)
fault_injection = IncludeLaunchDescription(
AnyLaunchDescriptionSource(test_fault_injection_launch_file), )
return launch.LaunchDescription([
fault_injection,
# Start tests right away - no need to wait for anything
launch_testing.actions.ReadyToTest(),
])
def generate_test_description():
#path_to_test = os.path.dirname(__file__)
file_path = pathlib.Path(__file__)
# Here, parent first removes the file name
parent_file_path = pathlib.Path(__file__).parent
thruster_manager_launch = os.path.join(
get_package_share_directory('uuv_thruster_manager'), 'launch',
'thruster_manager.launch')
thruster_manager_yaml = os.path.join(
str(parent_file_path),
'test_vehicle_thruster_manager_proportional.yaml')
xacro_file = os.path.join(str(parent_file_path),
'test_vehicle_y_axis.urdf.xacro')
output = os.path.join(str(parent_file_path),
'robot_description_y_axis.urdf')
doc = xacro.process(xacro_file)
ensure_path_exists(output)
try:
with open(output, 'w') as file_out:
file_out.write(doc)
except IOError as e:
print("Failed to open output: ", exc=e)
args = output.split()
# ('axis', 'x')
launch_args = [('model_name', 'test_vehicle'), ('output_dir', '/tmp'),
('config_file', thruster_manager_yaml),
('reset_tam', 'true'), ('urdf_file', output)]
thruster_manager_launch_desc = IncludeLaunchDescription(
AnyLaunchDescriptionSource(thruster_manager_launch),
launch_arguments=launch_args)
joint_state_publisher = launch_ros.actions.Node(
node_namespace='test_vehicle',
package="joint_state_publisher",
node_executable="joint_state_publisher",
node_name="joint_state_publisher",
#parameters=[{'source_list':'test_vehicle/joint_states'}],
#remappings=[('joint_states', '/test_vehicle/joint_states')],
arguments=args,
output='screen',
parameters=[{
'use_sim_time': False
}, {
'rate': 100
}],
)
robot_state_description = launch_ros.actions.Node(
node_namespace='test_vehicle',
package='robot_state_publisher',
node_executable='robot_state_publisher',
#parameters=[{'robot_description', doc}]
# TODO To replace in foxy with parameters=[{'robot_description', Command('ros2 run xacro...')}]
arguments=args,
output='screen',
parameters=[{
'use_sim_time': False
}], # Use subst here
)
return (launch.LaunchDescription([
joint_state_publisher,
robot_state_description,
thruster_manager_launch_desc,
# Start tests right away - no need to wait for anything
launch_testing.actions.ReadyToTest(),
]))
def launch_setup(context, *args, **kwargs):
# Perform substitutions
debug = Lc('debug').perform(context)
namespace = Lc('namespace').perform(context)
x = Lc('x').perform(context)
y = Lc('y').perform(context)
z = Lc('z').perform(context)
roll = Lc('roll').perform(context)
pitch = Lc('pitch').perform(context)
yaw = Lc('yaw').perform(context)
# use_sim_time = Lc('use_sim_time').perform(context)
use_world_ned = Lc('use_ned_frame').perform(context)
# Request sim time value to the global node
res = is_sim_time(return_param=False, use_subprocess=True)
# Xacro
#xacro_file = PathJoinSubstitution(get_package_share_directory('uuv_descriptions'),'robots','rexrov_')
xacro_file = os.path.join(
get_package_share_directory('uuv_descriptions'), 'robots',
'rexrov_' + (Lc('mode')).perform(context) + '.xacro')
# Build the directories, check for existence
path = os.path.join(
get_package_share_directory('uuv_descriptions'),
'robots',
'generated',
namespace,
)
if not pathlib.Path(path).exists():
try:
# Create directory if required and sub-directory
os.makedirs(path)
except OSError:
print("Creation of the directory %s failed" % path)
output = os.path.join(path, 'robot_description')
if not pathlib.Path(xacro_file).exists():
exc = 'Launch file ' + xacro_file + ' does not exist'
raise Exception(exc)
mapping = {}
if to_bool(use_world_ned):
mappings = {
'debug': debug,
'namespace': namespace,
'inertial_reference_frame': 'world_ned'
}
else:
mappings = {
'debug': debug,
'namespace': namespace,
'inertial_reference_frame': 'world'
}
doc = xacro.process(xacro_file, mappings=mappings)
with open(output, 'w') as file_out:
file_out.write(doc)
# URDF spawner
args = ('-gazebo_namespace /gazebo '
'-x %s -y %s -z %s -R %s -P %s -Y %s -entity %s -file %s' %
(x, y, z, roll, pitch, yaw, namespace, output)).split()
# Urdf spawner. NB: node namespace does not prefix the topic,
# as using a leading /
urdf_spawner = Node(
node_name='urdf_spawner',
package='gazebo_ros',
node_executable='spawn_entity.py',
output='screen',
parameters=[{
'use_sim_time': res
}],
# To replace in foxy with parameters=[{'robot_description', Command('ros2 run xacro...')}]
arguments=args)
# A joint state publisher plugin already is started with the model, no need to use the default joint state publisher
args = (output).split()
robot_state_publisher = Node(
node_name='robot_state_publisher',
package='robot_state_publisher',
node_executable='robot_state_publisher',
# To replace in foxy with parameters=[{'robot_description', Command('ros2 run xacro...')}]
arguments=args,
output='screen',
parameters=[{
'use_sim_time': res
}] # Use subst here
)
# Message to tf
message_to_tf_launch = os.path.join(
get_package_share_directory('uuv_assistants'), 'launch',
'message_to_tf.launch')
if not pathlib.Path(message_to_tf_launch).exists():
exc = 'Launch file ' + message_to_tf_launch + ' does not exist'
raise Exception(exc)
launch_args = [('namespace', namespace), ('world_frame', 'world'),
('child_frame_id', '/' + namespace + '/base_link')]
message_to_tf_launch = IncludeLaunchDescription(
AnyLaunchDescriptionSource(message_to_tf_launch),
launch_arguments=launch_args)
group = GroupAction([
PushRosNamespace(namespace),
urdf_spawner,
robot_state_publisher,
])
return [group, message_to_tf_launch]