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()
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)
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)
def __init__(self): rospy.init_node("HAL") self.image = None self.camera = ListenerCamera("/F1ROS/cameraL/image_raw") self.motors = PublisherMotors("/F1ROS/cmd_vel", 4, 0.3) self.camera_lock = threading.Lock()
def __init__(self): rospy.init_node("HAL") self.motors = PublisherMotors("/roombaROS/cmd_vel", 4, 0.3) self.pose3d = ListenerPose3d("/roombaROS/odom") self.laser = ListenerLaser("/roombaROS/laser/scan") self.bumper = ListenerBumper("/roombaROS/events/bumper") self.camera_lock = threading.Lock()
def __init__(self): rospy.init_node("HAL") self.image = None self.camera = ListenerCamera("/F1ROS/cameraL/image_raw") self.motors = PublisherMotors("/F1ROS/cmd_vel", 4, 0.3) self.pose3d = ListenerPose3d("/F1ROS/odom") self.laser = ListenerLaser("/F1ROS/laser/scan")
def __init__(self): self.config = Config() self.motors = PublisherMotors(self.config.topic_motors, self.config.max_velV, self.config.max_velW) self.pose3d = ListenerPose3d(self.config.topic_pose) self.sonar_0 = ListenerSonar(self.config.topic_sonar_0) self.sonar_1 = ListenerSonar(self.config.topic_sonar_1) self.sonar_2 = ListenerSonar(self.config.topic_sonar_2) self.sonar_3 = ListenerSonar(self.config.topic_sonar_3) self.sonar_4 = ListenerSonar(self.config.topic_sonar_4) self.sonar_5 = ListenerSonar(self.config.topic_sonar_5) self.sonar_6 = ListenerSonar(self.config.topic_sonar_6) self.sonar_7 = ListenerSonar(self.config.topic_sonar_7) self.laser = ListenerLaser(self.config.topic_laser)
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
print("Publishing " + "Stop.Motors" + " with ROS messages") topicM = cfg.getProperty("Stop.Motors" + ".Topic") maxW = cfg.getPropertyWithDefault("Stop.Motors" + ".maxW", 0.5) if not maxW: maxW = 0.5 print("Stop.Motors" + ".maxW not provided, the default value is used: " + repr(maxW)) maxV = cfg.getPropertyWithDefault("Stop.Motors" + ".maxV", 5) if not maxV: maxV = 5 print("Stop.Motors" + ".maxV not provided, the default value is used: " + repr(maxV)) # motors = PublisherMotors("/opel/cmd_vel", maxV, maxW) motors = PublisherMotors(topicM, maxV, maxW) # ----------------- P O S E 3 D ------------------------------------- print("Receiving " + "Stop.Pose3D" + " from ROS messages") topicP = cfg.getProperty("Stop.Pose3D" + ".Topic") pose3d = ListenerPose3d(topicP) # pose3d = ListenerPose3d("/opel/odom") # -------- C A M E R A C E N T R A L -------------------------------------- print("Receiving " + "Stop.CameraC" + " CameraData from ROS messages") topicCameraC = cfg.getProperty("Stop.CameraC" + ".Topic") cameraC = ListenerCamera(topicCameraC) # cameraC = ListenerCamera("/opel/cameraC/image_raw") # -------- C A M E R A L E F T -------------------------------------------- print("Receiving " + "Stop.CameraL" + " CameraData from ROS messages")
from gui.threadGUI import ThreadGUI from PyQt5.QtWidgets import QApplication from base import MyAlgorithm from interfaces.infrared import ListenerInfrared from interfaces.motors import PublisherMotors from interfaces.clock import ListenerClock # Execute the application if __name__ == "__main__": rospy.init_node("ObstacleAvoidanceER") clock = [ListenerClock("ga1_clock")] infrared = [ListenerInfrared("ga1/roombaIR/sensor/infrared")] motors = [PublisherMotors("ga1/roombaIR/cmd_vel", 10, 10, clock[0])] algorithm = MyAlgorithm(infrared, motors) app = QApplication(sys.argv) myGUI = TestWindow() myGUI.setAlgorithm(algorithm) myGUI.show() t2 = ThreadGUI(myGUI) t2.daemon = True t2.start() sys.exit(app.exec_())
from gui.threadGUI import ThreadGUI from MyAlgorithm import MyAlgorithm import rospy import threading from jderobotTypes import LaserData, CMDVel, Pose3d, SonarData from gui.threadPublisher import ThreadPublisher sys.path.append('/home/vladkrav/TFM/AmigoBot/web-template') from interfaces.motors import PublisherMotors from interfaces.pose3d import ListenerPose3d from interfaces.laser import ListenerLaser from interfaces.sonar import ListenerSonar if __name__ == "__main__": motors = PublisherMotors("/robot0/cmd_vel", 0.2, 0.2) pose3d = ListenerPose3d("/robot0/odom") laser = ListenerLaser("/robot0/laser_1") sonar_0 = ListenerSonar("/robot0/sonar_0") sonar_1 = ListenerSonar("/robot0/sonar_1") sonar_2 = ListenerSonar("/robot0/sonar_2") sonar_3 = ListenerSonar("/robot0/sonar_3") sonar_4 = ListenerSonar("/robot0/sonar_4") sonar_5 = ListenerSonar("/robot0/sonar_5") sonar_6 = ListenerSonar("/robot0/sonar_6") sonar_7 = ListenerSonar("/robot0/sonar_7") sonar = [sonar_0, sonar_1, sonar_2, sonar_3, sonar_4, sonar_5, sonar_6, sonar_7] #Run your Algorithm with your solution algorithm=MyAlgorithm(pose3d, motors, laser, sonar)
# ------------ M O T O R S ---------------------------------- print("Publishing "+ "Stop.Motors" + " with ROS messages") topicM = cfg.getProperty("Stop.Motors"+".Topic") maxW = cfg.getPropertyWithDefault("Stop.Motors"+".maxW", 0.5) if not maxW: maxW = 0.5 print ("Stop.Motors"+".maxW not provided, the default value is used: "+ repr(maxW)) maxV = cfg.getPropertyWithDefault("Stop.Motors"+".maxV", 5) if not maxV: maxV = 5 print ("Stop.Motors"+".maxV not provided, the default value is used: "+ repr(maxV)) # motors = PublisherMotors("/opel/cmd_vel", maxV, maxW) motors = PublisherMotors(topicM, maxV, maxW) # ----------------- P O S E 3 D ------------------------------------- print("Receiving " + "Stop.Pose3D" + " from ROS messages") topicP = cfg.getProperty("Stop.Pose3D"+".Topic") pose3d = ListenerPose3d(topicP) # pose3d = ListenerPose3d("/opel/odom") # -------- C A M E R A C E N T R A L -------------------------------------- print("Receiving " + "Stop.CameraC" + " CameraData from ROS messages") topicCameraC = cfg.getProperty("Stop.CameraC"+".Topic") cameraC = ListenerCamera(topicCameraC) # cameraC = ListenerCamera("/opel/cameraC/image_raw")
sys.argv.remove(arg) if __name__ == '__main__': if len(sys.argv) < 2: print('ERROR: python2 globalNavigation.py [MAP CONFIG file] [YAML CONFIG file]') sys.exit(-1) app = QApplication(sys.argv) frame = MainWindow() grid = Grid(frame) removeMapFromArgs() motors = PublisherMotors("/taxi_holo/cmd_vel", 4, 0.3) pose = ListenerPose3d("/taxi_holo/odom") vel = Velocity(0, 0, motors.getMaxV(), motors.getMaxW()) frame.setVelocity(vel) sensor = Sensor(grid, pose, True) sensor.setGetPathSignal(frame.getPathSig) frame.setGrid(grid) frame.setSensor(sensor) algorithm = MyAlgorithm(grid, sensor, vel) frame.setAlgorithm(algorithm) frame.show() t1 = ThreadMotors(motors, vel) t1.daemon = True
from interfaces.camera import ListenerCamera from interfaces.pose3d import ListenerPose3d from interfaces.laser import ListenerLaser from interfaces.motors import PublisherMotors if __name__ == "__main__": cfg = config.load(sys.argv[1]) #cam_path = cfg.getProperty("ObstacleAvoidance.CameraPath") mot_path = cfg.getProperty("ObstacleAvoidance.MotorsPath") odo_path = cfg.getProperty("ObstacleAvoidance.Pose3DPath") las_path = cfg.getProperty("ObstacleAvoidance.LaserPath") #camera = ListenerCamera(cam_path) motors = PublisherMotors(mot_path, 4, 0.3) pose3d = ListenerPose3d(odo_path) laser = ListenerLaser(las_path) algorithm=MyAlgorithm(pose3d, laser, motors) app = QApplication(sys.argv) myGUI = MainWindow() #myGUI.setCamera(camera) myGUI.setMotors(motors) myGUI.setPose3D(pose3d) myGUI.setLaser(laser) myGUI.setAlgorithm(algorithm) myGUI.show() t2 = ThreadGUI(myGUI)
robot = cfg.getProperty("FollowLineTurtlebot.Robot") maxv = cfg.getPropertyWithDefault("FollowLineTurtlebot.MaxV", 2) maxw = cfg.getPropertyWithDefault("FollowLineTurtlebot.MaxV", 0.3) if (robot == "simkobuki"): cam_path = cfg.getProperty("FollowLineTurtlebot.SimCameraPath") mot_path = cfg.getProperty("FollowLineTurtlebot.SimMotorsPath") else: # realkobuki cam_path = cfg.getProperty("FollowLineTurtlebot.RealCameraPath") mot_path = cfg.getProperty("FollowLineTurtlebot.RealMotorsPath") camera = ListenerCamera(cam_path) motors = PublisherMotors(mot_path, maxv, maxw) algorithm=MyAlgorithm(camera, motors) app = QApplication(sys.argv) myGUI = MainWindow() myGUI.setCamera(camera) myGUI.setMotors(motors) myGUI.setAlgorithm(algorithm) myGUI.show() t2 = ThreadGUI(myGUI) t2.daemon=True t2.start() sys.exit(app.exec_())
if __name__ == '__main__': if len(sys.argv) < 2: print( 'ERROR: python2 globalNavigation.py [MAP CONFIG file] [YAML CONFIG file]' ) sys.exit(-1) app = QApplication(sys.argv) frame = MainWindow() grid = Grid(frame) removeMapFromArgs() motors = PublisherMotors("/amazon_warehouse_robot/cmd_vel", 0.5, 0.01) pose = ListenerPose3d("/amazon_warehouse_robot/odom") vel = Velocity(0, 0, motors.getMaxV(), motors.getMaxW()) frame.setVelocity(vel) sensor = Sensor(grid, pose, True) sensor.setGetPathSignal(frame.getPathSig) frame.setGrid(grid) frame.setSensor(sensor) algorithm = MyAlgorithm(grid, sensor, vel) frame.setAlgorithm(algorithm) frame.show() t1 = ThreadMotors(motors, vel) t1.daemon = True
def __init__(self): rospy.init_node("HAL") self.pose3d = ListenerPose3d("/taxi_holo/odom") self.motors = PublisherMotors("/taxi_holo/cmd_vel", 4, 0.3) self.camera_lock = threading.Lock()
import sys, os from PyQt5.QtWidgets import QApplication from gui.GUI import MainWindow from gui.threadGUI import ThreadGUI from MyAlgorithm import MyAlgorithm from interfaces.camera import ListenerCamera from interfaces.pose3d import ListenerPose3d from interfaces.laser import ListenerLaser from interfaces.motors import PublisherMotors if __name__ == "__main__": camera = ListenerCamera("/F1ROS/cameraL/image_raw") motors = PublisherMotors("/F1ROS/cmd_vel", 3, 0.5) pose3d = ListenerPose3d("/F1ROS/odom") laser = ListenerLaser("/F1ROS/laser/scan") algorithm = MyAlgorithm(pose3d, laser, motors) app = QApplication(sys.argv) myGUI = MainWindow() myGUI.setCamera(camera) myGUI.setMotors(motors) myGUI.setPose3D(pose3d) myGUI.setLaser(laser) myGUI.setAlgorithm(algorithm) myGUI.show() t2 = ThreadGUI(myGUI)
# Francisco Miguel Rivas Montero <*****@*****.**> # import sys from gui.GUI import MainWindow from gui.threadGUI import ThreadGUI from MyAlgorithm import MyAlgorithm from PyQt5.QtWidgets import QApplication from interfaces.camera import ListenerCamera from interfaces.pose3d import ListenerPose3d from interfaces.motors import PublisherMotors if __name__ == "__main__": camera = ListenerCamera("/F1ROS/cameraL/image_raw") motors = PublisherMotors("/F1ROS/cmd_vel", 4, 0.3) pose3d = ListenerPose3d("/F1ROS/odom") pose3dphantom = ListenerPose3d("/F1ROS_phantom/odom") algorithm = MyAlgorithm(camera, motors, pose3d) app = QApplication(sys.argv) myGUI = MainWindow() myGUI.setCamera(camera) myGUI.setMotors(motors) myGUI.setPose3D(pose3d, pose3dphantom) myGUI.setAlgorithm(algorithm) myGUI.show() t2 = ThreadGUI(myGUI) t2.daemon = True t2.start()
sys.argv.remove(arg) if __name__ == '__main__': if len(sys.argv) < 2: print('ERROR: python2 globalNavigation.py [MAP CONFIG file] [YAML CONFIG file]') sys.exit(-1) app = QApplication(sys.argv) frame = MainWindow() grid = Grid(frame) removeMapFromArgs() motors = PublisherMotors("/cmd_vel", 0.5, 0.1) pose = ListenerPose3d("/odom") vel = Velocity(0, 0, motors.getMaxV(), motors.getMaxW()) frame.setVelocity(vel) sensor = Sensor(grid, pose, True) sensor.setGetPathSignal(frame.getPathSig) frame.setGrid(grid) frame.setSensor(sensor) algorithm = MyAlgorithm(grid, sensor, vel) frame.setAlgorithm(algorithm) frame.show() t1 = ThreadMotors(motors, vel) t1.daemon = True