From 1ef04335eab8b3cd540e5e1802a6a24bdd32fdaf Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Mon, 7 Jun 2021 17:59:46 +0200
Subject: [PATCH] Add local_ekf launch. Add local_ekf and mux yaml files

---
 config/ana_local_ekf.yaml | 39 +++++++++++++++++++++++++++++++++++++++
 config/ana_mux.yaml       | 26 ++++++++++++++++++++++++++
 launch/local_ekf.launch   | 20 ++++++++++++++++++++
 3 files changed, 85 insertions(+)
 create mode 100644 config/ana_local_ekf.yaml
 create mode 100644 config/ana_mux.yaml
 create mode 100644 launch/local_ekf.launch

diff --git a/config/ana_local_ekf.yaml b/config/ana_local_ekf.yaml
new file mode 100644
index 0000000..09d0fd5
--- /dev/null
+++ b/config/ana_local_ekf.yaml
@@ -0,0 +1,39 @@
+frequency: 30
+sensor_timeout: 1.0
+transform_time_offset: 0.0
+two_d_mode: true
+print_diagnostics: true
+debug: false
+debug_out_file: /tmp/debug_ekf_localization.txt
+use_control: false
+control_config: [true, false, false, false, false, true]
+
+publish_tf: true
+map_frame: map
+odom_frame: ana/odom
+base_link_frame: ana/base_footprint
+world_frame: ana/odom
+
+odom0: /ana/odom
+odom0_config: [false, false, false,
+               false, false, false,
+               true,  false, false,
+               false, false, true,
+               false, false, false]
+
+imu0: /ana/sensors/imu
+imu0_config: [false, false, false,
+              false, false, true,
+              false, false, false,
+              false, false, true,
+              false,  false, false]
+
+imu0_differential: false
+imu0_relative: true
+imu0_remove_gravitational_acceleration: true
+
+#meaning_config: [x,     y,      z,
+#                 roll,  pitch,  yaw,
+#                 vx,    vy,     vz,
+#                 vroll, vpitch, vyaw,
+#                 ax,    ay,     az]
diff --git a/config/ana_mux.yaml b/config/ana_mux.yaml
new file mode 100644
index 0000000..f8dad3f
--- /dev/null
+++ b/config/ana_mux.yaml
@@ -0,0 +1,26 @@
+subscribers:
+  - name:        "Default input"
+    topic:       "/ana/default/cmd_vel"
+    timeout:     0.1
+    priority:    0
+    short_desc:  "The default cmd_vel, controllers unaware that we are multiplexing cmd_vel should come here"
+
+  - name:        "Platform orientation"
+    topic:       "/ana/orientation/cmd_vel"
+    timeout:     0.1
+    priority:    4
+    short_desc:  "Platform orientation to people"
+
+  - name:        "Navigation stack"
+    topic:       "/ana/navigation/cmd_vel"
+    timeout:     0.5
+    priority:    5
+    short_desc:  "Navigation stack controller"
+
+  - name:        "Teleop"
+    topic:       "/ana/teleop/cmd_vel"
+    timeout:     0.1
+    priority:    10
+    short_desc:  "Teleop wii/ps3 pad"
+
+publisher:       "/ana/cmd_vel"
diff --git a/launch/local_ekf.launch b/launch/local_ekf.launch
new file mode 100644
index 0000000..91da32d
--- /dev/null
+++ b/launch/local_ekf.launch
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<!-- -->
+<launch>
+
+  <arg name="name" default="ana"/>
+  <arg name="local_ekf_config_file"  default="$(find iri_ana_rosnav)/params/local_ekf_odom_imu.yaml"/>
+
+  <!-- combines odom+imu to generate odom_combined odometry -->
+  <node pkg="robot_localization"
+        type="ekf_localization_node"
+        name="local_ekf"
+        ns="$(arg name)"
+        clear_params="true"
+        output="screen">
+    <rosparam command="load" file="$(arg local_ekf_config_file)" />
+    <remap from="odometry/filtered" to="/$(arg name)/local_odom_combined"/>
+    <remap from="set_pose" to="/$(arg name)/local_ekf/set_pose"/>
+    <remap from="cmd_vel" to="/$(arg name)/cmd_vel"/>
+  </node>
+</launch>
-- 
GitLab