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_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 _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_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 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 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 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 _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 __init__(self, description_file=None, load=True): if description_file is None and len(sys.argv) > 1: description_file = sys.argv[1] self._save_files = rospy.get_param('~save_files', False) self._path_to_save_files = rospy.get_param('~path_to_save_files', "/tmp/") self._file_name = rospy.get_param('~file_name', "generated_robot") # ARM self.rospack = rospkg.RosPack() self.package_path = self.rospack.get_path('sr_multi_moveit_config') self.robot = Robot() with open(description_file, "r") as stream: yamldoc = yaml.load(stream) self.robot.set_parameters(yamldoc) new_srdf_file_name = "generated_robot.srdf" self.start_new_srdf(new_srdf_file_name) for manipulator in self.robot.manipulators: if manipulator.has_arm: # Read arm srdf arm_srdf_path = manipulator.arm.moveit_path + "/" + manipulator.arm.name + ".srdf" with open(arm_srdf_path, 'r') as stream: self.arm_srdf_xml = parse(stream) xacro.process_includes(self.arm_srdf_xml, os.path.dirname(sys.argv[0])) xacro.process_doc(self.arm_srdf_xml) if manipulator.has_hand: # Generate and read hand srdf hand_urdf_path = self.rospack.get_path( 'sr_description') + "/robots/" + manipulator.hand.name with open(hand_urdf_path, 'r') as hand_urdf_xacro_file: hand_urdf_xml = parse(hand_urdf_xacro_file) xacro.process_includes(hand_urdf_xml, os.path.dirname(sys.argv[0])) xacro.process_doc(hand_urdf_xml) hand_urdf = hand_urdf_xml.toprettyxml(indent=' ') srdfHandGenerator = SRDFHandGenerator(hand_urdf, load=False, save=False) self.hand_srdf_xml = srdfHandGenerator.get_hand_srdf() comment = ["Manipulator:" + manipulator.name] self.add_comments(comment) # Add groups and group states if manipulator.has_arm: self.parse_arm_groups(manipulator) if manipulator.has_hand: self.parse_hand_groups(manipulator) # Add end effectors comment = [ "END EFFECTOR: Purpose: Represent information about an end effector." ] self.add_comments(comment) if manipulator.has_hand: self.parse_hand_end_effectors(manipulator) if manipulator.has_arm: self.parse_arm_end_effectors(manipulator) # Add virtual joints if manipulator.has_arm: pass # self.parse_arm_virtual_joint(manipulator) else: self.parse_hand_virtual_joint(manipulator) # Add disable collisions comment = [ "DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially " + "come into collision with any other link in the robot. This tag disables collision checking " + "between a specified pair of links." ] self.add_comments(comment) if manipulator.has_arm: self.parse_arm_collisions(manipulator) if manipulator.has_hand: self.parse_hand_collisions(manipulator) # Finish and close file self.new_robot_srdf.write('</robot>\n') self.new_robot_srdf.close() if load: with open(self.package_path + "/config/" + new_srdf_file_name, 'r') as stream: srdf = parse(stream) rospy.loginfo("Loading SRDF on parameter server") robot_description_param = rospy.resolve_name( 'robot_description') + "_semantic" rospy.set_param(robot_description_param, srdf.toprettyxml(indent=' ')) if self._save_files: rospy.loginfo("Robot urdf and srdf have been saved to %s" % self._path_to_save_files) # srdf: File is already generated so just need to be copied to specified location copy2(self.package_path + "/config/" + new_srdf_file_name, self._path_to_save_files + "/" + self._file_name + ".srdf") # urdf: File can be copied from rosparam if rospy.has_param('/robot_description'): urdf_str = rospy.get_param('/robot_description') urdf_file = open( self._path_to_save_files + "/" + self._file_name + ".urdf", "wb") urdf_file.write(urdf_str) urdf_file.close() rospy.loginfo("generated_robot.srdf has been generated and saved.")
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 xacro_parse(filename): doc = xacro.parse(None, filename) xacro.process_doc(doc, in_order=True) return doc.toprettyxml(indent=' ')
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 __init__(self, urdf_str=None, load=True, save=True): if urdf_str is None: while not rospy.has_param('robot_description'): rospy.sleep(0.5) rospy.loginfo("waiting for robot_description") # load the urdf from the parameter server urdf_str = rospy.get_param('robot_description') robot = URDF.from_xml_string(urdf_str) extracted_prefix = False prefix = "" ff = mf = rf = lf = th = False is_lite = True is_biotac = False hand_name = "right_hand" # Check if hand has the old biotac sensors for key in robot.link_map: link = robot.link_map[key] if link.visual: if hasattr(link.visual.geometry, 'filename'): filename = os.path.basename(link.visual.geometry.filename) if filename == "biotac_decimated.dae": is_biotac = True break for key in robot.joint_map: # any joint is supposed to have the same prefix and a joint name with 4 chars if not extracted_prefix: prefix = key.split("_")[0] + "_" rospy.loginfo("Found prefix:" + prefix) extracted_prefix = True if prefix == "lh_": hand_name = "left_hand" if not ff and key.endswith("FFJ4"): ff = True if not mf and key.endswith("MFJ4"): mf = True if not rf and key.endswith("RFJ4"): rf = True if not lf and key.endswith("LFJ4"): lf = True if not th and key.endswith("THJ4"): th = True if is_lite and key.endswith("WRJ2"): is_lite = False rospy.logdebug("Found fingers (ff mf rf lf th)" + str(ff) + str(mf) + str(rf) + str(lf) + str(th)) rospy.logdebug("is_lite: " + str(is_lite)) rospy.logdebug("is_biotac: " + str(is_biotac)) rospy.logdebug("Hand name: " + str(hand_name)) mappings = load_mappings([ 'prefix:=' + str(prefix), 'robot_name:=' + robot.name, 'ff:=' + str(int(ff)), 'mf:=' + str(int(mf)), 'rf:=' + str(int(rf)), 'lf:=' + str(int(lf)), 'th:=' + str(int(th)), 'is_lite:=' + str(int(is_lite)), 'is_biotac:=' + str(int(is_biotac)), 'hand_name:=' + str(hand_name) ]) # the prefix version of the srdf_xacro must be loaded rospack = rospkg.RosPack() package_path = rospack.get_path('sr_moveit_hand_config') srdf_xacro_filename = package_path + "/config/shadowhands_prefix.srdf.xacro" rospy.loginfo("File loaded " + srdf_xacro_filename) # open and parse the xacro.srdf file srdf_xacro_file = open(srdf_xacro_filename, 'r') self.srdf_xacro_xml = parse(srdf_xacro_file) # expand the xacro xacro.process_includes(self.srdf_xacro_xml, os.path.dirname(sys.argv[0])) xacro.process_doc(self.srdf_xacro_xml, mappings=mappings) if len(sys.argv) > 1: OUTPUT_PATH = sys.argv[1] # reject ROS internal parameters and detect termination if (OUTPUT_PATH.startswith("_") or OUTPUT_PATH.startswith("--")): OUTPUT_PATH = None else: OUTPUT_PATH = None if load: rospy.loginfo("Loading SRDF on parameter server") robot_description_param = rospy.resolve_name( 'robot_description') + "_semantic" rospy.set_param(robot_description_param, self.srdf_xacro_xml.toprettyxml(indent=' ')) if save: OUTPUT_PATH = package_path + "/config/generated_shadowhand.srdf" FW = open(OUTPUT_PATH, "wb") FW.write(self.srdf_xacro_xml.toprettyxml(indent=' ')) FW.close() OUTPUT_PATH = package_path + "/config/generated_shadowhand.urdf" FW = open(OUTPUT_PATH, "wb") FW.write(urdf_str) FW.close() srdf_xacro_file.close()
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') 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 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 ])