class PingGUI(Plugin):
def __init__(self, context):
super(PingGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('PingGUI')
self.msg = None
# Create a container widget and give it a layout
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._label = QLabel("xx ms latency")
p = self._label.palette()
p.setColor(self._label.backgroundRole(), Qt.red)
self._label.setPalette(p)
self._layout.addWidget(self._label)
self.set_bg_color(100, 100, 100)
rospy.Subscriber("/ping/delay", Float64, self.ping_cb)
context.add_widget(self._container)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_gui)
self._update_plot_timer.start(1)
def update_gui(self):
if not self.msg:
return
msg = self.msg
# msec
# 100 -> green, 1000 -> red
# normalize within 1000 ~ 100
orig_latency = msg.data
msg_data = orig_latency
if msg.data > 1000:
msg_data = 1000
elif msg.data < 100:
msg_data = 100
ratio = (msg_data - 100) / (1000 - 100)
color_r = ratio * 255.0
color_g = (1 - ratio) * 255.0
# devnull = open(os.devnull, "w")
# with RedirectStdStreams(stdout=devnull, stderr=devnull):
self.set_bg_color(color_r, color_g, 0)
self._label.setText("%d ms latency" % (orig_latency))
def set_bg_color(self, r, g, b):
self._label.setStyleSheet("QLabel { display: block; background-color: rgba(%d, %d, %d, 255); text-align: center; font-size: 30px;}" % (r, g, b))
def ping_cb(self, msg):
self.msg = msg
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class CalibrationMovementsGUI(QWidget):
NOT_INITED_YET = 0
BAD_PLAN = 1
GOOD_PLAN = 2
MOVED_TO_POSE = 3
BAD_STARTING_POSITION = 4
GOOD_STARTING_POSITION = 5
def __init__(self):
super(CalibrationMovementsGUI, self).__init__()
move_group_name = rospy.get_param('~move_group', 'manipulator')
self.angle_delta = math.radians(
rospy.get_param('~rotation_delta_degrees', 25))
self.translation_delta = rospy.get_param('~translation_delta_meters',
0.1)
max_velocity_scaling = rospy.get_param('~max_velocity_scaling', 0.5)
max_acceleration_scaling = rospy.get_param('~max_acceleration_scaling',
0.5)
self.local_mover = CalibrationMovements(move_group_name,
max_velocity_scaling,
max_acceleration_scaling)
self.current_pose = -1
self.current_plan = None
self.initUI()
self.state = CalibrationMovementsGUI.NOT_INITED_YET
def initUI(self):
self.layout = QVBoxLayout()
self.labels_layout = QHBoxLayout()
self.buttons_layout = QHBoxLayout()
self.progress_bar = QProgressBar()
self.pose_number_lbl = QLabel('0/8')
self.bad_plan_lbl = QLabel('No plan yet')
self.guide_lbl = QLabel('Hello')
self.check_start_pose_btn = QPushButton('Check starting pose')
self.check_start_pose_btn.clicked.connect(
self.handle_check_current_state)
self.next_pose_btn = QPushButton('Next Pose')
self.next_pose_btn.clicked.connect(self.handle_next_pose)
self.plan_btn = QPushButton('Plan')
self.plan_btn.clicked.connect(self.handle_plan)
self.execute_btn = QPushButton('Execute')
self.execute_btn.clicked.connect(self.handle_execute)
self.labels_layout.addWidget(self.pose_number_lbl)
self.labels_layout.addWidget(self.bad_plan_lbl)
self.buttons_layout.addWidget(self.check_start_pose_btn)
self.buttons_layout.addWidget(self.next_pose_btn)
self.buttons_layout.addWidget(self.plan_btn)
self.buttons_layout.addWidget(self.execute_btn)
self.layout.addWidget(self.progress_bar)
self.layout.addLayout(self.labels_layout)
self.layout.addWidget(self.guide_lbl)
self.layout.addLayout(self.buttons_layout)
self.setLayout(self.layout)
self.plan_btn.setEnabled(False)
self.execute_btn.setEnabled(False)
self.setWindowTitle('Local Mover')
self.show()
def updateUI(self):
self.progress_bar.setMaximum(len(self.local_mover.poses))
self.progress_bar.setValue(self.current_pose + 1)
self.pose_number_lbl.setText('{}/{}'.format(
self.current_pose + 1, len(self.local_mover.poses)))
if self.state == CalibrationMovementsGUI.BAD_PLAN:
self.bad_plan_lbl.setText('BAD plan!! Don\'t do it!!!!')
self.bad_plan_lbl.setStyleSheet('QLabel { background-color : red}')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.bad_plan_lbl.setText('Good plan')
self.bad_plan_lbl.setStyleSheet(
'QLabel { background-color : green}')
else:
self.bad_plan_lbl.setText('No plan yet')
self.bad_plan_lbl.setStyleSheet('')
if self.state == CalibrationMovementsGUI.NOT_INITED_YET:
self.guide_lbl.setText(
'Bring the robot to a plausible position and check if it is a suitable starting pose'
)
elif self.state == CalibrationMovementsGUI.BAD_STARTING_POSITION:
self.guide_lbl.setText('Cannot calibrate from current position')
elif self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION:
self.guide_lbl.setText('Ready to start: click on next pose')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.guide_lbl.setText(
'The plan seems good: press execute to move the robot')
elif self.state == CalibrationMovementsGUI.BAD_PLAN:
self.guide_lbl.setText('Planning failed: try again')
elif self.state == CalibrationMovementsGUI.MOVED_TO_POSE:
self.guide_lbl.setText(
'Pose reached: take a sample and go on to next pose')
can_plan = self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.plan_btn.setEnabled(can_plan)
can_move = self.state == CalibrationMovementsGUI.GOOD_PLAN
self.execute_btn.setEnabled(can_move)
def handle_check_current_state(self):
self.local_mover.compute_poses_around_current_state(
self.angle_delta, self.translation_delta)
joint_limits = [math.radians(90)] * 5 + [math.radians(180)] + [
math.radians(350)
] # TODO: make param
if self.local_mover.check_poses(joint_limits):
self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
else:
self.state = CalibrationMovementsGUI.BAD_STARTING_POSITION
self.current_pose = -1
self.updateUI()
def handle_next_pose(self):
self.guide_lbl.setText('Going to center position')
if self.current_pose != -1:
plan = self.local_mover.plan_to_start_pose()
if plan is None:
self.guide_lbl.setText(
'Failed planning to center position: try again')
else:
self.local_mover.execute_plan(plan)
if self.current_pose < len(self.local_mover.poses) - 1:
self.current_pose += 1
self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.updateUI()
def handle_plan(self):
self.guide_lbl.setText(
'Planning to the next position. Click on execute when a good one was found'
)
if self.current_pose >= 0:
self.current_plan = self.local_mover.plan_to_pose(
self.local_mover.poses[self.current_pose])
if CalibrationMovements.is_crazy_plan(
self.current_plan, self.local_mover.fallback_joint_limits
): #TODO: sort out this limits story
self.state = CalibrationMovementsGUI.BAD_PLAN
else:
self.state = CalibrationMovementsGUI.GOOD_PLAN
self.updateUI()
def handle_execute(self):
if self.current_plan is not None:
self.guide_lbl.setText('Going to the selected pose')
self.local_mover.execute_plan(self.current_plan)
self.state = CalibrationMovementsGUI.MOVED_TO_POSE
self.updateUI()
class CalibrationMovementsGUI(QWidget):
NOT_INITED_YET = 0
BAD_PLAN = 1
GOOD_PLAN = 2
MOVED_TO_POSE = 3
BAD_STARTING_POSITION = 4
GOOD_STARTING_POSITION = 5
CHECKING_STARTING_POSITION = 6
MOVEMENT_FAILED = 7
def __init__(self):
super(CalibrationMovementsGUI, self).__init__()
self.handeye_client = HandeyeClient()
self.current_target_pose = -1 # -1 is home
self.target_poses = None
self.plan_was_successful = None
self.state = CalibrationMovementsGUI.NOT_INITED_YET
self.layout = QVBoxLayout()
self.labels_layout = QHBoxLayout()
self.buttons_layout = QHBoxLayout()
self.progress_bar = QProgressBar()
self.pose_number_lbl = QLabel('0/0')
self.bad_plan_lbl = QLabel('No plan yet')
self.bad_plan_lbl.setAlignment(Qt.AlignCenter)
self.guide_lbl = QLabel('Hello')
self.guide_lbl.setWordWrap(True)
self.check_start_pose_btn = QPushButton('Check starting pose')
self.check_start_pose_btn.clicked.connect(
self.handle_check_current_state)
self.next_pose_btn = QPushButton('Next Pose')
self.next_pose_btn.clicked.connect(self.handle_next_pose)
self.plan_btn = QPushButton('Plan')
self.plan_btn.clicked.connect(self.handle_plan)
self.execute_btn = QPushButton('Execute')
self.execute_btn.clicked.connect(self.handle_execute)
self.labels_layout.addWidget(self.pose_number_lbl)
self.labels_layout.addWidget(self.bad_plan_lbl)
self.buttons_layout.addWidget(self.check_start_pose_btn)
self.buttons_layout.addWidget(self.next_pose_btn)
self.buttons_layout.addWidget(self.plan_btn)
self.buttons_layout.addWidget(self.execute_btn)
self.layout.addWidget(self.progress_bar)
self.layout.addLayout(self.labels_layout)
self.layout.addWidget(self.guide_lbl)
self.layout.addLayout(self.buttons_layout)
self.setLayout(self.layout)
self.plan_btn.setEnabled(False)
self.execute_btn.setEnabled(False)
self.setWindowTitle('Local Mover')
self.show()
def update_ui(self):
if self.target_poses:
count_target_poses = len(self.target_poses)
else:
count_target_poses = 1
self.progress_bar.setMaximum(count_target_poses)
self.progress_bar.setValue(self.current_target_pose + 1)
self.pose_number_lbl.setText('{}/{}'.format(
self.current_target_pose + 1, count_target_poses))
if self.state == CalibrationMovementsGUI.BAD_PLAN:
self.bad_plan_lbl.setText('BAD plan!! Don\'t do it!!!!')
self.bad_plan_lbl.setStyleSheet('QLabel { background-color : red}')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.bad_plan_lbl.setText('Good plan')
self.bad_plan_lbl.setStyleSheet(
'QLabel { background-color : green}')
else:
self.bad_plan_lbl.setText('No plan yet')
self.bad_plan_lbl.setStyleSheet('')
if self.state == CalibrationMovementsGUI.NOT_INITED_YET:
self.guide_lbl.setText(
'Bring the robot to a plausible position and check if it is a suitable starting pose'
)
elif self.state == CalibrationMovementsGUI.CHECKING_STARTING_POSITION:
self.guide_lbl.setText(
'Checking if the robot can translate and rotate in all directions from the current pose'
)
elif self.state == CalibrationMovementsGUI.BAD_STARTING_POSITION:
self.guide_lbl.setText('Cannot calibrate from current position')
elif self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION:
self.guide_lbl.setText('Ready to start: click on next pose')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.guide_lbl.setText(
'The plan seems good: press execute to move the robot')
elif self.state == CalibrationMovementsGUI.BAD_PLAN:
self.guide_lbl.setText('Planning failed: try again')
elif self.state == CalibrationMovementsGUI.MOVED_TO_POSE:
self.guide_lbl.setText(
'Pose reached: take a sample and go on to next pose')
can_plan = self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.plan_btn.setEnabled(can_plan)
can_move = self.state == CalibrationMovementsGUI.GOOD_PLAN
self.execute_btn.setEnabled(can_move)
QCoreApplication.processEvents()
def handle_check_current_state(self):
self.state = CalibrationMovementsGUI.CHECKING_STARTING_POSITION
self.update_ui()
res = self.handeye_client.check_starting_pose()
if res.can_calibrate:
self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
else:
self.state = CalibrationMovementsGUI.BAD_STARTING_POSITION
self.current_target_pose = res.target_poses.current_target_pose_index
self.target_poses = res.target_poses.target_poses
self.plan_was_successful = None
self.update_ui()
def handle_next_pose(self):
res = self.handeye_client.select_target_pose(self.current_target_pose +
1)
self.current_target_pose = res.target_poses.current_target_pose_index
self.target_poses = res.target_poses.target_poses
self.plan_was_successful = None
self.state = CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.update_ui()
def handle_plan(self):
self.guide_lbl.setText(
'Planning to the next position. Click on execute when a good one was found'
)
res = self.handeye_client.plan_to_selected_target_pose()
self.plan_was_successful = res.success
if self.plan_was_successful:
self.state = CalibrationMovementsGUI.GOOD_PLAN
else:
self.state = CalibrationMovementsGUI.BAD_PLAN
self.update_ui()
def handle_execute(self):
if self.plan_was_successful:
self.guide_lbl.setText('Going to the selected pose')
res = self.handeye_client.execute_plan()
if res.success:
self.state = CalibrationMovementsGUI.MOVED_TO_POSE
else:
self.state = CalibrationMovementsGUI.MOVEMENT_FAILED
self.update_ui()
class PingGUI(Plugin):
def __init__(self, context):
super(PingGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName("PingGUI")
self.msg = None
# Create a container widget and give it a layout
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._label = QLabel("xx ms latency")
p = self._label.palette()
p.setColor(self._label.backgroundRole(), Qt.red)
self._label.setPalette(p)
self._layout.addWidget(self._label)
self.set_bg_color(100, 100, 100)
rospy.Subscriber("/ping/delay", Float64, self.ping_cb)
context.add_widget(self._container)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_gui)
self._update_plot_timer.start(1)
def update_gui(self):
if not self.msg:
return
msg = self.msg
# msec
# 100 -> green, 1000 -> red
# normalize within 1000 ~ 100
orig_latency = msg.data
msg_data = orig_latency
if msg.data > 1000:
msg_data = 1000
elif msg.data < 100:
msg_data = 100
ratio = (msg_data - 100) / (1000 - 100)
color_r = ratio * 255.0
color_g = (1 - ratio) * 255.0
# devnull = open(os.devnull, "w")
# with RedirectStdStreams(stdout=devnull, stderr=devnull):
self.set_bg_color(color_r, color_g, 0)
self._label.setText("%d ms latency" % (orig_latency))
def set_bg_color(self, r, g, b):
self._label.setStyleSheet(
"QLabel { display: block; background-color: rgba(%d, %d, %d, 255); text-align: center; font-size: 30px;}"
% (r, g, b)
)
def ping_cb(self, msg):
self.msg = msg
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
def __init__(self, context):
super(JointControlWidget, self).__init__()
self.updateStateSignal.connect(self.on_update_state)
self.updateGhostSignal.connect(self.on_update_ghost)
self.joint_states = JointState()
self.ghost_joint_states = JointState()
self._widget = context
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget()
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radioGroup.addButton(self.radio_ghost_target, 0)
self.radio_ghost_target.setChecked(True)
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_robot_target, 1)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_robot_target)
hbox_radio.addStretch()
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
duration_box = QHBoxLayout()
duration_box.setAlignment(Qt.AlignLeft)
duration_box.addWidget(QLabel("Trajectory duration (s):"))
self.traj_duration_spin = QDoubleSpinBox()
self.traj_duration_spin.setValue(1.0)
self.traj_duration_spin.valueChanged.connect(self.on_traj_duration_changed)
duration_box.addWidget(self.traj_duration_spin)
self.update_controllers_buttonn = QPushButton("Update Controllers")
self.update_controllers_buttonn.pressed.connect(self.on_update_controllers)
duration_box.addWidget(self.update_controllers_buttonn)
vbox.addLayout(duration_box)
widget = QWidget()
hbox = QHBoxLayout()
# Left to right layout
self.joint_control = JointControl(self, hbox)
widget.setLayout(hbox)
vbox.addWidget(widget)
print "Add buttons to apply all ..."
all_widget = QWidget()
all_box = QHBoxLayout()
self.snap_to_ghost_button = QPushButton("SnapAllGhost")
self.snap_to_ghost_button.pressed.connect(self.on_snap_ghost_pressed)
all_box.addWidget(self.snap_to_ghost_button)
self.snap_to_current_button = QPushButton("SnapAllCurrent")
self.snap_to_current_button.pressed.connect(self.on_snap_current_pressed)
all_box.addWidget(self.snap_to_current_button)
self.apply_to_robot_button = QPushButton("ApplyAllRobot")
self.apply_to_robot_button.pressed.connect(self.on_apply_robot_pressed)
all_box.addWidget(self.apply_to_robot_button)
self.apply_to_robot_button = QPushButton("Apply WBC Robot")
self.apply_to_robot_button.pressed.connect(self.on_apply_wbc_robot_pressed)
all_box.addWidget(self.apply_to_robot_button)
all_widget.setLayout(all_box)
vbox.addWidget(all_widget)
override_box = QHBoxLayout()
self.override = QCheckBox()
self.override.setChecked(False)
self.override.stateChanged.connect(self.on_override_changed)
override_box.addWidget(self.override)
override_label = QLabel("SAFETY OVERRIDE")
override_label.setStyleSheet('QLabel { color: red }')
override_box.addWidget(override_label)
override_box.addStretch()
vbox.addLayout(override_box)
vbox.addStretch()
self._widget.setLayout(vbox)
self.first_time = True
self.stateSubscriber = rospy.Subscriber('/joint_states', JointState, self.state_callback_fnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghost_callback_fnc)
self.wbc_robot_pub = rospy.Publisher('/flor/wbc_controller/joint_states', JointState, queue_size=10)
self.time_last_update_state = time.time()
self.time_last_update_ghost = time.time()
