class MainNode():
def __init__(self):
rospy.init_node('default_offboard', anonymous=True)
self.rate = rospy.Rate(20)
self.mav1 = Mav("mavros")
self._command_sub = rospy.Subscriber("pose_command", String,
self._pose_command_callback)
# wait for FCU connection
self.mav1.wait_for_connection()
print("mavs connected")
self.sm = StateMachine()
self.sm.set_params(self.mav1)
self.sm.set_current_state(self.sm.States.TAKE_OFF)
def loop(self):
# enter the main loop
while (True):
# print "Entered whiled loop"
self.sm.execute()
self.rate.sleep()
def _pose_command_callback(self, topic=String()):
text = topic.data
textarr = np.array(text.split(";"))
arr = textarr.astype(np.float)
print("new target pose: " + str(arr.tolist()))
pose = create_setpoint_message_xyz_yaw(arr[0], arr[1], arr[2], arr[3])
self.mav1.set_target_pose(pose)
self.mav1.max_speed = 1
self.sm.set_next_state(self.sm.States.IDLE)
self.sm.set_current_state(self.sm.States.WAITING_TO_ARRIVE)
pass
class MainNode():
def __init__(self):
rospy.init_node('default_offboard', anonymous=True)
self.rate = rospy.Rate(20)
self.mav1 = Mav("uav1/mavros")
self.mav2 = Mav("uav2/mavros")
self.sm = StateMachine()
self.sm.set_params((self.mav1, self.mav2))
self._command_sub = rospy.Subscriber("pose_command", String,
self._pose_command_callback)
# wait for FCU connection
self.mav1.wait_for_connection()
self.mav2.wait_for_connection()
self.last_request = rospy.Time.now() - rospy.Duration(5.0)
self.sm.set_current_state(self.sm.States.TAKE_OFF)
def loop(self):
# enter the main loop
while (True):
# print "Entered whiled loop"
self.arm_and_set_mode()
self.sm.execute()
self.rate.sleep()
def arm_and_set_mode(self):
if rospy.Time.now() - self.last_request > rospy.Duration(1.0):
if self.mav1.UAV_state.mode != "OFFBOARD":
self.mav1.set_mode(0, 'OFFBOARD')
print("enabling offboard mode")
self.last_request = rospy.Time.now()
if not self.mav1.UAV_state.armed:
if self.mav1.set_arming(True):
print("Vehicle armed")
self.last_request = rospy.Time.now()
if self.mav2.UAV_state.mode != "OFFBOARD":
self.mav2.set_mode(0, 'OFFBOARD')
print("enabling offboard mode")
self.last_request = rospy.Time.now()
if not self.mav2.UAV_state.armed:
if self.mav2.set_arming(True):
print("Vehicle armed")
self.last_request = rospy.Time.now()
def _pose_command_callback(self, topic=String()):
text = topic.data
textarr = np.array(text.split(";"))
arr = textarr.astype(np.float)
print("new target pose: " + str(arr.tolist()))
pose = create_setpoint_message_xyz_yaw(arr[0], arr[1], arr[2], arr[3])
self.mav1.set_target_pose(pose)
self.mav2.set_target_pose(pose)
self.sm.set_next_state(self.sm.States.IDLE)
self.sm.set_current_state(self.sm.States.WAITING_TO_ARRIVE)
pass
class Gui(QMainWindow):
"""
Main GUI Class
contains the main function and interfaces between
the GUI and functions
"""
def __init__(self, parent=None):
QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Set GUI to track mouse """
QWidget.setMouseTracking(self, True)
"""
Dynamixel bus
TODO: add other motors here as needed with their correct address"""
# self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
# port_num = self.dxlbus.port()
# base = DXL_XL(port_num, 1)
# shld = DXL_XL(port_num, 2)
# elbw = DXL_XL(port_num, 3)
# wrst = DXL_XL(port_num, 4)
# grip = DXL_XL(port_num, 5)
"""Objects Using Other Classes"""
self.camera = PiCamera()
# self.rexarm = Rexarm((base, shld, elbw, wrst, grip))
# self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp)
global explore_obj
explore_obj = Exploration(self.sm)
self.taskThread = TaskThread(self.sm)
self.rgb_image = None
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_current_state, "calibrate"))
self.ui.btnUser2.setText("Moving Arm")
self.ui.btnUser2.clicked.connect(
partial(self.sm.set_current_state, "moving_arm"))
self.ui.btnUser3.setText("Return Home")
self.ui.btnUser3.clicked.connect(
partial(self.sm.set_current_state, "arm_return_home"))
self.ui.btnUser4.setText("Exploration")
self.ui.btnUser4.clicked.connect(
partial(self.sm.set_current_state, "exploration"))
self.ui.btnUser5.setText("Swipe")
self.ui.btnUser5.clicked.connect(
partial(self.sm.set_current_state, "swipe"))
self.ui.btnUser6.setText("Put back block")
self.ui.btnUser6.clicked.connect(
partial(self.sm.set_current_state, "put_back_block"))
self.ui.btnUser7.setText("Moving back")
self.ui.btnUser7.clicked.connect(
partial(self.sm.set_current_state, "moving_back"))
self.ui.btnUser8.setText("Reach block")
self.ui.btnUser8.clicked.connect(
partial(self.sm.set_current_state, "reach_block"))
self.ui.btnUser9.setText("Moving to side")
self.ui.btnUser9.clicked.connect(
partial(self.sm.set_current_state, "moving_to_block_side"))
# self.ui.btn_task1.clicked.connect(
# partial(self.taskThread.set_task_num, 1))
self.ui.btn_task1.clicked.connect(partial(self.taskThread.run))
# Button for exploration code to run
# self.ui.explore_btn.clicked.connect(self.taskThread.start)
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.rdoutStatus.setText("Waiting for input")
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""initalize rexarm"""
self.rexarm.initialize()
"""Setup Threads"""
self.videoThread = VideoThread(self.camera)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.updateAprilTags.connect(self.updateAprilTags)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
# Moving line to button to start
# self.taskThread.start()
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(
self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(
self.updateStatusMessage)
self.displayThread.start()
"""
Setup Timer
this runs the trackMouse function every 50ms
"""
self._timer = QTimer(self)
self._timer.timeout.connect(self.trackMouse)
self._timer.start(50)
""" Slots attach callback functions to signals emitted from threads"""
# Convert image to Qt image for display.
def convertImage(self, image):
qimg = QImage(image, image.shape[1], image.shape[0],
QImage.Format_RGB888)
return QPixmap.fromImage(qimg)
# TODO: Add more QImage in the list for visualization and debugging
@pyqtSlot(list)
def setImage(self, image_list):
rgb_image = image_list[0]
apriltag_image = image_list[1]
block_image = image_list[2]
if (self.ui.radioVideo.isChecked()):
self.ui.videoDisplay.setPixmap(self.convertImage(rgb_image))
self.rgb_image = rgb_image
if (self.ui.radioApril.isChecked()):
self.ui.videoDisplay.setPixmap(self.convertImage(apriltag_image))
if (self.ui.radioUsr1.isChecked()):
self.ui.videoDisplay.setPixmap(self.convertImage(block_image))
@pyqtSlot(list)
def updateAprilTags(self, tags):
self.sm.tags = tags
@pyqtSlot(list)
def updateJointReadout(self, joints):
self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D)))
self.ui.rdoutShoulderJC.setText(str("%+.2f" % (joints[1] * R2D)))
self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D)))
self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D)))
if (len(joints) > 5):
self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D)))
else:
self.ui.rdoutWrist3JC.setText(str("N.A."))
@pyqtSlot(list)
def updateEndEffectorReadout(self, pos):
self.ui.rdoutX.setText(str("%+.4f" % (pos[0])))
self.ui.rdoutY.setText(str("%+.4f" % (pos[1])))
self.ui.rdoutZ.setText(str("%+.4f" % (pos[2])))
self.ui.rdoutT.setText(str("%+.4f" % (pos[3])))
@pyqtSlot(str)
def updateStatusMessage(self, msg):
self.ui.rdoutStatus.setText(msg)
""" Other callback functions attached to GUI elements"""
def estop(self):
self.rexarm.estop = True
self.sm.set_current_state("estop")
def sliderChange(self):
"""
Function to change the slider labels when sliders are moved
and to command the arm to the given position
"""
self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
self.ui.rdoutWrist2.setText(str(self.ui.sldrWrist2.value()))
self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))
self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] *
self.rexarm.num_joints,
update_now=False)
self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() /
100.0,
update_now=False)
joint_positions = np.array([
self.ui.sldrBase.value() * D2R,
self.ui.sldrShoulder.value() * D2R,
self.ui.sldrElbow.value() * D2R,
self.ui.sldrWrist.value() * D2R,
self.ui.sldrGrip1.value() * D2R
])
self.rexarm.set_positions(joint_positions, update_now=False)
def directControlChk(self, state):
if state == Qt.Checked:
self.sm.set_current_state("manual")
self.ui.SliderFrame.setEnabled(True)
else:
self.sm.set_current_state("idle")
self.ui.SliderFrame.setEnabled(False)
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: Display the rgb/hsv value
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutRGB.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,-)" % (x, y))
try:
rgb = self.rgb_image[y, x] # [r, g, b] of this pixel
except TypeError:
# get None for rgb_image
print("trackMouse(): Didn't get RGB value")
return
hsv_image = cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2HSV)
hsv = hsv_image[y, x]
# print(hsv_image)
# self.ui.rdoutRGB.setText("({},{},{})".format(rgb[0], rgb[1], rgb[2]))
self.ui.rdoutRGB.setText("({},{},{})".format(
hsv[0], hsv[1], hsv[2]))
def mousePressEvent(self, QMouseEvent):
"""
Function used to record mouse click positions for calibration
"""
""" Get mouse posiiton """
x = QMouseEvent.x()
y = QMouseEvent.y()
""" If mouse position is not over the camera image ignore """
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)):
return
class Gui(QMainWindow):
"""
Main GUI Class
contains the main function and interfaces between
the GUI and functions
"""
def __init__(self, parent=None):
QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Set GUI to track mouse """
QWidget.setMouseTracking(self, True)
"""
Dynamixel bus
TODO: add other motors here as needed with their correct address"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = self.dxlbus.port()
base = DXL_XL(port_num, 1)
shld = DXL_XL(port_num, 2)
elbw = DXL_XL(port_num, 3)
wrst = DXL_XL(port_num, 4)
grip = DXL_XL(port_num, 5)
"""Objects Using Other Classes"""
self.camera = MBotCamera()
self.mbot_arm = MBotArm((base, shld, elbw, wrst, grip))
self.tp = TrajectoryPlanner(self.mbot_arm)
self.sm = StateMachine(self.mbot_arm, self.tp)
self.rgb_image = None
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btnUser1.setText("Detect Tags")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_current_state, "detect_tags"))
self.ui.btnUser2.setText("Stop Detection")
self.ui.btnUser2.clicked.connect(
partial(self.sm.set_current_state, "stop_detection"))
self.ui.btnUser3.setText("Calibrate Extrinsics")
self.ui.btnUser3.clicked.connect(
partial(self.sm.set_current_state, "calibrate_extrinsics"))
self.ui.btnUser3.setText("Test Release")
self.ui.btnUser3.clicked.connect(
partial(self.sm.set_current_state, "release_block"))
self.ui.btn_exec.setText("Test Grasp")
self.ui.btn_exec.clicked.connect(
partial(self.sm.set_current_state, "grab_block"))
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.rdoutStatus.setText("Waiting for input")
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""initalize mbot_arm"""
self.mbot_arm.initialize()
"""Setup Threads"""
self.videoThread = VideoThread(self.camera)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.updateAprilTags.connect(self.updateAprilTags)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.displayThread = DisplayThread(self.mbot_arm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(
self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(
self.updateStatusMessage)
self.displayThread.start()
"""
Setup Timer
this runs the trackMouse function every 50ms
"""
self._timer = QTimer(self)
self._timer.timeout.connect(self.trackMouse)
self._timer.start(50)
""" Slots attach callback functions to signals emitted from threads"""
# Convert image to Qt image for display.
def convertImage(self, image):
qimg = QImage(image, image.shape[1], image.shape[0],
QImage.Format_RGB888)
return QPixmap.fromImage(qimg)
# TODO: Add more QImage in the list for visualization and debugging
@pyqtSlot(list)
def setImage(self, image_list):
rgb_image = image_list[0]
apriltag_image = image_list[1]
if (self.ui.radioVideo.isChecked()):
self.ui.videoDisplay.setPixmap(self.convertImage(rgb_image))
self.rgb_image = rgb_image
if (self.ui.radioApril.isChecked()):
self.ui.videoDisplay.setPixmap(self.convertImage(apriltag_image))
self.rgb_image = apriltag_image
@pyqtSlot(list)
def updateAprilTags(self, tags):
self.sm.tags = tags
self.sm.new_image = True
@pyqtSlot(list)
def updateJointReadout(self, joints):
self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D)))
self.ui.rdoutShoulderJC.setText(str("%+.2f" % (joints[1] * R2D)))
self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D)))
self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D)))
@pyqtSlot(list)
def updateEndEffectorReadout(self, pos):
self.ui.rdoutX.setText(str("%+.2f" % (pos[0])))
self.ui.rdoutY.setText(str("%+.2f" % (pos[1])))
self.ui.rdoutZ.setText(str("%+.2f" % (pos[2])))
self.ui.rdoutT.setText(str("%+.2f" % (pos[3])))
@pyqtSlot(str)
def updateStatusMessage(self, msg):
self.ui.rdoutStatus.setText(msg)
""" Other callback functions attached to GUI elements"""
def estop(self):
self.mbot_arm.estop = True
self.sm.set_current_state("estop")
def sliderChange(self):
"""
Function to change the slider labels when sliders are moved
and to command the arm to the given position
"""
self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))
self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
self.mbot_arm.set_torque_limits(
[self.ui.sldrMaxTorque.value() / 100.0] * self.mbot_arm.num_joints,
update_now=False)
self.mbot_arm.set_speeds_normalized_global(self.ui.sldrSpeed.value() /
100.0,
update_now=False)
joint_positions = np.array([
self.ui.sldrBase.value() * D2R,
self.ui.sldrShoulder.value() * D2R,
self.ui.sldrElbow.value() * D2R,
self.ui.sldrWrist.value() * D2R,
self.ui.sldrGrip1.value() * D2R
])
self.mbot_arm.set_positions(joint_positions, update_now=False)
def directControlChk(self, state):
if state == Qt.Checked:
self.sm.set_current_state("manual")
self.ui.SliderFrame.setEnabled(True)
else:
self.sm.set_current_state("idle")
self.ui.SliderFrame.setEnabled(False)
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: Display the rgb/hsv value
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutRGB.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,-)" % (x, y))
#rgb = self.rgb_image[y, x] # [r, g, b] of this pixel
#hsv_image = cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2HSV)
#self.ui.rdoutRGB.setText("(R,G,B)")
def mousePressEvent(self, QMouseEvent):
"""
Function used to record mouse click positions for calibration
"""
""" Get mouse posiiton """
x = QMouseEvent.x()
y = QMouseEvent.y()
""" If mouse position is not over the camera image ignore """
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
