avidbots · JoaoCostaIFG · Nov 28, 2022 · Nov 28, 2022 · Nov 28, 2022 · Nov 28, 2022
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,39 +1,30 @@
-cmake_minimum_required(VERSION 3.16)
+cmake_minimum_required(VERSION 3.5)
 project(turtlebot_flatland)
 
 # Ensure we're using c++11
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-)
-
+find_package(ament_cmake REQUIRED)
 
-catkin_package(
-)
 
 ###########
 ## Build ##
 ###########
 
-include_directories(
-  ${catkin_INCLUDE_DIRS}
-)
-
 install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  DESTINATION share/${PROJECT_NAME}/
 )
 
 install(DIRECTORY maps
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  DESTINATION share/${PROJECT_NAME}/
 )
 
 install(DIRECTORY robot
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  DESTINATION share/${PROJECT_NAME}/
 )
 
 install(DIRECTORY rviz
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  DESTINATION share/${PROJECT_NAME}/
 )
+
+ament_package()
diff --git a/launch/turtlebot_in_flatland.launch b/launch/turtlebot_in_flatland.launch
@@ -26,80 +26,80 @@
   You can override these default values:
     roslaunch flatland_Server server.launch world_path:="/some/world.yaml" initial_rate:="30.0"
   -->
-  <arg name="world_path"   default="$(find turtlebot_flatland)/maps/hospital_section.world.yaml"/>
+  <arg name="world_path"   default="$(find-pkg-share turtlebot_flatland)/maps/hospital_section.world.yaml"/>
   <arg name="update_rate" default="100.0"/>
   <arg name="step_size" default="0.01"/>
   <arg name="viz_pub_rate" default="30.0"/>
   <arg name="show_viz" default="true"/>
 
-  <env name="ROSCONSOLE_FORMAT" value="[${severity} ${time} ${logger}]: ${message}" />
+  <set_env name="ROSCONSOLE_FORMAT" value="[${severity} ${time} ${logger}]: ${message}" />
 
-  <param name="use_sim_time" value="true"/>  
+  <arg name="use_sim_time" default="true"/>
 
   <!-- launch flatland server -->
-  <node name="flatland_server" pkg="flatland_server" type="flatland_server" output="screen">  
+  <node name="flatland_server" pkg="flatland_server" exec="flatland_server" output="screen">  
     <!-- Use the arguments passed into the launchfile for this node -->
-    <param name="world_path" value="$(arg world_path)" />
-    <param name="update_rate" value="$(arg update_rate)" />
-    <param name="step_size" value="$(arg step_size)" />
-    <param name="show_viz" value="$(arg show_viz)" />
-    <param name="viz_pub_rate" value="$(arg viz_pub_rate)" />
+    <param name="world_path" value="$(var world_path)" />
+    <param name="update_rate" value="$(var update_rate)" />
+    <param name="step_size" value="$(var step_size)" />
+    <param name="show_viz" value="$(var show_viz)" />
+    <param name="viz_pub_rate" value="$(var viz_pub_rate)" />
 
   </node>
 
   <!--  ***************** Robot Model *****************  -->
-  <node name="spawn_model" pkg="rosservice" type="rosservice"
-    args="call --wait /spawn_model &quot;{
-      yaml_path: '$(find turtlebot_flatland)/robot/turtlebot.model.yaml',
-      name: 'turtlebot0',
-      ns: '',
-      pose: {x: $(arg initial_pose_x), y: $(arg initial_pose_y), theta: $(arg initial_pose_a)}}&quot;"
-    />
+<!--   <node name="spawn_model" pkg="rosservice" exec="rosservice" -->
+<!--     args="call -/\-wait /spawn_model &quot;{ -->
+<!--       yaml_path: '$(find-pkg-share turtlebot_flatland)/robot/turtlebot.model.yaml', -->
+<!--       name: 'turtlebot0', -->
+<!--       ns: '', -->
+<!--       pose: {x: $(var initial_pose_x), y: $(var initial_pose_y), theta: $(var initial_pose_a)}}&quot;" -->
+<!--     /> -->
 
   <!-- Command Velocity multiplexer -->
-  <node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
-  <node pkg="nodelet" type="nodelet" name="cmd_vel_mux" args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
-    <param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml"/>
-    <remap from="cmd_vel_mux/output" to="cmd_vel"/>
-  </node>
+<!--   <node pkg="nodelet" exec="nodelet" name="mobile_base_nodelet_manager" args="manager"/> -->
+<!--   <node pkg="nodelet" exec="nodelet" name="cmd_vel_mux" args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager"> -->
+<!--     <param name="yaml_cfg_file" value="$(find-pkg-share turtlebot_bringup)/param/mux.yaml"/> -->
+<!--     <remap from="cmd_vel_mux/output" to="cmd_vel"/> -->
+<!--   </node> -->
 
   <!-- ****** Maps ***** -->
-  <node name="map_server" pkg="map_server" type="map_server" args="$(find turtlebot_flatland)/maps/hospital_section.yaml">
-   <param name="frame_id" value="$(arg global_frame_id)"/>
+  <node name="map_server" pkg="nav2_map_server" exec="map_server" args="$(find-pkg-share turtlebot_flatland)/maps/hospital_section.yaml">
+   <param name="frame_id" value="$(var global_frame_id)"/>
   </node>
 
   <!--  ************** Navigation  ***************  -->
 
-  <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml">
-   <arg name="odom_topic" value="$(arg odom_topic)"/>
-   <arg name="laser_topic" value="$(arg laser_topic)"/>
-   <arg name="odom_frame_id"   value="$(arg odom_frame_id)"/>
-   <arg name="base_frame_id"   value="$(arg base_frame_id)"/>
-   <arg name="global_frame_id" value="$(arg global_frame_id)"/>
-  </include>
+  <!-- <include file="$(find-pkg-share turtlebot3_navigation2)/launch/includes/move_base.launch.xml"> -->
+  <!--  <arg name="odom_topic" value="$(var odom_topic)"/> -->
+  <!--  <arg name="laser_topic" value="$(var laser_topic)"/> -->
+  <!--  <arg name="odom_frame_id"   value="$(var odom_frame_id)"/> -->
+  <!--  <arg name="base_frame_id"   value="$(var base_frame_id)"/> -->
+  <!--  <arg name="global_frame_id" value="$(var global_frame_id)"/> -->
+  <!-- </include> -->
 
  <!-- ***************** Manually setting some parameters ************************* -->
-    <param name="move_base/local_costmap/obstacle_layer/scan/min_obstacle_height" value="$(arg min_obstacle_height)"/>
-    <param name="move_base/local_costmap/obstacle_layer/scan/max_obstacle_height" value="$(arg max_obstacle_height)"/>
-    <param name="move_base/global_costmap/obstacle_layer/scan/min_obstacle_height" value="$(arg min_obstacle_height)"/>
-    <param name="move_base/global_costmap/obstacle_layer/scan/max_obstacle_height" value="$(arg max_obstacle_height)"/>
+    <arg name="move_base/local_costmap/obstacle_layer/scan/min_obstacle_height" default="$(var min_obstacle_height)"/>
+    <arg name="move_base/local_costmap/obstacle_layer/scan/max_obstacle_height" default="$(var max_obstacle_height)"/>
+    <arg name="move_base/global_costmap/obstacle_layer/scan/min_obstacle_height" default="$(var min_obstacle_height)"/>
+    <arg name="move_base/global_costmap/obstacle_layer/scan/max_obstacle_height" default="$(var max_obstacle_height)"/>
 
 
   <!--  ************** AMCL ************** -->
-  <include file="$(find turtlebot_navigation)/launch/includes/amcl/amcl.launch.xml">
-    <arg name="scan_topic" value="$(arg laser_topic)"/>
-    <arg name="use_map_topic" value="true"/>
-    <arg name="odom_frame_id" value="$(arg odom_frame_id)"/>
-    <arg name="base_frame_id" value="$(arg base_frame_id)"/>
-    <arg name="global_frame_id" value="$(arg global_frame_id)"/>
-    <arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
-    <arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
-    <arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
-  </include>
+  <!-- <include file="$(find-pkg-share turtlebot3_navigation2)/launch/includes/amcl/amcl.launch.xml"> -->
+  <!--   <arg name="scan_topic" value="$(var laser_topic)"/> -->
+  <!--   <arg name="use_map_topic" value="true"/> -->
+  <!--   <arg name="odom_frame_id" value="$(var odom_frame_id)"/> -->
+  <!--   <arg name="base_frame_id" value="$(var base_frame_id)"/> -->
+  <!--   <arg name="global_frame_id" value="$(var global_frame_id)"/> -->
+  <!--   <arg name="initial_pose_x" value="$(var initial_pose_x)"/> -->
+  <!--   <arg name="initial_pose_y" value="$(var initial_pose_y)"/> -->
+  <!--   <arg name="initial_pose_a" value="$(var initial_pose_a)"/> -->
+  <!-- </include> -->
 
   <!--  **************** Visualisation ****************  -->
-  <group if="$(arg show_viz)">
-    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot_flatland)/rviz/robot_navigation.rviz"/>
+  <group if="$(var show_viz)">
+    <node name="rviz" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share turtlebot_flatland)/rviz/robot_navigation.rviz"/>
   </group>
 
 </launch>
diff --git a/launch/turtlebot_in_flatland.launch.py b/launch/turtlebot_in_flatland.launch.py
@@ -0,0 +1,138 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, ExecuteProcess
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, FindExecutable
+import launch.conditions as conditions
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    #pkg_share = get_package_share_directory("turtlebot_flatland")
+    pkg_share = FindPackageShare("turtlebot_flatland")
+
+    global_frame_id = LaunchConfiguration("global_frame_id")
+    min_obstacle_height = LaunchConfiguration("min_obstacle_height")
+    max_obstacle_height = LaunchConfiguration("max_obstacle_height")
+
+    initial_pose_x = LaunchConfiguration("initial_pose_x")
+    initial_pose_y = LaunchConfiguration("initial_pose_y")
+    initial_pose_a = LaunchConfiguration("initial_pose_a")
+
+    world_path = LaunchConfiguration("world_path")
+    update_rate = LaunchConfiguration("update_rate")
+    step_size = LaunchConfiguration("step_size")
+    show_viz = LaunchConfiguration("show_viz")
+    viz_pub_rate = LaunchConfiguration("viz_pub_rate")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+
+    ld = LaunchDescription(
+        [
+            DeclareLaunchArgument(name="laser_topic", default_value="scan"), # default laser topic in flatland
+            DeclareLaunchArgument(name="odom_topic", default_value="odom"),
+            DeclareLaunchArgument(name="odom_frame_id", default_value="odom"),
+            DeclareLaunchArgument(name="base_frame_id", default_value="base"),
+            DeclareLaunchArgument(name="global_frame_id", default_value="map"),
+            # Name of the map to use (without path nor extension) and initial position
+            DeclareLaunchArgument(name="initial_pose_x", default_value="3.0"),
+            DeclareLaunchArgument(name="initial_pose_y", default_value="7.0"),
+            DeclareLaunchArgument(name="initial_pose_a", default_value="0.0"),
+            DeclareLaunchArgument(name="min_obstacle_height", default_value="0.0"),
+            DeclareLaunchArgument(name="max_obstacle_height", default_value="5.0"),
+
+            # ******************** flatland********************
+            # You can override these default values:
+            #   roslaunch flatland_Server server.launch world_path:="/some/world.yaml" initial_rate:="30.0"
+            DeclareLaunchArgument(
+                name="world_path",
+                default_value=PathJoinSubstitution([pkg_share, "maps/hospital_section.world.yaml"]),
+            ),
+            DeclareLaunchArgument(name="update_rate", default_value="100.0"),
+            DeclareLaunchArgument(name="step_size", default_value="0.01"),
+            DeclareLaunchArgument(name="show_viz", default_value="true"),
+            DeclareLaunchArgument(name="viz_pub_rate", default_value="30.0"),
+            DeclareLaunchArgument(name="use_sim_time", default_value="true"),
+
+            SetEnvironmentVariable(name="ROSCONSOLE_FORMAT", value="[${severity} ${time} ${logger}]: ${message}"),
+
+            # launch flatland server
+            Node(
+                name="flatland_server",
+                package="flatland_server",
+                executable="flatland_server",
+                output="screen",
+                parameters=[
+                    # Use the arguments passed into the launchfile for this node
+                    {"world_path": world_path},
+                    {"update_rate": update_rate},
+                    {"step_size": step_size},
+                    {"show_viz": show_viz},
+                    {"viz_pub_rate": viz_pub_rate},
+                    {"use_sim_time": use_sim_time},
+                ],
+            ),
+
+            # ***************** Robot Model *****************
+            ExecuteProcess(
+                cmd=[[
+                    FindExecutable(name='ros2'),
+                    " service ",
+                    "call ",
+                    "/spawn_model ",
+                    "flatland_msgs/srv/SpawnModel ",
+                    "\"{yaml_path: '", PathJoinSubstitution([pkg_share, 'robot/turtlebot.model.yaml']),
+                    "', name: 'turtlebot0', ns: '', pose: ",
+                    "{x: ", initial_pose_x, ", y: ", initial_pose_y, ", theta: ", initial_pose_a, "}}\"",
+                ]],
+                shell=True,
+            ),
+
+            # ****** Maps *****
+            Node(
+                name="map_server",
+                package="nav2_map_server",
+                executable="map_server",
+                output='screen',
+                parameters=[
+                    {"yaml_filename": PathJoinSubstitution([pkg_share, "maps/hospital_section.yaml"])},
+                    {"frame_id": global_frame_id},
+                ],
+            ),
+            Node(
+                name='lifecycle_manager_navigation',
+                package='nav2_lifecycle_manager',
+                executable='lifecycle_manager',
+                output='screen',
+                parameters=[
+                    {'autostart': False},
+                    {'node_names': ["map_server"]},
+                ],
+            ),
+            Node(
+                name="tf",
+                package="tf2_ros",
+                executable="static_transform_publisher",
+                arguments=["0", "0", "0", "0", "0", "0", "map", "odom"],
+            ),
+
+            # ***************** Manually setting some parameters *************************
+            DeclareLaunchArgument(name="move_base/local_costmap/obstacle_layer/scan/min_obstacle_height", default_value=min_obstacle_height),
+            DeclareLaunchArgument(name="move_base/local_costmap/obstacle_layer/scan/max_obstacle_height", default_value=max_obstacle_height),
+            DeclareLaunchArgument(name="move_base/global_costmap/obstacle_layer/scan/min_obstacle_height", default_value=min_obstacle_height),
+            DeclareLaunchArgument(name="move_base/global_costmap/obstacle_layer/scan/max_obstacle_height", default_value=max_obstacle_height),
+
+            # **************** Visualisation ****************
+            Node(
+                name="rviz",
+                package="rviz2",
+                executable="rviz2",
+                arguments=["-d", PathJoinSubstitution([pkg_share, "rviz/robot_navigation.rviz"])],
+                parameters=[{"use_sim_time": use_sim_time}],
+                condition=conditions.IfCondition(show_viz),
+            ),
+        ]
+    )
+    return ld
+
+
+if __name__ == "__main__":
+    generate_launch_description()
diff --git a/package.xml b/package.xml
@@ -10,13 +10,16 @@
   <author email="[email protected]">Joseph Duchesne</author>
   <license>BSD</license>
 
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <buildtool_depend>catkin</buildtool_depend>
-  <run_depend>flatland_msgs</run_depend>
-  <run_depend>flatland_plugins</run_depend>
-  <run_depend>navigation</run_depend>
-  <run_depend>turtlebot_bringup</run_depend>
-  <run_depend>turtlebot_navigation</run_depend>
-  <run_depend>yocs_virtual_sensor</run_depend>
-  <run_depend>yocs_velocity_smoother</run_depend>
+  <!-- <run_depend>flatland_msgs</run_depend> -->
+  <!-- <run_depend>flatland_plugins</run_depend> -->
+  <run_depend>nav2_map_server</run_depend>
+  <run_depend>nav2_rviz_plugins</run_depend>
+  <run_depend>turtlebot3_bringup</run_depend>
+  <run_depend>turtlebot3_navigation2</run_depend>
+  <!-- <run_depend>yocs_virtual_sensor</run_depend> -->
+  <!-- <run_depend>yocs_velocity_smoother</run_depend> -->
+
+  <export><build_type>ament_cmake</build_type></export>
 </package>