Further nav2 work

src/dcaiti_control/launch/launch_sim.py

@@ -25,7 +25,7 @@ def generate_launch_description():
     package_name='dcaiti_control' #<--- CHANGE ME
 
     use_ros2_control = LaunchConfiguration('use_ros2_control')
-    world_path='~/.gazebo/models'
+    world_path='worlds/cafe.world'
 
     rsp = IncludeLaunchDescription(
                 PythonLaunchDescriptionSource([os.path.join(
@@ -34,6 +34,8 @@ def generate_launch_description():
     )
     gazebo_params_path = os.path.join(get_package_share_directory(package_name),'config','gazebo_config.yml')
     # Include the Gazebo launch file, provided by the gazebo_ros package
+  
+
     gazebo = IncludeLaunchDescription(
                 PythonLaunchDescriptionSource([os.path.join(
                     get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
@@ -44,7 +46,8 @@ def generate_launch_description():
     spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
         arguments=[
             '-topic', 'robot_description',
-            '-entity', 'dcaiti_control'
+            '-entity', 'dcaiti_control',
+            '-z', '0.2',
             ],
         output='screen'
     )

src/dcaiti_control/launch/rsp.launch.py

@@ -32,7 +32,8 @@ def generate_launch_description():
 
     node_joint_state_publisher = Node(
         package='joint_state_publisher',
-        executable='joint_state_publisher'
+        executable='joint_state_publisher',
+        parameters=[{'use_sim_time': use_sim_time}]
     )
 
 

src/dcaiti_navigation/launch/start_nav2.py

@@ -14,10 +14,7 @@ from launch_ros.substitutions import FindPackageShare
 def generate_launch_description():
 
   # Set the path to different files and folders.
-  pkg_share = get_package_share_directory('dcaiti_control')
+  pkg_share = get_package_share_directory('dcaiti_navigation')
-  world_file_name = 'basic_mobile_bot_world/smalltown.world'
-  world_path = os.path.join(pkg_share, 'worlds', world_file_name)
-  static_map_path = os.path.join(pkg_share, 'maps', 'smalltown_world.yaml')
   nav2_params_path = os.path.join(pkg_share, 'params', 'nav2_params.yaml')
   behavior_tree_xml_path = os.path.join(get_package_share_directory('nav2_bt_navigator'), 'behavior_trees', 'navigate_w_replanning_and_recovery.xml')
   
@@ -61,64 +58,21 @@ def generate_launch_description():
     default_value=behavior_tree_xml_path,
     description='Full path to the behavior tree xml file to use')
     
-  declare_map_yaml_cmd = DeclareLaunchArgument(
-    name='map',
-    default_value=static_map_path,
-    description='Full path to map file to load')
-    
   declare_params_file_cmd = DeclareLaunchArgument(
     name='params_file',
     default_value=nav2_params_path,
     description='Full path to the ROS2 parameters file to use for all launched nodes')
     
-  declare_simulator_cmd = DeclareLaunchArgument(
-    name='headless',
-    default_value='False',
-    description='Whether to execute gzclient')
-
-  declare_slam_cmd = DeclareLaunchArgument(
-    name='slam',
-    default_value='False',
-    description='Whether to run SLAM')
-    
-  declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
-    name='use_robot_state_pub',
-    default_value='True',
-    description='Whether to start the robot state publisher')
-
-  declare_use_rviz_cmd = DeclareLaunchArgument(
-    name='use_rviz',
-    default_value='True',
-    description='Whether to start RVIZ')
-    
   declare_use_sim_time_cmd = DeclareLaunchArgument(
     name='use_sim_time',
     default_value='True',
     description='Use simulation (Gazebo) clock if true')
    
-  declare_use_simulator_cmd = DeclareLaunchArgument(
-    name='use_simulator',
-    default_value='True',
-    description='Whether to start the simulator')
-
-  declare_world_cmd = DeclareLaunchArgument(
-    name='world',
-    default_value=world_path,
-    description='Full path to the world model file to load')
-   
   # Specify the actions   
 
   # Launch the ROS 2 Navigation Stack
   start_ros2_navigation_cmd = IncludeLaunchDescription(
-    PythonLaunchDescriptionSource(get_package_share_directory('nav2'),'launch','bringup_launch.py'),
+    PythonLaunchDescriptionSource(get_package_share_directory('nav2'),'launch','navigation_launch.py'))
-    launch_arguments = {'namespace': namespace,
-                        'use_namespace': use_namespace,
-                        'slam': slam,
-                        'map': map_yaml_file,
-                        'use_sim_time': use_sim_time,
-                        'params_file': params_file,
-                        'default_bt_xml_filename': default_bt_xml_filename,
-                        'autostart': autostart}.items())
 
   # Create the launch description and populate
   ld = LaunchDescription()
@@ -128,15 +82,8 @@ def generate_launch_description():
   ld.add_action(declare_use_namespace_cmd)
   ld.add_action(declare_autostart_cmd)
   ld.add_action(declare_bt_xml_cmd)
-  ld.add_action(declare_map_yaml_cmd)
   ld.add_action(declare_params_file_cmd)
-  ld.add_action(declare_simulator_cmd)
-  ld.add_action(declare_slam_cmd)
-  ld.add_action(declare_use_robot_state_pub_cmd)  
-  ld.add_action(declare_use_rviz_cmd) 
   ld.add_action(declare_use_sim_time_cmd)
-  ld.add_action(declare_use_simulator_cmd)
-  ld.add_action(declare_world_cmd)
 
   # Add any actions
   ld.add_action(start_ros2_navigation_cmd)

src/dcaiti_navigation/params/nav2_params.yaml

@@ -188,7 +188,7 @@ local_costmap:
       width: 10
       height: 10
       resolution: 0.05
-      robot_radius: 0.75
+      footprint: "[ [0.21, 0.195], [0.21, -0.195], [-0.21, -0.195], [-0.21, 0.195] ]"
       plugins: ["voxel_layer", "inflation_layer"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
@@ -232,7 +232,7 @@ global_costmap:
       global_frame: map
       robot_base_frame: base_link
       use_sim_time: True
-      robot_radius: 0.75
+      robot_radius: 0.21
       resolution: 0.05
       track_unknown_space: true
       plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
@@ -339,12 +339,10 @@ slam_toolbox:
     ceres_dogleg_type: TRADITIONAL_DOGLEG
     ceres_loss_function: None
 
-    # ROS Parameters
-    odom_frame: odom
-    map_frame: map
     base_frame: base_link
+    map_update_interval: 1.0
     scan_topic: /scan
-    mode: mapping #localization
+    odom_topic: /diff_cont/odom
 
     # if you'd like to immediately start continuing a map at a given pose
     # or at the dock, but they are mutually exclusive, if pose is given

src/dcaitirobot/config/slam.yml

@@ -2,5 +2,5 @@ slam_toolbox:
   ros__parameters:
     base_frame: base_link
     map_update_interval: 1.0
-    scan_topic: /scan_no_nan
+    scan_topic: /scan
-    odom_topic: /odom
+    odom_topic: /diff_cont/odom

src/dcaitirobot/launch/launch_slam.py

@@ -24,27 +24,6 @@ def generate_launch_description():
                     'slam_params_file': slam_params}.items()
     )
 
-    node_fix_lidar = Node(
-                package="dcaitirobot",
-                executable="fixlidar",
-                name="fixlidar",
-            )
-
-    node_scale_odom = Node(
-                package="dcaitirobot",
-                executable="scaleodom",
-                name="scaleodom",
-            )
-
-    tfs1 = Node(package = "tf2_ros", 
-                       executable = "static_transform_publisher",
-                       arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "velodyne"])
-
-    tfs2 = Node(package = "tf2_ros", 
-                       executable = "static_transform_publisher",
-                       arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "base_footprint"])
-
-
     return LaunchDescription([
-        slam_toolbox, tfs1, tfs2, node_fix_lidar, node_scale_odom
+        slam_toolbox
     ])