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', ), ])