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):
