def generate_launch_description(): # robot state publisher my_rotate_bot_path = os.path.join( get_package_share_directory('my_rotate_bot')) urdf_file = os.path.join(my_rotate_bot_path, 'urdf', 'model.urdf') doc = xacro.parse(open(urdf_file)) xacro.process_doc(doc) robot_description = {'robot_description': doc.toxml()} node_robot_state_publisher = Node(package='robot_state_publisher', executable='robot_state_publisher', output='screen', parameters=[robot_description]) # gazebo gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource([ os.path.join(get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py' ]), ) # spawn robot spawn_entity = Node( package='gazebo_ros', executable='spawn_entity.py', arguments=['-topic', 'robot_description', '-entity', 'rotate_box_bot'], output='screen') return LaunchDescription([ gazebo, node_robot_state_publisher, spawn_entity, ])
def _add_cb(self, req): name = req.name xacro_name = os.path.join(self._urdf_dir, name + '_macro.urdf.xacro') macro_name = 'assy_part_' + re.split('[_-]', name)[0] position = req.pose.pose.position orientation = req.pose.pose.orientation xacro_desc = ModelSpawnerServer._Model.format( name, xacro_name, macro_name, req.prefix, req.pose.header.frame_id, position.x, position.y, position.z, *tfs.euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])) try: # Expand and process Xacro into XML format. doc = xacro.parse(xacro_desc) # Create DOM tree. xacro.process_doc(doc) # Expand and process macros. desc = doc.toprettyxml(indent=' ', encoding='utf8') self._models[req.prefix + name] \ = xml.dom.minidom.parseString(desc).childNodes[0] self._publisher.publish(mmsg.ModelDescription.ADD, req.prefix + name, desc) return msrv.AddResponse(True) except Exception as e: rospy.logerr(e) return msrv.AddResponse(False)
def generate_launch_description(): gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource([ os.path.join(get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py' ]), ) gazebo_ros2_control_demos_path = os.path.join( get_package_share_directory('gazebo_ros2_control_demos')) xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf', 'test_cart_position.xacro.urdf') doc = xacro.parse(open(xacro_file)) xacro.process_doc(doc) params = {'robot_description': doc.toxml()} node_robot_state_publisher = Node(package='robot_state_publisher', executable='robot_state_publisher', output='screen', parameters=[params]) spawn_entity = Node( package='gazebo_ros', executable='spawn_entity.py', arguments=['-topic', 'robot_description', '-entity', 'cartpole'], output='screen') return LaunchDescription([ gazebo, node_robot_state_publisher, spawn_entity, ])
def generate_launch_description(): # Get URDF via xacro racecar_description_share_dir = os.path.join( get_package_share_directory('racecar_description')) urdf_file_name = 'racecar.urdf.xml' xacro_file_name = 'racecar.xacro.xml' xacro_file = os.path.join(racecar_description_share_dir, 'urdf', urdf_file_name) doc = xacro.parse(open(xacro_file)) xacro.process_doc(doc) robot_description = {"robot_description": doc.toxml()} racecar_controller = os.path.join( get_package_share_directory("racecar_control"), "config", "racecar_control.yaml", ) return LaunchDescription([ Node( package="controller_manager", executable="ros2_control_node", namespace='racecar', parameters=[robot_description, racecar_controller], output={ "stdout": "screen", "stderr": "screen" }, ) ])
def generate_launch_description(): gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource([ os.path.join(get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py' ]), ) gazebo_ros2_control_demos_path = os.path.join( get_package_share_directory('gazebo_ros2_control_demos')) xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf', 'test_cart_velocity.xacro.urdf') doc = xacro.parse(open(xacro_file)) xacro.process_doc(doc) params = {'robot_description': doc.toxml()} node_robot_state_publisher = Node(package='robot_state_publisher', executable='robot_state_publisher', output='screen', parameters=[params]) spawn_entity = Node( package='gazebo_ros', executable='spawn_entity.py', arguments=['-topic', 'robot_description', '-entity', 'cartpole'], output='screen') load_joint_state_controller = ExecuteProcess(cmd=[ 'ros2', 'control', 'load_controller', '--set-state', 'start', 'joint_state_broadcaster' ], output='screen') load_joint_trajectory_controller = ExecuteProcess(cmd=[ 'ros2', 'control', 'load_controller', '--set-state', 'start', 'velocity_controller' ], output='screen') load_imu_sensor_broadcaster = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', 'imu_sensor_broadcaster'], output='screen') return LaunchDescription([ RegisterEventHandler(event_handler=OnProcessExit( target_action=spawn_entity, on_exit=[load_joint_state_controller], )), RegisterEventHandler(event_handler=OnProcessExit( target_action=load_joint_state_controller, on_exit=[load_joint_trajectory_controller], )), RegisterEventHandler(event_handler=OnProcessExit( target_action=load_joint_trajectory_controller, on_exit=[load_imu_sensor_broadcaster], )), gazebo, node_robot_state_publisher, spawn_entity, ])
def quick_xacro(xml, cli=None, **kwargs): if cli: opts, _ = xacro.process_cli_args(cli, require_input=False) kwargs.update(vars(opts)) doc = xacro.parse(xml) xacro.process_doc(doc, **kwargs) return doc
def quick_xacro(self, xml, cli=None, **kwargs): args = {} if cli: opts, _ = xacro.cli.process_args(cli, require_input=False) args.update(vars(opts)) # initialize with cli args args.update(kwargs) # explicit function args have highest priority doc = xacro.parse(xml) xacro.process_doc(doc, **args) return doc
def quick_xacro(self, xml, cli=None, **kwargs): args = {} if cli: opts, _ = xacro.cli.process_args(cli, require_input=False) args.update(vars(opts)) # initialize with cli args args.update(dict(in_order = self.in_order)) # set in_order option from test class args.update(kwargs) # explicit function args have highest priority doc = xacro.parse(xml) xacro.process_doc(doc, **args) return doc
def main(): if len(sys.argv) == 1 or len(sys.argv) > 3 or 'help' in sys.argv or '--help' in sys.argv or '-h' in sys.argv: sys.stdout.write("""usage: {0} <input.sdf> [<output.urdf>]\n\n By default content is printed to stdout.\n\n""".format(sys.argv[0])) exit(1) if len(sys.argv) == 3 and os.path.realpath(sys.argv[1]) == os.path.realpath(sys.argv[2]): raise Exception("Input and output filenames is the same file") xml = parse(None, sys.argv[1]) output = sys.stdout if len(sys.argv) == 3: output = open(sys.argv[2], "w") doc = Item(xml) doc.convert() output.write(doc.toxml())
def xacro_string_to_payload(xacro_str, xacro_fname=None, **kwargs): if xacro_fname is not None: xacro.restore_filestack([xacro_fname]) else: xacro.restore_filestack([]) if 'mappings' not in kwargs: kwargs['mappings'] = {'payload_extensions': 'true'} doc = xacro.parse(xacro_str) xacro.process_doc(doc, **kwargs) urdf_str = doc.toprettyxml(indent=' ') return urdf_to_payload(urdf_str)
def __init__(self, timeline_frame): QWidget.__init__(self) self.timeline_frame = timeline_frame rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_bag_annotation'), 'resource', 'annotation_widget.ui') loadUi(ui_file, self) self.setObjectName('AnnotationWidget') rd_file = os.path.join(rp.get_path('turtlebot_description'), 'robots', 'kobuki_hexagons_kinect.urdf.xacro') a = xacro.parse(rd_file) xacro.process_includes(a, rd_file) xacro.eval_self_contained(a) rospy.set_param('robot_description', a.toxml()) self._cur_annotation = None self.save_button.clicked[bool].connect(self.save)
def _get_configured_urdf(self, config): """ Call the XACRO to URDF conversion using the parameters given in `config`. :param dynamic_reconfigure.Config config: Arguments of the XACRO model. :return: The URDF file contents. :rtype: basestring """ doc = xacro.parse(None, self._xacro_file) self.__remove_comments_from_xml_doc(doc) xacro.process_doc(doc, mappings=dict([(key, str(val)) for (key, val) in config.iteritems() ]), in_order=True) return doc.toxml()
def generate_launch_description(): # robot state publisher my_rotate_bot_path = os.path.join( get_package_share_directory(PACKAGE_NAME)) urdf_file = os.path.join(my_rotate_bot_path, 'urdf', 'model.urdf') doc = xacro.parse(open(urdf_file)) xacro.process_doc(doc) robot_description = {'robot_description': doc.toxml()} node_robot_state_publisher = Node(package='robot_state_publisher', executable='robot_state_publisher', output='screen', parameters=[robot_description]) node_joint_state_publisher = Node(package='joint_state_publisher', executable='joint_state_publisher', output='screen', parameters=[robot_description]) rviz_config_dir = os.path.join(get_package_share_directory(PACKAGE_NAME), 'config', 'rviz.config.rviz') # , rviz = Node(package='rviz2', node_executable='rviz2', node_name='rviz2', output='screen', arguments=['-d', rviz_config_dir]) joint_state_publisher_gui = Node( package='joint_state_publisher_gui', node_executable='joint_state_publisher_gui', node_name='joint_state_gui', output='screen') return LaunchDescription([ rviz, node_joint_state_publisher, joint_state_publisher_gui, node_robot_state_publisher, ])
def generate_launch_description(): gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource([ os.path.join(get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py' ]), ) bbot_path = os.path.join(get_package_share_directory('bbot')) xacro_file = os.path.join(bbot_path, 'sdf', 'robot.xacro.sdf') sdf_file = os.path.join(bbot_path, 'sdf', 'robot.sdf') doc = xacro.parse(open(xacro_file)) xacro.process_doc(doc) # Open a file fd = os.open(sdf_file, os.O_RDWR | os.O_CREAT) os.ftruncate(fd, 0) os.lseek(fd, 0, os.SEEK_SET) b = str.encode(doc.toxml()) os.write(fd, b) # Close opened file os.close(fd) spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', arguments=[ '-entity', 'bbot', '-file', sdf_file, '-x', '0.061475', '-y', '-0.058621', '-z', '0.058962', '-R', '-0.000043', '-P', '0.563955', '-Y', '0.425422' ], output='screen') return LaunchDescription([ gazebo, spawn_entity, ])
def generate_launch_description(): # robot state publisher my_rotate_bot_path = os.path.join( get_package_share_directory('my_rotate_bot')) urdf_file = os.path.join(my_rotate_bot_path, 'urdf', 'model.urdf') doc = xacro.parse(open(urdf_file)) xacro.process_doc(doc) robot_description = {'robot_description': doc.toxml()} # ros_control controllers_file = os.path.join(my_rotate_bot_path, 'controllers', 'ros2_control_controllers.yaml') controller_manager = Node(package="controller_manager", executable="ros2_control_node", parameters=[robot_description, controllers_file], output='screen') return LaunchDescription([ controller_manager, ])
def xacro(template_xml, **kwargs): doc = parse(template_xml) process_doc(doc, **kwargs) xml = doc.toprettyxml(indent=' ') xml = xml.replace(' xmlns:xacro="http://ros.org/wiki/xacro"', '', 1) return xml
def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') world_file_name = 'warehouse.world' pkg_dir = get_package_share_directory('orthopillar_robot_spawner_pkg') default_rviz_config_path = os.path.join(pkg_dir, 'rviz/config.rviz') os.environ["GAZEBO_MODEL_PATH"] = os.path.join(pkg_dir, 'models') world = os.path.join(pkg_dir, 'worlds', world_file_name) launch_file_dir = os.path.join(pkg_dir, 'launch') urdf_file = os.path.join(pkg_dir, 'models/orthopillar', 'model.urdf') doc = xacro.parse(open(urdf_file)) xacro.process_doc(doc) robot_description = {'robot_description': doc.toxml()} #urdf_file_name = 'urdf/model.urdf.xml' #urdf = os.path.join( # get_package_share_directory('orthopillar_robot_spawner_pkg'), # urdf_file_name) #with open(urdf, 'r') as infp: # robot_description = infp.read() robot_state_publisher_node = launch_ros.actions.Node( package='robot_state_publisher', executable='robot_state_publisher', output='screen', parameters=[robot_description]) joint_state_publisher_node = launch_ros.actions.Node( package='joint_state_publisher', executable='joint_state_publisher', name='joint_state_publisher', #condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui')) ) joint_state_publisher_gui_node = launch_ros.actions.Node( package='joint_state_publisher_gui', executable='joint_state_publisher_gui', name='joint_state_publisher_gui', #condition=launch.conditions.IfCondition(LaunchConfiguration('gui')) ) rviz_node = launch_ros.actions.Node( package='rviz2', executable='rviz2', name='rviz2', output='screen', arguments=['-d', LaunchConfiguration('rvizconfig')], ) gazebo = ExecuteProcess(cmd=[ 'gazebo', '--verbose', world, '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so' ], output='screen') #GAZEBO_MODEL_PATH has to be correctly set for Gazebo to be able to find the model #spawn_entity = Node(package='gazebo_ros', node_executable='spawn_entity.py', # arguments=['-entity', 'demo', 'x', 'y', 'z'], # output='screen') spawn_entity = Node( package='orthopillar_robot_spawner_pkg', executable='spawn_demo', arguments=['orthopillar', 'demo', '-1.5', '-4.0', '0.0'], output='screen') #robot_state_publisher = Node( package='robot_state_publisher', # executable='robot_state_publisher', # name='robot_state_publisher', # output='screen', # parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_description}], # arguments=[urdf]) #state_publisher = Node( package='orthopillar_robot_spawner_pkg', # executable='state_publisher', # name='state_publisher', # output='screen') return launch.LaunchDescription([ launch.actions.DeclareLaunchArgument( name='rvizconfig', default_value=default_rviz_config_path, description='Absolute path to rviz config file'), gazebo, spawn_entity, robot_state_publisher_node, joint_state_publisher_node, joint_state_publisher_gui_node, #state_publisher rviz_node ])
def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') package_name = "gcamp_gazebo" robot_file = "skidbot.urdf" rviz_file = "test.rviz" gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join(get_package_share_directory("gazebo_ros"), "launch"), "/gazebo.launch.py", ] ), ) urdf = os.path.join(get_package_share_directory(package_name), "urdf", robot_file) rviz = os.path.join(get_package_share_directory(package_name), "rviz", rviz_file) xml = open(urdf, "r").read() xml = xml.replace('"', '\\"') xacro_file = os.path.join( get_package_share_directory(package_name), "urdf", robot_file ) doc = xacro.parse(open(xacro_file)) xacro.process_doc(doc) params = {"robot_description": doc.toxml()} # node_robot_state_publisher = Node( # package="robot_state_publisher", # executable="robot_state_publisher", # output="screen", # parameters=[params], # ) with open(urdf, 'r') as infp: robot_desc = infp.read() robot_state_publisher_node = Node( package='robot_state_publisher', executable='robot_state_publisher', parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}], arguments=[urdf], # parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}] ) joint_state_publisher_node = Node( package='joint_state_publisher', executable='joint_state_publisher', name='joint_state_publisher', condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui')) ) joint_state_publisher_gui_node = Node( package='joint_state_publisher_gui', executable='joint_state_publisher_gui', name='joint_state_publisher_gui', condition=launch.conditions.IfCondition(LaunchConfiguration('gui')) ) rviz_node = Node( package='rviz2', executable='rviz2', name='rviz2', arguments=['-d', LaunchConfiguration('rvizconfig')], ) spawn_entity = Node( package="gazebo_ros", executable="spawn_entity.py", arguments=["-topic", "robot_description", "-entity"], output="screen", ) return LaunchDescription( [ DeclareLaunchArgument(name='gui', default_value='True', description='Flag to enable joint_state_publisher_gui'), DeclareLaunchArgument(name='rvizconfig', default_value=rviz, description='Absolute path to rviz config file'), DeclareLaunchArgument(name='model', default_value=urdf, description='Absolute path to robot urdf file'), robot_state_publisher_node, joint_state_publisher_node, joint_state_publisher_gui_node, rviz_node, # gazebo, # spawn_entity, ] )
def generate_launch_description(): gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), ) xacro_file = os.path.join(get_package_share_directory('turtlebot3_manipulation_gazebo'), 'urdf', 'turtlebot3_pi_manipulator.xacro') doc = xacro.parse(open(xacro_file)) xacro.process_doc(doc) params = {'robot_description': doc.toxml()} node_robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', output='screen', parameters=[params] ) spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', arguments=['-topic', 'robot_description', '-entity', 'omx'], # TODO - 2) change entity INSTANCE name # (if spawning multiple robots, each -entity needs to be unique) output='screen') load_joint_state_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], output='screen' ) load_joint_trajectory_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'], output='screen' ) # TODO 3): if adding effort or velocity controllers, add here as above: these depend on ros2-control and # ros2_controllers # load_velocity_controller = ExecuteProcess( # cmd=['ros2', 'control', 'load_start_controller', 'velocity_controller'], # output='screen' # ) load_effort_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_start_controller', 'effort_controllers'], output='screen' ) return LaunchDescription([ RegisterEventHandler( event_handler=OnProcessExit( target_action=spawn_entity, on_exit=[load_joint_state_controller], ) ), RegisterEventHandler( event_handler=OnProcessExit( target_action=load_joint_state_controller, on_exit=[load_joint_trajectory_controller, load_effort_controller], ) ), gazebo, node_robot_state_publisher, spawn_entity, ])
def xacro_parse(filename): doc = xacro.parse(None, filename) xacro.process_doc(doc, in_order=True) return doc.toprettyxml(indent=' ')
def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') rviz_file = "skidbot.rviz" robot_file = "skidbot.urdf" package_name = "gcamp_gazebo" world_file_name = "gcamp_world.world" pkg_path = os.path.join(get_package_share_directory(package_name)) pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') urdf_file = os.path.join(pkg_path, "urdf", robot_file) rviz_config = os.path.join(pkg_path, "rviz", rviz_file) world_path = os.path.join(pkg_path, "worlds", world_file_name) # Start Gazebo server start_gazebo_server_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')), launch_arguments={'world': world_path}.items() ) # Start Gazebo client start_gazebo_client_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')) ) # Robot State Publisher doc = xacro.parse(open(urdf_file)) xacro.process_doc(doc) robot_description = {'robot_description': doc.toxml()} robot_state_publisher_node = Node( package='robot_state_publisher', executable='robot_state_publisher', output='screen', parameters=[robot_description] ) spawn_entity = Node( package='gazebo_ros', executable='spawn_entity.py', output='screen', arguments=['-topic', 'robot_description', '-entity', 'skidbot'], ) rviz_start = ExecuteProcess( cmd=["ros2", "run", "rviz2", "rviz2", "-d", rviz_config], output="screen" ) # create and return launch description object return LaunchDescription( [ TimerAction( period=3.0, actions=[rviz_start] ), # start gazebo, notice we are using libgazebo_ros_factory.so instead of libgazebo_ros_init.so # That is because only libgazebo_ros_factory.so contains the service call to /spawn_entity # ExecuteProcess( # cmd=["gazebo", "--verbose", world_path, "-s", "libgazebo_ros_factory.so"], # output="screen", # ), start_gazebo_server_cmd, start_gazebo_client_cmd, robot_state_publisher_node, # tell gazebo to spwan your robot in the world by calling service spawn_entity, ] )
def generate_launch_description(): gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource([ os.path.join(get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py' ]), ) # NOTE: Load the processed xacro file directly description_location = os.path.join( get_package_share_directory('racecar_description')) xacro_file = os.path.join(description_location, 'urdf', 'racecar.xacro') doc = xacro.parse(open(xacro_file)) xacro.process_doc(doc) robot_description_value = doc.toxml() params = {"robot_description": robot_description_value} node_robot_state_publisher = Node(package='robot_state_publisher', executable='robot_state_publisher', output='screen', parameters=[params]) spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', arguments=[ '-topic', 'robot_description', '-entity', 'racecar', '-x', '0', '-y', '0', '-z', '0.5' ], output='screen') # NOTE: THIS IS DEPRECATED VERSION OF LOADING load_joint_state_controller = ExecuteProcess(cmd=[ 'ros2', 'control', 'load_start_controller', 'joint_state_broadcaster' ], output='screen') # NOTE: THIS IS DEPRECATED VERSION OF LOADING CONTROLLER controllers = [ 'left_rear_wheel_velocity_controller', 'right_rear_wheel_velocity_controller', 'left_front_wheel_velocity_controller', 'right_front_wheel_velocity_controller', 'left_steering_hinge_position_controller', 'right_steering_hinge_position_controller' ] load_controllers = [ ExecuteProcess(cmd=['ros2', 'control', 'load_start_controller', state], output='screen') for state in controllers ] racecar_controller = os.path.join( get_package_share_directory("racecar_control"), "config", "racecar_control.yaml", ) return LaunchDescription([ RegisterEventHandler(event_handler=OnProcessExit( target_action=spawn_entity, on_exit=[load_joint_state_controller], )), RegisterEventHandler(event_handler=OnProcessExit( target_action=load_joint_state_controller, on_exit=load_controllers, )), gazebo, node_robot_state_publisher, spawn_entity, ])
def __init__(self, path=None, data=None): assert data or path self._dom = xacro.parse(data, path) joints, links = get_links_and_joints(self._dom) self._links = parse_links(links) self._joints = tuple(parse_joints(joints, self._links))