def run(self, robot_config_values, room_size, rects, start_pos, start_angle, square_size, cartographer_grid): self.is_running = True self.commands_queue = Queue() # Clear map and set sizes for views self.map_window.start(room_size) self.map_scene.draw_map(room_size, rects, start_pos, start_angle) self.scanner_window.start(room_size) self.discovered_map_window.start(room_size, square_size) # Create some objects for the RobotController to use, as we have all the data here hardware = Hardware(start_pos, start_angle, room_size, rects, robot_config_values, self.map_change_pos, self.map_change_rotation) cartographer = Cartographer(room_size, square_size, self.cartographer_grid_values_callback, cartographer_grid) self.controller = RobotController(room_size, hardware, cartographer, self.commands_queue, self.scanner_draw_points, self.scanner_draw_lines, self.scanner_draw_robot, self.scanner_clear) self.controller.start()
def talker(): # ROS rospy.init_node('controller-talker-lorang', anonymous=True) rate = rospy.Rate(5) # 1Hz = 1s # Robot Controller robot = RobotController() # Kinematic solver kine_solver = RRRSolver(0.095, 0.15, 0.15, [-180., 180.], [0., 180.], [0., 180.]) kine_solver.calibration([0., 180., 110.], [1, -1, -1], [-180., 180.], [80., 150.], [80., 135.]) # Rectangle pts_1 = [0.14, 0, 0.2] pts_2 = [0.22, 0, 0.25] pts_3 = [0.14, 0, 0.25] pts_4 = [0.22, 0, 0.2] # Compute trajectory traj = kine_solver.linear_trajectory([pts_1, pts_3, pts_2, pts_4, pts_1], 20) counter = 0 while not rospy.is_shutdown(): # Publish the angles robot.move_to(traj[counter]) counter += 1 if counter >= len(traj): counter = len(traj) - 1 rate.sleep()
def setupUi(self, MainWindow): super().setupUi(MainWindow) self.robot_controller = RobotController() self.robot_controller.connect_feedback(self.feedback_changed) self.robot_controller.connect_log(self.update_log) self.pbUp.clicked.connect(self.pbUp_clicked) self.pbDown.clicked.connect(self.pbDown_clicked) self.pbResetError.clicked.connect(self.pbResetError_clicked) self.pbPauseMotion.clicked.connect(self.pbPauseMotion_clicked) self.pbResumeMotion.clicked.connect(self.pbResumeMotion_clicked) self.pbClearMotion.clicked.connect(self.pbClearMotion_clicked) self.sldAngSpeedValue.valueChanged.connect(self.joint_speed_slided) self.sldCartSpeedValue.valueChanged.connect(self.cart_speed_slided) self.pbReadCurrAng.clicked.connect(self.pbReadCurrAng_clicked) self.pbMoveNewAngVal.clicked.connect(self.pbMoveNewAngVal_clicked) self.pbReadCurrCart.clicked.connect(self.pbReadCurrCart_clicked) self.pbMoveNewCartVal.clicked.connect(self.pbMoveNewCartVal_clicked) for i in range(0, 6): pbTheta_inc = getattr(self, f'pbTheta{i+1}_inc') pbTheta_dec = getattr(self, f'pbTheta{i+1}_dec') pbTheta_inc.clicked.connect(partial(self.pbTheta_inc_clicked, i)) pbTheta_dec.clicked.connect(partial(self.pbTheta_dec_clicked, i)) for i in range(0, 6): pbCart_inc = getattr(self, f'pbCart{i+1}_inc') pbCart_dec = getattr(self, f'pbCart{i+1}_dec') pbCart_inc.clicked.connect(partial(self.pbCart_inc_clicked, i)) pbCart_dec.clicked.connect(partial(self.pbCart_dec_clicked, i))
def start_training(trace_history, room_dst, person_loc, approach_person, sound_wave_model): rc = RobotController() mc = ModelController() for i, trace in enumerate(trace_history): print("-" * 60) print("Current Time: {}, Sample: {}".format(rospy.get_rostime().secs, i)) target_room = trace["target"] # robot back to origin explore_room(rc, room_dst, trace["origin"]) # move a person to dst and emit sound px, py, pidx = set_person_pose(mc, person_loc, target_room) mc.spawn_model("sound_wave", sound_wave_model, px, py, 2) rospy.sleep(3) mc.delete_model("sound_wave") # robot actively explore the room according to the ranking result for next_room in trace["trace"]: if next_room == target_room: print("Sample {} find target room: {}".format(i, next_room)) app_pos = approach_person[str(target_room)][pidx] rc.goto(app_pos["x"], app_pos["y"], app_pos["yaw"]) rospy.sleep(1) else: print("Sample {} explore room: {}".format(i, next_room)) explore_room(rc, room_dst, next_room)
def handle(self): #client = self.request sock = SimpleMessageSocket(self.request) address = self.client_address[0] print('connected from {}'.format(address)) ctl = RobotController() msg_handlers = { SimpleMessageType.SET_IO: self.on_set_io, SimpleMessageType.SET_POSTURE: self.on_set_posture, SimpleMessageType.GET_POSTURE: self.on_get_posture, SimpleMessageType.SYS_STAT: self.on_sys_stat, SimpleMessageType.SET_TOOL_OFFSET: self.on_set_tool_offset } try: while True: recv_msg = sock.recv() msg_handler = msg_handlers.get(recv_msg.msg_type, self.on_unkown_message) reply_msg = msg_handler(ctl, recv_msg) sock.send(reply_msg) finally: ctl.close()
def handle(self): #client = self.request sock = SimpleMessageSocket(self.request) address = self.client_address[0] print('connected with {}'.format(address)) ctl = RobotController() motion_thread = MotionThread(ctl) motion_thread.start() msg_handlers = { SimpleMessageType.JOINT_TRAJ_PT: self.on_joint_traj_pt, SimpleMessageType.EXECUTE_PROGRAM: self.on_execute_program } try: while True: recv_msg = sock.recv() msg_handler = msg_handlers.get(recv_msg.msg_type, self.on_unkown_message) reply_msg = msg_handler(ctl, recv_msg, motion_thread) sock.send(reply_msg) finally: ctl.close()
def main(): try: motor_controller = MotorController() sonar = Sonar(18, 17) controller = RobotController(sonar, motor_controller) server = Server(controller, "192.168.99.11", 8080) server.listen() finally: sonar.stop()
def __init__(self, video_server): threading.Thread.__init__(self) self.video_server = video_server self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.server_address = (IP, PORT) print('starting up on {} port {}'.format(*self.server_address)) self.socket.bind(self.server_address) self.socket.listen(1) self.is_listening = True self.robot_controller = RobotController()
def __init__(self): self.ctrl = RobotController(self.CONTROL_RATE) self.recorder = RobotRecorder(save_dir='', use_aux=False, save_actions=False, save_images=False) self.image_publisher = rospy.Publisher('/robot/head_display', Image_msg, queue_size=2) self.load_splashes() self.control_rate = rospy.Rate(self.PREDICT_RATE) self.ui_rate = rospy.Rate(self.UI_RATE) self.robot_predictor = setup_predictor( self.ROBOT_MODEL, self.ROBOT_BIAS, self.CROP_H_MIN, self.CROP_H_MAX, self.CROP_W_MIN, self.CROP_W_MAX) self.human_predictor = setup_predictor_human( self.HUMAN_MODEL, self.HUMAN_BIAS, self.CROP_H_MIN, self.CROP_H_MAX, self.CROP_W_MIN, self.CROP_W_MAX) self.is_human = False self.predictor = self.robot_predictor rospy.Subscriber("/keyboard/keyup", Key, self.keyboard_up_listener) self.weiss_pub = rospy.Publisher('/wsg_50_driver/goal_position', Cmd, queue_size=10) # rospy.Subscriber("/wsg_50_driver/status", Status, self.weiss_status_listener) self._navigator = intera_interface.Navigator() self.demo_collect = self._navigator.register_callback( self.record_demo, 'right_button_ok') self.swap_key = self._navigator.register_callback( self.swap_sawyer_cuff, 'right_button_back') self.start_key = self._navigator.register_callback( self.start_traj_cuff, 'right_button_square') self.neutral_key = self._navigator.register_callback( self.neutral_cuff, 'right_button_show') self.x_button = self._navigator.register_callback( self.gripper_cuff, 'right_button_triangle') self.calc = EE_Calculator() print 'ONE SHOT READY!' self.running = False self.demo_imgs = None self.move_netural() rospy.spin()
def __init__(self, model_path, vgg19_path): self.ctrl = RobotController() self.recorder = RobotRecorder(save_dir='', seq_len=60, use_aux=False, save_video=True, save_actions=False, save_images=False) self.action_interval = 20 #Hz self.action_sequence_length = 60 self.traj_duration = self.action_sequence_length * self.action_interval self.action_rate = rospy.Rate(self.action_interval) self.control_rate = rospy.Rate(20) self.predictor = setup_predictor(model_path, vgg19_path) self.save_ctr = 0 self.ctrl.set_neutral() self.s = 0
def __init__(self, model_path, vgg19_path): self.ctrl = RobotController() self.recorder = RobotRecorder(save_dir='', seq_len=60, use_aux=False, save_video=True, save_actions=False, save_images=False) self.action_interval = 10 #Hz self.action_sequence_length = 10 self.control_rate = rospy.Rate(self.action_interval) self.predictor = setup_predictor(model_path, vgg19_path) self.save_ctr = 0 self.s = 0 self.imp_ctrl_publisher = rospy.Publisher('desired_joint_pos', JointState, queue_size=1) self.imp_ctrl_release_spring_pub = rospy.Publisher('release_spring', Float32, queue_size=10) self.imp_ctrl_active = rospy.Publisher('imp_ctrl_active', Int64, queue_size=10) self.move_neutral()
rospy.Duration(0.1)) rospy.sleep(0.1) # goal[1] -= .2 # total_nsecs = time.nsecs + 1e8 # fractional, integer = math.modf(total_nsecs/1e9) # time.secs += int(integer) # time.nsecs += fractional*1e9 # controller.move_to_goal(*(goal + HOME_ORI), time=time) elif moving and hit: moving = False hit = False controller.move_to_goal(*HOME) # elif moving: # moving = False # controller.move_to_goal(*HOME) if __name__ == '__main__': rospy.init_node('ball_controller') controller = RobotController(8) # hit back controller controller_hit = RobotController(8) controller.move_to_goal(*HOME) sub = rospy.Subscriber('/ball_detection/predicted_ball_state', PosVelTimed, callback, queue_size=10) rospy.spin()
def __init__(self): self.robot = Robot() self.robot.pose.position.x = 3.0 self.robot.pose.position.y = 5.0 self.robot_controller = RobotController(self.robot)
import traceback import argparse import robot_util import os.path import networking import time import schedule import sys import watchdog import logging import logging.handlers import json import atexit from robot_controller import RobotController robot = RobotController() if (sys.version_info > (3, 0)): import importlib import _thread as thread else: import thread # borrowed unregister function from # https://stackoverflow.com/questions/32097982/cannot-unregister-functions-from-atexit-in-python-2-7 def unregister(func, *targs, **kargs): """unregister a function previously registered with atexit. use exactly the same aguments used for before register. """ for i in range(0, len(atexit._exithandlers)): if (func, targs, kargs) == atexit._exithandlers[i]:
def __init__(self, robot_port): self.game = Game() self.robot = RobotController(robot_port)
app = Flask(__name__) # FlaskのLoggerを無効化する # app.logger.disabled = True # werkzeugのLoggerを無効化する # werkzeug_logger = logging.getLogger('werkzeug') # werkzeug_logger.disabled = True # logger = logging.getLogger() # logger.addHandler(logging.FileHandler("/dev/null")) # host = 'dm' host = 'localhost' port = 8080 app = Flask(__name__) rc = RobotController() topic_history_length = 6 @app.route('/') def index(): return render_template('index.html') @app.route('/stt', methods=['POST']) def stt(): date = request.form['date'] time = request.form['time'] text = request.form['text'] user = request.form['user'] topics, persons = rc.main(date, time, user, text)
from robot_controller import RobotController from graceful_killer import GracefulKiller from argparse import ArgumentParser from curtsies import Input import numpy as np from time import sleep from vision.classify import Classifier from vision.squeeze_classify import SqueezeClassifier from picamera import PiCamera parser = ArgumentParser() parser.add_argument("--debug", help="Use debug mode") args = parser.parse_args() thermal = ThermalSensor() controller = RobotController() state = State(StateName.INIT, controller, {"a": 0}) killer = GracefulKiller(state) classifier = SqueezeClassifier('./models/squeezenet_v1.1.prototxt', './models/squeezenet_v1.1.caffemodel', debug=args.debug) ''' classifier = Classifier('models/MobileNetSSD_deploy.prototxt.txt', 'models/MobileNetSSD_deploy.caffemodel', debug=args.debug) ''' def is_cat(pixels):