From: Henrique Scharlau Coelho <henriquescharlaucoelho@gmail.com>
Date: Tue, 12 Dec 2023 01:13:52 +0000 (-0300)
Subject: added slam_toolbox and fixed obstacle layer costmap
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=27bd5030942eb484021d1d69f4c2d1ce9204391d;p=twil.git

added slam_toolbox and fixed obstacle layer costmap
---

diff --git a/twil_2dnav/config/planner_params.yaml b/twil_2dnav/config/planner_params.yaml
index bcc6123..9bfb6b7 100644
--- a/twil_2dnav/config/planner_params.yaml
+++ b/twil_2dnav/config/planner_params.yaml
@@ -18,45 +18,26 @@ global_costmap:
       robot_radius: 0.3
       resolution: 0.05
       track_unknown_space: true
-      plugins: ["static_layer", "inflation_layer", "voxel_layer", "obstacle_layer"]
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True
-      inflation_layer:
-        plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.35
-      voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
-        footprint_clearing_enabled: true
-        max_obstacle_height: 2.0
-        # publish_voxel_map: True
-        origin_z: 0.0
-        z_resolution: 0.05
-        z_voxels: 16
-        max_obstacle_height: 2.0
-        unknown_threshold: 15
-        mark_threshold: 0
-        observation_sources: pointcloud
-        combination_method: 1
-        pointcloud:  # no frame set, uses frame from message
-          topic: /camera/depth/points
+        observation_sources: scan
+        scan:
+          topic: /camera/depth/image_scan
           max_obstacle_height: 2.0
-          min_obstacle_height: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-          raytrace_max_range: 2.5
-          raytrace_min_range: 0.0
           clearing: True
           marking: True
-          data_type: "PointCloud2"
-      obstacle_layer:
-        plugin: "nav2_costmap_2d::ObstacleLayer"
-        enabled: True
-        combination_method: 0 # o padrão é 1, que parece ser o melhor na maioria dos casos
-        observation_sources: pointcloud
-        pointcloud:  
-          topic: /camera/depth/points
-          data_type: "PointCloud2"
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.35
       always_send_full_costmap: True
diff --git a/twil_2dnav/config/slam_toolbox_mapping_params.yaml b/twil_2dnav/config/slam_toolbox_mapping_params.yaml
new file mode 100644
index 0000000..59905ca
--- /dev/null
+++ b/twil_2dnav/config/slam_toolbox_mapping_params.yaml
@@ -0,0 +1,76 @@
+# modified version of online async parameters file of the slam_toolbox package
+slam_toolbox:
+  ros__parameters:
+
+    # Plugin params
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # ROS Parameters
+    odom_frame: odom
+    map_frame: map
+    base_frame: twil_origin
+    scan_topic: /camera/depth/image_scan
+    use_map_saver: true
+    mode: mapping
+    # mode: mapping #localization
+
+    # if you'd like to immediately start continuing a map at a given pose
+    # or at the dock, but they are mutually exclusive, if pose is given
+    # will use pose
+    # map_file_name: /map
+    # map_start_pose: [0.0, 0.0, 0.0]
+    # map_start_at_dock: true
+
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.02 #if 0 never publishes odometry
+    map_update_interval: 5.0
+    resolution: 0.05
+    max_laser_range: 10.0 #for rastering images
+    minimum_time_interval: 0.5
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.
+    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+    enable_interactive_mode: true
+
+    # General Parameters
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.5
+    minimum_travel_heading: 0.5
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1  
+    link_scan_maximum_distance: 1.5
+    loop_search_maximum_distance: 3.0
+    do_loop_closing: true 
+    loop_match_minimum_chain_size: 10           
+    loop_match_maximum_variance_coarse: 3.0  
+    loop_match_minimum_response_coarse: 0.35    
+    loop_match_minimum_response_fine: 0.45
+
+    # Correlation Parameters - Correlation Parameters
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1 
+
+    # Correlation Parameters - Loop Closure Parameters
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # Scan Matcher Parameters
+    distance_variance_penalty: 0.5      
+    angle_variance_penalty: 1.0    
+
+    fine_search_angle_offset: 0.00349     
+    coarse_search_angle_offset: 0.349   
+    coarse_angle_resolution: 0.0349        
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true
diff --git a/twil_2dnav/config/twil_2dnav.rviz b/twil_2dnav/config/twil_2dnav.rviz
index 9a141ab..b36eec3 100644
--- a/twil_2dnav/config/twil_2dnav.rviz
+++ b/twil_2dnav/config/twil_2dnav.rviz
@@ -28,7 +28,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: PointCloud2
 Visualization Manager:
   Class: ""
   Displays:
@@ -536,7 +536,7 @@ Visualization Manager:
         LaserScan: true
         Local Costmap: false
         Local Plan: false
-        Map: false
+        Map: true
         Odometry: false
         PointCloud2: false
         Pose Reference: false
@@ -568,7 +568,7 @@ Visualization Manager:
         LaserScan: true
         Local Costmap: false
         Local Plan: false
-        Map: false
+        Map: true
         Odometry: false
         PointCloud2: true
         Pose Reference: false
@@ -646,6 +646,27 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: false
       Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /slam_map
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /slam_map_updates
+      Use Timestamp: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
diff --git a/twil_2dnav/config/twist_mrac_linearizing_controller_no_tf.yaml b/twil_2dnav/config/twist_mrac_linearizing_controller_no_tf.yaml
new file mode 100644
index 0000000..adf99f2
--- /dev/null
+++ b/twil_2dnav/config/twist_mrac_linearizing_controller_no_tf.yaml
@@ -0,0 +1,28 @@
+# Watch-out: The indentation here is relevant to the semantic!
+
+joint_state_broadcaster:
+  ros__parameters:
+    extra_joints:
+      - caster_wheel_joint
+      - caster_base_joint
+      - front_caster_wheel_joint
+      - front_caster_base_joint
+      - tower_joint
+
+twist_mrac_linearizing_controller:
+  ros__parameters:
+    joints:
+      - left_wheel_joint
+      - right_wheel_joint
+    F: [0.0, 0.08444758509282763, 3.770688129256381, 0.0]
+    G: [2.6468901285322475, 2.6468901285322475, -16.084061415321404, 16.084061415321404]
+    Alpha: [10.0, 10.0]
+    wheel_separation: 0.322
+    wheel_radius: [0.075, 0.075]
+    odom_frame_id: "odom"
+    # o base_frame está diferente do robo real para a tf odom->base_link poder ser publicada por outro pacote(robot_localization, por exemplo)
+    base_frame_id: "controller_odom_frame"
+    priority: 99
+    time_step: 0.01
+    adaptive: false
+    Epsilon: [0.01, 0.01]
diff --git a/twil_2dnav/launch/gazebo.launch.xml b/twil_2dnav/launch/gazebo.launch.xml
index 3afe2ad..902349d 100644
--- a/twil_2dnav/launch/gazebo.launch.xml
+++ b/twil_2dnav/launch/gazebo.launch.xml
@@ -27,7 +27,8 @@
 	<arg name="gps" default="false"/>
 	<arg name="world" default ="$(find-pkg-share ufrgs_gazebo)/worlds/centenario.world"/>
 	<arg name="controller" default="twist_mrac_linearizing_controller"/>
-	<arg name="config" default="$(find-pkg-share twil_bringup)/config/$(var controller).yaml"/>
+	<arg name="localization" default="slam_toolbox_mapping"/>
+	<arg name="config" default="$(find-pkg-share twil_2dnav)/config/$(var controller)_no_tf.yaml"/>
 
 	<include file="$(find-pkg-share twil_bringup)/launch/gazebo.launch.xml">
 		<arg name="pause" value="$(var pause)"/>
@@ -44,6 +45,7 @@
 	<include file="$(find-pkg-share twil_2dnav)/launch/nav2_navigator.launch.xml">
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
 		<arg name="controller" value="$(var controller)"/>
+        <arg name="localization" value="$(var localization)"/>
 	</include>
 
 	<node name="rviz" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share twil_2dnav)/config/twil_2dnav.rviz">
diff --git a/twil_2dnav/launch/localization_amcl.launch.xml b/twil_2dnav/launch/localization_amcl.launch.xml
index e8a3229..2beb1af 100644
--- a/twil_2dnav/launch/localization_amcl.launch.xml
+++ b/twil_2dnav/launch/localization_amcl.launch.xml
@@ -1,4 +1,11 @@
 <launch>
+    <arg name="use_sim_time" default="false"/>
+
+    <node pkg="nav2_map_server" name="map_server" exec="map_server">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param name="yaml_filename" value="$(find-pkg-share ufrgs_maps)/map/centenario1.yaml"/>
+    </node>
+
     <node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" exec="depthimage_to_laserscan_node">
         <remap from="depth" to="/camera/depth/image_raw"/> 
         <remap from="depth_camera_info" to="/camera/depth/camera_info"/> 
diff --git a/twil_2dnav/launch/localization_slam_toolbox_mapping.launch.xml b/twil_2dnav/launch/localization_slam_toolbox_mapping.launch.xml
new file mode 100644
index 0000000..a955bf8
--- /dev/null
+++ b/twil_2dnav/launch/localization_slam_toolbox_mapping.launch.xml
@@ -0,0 +1,32 @@
+<launch>
+    <arg name="use_sim_time" default="false"/>
+
+    <node pkg="nav2_map_server" name="map_server" exec="map_server">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param name="yaml_filename" value="$(find-pkg-share ufrgs_maps)/map/centenario1.yaml"/>
+    </node>
+
+    <node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" exec="depthimage_to_laserscan_node">
+        <remap from="depth" to="/camera/depth/image_raw"/> 
+        <remap from="depth_camera_info" to="/camera/depth/camera_info"/> 
+        <remap from="scan" to="/camera/depth/image_scan"/> 
+        <param name="output_frame" type="str" value="camera_link"/> 
+    </node>
+
+    <node pkg="slam_toolbox" exec="async_slam_toolbox_node" name="slam_toolbox">
+        <param name="use_sim_time" value="$(var use_sim_time)" />
+        <param from="$(find-pkg-share twil_2dnav)/config/slam_toolbox_mapping_params.yaml"/>
+        <remap from="map" to="slam_map"/> 
+    </node>
+
+    <node pkg="nav2_lifecycle_manager" exec="lifecycle_manager" name="lifecycle_manager_nav2">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param name="autostart" value="$(var autostart)"/>
+        <param name="node_names" value="[
+            'map_server',
+            'controller_server',
+            'planner_server',
+            'behavior_server',
+            'bt_navigator']"/>
+    </node>
+</launch>
diff --git a/twil_2dnav/launch/nav2_navigator.launch.xml b/twil_2dnav/launch/nav2_navigator.launch.xml
index 9a3e8be..5839133 100644
--- a/twil_2dnav/launch/nav2_navigator.launch.xml
+++ b/twil_2dnav/launch/nav2_navigator.launch.xml
@@ -24,12 +24,7 @@
     <arg name="map" default="$(find-pkg-share ufrgs_maps)/map/centenario1.yaml"/>
     <arg name="autostart" default="true"/>
     <arg name="controller" default=""/>
-    <arg name="localization" default="amcl"/>
-
-    <node pkg="nav2_map_server" name="map_server" exec="map_server">
-        <param name="use_sim_time" value="$(var use_sim_time)"/>
-        <param name="yaml_filename" value="$(var map)"/>
-    </node>
+    <arg name="localization" default="slam_toolbox_mapping"/>
 
     <node pkg="nav2_controller" exec="controller_server" name="controller_server">
         <param name="use_sim_time" value="$(var use_sim_time)"/>
@@ -62,6 +57,7 @@
     </node>
 
 	<include file="$(find-pkg-share twil_2dnav)/launch/localization_$(var localization).launch.xml">
+        <arg name="use_sim_time" value="$(var use_sim_time)"/>
 	</include>
 
 </launch>
diff --git a/twil_bringup/config/twist_mrac_linearizing_controller.yaml b/twil_bringup/config/twist_mrac_linearizing_controller.yaml
index 9b9aeab..acfedd0 100644
--- a/twil_bringup/config/twist_mrac_linearizing_controller.yaml
+++ b/twil_bringup/config/twist_mrac_linearizing_controller.yaml
@@ -20,10 +20,7 @@ twist_mrac_linearizing_controller:
     wheel_separation: 0.322
     wheel_radius: [0.075, 0.075]
     odom_frame_id: "odom"
-    # 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"
+    base_frame_id: "twil_origin"
     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 f09ebc8..3cf65f9 100644
--- a/twil_bringup/launch/twist_mrac_linearizing_controller.launch.xml
+++ b/twil_bringup/launch/twist_mrac_linearizing_controller.launch.xml
@@ -24,9 +24,6 @@
 
 	<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"/>