use new inflation_layer in layered costmap2d

diff --git a/README b/README
index c2fd48224e42978c1296c8d60da96a669a76680f..b14225f742f52b1896f0107f5aace34476be0a84 100644 (file)
--- a/README
+++ b/README
@@ -6,7 +6,6 @@ Its is split in the Nodes:
  Inputs are send via ft/get_inputs message.
  Outputs are set with ft/set_output and ft/set_motor message.
 -robo_explorer.py: Provides the Robo Explorer functions:
- Laserscan is faked by the Sonar Sensor.
 
 You need to use the Robo Explorer with Wheels instead of Tracks because the Tracks are too inaccurate for Odometry.
 I used the Wheel Setup from Mobile Robots 2.

diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml
index 1e07752fc336500868bda9cbda564f5af711cfcb..b4f0f067fe10d921376089f06ed3857acdfd1b17 100644 (file)
--- a/config/costmap_common_params.yaml
+++ b/config/costmap_common_params.yaml
@@ -3,7 +3,3 @@ raytrace_range: 3.0
 #footprint: [[x0, y0], [x1, y1], ... [xn, yn]]
 robot_radius: 0.105
 inflation_radius: 0.15
-
-observation_sources: laser_scan_sensor
-
-laser_scan_sensor: {sensor_frame: forward_sensor, data_type: LaserScan, topic: scan, marking: true, clearing: true}

diff --git a/config/explorer.rviz b/config/explorer.rviz
index e413e5f9ad6aa7a3e9de764353fdb758d1ec11e9..622857872497fa8b8bb01761fd94713be2c373a1 100644 (file)
--- a/config/explorer.rviz
+++ b/config/explorer.rviz
@@ -5,7 +5,6 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /TF1/Frames1
-        - /Map1/Status1
         - /Map1/Orientation1
       Splitter Ratio: 0.618123
     Tree Height: 656
@@ -27,7 +26,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: LaserScan
+    SyncSource: ""
   - Class: rviz/Tool Properties
     Expanded: ~
     Name: Tool Properties
@@ -57,35 +56,6 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 0
-        Min Value: 0
-        Value: true
-      Axis: Z
-      Channel Name: x
-      Class: rviz/LaserScan
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4.20249
-      Min Color: 255; 255; 255
-      Min Intensity: 3.74045
-      Name: LaserScan
-      Position Transformer: XYZ
-      Queue Size: 16
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.01
-      Style: Flat Squares
-      Topic: /scan
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
     - Alpha: 1
       Class: rviz/Polygon
       Color: 25; 255; 0
@@ -98,7 +68,13 @@ Visualization Manager:
       Class: rviz/Path
       Color: 25; 255; 0
       Enabled: true
+      Line Style: Lines
+      Line Width: 0.03
       Name: Global Plan
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
       Topic: /move_base/TrajectoryPlannerROS/global_plan
       Value: true
     - Alpha: 1
@@ -106,7 +82,13 @@ Visualization Manager:
       Class: rviz/Path
       Color: 0; 25; 255
       Enabled: true
+      Line Style: Lines
+      Line Width: 0.03
       Name: Local Plan
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
       Topic: /move_base/TrajectoryPlannerROS/local_plan
       Value: true
     - Alpha: 1
@@ -114,7 +96,13 @@ Visualization Manager:
       Class: rviz/Path
       Color: 25; 255; 0
       Enabled: true
+      Line Style: Lines
+      Line Width: 0.03
       Name: Planner Plan
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
       Topic: /move_base/NavfnROS/plan
       Value: true
     - Alpha: 1
@@ -195,11 +183,11 @@ Visualization Manager:
       Buffer Length: 1
       Class: rviz/Range
       Color: 255; 255; 255
-      Enabled: false
+      Enabled: true
       Name: Range
       Queue Size: 100
-      Topic: /sonar_forward_lower
-      Value: false
+      Topic: /sonar
+      Value: true
     - Alpha: 0.7
       Class: rviz/Map
       Color Scheme: costmap
@@ -212,10 +200,10 @@ Visualization Manager:
       Class: rviz/Map
       Color Scheme: map
       Draw Behind: false
-      Enabled: true
+      Enabled: false
       Name: Map
       Topic: /map
-      Value: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -240,22 +228,22 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 3.63988
+      Distance: 4.16269
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 3.49739
-        Y: 1.95458
-        Z: 0.198305
+        X: 5.27226
+        Y: 3.90177
+        Z: 0.615206
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 1.5598
+      Pitch: 1.4798
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 4.71126
+      Yaw: 3.02126
     Saved: ~
 Window Geometry:
   Displays:

diff --git a/config/global_costmap_params.yaml b/config/global_costmap_params.yaml
index 3a208225af6c8a59cb317beaffc63838a3813d26..33c1f85e6a5d638d2424611b3da937329d2bd64f 100644 (file)
--- a/config/global_costmap_params.yaml
+++ b/config/global_costmap_params.yaml
@@ -6,3 +6,6 @@ global_costmap:
   rolling_window: true
   width: 10.0
   height: 10.0
+  plugins:
+  - {name: sonar,   type: "range_sensor_layer::RangeSensorLayer"}
+  - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}

diff --git a/config/local_costmap_params.yaml b/config/local_costmap_params.yaml
index c8c35defd35cdc70bab15ca23f57fa649ba1cd5b..f3cfe7c402c847c3a1357f888a4ba69d28543151 100644 (file)
--- a/config/local_costmap_params.yaml
+++ b/config/local_costmap_params.yaml
@@ -8,3 +8,6 @@ local_costmap:
   width: 4.0
   height: 4.0
   resolution: 0.01
+  plugins:
+  - {name: sonar,   type: "range_sensor_layer::RangeSensorLayer"}
+  - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}

diff --git a/config/map.png b/config/map.png
deleted file mode 100644 (file)
index edbf44f..0000000
Binary files a/config/map.png and /dev/null differ

diff --git a/explorer_configuration.launch b/explorer_configuration.launch
index 8b05a6d7873855d73a8bade7109a45c71e9bb85c..4bff958d95845818ee8c56014dd4e0fc6bb7177e 100644 (file)
--- a/explorer_configuration.launch
+++ b/explorer_configuration.launch
@@ -6,8 +6,6 @@
        </node>
 
        <node pkg="roboint" type="robo_explorer.py" name="robo_explorer" output="screen">
-               <!-- fake laser scan with ultra sonic range finder -->
-               <param name="ultrasonic_laser" value="True" />
                <!-- Distance between both wheels in meter (18.55cm) -->
                <param name="wheel_dist" value="0.1855" />
                <!-- Size of wheel Diameter in meter (5.15cm) * gear ratio (0.5) = 2.575cm -->
@@ -16,8 +14,5 @@
 
        <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
 
-       <arg name="map_file" default="$(find roboint)/config/map.png" />
-       <include file="$(find roboint)/launch/move_base.launch">
-               <arg name="map_file" value="$(arg map_file)" />
-       </include>
+       <include file="$(find roboint)/launch/move_base.launch"/>
 </launch>

diff --git a/launch/move_base.launch b/launch/move_base.launch
index 378dd07bcbc06c4ba58393be55aa99435aa47d67..f78142f6fd060cf133f0cf7d2dc12a62a9deda19 100644 (file)
--- a/launch/move_base.launch
+++ b/launch/move_base.launch
@@ -1,11 +1,5 @@
 <?xml version="1.0"?>
 <launch>
-       <!-- Run the map server -->
-       <arg name="map_file" default="$(find roboint)/config/map.png" />
-       <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file) 0.01" >
-               <param name="frame_id" value="odom" />
-       </node>
-
        <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
                <param name="controller_frequency" value="4.0" />
                <rosparam file="$(find roboint)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />

diff --git a/scripts/robo_explorer.py b/scripts/robo_explorer.py
index db7a2271d99713919f260c373fda1075fac6b3bb..bf3522927532cc94d8223e0fd9628448ccb5344c 100755 (executable)
--- a/scripts/robo_explorer.py
+++ b/scripts/robo_explorer.py
@@ -4,7 +4,7 @@ import rospy
 import tf
 from math import *
 from geometry_msgs.msg import Twist, TransformStamped, Point32, PoseWithCovarianceStamped
-from sensor_msgs.msg import LaserScan
+from sensor_msgs.msg import Range
 from nav_msgs.msg import Odometry
 from roboint.msg import Motor
 from roboint.msg import Inputs
@@ -24,8 +24,6 @@ class RoboExplorer:
                self.y_last = 0
                self.alpha_last = 0
 
-               # fake laser scan with ultra sonic range finder
-               self.enable_ultrasonic_laser = bool(rospy.get_param('~ultrasonic_laser', "True"))
                # Distance between both wheels in meter (18.55cm)
                self.wheel_dist = float(rospy.get_param('~wheel_dist', "0.1855"))
                # Size of wheel Diameter in meter (5.15cm) * gear ratio (0.5) = 2.575cm 
@@ -36,8 +34,7 @@ class RoboExplorer:
                self.speed_constant = float(rospy.get_param('~speed_constant', "-1.7"))
 
                self.pub_motor = rospy.Publisher("ft/set_motor", Motor, queue_size=16)
-               if self.enable_ultrasonic_laser:
-                       self.pub_scan = rospy.Publisher("scan", LaserScan, queue_size=16)
+               self.pub_sonar = rospy.Publisher("sonar", Range, queue_size=16)
                self.pub_odom = rospy.Publisher("odom", Odometry, queue_size=16)
                
                rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
@@ -59,8 +56,7 @@ class RoboExplorer:
                self.update_odometry(msg, current_time)
                if (current_time - self.last_time).to_nsec() > 100e6: # send every 100ms
                        self.send_odometry(msg, current_time)
-                       if self.enable_ultrasonic_laser:
-                               self.send_laser_scan(msg, current_time)
+                       self.send_range(msg, current_time)
                        self.last_time = current_time
 
        def update_odometry(self, msg, current_time):
@@ -136,25 +132,17 @@ class RoboExplorer:
                # publish the message
                self.pub_odom.publish(odom)
                
-       def send_laser_scan(self, msg, current_time):
-               # actually ultra sonic range finder
-               num_points = 60 # The base planner needs at least 30 points to work in the default config
-               opening_angle = 30*pi/180 # each side
-               scan = LaserScan()
+       def send_range(self, msg, current_time):
+               # ultra sonic range finder
+               scan = Range()
                scan.header.stamp = current_time
                scan.header.frame_id = "forward_sensor"
-               scan.angle_min = -opening_angle
-               scan.angle_max = opening_angle
-               scan.angle_increment = (2*opening_angle)/num_points
-               scan.time_increment = 0.0
-               scan.range_min = 0.0
-               scan.range_max = 4.0
-               for i in range(num_points):
-                       scan.ranges.append(msg.d1/100.0)
-               #scan.intensities.append(0.5)
-               #scan.intensities.append(1.0)
-               #scan.intensities.append(0.5)
-               self.pub_scan.publish(scan)
+               scan.radiation_type = 0
+               scan.field_of_view = 60*pi/180
+               scan.min_range = 0.0
+               scan.max_range = 4.0
+               scan.range = msg.d1/100.0
+               self.pub_sonar.publish(scan)
 
        # test with rostopic pub -1 cmd_vel geometry_msgs/Twist '[0, 0, 0]' '[0, 0, 0]'
        def cmdVelReceived(self, msg):