From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Mon, 19 Jul 2021 05:50:00 +0000 (-0300)
Subject: Add localization using the arc_odometry package.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=465fe8e4d10f1ffae4577eb20f94dc654ccd4ef7;p=tatuira.git

Add localization using the arc_odometry package.
---

diff --git a/tatuira_bringup/config/bypass.yaml b/tatuira_bringup/config/bypass.yaml
index cc6b5ee..aa746e6 100644
--- a/tatuira_bringup/config/bypass.yaml
+++ b/tatuira_bringup/config/bypass.yaml
@@ -9,3 +9,6 @@ left_wheel_controller:
 joint_states_publisher:
         type: joint_state_controller/JointStateController
         publish_rate: 100
+        extra_joints:
+                - name: front_bearing_0_joint
+                - name: front_wheel_rim_joint
diff --git a/tatuira_bringup/config/odometry_publisher.yaml b/tatuira_bringup/config/odometry_publisher.yaml
new file mode 100644
index 0000000..e841dba
--- /dev/null
+++ b/tatuira_bringup/config/odometry_publisher.yaml
@@ -0,0 +1,9 @@
+# Watch-out: The indentation here is relevant to the semantic!
+
+odometry_publisher:
+  wheel_separation: 0.8
+  wheel_radius: [0.12319, 0.12319]
+  odom_frame_id: "odom"
+  base_frame_id: "wheel_center"
+  publish_rate: 100
+  
\ No newline at end of file
diff --git a/tatuira_bringup/config/pid.yaml b/tatuira_bringup/config/pid.yaml
index 8494efa..12ce0c5 100644
--- a/tatuira_bringup/config/pid.yaml
+++ b/tatuira_bringup/config/pid.yaml
@@ -11,3 +11,6 @@ left_wheel_controller:
 joint_states_publisher:
         type: joint_state_controller/JointStateController
         publish_rate: 100
+        extra_joints:
+                - name: front_bearing_0_joint
+                - name: front_wheel_rim_joint
diff --git a/tatuira_bringup/launch/gazebo.launch b/tatuira_bringup/launch/gazebo.launch
index 22b797b..84769e5 100644
--- a/tatuira_bringup/launch/gazebo.launch
+++ b/tatuira_bringup/launch/gazebo.launch
@@ -5,6 +5,8 @@
 
 	<arg name="controller" default="pid"/>
 	<arg name="config" default="$(find tatuira_bringup)/config/$(arg controller).yaml"/>
+
+	<arg name="odometry" default="false"/>
 	
 	<include file="$(find tatuira_description)/launch/gazebo.launch" >
 		<arg name="paused" value="$(arg paused)"/>
@@ -15,4 +17,16 @@
 	<rosparam file="$(arg config)" command="load"/>
 
 	<include file="$(find tatuira_bringup)/launch/$(arg controller).launch" />
+
+	<group if="$(arg odometry)">
+		<rosparam file="$(find tatuira_bringup)/config/odometry_publisher.yaml" command="load" />
+
+		<node pkg="arc_odometry" type="odometry_publisher" name="odometry_publisher" />
+
+		<node pkg="tf2_ros" type="static_transform_publisher" name="world_frame_publisher" args="0 0 0 0 0 0 1 map world" />
+		<node pkg="tf2_ros" type="static_transform_publisher" name="odom_frame_publisher" args="0 -0.42294 0 0 0 0.7071068 0.7071068 map odom" />
+		<node pkg="tf2_ros" type="static_transform_publisher" name="wheel_center_publisher" args="0.42294 0 0 0 0 -0.7071068 0.7071068 wheel_center origin_link" />
+
+	</group>
+
 </launch>
diff --git a/tatuira_description/rviz/display.rviz b/tatuira_description/rviz/display.rviz
index 7fb0d18..cd0f752 100644
--- a/tatuira_description/rviz/display.rviz
+++ b/tatuira_description/rviz/display.rviz
@@ -4,12 +4,10 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /Global Options1
-        - /Status1
-        - /TF1
         - /TF1/Frames1
+        - /Ground Truth1/Shape1
       Splitter Ratio: 0.5
-    Tree Height: 730
+    Tree Height: 732
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -18,7 +16,7 @@ Panels:
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.588679
+    Splitter Ratio: 0.5886790156364441
   - Class: rviz/Views
     Expanded:
       - /Current View1
@@ -29,6 +27,10 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
@@ -38,7 +40,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.03
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -261,83 +263,90 @@ Visualization Manager:
       Frames:
         All Enabled: false
         awac_link:
-          Value: true
+          Value: false
         center_0_link:
-          Value: true
+          Value: false
         center_1_link:
-          Value: true
+          Value: false
         front_0_link:
-          Value: true
+          Value: false
         front_1_link:
-          Value: true
+          Value: false
         front_2_link:
-          Value: true
+          Value: false
         front_axle_link:
-          Value: true
+          Value: false
         front_bearing_0_link:
-          Value: true
+          Value: false
         front_bearing_1_link:
-          Value: true
+          Value: false
         front_bearing_2_link:
-          Value: true
+          Value: false
         front_fork_base_link:
-          Value: true
+          Value: false
         front_fork_fixture_link:
-          Value: true
+          Value: false
         front_fork_leg_left_link:
-          Value: true
+          Value: false
         front_fork_leg_right_link:
-          Value: true
+          Value: false
         front_fork_plate_link:
-          Value: true
+          Value: false
         front_wheel_rim_link:
-          Value: true
+          Value: false
         front_wheel_tire_link:
-          Value: true
+          Value: false
         left_back_0_link:
-          Value: true
+          Value: false
         left_back_1_link:
-          Value: true
+          Value: false
         left_motor_link:
-          Value: true
+          Value: false
         left_motor_support_0_link:
-          Value: true
+          Value: false
         left_motor_support_1_link:
-          Value: true
+          Value: false
         left_motor_support_2_link:
-          Value: true
+          Value: false
         left_motor_support_3_link:
-          Value: true
+          Value: false
         left_pinion_link:
-          Value: true
+          Value: false
         left_rim_link:
-          Value: true
+          Value: false
         left_tire_link:
-          Value: true
+          Value: false
         map:
-          Value: true
+          Value: false
+        odom:
+          Value: false
         origin_link:
-          Value: true
+          Value: false
         right_back_0_link:
-          Value: true
+          Value: false
         right_back_1_link:
-          Value: true
+          Value: false
         right_motor_link:
-          Value: true
+          Value: false
         right_motor_support_0_link:
-          Value: true
+          Value: false
         right_motor_support_1_link:
-          Value: true
+          Value: false
         right_motor_support_2_link:
-          Value: true
+          Value: false
         right_motor_support_3_link:
-          Value: true
+          Value: false
         right_pinion_link:
-          Value: true
+          Value: false
         right_rim_link:
-          Value: true
+          Value: false
         right_tire_link:
-          Value: true
+          Value: false
+        wheel_center:
+          Value: false
+        world:
+          Value: false
+      Marker Alpha: 1
       Marker Scale: 1
       Name: TF
       Show Arrows: false
@@ -345,62 +354,137 @@ Visualization Manager:
       Show Names: false
       Tree:
         map:
-          origin_link:
-            center_0_link:
-              awac_link:
-                {}
-              center_1_link:
-                {}
-              front_0_link:
-                front_1_link:
-                  {}
-                front_2_link:
-                  front_fork_fixture_link:
-                    front_axle_link:
-                      front_bearing_0_link:
-                        front_fork_base_link:
-                          front_bearing_1_link:
-                            front_bearing_2_link:
-                              {}
-                          front_fork_leg_left_link:
-                            {}
-                          front_fork_leg_right_link:
-                            front_wheel_rim_link:
-                              front_wheel_tire_link:
-                                {}
-                          front_fork_plate_link:
-                            {}
-              left_back_0_link:
-                left_back_1_link:
-                  left_motor_support_0_link:
-                    left_motor_support_1_link:
+          odom:
+            wheel_center:
+              origin_link:
+                center_0_link:
+                  awac_link:
+                    {}
+                  center_1_link:
+                    {}
+                  front_0_link:
+                    front_1_link:
                       {}
-                    left_motor_support_2_link:
-                      left_rim_link:
-                        left_tire_link:
-                          {}
-                    left_motor_support_3_link:
-                      left_motor_link:
-                        left_pinion_link:
-                          {}
-              right_back_0_link:
-                right_back_1_link:
-                  right_motor_support_0_link:
-                    right_motor_support_1_link:
-                      {}
-                    right_motor_support_2_link:
-                      right_rim_link:
-                        right_tire_link:
+                    front_2_link:
+                      front_fork_fixture_link:
+                        front_axle_link:
+                          front_bearing_0_link:
+                            front_fork_base_link:
+                              front_bearing_1_link:
+                                front_bearing_2_link:
+                                  {}
+                              front_fork_leg_left_link:
+                                {}
+                              front_fork_leg_right_link:
+                                front_wheel_rim_link:
+                                  front_wheel_tire_link:
+                                    {}
+                              front_fork_plate_link:
+                                {}
+                  left_back_0_link:
+                    left_back_1_link:
+                      left_motor_support_0_link:
+                        left_motor_support_1_link:
                           {}
-                    right_motor_support_3_link:
-                      right_motor_link:
-                        right_pinion_link:
+                        left_motor_support_2_link:
+                          left_rim_link:
+                            left_tire_link:
+                              {}
+                        left_motor_support_3_link:
+                          left_motor_link:
+                            left_pinion_link:
+                              {}
+                  right_back_0_link:
+                    right_back_1_link:
+                      right_motor_support_0_link:
+                        right_motor_support_1_link:
                           {}
+                        right_motor_support_2_link:
+                          right_rim_link:
+                            right_tire_link:
+                              {}
+                        right_motor_support_3_link:
+                          right_motor_link:
+                            right_pinion_link:
+                              {}
+          world:
+            {}
       Update Interval: 0
       Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 100
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Queue Size: 10
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic: /odom
+      Unreliable: false
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 100
+      Name: Ground Truth
+      Position Tolerance: 0.10000000149011612
+      Queue Size: 10
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 25; 255; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic: /ground_truth
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
+    Default Light: true
     Fixed Frame: map
     Frame Rate: 30
   Name: root
@@ -412,7 +496,10 @@ Visualization Manager:
     - Class: rviz/FocusCamera
     - Class: rviz/Measure
     - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
       Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
     - Class: rviz/SetGoal
       Topic: /move_base_simple/goal
     - Class: rviz/PublishPoint
@@ -422,22 +509,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 4.47293
+      Distance: 4.472929954528809
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.06
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Field of View: 0.7853981852531433
       Focal Point:
         X: 0
         Y: 0
         Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.01
-      Pitch: 0.945398
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.9453979730606079
       Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 0.125398
+      Yaw: 0.12539799511432648
     Saved: ~
 Window Geometry:
   Displays:
@@ -445,7 +535,7 @@ Window Geometry:
   Height: 1023
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000367fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003800000367000000b900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000367fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000038000003670000009f00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000068c0000003efc0100000002fb0000000800540069006d006501000000000000068c0000025a00fffffffb0000000800540069006d00650100000000000004500000000000000000000004070000036700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000365fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000365000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000365fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000365000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000068c0000003efc0100000002fb0000000800540069006d006501000000000000068c0000023f00fffffffb0000000800540069006d00650100000000000004500000000000000000000004070000036500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
diff --git a/tatuira_description/xacro/main.urdf.xacro b/tatuira_description/xacro/main.urdf.xacro
index 1933c09..ba94657 100644
--- a/tatuira_description/xacro/main.urdf.xacro
+++ b/tatuira_description/xacro/main.urdf.xacro
@@ -75,6 +75,16 @@
 			<!--<robotNamespace>/tatuira</robotNamespace>-->
 			<controlPeriod>0.001</controlPeriod>
 		</plugin>
+
+		<plugin name="gound_truth" filename="libgazebo_ros_p3d.so">
+			<updateRate>100.0</updateRate>
+			<bodyName>origin_link</bodyName>
+			<topicName>ground_truth</topicName>
+			<gaussianNoise>0.0</gaussianNoise>
+			<frameName>world</frameName>
+			<xyzOffset>0 0 0</xyzOffset>
+			<rpyOffset>0 0 ${M_PI/2}</rpyOffset>
+		</plugin>
 	</gazebo>
 
 </robot>