From: Henrique Scharlau Coelho <henriquescharlaucoelho@gmail.com>
Date: Sat, 25 Nov 2023 22:27:17 +0000 (-0300)
Subject: odom changes(this commit shoud break twil_bringup)
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=a328852273b3f3e8298429e8d81dafd52215ba31;p=twil.git

odom changes(this commit shoud break twil_bringup)

warning: the base_link of the twist_mrac controller was changed so
it doesnt conflict with the tf published by the robot_localization.
this means that the twist_mrac controller will probably not work.
this will be fixed after the odom is tested.

this commit uses the robot_localization to publish the tf and
the odometry topic used by nav2.
the only sensor used in the ekf is the controller odom, so the
result is pretty much the same as just the controller odom, but
the plan is to use the robot_localization node to publish the tf
and odom info of other sensor and compare with this one.
the plan is to have a branch created for each odom info (controller,
ground truth, imu) invidually and fused then compare the results of each
one. finally, the best one will be merged with the original branch.
---

diff --git a/twil_2dnav/config/ekf_params.yaml b/twil_2dnav/config/ekf_params.yaml
index 59ebd3d..a7edce8 100644
--- a/twil_2dnav/config/ekf_params.yaml
+++ b/twil_2dnav/config/ekf_params.yaml
@@ -2,22 +2,43 @@ ekf_filter_node:
     ros__parameters:
         frequency: 30.0
         two_d_mode: true
-        publish_acceleration: true # Whether to publish the acceleration state. Defaults to false if unspecified.
-        publish_tf: false # atualmente a tf é publicado pelo twist_mrac e não tem parametro para desativar
+        publish_acceleration: false # Whether to publish the acceleration state. Defaults to false if unspecified.
+        publish_tf: true 
         map_frame: map
         odom_frame: odom
+        # base_link_frame: twil_origin_localization_test
         base_link_frame: twil_origin
         world_frame: odom
-        # config order: (X,Y,Z,roll,pitch,yaw,X˙,Y˙,Z˙,roll˙,pitch˙,yaw˙,X¨,Y¨,Z¨)
+
+        initial_state: [0.0,  0.0,  0.0,
+                        0.0,  0.0,  0.0,
+                        0.0,  0.0,  0.0,
+                        0.0,  0.0,  0.0,
+                        0.0,  0.0,  0.0]
         # odom0 topic is set on launch file
-        odom0_config: [true,  true,  true,
+        # config order: (X,Y,Z,roll,pitch,yaw,X˙,Y˙,Z˙,roll˙,pitch˙,yaw˙,X¨,Y¨,Z¨)
+        # robot_localization docs recommendation
+        # odom0_config: [false, false, false,
+        #                false, false, false,
+        #                true, true, false,
+        #                false, false, true,
+        #                false, false, false]
+        odom0_config: [true,  true,  false,
+                       false, false, true,
                        false, false, false,
                        false, false, false,
-                       false, false, true,
                        false, false, false]
-        imu0: sensor/imu
-        imu0_config: [false, false, false,
-                      true,  true,  true,
-                      false, false, false,
-                      false, false, false,
-                      false, false, false]
\ No newline at end of file
+        
+        # imu0: sensor/imu
+        # robot localization recommendation
+        # imu0_config: [false, false, false,
+        #               false, false, true,
+        #               false, false, false,
+        #               false, false, true,
+        #               true, false, false]
+        # nav2 docs recommendation
+        # imu0_config: [false, false, false,
+        #               true,  true,  true,
+        #               false, false, false,
+        #               false, false, false,
+        #               false, false, false]
\ No newline at end of file
diff --git a/twil_2dnav/launch/nav2_navigator.launch.xml b/twil_2dnav/launch/nav2_navigator.launch.xml
index d303822..654c191 100644
--- a/twil_2dnav/launch/nav2_navigator.launch.xml
+++ b/twil_2dnav/launch/nav2_navigator.launch.xml
@@ -48,14 +48,14 @@
         <param from="$(find-pkg-share twil_2dnav)/config/behavior_params.yaml"/>
         <param name="robot_base_frame" value="twil_origin"/>
         <remap from="cmd_vel" to="$(var controller)/command"/>
+        <!-- porque tem dois cmd_vel(aqui e no nav2_controller)?? -->
     </node>
 
 
     <node pkg="nav2_bt_navigator" exec="bt_navigator" name="bt_navigator">
         <param name="use_sim_time" value="$(var use_sim_time)"/>
         <param from="$(find-pkg-share twil_2dnav)/config/bt_navigator_params.yaml"/>
-        <param name="robot_base_frame" value="twil_origin"/>
-        <remap from="odom" to="$(var controller)/odom"/>
+        <!-- <remap from="odom" to="$(var controller)/odom"/> -->
     </node>
 
     <node pkg="robot_localization" exec="ekf_node" name="ekf_filter_node">
diff --git a/twil_bringup/config/twist_mrac_linearizing_controller.yaml b/twil_bringup/config/twist_mrac_linearizing_controller.yaml
index acfedd0..9b9aeab 100644
--- a/twil_bringup/config/twist_mrac_linearizing_controller.yaml
+++ b/twil_bringup/config/twist_mrac_linearizing_controller.yaml
@@ -20,7 +20,10 @@ twist_mrac_linearizing_controller:
     wheel_separation: 0.322
     wheel_radius: [0.075, 0.075]
     odom_frame_id: "odom"
-    base_frame_id: "twil_origin"
+    # a transformada vai ser publicada pelo robot_localization posteriormente
+    # entao coloquei um nome diferente pra nao afetar a publicada pelo robot_localization e poder comparar as duas
+    # base_frame_id: "twil_origin"
+    base_frame_id: "twil_origin_controller_frame"
     priority: 99
     time_step: 0.01
     adaptive: false
diff --git a/twil_bringup/launch/twist_mrac_linearizing_controller.launch.xml b/twil_bringup/launch/twist_mrac_linearizing_controller.launch.xml
index 3cf65f9..f09ebc8 100644
--- a/twil_bringup/launch/twist_mrac_linearizing_controller.launch.xml
+++ b/twil_bringup/launch/twist_mrac_linearizing_controller.launch.xml
@@ -24,6 +24,9 @@
 
 	<node name="controller_spawner" pkg="controller_manager" exec="spawner"
 		args="-t effort_controllers/TwistMracLinearizingController -p $(var config) twist_mrac_linearizing_controller"/>
+        <!-- eu queria passar parametros pra esse nodo no launch mas não deu porque os parametros sao passados no arg do comando -->
+        <!-- nao tem como passar esses parametros com um <param from="..."?> -->
+        <!-- nos exemplos do ros_control é usado um ros2_control_node que recebe os parametros... -->
 
 	<node name="joint_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
 		args="-t joint_state_broadcaster/JointStateBroadcaster -p $(var config) joint_state_broadcaster"/>