Added almost working nav2 setup

.gitmodules

@@ -1,3 +1,6 @@
 [submodule "src/serial"]
 	path = src/serial
 	url = https://github.com/joshnewans/serial.git
+[submodule "src/twist_mux"]
+	path = src/twist_mux
+	url = https://github.com/ros-teleop/twist_mux.git

src/dcaiti_control/config/dcaiti_config.yml

@@ -20,7 +20,6 @@ diff_cont:
     wheel_separation: 0.215
     wheel_radius: 0.049338032
 
-
     use_stamped_vel: false
 
 joint_limits:

src/dcaiti_control/config/twist_mux.yml

@@ -0,0 +1,12 @@
+twist_mux:
+  ros__parameters:
+    topics:
+      navigation:
+        topic   : cmd_vel_nav
+        timeout : 0.5
+        priority: 10
+
+      keyboard:
+        topic   : cmd_vel_keyboard
+        timeout : 0.5
+        priority: 100

src/dcaiti_control/launch/rsp.launch.py

@@ -4,6 +4,8 @@ from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
 from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import IncludeLaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch_ros.actions import Node
 from launch.substitutions import Command
@@ -22,6 +24,12 @@ def generate_launch_description():
     xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
     robot_description_config = Command(['xacro ', xacro_file, ' use_ros2_control:=', use_ros2_control, ' use_sim:=', use_sim_time])
     
+    twist_mux_params = os.path.join(
+        get_package_share_directory('dcaiti_control'), # <-- Replace with your package name
+        'config',
+        'twist_mux.yml'
+        )
+
     # Create a robot_state_publisher node
     params = {'robot_description': robot_description_config, 'use_sim_time': use_sim_time}
     node_robot_state_publisher = Node(
@@ -36,6 +44,14 @@ def generate_launch_description():
         parameters=[{'use_sim_time': use_sim_time}]
     )
 
+    twist_mux = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+            get_package_share_directory('twist_mux'),'launch','twist_mux_launch.py'
+        )]), 
+        launch_arguments={
+            'cmd_vel_out': '/diff_cont/cmd_vel_unstamped', 
+            'config_topics': twist_mux_params}.items()
+    )
 
 
     # Launch!
@@ -50,5 +66,6 @@ def generate_launch_description():
             description='Use ros2_control if true'),
 
         node_robot_state_publisher,
-        node_joint_state_publisher
+        node_joint_state_publisher,
+        twist_mux,
     ])

src/dcaiti_navigation/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(dcaiti_navigation)
+
+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)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(
+  DIRECTORY config params launch worlds
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/dcaiti_navigation/launch/start_nav2.py

@@ -4,9 +4,10 @@
 # https://automaticaddison.com
 
 import os
+from pathlib import Path
 from ament_index_python import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import Command, LaunchConfiguration, PythonExpression
 from launch_ros.substitutions import FindPackageShare
@@ -28,15 +29,6 @@ def generate_launch_description():
   use_namespace = LaunchConfiguration('use_namespace')
   use_sim_time = LaunchConfiguration('use_sim_time')
   
-  # Map fully qualified names to relative ones so the node's namespace can be prepended.
-  # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
-  # https://github.com/ros/geometry2/issues/32
-  # https://github.com/ros/robot_state_publisher/pull/30
-  # TODO(orduno) Substitute with `PushNodeRemapping`
-  #              https://github.com/ros2/launch_ros/issues/56
-  remappings = [('/tf', 'tf'),
-                ('/tf_static', 'tf_static')]
-  
   # Declare the launch arguments  
   declare_namespace_cmd = DeclareLaunchArgument(
     name='namespace',
@@ -68,24 +60,39 @@ def generate_launch_description():
     default_value='True',
     description='Use simulation (Gazebo) clock if true')
 
-  # Specify the actions   
+  slam_params = os.path.join(
+        get_package_share_directory('dcaiti_navigation'), # <-- Replace with your package name
+        'params',
+        'nav2_params.yaml'
+        )
+  
 
   # Launch the ROS 2 Navigation Stack
   start_ros2_navigation_cmd = IncludeLaunchDescription(
-    PythonLaunchDescriptionSource(get_package_share_directory('nav2'),'launch','navigation_launch.py'))
+    PythonLaunchDescriptionSource([os.path.join(
+    get_package_share_directory('nav2_bringup'),'launch','navigation_launch.py')]))
+  
+
+  
+  slam_toolbox = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('slam_toolbox'),'launch','online_async_launch.py'
+            )]), 
+            launch_arguments={
+                'use_sim_time': 'true', 
+                'slam_params_file': slam_params}.items()
+  )
 
   # Create the launch description and populate
-  ld = LaunchDescription()
+  ld = LaunchDescription([
-
+      declare_namespace_cmd,
-  # Declare the launch options
+      declare_use_namespace_cmd,
-  ld.add_action(declare_namespace_cmd)
+      declare_autostart_cmd,
-  ld.add_action(declare_use_namespace_cmd)
+      declare_bt_xml_cmd,
-  ld.add_action(declare_autostart_cmd)
+      declare_params_file_cmd,
-  ld.add_action(declare_bt_xml_cmd)
+      declare_use_sim_time_cmd,
-  ld.add_action(declare_params_file_cmd)
+      start_ros2_navigation_cmd,
-  ld.add_action(declare_use_sim_time_cmd)
+      slam_toolbox,
-
+  ])
-  # Add any actions
-  ld.add_action(start_ros2_navigation_cmd)
 
   return ld

src/dcaiti_navigation/package.xml

@@ -7,12 +7,12 @@
   <maintainer email="you@example.com">korjakow</maintainer>
   <license>TODO: License declaration</license>
 
-  <test_depend>ament_copyright</test_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
-  <test_depend>ament_flake8</test_depend>
+
-  <test_depend>ament_pep257</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>python3-pytest</test_depend>
+  <test_depend>ament_lint_common</test_depend>
 
   <export>
-    <build_type>ament_python</build_type>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>

src/dcaiti_navigation/params/nav2_params.yaml

@@ -32,7 +32,7 @@ amcl:
     set_initial_pose: False
     sigma_hit: 0.2
     tf_broadcast: true
-    transform_tolerance: 1.0
+    transform_tolerance: 5.0
     update_min_a: 0.2
     update_min_d: 0.25
     z_hit: 0.5
@@ -155,7 +155,7 @@ controller_server:
       sim_time: 1.7
       linear_granularity: 0.05
       angular_granularity: 0.025
-      transform_tolerance: 0.2
+      transform_tolerance: 5.0
       xy_goal_tolerance: 0.25
       trans_stopped_velocity: 0.25
       short_circuit_trajectory_evaluation: True
@@ -354,9 +354,9 @@ slam_toolbox:
     debug_logging: false
     throttle_scans: 1
     transform_publish_period: 0.02 #if 0 never publishes odometry
-    map_update_interval: 5.0
+    map_update_interval: 0.5
     resolution: 0.05
-    max_laser_range: 20.0 #for rastering images
+    max_laser_range: 100.0 #for rastering images
     minimum_time_interval: 0.5
     transform_timeout: 0.2
     tf_buffer_duration: 30.

src/dcaiti_navigation/setup.cfg

@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/dcaiti_navigation
-[install]
-install_scripts=$base/lib/dcaiti_navigation

src/dcaiti_navigation/setup.py

@@ -1,28 +0,0 @@
-from glob import glob
-import os
-from setuptools import find_packages, setup
-
-package_name = 'dcaiti_navigation'
-
-setup(
-    name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
-    data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name), glob('launch/*.py')),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='korjakow',
-    maintainer_email='you@example.com',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
-    entry_points={
-        'console_scripts': [
-        ],
-    },
-)

src/dcaiti_navigation/test/test_copyright.py

@@ -1,25 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'

src/dcaiti_navigation/test/test_flake8.py

@@ -1,25 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
-    rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)

src/dcaiti_navigation/test/test_pep257.py

@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'

src/dcaitirobot/.gitignore

@@ -47,6 +47,3 @@ qtcreator-*
 
 # Emacs
 .#*
-
-# Catkin custom files
-CATKIN_IGNORE

src/dcaitirobot/setup.py

@@ -13,8 +13,7 @@ setup(
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
         (os.path.join('share', package_name), glob('launch/*.py')),
-        (os.path.join('share', 'config'), glob('config/*.yml')),
+        ],
-    ],
     install_requires=['setuptools'],
     zip_safe=True,
     maintainer='nvidia',

src/twist_mux

@@ -0,0 +1 @@
+Subproject commit c50e8bd4bdcff856969979b0b640fad9190b17a8