preliminary nav2 stuff

utahrobotics · Sep 25, 2023 · 80369f0 · 80369f0
1 parent 441b8e3
commit 80369f0
Show file tree

Hide file tree

Showing 10 changed files with 248 additions and 17 deletions.
diff --git a/lunadev/luna.dockerfile b/lunadev/luna.dockerfile
@@ -56,14 +56,19 @@ RUN apt-get install ros-humble-nav2-bringup '~ros-humble-turtlebot3-.*' -y --no-
 # Install rviz2
 RUN apt-get install -y --no-install-recommends \
     ros-humble-rviz2 \
-    ros-humble-rviz-visual-tools
+    ros-humble-rviz-visual-tools \
+    ros-humble-imu-tools
 
 # Install VNC and socat
 RUN apt-get install -y x11vnc socat xvfb
 
 # Install Localization algorithm
 RUN apt-get install -y --no-install-recommends ros-humble-robot-localization
 
+# Install mapping and localization package
+# Note: This is quite big
+RUN apt-get install -y --no-install-recommends ros-humble-rtabmap-ros
+
 # Configure git
 RUN git config --global pull.rebase false
 

diff --git a/src/bot_descriptions/CMakeLists.txt b/src/bot_descriptions/CMakeLists.txt
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(bot_descriptions)
+
+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 src
+  DESTINATION share/${PROJECT_NAME}
+)
+
+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()
+
+ament_package()
diff --git a/src/bot_descriptions/package.xml b/src/bot_descriptions/package.xml
@@ -0,0 +1,18 @@
+<?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>bot_descriptions</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/bot_descriptions/src/description/lunabot_description.urdf b/src/bot_descriptions/src/description/lunabot_description.urdf
@@ -0,0 +1,89 @@
+<?xml version="1.0"?>
+<robot name="lunabot" xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <!-- Define robot constants -->
+    <xacro:property name="base_width" value="0.31"/>
+    <xacro:property name="base_length" value="0.42"/>
+    <xacro:property name="base_height" value="0.18"/>
+
+    <xacro:property name="wheel_radius" value="0.10"/>
+    <xacro:property name="wheel_width" value="0.04"/>
+    <xacro:property name="wheel_ygap" value="0.025"/>
+    <xacro:property name="wheel_zoff" value="0.05"/>
+    <xacro:property name="wheel_xoff" value="0.12"/>
+
+    <!-- Robot Base -->
+    <link name="base_link">
+        <visual>
+            <geometry>
+                <box size="${base_length} ${base_width} ${base_height}"/>
+            </geometry>
+            <material name="Cyan">
+                <color rgba="0 1.0 1.0 1.0"/>
+            </material>
+        </visual>
+    </link>
+
+    <!-- Robot Footprint -->
+    <link name="base_footprint"/>
+
+    <joint name="base_joint" type="fixed">
+        <parent link="base_link"/>
+        <child link="base_footprint"/>
+        <origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
+    </joint>
+
+    <link name="pozyx_frame">
+        <visual>
+            <geometry>
+                <box size="0.1 0.1 0.1"/>
+            </geometry>
+        </visual>
+    </link>
+
+    <joint name="pozyx_joint" type="fixed">
+        <parent link="base_link"/>
+        <child link="pozyx_frame"/>
+        <origin xyz="0 0 0.01"/>
+    </joint>
+
+    <link name="camera_imu_optical_frame">
+        <visual>
+            <geometry>
+                <box size="0.1 0.1 0.1"/>
+            </geometry>
+        </visual>
+    </link>
+
+    <joint name="camera_imu_optical_joint" type="fixed">
+        <parent link="base_link"/>
+        <child link="camera_imu_optical_frame"/>
+        <origin xyz="0 0 0.01"/>
+    </joint>
+
+    <!-- Wheels -->
+    <!-- <xacro:macro name="wheel" params="prefix x_reflect y_reflect">
+    <link name="${prefix}_link">
+        <visual>
+        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
+        <geometry>
+            <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
+        </geometry>
+        <material name="Gray">
+            <color rgba="0.5 0.5 0.5 1.0"/>
+        </material>
+        </visual>
+    </link>
+
+    <joint name="${prefix}_joint" type="continuous">
+        <parent link="base_link"/>
+        <child link="${prefix}_link"/>
+        <origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/>
+        <axis xyz="0 1 0"/>
+    </joint>
+    </xacro:macro>
+
+    <xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />
+    <xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" /> -->
+
+</robot>
diff --git a/src/launchers/config/ekf.yaml b/src/launchers/config/ekf.yaml
@@ -0,0 +1,47 @@
+### ekf config file ###
+ekf_filter_node:
+    ros__parameters:
+# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+# computation until it receives at least one message from one of theinputs. It will then run continuously at the
+# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+        frequency: 30.0
+
+# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+# by, for example, an IMU. Defaults to false if unspecified.
+        two_d_mode: true
+
+# Whether to publish the acceleration state. Defaults to false if unspecified.
+        publish_acceleration: true
+
+# Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified.
+        publish_tf: true
+
+# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
+# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"
+#    to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
+#    observations) then:
+#     3a. Set your "world_frame" to your map_frame value
+#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
+#         from robot_localization! However, that instance should *not* fuse the global data.
+        map_frame: map              # Defaults to "map" if unspecified
+        odom_frame: odom            # Defaults to "odom" if unspecified
+        base_link_frame: base_link  # Defaults to "base_link" ifunspecified
+        world_frame: odom           # Defaults to the value ofodom_frame if unspecified
+
+        pose0: /pozyx_pose
+        pse0_config: [true,  true,  true,
+                       true, true, true,
+                       false, false, false,
+                       false, false, false,
+                       false, false, false]
+
+        imu0: /camera/imu
+        imu0_config: [false, false, false,
+                      false,  false,  false,
+                      true, true, true,
+                      true, true, true,
+                      false, false, false]
diff --git a/src/launchers/launch/lunabot.py b/src/launchers/launch/lunabot.py
@@ -5,17 +5,40 @@
 
 from launch import LaunchDescription
 from launch_ros.actions import Node
-from launch.actions import IncludeLaunchDescription
+from launch_ros.substitutions import FindPackageShare
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, LaunchConfiguration
 
 
 def generate_launch_description():
+    launchers_pkg_share = get_package_share_directory('launchers')
+    robot_desc_pkg_share = get_package_share_directory('bot_descriptions')
+    default_model_path = os.path.join(
+        robot_desc_pkg_share,
+        'src/description/lunabot_description.urdf'
+    )
+
     return LaunchDescription([
         # Node(
         #     package='drive',
         #     executable='drive',
         #     name='drive'
         # ),
+        # Declare udf
+        DeclareLaunchArgument(
+            name='model',
+            default_value=default_model_path,
+            description='Absolute path to robot urdf file'
+        ),
+        # Start URDF publisher
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            parameters=[
+                {'robot_description': Command(['xacro ', LaunchConfiguration('model')])}
+            ]
+        ),
         # Start telemetry
         Node(
             package='telemetry',
@@ -34,11 +57,22 @@ def generate_launch_description():
             executable='positioning',
             name='positioning'
         ),
+        # Start localization algorithm
+        Node(
+            package='robot_localization',
+            executable='ekf_node',
+            name='ekf_filter',
+            output='screen',
+            parameters=[
+                os.path.join(launchers_pkg_share, 'config/ekf.yaml'),
+                # {'use_sim_time': LaunchConfiguration('use_sim_time')}
+            ]
+        ),
         # Start RealSense camera
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource([
                 os.path.join(
-                    get_package_share_directory('launchers'), 'launch'
+                    launchers_pkg_share, 'launch'
                 ),
                 '/rs_launch.py'
             ])

diff --git a/src/launchers/launch/rs_launch.py b/src/launchers/launch/rs_launch.py
@@ -27,7 +27,7 @@
     {'name': 'usb_port_id',                  'default': "''", 'description': 'choose device by usb port id'},
     {'name': 'device_type',                  'default': "''", 'description': 'choose device by type'},
     {'name': 'config_file',                  'default': "''", 'description': 'yaml config file'},
-    {'name': 'unite_imu_method',             'default': "0", 'description': '[0-None, 1-copy, 2-linear_interpolation]'},
+    {'name': 'unite_imu_method',             'default': "2", 'description': '[0-None, 1-copy, 2-linear_interpolation]'},
     {'name': 'json_file_path',               'default': "''", 'description': 'allows advanced configuration'},
     {'name': 'log_level',                    'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'},
     {'name': 'output',                       'default': 'screen', 'description': 'pipe node output [screen|log]'},
@@ -59,7 +59,7 @@
     {'name': 'initial_reset',                'default': 'false', 'description': "''"},                           
     {'name': 'allow_no_texture_points',      'default': 'false', 'description': "''"},                           
     {'name': 'pointcloud.ordered_pc',        'default': 'false', 'description': ''},
-    {'name': 'publish_tf',                   'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'},
+    {'name': 'publish_tf',                   'default': 'false', 'description': '[bool] enable/disable publishing static & dynamic TF'},
     {'name': 'tf_publish_rate',              'default': '0.0', 'description': '[double] rate in Hz for publishing dynamic TF'},
     {'name': 'diagnostics_period',           'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'},
     {'name': 'decimation_filter.enable',     'default': 'false', 'description': 'Rate of publishing static_tf'},

diff --git a/src/launchers/package.xml b/src/launchers/package.xml
@@ -7,6 +7,10 @@
   <maintainer email="[email protected]">root</maintainer>
   <license>TODO: License declaration</license>
 
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>robot_localization</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

diff --git a/src/launchers/setup.py b/src/launchers/setup.py
@@ -12,6 +12,10 @@
         (
             os.path.join('share', package_name, 'launch'),
             glob(os.path.join('launch', '*.[pxy][yma]*'))
+        ),
+        (
+            os.path.join('share', package_name, 'config'),
+            glob(os.path.join('config', '*.yaml'))
         )
     ],
     install_requires=['setuptools'],

diff --git a/src/positioning/positioning/positioning.py b/src/positioning/positioning/positioning.py
@@ -2,21 +2,17 @@
 from threading import Thread
 from rclpy.node import Node
 from positioning.pozyx_localizer import PozyxLocalizer
-from geometry_msgs.msg import Pose, PoseWithCovariance
+from geometry_msgs.msg import PoseWithCovariance, PoseWithCovarianceStamped
+from std_msgs.msg import Header
 
 
 class Positioning(Node):
     def __init__(self):
         super().__init__("positioning")
         self.localizer = PozyxLocalizer("PozyxConfig.yaml")
-        self.pose_cov_pub = self.create_publisher(
-            PoseWithCovariance,
-            'pozyx_pose',
-            10
-        )
         self.pose_pub = self.create_publisher(
-            Pose,
-            'pozyx_pose_no_cov',
+            PoseWithCovarianceStamped,
+            'pozyx_pose',
             10
         )
 
@@ -27,11 +23,14 @@ def run(self):
         try:
             while True:
                 self.localizer.loop()
-                self.pose_cov_pub.publish(
-                    PoseWithCovariance(pose=self.localizer.pose)
-                )
                 self.pose_pub.publish(
-                    self.localizer.pose
+                    PoseWithCovarianceStamped(
+                        header=Header(
+                            stamp=self.get_clock().now().to_msg(),
+                            frame_id="pozyx_frame"
+                        ),
+                        pose=PoseWithCovariance(pose=self.localizer.pose)
+                    )
                 )
         except KeyboardInterrupt:
             return