def load_settings_json(self, path_to_settings_file):
"""
Load the settings stored in the JSON file
"""
file = open(path_to_settings_file, 'r')
json_text = file.read().strip()
file.close()
for (device_serial, device) in self._enabled_devices.items():
# Get the active profile and load the json file which contains settings readable by the realsense
device = device.pipeline_profile.get_device()
advanced_mode = rs.rs400_advanced_mode(device)
advanced_mode.load_json(json_text)
def get_debug_device(serial_no):
ctx = rs.context()
devices = ctx.query_devices()
found_dev = False
for dev in devices:
if len(serial_no) == 0 or serial_no == dev.get_info(rs.camera_info.serial_number):
found_dev = True
break
if not found_dev:
print('No RealSense device found' + str('.' if len(serial_no) == 0 else ' with serial number: '+serial_no))
return 0
# set to advance mode:
advanced = rs.rs400_advanced_mode(dev)
if not advanced.is_enabled():
advanced.toggle_advanced_mode(True)
# print(a few basic information about the device)
print(' Device PID: ', dev.get_info(rs.camera_info.product_id))
print(' Device name: ', dev.get_info(rs.camera_info.name))
print(' Serial number: ', dev.get_info(rs.camera_info.serial_number))
print(' Firmware version: ', dev.get_info(rs.camera_info.firmware_version))
debug = rs.debug_protocol(dev)
return debug
DS5_product_ids = ["0AD1", "0AD2", "0AD3", "0AD4", "0AD5", "0AF6", "0AFE", "0AFF", "0B00", "0B01", "0B03", "0B07"]
def find_device_that_supports_advanced_mode() :
ctx = rs.context()
ds5_dev = rs.device()
devices = ctx.query_devices();
for dev in devices:
if dev.supports(rs.camera_info.product_id) and str(dev.get_info(rs.camera_info.product_id)) in DS5_product_ids:
if dev.supports(rs.camera_info.name):
print("Found device that supports advanced mode:", dev.get_info(rs.camera_info.name))
return dev
raise Exception("No device that supports advanced mode was found")
try:
dev = find_device_that_supports_advanced_mode()
advnc_mode = rs.rs400_advanced_mode(dev)
print("Advanced mode is", "enabled" if advnc_mode.is_enabled() else "disabled")
# Loop until we successfully enable advanced mode
while not advnc_mode.is_enabled():
print("Trying to enable advanced mode...")
advnc_mode.toggle_advanced_mode(True)
# At this point the device will disconnect and re-connect.
print("Sleeping for 5 seconds...")
time.sleep(5)
# The 'dev' object will become invalid and we need to initialize it again
dev = find_device_that_supports_advanced_mode()
advnc_mode = rs.rs400_advanced_mode(dev)
print("Advanced mode is", "enabled" if advnc_mode.is_enabled() else "disabled")
# Get each control's current value
def find_device_that_supports_advanced_mode():
ctx = rs.context()
ds5_dev = rs.device()
devices = ctx.query_devices()
for dev in devices:
if dev.supports(rs.camera_info.product_id) and str(
dev.get_info(
rs.camera_info.product_id)) in DS5_product_ids:
if dev.supports(rs.camera_info.name):
print("Found device that supports advanced mode:",
dev.get_info(rs.camera_info.name))
return dev
raise Exception("No device that supports advanced mode was found")
dev = find_device_that_supports_advanced_mode()
advnc_mode = rs.rs400_advanced_mode(dev)
print("Advanced mode is",
"enabled" if advnc_mode.is_enabled() else "disabled")
# Loop until we successfully enable advanced mode
while not advnc_mode.is_enabled():
print("Trying to enable advanced mode...")
advnc_mode.toggle_advanced_mode(True)
# At this point the device will disconnect and re-connect.
print("Sleeping for 5 seconds...")
time.sleep(5)
# The 'dev' object will become invalid and we need to initialize it again
dev = find_device_that_supports_advanced_mode()
advnc_mode = rs.rs400_advanced_mode(dev)
print("Advanced mode is",
"enabled" if advnc_mode.is_enabled() else "disabled")
def realsense_configure_setting(setting_file):
device = find_device_that_supports_advanced_mode()
advnc_mode = rs.rs400_advanced_mode(device)
realsense_enable_advanced_mode(advnc_mode)
realsense_load_settings_file(advnc_mode, setting_file)
json_temp['stream-fps'])
# Start streaming
profile_1 = pipeline_1.start(config_1)
profile_2 = pipeline_2.start(config_2)
with open("Settings_temp.json", \
'w') as file:
json.dump(json_temp, file, indent=2)
with open("Settings_temp.json", \
'r') as file:
json_text = file.read()
# Get the active profile and load the json file which contains settings
# readable by the realsense
advanced_mode_1 = rs.rs400_advanced_mode(profile_1.get_device())
advanced_mode_1.load_json(json_text)
advanced_mode_2 = rs.rs400_advanced_mode(profile_2.get_device())
advanced_mode_2.load_json(json_text)
# Load in File if .bag file was selected
if technique == 2:
# Configure depth and color streams...
# ...from Camera 1
pipeline_1 = rs.pipeline()
config_1 = rs.config()
# ...from Camera 2
pipeline_2 = rs.pipeline()
config_2 = rs.config()
def rs_load_json(devices, file):
with open(file, 'r') as f: ff = f.read().strip()
for (pipe, profile) in devices.values():
advanced_mode = rs.rs400_advanced_mode(profile.get_device())
advanced_mode.load_json(ff)
def __init__(self, lower=(96, 59, 53,), upper=(160, 255, 130), callback=None) -> None:
self.lower, self.upper = lower, upper
# os.system('rosnode kill image_server')
self.pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# set dep-th units, this should give us better resolution sub 5m?
device: rs.device = rs.context().query_devices()[0]
advnc_mode = rs.rs400_advanced_mode(device)
depth_table_control_group: STDepthTableControl = advnc_mode.get_depth_table()
depth_table_control_group.depthUnits = 500
advnc_mode.set_depth_table(depth_table_control_group)
# Start streaming
profile = self.pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor: DepthSensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: ", depth_scale)
align_to = rs.stream.color
self.align = rs.align(align_to)
for i in range(10):
try:
sensors = sum([dev.query_sensors() for dev in rs.context().query_devices()], [])
color_sensor = next(
sensor for sensor in sensors if sensor.get_info(rs.camera_info.name) == "RGB Camera")
break
except Exception as e:
print(e)
sleep(0.3)
print("Setting RGB camera sensor settings")
# exposure: 166.0
# white balance: 4600.0
# gain: 64.0
# auto exposure enabled: 1.0
# exposure: 166.0
# white balance: 4600.0
# gain: 64.0
color_sensor.set_option(rs.option.saturation, 60)
color_sensor.set_option(rs.option.exposure, 166)
color_sensor.set_option(rs.option.enable_auto_exposure, 0)
color_sensor.set_option(rs.option.white_balance, 4600)
color_sensor.set_option(rs.option.enable_auto_white_balance, 0)
color_sensor.set_option(rs.option.gain, 64)
align = rs.align(align_to)
# color_sensor.set_option(rs.option.enable_auto_exposure, 0)
print("auto exposure enabled: {}".format(color_sensor.get_option(rs.option.enable_auto_exposure)))
print("exposure: {}".format(color_sensor.get_option(rs.option.exposure))) # 166
print("white balance: {}".format(color_sensor.get_option(rs.option.white_balance)))
print("gain: {}".format(color_sensor.get_option(rs.option.gain))) # 64
self.average = StreamingMovingAverage(20)
self.average_raw = StreamingMovingAverage(20)
self.average_fps = StreamingMovingAverage(20)
self.average_area = StreamingMovingAverage(20)
self.color = None
self.distance = None
self.fps = 0
self.area = 0
sleep(2)
def __init__(self, scanDuration, rootDir, sharpening, configFile, fps, width, height, visualPreset, laserPower, exposure, gain) :
self.scanDuration = scanDuration
self.fps = fps
self.rootDir = rootDir
self.sharpening = sharpening
self.width = width
self.height = height
self.configFile = configFile
self.captureName = None
self.closed = False
if self.configFile :
try :
with open(self.configFile) as file:
configString = json.load(file)
configDict = configString
configString = json.dumps(configString)
self.fps = int(configDict["stream-fps"])
self.width = int(configDict["stream-width"])
self.height = int(configDict["stream-height"])
Logger.printSuccess("Parameters successfully loaded !")
except :
Logger.printError("Could not read the RealSense configuration file")
self.window = cv2.namedWindow("Capture", cv2.WINDOW_AUTOSIZE)
Logger.printInfo("Initializing scanner ...")
try :
#Initialization
self.pipeline = rs2.pipeline()
self.config = rs2.config()
#Enable streams
self.config.enable_stream(rs2.stream.depth, self.width, self.height, rs2.format.z16, self.fps)
self.config.enable_stream(rs2.stream.color, self.width, self.height, rs2.format.bgr8, self.fps)
#Start streaming
self.profile = self.pipeline.start(self.config)
except Exception as e :
Logger.printError("Unable to start the stream, gonna quit.\nException -> " + str(e))
exit()
self.device = self.profile.get_device()
self.depthSensor = self.device.first_depth_sensor()
try :
#Set parameters
if configFile :
advancedMode = rs2.rs400_advanced_mode(self.device)
print(configString)
advancedMode.load_json(json_content=configString)
else :
"""
0 -> Custom
1 -> Default
2 -> Hand
3 -> High Accuracy
4 -> High Density
5 -> Medium Density
"""
self.depthSensor.set_option(rs2.option.visual_preset, visualPreset)
self.depthSensor.set_option(rs2.option.laser_power, laserPower)
self.depthSensor.set_option(rs2.option.gain, gain)
self.depthSensor.set_option(rs2.option.exposure, exposure)
#self.depthSensor.set_option(rs2.option.max_distance, 4.0)
except Exception as e :
Logger.printError("Unable to set one parameter on the RealSense, gonna continue to run.\nException -> " + str(e))
pass
self.intrinsics = self.profile.get_stream(rs2.stream.depth).as_video_stream_profile().get_intrinsics()
#Create an align object -> aligning depth frames to color frames
self.aligner = rs2.align(rs2.stream.color)
Logger.printSuccess("Scanner successfully initialized !")
self.depthDataset = []
def test_advanced_mode():
## License: Apache 2.0. See LICENSE file in root directory.
## Copyright(c) 2017 Intel Corporation. All Rights Reserved.
#####################################################
## rs400 advanced mode tutorial ##
#####################################################
# First import the library
import time
import json
DS5_product_ids = [
"0AD1", "0AD2", "0AD3", "0AD4", "0AD5", "0AF6", "0AFE", "0AFF", "0B00",
"0B01", "0B03", "0B07"
]
def find_device_that_supports_advanced_mode():
ctx = rs.context()
ds5_dev = rs.device()
devices = ctx.query_devices()
for dev in devices:
if dev.supports(rs.camera_info.product_id) and str(
dev.get_info(
rs.camera_info.product_id)) in DS5_product_ids:
if dev.supports(rs.camera_info.name):
print("Found device that supports advanced mode:",
dev.get_info(rs.camera_info.name))
return dev
raise Exception("No device that supports advanced mode was found")
try:
dev = find_device_that_supports_advanced_mode()
advnc_mode = rs.rs400_advanced_mode(dev)
print("Advanced mode is",
"enabled" if advnc_mode.is_enabled() else "disabled")
# Loop until we successfully enable advanced mode
while not advnc_mode.is_enabled():
print("Trying to enable advanced mode...")
advnc_mode.toggle_advanced_mode(True)
# At this point the device will disconnect and re-connect.
print("Sleeping for 5 seconds...")
time.sleep(5)
# The 'dev' object will become invalid and we need to initialize it again
dev = find_device_that_supports_advanced_mode()
advnc_mode = rs.rs400_advanced_mode(dev)
print("Advanced mode is",
"enabled" if advnc_mode.is_enabled() else "disabled")
optionfile = "/home/jasy/FDexter/RealSense/garnichtsoschlechtesetting.json"
#optionfile = "/home/jasy/FDexter/RealSense/holefilletc.json"
with open(optionfile, "r") as read_file:
print('oh')
settings = json.load(read_file)
#settings = read_file
print(settings)
#json_string = str(settings).replace('u', 'bla')
if type(next(iter(settings))) != str:
settings_ = {
k.encode('utf-8'): v.encode("utf-8")
for k, v in settings.items()
}
json_string = str(settings_).replace("'", '\"')
advnc_mode.load_json(json_string)
# # Get each control's current value
#print("Depth Control: \n", advnc_mode.get_depth_control())
# print("RSM: \n", advnc_mode.get_rsm())
# print("RAU Support Vector Control: \n", advnc_mode.get_rau_support_vector_control())
# print("Color Control: \n", advnc_mode.get_color_control())
# print("RAU Thresholds Control: \n", advnc_mode.get_rau_thresholds_control())
# print("SLO Color Thresholds Control: \n", advnc_mode.get_slo_color_thresholds_control())
# print("SLO Penalty Control: \n", advnc_mode.get_slo_penalty_control())
# print("HDAD: \n", advnc_mode.get_hdad())
# print("Color Correction: \n", advnc_mode.get_color_correction())
# print("Depth Table: \n", advnc_mode.get_depth_table())
# print("Auto Exposure Control: \n", advnc_mode.get_ae_control())
# print("Census: \n", advnc_mode.get_census())
except Exception as e:
print(e)
pass
def loadConfiguration(self):
self.dev = self.cfg.get_device()
self.advnc_mode = rs.rs400_advanced_mode(self.dev)
print("Advanced mode is", "enabled" if self.advnc_mode.is_enabled() else "disabled")
self.advnc_mode.load_json(self.json_string)
def activate_rs_settings():
try:
dev = find_device_that_supports_advanced_mode()
advnc_mode = rs.rs400_advanced_mode(dev)
print("Advanced mode is",
"enabled" if advnc_mode.is_enabled() else "disabled")
# Loop until we successfully enable advanced mode
while not advnc_mode.is_enabled():
print("Trying to enable advanced mode...")
advnc_mode.toggle_advanced_mode(True)
# At this point the device will disconnect and re-connect.
print("Sleeping for 5 seconds...")
time.sleep(5)
# The 'dev' object will become invalid and we need to initialize it again
dev = find_device_that_supports_advanced_mode()
advnc_mode = rs.rs400_advanced_mode(dev)
print("Advanced mode is",
"enabled" if advnc_mode.is_enabled() else "disabled")
# Get each control's current value
print("Depth Control: '\n'", advnc_mode.get_depth_control())
print("RSM: /n", advnc_mode.get_rsm())
print("RAU Support Vector Control: /n",
advnc_mode.get_rau_support_vector_control())
print("Color Control: /n", advnc_mode.get_color_control())
print("RAU Thresholds Control: /n",
advnc_mode.get_rau_thresholds_control())
print("SLO Color Thresholds Control: /n",
advnc_mode.get_slo_color_thresholds_control())
print("SLO Penalty Control: /n", advnc_mode.get_slo_penalty_control())
print("HDAD: /n", advnc_mode.get_hdad())
print("Color Correction: /n", advnc_mode.get_color_correction())
print("Depth Table: /n", advnc_mode.get_depth_table())
print("Auto Exposure Control: /n", advnc_mode.get_ae_control())
print("Census: /n", advnc_mode.get_census())
# To get the minimum and maximum value of each control use the mode value:
query_min_values_mode = 1
query_max_values_mode = 2
current_std_depth_control_group = advnc_mode.get_depth_control()
min_std_depth_control_group = advnc_mode.get_depth_control(
query_min_values_mode)
max_std_depth_control_group = advnc_mode.get_depth_control(
query_max_values_mode)
print("Depth Control Min Values: \n ", min_std_depth_control_group)
print("Depth Control Max Values: \n ", max_std_depth_control_group)
# Set some control with a new (median) value
current_std_depth_control_group.scoreThreshA = int(
(max_std_depth_control_group.scoreThreshA -
min_std_depth_control_group.scoreThreshA) / 2)
advnc_mode.set_depth_control(current_std_depth_control_group)
print("After Setting new value, Depth Control: \n",
advnc_mode.get_depth_control())
# Serialize all controls to a Json string
serialized_string = advnc_mode.serialize_json()
print("Controls as JSON: \n", serialized_string)
# camsettings2310
with open("config.json") as file:
as_json_object = json.load(file)
# We can also load controls from a json string
# The C++ JSON parser requires double-quotes for the json object so we need
# to replace the single quote of the pythonic json to double-quotes
json_string = str(as_json_object).replace("'", '\"')
advnc_mode.load_json(json_string)
print("------CONFIG LOADED---------")
except Exception as e:
print(e)
pass
def run(self):
global sharedIsrunning, sharedIsrunningLock
global sharedVision, sharedVisionLock
# Load camera advanced settings
ctx = rs.context()
devices = ctx.query_devices()
dev = devices[0]
advnc_mode = rs.rs400_advanced_mode(dev)
while not advnc_mode.is_enabled():
print("Activating advanced mode")
advnc_mode.toggle_advanced_mode(True)
time.sleep(5)
with open('cameraSettings.json', 'r') as json_file:
as_json_object = json.load(json_file)
# For Python 2, the values in 'as_json_object' dict need to be converted from unicode object to utf-8
if type(next(iter(as_json_object))) != str:
as_json_object = {
k.encode('utf-8'): v.encode("utf-8")
for k, v in as_json_object.items()
}
# The C++ JSON parser requires double-quotes for the json object so we need
# to replace the single quote of the pythonic json to double-quotes
json_string = str(as_json_object).replace("'", '\"')
advnc_mode.load_json(json_string)
#Create a config and configure the pipeline to stream
# color and depth streams - should be kept same, otherwise projection won't work
config = rs.config()
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 15)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 15)
# Define the codec and create VideoWriter object
if self.config.isRecording:
fourcc = cv2.VideoWriter_fourcc(*'x264')
videoFPS = 15.
videoTimer = self.config.startTime
videoFrames = 0
videoColored = cv2.VideoWriter(self.config.exportName + '.avi',
fourcc, videoFPS, (1280, 720))
config.enable_record_to_file(self.config.exportName + '.bag')
exportJoints = open(self.config.exportName + "_joints.py", 'w')
exportJoints.write("times = []\n")
exportJoints.write("names = []\n")
exportJoints.write("keys = []\n")
exportKeypoints = open(self.config.exportName + "_keypoints.py",
'w')
exportKeypoints.write("times = []\n")
exportKeypoints.write("keypoints3D = []\n")
exportKeypoints.write("keypoints = []\n")
# Create a pipeline
pipeline = rs.pipeline()
# Start streaming
profile = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
# Create an align object
# rs.align allows us to perform alignment of depth frames to others frames
# The "align_to" is the stream type to which we plan to align depth frames.
align_to = rs.stream.color
align = rs.align(align_to)
# Starting OpenPose
opWrapper = op.WrapperPython()
# Custom Params (refer to include/openpose/flags.hpp for more parameters)
params = dict()
params["model_folder"] = "/home/naolaptop/Software/openpose/models"
params["net_resolution"] = "-1x144"
params["number_people_max"] = 1
params["tracking"] = 0
opWrapper.configure(params)
opWrapper.start()
# Robot - camera calibration
calibrationArm = 'LArm'
tempMarkerZ = []
tempMarkerPositions = []
tempRobotPositions = []
tempRobotTransformations = []
markerPositions = []
robotPositions = []
robotTransformations = []
closestObject = None
robotOffset = {
'ChestBoard/Button':
np.array(self.config.motion.getPosition("ChestBoard/Button")[:3]),
'CameraTop':
np.array(self.config.motion.getPosition("CameraTop")[:3])
}
nosePosition2D = [None, None]
try:
if self.config.startup:
self.config.motion.doBlocking('calibration')
self.config.motion.doBlocking('calibration0')
elif self.config.isRecording:
self.exportSetup(self.config.exportName, self.config.SCENE,
self.config.toRobRotation,
self.config.toRobTranslation)
videoFrameTimer = time.time()
sharedIsrunningLock.acquire()
while sharedIsrunning:
sharedIsrunningLock.release()
# Get frameset of color and depth
frames = pipeline.wait_for_frames()
# Align the depth frame to color frame
aligned_frames = align.process(frames)
# Get aligned frames
aligned_depth_frame = aligned_frames.get_depth_frame(
) # aligned_depth_frame
aligned_color_frame = aligned_frames.get_color_frame()
# Validate that both frames are valid
if not aligned_depth_frame or not aligned_color_frame:
sharedIsrunningLock.acquire()
continue
# Intrinsics
depth_intrin = aligned_depth_frame.profile.as_video_stream_profile(
).intrinsics
# Get depth and color values in a np array
depth_image = np.asanyarray(aligned_depth_frame.get_data())
color_image = np.asanyarray(aligned_color_frame.get_data())
# Measure processing time
timeP = dt.datetime.now()
# Update robot offset to be aligned with current frame
robotOffset = {
'ChestBoard/Button':
np.array(
self.config.motion.getPosition(
"ChestBoard/Button", legCorrection=False)[:3]),
'CameraTop':
np.array(
self.config.motion.getPosition(
"CameraTop", legCorrection=False)[:3])
}
robotOffset2D = {}
for part in robotOffset:
robotOffset2D[part] = self.torso3DToPixel(
depth_intrin, robotOffset[part],
self.config.toRobRotation,
self.config.toRobTranslation)
if self.config.startup:
# Calibration of camera position
markers = cv2.aruco.detectMarkers(
color_image,
cv2.aruco.getPredefinedDictionary(
cv2.aruco.DICT_7X7_50))
marker3DCenter = np.array([0, 0, 0])
for markerIndex in range(len(markers[0])):
marker = markers[0][markerIndex]
markerID = markers[1][markerIndex][0]
markerCenter = marker[0, :, :].sum(axis=0) * 0.25
marker3DCenter = self.pixelTo3D(
depth_intrin, depth_image, depth_scale,
markerCenter[0], markerCenter[1])
if marker3DCenter[2] > 0:
robotPosition = np.array(
self.config.motion.getPosition(calibrationArm)[:3])
tempMarkerZ.append(
(marker3DCenter[2], len(tempMarkerPositions)))
tempMarkerPositions.append(marker3DCenter)
tempRobotPositions.append(robotPosition)
tempRobotTransformations.append(
self.config.motion.getTransform(calibrationArm))
#print("tempCAM", marker3DCenter)
#print("tempROBOT", robotPosition)
if len(tempMarkerZ
) > 1: # how many samples of each move
tempIndex = sorted(
tempMarkerZ,
key=lambda s: s[0])[len(tempMarkerZ) // 2][
1] # Get index of median
markerPositions.append(
tempMarkerPositions[tempIndex])
robotTransformations.append(
tempRobotTransformations[tempIndex])
robotPositions.append(
tempRobotPositions[tempIndex])
print("CAM", markerPositions[-1])
print("ROBOT", robotPositions[-1])
tempMarkerZ = []
tempMarkerPositions = []
tempRobotPositions = []
tempRobotTransformations = []
if len(markerPositions) < 7:
self.config.motion.doBlocking(
'calibration' + str(len(markerPositions)))
else:
A = np.array(markerPositions)
B = np.array(robotPositions)
self.config.toRobRotation, self.config.toRobTranslation = rigid_transform_3D(
A, B)
print(
"Calibration done: ", "\"" + json.dumps([
self.config.toRobRotation.tolist(),
self.config.toRobTranslation.tolist()
]) + "\"")
self.config.motion.doBlocking('Stand')
self.config.startup = False
if self.config.isRecording:
self.exportSetup(self.config.exportName,
self.config.SCENE,
self.config.toRobRotation,
self.config.toRobTranslation)
videoFrameTimer = time.time()
sharedIsrunningLock.acquire()
continue
else:
visibleObjects = {}
closestObject = None
self.config.ROBOT['vision']['objects']['nose'][
'current'] = False
# Process Image
keypoints3DPrint = {}
datum = op.Datum()
imageToProcess = color_image
for part in robotOffset:
if not math.isnan(robotOffset2D[part][0]):
rob2DTuple = self.toTupleInt(robotOffset2D[part])
cv2.circle(imageToProcess, rob2DTuple, 10,
(0, 0, 0), 10)
datum.cvInputData = imageToProcess
opWrapper.emplaceAndPop([datum])
# Keypoints detection
distances = []
if datum.poseKeypoints.size > 1:
keypoints3D = self.keypointsTo3D(
depth_intrin,
depth_image,
depth_scale,
datum.poseKeypoints[0],
filtered=[
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
19, 20, 21, 22, 23, 24, 25
],
area=5)
for keypointIndex in keypoints3D.keys():
# Closest object detection
keypoint3D = keypoints3D[keypointIndex]
keypoints3DPrint[keypointIndex] = keypoints3D[
keypointIndex].tolist()
keypoint2D = datum.poseKeypoints[0][keypointIndex]
coords = np.dot(
self.config.toRobRotation,
keypoint3D) + self.config.toRobTranslation
partDistances = {}
distance = (None, float('inf'))
for part in robotOffset:
partDistances[part] = np.linalg.norm(
coords - robotOffset[part])
if partDistances[part] < distance[1]:
distance = (part, partDistances[part])
if keypoint3D[2] > 0:
visibleObjects[keypointIndex] = {
'position': keypoint2D,
'coords': coords,
'coordsOrigin': keypoint3D,
'distance': distance[1],
'distanceKey': distance[0],
'partDistances': partDistances
}
distances.append(distance[1])
if closestObject == None or distance[
1] < visibleObjects[closestObject][
'distance']:
closestObject = keypointIndex
else:
datum = datumFiller()
if len(distances) < 3 or np.var(distances, ddof=1) > 0.1:
closestObject = None
if not closestObject is None:
# Measurements
distance = visibleObjects[closestObject]['distance']
noseKeypoint = filter(
lambda x: x in visibleObjects.keys(),
[0, 16, 15, 18, 17])
if len(noseKeypoint) > 0:
noseKeypoint = noseKeypoint[0]
noseCopy = dict(visibleObjects[noseKeypoint])
noseCopy['current'] = True
self.config.ROBOT['vision']['objects'][
'nose'] = noseCopy
#z = visibleObjects[noseKeypoint]['coordsOrigin'][2]
#if abs(visibleObjects[noseKeypoint]['distance'] - visibleObjects[closestObject]['distance']) > 0.3:
# z = visibleObjects[closestObject]['coordsOrigin'][2]
#nose3D = np.array(rs.rs2_deproject_pixel_to_point(depth_intrin, [float(visibleObjects[noseKeypoint]['position'][0]), float(visibleObjects[noseKeypoint]['position'][1])], z))
#coords = np.dot(self.config.toRobRotation, nose3D) + self.config.toRobTranslation - robotOffset['CameraTop']
#if (noseDistance < 1.5 and noseKeypoint == 0) or noseDistance > 1.5:
# visibleObjects[noseKeypoint]['distance'] = noseDistance
# visibleObjects[noseKeypoint]['distanceKey'] = 'CameraTop'
processingTime = dt.datetime.now() - timeP
self.config.ROBOT['vision']['distance']['velocity'] = (
distance -
self.config.ROBOT['vision']['distance']['current']
) / processingTime.total_seconds()
self.config.ROBOT['vision']['distance'][
'current'] = distance
updated = False
for i in range(len(self.config.ZONE)):
if distance <= self.config.ZONE[i]:
self.config.ROBOT['vision']['zone'][
'velocity'] = i + 1 - self.config.ROBOT[
'vision']['zone']['current']
self.config.ROBOT['vision']['zone'][
'current'] = i + 1
updated = True
break
if not updated:
self.config.ROBOT['vision']['zone'][
'velocity'] = 4 - self.config.ROBOT['vision'][
'zone']['current']
self.config.ROBOT['vision']['zone']['current'] = 4
else:
self.config.ROBOT['vision']['zone']['velocity'] = 0
self.config.ROBOT['touch'][
'side'] = self.config.skin.whichSide(True)
if self.config.ROBOT['touch']['side'] != None:
self.config.ROBOT['vision']['zone']['current'] = 0
sharedVisionLock.acquire()
sharedVision = dict(self.config.ROBOT['vision'])
sharedVisionLock.release()
# Processing time
processingTime = dt.datetime.now() - timeP
if self.config.isDebug:
print(
str(round(
(time.time() - videoTimer) / 60., 2)) + " min " +
str(round(1 / processingTime.total_seconds(), 2)) +
" FPS") # Frame-rate
# Prepare image to be used by opencv in order to show it on the screen
# mostly used for testing/debug, we will probably need to remove this in actual scripts
# to improve performance.
if self.config.isView or self.config.isRecording:
depth_image_3d = np.dstack(
(depth_image, depth_image, depth_image
)) #depth image is 1 channel, color is 3 channels
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_image, alpha=0.03),
cv2.COLORMAP_JET)
try:
images = datum.cvOutputData #np.hstack((datum.cvOutputData, depth_colormap))
except:
images = color_image #np.hstack((color_image, depth_colormap
if self.config.isView:
cv2.namedWindow('RealSense', cv2.WINDOW_NORMAL)
if images is not None:
if self.config.ROBOT['vision']['zone'][
'current'] <= 3 and self.config.ROBOT[
'vision']['zone']['current'] > 0 and (
self.config.ROBOT['vision']['objects']
['nose']['current']
or nosePosition2D[0] != None):
if self.config.ROBOT['vision']['objects']['nose'][
'current']:
nosePosition2D = self.config.ROBOT['vision'][
'objects']['nose']['position']
nose2DTuple = self.toTupleInt(nosePosition2D)
rob2DTuple = self.toTupleInt(
robotOffset2D['CameraTop'])
cv2.arrowedLine(
images,
rob2DTuple,
nose2DTuple, (0, 255, 0),
int(10 * self.config.ROBOT['vision']['objects']
['nose']['partDistances']['CameraTop']) +
1,
tipLength=0.01 +
0.05 * self.config.ROBOT['vision']['objects']
['nose']['partDistances']['CameraTop'])
text = "|" + str(
round(
self.config.ROBOT['vision']['objects']
['nose']['partDistances']['CameraTop'],
2)) + "| m"
cv2.putText(images, text, nose2DTuple,
cv2.FONT_HERSHEY_DUPLEX, 2, (0, 0, 0),
5)
cv2.putText(images, text, nose2DTuple,
cv2.FONT_HERSHEY_DUPLEX, 2,
(255, 255, 255), 4)
nose2DTuple = (nose2DTuple[0], nose2DTuple[1] + 35)
text = str(
map(
round, self.config.ROBOT['vision']
['objects']['nose']['coords'],
len(self.config.ROBOT['vision']['objects']
['nose']['coords']) * [2]))
cv2.putText(images, text, nose2DTuple,
cv2.FONT_HERSHEY_DUPLEX, 1.1,
(0, 0, 0), 3)
cv2.putText(images, text, nose2DTuple,
cv2.FONT_HERSHEY_DUPLEX, 1.1,
(255, 255, 255), 2)
if closestObject != None:
closestPosition2D = visibleObjects[closestObject][
'position']
closest2DTuple = self.toTupleInt(closestPosition2D)
rob2DTuple = self.toTupleInt(robotOffset2D[
visibleObjects[closestObject]['distanceKey']])
cv2.circle(images, closest2DTuple, 10, (0, 0, 255),
10)
cv2.arrowedLine(images,
rob2DTuple,
closest2DTuple, (255, 0, 0),
5,
tipLength=0.05)
text = "|" + str(
round(
visibleObjects[closestObject]['distance'],
2)) + "| m"
cv2.putText(images, text, closest2DTuple,
cv2.FONT_HERSHEY_DUPLEX, 2, (0, 0, 0),
5)
cv2.putText(images, text, closest2DTuple,
cv2.FONT_HERSHEY_DUPLEX, 2,
(255, 255, 255), 4)
closest2DTuple = (closest2DTuple[0],
closest2DTuple[1] + 35)
text = str(
map(
round,
visibleObjects[closestObject]['coords'],
len(visibleObjects[closestObject]
['coords']) * [2]))
cv2.putText(images, text, closest2DTuple,
cv2.FONT_HERSHEY_DUPLEX, 1.1,
(0, 0, 0), 3)
cv2.putText(images, text, closest2DTuple,
cv2.FONT_HERSHEY_DUPLEX, 1.1,
(255, 255, 255), 2)
text = "Action: " + str(self.config.motion.lastMove())
cv2.putText(images, text, (10, 35),
cv2.FONT_HERSHEY_DUPLEX, 1.1, (0, 0, 0), 3)
cv2.putText(images, text, (10, 35),
cv2.FONT_HERSHEY_DUPLEX, 1.1,
(255, 255, 255), 2)
text = "Zone: " + str(
self.config.ROBOT['vision']['zone']['current'])
cv2.putText(images, text, (10, 75),
cv2.FONT_HERSHEY_DUPLEX, 1.1, (0, 0, 0), 3)
cv2.putText(images, text, (10, 75),
cv2.FONT_HERSHEY_DUPLEX, 1.1,
(255, 255, 255), 2)
text = "Touch: " + str(
self.config.ROBOT['touch']
['side']) # touch is no longer avalaible here
cv2.putText(images, text, (10, 115),
cv2.FONT_HERSHEY_DUPLEX, 1.1, (0, 0, 0), 3)
cv2.putText(images, text, (10, 115),
cv2.FONT_HERSHEY_DUPLEX, 1.1,
(255, 255, 255), 2)
text = dt.datetime.now().strftime("%Y.%m.%d %H:%M:%S")
cv2.putText(images, text, (10, 700),
cv2.FONT_HERSHEY_DUPLEX, 1.1, (0, 0, 0), 3)
cv2.putText(images, text, (10, 700),
cv2.FONT_HERSHEY_DUPLEX, 1.1,
(255, 255, 255), 2)
if self.config.isRecording and not self.config.startup:
timer = time.time() - videoTimer
while videoFrames < timer * videoFPS:
# Export keypoints
exportKeypoints.write("times.append(" +
str(timer) + ")\n")
exportKeypoints.write("keypoints3D.append(" +
str(keypoints3DPrint) +
")\n")
exportKeypoints.write(
"keypoints.append(" +
str(datum.poseKeypoints[0].tolist()) +
")\n")
videoColored.write(images)
videoFrames += 1
timer = time.time() - videoTimer
# Export robot angles
bodyJoints = self.config.motion.getBodyNames(
"Body")
exportJoints.write("times.append(" + str(timer) +
")\n")
exportJoints.write("names.append(" +
str(bodyJoints) + ")\n")
exportJoints.write(
"keys.append(" +
str(self.config.motion.getAngles("Body")) +
")\n")
if self.config.isView:
cv2.imshow('RealSense', images)
key = cv2.waitKey(1)
# Press esc or 'q' to close the image window
if key & 0xFF == ord('q') or key == 27:
break
sharedIsrunningLock.acquire()
finally:
if self.config.isRecording:
videoColored.release()
exportJoints.close()
exportKeypoints.close()
if self.config.isView:
cv2.destroyAllWindows()
# Stop streaming
pipeline.stop()
sharedIsrunningLock.acquire()
sharedIsrunning = False
sharedIsrunningLock.release()
print("Qutting video.")
def run(self):
try:
machien = {'id': 1, 'ip': 'http://192.168.0.10'}
response_machien = requests.put(SERVER_URL + "/myfarm/register/ip",
data=machien)
print(f" machien ip set server : {response_machien.status_code}")
# GUI 코드
while True:
response_user = requests.get(SERVER_URL + "/user/info/3")
print(f" user data : {response_user.status_code}")
if response_user.status_code == 200:
break
time.sleep(5)
# while True :
# time.sleep(1)
# print("exist check ...")
# pipeline.start()
# frames = pipeline.wait_for_frames()
# color_frame = frames.get_color_frame()
# color_image = np.asarray(color_frame.get_data())
## take the only location of mushroom pot -> 1/3 * width,1/2*height
# recent_image = color_image[100:350, 290:550]
# check_image = cv2.imread('./check.jpg')[100:350, 290:550]
# cv2.imwrite('./rec.jpg',check_image)
# cv2.imwrite('./recent.jpg',recent_image)
# hist_recent = cv2.calcHist(recent_image, [0,1], None, [180,256], [0,180,0,256])
# hist_check = cv2.calcHist(check_image, [0,1], None, [180,256], [0,180,0,256])
# number = cv2.compareHist(hist_recent, hist_check, cv2.HISTCMP_CORREL)
# print(number)
# pipeline.stop()
# if number > 0.4:
# print('Not exist')
# else:
# 배지입력 확인
# print("Exist !!")
# break
while self.isRun:
params = {'pin': PIN}
response = requests.get(SERVER_URL + '/farm/exist',
params=params)
if response.status_code == 200:
prg_id = response.text
break
else:
print("Not prg")
time.sleep(5) # 429 에러 방지
params = {'id': prg_id, 'type': 'custom'}
response = requests.get(SERVER_URL + '/farm/data', params=params)
result = json.loads(response.text)
water_num = result['water']
temp_array = result['temperature']
hum_array = result['humidity']
params_status = {'id': "1", "status": "true"}
response_status = requests.put(SERVER_URL + '/myfarm/status',
params=params_status)
print(f"machien status : {response_status.status_code}")
data_len = len(temp_array)
data = [None] * data_len
for i in range(0, data_len):
temp_array[i] = str(temp_array[i]['setting_value'])
hum_array[i] = str(hum_array[i]['setting_value'])
data[i] = R + C + temp_array[i] + S + hum_array[i]
print(f"water_num : {water_num}")
print(f"temp_array : {temp_array}")
print(f"hum_array : {hum_array}")
print(f"total_data : {data}")
# return True if data else False
# HOUR * 24 / water_num
WATER_TIME = HOUR * 24 / water_num
serial_send_len = 0
hour = 0 # 시간 초로 변환
# WATER_TIME
water_time = 10000 # 값 받아 오면 연산할 물주기 시간
# MOTER_TIME
moter_time = MOTER_TIME
picTime = 100000
now = datetime.datetime.now()
Arduino = serial.Serial(port='COM5', baudrate=9600)
print(f"data success : {data}")
seconds = 0
dt2 = None
loadTime = 0
hum = 0
temp = 0
while self.isRun:
dt1 = datetime.datetime.now()
result = dt1 - now
seconds = int(result.total_seconds()) - loadTime
print(
f"Python hour : {hour}, water_time : {water_time} moter_time : {moter_time} image : {picTime}"
)
print(f"Python seconds : {seconds}")
if Arduino.readable():
LINE = Arduino.readline()
code = str(LINE.decode().replace('\n', ''))
print(code)
hum = code[18:20]
temp = code[38:40]
# socket_data(temp, hum)
self.signal.emit(str(temp), str(hum))
if seconds - 2 <= hour <= seconds + 2:
if len(data) - 1 < serial_send_len:
response_status_prg = requests.put(
SERVER_URL + f'/farm/end?id={prg_id}')
print(
f"prg status : {response_status_prg.status_code}")
params_status = {'id': "1", "status": "false"}
response_status = requests.put(SERVER_URL +
'/myfarm/status',
params=params_status)
print(
f"machien status : {response_status.status_code}")
break
Arduino.write(data[serial_send_len].encode('utf-8'))
req_data_humi_temp = {
'prgId': prg_id,
'tempValue': temp,
'humiValue': hum
}
humi_temp_res = requests.post(SERVER_URL + "/data/add",
data=req_data_humi_temp)
print(f"Python res_temp : {humi_temp_res.status_code}")
serial_send_len += 1
hour += DAY
if seconds - 2 <= water_time <= seconds + 2:
Arduino.write(WATER_ORDER.encode('utf-8'))
dt2 = datetime.datetime.now()
while Arduino.readable():
LINE = Arduino.readline()
code = str(LINE.decode().replace('\n', ''))
print(code)
if code[0:3] == 'end':
loadTime += int((datetime.datetime.now() -
dt2).total_seconds())
break
water_time += WATER_TIME
if seconds - 2 <= moter_time - HOUR / 3 <= seconds + 2:
if pipeline_check == False:
pipeline_check = True
Arduino.write(MOTER_ORDER.encode('utf-8'))
dt2 = datetime.datetime.now()
# 3d config
device = rs.context().query_devices()[0]
advnc_mode = rs.rs400_advanced_mode(device)
depth_table_control_group = advnc_mode.get_depth_table(
)
depth_table_control_group.disparityShift = 60
depth_table_control_group.depthClampMax = 4000
depth_table_control_group.depthClampMin = 0
advnc_mode.set_depth_table(depth_table_control_group)
pipeline1 = rs.pipeline()
config1 = rs.config()
config1.enable_stream(rs.stream.depth, rs.format.z16,
30)
config1.enable_stream(rs.stream.color, rs.format.bgr8,
30)
# Start streaming
profile1 = pipeline1.start(config)
# Get stream profile and camera intrinsics
profile = pipeline1.get_active_profile()
depth_profile = rs.video_stream_profile(
profile1.get_stream(rs.stream.depth))
pc = rs.pointcloud()
decimate = rs.decimation_filter()
decimate.set_option(rs.option.filter_magnitude, 1)
file_name_3d = [0, 90, 180, 270]
file_names_3d = [
'./3dScan{}.ply'.format(i) for i in file_name_3d
]
i = 0
while Arduino.readable():
LINE = Arduino.readline()
code = str(LINE.decode().replace('\n', ''))
print(code)
print(f"i : {i}")
if code[0:3] == 'end':
loadTime += int((datetime.datetime.now() -
dt2).total_seconds())
break
take_3Dpicture(file_names_3d[i], pipeline1,
decimate, pc)
i += 0 if i >= 3 else 1
files = {'ply': open(file_names_3d[0], 'rb')}
req_data_3d = [("machineid", 1)]
res_3d = requests.post(SERVER_URL + "/upload/ply",
files=files,
data=req_data_3d)
print(f"Python res_3d : {res_3d.status_code}")
pipeline1.stop()
moter_time += MOTER_TIME
pipeline_check = False
else:
moter_time += 50
if seconds - 2 <= picTime + 30 <= seconds + 2:
if pipeline_check == False:
pipeline_check = True
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor(
)
frames = pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
# Convert images to numpy arrays
color_image = np.asanyarray(color_frame.get_data())
cv2.imwrite('./color_sensor.jpg', color_image)
files = {
'compost':
('compost.jpg', open('./color_sensor.jpg', 'rb'))
}
data1 = [('machineid', 1)]
response_image = requests.post(SERVER_URL +
"/upload/compost",
files=files,
data=data1)
print(
f"Python res_image : {response_image.status_code}")
pipeline.stop()
picTime += D2_TIME
# Turning moter
# MUSHROOM DETECTION , requeset
frames = pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
color_image = np.asanyarray(color_frame.get_data())
# detections = detection(color_image)
# boxes = get_shiitake_location(detections['detection_boxes'], detections['detection_classes'],
# 0.5)
# print(boxes)
pipeline_check = False
pipeline.stop()
# for box in boxes:
# size = get_size(box)
# res = make_data(52, box, size)
# print(res)
else:
picTime += 50
except Exception:
self.errors.emit(100)
self.isRun = False
