[Multimap - manual merge]

+ conflicts found from kinetic to melodic

summit_xl_description/robots/versions/summit_xl_180307A.urdf.xacro

@@ -69,6 +69,11 @@
 		<!--xacro:sensor_axis prefix="$(arg prefix)front_ptz_camera" parent="$(arg prefix)base_link">
 			<origin xyz="0.19 0 0.26" rpy="0 ${15*PI/180} 0"/>		
 		</xacro:sensor_axis-->
+
+		<!--xacro:sensor_vlp16 prefix="$(arg prefix)front_laser" parent="$(arg prefix)base_link" prefix_topic="front_laser">
+			<origin xyz="0.0 0.0 0.33" rpy="0 0 0"/>
+		</xacro:sensor_vlp16-->
+
 	    <!-- RGBD -->
 	    <!--xacro:sensor_orbbec_astra prefix="$(arg prefix)front_rgbd_camera" parent="$(arg prefix)base_link" prefix_topic="front_rgbd_camera">
 			<origin xyz="0.324 0.012 0.172" rpy="0 0 0"/> 

summit_xl_localization/config/amcl.yaml

@@ -1,5 +1,5 @@
 # Publish scans from best pose at a max of 10 Hz 
-use_map_topic: false
+use_map_topic: true
 odom_model_type: $(arg odom_model_type)
 odom_alpha5: 0.1
 transform_tolerance: 0.2
@@ -23,7 +23,7 @@ laser_lambda_short: 0.1
 laser_lambda_short: 0.1
 laser_model_type: likelihood_field
 # laser_model_type: beam
-laser_likelihood_max_dist: 2.0
+laser_likelihood_max_dist: 10.0
 update_min_d: 0.2
 update_min_a: 0.5
 odom_frame_id: $(arg odom_frame)

summit_xl_localization/launch/amcl.launch

@@ -22,7 +22,6 @@
     <remap from="map" to="$(arg map_topic)"/>
 
     <rosparam command="load" file="$(arg config_path)/amcl.yaml" subst_value="true"/>
-
   </node>
 
 </launch>

summit_xl_navigation/config/costmap_common_params.yaml

@@ -1,4 +1,4 @@
-footprint: [[0.375, -0.34], [0.375, 0.34], [-0.375,0.34], [-0.375, -0.34]]
+footprint: [[0.35, -0.3], [0.35, 0.3], [-0.35,0.3], [-0.35, -0.3]]
 
 publish_frequency: 1.0
 
@@ -20,12 +20,8 @@ obstacle_laser_layer:
     marking: true
     clearing: true
 
-obstacle_camera_layer:
+obstacle_layer:
   obstacle_range: 2.5
-  raytrace_range: 5.5
-  observation_sources: front_rgbd_to_scan
-  front_rgbd_to_scan:
-    data_type: LaserScan
-    topic: $(arg front_rgbd_to_scan_topic)
-    marking: true
-    clearing: true
+  raytrace_range: 3.0
+  observation_sources: scan
+  scan: {sensor_frame: summit_xl_front_laser_link, data_type: LaserScan, topic: scan, marking: true, clearing: true}

summit_xl_navigation/config/global_costmap_params_no_map.yaml

@@ -0,0 +1,14 @@
+#Independent settings for the planner's costmap
+global_costmap:
+    publish_voxel_map: true
+    global_frame: summit_xl_a_odom
+    robot_base_frame: summit_xl_a_base_footprint
+    update_frequency: 5.0
+    static_map: false
+    rolling_window: true
+    width: 30.0
+    height: 30.0
+    resolution: 0.025
+    origin_x: 0.0
+    origin_y: 0.0
+    inflation_radius: 0.26  

summit_xl_navigation/config/local_costmap_params.yaml

@@ -1,13 +1,13 @@
 local_costmap:
   global_frame: $(arg odom_frame)
   robot_base_frame: $(arg base_frame)
-  update_frequency: 5.0
+  update_frequency: 10.0
   static_map: false
   rolling_window: true
-  width: 7.0 
-  height: 7.0 
+  width: 10.0 #7.0 
+  height: 10.0 #7.0 
   resolution: 0.05 
-  
+
   # padding is 0.1 by default, making difficult to pass through narrow places
   footprint_padding: 0.0
 

summit_xl_navigation/config/move_base.yaml

@@ -0,0 +1,50 @@
+# Planner selection
+base_global_planner: "navfn/NavfnROS"
+base_local_planner: "base_local_planner/TrajectoryPlannerROS"
+
+# Recovery behaviors are defined in robot folders
+
+# Oscillation
+oscillation_timeout: 10.0
+oscillation_distance: 0.5
+
+# Global planner
+planner_frequency: 0.0
+planner_patience: 5.0
+NavfnROS:
+  allow_unknown: false  # TODO: investigate this
+  default_tolerance: 0.0
+  # do not restrict planner
+  planner_window_x: 0.0
+  planner_window_y: 0.0
+  # debug
+  visualize_potential: false
+
+# Local planner
+controller_frequency: 25.0
+controller_patience: 15.0
+TrajectoryPlannerROS:
+  # base vel/accel profile is in robot folders
+  # tolerances (defaults)
+  yaw_goal_tolerance: 0.15
+  xy_goal_tolerance: 0.2
+  latch_xy_goal_tolerance: true
+  # forward simulation
+  sim_time: 1.0
+  sim_granularity: 0.025
+  angular_sim_granularity: 0.025
+  vx_samples: 3
+  vtheta_samples: 10
+  # scoring (defaults)
+  meter_scoring: true
+  pdist_scale: 8.0
+  gdist_scale: 12.0
+  occdist_scale: 0.1
+  heading_lookahead: 0.325
+  heading_scoring_timestep: 0.8
+  heading_scoring: true
+  dwa: false
+  # other
+  oscillation_reset_dist: 0.05
+  # debug
+  publish_cost_grid_pc: false

summit_xl_navigation/config/teb_local_planner_diff_params.yaml

@@ -1,8 +1,8 @@
 base_local_planner: teb_local_planner/TebLocalPlannerROS
 
 TebLocalPlannerROS:
-  odom_topic: $(arg odom_topic)
-  map_frame: $(arg global_frame)
+  odom_topic: odom
+  map_frame: map
 
   # Trajectory
 
@@ -23,29 +23,41 @@ TebLocalPlannerROS:
 
   # Robot
 
-  max_vel_x: 0.6 #1.2
+#  max_vel_x: 1.2
+  max_vel_x: 3.0
   max_vel_x_backwards: 0.15
-  max_vel_theta: 0.75
+#  max_vel_theta: 0.75
+  max_vel_theta: 1.0
   max_vel_y: 0 # not used, is differential
   acc_lim_y: 0 # not used, is differential
-  acc_lim_x: 0.2
-  acc_lim_theta: 0.2
+#  acc_lim_x: 0.2
+#  acc_lim_theta: 0.2
+  acc_lim_x: 3.0
+  acc_lim_theta: 3.0
   min_turning_radius: 0.0
   wheelbase: 0.0 # not used, is differential
   cmd_angle_instead_rotvel: false # not used, is differential
   footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
-    type: "polygon"
-    vertices: [[0.375, -0.34], [0.375, 0.34], [-0.375,0.34], [-0.375, -0.34]]
+    #type: "circular"
+    #radius: 0.5 # for type "circular"
+    type: "line"
+    line_start: [-0.0545, 0.0] # for type "line"
+    line_end: [0.0545, 0.0] # for type "line"
+    #front_offset: 0.2 # for type "two_circles"
+    #front_radius: 0.2 # for type "two_circles"
+    #rear_offset: 0.2 # for type "two_circles"
+    #rear_radius: 0.2 # for type "two_circles"
+    #vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
 
   # GoalTolerance
 
-  xy_goal_tolerance: 0.1
-  yaw_goal_tolerance: 0.05
+  xy_goal_tolerance: 0.5
+  yaw_goal_tolerance: 0.5
   free_goal_vel: False #False
 
   # Obstacles
 
-  min_obstacle_dist: 0.3 # minimum distance to obstacle: it depends on the footprint_model
+  min_obstacle_dist: 0.435 # minimum distance to obstacle: it depends on the footprint_model
   include_costmap_obstacles: True # use the local costmap
   costmap_obstacles_behind_robot_dist: 1.0 # distance at which obstacles behind the robot are taken into account
   obstacle_poses_affected: 30 # unused if legacy_obstacle_association is false
@@ -56,7 +68,9 @@ TebLocalPlannerROS:
   costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
   costmap_converter_spin_thread: True
   costmap_converter_rate: 5
-
+  # costmap_converter/CostmapToLinesDBSRANSAC:
+  #    cluster_max_distance: 0.4
+
   # Optimization
 
   no_inner_iterations: 5

summit_xl_navigation/config/teb_local_planner_omni_params.yaml

@@ -1,8 +1,8 @@
 base_local_planner: teb_local_planner/TebLocalPlannerROS
 
 TebLocalPlannerROS:
-  odom_topic: $(arg odom_topic)
-  map_frame: $(arg global_frame)
+  odom_topic: odom
+  map_frame: map
 
   # Trajectory
 
@@ -23,29 +23,43 @@ TebLocalPlannerROS:
 
   # Robot
 
-  max_vel_x: 0.75
+#  max_vel_x: 1.2
+  max_vel_x: 1.5
   max_vel_x_backwards: 0.15
-  max_vel_theta: 0.785
-  max_vel_y: 0.5
-  acc_lim_y: 0.2
-  acc_lim_x: 0.2
-  acc_lim_theta: 0.2
+#  max_vel_theta: 0.2
+  max_vel_theta: 1.0
+  max_vel_y: 0.5 # not used, is differential
+  acc_lim_y: 0.5 # not used, is differential
+#  acc_lim_x: 0.2
+  acc_lim_x: 1.5
+#  acc_lim_theta: 0.2
+  acc_lim_theta: 1.0
   min_turning_radius: 0.0
   wheelbase: 0.0
   cmd_angle_instead_rotvel: false
   footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
-    type: "polygon"
-    vertices: [[0.375, -0.34], [0.375, 0.34], [-0.375,0.34], [-0.375, -0.34]]
+    #type: "circular"
+    #radius: 0.5 # for type "circular"
+    type: "line"
+    line_start: [-0.0545, 0.0] # for type "line"
+    line_end: [0.0545, 0.0] # for type "line"
+    #front_offset: 0.2 # for type "two_circles"
+    #front_radius: 0.2 # for type "two_circles"
+    #rear_offset: 0.2 # for type "two_circles"
+    #rear_radius: 0.2 # for type "two_circles"
+    #vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
 
+
+  enable_allowable_speed: true # added by Jeongwoo Han
   # GoalTolerance
 
-  xy_goal_tolerance: 0.2
-  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.05
+  yaw_goal_tolerance: 0.05
   free_goal_vel: False
 
   # Obstacles
 
-  min_obstacle_dist: 0.30 # minimum distance to obstacle: it depends on the footprint_model
+  min_obstacle_dist: 0.435 # minimum distance to obstacle: it depends on the footprint_model
   include_costmap_obstacles: True # use the local costmap
   costmap_obstacles_behind_robot_dist: 1.0 # distance at which obstacles behind the robot are taken into account
   obstacle_poses_affected: 30 # unused if legacy_obstacle_association is false
@@ -56,7 +70,9 @@ TebLocalPlannerROS:
   costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
   costmap_converter_spin_thread: True
   costmap_converter_rate: 5
-
+  # costmap_converter/CostmapToLinesDBSRANSAC:
+  #    cluster_max_distance: 0.4
+
   # Optimization
 
   no_inner_iterations: 5
@@ -70,8 +86,8 @@ TebLocalPlannerROS:
   weight_acc_lim_x: 1
   weight_acc_lim_y: 1
   weight_acc_lim_theta: 1
-  weight_kinematics_nh: 10 # is a nonholonomic robot
-  weight_kinematics_forward_drive: 50 # prefer forward driving, for differential
+  weight_kinematics_nh: 5 # is a nonholonomic robot
+  weight_kinematics_forward_drive: 10 # prefer forward driving, for differential
   weight_kinematics_turning_radius: 1 # prefer turns that respect the min_turning_radius, not used if differential (min_turning_radius = 0)
   weight_optimaltime: 10 # prefer trajectories with less transition time
   weight_obstacle: 50 # prefer trajectories that respect the min_obstacle_dist

summit_xl_navigation/launch/move_base.launch

@@ -18,22 +18,50 @@
   <arg name="global_frame" default="$(arg prefix)map"/>
   <arg name="odom_frame" default="$(arg prefix)odom"/>
   <arg name="base_frame" default="$(arg prefix)base_footprint"/>
+  <arg name="scan_topic" default="front_laser/scan"/>
+  <!--arg name="base_global_planner" default="voronoi_planner/VoronoiPlanner"/-->
+  <arg name="base_global_planner" default="global_planner/GlobalPlanner"/>
+
+  <!-- Run move_base -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base">
+	<rosparam file="$(find summit_xl_navigation)/config/move_base_params.yaml" command="load" />
+	<rosparam file="$(find summit_xl_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
+	<rosparam file="$(find summit_xl_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+	<rosparam file="$(find summit_xl_navigation)/config/local_costmap_params.yaml" command="load" />
+	<rosparam file="$(find summit_xl_navigation)/config/global_costmap_params_map.yaml" command="load" />
+	<!--rosparam file="$(find summit_xl_navigation)/config/teb_local_planner_params.yaml" command="load" /-->
+	<rosparam file="$(find summit_xl_navigation)/config/teb_local_planner_omni_params.yaml" command="load" />
+	<!--rosparam file="$(find summit_xl_navigation)/config/teb_local_planner_diff_params.yaml" command="load" /-->
+	<!-- reset frame_id parameters using user input data -->
+	<param name="global_costmap/obstacle_layer/scan/sensor_frame" value="$(arg prefix)front_laser_link"/>
+	<param name="local_costmap/obstacle_layer/scan/sensor_frame" value="$(arg prefix)front_laser_link"/>
+	<param name="global_costmap/obstacle_layer/scan/topic" value="$(arg scan_topic)"/>
+	<param name="local_costmap/obstacle_layer/scan/topic" value="$(arg scan_topic)"/>
+	<param name="local_costmap/global_frame" value="$(arg odom_frame)"/>
+	<param name="local_costmap/robot_base_frame" value="$(arg base_frame)"/>
+	<param name="global_costmap/global_frame" value="$(arg global_frame)"/>	
+	<param name="global_costmap/robot_base_frame" value="$(arg base_frame)"/>
+	<param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame)"/>
+	<!--param name="TebLocalPlannerROS/map_frame" value="$(arg global_frame)"/-->
+	<!--param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" /-->    
+	<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"/>    
+	<!--param name="base_local_planner" value="eband_local_planner/EBandPlannerROS" /-->    
+	<!--param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /-->    	
+	<param name="controller_frequency" value="5.0" />
+	<param name="controller_patience" value="15.0" /> 
+
+	<!-- Global Planner Selection -->
+	<param name="base_global_planner" value="$(arg base_global_planner)"/>
 
-  <arg name="differential_robot" default="$(optenv ROBOT_DIFFERENTIAL_KINEMATICS true)"/>
-  <arg if="$(arg differential_robot)" name="kinematics" value="diff"/>
-  <arg unless="$(arg differential_robot)" name="kinematics" value="omni"/>
-  <arg name="local_planner" default="$(optenv ROBOT_MOVE_BASE_LOCAL_PLANNER eband)"/>
+	<!-- Global Planner Config -->
+	<!--param name="GlobalPlanner/lethal_cost" value="253" />
+	<param name="GlobalPlanner/neutral_cost" value="66" />
+	<param name="GlobalPlanner/cost_factor" value="0.55" />
+	<param name="GlobalPlanner/use_dijkstra" value="false" /-->
 
-  <include file="$(find summit_xl_navigation)/launch/move_base_$(arg local_planner).launch">
-    <arg name="config_path" value="$(arg config_path)"/>
-    <arg name="id_robot" value="$(arg id_robot)"/>
-
-    <!-- Topics argumments -->
-    <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/>
-    <arg name="odom_topic" value="$(arg odom_topic)"/>
-    <arg name="front_scan_topic" value="$(arg front_scan_topic)"/>
-    <arg name="rear_scan_topic" value="$(arg rear_scan_topic)"/>
-    <arg name="front_rgbd_to_scan_topic" value="$(arg front_rgbd_to_scan_topic)"/>
+	<!-- DWA Local Planner Config -->
+	<!--rosparam file="$(find summit_xl_navigation)/config/dwa_local_planner_params.yaml"/-->
+  </node>
 
     <!-- Frames arguments -->
     <arg name="global_frame" value="$(arg global_frame)"/>