class HQControlServer():
def __init__(self):
self.controller = Controller()
self.coef = 58823530
self.port = "50060"
if len(sys.argv) > 1:
self.port = sys.argv[1]
int(self.port)
self.context = zmq.Context()
self.socket = self.context.socket(zmq.REP)
self.socket.bind("tcp://*:%s" % self.port)
def listen_commands(self):
print "server is ready for commands"
while True:
# Wait for next request from client
message = self.socket.recv()
direction, velocity = json.loads(message)
if direction == 'forward':
self.handleButtonFWD(velocity)
if direction == 'back':
self.handleButtonBack(velocity)
if direction == 'left':
self.handleButtonL(velocity)
if direction == 'right':
self.handleButtonR(velocity)
if direction == 'stop':
self.handleButtonSTP()
self.socket.send("OK")
def from_real_to_drive_vel(self, real):
return float(real) * self.coef
def handleButtonFWD(self, vel):
self.controller.set_global_velocity(self.from_real_to_drive_vel(vel))
self.controller.go_forward()
def handleButtonBack(self, vel):
self.controller.set_global_velocity(self.from_real_to_drive_vel(vel))
self.controller.go_back()
def handleButtonR(self, vel):
self.controller.set_global_velocity(self.from_real_to_drive_vel(vel))
self.controller.turn_right()
def handleButtonL(self, vel):
self.controller.set_global_velocity(self.from_real_to_drive_vel(vel))
self.controller.turn_left()
def handleButtonSTP(self):
self.controller.stop_motor()
def test_Controller(self, vel=5000000):
test_object = Controller(self.port)
self.start_info(test_object)
test_object.set_global_velocity(vel/2)
test_object.go_forward()
time.sleep(2)
test_object.set_global_velocity(vel/2)
test_object.go_back()
time.sleep(2)
test_object.set_global_velocity(vel/5)
test_object.turn_right()
time.sleep(5)
test_object.turn_left()
time.sleep(5)
test_object.stop_motor()
class ROSBaseControl:
def __init__(self):
rospy.init_node('base_control')
self.finished = Event()
self.controller = Controller('/dev/ttyS0')
rospy.loginfo("Started base_control")
print "Started base_control"
# odom...
self.rate = float(rospy.get_param("~base_controller_rate", 20))
now = rospy.Time.now()
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = now + self.t_delta
self.then = now # time for determining dx/dy
# internal data
self.x = 0 # position in xy plane
self.y = 0
self.th = 0 # rotation in radians
self.vx = 0.0
self.vy = 0.0
self.vth = 0.0
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry)
self.odomBroadcaster = TransformBroadcaster()
def callback_cmd(self, msg):
# TODO: from linear & radial velocities to linear vel on right/left wheel
rospy.loginfo("Received a /cmd_vel message!")
print "Received a /cmd_vel message!"
# from m/s to controller velocity
coef = 58823530
# L = distance between wheels
L = 0.495
if msg.linear.x == 0 and msg.angular.z == 0:
self.controller.stop_motor()
else:
rwheel_vel = (2*msg.linear.x - L*msg.angular.z)/2
lwheel_vel = (2*msg.linear.x + L*msg.angular.z)/2
print rwheel_vel, lwheel_vel
self.controller.set_lwheel_velocity(abs(lwheel_vel*coef))
self.controller.set_rwheel_velocity(abs(rwheel_vel*coef))
self.controller.go_forward()
self.vx = msg.linear.x
#self.vth = msg.angular.z
def listener(self):
rospy.Subscriber('/cmd_vel', Twist, self.callback_cmd)
rosRate = rospy.Rate(self.rate)
while not rospy.is_shutdown() and not self.finished.isSet():
now = rospy.Time.now()
if now > self.t_next:
dt = now - self.then
self.then = now
dt = dt.to_sec()
delta_x = (self.vx * cos(self.th) - self.vy * sin(self.th)) * dt
delta_y = (self.vx * sin(self.th) + self.vy * cos(self.th)) * dt
delta_th = self.vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = self.vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.vth
self.odomPub.publish(odom)
self.t_next = now + self.t_delta
rosRate.sleep()
class RobotHQ(QtGui.QWidget):
# TODO: 5 buttons, 2 text fields
# GUI (forward/back/right/left/stop, velocity, distance) -> cmd
def __init__(self, port=1, server=None):
self.controller = Controller(port)
self.server = server
self.coef = 58823530
self.app = QtGui.QApplication(sys.argv)
super(RobotHQ, self).__init__()
self.setGeometry(500, 500, 700, 300)
self.setWindowTitle("Mobile Robot control")
self.init_ui()
self.center()
if server:
self.zmq_port = "50060"
self.context = zmq.Context()
print "Connecting to server..."
self.socket = self.context.socket(zmq.REQ)
self.socket.connect("tcp://%s:%s" % (self.server, self.zmq_port))
def init_ui(self):
btn_fwd = QtGui.QPushButton("Move Forward", self)
btn_fwd.clicked.connect(self.handleButtonFWD)
btn_fwd.resize(btn_fwd.sizeHint())
btn_fwd.move(115, 10)
btn_fwd.setFixedWidth(150)
btn_back = QtGui.QPushButton("Back", self)
btn_back.clicked.connect(self.handleButtonBack)
btn_back.resize(btn_back.sizeHint())
btn_back.move(115, 65)
btn_back.setFixedWidth(150)
btn_stop = QtGui.QPushButton("Stop", self)
btn_stop.clicked.connect(self.handleButtonSTP)
btn_stop.resize(btn_stop.sizeHint())
btn_stop.move(115, 95)
btn_stop.setFixedWidth(150)
btn_left = QtGui.QPushButton("Turn Left", self)
btn_left.clicked.connect(self.handleButtonL)
btn_left.resize(btn_stop.sizeHint())
btn_left.move(10, 37)
btn_left.setFixedWidth(150)
btn_right = QtGui.QPushButton("Turn Right", self)
btn_right.clicked.connect(self.handleButtonR)
btn_right.resize(btn_stop.sizeHint())
btn_right.move(210, 37)
btn_right.setFixedWidth(150)
lb_vel = QtGui.QLabel("Velocity [m/s]", self)
lb_vel.move(15, 135)
self.le_vel = QtGui.QLineEdit("0.02", self)
self.le_vel.move(10, 150)
self.le_vel.setFixedWidth(150)
lb_dis = QtGui.QLabel("Position", self)
lb_dis.move(220, 135)
self.le_pos = QtGui.QLineEdit(self)
self.le_pos.move(215, 150)
self.le_pos.setFixedWidth(150)
def from_real_to_drive_vel(self, real):
return float(real) * self.coef
def handleButtonFWD(self):
if self.server:
cmd = []
cmd.append("forward")
cmd.append(str(self.le_vel.text()))
v_send = json.dumps(cmd)
self.socket.send(v_send)
# Get the reply.
message = self.socket.recv()
print message
else:
self.controller.set_global_velocity(self.from_real_to_drive_vel(self.le_vel.text()))
if self.le_pos.text() != "":
# TODO: implement self.controller.set_possition(int(self.le_pos.text()))
pass
self.controller.go_forward()
def handleButtonBack(self):
if self.server:
cmd = []
cmd.append("back")
cmd.append(str(self.le_vel.text()))
v_send = json.dumps(cmd)
self.socket.send(v_send)
# Get the reply.
message = self.socket.recv()
print message
else:
self.controller.set_global_velocity(self.from_real_to_drive_vel(self.le_vel.text()))
self.controller.go_back()
def handleButtonR(self):
if self.server:
cmd = []
cmd.append("right")
cmd.append(str(self.le_vel.text()))
v_send = json.dumps(cmd)
self.socket.send(v_send)
# Get the reply.
message = self.socket.recv()
print message
else:
self.controller.set_global_velocity(self.from_real_to_drive_vel(self.le_vel.text()))
self.controller.turn_right()
def handleButtonL(self):
if self.server:
cmd = []
cmd.append("left")
cmd.append(str(self.le_vel.text()))
v_send = json.dumps(cmd)
self.socket.send(v_send)
# Get the reply.
message = self.socket.recv()
print message
else:
self.controller.set_global_velocity(self.from_real_to_drive_vel(self.le_vel.text()))
self.controller.turn_left()
def handleButtonSTP(self):
if self.server:
cmd = []
cmd.append("stop")
cmd.append(str(self.le_vel.text()))
v_send = json.dumps(cmd)
self.socket.send(v_send)
# Get the reply.
message = self.socket.recv()
print message
else:
self.controller.stop_motor()
def closeEvent(self, event):
reply = QtGui.QMessageBox.question(
self, "Message", "Are you sure to quit?", QtGui.QMessageBox.Yes | QtGui.QMessageBox.No, QtGui.QMessageBox.No
)
if reply == QtGui.QMessageBox.Yes:
event.accept()
else:
event.ignore()
def center(self):
qr = self.frameGeometry()
cp = QtGui.QDesktopWidget().availableGeometry().center()
qr.moveCenter(cp)
self.move(qr.topLeft())
def start_gui(self):
self.show()
sys.exit(self.app.exec_())