update clearance value and add cost map publishing

Author: Alpaca-zip

launch/turtlebot3_vgraph_navigation.launch

@@ -8,7 +8,7 @@
   <arg name="map_file" default="$(find vgraph_planner)/maps/map.yaml"/>
   <arg name="test_folder" default="$(find vgraph_planner)/test"/>
   <arg name="down_scale_factor" default="0.3"/>
-  <arg name="clearance" default="0.2"/>
+  <arg name="clearance" default="0.1"/>
   <arg name="open_rviz" default="true"/>
 
   <include file="$(find vgraph_planner)/launch/include/turtlebot3_world.launch">

rviz/turtlebot3_navigation.rviz

@@ -195,6 +195,16 @@ Visualization Manager:
       Topic: /path
       Unreliable: false
       Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Costmap
+      Topic: /cost_map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

scripts/vgraph_planner.py

@@ -21,6 +21,7 @@
 import mip
 import rospy
 import yaml
+from nav_msgs.msg import OccupancyGrid
 from nav_msgs.msg import Path
 from geometry_msgs.msg import PoseStamped
 from geometry_msgs.msg import PoseWithCovarianceStamped
@@ -37,6 +38,7 @@ def __init__(self):
         self.start_point = None
         self.end_point = None
 
+        self.costmap_pub = rospy.Publisher("/cost_map", OccupancyGrid, queue_size=10)
         self.path_pub = rospy.Publisher("/path", Path, queue_size=10)
         rospy.Subscriber(
             "/initialpose", PoseWithCovarianceStamped, self.initial_pose_callback
@@ -53,6 +55,8 @@ def __init__(self):
             self.up_scaled_image,
         ) = self.change_image_resolution(self.original_image, self.down_scale_factor)
 
+        self.publish_initial_cost_map()
+
     def __del__(self):
         pass
 
@@ -95,16 +99,18 @@ def pose_to_pixel(self, pose):
     def pixel_to_pose(self, pixel):
         origin_x = self.origin[0]
         origin_y = self.origin[1]
-        pose_x = origin_x + pixel[0] * self.resolution
-        pose_y = origin_y + (self.original_image.height - pixel[1]) * self.resolution
+        pose_x = origin_x + (pixel[0] + 0.5) * self.resolution
+        pose_y = (
+            origin_y + (self.original_image.height - pixel[1] - 0.5) * self.resolution
+        )
 
         return (pose_x, pose_y)
 
     def make_plan(self, start, goal):
         print("\033[92m[Make Plan] Process started.\033[0m")
         corners = []
         corners.append(start)
-        self.find_black_pixel_corners(self.up_scaled_image, corners)
+        corners = self.find_black_pixel_corners(self.up_scaled_image, corners)
         corners.append(goal)
 
         valid_edges = self.get_valid_edges(self.up_scaled_image, corners)
@@ -116,6 +122,8 @@ def make_plan(self, start, goal):
 
             self.publish_path(shortest_path_edges)
 
+            self.publish_cost_map(self.up_scaled_image, shortest_path_edges)
+
             path_graph_image = self.draw_path_with_markers(
                 self.up_scaled_image, valid_edges
             )
@@ -181,6 +189,48 @@ def change_image_resolution(self, image, factor):
 
         return enlarged_image, down_scaled_image, up_scaled_image
 
+    def publish_initial_cost_map(self):
+        while True:
+            if (
+                self.up_scaled_image is not None
+                and self.costmap_pub.get_num_connections() > 0
+            ):
+                self.publish_cost_map(self.up_scaled_image)
+                break
+            rospy.sleep(5.0)
+
+    def publish_cost_map(self, original_image, edges=None):
+        rgb_image = original_image.convert("RGB")
+        draw = ImageDraw.Draw(rgb_image)
+
+        if edges is not None:
+            for start, end in edges:
+                draw.line([start, end], fill=(128, 128, 128), width=1)
+
+        cost_map_msg = OccupancyGrid()
+        cost_map_msg.header.frame_id = "map"
+        cost_map_msg.header.stamp = rospy.Time.now()
+        cost_map_msg.info.resolution = self.resolution
+        cost_map_msg.info.width = rgb_image.width
+        cost_map_msg.info.height = rgb_image.height
+        cost_map_msg.info.origin.position.x = self.origin[0]
+        cost_map_msg.info.origin.position.y = self.origin[1]
+        cost_map_msg.info.origin.orientation.w = 1.0
+        cost_map_msg.data = []
+
+        for y in reversed(range(rgb_image.height)):
+            for x in range(rgb_image.width):
+                original_pixel = original_image.getpixel((x, y))
+                rgb_pixel = rgb_image.getpixel((x, y))
+                if original_pixel == 0 and rgb_pixel == (128, 128, 128):
+                    cost_map_msg.data.append(50)
+                elif original_pixel == 0:
+                    cost_map_msg.data.append(100)
+                else:
+                    cost_map_msg.data.append(0)
+
+        self.costmap_pub.publish(cost_map_msg)
+
     def find_black_pixels(self, image):
         black_pixels = []
 
@@ -193,7 +243,8 @@ def find_black_pixels(self, image):
 
     def enlarge_black_pixels(self, image, black_pixels, width):
         new_image = image.copy()
-        pixel_width = round(width / self.resolution)
+        pixel_width = math.ceil(width / self.resolution)
+        pixel_width = pixel_width + 1
         new_black_pixels = set(black_pixels)
 
         for x, y in black_pixels:
@@ -232,6 +283,8 @@ def find_black_pixel_corners(self, image, corners):
                     if black_neighbors == 3:
                         corners.append((x, y))
 
+        return corners
+
     def get_valid_edges(self, image, corners):
         valid_edges = []