localization with basic configuration of amcl, but no data fusion implemented, maybe...

diff --git a/twil_description/xacro/caster_wheel.urdf.xacro b/twil_description/xacro/caster_wheel.urdf.xacro
index a1aaf19..27dd15c 100644 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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