add ekf support

config/ekf.yaml

@@ -0,0 +1,60 @@
+### 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 #was 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
+
+        odom0: /odom
+        # The order of the values of this parameter is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az.
+        # Configure it for x,y,z,vyaw on the odom topic
+        odom0_config: [true , true , false,
+                       false, false, true,
+                       false, false, false,
+                       false, false, true,
+                       false, false, false]
+        odom0_differential: true
+        odom0_relative: true
+        
+        imu0: /imu
+        # The order of the values of this parameter is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az.
+        # Configure it for ax,ay,az linear acceleration data for now. Shall not contain values already derived by each other.
+        # Example: Since angular velocity is fused internally to the IMU to provide the roll, pitch and yaw estimates, we should not fuse in the angular velocities used to derive that information. We also do not fuse in angular velocity due to the noisy characteristics it has when not using exceptionally high quality (and expensive) IMUs.
+        imu0_config: [false, false, false,
+                      false, false, false,
+                      false, false, false,
+                      false, false, true,
+                      true, true, false]
+        imu0_differential: true
+        imu0_relative: true
+
+        # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
+        # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
+        imu0_remove_gravitational_acceleration: true

launch/robot_localization.launch.py

@@ -0,0 +1,148 @@
+# Copyright 2022 Factor Robotics
+#
+# 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.
+
+import os
+from launch import LaunchDescription
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import LifecycleNode
+from launch_ros.descriptions import ParameterValue
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("cps_rmp220_support"),
+                    "description",
+                    "robot.urdf.xacro"
+                ]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("cps_rmp220_support"),
+            "config",
+            "diffbot_controllers.yaml",
+        ]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, robot_controllers],
+        output="both",
+    )
+
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
+    )
+
+    robot_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["diffbot_base_controller", "-c", "/controller_manager"],
+    )
+
+    joystick_spawner = Node(
+        package="joy",
+        executable="joy_node"
+    )
+
+    teleop_spawner = Node(
+        package="rmp220_teleop",
+        executable="rmp220_teleop"
+    )
+
+    cam_node = Node(
+        package="ros2_cam_openCV",
+        executable="cam_node"
+    )
+
+    use_sim_time = False
+    slam_params_file = PathJoinSubstitution(
+                [
+                    FindPackageShare("cps_rmp220_support"),
+                    "config",
+                    "mapper_params_online_async.yaml"
+                ]
+            )
+    mapper_node = Node(
+        package="slam_toolbox",
+        executable="async_slam_toolbox_node",
+        name='slam_toolbox_node',
+        output='screen',
+        parameters=[
+            slam_params_file,
+            {'use_sim_time': use_sim_time}
+        ],
+    )
+
+    lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml')         
+    lidar_node = LifecycleNode(
+        package='lslidar_driver',
+        executable='lslidar_driver_node',
+        name='lslidar_driver_node',
+        output='screen',
+        emulate_tty=True,
+        namespace='',
+        parameters=[lidar_dir],
+    )
+
+    robot_localization_node = Node(
+       package='robot_localization',
+       executable='ekf_node',
+       name='ekf_filter_node',
+       output='screen',
+       parameters = [ PathJoinSubstitution(
+                [
+                    FindPackageShare("cps_rmp220_support"),
+                    "config",
+                    "ekf.yaml"
+                ]
+            ) ,
+            #{'use_sim_time': LaunchConfiguration('use_sim_time')}
+            ]
+    )
+
+    return LaunchDescription([
+        #control_node,
+        #robot_state_pub_node,
+        #joint_state_broadcaster_spawner,
+        #robot_controller_spawner,
+        #joystick_spawner,
+        #teleop_spawner,
+        #cam_node,
+        #lidar_node,
+        #mapper_node,
+        robot_localization_node
+    ])