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