Merge pull request #22 from butia-bots/add-doris-gazebo

Add doris gazebo

butia_navigation_description/config/amcl.launch.xml

@@ -1,15 +1,19 @@
 <launch>
 
+  <arg name="scan_topic" default="scan" />
+
   <node pkg="amcl" type="amcl" name="amcl" output="screen">
     <param name="min_particles" value="500"/>
     <param name="kld_err" value="0.05"/>
     <param name="odom_alpha1" value="0.2"/>
     <param name="odom_alpha2" value="0.2"/>
     <param name="odom_alpha3" value="0.8"/>
     <param name="odom_alpha4" value="0.1"/>
+    <param name="initial_pose_y" value="-0.3"/>
+    <param name="initial_pose_x" value="-1.2"/>
     <param name="resample_interval" value="1"/>
 
-    <!-- <remap from="scan" to="$(arg scan_topic)"/> -->
+    <remap from="scan" to="$(arg scan_topic)"/>
   </node>
 
 </launch>

butia_navigation_description/config/param/costmap_common_params.yaml

@@ -3,7 +3,7 @@ max_obstacle_height: 1.9  # assume something like an arm is mounted on top of th
 # Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation)
 # robot_radius: 0.35  # distance a circular robot should be clear of the obstacle (kobuki: 0.18)
 # footprint: [[0.75, 0.0], [0.30, -0.25], [-0.30, -0.25], [-0.30, 0.25], [0.30, 0.25]]   # if the robot is not circular
-footprint: [[0.75, 0.0], [0.75, 0.075], [0.30, 0.075], [0.30, 0.25], [-0.30, 0.25], [-0.30, -0.25], [0.30, -0.25], [0.30, -0.075], [0.75, -0.075]]
+footprint: [[0.35, 0.0], [0.35, 0.075], [0.29, 0.075], [0.29, 0.24], [-0.29, 0.24], [-0.29, -0.24], [0.29, -0.24], [0.29, -0.075], [0.35, -0.075]]
 
 map_type: voxel
 
@@ -18,39 +18,39 @@ obstacle_layer:
   combination_method:   1
   track_unknown_space:  true    # true needed for disabling global path planning through unknown space
   footprint_clearing_enabled: true
-  obstacle_range: 3.5
   raytrace_range: 8.0
   publish_voxel_map: false
-  observation_sources:  body_scan ground_scan
+  observation_sources: body_scan ground_scan
   body_scan:
     data_type: LaserScan
     topic: scan
     marking: true   # whether the sensor will be used to add obstacle information to the costmap
     clearing: true  # whether the sensor will be used to clear obstacle information from the costmap
-    min_obstacle_height: 0.68
+    min_obstacle_height: 0.2
     max_obstacle_height: 1.9
     observation_persistence: 0.0  # parametros sendo testados
     raytrace_range: 8.0
+    obstacle_range: 3.5
     inf_is_valid: true
   ground_scan:
     data_type: LaserScan
     topic: scan2
     marking: true   # whether the sensor will be used to add obstacle information to the costmap
     clearing: true  # whether the sensor will be used to clear obstacle information from the costmap
     min_obstacle_height: 0.0
-    max_obstacle_height: 0.68
+    max_obstacle_height: 0.2
     observation_persistence: 0.0  # parametros sendo testados
+    obstacle_range: 1.5
     raytrace_range: 4.0
     inf_is_valid: true  
-    footprint_clearing_enabled: true
   # for debugging only, let's you see the entire voxel grid
 
 
 #cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
 inflation_layer:
   enabled:              true
-  cost_scaling_factor:  2.58  # exponential rate at which the obstacle cost drops off (default: 10)
-  inflation_radius:     1.75  # max. distance from an obstacle at which costs are incurred for planning paths. (default: 0.4)
+  cost_scaling_factor:  10  # exponential rate at which the obstacle cost drops off (default: 10)
+  inflation_radius:     0.55 # max. distance from an obstacle at which costs are incurred for planning paths. (default: 0.4)
 
 static_layer:
   enabled:              true

butia_navigation_description/config/param/dwa_local_planner_params.yaml

@@ -2,7 +2,7 @@ DWAPlannerROS:
 
 # Robot Configuration Parameters - Kobuki
   max_vel_x: 0.4  ## 0.55
-  min_vel_x: 0.0  ##
+  min_vel_x: 0  ##
 
   max_vel_y: 0.0  # diff drive robot
   min_vel_y: 0.0  # diff drive robot
@@ -24,8 +24,8 @@ DWAPlannerROS:
   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
+  yaw_goal_tolerance: 0.05  # 0.3
+  xy_goal_tolerance: 0.1  # 0.15
   latch_xy_goal_tolerance: true  # simply rotate after reaching xy, even if it ends up outside the tolerance after doing so
 
 # Forward Simulation Parameters

butia_navigation_gazebo/maps/cafe.yaml

@@ -0,0 +1,7 @@
+image: cafe.pgm
+resolution: 0.050000
+origin: [-13.800000, -21.800000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

butia_navigation_gazebo/maps/demo_world.yaml

@@ -0,0 +1,7 @@
+image: /home/cris/catkin_ws/src/butia_navigation_system/butia_navigation_gazebo/maps/demo_world.pgm
+resolution: 0.050000
+origin: [-12.200000, -13.800000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

butia_navigation_gazebo/maps/room.yaml

@@ -0,0 +1,7 @@
+image: room.pgm
+resolution: 0.050000
+origin: [-15.400000, -15.400000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

butia_navigation_gazebo/maps/willow.yaml

@@ -1,6 +1,6 @@
 image: willow.pgm
 resolution: 0.050000
-origin: [-23.400000, -13.800000, 0.000000]
+origin: [-17.000000, -18.600000, 0.000000]
 negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.196

butia_navigation_gazebo/navigate-doris.launch

@@ -0,0 +1,81 @@
+<?xml version="1.0"?>
+<launch>
+    <!--env name="GAZEBO_MODEL_PATH" value="$GAZEBO_MODEL_PATH:$(find butia_navigation_gazebo)/models" />
+    <env name="GAZEBO_RESOURCE_PATH" value="$GAZEBO_RESOURCE_PATH:$(find butia_navigation_gazebo)/models" /-->
+
+    <arg name="paused" default="false" />
+    <arg name="use_sim_time" default="true" />
+    <arg name="gui" default="true" />
+    <arg name="headless" default="false" />
+    <arg name="debug" default="false" />
+    <arg name="verbose" default="true" />
+
+    <arg name="start_sim" default="true" />
+
+    <!-- Select a map and world
+  Willow Gagage:
+    <arg name="world" default="worlds/willowgarage.world" />
+    <arg name="map" default="$(find butia_navigation_gazebo)/maps/willow.yaml" />
+  Playground:
+    <arg name="world" default="$(find butia_navigation_gazebo)/worlds/playground.world" />
+    <arg name="map" default="$(find butia_navigation_gazebo)/maps/playground.yaml" />
+  -->
+    <arg name="world" default="worlds/willowgarage.world" />
+    <arg name="map" default="$(find butia_navigation_gazebo)/maps/willow.yaml" />
+
+    <!-- Set command arguments -->
+    <group if="$(arg use_sim_time)">
+        <param name="/use_sim_time" value="true" />
+    </group>
+
+    <arg unless="$(arg paused)" name="command_arg1" value="" />
+    <arg if="$(arg paused)" name="command_arg1" value="-u" />
+    <arg unless="$(arg headless)" name="command_arg2" value="" />
+    <arg if="$(arg headless)" name="command_arg2" value="-r" />
+    <arg unless="$(arg verbose)" name="command_arg3" value="" />
+    <arg if="$(arg verbose)" name="command_arg3" value="--verbose" />
+    <arg unless="$(arg debug)" name="script_type" value="gzserver" />
+    <arg if="$(arg debug)" name="script_type" value="debug" />
+
+    <group if="$(arg start_sim)">
+      <!-- Start gazebo server-->
+      <node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="false" output="screen" args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) $(arg world)" />
+
+
+      <!-- Start gazebo client -->
+      <group if="$(arg gui)">
+          <node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen" />
+      </group>
+      <!-- Spawn robot -->
+      <arg name="urdf" default="$(find doris_description)/urdf/doris_description.urdf.xacro" />
+      <arg name="x" default="0" />
+      <arg name="y" default="0" />
+      <arg name="z" default="0" />
+      <arg name="name" default="pioneer3at_robot" />
+      <include file="$(find butia_navigation_gazebo)/spawn.launch">
+        <arg name="urdf" value="$(arg urdf)" />
+        <arg name="x" value="$(arg x)" />
+        <arg name="y" value="$(arg y)" />
+        <arg name="z" value="$(arg z)" />
+      </include>
+    </group>
+
+    <!-- Start map_server -->
+    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map)" />
+
+    <!-- Localization -->
+    <include file="$(find butia_navigation_description)/config/amcl.launch.xml" />
+
+    <!-- Move base -->
+    <arg name="cmd_vel" default="cmd_vel" />
+    <arg name="odom" default="odom" />
+
+    <include file="$(find butia_navigation_description)/config/move_base.launch.xml">
+        <arg name="cmd_vel_topic" value="$(arg cmd_vel)" />
+        <arg name="odom_topic" value="$(arg odom)" />
+    </include>
+
+    <!-- RVIZ to view the visualization -->
+    <node name="RVIZ" pkg="rviz" type="rviz" args=" -d $(find butia_navigation_gazebo)/navigate-pioneer3at.rviz" required="true" />
+
+</launch>