fixed blob detection for racecar

racecar_behaviors/racecar_behaviors/blob_detector.py

@@ -104,31 +104,34 @@ def reset_callback(self, request, response):
         return response
 
     def depth_callback(self, depth_msg):
+        self.get_logger().info('K')
         self.depth = depth_msg
-        self.get_logger().info(f"Depth encoding: {depth_msg.encoding}")
+        # self.get_logger().info(f"Depth encoding: {depth_msg.encoding}")
 
     def info_callback(self, info_msg):
         self.info = info_msg
-        self.get_logger().info('K')
 
     def image_callback(self, image):
         depth = self.depth
         info = self.info
+        cv_image = np.zeros((1,1))
+        cv_depth = np.zeros((1,1))
 
         # self.get_logger().info(f"Image encoding: {image.encoding}")
 
         if self.get_clock().now().to_msg().sec - self.wait_time < 5:
             self.twist_pub.publish(Twist())
             return
         try:
-            cv_image = self.bridge.imgmsg_to_cv2(image, image.encoding)
+            cv_image = self.bridge.imgmsg_to_cv2(image, "bgr8")
         except CvBridgeError as e:
-            print(e)
+             self.get_logger().info(str(e))
 
         try:
-            cv_depth = self.bridge.imgmsg_to_cv2(depth, depth.encoding)
+            cv_depth = self.bridge.imgmsg_to_cv2(depth, "32FC1")
+            self.get_logger().info(str(depth))
         except Exception as e:
-            print(e)
+            pass
 
         hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
 
@@ -138,7 +141,7 @@ def image_callback(self, image):
         closestObject = [0,0,0]
         if len(keypoints) > 0:
 
-            self.get_logger().info("detected")
+            # self.get_logger().info("detected")
 
             cv_image = cv2.drawKeypoints(cv_image, keypoints, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
 
@@ -150,7 +153,6 @@ def image_callback(self, image):
                 # self.get_logger().info(str(cv_image.shape[1]))
 
                 if self.info.k[0] > 0 and keypoints[i].pt[0] >= self.border and keypoints[i].pt[0] < cv_image.shape[1]-self.border:
-                    self.get_logger().info('ici')
                     pts_uv = np.array([[[keypoints[i].pt[0], keypoints[i].pt[1]]]], dtype=np.float32)
                     info_K = np.array(self.info.k).reshape([3, 3])
                     info_D = np.array(self.info.d)
@@ -161,7 +163,7 @@ def image_callback(self, image):
                     y = pts_uv[0][0][1]
 
                     out_msg = str(i+1) + ',' + str(len(keypoints)) + ',' + str(keypoints[i].pt[0]) + ',' + str(keypoints[i].pt[1]) + ',' + str(x) + ',' + str(y) + ',' + str(angle*180/np.pi)
-                    self.get_logger().info(out_msg)
+                    # self.get_logger().info(out_msg)
 
                     # Get depth.
                     u = int(x * self.info.p[0] + self.info.p[2])
@@ -235,12 +237,11 @@ def image_callback(self, image):
 
 
         # debugging topic
-        # if self.image_pub.get_num_connections()>0:
-        #     cv_image = cv2.bitwise_and(cv_image, cv_image, mask=mask)
-        #     try:
-        #         self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
-        #     except CvBridgeError as e:
-        #         print(e)
+        cv_image = cv2.bitwise_and(cv_image, cv_image, mask=mask)
+        try:
+            self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
+        except CvBridgeError as e:
+            print(e)
 
 
 def main(args=None):

racecar_behaviors/racecar_behaviors/laserscan_to_pointcloud.py

@@ -12,11 +12,7 @@ def __init__(self):
         super().__init__('laserscan_to_pointcloud')
         self.lp = lg.LaserProjection()
         self.pc_pub = self.create_publisher(PointCloud2, 'converted_pc', 1)
-        self.subscription = self.create_subscription(
-            LaserScan,
-            '/scan',
-            self.scan_callback,
-            1)
+        self.subscription = self.create_subscription( LaserScan, '/scan', self.scan_callback, 1)
 
     def scan_callback(self, msg):
         pc2_msg = self.lp.projectLaser(msg)

racecar_bringup/launch/bringup.launch.py

@@ -53,12 +53,15 @@ def generate_launch_description():
                 'inverted': False,
                 'angle_compensate': True,
             }],
+            remappings=[('/scan', '/racecar/scan')],
         ),
 
         Node(
-            package='racecar_web_interface',
-            executable='ros2_publisher_camera',
+            package='v4l2_camera',
+            executable='v4l2_camera_node',
             name='camera',
+            remappings=[('image_raw', 'racecar/camera'),
+                        ('camera_info', 'racecar/camera_info')],
         ),
 
     ])