Skip to content
Snippets Groups Projects
Commit e8821123 authored by pbeuchat's avatar pbeuchat
Browse files

Updated control policy skeleton node to take encoder direction based on the... 

Updated control policy skeleton node to take encoder direction based on the sign of the current motor duty cycle commands
parent 0b4d79aa
Branches
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 308 additions and 99 deletions
catkin_ws/src/asclinic_pkg/launch/camera_capture.launch
+ 1
− 1
View file @ e8821123
......@@ -16,7 +16,7 @@
/>
<param
name = "camera_capture_usb_camera_device_number"
value = "0"
value = "2"
/>
<param
name = "camera_capture_desired_camera_frame_height"
......
catkin_ws/src/asclinic_pkg/src/nodes/aruco_detector.py
+ 81
− 41
View file @ e8821123
......@@ -77,7 +77,7 @@ from cv_bridge import CvBridge, CvBridgeError
# DEFINE THE PARAMETERS
# > For the verbosity level of displaying info
ARUCO_DETECTOR_VERBOSITY = 1
DEFAULT_ARUCO_DETECTOR_VERBOSITY = 1
# Note: the levels of increasing verbosity are defined as:
# 0 : Info is not displayed. Warnings and errors are still displayed
# 1 : Startup info is displayed
......@@ -85,15 +85,15 @@ ARUCO_DETECTOR_VERBOSITY = 1
# > For the number of the USB camera device
# i.e., for /dev/video0, this parameter should be 0
USB_CAMERA_DEVICE_NUMBER = 0
DEFAULT_USB_CAMERA_DEVICE_NUMBER = 0
# > Properties of the camera images captured
DESIRED_CAMERA_FRAME_HEIGHT = 1080
DESIRED_CAMERA_FRAME_WIDTH = 1920
DESIRED_CAMERA_FPS = 5
DEFAULT_DESIRED_CAMERA_FRAME_HEIGHT = 1080
DEFAULT_DESIRED_CAMERA_FRAME_WIDTH = 1920
DEFAULT_DESIRED_CAMERA_FPS = 5
# > For the size of the aruco marker
MARKER_SIZE = 0.250
DEFAULT_MARKER_SIZE = 0.250
# NOTE: the unit are arbitary and the units of "tvec" match
# the units of the marker size.
......@@ -101,17 +101,17 @@ MARKER_SIZE = 0.250
# Note: ensure that this path already exists
# Note: one image is saved each time a message is received
# on the "request_save_image" topic.
SAVE_IMAGE_PATH = "/home/asc/saved_camera_images/"
DEFAULT_SAVE_IMAGE_PATH = "/home/asc/saved_camera_images/"
# > A flag for whether to save all images that contain
# an aruco marker
SHOULD_SAVE_ALL_ARUCO_IMAGES = False
DEFAULT_SHOULD_SAVE_ALL_ARUCO_IMAGES = False
# > A flag for whether to publish the images captured
SHOULD_PUBLISH_CAMERA_IMAGES = False
DEFAULT_SHOULD_PUBLISH_CAMERA_IMAGES = False
# > A flag for whether to display the images captured
SHOULD_SHOW_CAMERA_IMAGES = False
DEFAULT_SHOULD_SHOW_CAMERA_IMAGES = False
......@@ -121,34 +121,74 @@ class ArucoDetector:
# Get the parameters:
# > For the verbosity level of displaying info
ARUCO_DETECTOR_VERBOSITY = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_verbosity")
if (rospy.has_param(node_namespace + node_name + "/" + "aruco_detector_verbosity")):
self.aruco_detector_verbosity = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_verbosity")
else:
rospy.logwarn("[ARUCO DETECTOR] FAILED to get \"aruco_detector_verbosity\" parameter. Using default value instead.")
self.aruco_detector_verbosity = DEFAULT_ARUCO_DETECTOR_VERBOSITY
# > For the number of the USB camera device
USB_CAMERA_DEVICE_NUMBER = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_usb_camera_device_number")
if (rospy.has_param(node_namespace + node_name + "/" + "aruco_detector_usb_camera_device_number")):
usb_camera_device_number = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_usb_camera_device_number")
else:
rospy.logwarn("[ARUCO DETECTOR] FAILED to get \"aruco_detector_usb_camera_device_number\" parameter. Using default value instead.")
usb_camera_device_number = DEFAULT_USB_CAMERA_DEVICE_NUMBER
# > For the camera frame height
DESIRED_CAMERA_FRAME_HEIGHT = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_desired_camera_frame_height")
if (rospy.has_param(node_namespace + node_name + "/" + "aruco_detector_desired_camera_frame_height")):
desired_camera_frame_height = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_desired_camera_frame_height")
else:
rospy.logwarn("[ARUCO DETECTOR] FAILED to get \"aruco_detector_desired_camera_frame_height\" parameter. Using default value instead.")
desired_camera_frame_height = DEFAULT_DESIRED_CAMERA_FRAME_HEIGHT
# > For the camera frame width
DESIRED_CAMERA_FRAME_WIDTH = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_desired_camera_frame_width")
if (rospy.has_param(node_namespace + node_name + "/" + "aruco_detector_desired_camera_frame_width")):
desired_camera_frame_width = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_desired_camera_frame_width")
else:
rospy.logwarn("[ARUCO DETECTOR] FAILED to get \"aruco_detector_desired_camera_frame_width\" parameter. Using default value instead.")
desired_camera_frame_width = DEFAULT_DESIRED_CAMERA_FRAME_WIDTH
# > For the camera fps
DESIRED_CAMERA_FPS = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_desired_camera_fps")
if (rospy.has_param(node_namespace + node_name + "/" + "aruco_detector_desired_camera_fps")):
desired_camera_fps = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_desired_camera_fps")
else:
rospy.logwarn("[ARUCO DETECTOR] FAILED to get \"aruco_detector_desired_camera_fps\" parameter. Using default value instead.")
desired_camera_fps = DEFAULT_DESIRED_CAMERA_FPS
# > For the size of the aruco marker
MARKER_SIZE = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_marker_size")
if (rospy.has_param(node_namespace + node_name + "/" + "aruco_detector_marker_size")):
self.marker_size = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_marker_size")
else:
rospy.logwarn("[ARUCO DETECTOR] FAILED to get \"aruco_detector_marker_size\" parameter. Using default value instead.")
self.marker_size = DEFAULT_MARKER_SIZE
# > For where to save images captured by the camera
SAVE_IMAGE_PATH = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_save_image_path")
if (rospy.has_param(node_namespace + node_name + "/" + "aruco_detector_save_image_path")):
self.save_image_path = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_save_image_path")
else:
rospy.logwarn("[ARUCO DETECTOR] FAILED to get \"aruco_detector_save_image_path\" parameter. Using default value instead.")
self.save_image_path = DEFAULT_SAVE_IMAGE_PATH
# > For whether to save any images that contain an aruco marker
SHOULD_SAVE_ALL_ARUCO_IMAGES = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_should_save_all_aruco_images")
if (rospy.has_param(node_namespace + node_name + "/" + "aruco_detector_should_save_all_aruco_images")):
self.should_save_all_aruco_images = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_should_save_all_aruco_images")
else:
rospy.logwarn("[ARUCO DETECTOR] FAILED to get \"aruco_detector_should_save_all_aruco_images\" parameter. Using default value instead.")
self.should_save_all_aruco_images = DEFAULT_SHOULD_SAVE_ALL_ARUCO_IMAGES
# > For whether to publish the images captured
SHOULD_PUBLISH_CAMERA_IMAGES = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_should_publish_camera_images")
if (rospy.has_param(node_namespace + node_name + "/" + "aruco_detector_should_publish_camera_images")):
self.should_publish_camera_images = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_should_publish_camera_images")
else:
rospy.logwarn("[ARUCO DETECTOR] FAILED to get \"aruco_detector_should_publish_camera_images\" parameter. Using default value instead.")
self.should_publish_camera_images = DEFAULT_SHOULD_PUBLISH_CAMERA_IMAGES
# > For whether to display the images captured
SHOULD_SHOW_CAMERA_IMAGES = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_should_show_camera_images")
if (rospy.has_param(node_namespace + node_name + "/" + "aruco_detector_should_show_camera_images")):
self.should_show_camera_images = rospy.get_param(node_namespace + node_name + "/" + "aruco_detector_should_show_camera_images")
else:
rospy.logwarn("[ARUCO DETECTOR] FAILED to get \"aruco_detector_should_show_camera_images\" parameter. Using default value instead.")
self.should_show_camera_images = DEFAULT_SHOULD_SHOW_CAMERA_IMAGES
......@@ -172,16 +212,16 @@ class ArucoDetector:
# Specify the details for camera to capture from
# > Put the desired video capture properties into local variables
self.camera_frame_width = DESIRED_CAMERA_FRAME_WIDTH
self.camera_frame_height = DESIRED_CAMERA_FRAME_HEIGHT
self.camera_fps = DESIRED_CAMERA_FPS
self.camera_frame_width = desired_camera_frame_width
self.camera_frame_height = desired_camera_frame_height
self.camera_fps = desired_camera_fps
# > For capturing from a USB camera:
# > List the contents of /dev/video* to determine
# the number of the USB camera
# > If "v4l2-ctl" command line tool is installed then list video devices with:
# v4l2-ctl --list-devices
self.camera_setup = USB_CAMERA_DEVICE_NUMBER
self.camera_setup = usb_camera_device_number
# > For capture from a camera connected via the MIPI CSI cable connectors
# > This specifies the gstreamer pipeline for video capture
......@@ -195,7 +235,7 @@ class ArucoDetector:
# Display the properties of the camera upon initialisation
# > A list of all the properties available can be found here:
# https://docs.opencv.org/4.x/d4/d15/group__videoio__flags__base.html#gaeb8dd9c89c10a5c63c139bf7c4f5704d
if (ARUCO_DETECTOR_VERBOSITY >= 1):
if (self.aruco_detector_verbosity >= 1):
print("\n[ARUCO DETECTOR] Camera properties upon initialisation:")
print("CV_CAP_PROP_FRAME_HEIGHT : '{}'".format(self.cam.get(cv2.CAP_PROP_FRAME_HEIGHT)))
print("CV_CAP_PROP_FRAME_WIDTH : '{}'".format(self.cam.get(cv2.CAP_PROP_FRAME_WIDTH)))
......@@ -229,7 +269,7 @@ class ArucoDetector:
self.cam.set(cv2.CAP_PROP_BUFFERSIZE, 1)
# Display the properties of the camera after setting the desired values
if (ARUCO_DETECTOR_VERBOSITY >= 1):
if (self.aruco_detector_verbosity >= 1):
print("\n[ARUCO DETECTOR] Camera properties upon initialisation:")
print("CV_CAP_PROP_FRAME_HEIGHT : '{}'".format(self.cam.get(cv2.CAP_PROP_FRAME_HEIGHT)))
print("CV_CAP_PROP_FRAME_WIDTH : '{}'".format(self.cam.get(cv2.CAP_PROP_FRAME_WIDTH)))
......@@ -274,7 +314,7 @@ class ArucoDetector:
#results = cv2.aruco.detectMarkers(current_frame_gray, self.aruco_dict, parameters=self.aruco_parameters)
# Define the marker corner point in the marker frame
marker_size_half = 0.5 * MARKER_SIZE
marker_size_half = 0.5 * self.marker_size
self.single_marker_object_points = np.array([ \
[-marker_size_half, marker_size_half, 0.0], \
[ marker_size_half, marker_size_half, 0.0], \
......@@ -297,7 +337,7 @@ class ArucoDetector:
# > Get the dimensions of the frame
dimensions = current_frame.shape
# > Display the dimensions
if (ARUCO_DETECTOR_VERBOSITY >= 1):
if (self.aruco_detector_verbosity >= 1):
rospy.loginfo("[ARUCO DETECTOR] As a double check of the camera properties set, a frame captured just now has dimensions = " + str(dimensions))
# > Also check the values
if (not(dimensions[0]==self.camera_frame_height) or not(dimensions[1]==self.camera_frame_width)):
......@@ -310,7 +350,7 @@ class ArucoDetector:
self.camera_frame_sequence_number = 0
# Display the status
if (ARUCO_DETECTOR_VERBOSITY >= 1):
if (self.aruco_detector_verbosity >= 1):
rospy.loginfo("[ARUCO DETECTOR] Node initialisation complete")
# Initialise a timer for capturing the camera frames
......@@ -345,19 +385,19 @@ class ArucoDetector:
# Process any ArUco markers that were detected
if aruco_ids is not None:
# Display the number of markers found
if (ARUCO_DETECTOR_VERBOSITY >= 2):
if (self.aruco_detector_verbosity >= 2):
if (len(aruco_ids)==1):
rospy.loginfo("[ARUCO DETECTOR] Found " + "{:3}".format(len(aruco_ids)) + " marker with id = " + str(aruco_ids[:,0]))
else:
rospy.loginfo("[ARUCO DETECTOR] Found " + "{:3}".format(len(aruco_ids)) + " markers with ids = " + str(aruco_ids[:,0]))
# Set the save image flag as appropriate
if (SHOULD_SAVE_ALL_ARUCO_IMAGES):
if (self.should_save_all_aruco_images):
self.should_save_image = True
# Outline all of the markers detected found in the image
# > Only if this image will be used
if (SHOULD_PUBLISH_CAMERA_IMAGES or self.should_save_image or SHOULD_SHOW_CAMERA_IMAGES):
if (self.should_publish_camera_images or self.should_save_image or self.should_show_camera_images):
current_frame_with_marker_outlines = cv2.aruco.drawDetectedMarkers(current_frame.copy(), aruco_corners_of_all_markers, aruco_ids, borderColor=(0, 220, 0))
# Initialize the message for publishing the markers
......@@ -391,14 +431,14 @@ class ArucoDetector:
# Note: the cv2.aruco.estimatePoseSingleMarkers" function was deprecated in OpenCV version 4.7
# > The recommended alternative "cv2.solvePnP" is used above.
#this_rvec_estimate, this_tvec_estimate, _objPoints = cv2.aruco.estimatePoseSingleMarkers(corners_of_this_marker, MARKER_SIZE, self.intrinic_camera_matrix, self.intrinic_camera_distortion)
#this_rvec_estimate, this_tvec_estimate, _objPoints = cv2.aruco.estimatePoseSingleMarkers(corners_of_this_marker, self.marker_size, self.intrinic_camera_matrix, self.intrinic_camera_distortion)
#rvec = this_rvec_estimate[0]
#tvec = this_tvec_estimate[0]
# Draw the marker's axes onto the image
# > Only if this image will be used
if (SHOULD_PUBLISH_CAMERA_IMAGES or self.should_save_image or SHOULD_SHOW_CAMERA_IMAGES):
current_frame_with_marker_outlines = cv2.drawFrameAxes(current_frame_with_marker_outlines, self.intrinic_camera_matrix, self.intrinic_camera_distortion, rvec, tvec, 0.5*MARKER_SIZE)
if (self.should_publish_camera_images or self.should_save_image or self.should_show_camera_images):
current_frame_with_marker_outlines = cv2.drawFrameAxes(current_frame_with_marker_outlines, self.intrinic_camera_matrix, self.intrinic_camera_distortion, rvec, tvec, 0.5*self.marker_size)
# At this stage, the variable "rvec" and "tvec" respectively
# describe the rotation and translation of the marker frame
......@@ -420,7 +460,7 @@ class ArucoDetector:
# > y-axis points to the down when looking out of the lens along the z-axis
# Display the rvec and tvec
if (ARUCO_DETECTOR_VERBOSITY >= 2):
if (self.aruco_detector_verbosity >= 2):
rospy.loginfo("[ARUCO DETECTOR] for id = " + str(this_id) + ", tvec = [ " + str(tvec[0]) + " , " + str(tvec[1]) + " , " + str(tvec[2]) + " ]" )
#rospy.loginfo("[ARUCO DETECTOR] for id = " + str(this_id) + ", rvec = [ " + str(rvec[0]) + " , " + str(rvec[1]) + " , " + str(rvec[2]) + " ]" )
......@@ -447,7 +487,7 @@ class ArucoDetector:
current_frame_with_marker_outlines = current_frame
# Publish the camera frame
if (SHOULD_PUBLISH_CAMERA_IMAGES):
if (self.should_publish_camera_images):
#rospy.loginfo("[ARUCO DETECTOR] Now publishing camera frame")
try:
self.image_publisher.publish(self.cv_bridge.cv2_to_imgmsg(current_frame_with_marker_outlines, "bgr8"))
......@@ -459,16 +499,16 @@ class ArucoDetector:
# Increment the image counter
self.save_image_counter += 1
# Write the image to file
temp_filename = SAVE_IMAGE_PATH + "aruco_image" + str(self.save_image_counter) + ".jpg"
temp_filename = self.save_image_path + "aruco_image" + str(self.save_image_counter) + ".jpg"
cv2.imwrite(temp_filename,current_frame_with_marker_outlines)
# Display the path to where the image was saved
if (ARUCO_DETECTOR_VERBOSITY >= 2):
if (self.aruco_detector_verbosity >= 2):
rospy.loginfo("[ARUCO DETECTOR] Saved camera frame to: " + temp_filename)
# Reset the flag to false
self.should_save_image = False
# Display the camera frame if requested
if (SHOULD_SHOW_CAMERA_IMAGES):
if (self.should_show_camera_images):
#rospy.loginfo("[ARUCO DETECTOR] Now displaying camera frame")
cv2.imshow("[ARUCO DETECTOR]", current_frame_with_marker_outlines)
......@@ -480,7 +520,7 @@ class ArucoDetector:
# Respond to subscriber receiving a message
def requestSaveImageSubscriberCallback(self, msg):
if (ARUCO_DETECTOR_VERBOSITY >= 1):
if (self.aruco_detector_verbosity >= 1):
rospy.loginfo("[ARUCO DETECTOR] Request received to save the next image")
# Set the flag for saving an image to true
self.should_save_image = True
......
catkin_ws/src/asclinic_pkg/src/nodes/camera_capture.py
+ 82
− 38
View file @ e8821123
......@@ -56,7 +56,7 @@ from cv_bridge import CvBridge, CvBridgeError
# DEFINE THE PARAMETERS
# > For the verbosity level of displaying info
CAMERA_CAPTURE_VERBOSITY = 1
DEFAULT_CAMERA_CAPTURE_VERBOSITY = 1
# Note: the levels of increasing verbosity are defined as:
# 0 : Info is not displayed. Warnings and errors are still displayed
# 1 : Startup info is displayed
......@@ -64,32 +64,32 @@ CAMERA_CAPTURE_VERBOSITY = 1
# > For the number of the USB camera device
# i.e., for /dev/video0, this parameter should be 0
USB_CAMERA_DEVICE_NUMBER = 0
DEFAULT_USB_CAMERA_DEVICE_NUMBER = 0
# > Properties of the camera images captured
DESIRED_CAMERA_FRAME_HEIGHT = 1080
DESIRED_CAMERA_FRAME_WIDTH = 1920
DESIRED_CAMERA_FPS = 5
DEFAULT_DESIRED_CAMERA_FRAME_HEIGHT = 1080
DEFAULT_DESIRED_CAMERA_FRAME_WIDTH = 1920
DEFAULT_DESIRED_CAMERA_FPS = 5
# > For the size of the chessboard grid
CHESSBOARD_SIZE_HEIGHT = 9
CHESSBOARD_SIZE_WIDTH = 6
DEFAULT_CHESSBOARD_SIZE_HEIGHT = 9
DEFAULT_CHESSBOARD_SIZE_WIDTH = 6
# > For where to save images captured by the camera
# Note: ensure that this path already exists
# Note: one image is saved each time a message is received
# on the "request_save_image" topic.
SAVE_IMAGE_PATH = "/home/asc/saved_camera_images/"
DEFAULT_SAVE_IMAGE_PATH = "/home/asc/saved_camera_images/"
# > A flag for whether to save any images that contains
# a camera calibration chessboard
SHOULD_SAVE_ALL_CHESSBOARD_IMAGES = True
DEFAULT_SHOULD_SAVE_ALL_CHESSBOARD_IMAGES = True
# > A flag for whether to publish the images captured
SHOULD_PUBLISH_CAMERA_IMAGES = False
DEFAULT_SHOULD_PUBLISH_CAMERA_IMAGES = False
# > A flag for whether to display the images captured
SHOULD_SHOW_IMAGES = False
DEFAULT_SHOULD_SHOW_IMAGES = False
......@@ -99,37 +99,81 @@ class CameraCapture:
# Get the parameters:
# > For the verbosity level of displaying info
CAMERA_CAPTURE_VERBOSITY = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_verbosity")
if (rospy.has_param(node_namespace + node_name + "/" + "camera_capture_verbosity")):
self.camera_capture_verbosity = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_verbosity")
else:
rospy.logwarn("[CAMERA CAPTURE] FAILED to get \"camera_capture_verbosity\" parameter. Using default value instead.")
self.camera_capture_verbosity = DEFAULT_CAMERA_CAPTURE_VERBOSITY
# > For the number of the USB camera device
USB_CAMERA_DEVICE_NUMBER = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_usb_camera_device_number")
if (rospy.has_param(node_namespace + node_name + "/" + "camera_capture_usb_camera_device_number")):
usb_camera_device_number = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_usb_camera_device_number")
else:
rospy.logwarn("[CAMERA CAPTURE] FAILED to get \"camera_capture_usb_camera_device_number\" parameter. Using default value instead.")
usb_camera_device_number = DEFAULT_USB_CAMERA_DEVICE_NUMBER
# > For the camera frame height
DESIRED_CAMERA_FRAME_HEIGHT = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_desired_camera_frame_height")
if (rospy.has_param(node_namespace + node_name + "/" + "camera_capture_desired_camera_frame_height")):
desired_camera_frame_height = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_desired_camera_frame_height")
else:
rospy.logwarn("[CAMERA CAPTURE] FAILED to get \"camera_capture_desired_camera_frame_height\" parameter. Using default value instead.")
desired_camera_frame_height = DEFAULT_DESIRED_CAMERA_FRAME_HEIGHT
# > For the camera frame width
DESIRED_CAMERA_FRAME_WIDTH = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_desired_camera_frame_width")
if (rospy.has_param(node_namespace + node_name + "/" + "camera_capture_desired_camera_frame_width")):
desired_camera_frame_width = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_desired_camera_frame_width")
else:
rospy.logwarn("[CAMERA CAPTURE] FAILED to get \"camera_capture_desired_camera_frame_width\" parameter. Using default value instead.")
desired_camera_frame_width = DEFAULT_DESIRED_CAMERA_FRAME_WIDTH
# > For the camera fps
DESIRED_CAMERA_FPS = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_desired_camera_fps")
if (rospy.has_param(node_namespace + node_name + "/" + "camera_capture_desired_camera_fps")):
desired_camera_fps = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_desired_camera_fps")
else:
rospy.logwarn("[CAMERA CAPTURE] FAILED to get \"camera_capture_desired_camera_fps\" parameter. Using default value instead.")
desired_camera_fps = DEFAULT_DESIRED_CAMERA_FPS
# > For the height of the chessboard grid
CHESSBOARD_SIZE_HEIGHT = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_chessboard_size_height")
if (rospy.has_param(node_namespace + node_name + "/" + "camera_capture_chessboard_size_height")):
self.chessboard_size_height = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_chessboard_size_height")
else:
rospy.logwarn("[CAMERA CAPTURE] FAILED to get \"camera_capture_chessboard_size_height\" parameter. Using default value instead.")
self.chessboard_size_height = DEFAULT_CHESSBOARD_SIZE_HEIGHT
# > For the width of the chessboard grid
CHESSBOARD_SIZE_WIDTH = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_chessboard_size_width")
if (rospy.has_param(node_namespace + node_name + "/" + "camera_capture_chessboard_size_width")):
self.chessboard_size_width = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_chessboard_size_width")
else:
rospy.logwarn("[CAMERA CAPTURE] FAILED to get \"camera_capture_chessboard_size_width\" parameter. Using default value instead.")
self.chessboard_size_width = DEFAULT_CHESSBOARD_SIZE_WIDTH
# > For where to save images captured by the camera
SAVE_IMAGE_PATH = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_save_image_path")
if (rospy.has_param(node_namespace + node_name + "/" + "camera_capture_save_image_path")):
self.save_image_path = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_save_image_path")
else:
rospy.logwarn("[CAMERA CAPTURE] FAILED to get \"camera_capture_save_image_path\" parameter. Using default value instead.")
self.save_image_path = DEFAULT_SAVE_IMAGE_PATH
# > For whether to save any images that contain a chessboard
SHOULD_SAVE_ALL_CHESSBOARD_IMAGES = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_should_save_all_chessboard_images")
if (rospy.has_param(node_namespace + node_name + "/" + "camera_capture_should_save_all_chessboard_images")):
self.should_save_all_chessboard_images = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_should_save_all_chessboard_images")
else:
rospy.logwarn("[CAMERA CAPTURE] FAILED to get \"camera_capture_should_save_all_chessboard_images\" parameter. Using default value instead.")
self.should_save_all_chessboard_images = DEFAULT_SHOULD_SAVE_ALL_CHESSBOARD_IMAGES
# > For whether to publish the images captured
SHOULD_PUBLISH_CAMERA_IMAGES = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_should_publish_camera_images")
if (rospy.has_param(node_namespace + node_name + "/" + "camera_capture_should_publish_camera_images")):
self.should_publish_camera_images = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_should_publish_camera_images")
else:
rospy.logwarn("[CAMERA CAPTURE] FAILED to get \"camera_capture_should_publish_camera_images\" parameter. Using default value instead.")
self.should_publish_camera_images = DEFAULT_SHOULD_PUBLISH_CAMERA_IMAGES
# > For whether to display the images captured
SHOULD_SHOW_IMAGES = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_should_show_camera_images")
if (rospy.has_param(node_namespace + node_name + "/" + "camera_capture_should_show_camera_images")):
self.should_show_images = rospy.get_param(node_namespace + node_name + "/" + "camera_capture_should_show_camera_images")
else:
rospy.logwarn("[CAMERA CAPTURE] FAILED to get \"camera_capture_should_show_camera_images\" parameter. Using default value instead.")
self.should_show_images = DEFAULT_SHOULD_SHOW_IMAGES
......@@ -150,16 +194,16 @@ class CameraCapture:
# Specify the details for camera to capture from
# > Put the desired video capture properties into local variables
self.camera_frame_width = DESIRED_CAMERA_FRAME_WIDTH
self.camera_frame_height = DESIRED_CAMERA_FRAME_HEIGHT
self.camera_fps = DESIRED_CAMERA_FPS
self.camera_frame_width = desired_camera_frame_width
self.camera_frame_height = desired_camera_frame_height
self.camera_fps = desired_camera_fps
# > For capturing from a USB camera:
# > List the contents of /dev/video* to determine
# the number of the USB camera
# > If "v4l2-ctl" command line tool is installed then list video devices with:
# v4l2-ctl --list-devices
self.camera_setup = USB_CAMERA_DEVICE_NUMBER
self.camera_setup = usb_camera_device_number
# > For capture from a camera connected via the MIPI CSI cable connectors
# > This specifies the gstreamer pipeline for video capture
......@@ -173,7 +217,7 @@ class CameraCapture:
# Display the properties of the camera upon initialisation
# > A list of all the properties available can be found here:
# https://docs.opencv.org/4.x/d4/d15/group__videoio__flags__base.html#gaeb8dd9c89c10a5c63c139bf7c4f5704d
if (CAMERA_CAPTURE_VERBOSITY >= 1):
if (self.camera_capture_verbosity >= 1):
print("\n[CAMERA CAPTURE] Camera properties upon initialisation:")
print("CV_CAP_PROP_FRAME_HEIGHT : '{}'".format(self.cam.get(cv2.CAP_PROP_FRAME_HEIGHT)))
print("CV_CAP_PROP_FRAME_WIDTH : '{}'".format(self.cam.get(cv2.CAP_PROP_FRAME_WIDTH)))
......@@ -207,7 +251,7 @@ class CameraCapture:
self.cam.set(cv2.CAP_PROP_BUFFERSIZE, 1)
# Display the properties of the camera after setting the desired values
if (CAMERA_CAPTURE_VERBOSITY >= 1):
if (self.camera_capture_verbosity >= 1):
print("\n[CAMERA CAPTURE] Camera properties upon initialisation:")
print("CV_CAP_PROP_FRAME_HEIGHT : '{}'".format(self.cam.get(cv2.CAP_PROP_FRAME_HEIGHT)))
print("CV_CAP_PROP_FRAME_WIDTH : '{}'".format(self.cam.get(cv2.CAP_PROP_FRAME_WIDTH)))
......@@ -241,7 +285,7 @@ class CameraCapture:
# > Get the dimensions of the frame
dimensions = current_frame.shape
# > Display the dimensions
if (CAMERA_CAPTURE_VERBOSITY >= 1):
if (self.camera_capture_verbosity >= 1):
rospy.loginfo("[CAMERA CAPTURE] As a double check of the camera properties set, a frame captured just now has dimensions = " + str(dimensions))
# > Also check the values
if (not(dimensions[0]==self.camera_frame_height) or not(dimensions[1]==self.camera_frame_width)):
......@@ -251,7 +295,7 @@ class CameraCapture:
self.camera_frame_width = dimensions[1]
# Display the status
if (CAMERA_CAPTURE_VERBOSITY >= 1):
if (self.camera_capture_verbosity >= 1):
rospy.loginfo("[CAMERA CAPTURE] Node initialisation complete")
# Initialise a timer for capturing the camera frames
......@@ -275,7 +319,7 @@ class CameraCapture:
# Check if the camera frame was successfully read
if (return_flag == True):
# Save camera images for which a chessboard is detected
if (SHOULD_SAVE_ALL_CHESSBOARD_IMAGES):
if (self.should_save_all_chessboard_images):
# Check if the camera frame contains a calibration
# chessboard that can be detected by OpenCV
# > Convert the image to gray scale
......@@ -291,18 +335,18 @@ 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 = (CHESSBOARD_SIZE_HEIGHT,CHESSBOARD_SIZE_WIDTH)
internal_corner_grid_size = (self.self.chessboard_size_height,self.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):
if (CAMERA_CAPTURE_VERBOSITY >= 2):
if (self.camera_capture_verbosity >= 2):
rospy.loginfo("[CAMERA CAPTURE] Chessboard FOUND, this image will be saved")
self.should_save_image = True
#else:
# rospy.loginfo("[CAMERA CAPTURE] Chessboard NOT found")
# Publish the camera frame
if (SHOULD_PUBLISH_CAMERA_IMAGES):
if (self.should_publish_camera_images):
#rospy.loginfo("[CAMERA CAPTURE] Now publishing camera frame")
try:
self.image_publisher.publish(self.cv_bridge.cv2_to_imgmsg(current_frame, "bgr8"))
......@@ -314,16 +358,16 @@ class CameraCapture:
# Increment the image counter
self.save_image_counter += 1
# Write the image to file
temp_filename = SAVE_IMAGE_PATH + "image" + str(self.save_image_counter) + ".jpg"
temp_filename = self.save_image_path + "image" + str(self.save_image_counter) + ".jpg"
cv2.imwrite(temp_filename,current_frame)
# Display the path to where the image was saved
if (CAMERA_CAPTURE_VERBOSITY >= 2):
if (self.camera_capture_verbosity >= 2):
rospy.loginfo("[CAMERA CAPTURE] Saved camera frame to: " + temp_filename)
# Reset the flag to false
self.should_save_image = False
# Display the camera frame
if (SHOULD_SHOW_IMAGES):
if (self.should_show_images):
#rospy.loginfo("[CAMERA CAPTURE] Now displaying camera frame")
cv2.imshow("[CAMERA CAPTURE]", current_frame)
else:
......@@ -334,7 +378,7 @@ class CameraCapture:
# Respond to subscriber receiving a message
def requestSaveImageSubscriberCallback(self, msg):
if (CAMERA_CAPTURE_VERBOSITY >= 1):
if (self.camera_capture_verbosity >= 1):
rospy.loginfo("[CAMERA CAPTURE] Request received to save the next image")
# Set the flag for saving an image
self.should_save_image = True
......
catkin_ws/src/asclinic_pkg/src/nodes/control_policy_skeleton.cpp
+ 58
− 2
View file @ e8821123
......@@ -12,6 +12,9 @@
//
// DESCRIPTION:
// C++ node as a skeleton from implementing a control policy
// This node is provided to exemplify bringing together the
// encoder counts and ArUco detection measurements into one node,
// and using those measurement to set a motor duty cycle action.
//
// ----------------------------------------------------------------------------
......@@ -75,6 +78,10 @@ float m_phiW2R_estimate = 0.0;
// > For the rotation of the wheel per encoder count
float m_wheel_rotation_per_count = (2.0 * 3.14159265 / static_cast<float>(m_encoder_counts_per_wheel_revolution)) * m_robot_wheel_radius;
// > For the current direction of motor drive
float m_motor_left_drive_direction = 0.0;
float m_motor_right_drive_direction = 0.0;
// > Publisher for the motor duty cycle requests
ros::Publisher m_motor_duty_cycle_request_publisher;
......@@ -104,6 +111,33 @@ void execute_control_policy()
// Current motor duty cycle subscriber callback
void currentMotorDutyCycleSubscriberCallback(const asclinic_pkg::LeftRightFloat32& msg)
{
// Display the values from the message received
if (m_control_policy_verbosity >= 2)
ROS_INFO_STREAM("[CONTROL POLICY SKELETON] Received current motor duty cycle (left,right,seq_num) = ( " << msg.left << " , " << msg.right << " , " << msg.seq_num << " )" );
// Update the member variables to match the direction
// of the current motor duty cycle.
// > For the left wheel
if (msg.left < -0.1)
m_motor_left_drive_direction = -1.0;
else if(msg.left > 0.1)
m_motor_left_drive_direction = 1.0;
else
m_motor_left_drive_direction = 0.0;
// > For the right wheel
if (msg.right < -0.1)
m_motor_right_drive_direction = -1.0;
else if(msg.right > 0.1)
m_motor_right_drive_direction = 1.0;
else
m_motor_right_drive_direction = 0.0;
}
// Encoder counts subscriber callback
// NOTE: this skeleton lets the receiving of the encoder counts
// messages determine the frequency at which the control
......@@ -115,8 +149,8 @@ void encoderCountsSubscriberCallback(const asclinic_pkg::LeftRightInt32& msg)
ROS_INFO_STREAM("[CONTROL POLICY SKELETON] Received encoder counts (left,right,seq_num) = ( " << msg.left << " , " << msg.right << " , " << msg.seq_num << " )" );
// Compute the angular change of the wheels
float delta_theta_left = static_cast<float>(msg.left) * m_wheel_rotation_per_count;
float delta_theta_right = static_cast<float>(msg.right) * m_wheel_rotation_per_count;
float delta_theta_left = static_cast<float>(msg.left) * m_motor_left_drive_direction * m_wheel_rotation_per_count;
float delta_theta_right = static_cast<float>(msg.right) * m_motor_right_drive_direction * m_wheel_rotation_per_count;
// Compute the change in displacement (delta s) and change in rotation (delta phi) resulting from the wheel rotations
float delta_s = (delta_theta_right + delta_theta_left) * 0.5 * m_robot_wheel_radius;
......@@ -146,6 +180,25 @@ void encoderCountsSubscriberCallback(const asclinic_pkg::LeftRightInt32& msg)
// ArUco Detections subscriber callback
// Details about the ArUco detection data:
// > The properties "rvec" and "tvec" respectively
// describe the rotation and translation of the
// marker frame relative to the camera frame, i.e.:
// tvec - is a vector of length 3 expressing the
// (x,y,z)-coordinates of the marker's center
// in the coordinate frame of the camera.
// rvec - is a vector of length 3 expressing the
// rotation of the marker's frame relative to
// the frame of the camera. This vector is an
// "axis angle" representation of the rotation.
// > Hence, a vector expressed in maker-frame coordinates
// can be transformed to camera-frame coordinates as:
// - Rmat = cv2.Rodrigues(rvec)
// - [x,y,z]_{in camera frame} = tvec + Rmat * [x,y,z]_{in marker frame}
// > Note: the camera frame convention is:
// - z-axis points along the optical axis, i.e., straight out of the lens
// - x-axis points to the right when looking out of the lens along the z-axis
// - y-axis points to the down when looking out of the lens along the z-axis
void arucoDetectionsSubscriberCallback(const asclinic_pkg::FiducialMarkerArray& msg)
{
// Display the data received
......@@ -211,6 +264,9 @@ int main(int argc, char* argv[])
// > Initialise a publisher for the motor duty cycle requests
m_motor_duty_cycle_request_publisher = nh_for_group.advertise<asclinic_pkg::LeftRightFloat32>("set_motor_duty_cycle", 1, true);
// > Initialise a subscriber for the current motor duty cycle
ros::Subscriber current_motor_duty_cycle_subscriber = nh_for_group.subscribe("current_motor_duty_cycle", 1, currentMotorDutyCycleSubscriberCallback);
// > Initialise a subscriber for the encoder counts sensor measurements
ros::Subscriber encoder_counts_subscriber = nh_for_group.subscribe("encoder_counts", 10, encoderCountsSubscriberCallback);
......
catkin_ws/src/asclinic_pkg/src/nodes/control_policy_skeleton.py
+ 86
− 17
View file @ e8821123
......@@ -14,7 +14,10 @@
# |___/
#
# DESCRIPTION:
# Python node as a skeleton from implementing a control policy
# Python node as a skeleton from implementing a control policy.
# This node is provided to exemplify bringing together the
# encoder counts and ArUco detection measurements into one node,
# and using those measurement to set a motor duty cycle action.
#
# ----------------------------------------------------------------------------
......@@ -37,20 +40,20 @@ from asclinic_pkg.msg import FiducialMarkerArray
# DEFINE THE PARAMETERS
# > For the verbosity level of displaying info
CONTROL_POLICY_VERBOSITY = 1
DEFAULT_CONTROL_POLICY_VERBOSITY = 1
# Note: the levels of increasing verbosity are defined as:
# 0 : Info is not displayed. Warnings and errors are still displayed.
# 1 : Startup info is displayed.
# 2 : Info each control action and state estimate update is displayed.
# > For the wheel base of the robot [in meters]
ROBOT_WHEEL_BASE = 0.22
DEFAULT_ROBOT_WHEEL_BASE = 0.22
# > For the wheel radius of the robot [in meters]
ROBOT_WHEEL_RADIUS = 0.072
DEFAULT_ROBOT_WHEEL_RADIUS = 0.072
# > For the number of encoder counts per revolution of a wheel
ENCODER_COUNTS_PER_WHEEL_REVOLUTION = 1680
DEFAULT_ENCODER_COUNTS_PER_WHEEL_REVOLUTION = 1680
......@@ -60,16 +63,32 @@ class ControlPolicySkeleton:
# GET THE PARAMETERS VALUES:
# > For the verbosity level of displaying info
CONTROL_POLICY_VERBOSITY = rospy.get_param(node_namespace + node_name + "/" + "control_policy_verbosity")
if (rospy.has_param(node_namespace + node_name + "/" + "control_policy_verbosity")):
self.control_policy_verbosity = rospy.get_param(node_namespace + node_name + "/" + "control_policy_verbosity")
else:
rospy.logwarn("[CONTROL POLICY SKELETON] FAILED to get \"control_policy_verbosity\" parameter. Using default value instead.")
self.control_policy_verbosity = DEFAULT_CONTROL_POLICY_VERBOSITY
# > For the wheel base dimension of the robot
ROBOT_WHEEL_BASE = rospy.get_param(node_namespace + node_name + "/" + "robot_wheel_base")
if (rospy.has_param(node_namespace + node_name + "/" + "robot_wheel_base")):
self.robot_wheel_base = rospy.get_param(node_namespace + node_name + "/" + "robot_wheel_base")
else:
rospy.logwarn("[CONTROL POLICY SKELETON] FAILED to get \"robot_wheel_base\" parameter. Using default value instead.")
self.robot_wheel_base = DEFAULT_ROBOT_WHEEL_BASE
# > For the wheel radius dimension of the robot
ROBOT_WHEEL_RADIUS = rospy.get_param(node_namespace + node_name + "/" + "robot_wheel_radius")
if (rospy.has_param(node_namespace + node_name + "/" + "robot_wheel_radius")):
self.robot_wheel_radius = rospy.get_param(node_namespace + node_name + "/" + "robot_wheel_radius")
else:
rospy.logwarn("[CONTROL POLICY SKELETON] FAILED to get \"robot_wheel_radius\" parameter. Using default value instead.")
self.robot_wheel_radius = DEFAULT_ROBOT_WHEEL_RADIUS
# > For the number of encoder counts per revolution of a wheel
ENCODER_COUNTS_PER_WHEEL_REVOLUTION = rospy.get_param(node_namespace + node_name + "/" + "encoder_counts_per_wheel_revolution")
if (rospy.has_param(node_namespace + node_name + "/" + "encoder_counts_per_wheel_revolution")):
self.encoder_counts_per_wheel_revolution = rospy.get_param(node_namespace + node_name + "/" + "encoder_counts_per_wheel_revolution")
else:
rospy.logwarn("[CONTROL POLICY SKELETON] FAILED to get \"encoder_counts_per_wheel_revolution\" parameter. Using default value instead.")
self.encoder_counts_per_wheel_revolution = DEFAULT_ENCODER_COUNTS_PER_WHEEL_REVOLUTION
......@@ -77,6 +96,9 @@ class ControlPolicySkeleton:
# > Initialise a publisher for the motor duty cycle requests
self.motor_duty_cycle_request_publisher = rospy.Publisher(node_namespace+"set_motor_duty_cycle", LeftRightFloat32, queue_size=1)
# > Initialise a subscriber for the current motor duty cycle
rospy.Subscriber(node_namespace+"current_motor_duty_cycle", LeftRightFloat32, self.currentMotorDutyCycleSubscriberCallback, queue_size=1)
# > Initialise a subscriber for the encoder counts sensor measurements
rospy.Subscriber(node_namespace+"encoder_counts", LeftRightInt32, self.encoderCountsSubscriberCallback, queue_size=10)
......@@ -92,15 +114,19 @@ class ControlPolicySkeleton:
self.phiW2R_estimate = 0.0
# > Compute the rotation of the wheel per encoder count
self.wheel_rotation_per_count = (2 * np.pi / ENCODER_COUNTS_PER_WHEEL_REVOLUTION) * ROBOT_WHEEL_RADIUS
self.wheel_rotation_per_count = (2.0 * np.pi / self.encoder_counts_per_wheel_revolution) * self.robot_wheel_radius
# > Initialize a sequence number for the motor duty cycle request actions
self.motor_action_sequence_number = 1
# > For the current direction of motor drive
self.motor_left_drive_direction = 0
self.motor_right_drive_direction = 0
# Display the status
if (CONTROL_POLICY_VERBOSITY >= 1):
if (self.control_policy_verbosity >= 1):
rospy.loginfo("[CONTROL POLICY SKELETON] Node initialisation complete." )
......@@ -124,6 +150,30 @@ class ControlPolicySkeleton:
# Increment the sequence number
self.motor_action_sequence_number = self.motor_action_sequence_number + 1
# Current motor duty cycle subscriber callback
def currentMotorDutyCycleSubscriberCallback(self, msg):
# Display the data received
if (self.control_policy_verbosity >= 2):
rospy.loginfo("[CONTROL POLICY SKELETON] Received current motor duty cycle (left,right,seq_num) = ( " + "{:6.1}".format(msg.left) + " , " + "{:6.1}".format(msg.right) + " , " + msg.seq_num + " )" )
# Update the member variables to match the direction
# of the current motor duty cycle.
# > For the left wheel
if (msg.left < -0.1):
self.motor_left_drive_direction = -1.0
elif(msg.left > 0.1):
self.motor_left_drive_direction = 1.0
else:
self.motor_left_drive_direction = 0.0
# > For the right wheel
if (msg.right < -0.1):
self.motor_right_drive_direction = -1.0
elif(msg.right > 0.1):
self.motor_right_drive_direction = 1.0
else:
self.motor_right_drive_direction = 0.0
# Encoder counts subscriber callback
# NOTE: this skeleton lets the receiving of the encoder counts
......@@ -132,16 +182,16 @@ class ControlPolicySkeleton:
def encoderCountsSubscriberCallback(self, msg):
# Display the data received
if (CONTROL_POLICY_VERBOSITY >= 2):
if (self.control_policy_verbosity >= 2):
rospy.loginfo("[CONTROL POLICY SKELETON] Received encoder counts (left,right,seq_num) = ( " + "{:6}".format(msg.left) + " , " + "{:6}".format(msg.right) + " , " + msg.seq_num + " )" )
# Compute the angular change of the wheels
delta_theta_left = msg.left * self.wheel_rotation_per_count
delta_theta_right = msg.right * self.wheel_rotation_per_count
delta_theta_left = msg.left * self.motor_left_drive_direction * self.wheel_rotation_per_count
delta_theta_right = msg.right * self.motor_right_drive_direction * self.wheel_rotation_per_count
# Compute the change in displacement (delta s) and change in rotation (delta phi) resulting from the wheel rotations
delta_s = (delta_theta_right + delta_theta_left) * 0.5 * ROBOT_WHEEL_RADIUS
delta_phi = (delta_theta_right - delta_theta_left) * 0.5 * ROBOT_WHEEL_RADIUS / ROBOT_WHEEL_BASE
delta_s = (delta_theta_right + delta_theta_left) * 0.5 * self.robot_wheel_radius
delta_phi = (delta_theta_right - delta_theta_left) * 0.5 * self.robot_wheel_radius / self.robot_wheel_base
# Get the current state estimate in local variables to avoid errors/misinterpretation related to the sequence of updates
xW_current = self.xW_estimate
......@@ -166,10 +216,29 @@ class ControlPolicySkeleton:
# ArUco Detections subscriber callback
# Details about the ArUco detection data:
# > The properties "rvec" and "tvec" respectively
# describe the rotation and translation of the
# marker frame relative to the camera frame, i.e.:
# tvec - is a vector of length 3 expressing the
# (x,y,z)-coordinates of the marker's center
# in the coordinate frame of the camera.
# rvec - is a vector of length 3 expressing the
# rotation of the marker's frame relative to
# the frame of the camera. This vector is an
# "axis angle" representation of the rotation.
# > Hence, a vector expressed in maker-frame coordinates
# can be transformed to camera-frame coordinates as:
# - Rmat = cv2.Rodrigues(rvec)
# - [x,y,z]_{in camera frame} = tvec + Rmat * [x,y,z]_{in marker frame}
# > Note: the camera frame convention is:
# - z-axis points along the optical axis, i.e., straight out of the lens
# - x-axis points to the right when looking out of the lens along the z-axis
# - y-axis points to the down when looking out of the lens along the z-axis
def arucoDetectionsSubscriberCallback(self, msg):
# Display the data received
if (CONTROL_POLICY_VERBOSITY >= 2):
if (self.control_policy_verbosity >= 2):
rospy.loginfo("[CONTROL POLICY SKELETON] Received aruco detections data for " + str(msg.num_markers) + " markers." )
# Iterate through the array of detected markers
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment