Merge pull request #7 from i-am-neet/master

v2.0.0

timda_bringup/launch/robot_sim.launch → timda_bringup/launch/robot_mapping_sim.launch

@@ -2,6 +2,11 @@
   <param name="use_sim_time" value="true" />
   <include file="$(find timda_gazebo)/launch/timda_sim.launch" />
   <include file="$(find ira_laser_tools)/launch/laserscan_multi_merger.launch" />
-  <include file="$(find timda_bringup)/launch/hector_slam.launch" />
+  <include file="$(find timda_bringup)/launch/hector_slam.launch">
+    <!-- w/o odom-->
+    <!-- <arg name="odom_frame" default="base_link" /> -->
+    <!-- w/ odom-->
+    <arg name="odom_frame" value="odom" />
+  </include>
   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find timda_description)/rviz/laser.rviz" />
 </launch>

timda_bringup/launch/robot_nav_sim.launch

@@ -0,0 +1,43 @@
+<launch>
+  <arg name="use_map" default="home-area" />
+
+  <arg name="odom_frame_id"   default="odom"/>
+  <arg name="base_frame_id"   default="base_footprint"/>
+  <arg name="global_frame_id" default="map"/>
+  <arg name="odom_topic" default="odom" />
+  <arg name="laser_topic" default="scan_multi" />
+
+  <param name="use_sim_time" value="true" />
+
+  <include file="$(find timda_gazebo)/launch/timda_sim.launch" />
+  <include file="$(find ira_laser_tools)/launch/laserscan_multi_merger.launch" />
+
+  <node pkg="map_server" type="map_server" name="map_server"
+        args="$(find timda_configs)/maps/$(arg use_map).yaml" />
+
+  <include file="$(find timda_bringup)/launch/amcl.launch" />
+
+  <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
+    <rosparam file="$(find timda_configs)/nav/costmap_common_params.yaml" command="load" ns="global_costmap" /> 
+    <rosparam file="$(find timda_configs)/nav/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find timda_configs)/nav/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find timda_configs)/nav/global_costmap_params.yaml" command="load" /> 
+    <!-- <rosparam file="$(find timda_configs)/nav/base_local_planner_params.yaml" command="load" /> -->
+    <rosparam file="$(find timda_configs)/nav/dwa_local_planner_params.yaml" command="load" />
+    <rosparam file="$(find timda_configs)/nav/move_base_params.yaml" command="load" />
+    <rosparam file="$(find timda_configs)/nav/global_planner_params.yaml" command="load" />
+    <rosparam file="$(find timda_configs)/nav/navfn_global_planner_params.yaml" command="load" />
+
+    <!-- reset frame_id parameters using user input data -->
+    <param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
+    <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
+    <param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
+    <param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
+    <param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>
+
+    <remap from="cmd_vel" to="mobile/cmd_vel" />
+    <remap from="odom" to="$(arg odom_topic)" />
+    <remap from="scan" to="$(arg laser_topic)" />
+  </node>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find timda_configs)/rviz/nav.rviz" />
+</launch>

timda_configs/nav/base_local_planner_params.yaml

@@ -0,0 +1,11 @@
+TrajectoryPlannerROS:
+  max_vel_x: 0.45
+  min_vel_x: 0.1
+  max_vel_theta: 1.0
+  min_in_place_vel_theta: 0.4
+
+  acc_lim_theta: 3.2
+  acc_lim_x: 2.5
+  acc_lim_y: 2.5
+
+  holonomic_robot: true

timda_configs/nav/costmap_common_params.yaml

@@ -0,0 +1,58 @@
+max_obstacle_height: 1.5 # assume something like an arm is mounted on top of the robot
+
+# TIMDA's shape
+footprint: [[0.3745, 0.1715], [0.4395, 0.287], [0.3915, 0.333], [0.27634, 0.271], [-0.3745, 0.271],
+            [-0.3745, -0.1715], [-0.4395, -0.287], [-0.3915, -0.333], [-0.27634, -0.271], [0.3745, -0.271]]
+#robot_radius: ir_of_robot
+
+map_type: voxel
+
+#Set the tolerance we're willing to have for tf transforms
+transform_tolerance: 0.05
+
+#Obstacle marking parameters
+obstacle_layer:
+  enabled: true
+  max_obstacle_height: 0.6
+  origin_z: 0.0
+  z_resolution: 0.2
+  z_voxels: 2
+  unknown_threshold: 15
+  mark_threshold: 0
+  combination_method: 1
+  track_unknown_space: true
+  obstacle_range: 2.5
+  raytrace_range: 3.0
+  publish_voxel_map: false
+  observation_sources: scan
+  scan:
+    sensor_frame: base_link
+    data_type: LaserScan
+    topic: scan_multi
+    marking: true
+    clearing: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 1.5
+
+#cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
+inflation_layer:
+  enabled:              true
+  cost_scaling_factor:  5.0  # exponential rate at which the obstacle cost drops off (default: 10)
+  inflation_radius:     0.525  # max. distance from an obstacle at which costs are incurred for planning paths.
+
+static_layer:
+  enabled:              true
+
+#Inscribed and circumscribed radius 
+inscribed_radius: 0.171.5
+circumscribed_radius: 0.525
+
+#Cost function parameters
+inflation_radius: 0.525
+cost_scaling_factor: 10.0
+
+# observation_sources: laser_scan_sensor # point_cloud_sensor
+
+# laser_scan_sensor: {sensor_frame: base_link, data_type: LaserScan, topic: scan_multi, marking: true, clearing: true}
+
+# point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}

timda_configs/nav/dwa_local_planner_params.yaml

@@ -0,0 +1,56 @@
+DWAPlannerROS:
+
+# Robot Configuration Parameters - Kobuki
+  max_vel_x: 0.5  # 0.55
+  min_vel_x: 0.0 
+
+  max_vel_y: 0.0  # diff drive robot
+  min_vel_y: 0.0  # diff drive robot
+
+  max_trans_vel: 0.5 # choose slightly less than the base's capability
+  min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+  trans_stopped_vel: 0.1
+
+  # Warning!
+  #   do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
+  #   are non-negligible and small in place rotational velocities will be created.
+
+  max_rot_vel: 5.0  # choose slightly less than the base's capability
+  min_rot_vel: 0.4  # this is the min angular velocity when there is negligible translational velocity
+  rot_stopped_vel: 0.4
+
+  acc_lim_x: 1.0 # maximum is theoretically 2.0, but we 
+  acc_lim_theta: 2.0
+  acc_lim_y: 0.0      # diff drive robot
+
+# Goal Tolerance Parameters
+  yaw_goal_tolerance: 0.3  # 0.05
+  xy_goal_tolerance: 0.15  # 0.10
+  # latch_xy_goal_tolerance: false
+
+# Forward Simulation Parameters
+  sim_time: 1.0       # 1.7
+  vx_samples: 6       # 3
+  vy_samples: 1       # diff drive robot, there is only one sample
+  vtheta_samples: 20  # 20
+
+# Trajectory Scoring Parameters
+  path_distance_bias: 64.0      # 32.0   - weighting for how much it should stick to the global path plan
+  goal_distance_bias: 24.0      # 24.0   - wighting for how much it should attempt to reach its goal
+  occdist_scale: 0.5            # 0.01   - weighting for how much the controller should avoid obstacles
+  forward_point_distance: 0.325 # 0.325  - how far along to place an additional scoring point
+  stop_time_buffer: 0.2         # 0.2    - amount of time a robot must stop in before colliding for a valid traj.
+  scaling_speed: 0.25           # 0.25   - absolute velocity at which to start scaling the robot's footprint
+  max_scaling_factor: 0.2       # 0.2    - how much to scale the robot's footprint when at speed.
+
+# Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.05  # 0.05   - how far to travel before resetting oscillation flags
+
+# Debugging
+  publish_traj_pc : true
+  publish_cost_grid_pc: true
+  global_frame_id: odom
+
+
+# Differential-drive robot configuration - necessary?
+#  holonomic_robot: false

timda_configs/nav/global_costmap_params.yaml

@@ -0,0 +1,12 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: base_footprint
+  update_frequency: 1.0
+  publish_frequency: 0.5
+  static_map: true
+  transform_tolerance: 0.05
+  #<!-- global map引入了以下三层，经融合构成了master map，用于global planner-->
+  plugins:
+    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
+    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
+    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

timda_configs/nav/global_planner_params.yaml

@@ -0,0 +1,20 @@
+
+GlobalPlanner:                                  # Also see: http://wiki.ros.org/global_planner
+  old_navfn_behavior: false                     # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false
+  use_quadratic: true                           # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true
+  use_dijkstra: true                            # Use dijkstra's algorithm. Otherwise, A*, default true
+  use_grid_path: false                          # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false
+
+  allow_unknown: true                           # Allow planner to plan through unknown space, default true
+                                                #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work
+  planner_window_x: 0.0                         # default 0.0
+  planner_window_y: 0.0                         # default 0.0
+  default_tolerance: 0.0                        # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0
+
+  publish_scale: 100                            # Scale by which the published potential gets multiplied, default 100
+  planner_costmap_publish_frequency: 0.0        # default 0.0
+
+  lethal_cost: 253                              # default 253
+  neutral_cost: 50                              # default 50
+  cost_factor: 3.0                              # Factor to multiply each cost from costmap by, default 3.0
+  publish_potential: true                       # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true

timda_configs/nav/local_costmap_params.yaml

@@ -0,0 +1,14 @@
+local_costmap:
+  global_frame: odom
+  robot_base_frame: base_footprint
+  update_frequency: 5.0
+  publish_frequency: 2.0
+  static_map: false
+  rolling_window: true
+  width: 4.0
+  height: 4.0
+  resolution: 0.05
+  transform_tolerance: 0.3
+  plugins:
+   - {name: obstacle_layer,      type: "costmap_2d::VoxelLayer"}
+   - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

timda_configs/nav/move_base_params.yaml

@@ -0,0 +1,17 @@
+shutdown_costmaps: false
+
+controller_frequency: 5.0
+controller_patience: 3.0
+
+
+planner_frequency: 1.0
+planner_patience: 5.0
+
+oscillation_timeout: 10.0
+oscillation_distance: 0.2
+
+# local planner - default is trajectory rollout
+base_local_planner: "dwa_local_planner/DWAPlannerROS"
+# <!--这里配置了local planner为dwa_local_planner -->
+# <!--这里配置了global planner为navfn/NavfnROS -->
+base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner

timda_configs/nav/navfn_global_planner_params.yaml

@@ -0,0 +1,10 @@
+
+NavfnROS:
+  visualize_potential: false    #Publish potential for rviz as pointcloud2, not really helpful, default false
+  allow_unknown: false          #Specifies whether or not to allow navfn to create plans that traverse unknown space, default true
+                                #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work
+  planner_window_x: 0.0         #Specifies the x size of an optional window to restrict the planner to, default 0.0
+  planner_window_y: 0.0         #Specifies the y size of an optional window to restrict the planner to, default 0.0
+
+  default_tolerance: 0.0        #If the goal is in an obstacle, the planer will plan to the nearest point in the radius of default_tolerance, default 0.0
+                                #The area is always searched, so could be slow for big values