From: Raul Cataluna Athayde <raul.cataluna@outlook.com>
Date: Sun, 4 Jul 2021 18:59:02 +0000 (-0300)
Subject: localization with basic configuration of amcl, but no data fusion implemented, maybe... 
X-Git-Tag: raulTCC~1
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=fdc0ff8fa6f71928c949abe7b03f207ff7805e03;p=twil.git

localization with basic configuration of amcl, but no data fusion implemented, maybe there some data fusion on the navigation stack, but yet unknown
---

diff --git a/twil_description/xacro/caster_wheel.urdf.xacro b/twil_description/xacro/caster_wheel.urdf.xacro
index a1aaf19..27dd15c 100644
--- a/twil_description/xacro/caster_wheel.urdf.xacro
+++ b/twil_description/xacro/caster_wheel.urdf.xacro
@@ -36,15 +36,15 @@
 
   <gazebo reference="${name}">
     <material>Gazebo/Grey</material>
-<!--
-    <mu1>1</mu1>
-    <mu2>1</mu2>
+
+    <mu1>100</mu1>
+    <mu2>100</mu2>
     <kp>1000000.0</kp>
     <kd>100.0</kd>
     <minDepth>0.001</minDepth>
     <maxVel>1.0</maxVel>
     <fdir1>1 0 0</fdir1>
--->
+
   </gazebo>
 
   </xacro:macro>
diff --git a/twil_description/xacro/fixed_wheel.urdf.xacro b/twil_description/xacro/fixed_wheel.urdf.xacro
index 31d48dc..b235dd4 100644
--- a/twil_description/xacro/fixed_wheel.urdf.xacro
+++ b/twil_description/xacro/fixed_wheel.urdf.xacro
@@ -49,7 +49,7 @@
 
   <gazebo reference="${name}">
     <material>Gazebo/FlatBlack</material>
-<!--
+
     <mu1>100000.0</mu1>
     <mu2>100000.0</mu2>
     <kp>500000.0</kp>
@@ -57,7 +57,7 @@
     <minDepth>0.001</minDepth>
     <maxVel>0.1</maxVel>
     <fdir1>1 0 0</fdir1>
--->
+
   </gazebo>
 
   </xacro:macro>
diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro
index 80e3882..b75a0fd 100644
--- a/twil_description/xacro/twil.urdf.xacro
+++ b/twil_description/xacro/twil.urdf.xacro
@@ -138,7 +138,7 @@
     <plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
 	    <frameName>map</frameName>
 	    <bodyName>twil_origin</bodyName>
-	    <topicName>true_odom</topicName>
+	    <topicName>ground_truth</topicName>
 	    <updateRate>30.0</updateRate>
    </plugin>
   </gazebo>
diff --git a/twil_location/launch/gazebo.launch b/twil_location/launch/gazebo.launch
index 78e45df..df9ad20 100644
--- a/twil_location/launch/gazebo.launch
+++ b/twil_location/launch/gazebo.launch
@@ -9,6 +9,10 @@
 
 	<remap from="/twist_mrac_linearizing_controller/odom" to="/odom" />
 	<remap from="/twist_mrac_linearizing_controller/command" to="/cmd_vel" />
+		
+	<!--<remap from="/camera/color/image_raw" to="/image_color" />-->
+  	<!--<remap from="/camera/color/camera_info" to= "/my_camera_info"/>-->
+  	<!--<remap from="/camera/depth/image_raw" to="/image_depth"/>-->
 
 	<include file="$(find twil_description)/launch/gazebo.launch" >
 		<arg name="paused" value="$(arg paused)"/>
@@ -24,18 +28,35 @@
 	<include file="$(find twil_bringup)/launch/$(arg controller).launch" />
 
 	<node pkg="tf2_ros" type="static_transform_publisher" name="odom_frame_publisher" args="0 0 0 0 0 0 1 map odom" />
+	
+	<!--<node pkg="tf2_ros" type="static_transform_publisher" name="Slam_map_frame_publisher" args="0 0 0 0 0 0 1 map Slam_map" />-->
+	
+	<!--<node pkg="tf2_ros" type="static_transform_publisher" name="ground_truth_frame_publisher" args="0 0 0 0 0 0 1 map ground_truth" />-->
+	
+	
+	
+	
+	<!--<node pkg="tf2_ros" type="static_transform_publisher" name="image_color_frame_publisher" args="0 0 0 0 0 0 1 true_odom image_color" />-->
+	
+	<!--<node pkg="tf2_ros" type="static_transform_publisher" name="camera_info_frame_publisher" args="0 0 0 0 0 0 1 true_odom my_camera_info" />-->
+	
+	<!--<node pkg="tf2_ros" type="static_transform_publisher" name="image_depth_frame_publisher" args="0 0 0 0 0 0 1 true_odom image_depth" />-->
+	
+	
+	
+	
 
 	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find twil_location)/rviz/twil_mapping.rviz" required="true"/>
 
 	<node pkg="twil_location" type="twil_location" respawn="false" name="twil_location" output="screen"/>
-	
+		
 	<include file="$(find twil_2dnav)/launch/move_base.launch" />
 	
 	<include file="$(find twil_location)/launch/location.launch" />
 	
-	<include file="$(find twil_location)/launch/plot.launch" />
+	<!--<include file="$(find twil_location)/launch/plot.launch" />-->
 
 
 </launch>
diff --git a/twil_location/launch/location.launch b/twil_location/launch/location.launch
index 6924902..db5a56c 100644
--- a/twil_location/launch/location.launch
+++ b/twil_location/launch/location.launch
@@ -1,64 +1,112 @@
 <launch>
-	<!--<node pkg="hector_mapping" type="hector_mapping" name="slam_mapping" output="screen" >-->
-  		<!--<remap from="scan" to="/slam_scan" />-->
-  		<!--<param name="odom_frame" value="/odom_frame_publisher" />-->
-  		<!--<param name="base_frame" value="/robot_state_publisher/twil_origin" />-->
-  		<!--<remap from="map" to="Slam_map" />-->
-	<!--</node>-->
+	<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen" >
+		
+		<remap from="/image" to="/camera/depth/image_raw"/>
+		<remap from="/camera_info" to="/camera/depth/camera_info"/> 
+		
+		<remap from="/scan" to="/amcl_scan"/>
+		
+		<param name="output_frame_id" value="camera_link"/>
+		
+		<!--ange_min (double, default: 0.45m)-->
+    			<!--The minimum ranges to return in meters. Ranges less than this will be output as -Inf. -->
+
+		<!--range_max (double, default: 10.0m)-->
+    			<!--The maximum ranges to return in meters. Ranges greater than this will be output as +Inf. -->
 	
-	<node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" >
-  		<remap from="/rgb/image" to="/camera/color/image_raw" />
-  		<remap from="/rgb/camera_info" to= "/camera/color/camera_info" />
-  		<remap from="/depth/image" to="/camera/depth/image_raw" />
-  		
-  		<remap from="/odom" to="/rgbd_odom" />
-  		
-  		<param name="frame_id" value="twil_origin" />
-  		<param name="Odom/Strategy" value="0" />
 	</node>
-	
-	<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
-    		<param name="output_frame" value="odom"/>
-    		<param name="freq" value="30.0"/>
-    		
-    		<param name="sensor_timeout" value="1.0"/>
-    		
-		<param name="odom_used" value="true"/>
-		<param name="imu_used" value="false"/>
-		<param name="vo_used" value="true"/>
 		
-		<param name="debug" value="false"/>
-		<param name="self_diagnose" value="false"/>
-				
-		<remap from="/vo" to="/rgbd_odom" />
+	<node pkg="amcl" type="amcl" name="amcl" output="screen" >
 		
-  	</node>
-	
-	<!--<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap">-->
-  		<!--<param name="Optimizer/Iterations" type="int" value="10"/>-->
-  		
-  		<!--<remap from="/odom" to="/rgbd_odom" />-->
-  		
-  		<!--<param name="subscribe_depth" type="bool" value="false"/>-->
-          	<!--<param name="subscribe_rgbd" type="bool" value="true"/>-->
-          	<!--<param name="subscribe_scan" type="bool" value="false"/>-->
-  		
+		<remap from="/scan" to="/amcl_scan"/>
+		
+		<param name="initial_pose_x" value="0"/>
+			<!--initial_pose_x (double, default: 0.0 meters)-->
+		<param name="initial_pose_y" value="0"/>
+			<!--initial_pose_y (double, default: 0.0 meters)-->
+		<param name="initial_pose_a" value="0"/>
+			<!--initial_pose_a (double, default: 0.0 radians)-->
+			<!--Initial pose mean (x,Y,yaw), used to initialize filter with Gaussian distribution. -->		 
+
+		<param name="initial_cov_xx" value="0.25"/>
+			<!--initial_cov_xx (double, default: 0.5*0.5 meters)-->
+		<param name="initial_cov_yy" value="0.25"/>
+			<!--initial_cov_yy (double, default: 0.5*0.5 meters)-->
+			<!--itial_cov_aa (double, default: (π/12)*(π/12) radian)--> 
+
+    			<!--Initial pose covariance (x*x,y*y,yaw*yaw), used to initialize filter with Gaussian distribution.-->
+    		
+    		<param name="use_map_topic" value="true"/>	
+    			<!--~use_map_topic (bool, default: false)-->
+    			<!--When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.-->
+    			
+    		<param name="base_frame_id " value="twil_origin"/>
+    		
+    		<param name="laser_min_range" value="0.45"/>    		
+    		<!--laser_min_range (double, default: -1.0)-->
+    			<!--Minimum scan range to be considered; -1.0 will cause the laser's reported minimum range to be used. -->
+
+		<param name="laser_max_range" value="10.0"/>
+		<!--laser_max_range (double, default: -1.0)-->
+    			<!--Maximum scan range to be considered; -1.0 will cause the laser's reported maximum range to be used. -->
+    		
+    		<param name="min_particles" value="1000.0"/>
+    		
+    		<param name="max_particles" value="5000.0"/>
+    		
+    		<param name="odom_model_type" value="diff"/>    		
+    			<!-- If ~odom_model_type is "diff" then we use the sample_motion_model_odometry algorithm from Probabilistic Robotics, p136; this model uses the noise parameters odom_alpha1 through odom_alpha4, as defined in the book.  -->
+    			
+    			<!--odom_alpha1 (double, default: 0.2)-->
+    				<!--Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion.--> 
+
+			<!--odom_alpha2 (double, default: 0.2)-->
+    				<!--Specifies the expected noise in odometry's rotation estimate from translational component of the robot's motion.--> 
+
+			<!--odom_alpha3 (double, default: 0.2)-->
+    				<!--pecifies the expected noise in odometry's translation estimate from the translational component of the robot's motion.--> 
+
+			<!--odom_alpha4 (double, default: 0.2)-->
+    				<!--Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion.-->
+    			
+    	</node>
+    			
+    			
+    			
+    			
+	<!--<node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" >-->
   		<!--<remap from="/rgb/image" to="/camera/color/image_raw" />-->
-  		<!--<remap from="/rgb/camera_info" to= "/camera/color/camera_info" />-->
-  		<!--<remap from="/depth/image" to="/camera/depth/image_raw" />-->	
-  		
-  		<!--<remap from="grid_map" to="Slam_map" />-->
+  		<!--<remap from="/rgb/camera_info" to= "/camera/depth/camera_info" />-->
+  		<!--<remap from="/depth/image" to="/camera/depth/image_raw" />-->
+  		  		
+  		<!--<remap from="/odom" to="/rgbd_odom" />-->
   		
   		<!--<param name="frame_id" value="twil_origin" />-->
-  		<!--<param name="queue_size" value="50" />-->
+  		<!--<param name="Odom/Strategy" value="0" />-->
+  		<!--Odom/Strategy = "0"[0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion]-->
+  		
+  		<!--<param name="Odom/Holonomic" value="false"/>-->
+  		<!--<param name="OdomF2M/BundleAdjustment" value="2" />-->
+  		<!--<param name="Odom/FilteringStrategy" value="0" />-->
   		
-  		<!--<param name="odom_frame_id" value="slam_odom" />-->
   		
-  		<!--<param name="approx_sync" value="false" />-->
-  		<!--<param name="queue_size" value="20" />-->
-  		<!--<param name="map_always_update" value="true" />-->
-  		<!--<param name="wait_for_transform_duration" value="1" />-->
- 
 	<!--</node>-->
-
+	
+	<!--<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">-->
+    		<!--<param name="output_frame" value="odom"/>-->
+    		<!--<param name="freq" value="30.0"/>-->
+    		
+    		<!--<param name="sensor_timeout" value="1.0"/>-->
+    		
+		<!--<param name="odom_used" value="true"/>-->
+		<!--<param name="imu_used" value="false"/>-->
+		<!--<param name="vo_used" value="true"/>-->
+		
+		<!--<param name="debug" value="false"/>-->
+		<!--<param name="self_diagnose" value="false"/>-->
+				
+		<!--<remap from="/vo" to="/rgbd_odom" />-->
+		
+  	<!--</node>-->
+  		
 </launch>
diff --git a/twil_location/launch/plot.launch b/twil_location/launch/plot.launch
index eb277a4..9525948 100644
--- a/twil_location/launch/plot.launch
+++ b/twil_location/launch/plot.launch
@@ -1,15 +1,15 @@
 <launch>
 
-	<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_x" output="screen" args = "/odom/pose/pose/position/x /rgbd_odom/pose/pose/position/x /true_odom/pose/pose/position/x /robot_pose_ekf/odom_combined/pose/pose/position/x"/>
+	<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_x" output="screen" args = "/odom/pose/pose/position/x /rgbd_odom/pose/pose/position/x /ground_truth/pose/pose/position/x /robot_pose_ekf/odom_combined/pose/pose/position/x"/>
 	
-	<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_y" output="screen" args = "/odom/pose/pose/position/y /rgbd_odom/pose/pose/position/y /true_odom/pose/pose/position/y /robot_pose_ekf/odom_combined/pose/pose/position/y" />
+	<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_y" output="screen" args = "/odom/pose/pose/position/y /rgbd_odom/pose/pose/position/y /ground_truth/pose/pose/position/y /robot_pose_ekf/odom_combined/pose/pose/position/y" />
 	
-	<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_ori_x" output="screen" args = "/odom/pose/pose/orientation/x /rgbd_odom/pose/pose/orientation/x /true_odom/pose/pose/orientation/x /robot_pose_ekf/odom_combined/pose/pose/orientation/x" />
+	<!--<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_ori_x" output="screen" args = "/twist_mrac_linearizing_controller/odom/pose/pose/orientation/x /rgbd_odom/pose/pose/orientation/x /true_odom/pose/pose/orientation/x /robot_pose_ekf/odom_combined/pose/pose/orientation/x" />-->
 
-	<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_ori_y" output="screen" args = "/odom/pose/pose/orientation/y /rgbd_odom/pose/pose/orientation/y /true_odom/pose/pose/orientation/y /robot_pose_ekf/odom_combined/pose/pose/orientation/y" />
+	<!--<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_ori_y" output="screen" args = "/twist_mrac_linearizing_controller/odom/pose/pose/orientation/y /rgbd_odom/pose/pose/orientation/y /true_odom/pose/pose/orientation/y /robot_pose_ekf/odom_combined/pose/pose/orientation/y" />-->
 	
-	<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_ori_z" output="screen" args = "/odom/pose/pose/orientation/z /rgbd_odom/pose/pose/orientation/z /true_odom/pose/pose/orientation/z /robot_pose_ekf/odom_combined/pose/pose/orientation/z" />
+	<!--<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_ori_z" output="screen" args = "/twist_mrac_linearizing_controller/odom/pose/pose/orientation/z /rgbd_odom/pose/pose/orientation/z /true_odom/pose/pose/orientation/z /robot_pose_ekf/odom_combined/pose/pose/orientation/z" />-->
 	
-	<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_ori_w" output="screen" args = "/odom/pose/pose/orientation/w /rgbd_odom/pose/pose/orientation/w /true_odom/pose/pose/orientation/w /robot_pose_ekf/odom_combined/pose/pose/orientation/w" />
+	<!--<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_ori_w" output="screen" args = "/twist_mrac_linearizing_controller/odom/pose/pose/orientation/w /rgbd_odom/pose/pose/orientation/w /true_odom/pose/pose/orientation/w /robot_pose_ekf/odom_combined/pose/pose/orientation/w" />-->
 	
 </launch>
diff --git a/twil_location/rviz/twil_mapping.rviz b/twil_location/rviz/twil_mapping.rviz
index 114a21a..35432ac 100644
--- a/twil_location/rviz/twil_mapping.rviz
+++ b/twil_location/rviz/twil_mapping.rviz
@@ -254,6 +254,82 @@ Visualization Manager:
       Unreliable: false
       Value: true
     - Alpha: 0.699999988079071
+    
+      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: 42
+      Name: rgbd_Odometry
+      Position Tolerance: 0.30000001192092896
+      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: /rgbd_odom
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988079071
+      
+      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: 42
+      Name: ground_truth
+      Position Tolerance: 0.30000001192092896
+      Queue Size: 10
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 25; 25; 255
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic: /ground_truth
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988079071
+      
+      
+      
+         
       Class: rviz/Map
       Color Scheme: costmap
       Draw Behind: false
@@ -265,19 +341,21 @@ Visualization Manager:
       Value: true
     - Alpha: 1
       Axes Length: 1
-      Axes Radius: 0.10000000149011612         
+      Axes Radius: 0.10000000149011612    
+           
       Class: rviz/Map
       Color Scheme: map
       Draw Behind: false
       Enabled: true
       Name: Map(SLAM)
-      Topic: /slam_gmapping/Slam_map
+      Topic: /Slam_map
       Unreliable: false
       Use Timestamp: false
       Value: true
     - Alpha: 1
       Axes Length: 1
-      Axes Radius: 0.10000000149011612        
+      Axes Radius: 0.10000000149011612 
+             
       Class: rviz/Pose
       Color: 255; 255; 0
       Enabled: true