class HAL:
IMG_WIDTH = 320
IMG_HEIGHT = 240
def __init__(self):
rospy.init_node("HAL")
# Shared memory variables
self.shared_image = SharedImage("halimage")
self.shared_v = SharedValue("velocity")
self.shared_w = SharedValue("angular")
# ROS Topics
self.camera = ListenerCamera("/F1ROS/cameraL/image_raw")
self.motors = PublisherMotors("/F1ROS/cmd_vel", 4, 0.3)
# Update thread
self.thread = ThreadHAL(self.update_hal)
# Function to start the update thread
def start_thread(self):
self.thread.start()
# Get Image from ROS Driver Camera
def getImage(self):
image = self.camera.getImage().data
self.shared_image.add(image)
# Set the velocity
def setV(self):
velocity = self.shared_v.get()
self.motors.sendV(velocity)
# Get the velocity
def getV(self):
velocity = self.shared_v.get()
return velocity
# Get the angular velocity
def getW(self):
angular = self.shared_w.get()
return angular
# Set the angular velocity
def setW(self):
angular = self.shared_w.get()
self.motors.sendW(angular)
def update_hal(self):
self.getImage()
self.setV()
self.setW()
# Destructor function to close all fds
def __del__(self):
self.shared_image.close()
self.shared_v.close()
self.shared_w.close()
print("Receiving " + "Stop.CameraL" + " CameraData from ROS messages")
topicCameraL = cfg.getProperty("Stop.CameraL" + ".Topic")
cameraL = ListenerCamera(topicCameraL)
# cameraL = ListenerCamera("/opel/cameraL/image_raw")
# -------- C A M E R A R I G H T ------------------------------------------
print("Receiving " + "Stop.CameraR" + " CameraData from ROS messages")
topicCameraR = cfg.getProperty("Stop.CameraR" + ".Topic")
cameraR = ListenerCamera(topicCameraR)
# cameraR = ListenerCamera("/opel/cameraR/image_raw")
print("Starting movement of dummy cars")
motorsDummy1 = PublisherMotors("/dummy1/cmd_vel", 4, 0.3)
motorsDummy2 = PublisherMotors("/dummy2/cmd_vel", 4, 0.3)
motorsDummy1.sendV(2.5)
motorsDummy2.sendV(3)
algorithm = MyAlgorithm(pose3d, cameraC, cameraL, cameraR, motors)
app = QApplication(sys.argv)
myGUI = MainWindow()
myGUI.setMotors(motors)
myGUI.setCameraC(cameraC)
myGUI.setCameraL(cameraL)
myGUI.setCameraR(cameraR)
myGUI.setPose3D(pose3d)
myGUI.setAlgorithm(algorithm)
myGUI.show()
t2 = ThreadGUI(myGUI)
class HAL:
IMG_WIDTH = 320
IMG_HEIGHT = 240
def __init__(self):
rospy.init_node("HAL")
# Shared memory variables
self.image = None
self.v = None
self.w = None
self.p3d = None
if os.getcwd() == "/":
f = open("/RoboticsAcademy/exercises/car_junction/web-template/stop_conf.yml", "r")
else:
f = open("stop_conf.yml", "r")
cfg = yaml.load(f)
ymlNode = cfg['Stop']
#node = rospy.init_node(ymlNode["NodeName"], anonymous=True)
# ------------ M O T O R S ----------------------------------
print("Publishing "+ "Stop.Motors" + " with ROS messages")
topicM = cfg['Stop']["Motors"]["Topic"]
maxW = cfg['Stop']["Motors"]["maxW"]
if not maxW:
maxW = 0.5
print ("Stop.Motors"+".maxW not provided, the default value is used: "+ repr(maxW))
maxV = cfg['Stop']["Motors"]["maxV"]
if not maxV:
maxV = 5
print ("Stop.Motors"+".maxV not provided, the default value is used: "+ repr(maxV))
self.motors = PublisherMotors(topicM, maxV, maxW)
# ----------------- P O S E 3 D -------------------------------------
print("Receiving " + "Stop.Pose3D" + " from ROS messages")
topicP = cfg['Stop']["Pose3D"]["Topic"]
self.pose3d = ListenerPose3d(topicP)
# -------- C A M E R A C E N T R A L --------------------------------------
print("Receiving " + "Stop.CameraC" + " CameraData from ROS messages")
topicCameraC = cfg['Stop']["CameraC"]["Topic"]
self.cameraC = ListenerCamera(topicCameraC)
# -------- C A M E R A L E F T --------------------------------------------
print("Receiving " + "Stop.CameraL" + " CameraData from ROS messages")
topicCameraL = cfg['Stop']["CameraL"]["Topic"]
self.cameraL = ListenerCamera(topicCameraL)
# -------- C A M E R A R I G H T ------------------------------------------
print("Receiving " + "Stop.CameraR" + " CameraData from ROS messages")
topicCameraR = cfg['Stop']["CameraR"]["Topic"]
self.cameraR = ListenerCamera(topicCameraR)
self.template = cv2.imread('assets/img/template.png',0)
# Get Image from ROS Driver Camera
def getImage(self, lr):
if (lr == 'left'):
image = self.cameraL.getImage().data
elif (lr == 'right'):
image = self.cameraR.getImage().data
elif (lr == 'center'):
image = self.cameraC.getImage().data
else:
print("Invalid camera")
return image
# Set the velocity
def setV(self, velocity):
self.v = velocity
self.motors.sendV(velocity)
# Get the velocity
def getV(self):
velocity = self.v
return velocity
# Get the angular velocity
def getW(self):
angular = self.w
return angular
# Set the angular velocity
def setW(self, angular):
self.w = angular
self.motors.sendW(angular)
def getPose3D(self):
return self.pose3d
def setPose3D(self, pose3d):
self.pose3d = pose3d
def getTemplate(self):
return self.template
def getYaw(self):
return self.pose3d.data.yaw