def generate_launch_description():
file_path = pathlib.Path(__file__)
# Here, parent first removes the file name
parent_file_path = pathlib.Path(__file__).parent
# Xacro
xacro_file = os.path.join(str(parent_file_path), 'model.xacro')
output = os.path.join(str(parent_file_path), 'robot_description.urdf')
doc = xacro.process(xacro_file)
with open(output, 'w') as file_out:
file_out.write(doc)
args = ('-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0 -entity vehicle -file ' +
output).split()
# args='-gazebo_namespace /gazebo -x 0 -y 0 -z 0 -R 0 -P 0 -Y 0 -entity $(var namespace) -file ' + output
# Urdf spawner
urdf_spawner = Node(
node_name='urdf_spawner_thrusters',
# node_namespace = 'vehicle',
package='gazebo_ros',
node_executable='spawn_entity.py',
output='screen',
# TODO To replace in foxy with parameters=[{'robot_description', Command('ros2 run xacro...')}]
arguments=args)
return (LaunchDescription([
urdf_spawner,
]))
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 setUpClass(cls):
parent_file_path = pathlib.Path(__file__).parent
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_x_axis.urdf.xacro')
doc = xacro.process(xacro_file)
# Initialize the ROS context for the test node
# FIXME Temp workaround TF for listener subscribing to relative topic
rclpy.init(args=[
'--ros-args', '--params-file', thruster_manager_yaml, '-r',
'__ns:=/test_vehicle', '-r', 'tf:=/tf', '-p',
'robot_description:=%s' % doc
])
# Name alias...why cls is not working ?
_class = TestThrusterManagerProportionalCorrect
_class._manager = ThrusterManager(
'test_thruster_manager_proportional_correct')
_class._thread = threading.Thread(target=rclpy.spin,
args=(_class._manager, ),
daemon=True)
_class._thread.start()
# Wait for the async initialization of the manager to finish
while not _class._manager.ready:
time.sleep(0.1)
def generate_launch_description():
file_path = pathlib.Path(__file__)
# Here, parent first removes the file name
parent_file_path = pathlib.Path(__file__).parent
# Xacro
xacro_file = os.path.join(
str(parent_file_path),
'model.xacro'
)
output = os.path.join(
str(parent_file_path),
'robot_description.urdf'
)
doc = xacro.process(xacro_file)
args = ('-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0 -spawn_service_timeout 30 -entity vehicle -topic robot_description').split()
#args='-gazebo_namespace /gazebo -x 0 -y 0 -z 0 -R 0 -P 0 -Y 0 -entity $(var namespace) -file ' + output
# There are currently no ways to pass the robot_description as a parameter
# to the urdf_spawner, see:
# https://github.com/ros-simulation/gazebo_ros_pkgs/pull/1039
# We should use the robot state publisher to publish the robot description
# (or pass a file from the disk to the urdf spawner)
robot_state_description = Node(
namespace = 'vehicle',
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[{'use_sim_time':False}, {'robot_description': doc}], # Use subst here
)
# Urdf spawner. NB: node namespace does not prefix the topic,
# as using a leading /
urdf_spawner = Node(
name = 'urdf_spawner_sphere',
namespace = 'vehicle',
package='gazebo_ros',
executable='spawn_entity.py',
arguments=args,
)
return (
LaunchDescription([
robot_state_description,
urdf_spawner,
])
)
def test_update(xacro_file, canonicalize_xml):
tree = XacroTree(xacro_file)
tree.update()
assert canonicalize_xml(tree.xml_string()) == canonicalize_xml(
xacro.process(xacro_file))
# check update of 'files' and 'dirs' attributes
assert len(tree.files) == 2
assert len(tree.dirs) == 2
assert tree.root_file in tree.files
assert os.path.dirname(tree.root_file) in tree.dirs
assert any(xfile.endswith('snippets/wheel.xacro') for xfile in tree.files)
assert any(xdir.endswith('snippets') for xdir in tree.dirs)
def test_start_stop(xacro_observer, xacro_file, event_handler_mock,
canonicalize_xml):
event_handler_mock.on_modified.assert_not_called()
xacro_observer.start(event_handler_mock)
assert xacro_observer.observer.is_alive()
assert len(xacro_observer.observer.emitters) == 2
assert canonicalize_xml(
xacro_observer.xacro_tree.xml_string()) == canonicalize_xml(
xacro.process(xacro_file))
xacro_observer.stop()
assert not xacro_observer.observer.is_alive()
assert len(xacro_observer.observer.emitters) == 0
def generate_launch_description():
# Get dir names
pkg_share = get_package_share_directory('diffbot2_description')
# Compute robot_description string
xacro_file = os.path.join(pkg_share, 'urdf/diffbot2.xacro')
robot_description = xacro.process(xacro_file)
# Launch description
launch_description = LaunchDescription()
# Define parameters
common_params = {
'robot_description': robot_description,
}
# Add nodes
launch_description.add_action(
launch.actions.DeclareLaunchArgument(
name='namespace', default_value='', description='Node namespace'
)
)
launch_description.add_action(
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
namespace=LaunchConfiguration('namespace'),
parameters=[common_params],
)
)
launch_description.add_action(
Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
output='screen',
namespace=LaunchConfiguration('namespace'),
parameters=[common_params],
),
)
return launch_description
def setUpClass(cls):
parent_file_path = pathlib.Path(__file__).parent
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_x_axis.urdf.xacro'
)
output = os.path.join(
str(parent_file_path),
'robot_description_x_axis.urdf'
)
#TODO Change in foxy
if not pathlib.Path(output).exists():
doc = xacro.process(xacro_file)
try:
with open(output, 'w') as file_out:
file_out.write(doc)
except IOError as e:
print("Failed to open output:", exc=e)
# Initialize the ROS context for the test node
# FIXME Temp workaround TF for listener subscribing to relative topic
rclpy.init(args=['--ros-args', '--params-file', thruster_manager_yaml,
'-r', '__ns:=/test_vehicle', '-r', 'tf:=/tf', '-p', 'urdf_file:=%s' % output])
# Name alias...why cls is not working ?
_class = TestThrusterManagerProportionalCorrect
_class._manager = ThrusterManager('test_thruster_manager_proportional_correct')
_class._thread = threading.Thread(target=rclpy.spin, args=(_class._manager,), daemon=True)
_class._thread.start()
# Wait for the async initialization of the manager to finish
while not _class._manager.ready:
time.sleep(0.1)
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]
DATA_FILES = [(f'share/{PACKAGE_NAME}', ['package.xml']),
('share/ament_index/resource_index/packages/', [f'resources/{PACKAGE_NAME}'])]
DATA_FILES += [(os.path.join('share', PACKAGE_NAME, str(directory)),
[str(file) for file in directory.rglob(pattern) if not file.is_dir() and file.parent == directory])
for folder, pattern in SHARE_DIRS for directory in Path(folder).rglob('**')]
BUILD_DIR = next((sys.argv[i + 1] for i, arg in enumerate(sys.argv) if arg == '--build-directory'), None)
if BUILD_DIR:
os.makedirs(BUILD_DIR, exist_ok=True)
for path, files in DATA_FILES:
for file in files:
if file.endswith('.xacro'):
new_file = os.path.join(BUILD_DIR, file.replace('.xacro', ''))
with xacro.open_output(new_file) as fd:
fd.write(xacro.process(file))
files.remove(file)
files.append(new_file)
INSTALL_REQUIRES = ['setuptools']
if os.path.isfile('requirements.txt'):
with open('requirements.txt') as file:
INSTALL_REQUIRES += [line.strip() for line in file.readlines()]
TESTS_REQUIRE = ['pytest']
if os.path.isfile('test-requirements.txt'):
with open('test-requirements.txt') as file:
TESTS_REQUIRE += [line.strip() for line in file.readlines()]
setup(
name=PACKAGE_NAME,
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_x_axis.urdf.xacro'
)
output = os.path.join(
str(parent_file_path),
'robot_description_x_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()
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",
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,
launch_testing.actions.ReadyToTest(),
])
)
def read_description(self):
description_xacro_path = os.path.join(os.path.dirname(__file__),
'mayday.urdf.xacro')
description_urdf = xacro.process(description_xacro_path)
self.description = URDF.from_xml_string(description_urdf)
def generate_launch_description():
# Load the URDF into a parameter
bringup_dir = get_package_share_directory('robomagellan')
xacro_path = os.path.join(bringup_dir, 'urdf', 'robot.urdf.xacro')
urdf = xacro.process(xacro_path)
# Load the driver config
driver_config = os.path.join(get_package_share_directory('robomagellan'),
'config', 'etherbotix.yaml')
return LaunchDescription([
# Robot state publisher
Node(
name='robot_state_publisher',
package='robot_state_publisher',
node_executable='robot_state_publisher',
parameters=[{
'robot_description': urdf
}],
),
# Etherbotix drivers
Node(
name='etherbotix',
package='etherbotix',
node_executable='etherbotix_driver',
parameters=[{
'robot_description': urdf
}, driver_config],
remappings=[('odom', 'base_controller/odom')],
output='screen',
),
# GPS publisher
Node(
name='gps_publisher',
package='etherbotix',
node_executable='gps_publisher_node',
parameters=[{
'frame_id': 'base_link'
}],
remappings=[('nmea_sentence', 'gps/nmea_sentence')],
),
# UM7 IMU
Node(
name='um7_driver',
package='um7',
node_executable='um7_node',
parameters=[{
'mag_updates': False,
'tf_ned_to_enu': False,
'update_rate': 100
}],
# This could be cleaned up when wildcards are implemented
# see https://github.com/ros2/rcl/issues/232
remappings=[('imu/data', 'imu_um7/data'),
('imu/mag', 'imu_um7/mag'), ('imu/rpy', 'imu_um7/rpy'),
('imu/temperature', 'imu_um7/temperature')]),
# TODO: add realsense driver
# TODO: add mux between nav and joystick
Node(
name='joy',
package='joy',
node_executable='joy_node',
parameters=[
{
'autorepeat_rate': 5.0
},
],
),
# Teleop
Node(
name='teleop',
package='teleop_twist_joy',
node_executable='teleop_node',
parameters=[{
'enable_button': 4,
'axis_linear': 4,
'scale_linear': 1.0,
'axis_angular': 0,
'scale_angular': 3.0
}],
remappings=[('cmd_vel', 'base_controller/command')],
output='screen',
),
])
