From a7fd072880adb621748f51b97c6dc219732da351 Mon Sep 17 00:00:00 2001
From: Paul Beuchat <pbeuchat@unimelb.edu.au>
Date: Thu, 7 Mar 2024 14:03:51 +1100
Subject: [PATCH] Fixes to minor bugs introduced by the previous commit to the
 camera and aruco nodes

---
 catkin_ws/src/asclinic_pkg/launch/aruco_detector.launch  | 2 +-
 catkin_ws/src/asclinic_pkg/launch/asclinic_system.launch | 2 +-
 catkin_ws/src/asclinic_pkg/launch/camera_capture.launch  | 6 +++---
 catkin_ws/src/asclinic_pkg/src/nodes/aruco_detector.py   | 5 +++++
 catkin_ws/src/asclinic_pkg/src/nodes/camera_capture.py   | 7 ++++++-
 5 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/catkin_ws/src/asclinic_pkg/launch/aruco_detector.launch b/catkin_ws/src/asclinic_pkg/launch/aruco_detector.launch
index 3b0b086..bda7e27 100644
--- a/catkin_ws/src/asclinic_pkg/launch/aruco_detector.launch
+++ b/catkin_ws/src/asclinic_pkg/launch/aruco_detector.launch
@@ -36,7 +36,7 @@
 			/>
 			<param
 				name   = "aruco_detector_save_image_path"
-				value  = "1"
+				value  = "/home/asc/saved_camera_images/"
 			/>
 			<param
 				name   = "aruco_detector_should_save_all_aruco_images"
diff --git a/catkin_ws/src/asclinic_pkg/launch/asclinic_system.launch b/catkin_ws/src/asclinic_pkg/launch/asclinic_system.launch
index c115c99..e7785c5 100644
--- a/catkin_ws/src/asclinic_pkg/launch/asclinic_system.launch
+++ b/catkin_ws/src/asclinic_pkg/launch/asclinic_system.launch
@@ -67,7 +67,7 @@
 			<param name = "aruco_detector_desired_camera_frame_width" value = "1920" />
 			<param name = "aruco_detector_desired_camera_fps" value = "5" />
 			<param name = "aruco_detector_marker_size" value = "0.250" />
-			<param name = "aruco_detector_save_image_path" value = "1" />
+			<param name = "aruco_detector_save_image_path" value = "/home/asc/saved_camera_images/" />
 			<param name = "aruco_detector_should_save_all_aruco_images" value = "False" />
 			<param name = "aruco_detector_should_publish_camera_images" value = "True" />
 			<param name = "aruco_detector_should_show_camera_images" value = "False" />
diff --git a/catkin_ws/src/asclinic_pkg/launch/camera_capture.launch b/catkin_ws/src/asclinic_pkg/launch/camera_capture.launch
index 467c634..6a4a13f 100644
--- a/catkin_ws/src/asclinic_pkg/launch/camera_capture.launch
+++ b/catkin_ws/src/asclinic_pkg/launch/camera_capture.launch
@@ -16,7 +16,7 @@
 			/>
 			<param
 				name   = "camera_capture_usb_camera_device_number"
-				value  = "2"
+				value  = "0"
 			/>
 			<param
 				name   = "camera_capture_desired_camera_frame_height"
@@ -40,11 +40,11 @@
 			/>
 			<param
 				name   = "camera_capture_save_image_path"
-				value  = "1"
+				value  = "/home/asc/saved_camera_images/"
 			/>
 			<param
 				name   = "camera_capture_should_save_all_chessboard_images"
-				value  = "False"
+				value  = "True"
 			/>
 			<param
 				name   = "camera_capture_should_publish_camera_images"
diff --git a/catkin_ws/src/asclinic_pkg/src/nodes/aruco_detector.py b/catkin_ws/src/asclinic_pkg/src/nodes/aruco_detector.py
index 2be57dc..e8c0684 100755
--- a/catkin_ws/src/asclinic_pkg/src/nodes/aruco_detector.py
+++ b/catkin_ws/src/asclinic_pkg/src/nodes/aruco_detector.py
@@ -349,6 +349,11 @@ class ArucoDetector:
         # Initialize a sequence number for each camera frame that is read
         self.camera_frame_sequence_number = 0
 
+        # Display command line command for publishing a
+        # request to save a camera image
+        if (self.aruco_detector_verbosity >= 1):
+            rospy.loginfo("[ARUCO DETECTOR] publish request from command line to save a single camera image using: rostopic pub --once " + node_namespace + "request_save_image std_msgs/UInt32 1")
+
         # Display the status
         if (self.aruco_detector_verbosity >= 1):
             rospy.loginfo("[ARUCO DETECTOR] Node initialisation complete")
diff --git a/catkin_ws/src/asclinic_pkg/src/nodes/camera_capture.py b/catkin_ws/src/asclinic_pkg/src/nodes/camera_capture.py
index c8344c3..152d8e5 100755
--- a/catkin_ws/src/asclinic_pkg/src/nodes/camera_capture.py
+++ b/catkin_ws/src/asclinic_pkg/src/nodes/camera_capture.py
@@ -294,6 +294,11 @@ class CameraCapture:
             self.camera_frame_height = dimensions[0]
             self.camera_frame_width  = dimensions[1]
 
+        # Display command line command for publishing a
+        # request to save a camera image
+        if (self.camera_capture_verbosity >= 1):
+            rospy.loginfo("[CAMERA CAPTURE] publish request from command line to save a single camera image using: rostopic pub --once " + node_namespace + "request_save_image std_msgs/UInt32 1")
+
         # Display the status
         if (self.camera_capture_verbosity >= 1):
             rospy.loginfo("[CAMERA CAPTURE] Node initialisation complete")
@@ -335,7 +340,7 @@ class CameraCapture:
                 #   > The second argument is the grid size of internal
                 #     corner points that the function should search for
                 #   > This function can be quite slow when a chessboard is not visible in the image
-                internal_corner_grid_size = (self.self.chessboard_size_height,self.self.chessboard_size_width)
+                internal_corner_grid_size = (self.chessboard_size_height,self.chessboard_size_width)
                 chessboard_found, chessboard_corners = cv2.findChessboardCorners(current_frame_as_gray, internal_corner_grid_size, flags=find_flags)
                 # If found, then set the save image flag to true
                 if (chessboard_found == True):
-- 
GitLab