Packages created in the standard Cmake format rather than previous depreciated Python format

URDF_Exporter_Ros2/URDF_Exporter_Ros2.py

@@ -80,12 +80,12 @@ def run(context):
         Write.write_transmissions_xacro(joints_dict, links_xyz_dict, inertial_dict, package_name, robot_name, save_dir)
         Write.write_gazebo_xacro(joints_dict, links_xyz_dict, inertial_dict, package_name, robot_name, save_dir)
         Write.write_display_launch(package_name, robot_name, save_dir)
-        Write.write_gazebo_launch(package_name, robot_name, save_dir)
+        Write.write_state_publisher_launch(package_name, robot_name, save_dir)
+        Write.write_rviz_launch(package_name, robot_name, save_dir)
 
         # copy over package files
-        utils.create_package(package_name, save_dir, package_dir)
-        utils.update_setup_py(save_dir, package_name)
-        utils.update_setup_cfg(save_dir, package_name)
+        utils.create_package(save_dir, package_dir)
+        utils.update_cmakelists(save_dir, package_name)
         utils.update_package_xml(save_dir, package_name)
 
         # Generate STl files

URDF_Exporter_Ros2/core/Write.py

@@ -269,9 +269,9 @@ def write_display_launch(package_name, robot_name, save_dir):
     with open(file_name, mode='w') as f:
         f.write(file_text)
 
-def write_gazebo_launch(package_name, robot_name, save_dir):
+def write_state_publisher_launch(package_name, robot_name, save_dir):
     """
-    write gazebo launch file "save_dir/launch/gazebo.launch"
+    write state_publisher launch file "save_dir/launch/state_publisher.launch"
 
 
     Parameter
@@ -285,8 +285,30 @@ def write_gazebo_launch(package_name, robot_name, save_dir):
     try: os.mkdir(save_dir + '/launch')
     except: pass
 
-    file_text = launch_templates.get_gazebo_launch_text(package_name, robot_name)
+    file_text = launch_templates.get_state_publisher_launch_text(package_name, robot_name)
 
-    file_name = os.path.join(save_dir, 'launch', 'gazebo.launch.py')
+    file_name = os.path.join(save_dir, 'launch', 'state_publisher.launch.py')
     with open(file_name, mode='w') as f:
         f.write(file_text)
+
+def write_rviz_launch(package_name, robot_name, save_dir):
+    """
+    write rviz launch file "save_dir/launch/rviz.launch"
+
+
+    Parameter
+    ---------
+    robot_name: str
+        name of the robot
+    save_dir: str
+        path of the repository to save
+    """
+
+    try: os.mkdir(save_dir + '/launch')
+    except: pass
+
+    file_text = launch_templates.get_rviz_launch_text(package_name, robot_name)
+
+    file_name = os.path.join(save_dir, 'launch', 'rviz.launch.py')
+    with open(file_name, mode='w') as f:
+        f.write(file_text)

URDF_Exporter_Ros2/core/launch_templates.py

@@ -1,148 +1,134 @@
-display_launch = """from launch_ros.actions import Node
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
-from launch.conditions import IfCondition, UnlessCondition
-import xacro
+display_launch = """import launch
+from launch.substitutions import Command, LaunchConfiguration
+import launch_ros
+from launch.conditions import IfCondition
 import os
-from ament_index_python.packages import get_package_share_directory
-
+from launch_ros.descriptions import ParameterValue
 
 def generate_launch_description():
-    share_dir = get_package_share_directory('%s')
-
-    xacro_file = os.path.join(share_dir, 'urdf', '%s.xacro')
-    robot_description_config = xacro.process_file(xacro_file)
-    robot_urdf = robot_description_config.toxml()
-
-    rviz_config_file = os.path.join(share_dir, 'config', 'display.rviz')
-
-    gui_arg = DeclareLaunchArgument(
-        name='gui',
-        default_value='True'
-    )
-
-    show_gui = LaunchConfiguration('gui')
-
-    robot_state_publisher_node = Node(
+    pkg_share = launch_ros.substitutions.FindPackageShare(package='%s').find('%s')
+    default_model_path = os.path.join(pkg_share, 'urdf/%s.xacro')
+    default_rviz_config_path = os.path.join(pkg_share, 'config/display.rviz')
+    world_path = os.path.join(pkg_share, 'worlds/room.sdf')
+    # sdf_path = os.path.join(pkg_share, 'models/urdf/%s/model.sdf')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    robot_state_publisher_node = launch_ros.actions.Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
-        name='robot_state_publisher',
-        parameters=[
-            {'robot_description': robot_urdf}
-        ]
+        parameters=[{'use_sim_time': use_sim_time}, {'robot_description': ParameterValue(Command(['xacro ', LaunchConfiguration('model')]), value_type=str)}]
     )
-
-    joint_state_publisher_node = Node(
-        condition=UnlessCondition(show_gui),
+    joint_state_publisher_node = launch_ros.actions.Node(
         package='joint_state_publisher',
         executable='joint_state_publisher',
-        name='joint_state_publisher'
+        name='joint_state_publisher',
+        parameters=[{'use_sim_time': use_sim_time}],
     )
-
-    joint_state_publisher_gui_node = Node(
-        condition=IfCondition(show_gui),
-        package='joint_state_publisher_gui',
-        executable='joint_state_publisher_gui',
-        name='joint_state_publisher_gui'
-    )
-
-    rviz_node = Node(
+    rviz_node = launch_ros.actions.Node(
         package='rviz2',
         executable='rviz2',
         name='rviz2',
-        arguments=['-d', rviz_config_file],
+        output='screen',
+        arguments=['-d', LaunchConfiguration('rvizconfig')],
+        parameters=[{'use_sim_time': use_sim_time}],
+    )
+    spawn_entity = launch_ros.actions.Node(
+        condition=IfCondition(use_sim_time),
+        package='gazebo_ros',
+        executable='spawn_entity.py',
+        arguments=['-entity', '%s', '-topic', 'robot_description'],
+        parameters=[{'use_sim_time': use_sim_time}],
         output='screen'
     )
 
-    return LaunchDescription([
-        gui_arg,
-        robot_state_publisher_node,
+    return launch.LaunchDescription([
+        launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='True',
+                                            description='Flag to enable use_sim_time'),
+        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
+                                            description='Absolute path to robot urdf file'),
+        launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
+                                            description='Absolute path to rviz config file'),
+        launch.actions.ExecuteProcess(condition=IfCondition(use_sim_time), cmd=['gazebo', '--verbose', '-s',
+                                            'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', world_path],
+                                        output='screen'),
         joint_state_publisher_node,
-        joint_state_publisher_gui_node,
-        rviz_node
+        robot_state_publisher_node,
+        spawn_entity,
+        rviz_node,
     ])
 """
 
-gazebo_launch = """from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import PathJoinSubstitution
-import os
-import xacro
-from ament_index_python.packages import get_package_share_directory
 
+state_publisher_launch = """import launch
+import os
+from launch.substitutions import Command, LaunchConfiguration
+import launch_ros
+from launch_ros.descriptions import ParameterValue
 
 def generate_launch_description():
-    share_dir = get_package_share_directory('%s')
-
-    xacro_file = os.path.join(share_dir, 'urdf', '%s.xacro')
-    robot_description_config = xacro.process_file(xacro_file)
-    robot_urdf = robot_description_config.toxml()
-
-    robot_state_publisher_node = Node(
+    pkg_share = launch_ros.substitutions.FindPackageShare(package='%s').find('%s')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    default_model_path = os.path.join(pkg_share, 'urdf/%s.xacro')
+    robot_state_publisher_node = launch_ros.actions.Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
-        name='robot_state_publisher',
-        parameters=[
-            {'robot_description': robot_urdf}
-        ]
+        parameters=[{'use_sim_time': use_sim_time}, {'robot_description': ParameterValue(Command(['xacro ', LaunchConfiguration('model')]), value_type=str)}]
     )
-
-    joint_state_publisher_node = Node(
+    joint_state_publisher_node = launch_ros.actions.Node(
         package='joint_state_publisher',
         executable='joint_state_publisher',
-        name='joint_state_publisher'
+        name='joint_state_publisher',
+        parameters=[{'use_sim_time': use_sim_time}],
     )
+    return launch.LaunchDescription([
+        launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='True',
+                                    description='Flag to enable use_sim_time'),
+        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
+                                            description='Absolute path to robot urdf file'),
+        robot_state_publisher_node,
+        joint_state_publisher_node
+    ])
+"""
+rviz_launch = """import launch
+import os
+from launch.substitutions import Command, LaunchConfiguration
+import launch_ros
+from launch_ros.descriptions import ParameterValue
 
-    gazebo_server = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([
-            PathJoinSubstitution([
-                FindPackageShare('gazebo_ros'),
-                'launch',
-                'gzserver.launch.py'
-            ])
-        ]),
-        launch_arguments={
-            'pause': 'true'
-        }.items()
-    )
+def generate_launch_description():
+    pkg_share = launch_ros.substitutions.FindPackageShare(package='%s').find('%s')
+    default_model_path = os.path.join(pkg_share, 'urdf/%s.xacro')
+    default_rviz_config_path = os.path.join(pkg_share, 'config/display.rviz')
+    use_sim_time = LaunchConfiguration('use_sim_time')
 
-    gazebo_client = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([
-            PathJoinSubstitution([
-                FindPackageShare('gazebo_ros'),
-                'launch',
-                'gzclient.launch.py'
-            ])
-        ])
-    )
+    rviz_node = launch_ros.actions.Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', LaunchConfiguration('rvizconfig')],
+        parameters= [{'use_sim_time': use_sim_time}],
 
-    urdf_spawn_node = Node(
-        package='gazebo_ros',
-        executable='spawn_entity.py',
-        arguments=[
-            '-entity', '%s',
-            '-topic', 'robot_description'
-        ],
-        output='screen'
     )
 
-    return LaunchDescription([
-        robot_state_publisher_node,
-        joint_state_publisher_node,
-        gazebo_server,
-        gazebo_client,
-        urdf_spawn_node,
+
+    return launch.LaunchDescription([
+        launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='False',
+                                    description='Flag to enable use_sim_time'),
+
+        launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
+                                            description='Absolute path to rviz config file'),
+
+
+        rviz_node
     ])
 """
 
-
 def get_display_launch_text(package_name, robot_name):
-    return display_launch % (package_name, robot_name)
+    return display_launch % (package_name, package_name, robot_name, robot_name, robot_name)
+
 
+def get_state_publisher_launch_text(package_name, robot_name):
+    return state_publisher_launch % (package_name, package_name, robot_name)
 
-def get_gazebo_launch_text(package_name, robot_name):
-    return gazebo_launch % (package_name, robot_name, robot_name)
+def get_rviz_launch_text(package_name, robot_name):
+    return rviz_launch % (package_name, package_name, robot_name)

URDF_Exporter_Ros2/package/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.8)
+project(fusion2urdf)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+install(
+  DIRECTORY launch urdf config meshes worlds
+  DESTINATION share/${PROJECT_NAME}
+)
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

URDF_Exporter_Ros2/package/package.xml

@@ -1,24 +1,24 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>package_name</name>
+  <name>fusion2urdf</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>The fusion2urdf package</description>
   <maintainer email="[email protected]">author</maintainer>
   <license>TODO: License declaration</license>
 
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
   <depend>xacro</depend>
   <depend>rviz2</depend>
   <depend>robot_state_publisher</depend>
   <depend>joint_state_publisher</depend>
   <depend>joint_state_publisher_gui</depend>
-
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
 
   <export>
-    <build_type>ament_python</build_type>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>