def __init__(self, context):
super(RescueControlDialog, self).__init__(context)
self.setObjectName('RescueControlDialog')
self.joint_states_pub={}
self._widget = QWidget()
vbox = QVBoxLayout()
#Add a Pushbutton for resetting the system state
resetPushButton = QPushButton("Reset State")
resetPushButton.pressed.connect(self.handleReset)
self._syscommand_pub = rospy.Publisher('syscommand', String)
vbox.addWidget(resetPushButton)
#Add a Pushbutton for adding a victim in front of the robot and projecting it against the wall
addVictimInFrontOfRobotPushButton = QPushButton("Add victim in front of robot")
addVictimInFrontOfRobotPushButton.pressed.connect(self.on_add_victim_in_front_of_robot_pressed)
vbox.addWidget(addVictimInFrontOfRobotPushButton)
#add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
def __init__(self, parent, fileName, top_widget_layout):
self.controllers = []
self.parent = parent
self.loadFile(fileName)
print "Initialize controllers..."
for controller in self.controllers:
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel);
frame.setFrameShadow(QFrame.Raised);
vbox = QVBoxLayout()
label = QLabel()
label.setText(controller.label)
vbox.addWidget(label);
print controller.name
for joint in controller.joints:
label = QLabel()
label.setText(joint.name)
vbox.addWidget(label);
#Add input for setting the biases
widget = QWidget()
hbox = QHBoxLayout()
hbox.addWidget(joint.sensor_bias_spinbox)
hbox.addWidget(joint.control_bias_spinbox)
hbox.addWidget(joint.gearing_bias_spinbox)
widget.setLayout(hbox)
vbox.addWidget(widget)
label = QLabel()
label.setText(" Sensor Control Gearing")
vbox.addWidget(label);
vbox.addStretch()
frame.setLayout(vbox)
top_widget_layout.addWidget(frame)
print "Done loading controllers"
def __init__(self, context):
super(StableStepDialog, self).__init__(context)
self.setObjectName('StableStepDialog')
self.bdi_state_msg = AtlasSimInterfaceState()
self.footstep_plan = rospy.Publisher('/flor/controller/footstep_plan',AtlasFootstepPlan, None, False, True, None, queue_size=10)
self.bdi_state_sub = rospy.Subscriber("/atlas/atlas_sim_interface_state", AtlasSimInterfaceState, self.simStateCallback)
self._widget = QWidget()
vbox = QVBoxLayout()
apply_command = QPushButton("Re-Center Steps")
apply_command.clicked.connect(self.apply_command_callback)
vbox.addWidget(apply_command)
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
def __init__(self, context):
super(SystemCommandDialog, self).__init__(context)
self.setObjectName('SystemCommandDialog')
self.sys_command_pub = rospy.Publisher('/syscommand',String, None, False, True, None, queue_size=10)
self._widget = QWidget()
vbox = QVBoxLayout()
reset_world_command = QPushButton("Reset World Model")
reset_world_command.clicked.connect(self.apply_reset_world_command_callback)
vbox.addWidget(reset_world_command)
save_octomap = QPushButton("Save Octomap")
save_octomap.clicked.connect(self.apply_save_octomap_command_callback)
vbox.addWidget(save_octomap)
save_pointcloud = QPushButton("Save Pointcloud")
save_pointcloud.clicked.connect(self.apply_save_pointcloud_command_callback)
vbox.addWidget(save_pointcloud)
save_image_left_eye = QPushButton("Save Image Head")
save_image_left_eye.clicked.connect(self.apply_save_image_left_eye_command_callback)
vbox.addWidget(save_image_left_eye)
save_image_left_hand = QPushButton("Save Left Hand Image")
save_image_left_hand.clicked.connect(self.apply_save_image_left_hand_command_callback)
vbox.addWidget(save_image_left_hand)
save_image_right_hand = QPushButton("Save Right Hand Image")
save_image_right_hand.clicked.connect(self.apply_save_image_right_hand_command_callback)
vbox.addWidget(save_image_right_hand)
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
def __init__(self, main, fileName, main_hbox):
self.main = main
self.chain = []
self.poses = []
self.chain_name = ""
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel);
frame.setFrameShadow(QFrame.Raised);
vbox = QVBoxLayout()
label = QLabel()
vbox.addWidget(label);
self.load_file(fileName, vbox)
self.trajectoryPublisher = rospy.Publisher('/trajectory_controllers/'+self.chain_name+'_traj_controller/trajectory' ,JointTrajectory, queue_size=10)
self.positionPublisher = rospy.Publisher('/trajectory_controllers/'+self.chain_name+'_traj_controller/joint_states',JointState, queue_size=10)
self.ghostPublisher = rospy.Publisher('/flor/ghost/set_joint_states',JointState, queue_size=10)
label.setText(self.chain_name)
vbox.addStretch(1)
frame.setLayout(vbox)
main_hbox.addWidget(frame)
def __init__(self, context):
super(SinusoidalTrajectoryDialog, self).__init__(context)
self.setObjectName('SinusoidalTrajectoryDialog')
#self.updateStateSignal = Signal(object)
self.updateStateSignal.connect(self.on_updateState)
self.robot = URDF.from_parameter_server()
self.joint_list = {}
for ndx,jnt in enumerate(self.robot.joints):
self.joint_list[jnt.name] = ndx
#print jnt.name, " ",self.joint_list[jnt.name]
self.chain=[]
self.chain_file = rospy.get_param("chain_file")
self.chain_name = rospy.get_param("chain_name")
yaml_file = self.chain_file+self.chain_name+"_chain.yaml"
print yaml_file
#define structures
self.robot_state = JointState()
self.robot_command = JointTrajectory()
stream = open(yaml_file, "r")
jointChain = yaml.load_all(stream)
print '\n\n'
for ndx, data in enumerate(jointChain):
print ndx," : ", data
self.delay_time = data["delay_time"]
self.amplitude = data["amplitude"]
self.frequency = data["frequency"]
self.frequency_limit = data["frequency_limit"]
self.iterations = data["iterations"]
self.joint_state_topic = data["joint_state_topic"]
self.trajectory_topic = data["trajectory_topic"]
joints = rospy.get_param(data["chain_param_name"])
for joint in joints:
print joint
self.robot_state.name.append(joint)
self.chain.append(JointData(self, joint) )
self.robot_command.joint_names = self.robot_state.name
stream.close()
self.robot_state.position = [0.0]*len(self.robot_state.name)
self.robot_state.velocity = [0.0]*len(self.robot_state.name)
self.robot_state.effort = [0.0]*len(self.robot_state.name)
self.robot_joint_state = JointState()
print "delay_time =",self.delay_time
print "amplitude =",self.amplitude
print "frequency =",self.frequency
print "iterations =",self.iterations
print "Robot State Structure",self.robot_state
print "Robot Command Structure",self.robot_command
# initialize structure to hold widget handles
self.cur_position_spinbox=[]
self.amplitude_spinbox=[]
self.frequency_spinbox=[]
self.iterations_spinbox=[]
self._widget = QWidget()
vbox = QVBoxLayout()
# Push buttons
hbox = QHBoxLayout()
snap_command = QPushButton("Snap Position")
snap_command.clicked.connect(self.snap_current_callback)
hbox.addWidget(snap_command)
check_limits = QPushButton("Check Limits Gains")
check_limits.clicked.connect(self.check_limits_callback)
hbox.addWidget(check_limits)
apply_command = QPushButton("Send Trajectory")
apply_command.clicked.connect(self.apply_command_callback)
hbox.addWidget(apply_command)
save_trajectory = QPushButton("Save Trajectory")
save_trajectory.clicked.connect(self.save_trajectory_callback)
hbox.addWidget(save_trajectory)
zero_ramp = QPushButton("Zero Values")
zero_ramp.clicked.connect(self.zero_values_callback)
hbox.addWidget(zero_ramp)
vbox.addLayout(hbox)
time_hbox = QHBoxLayout()
vbox_frequqency = QVBoxLayout()
vbox_frequqency.addWidget(QLabel("Frequency"))
self.frequency_spinbox = QDoubleSpinBox()
self.frequency_spinbox.setDecimals(5)
self.frequency_spinbox.setRange(0, self.frequency_limit)
self.frequency_spinbox.setSingleStep(0.05)
self.frequency_spinbox.valueChanged.connect(self.on_frequency_value)
self.frequency_spinbox.setValue(self.frequency)
vbox_frequqency.addWidget(self.frequency_spinbox)
time_hbox.addLayout(vbox_frequqency)
vbox_iterations = QVBoxLayout()
vbox_iterations.addWidget(QLabel("Iterations"))
self.iterations_spinbox = QDoubleSpinBox()
self.iterations_spinbox.setDecimals(5)
self.iterations_spinbox.setRange(0, 10)
self.iterations_spinbox.setSingleStep(1)
self.iterations_spinbox.valueChanged.connect(self.on_iterations_value)
self.iterations_spinbox.setValue(self.iterations)
vbox_iterations.addWidget(self.iterations_spinbox)
time_hbox.addLayout(vbox_iterations)
vbox.addLayout(time_hbox)
# Joints title
title_frame = QFrame()
title_frame.setFrameShape(QFrame.StyledPanel);
title_frame.setFrameShadow(QFrame.Raised);
title_hbox = QHBoxLayout()
title_hbox.addWidget(QLabel("Joints"))
title_hbox.addWidget(QLabel("Current Position"))
title_hbox.addWidget(QLabel("Amplitude"))
title_frame.setLayout(title_hbox)
vbox.addWidget(title_frame)
# Define the widgets for each joint
for i,joint in enumerate(self.chain):
#print i,",",joint
self.joint_widget( vbox, i)
#add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
# Define the connections to the outside world
self.jointSubscriber = rospy.Subscriber(self.joint_state_topic, JointState, self.stateCallbackFnc)
self.commandPublisher = rospy.Publisher(self.trajectory_topic , JointTrajectory, queue_size=10)
# Add the widget
context.add_widget(self._widget)
def __init__(self, context, add_execute_widget = True):
super(StepInterfaceWidget, self).__init__()
# init signal mapper
self.command_mapper = QSignalMapper(self)
self.command_mapper.mapped.connect(self._publish_step_plan_request)
# start widget
widget = context
error_status_widget = QErrorStatusWidget()
self.logger = Logger(error_status_widget)
vbox = QVBoxLayout()
# start control box
controls_hbox = QHBoxLayout()
# left coloumn
left_controls_vbox = QVBoxLayout()
left_controls_vbox.setMargin(0)
self.add_command_button(left_controls_vbox, "Rotate Left", PatternParameters.ROTATE_LEFT)
self.add_command_button(left_controls_vbox, "Strafe Left", PatternParameters.STRAFE_LEFT)
self.add_command_button(left_controls_vbox, "Step Up", PatternParameters.STEP_UP)
self.add_command_button(left_controls_vbox, "Center on Left", PatternParameters.FEET_REALIGN_ON_LEFT)
left_controls_vbox.addStretch()
controls_hbox.addLayout(left_controls_vbox, 1)
# center coloumn
center_controls_vbox = QVBoxLayout()
center_controls_vbox.setMargin(0)
self.add_command_button(center_controls_vbox, "Forward", PatternParameters.FORWARD)
self.add_command_button(center_controls_vbox, "Backward", PatternParameters.BACKWARD)
self.add_command_button(center_controls_vbox, "Step Over", PatternParameters.STEP_OVER)
self.add_command_button(center_controls_vbox, "Center Feet", PatternParameters.FEET_REALIGN_ON_CENTER)
self.add_command_button(center_controls_vbox, "Wide Stance", PatternParameters.WIDE_STANCE)
center_controls_vbox.addStretch()
controls_hbox.addLayout(center_controls_vbox, 1)
# right coloumn
right_controls_vbox = QVBoxLayout()
right_controls_vbox.setMargin(0)
self.add_command_button(right_controls_vbox, "Rotate Right", PatternParameters.ROTATE_RIGHT)
self.add_command_button(right_controls_vbox, "Strafe Right", PatternParameters.STRAFE_RIGHT)
self.add_command_button(right_controls_vbox, "Step Down", PatternParameters.STEP_DOWN)
self.add_command_button(right_controls_vbox, "Center on Right", PatternParameters.FEET_REALIGN_ON_RIGHT)
right_controls_vbox.addStretch()
controls_hbox.addLayout(right_controls_vbox, 1)
# end control box
add_layout_with_frame(vbox, controls_hbox, "Commands:")
# start settings
settings_hbox = QHBoxLayout()
settings_hbox.setMargin(0)
# start left column
left_settings_vbox = QVBoxLayout()
left_settings_vbox.setMargin(0)
# frame id
self.frame_id_line_edit = QLineEdit("/world")
add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit, "Frame ID:")
# do closing step
self.close_step_checkbox = QCheckBox()
self.close_step_checkbox.setText("Do closing step")
self.close_step_checkbox.setChecked(True)
left_settings_vbox.addWidget(self.close_step_checkbox)
# extra seperation
self.extra_seperation_checkbox = QCheckBox()
self.extra_seperation_checkbox.setText("Extra Seperation")
self.extra_seperation_checkbox.setChecked(False)
left_settings_vbox.addWidget(self.extra_seperation_checkbox)
left_settings_vbox.addStretch()
# number of steps
self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.step_number, "Number Steps:")
# start step index
self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.start_step_index, "Start Step Index:")
# end left column
settings_hbox.addLayout(left_settings_vbox, 1)
# start center column
center_settings_vbox = QVBoxLayout()
center_settings_vbox.setMargin(0)
# start foot selection
self.start_foot_selection_combo_box = QComboBox()
self.start_foot_selection_combo_box.addItem("AUTO")
self.start_foot_selection_combo_box.addItem("LEFT")
self.start_foot_selection_combo_box.addItem("RIGHT")
add_widget_with_frame(center_settings_vbox, self.start_foot_selection_combo_box, "Start foot selection:")
center_settings_vbox.addStretch()
# step Distance
self.step_distance = generate_q_double_spin_box(0.20, 0.0, 0.5, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.step_distance, "Step Distance (m):")
# side step distance
self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.side_step, "Side Step (m):")
# rotation per step
self.step_rotation = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(center_settings_vbox, self.step_rotation, "Step Rotation (deg):")
# end center column
settings_hbox.addLayout(center_settings_vbox, 1)
# start right column
right_settings_vbox = QVBoxLayout()
right_settings_vbox.setMargin(0)
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
# use terrain model
self.use_terrain_model_checkbox = QCheckBox()
self.use_terrain_model_checkbox.setText("Use Terrain Model")
self.use_terrain_model_checkbox.setChecked(False)
self.use_terrain_model_checkbox.stateChanged.connect(self.use_terrain_model_changed)
right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
# override mode
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_settings_vbox.addWidget(self.override_checkbox)
right_settings_vbox.addStretch()
# delta z
self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
add_widget_with_frame(right_settings_vbox, self.dz, "delta z per step (m):")
# end right column
settings_hbox.addLayout(right_settings_vbox, 1)
# end settings
add_layout_with_frame(vbox, settings_hbox, "Settings:")
# parameter set selection
self.parameter_set_widget = QParameterSetWidget(logger = self.logger)
add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")
# execute option
if add_execute_widget:
add_widget_with_frame(vbox, QExecuteStepPlanWidget(logger = self.logger, step_plan_topic = "step_plan"), "Execute:")
# add error status widget
add_widget_with_frame(vbox, error_status_widget, "Status:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init widget
self.parameter_set_widget.param_cleared_signal.connect(self.param_cleared)
self.parameter_set_widget.param_changed_signal.connect(self.param_selected)
self.commands_set_enabled(False)
# subscriber
self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet, self.set_start_feet_callback)
# publisher
self.step_plan_pub = rospy.Publisher("step_plan", StepPlan, queue_size=1)
# action clients
self.step_plan_request_client = actionlib.SimpleActionClient("step_plan_request", StepPlanRequestAction)
def __init__(self, context):
#super(PositionModeDialog, self).__init__(context)
#self.setObjectName('PositionModeDialog')
super(PositionModeWidget, self).__init__()
self.name = 'PositionModeWidget'
self.updateStateSignal.connect(self.on_updateState)
self.updatePelvisSignal.connect(self.on_updatePelvis)
self.updateGhostSignal.connect(self.on_updateGhost)
## Process standalone plugin command-line arguments
#from argparse import ArgumentParser
#parser = ArgumentParser()
## Add argument(s) to the parser.
#parser.add_argument("-f", "--file",
# dest="file",
# help="Input file name")
#args, unknowns = parser.parse_known_args(context.argv())
#print 'arguments: ', args
#print 'unknowns: ', unknowns
#
#if ((args.file == "") or (args.file == None)):
# print "No file specified - abort!"
# sys.exit(-1)
# Define how long the trajectories will take to execute
self.time_factor = rospy.get_param('~time_factor', 2.1)
print "Using ",self.time_factor, " as the time factor for all trajectories"
# Joint Name : Child Link :OSRF
self.joint_names=[]
self.joint_names.append('back_bkz') # : ltorso : 0
self.joint_names.append('back_bky') # : mtorso : 1
self.joint_names.append('back_bkx') # : utorso : 2
self.joint_names.append('neck_ry') # : head : 3
self.joint_names.append('l_leg_hpz') # : l_uglut : 4
self.joint_names.append('l_leg_hpx') # : l_lglut : 5
self.joint_names.append('l_leg_hpy') # : l_uleg : 6
self.joint_names.append('l_leg_kny') # : l_lleg : 7
self.joint_names.append('l_leg_aky') # : l_talus : 8
self.joint_names.append('l_leg_akx') # : l_foot : 9
self.joint_names.append('r_leg_hpz') # : r_uglut : 10
self.joint_names.append('r_leg_hpx') # : r_lglut : 11
self.joint_names.append('r_leg_hpy') # : r_uleg : 12
self.joint_names.append('r_leg_kny') # : r_lleg : 13
self.joint_names.append('r_leg_aky') # : r_talus : 14
self.joint_names.append('r_leg_akx') # : r_foot : 15
self.joint_names.append('l_arm_shz') # : l_clav : 16
self.joint_names.append('l_arm_shx') # : l_scap : 17
self.joint_names.append('l_arm_ely') # : l_uarm : 18
self.joint_names.append('l_arm_elx') # : l_larm : 19
self.joint_names.append('l_arm_wry') # : l_farm : 20
self.joint_names.append('l_arm_wrx') # : l_hand : 21
self.joint_names.append('l_arm_wry2') # : l_farm : 22
self.joint_names.append('r_arm_shz') # : r_clav : 23
self.joint_names.append('r_arm_shx') # : r_scap : 24
self.joint_names.append('r_arm_ely') # : r_uarm : 25
self.joint_names.append('r_arm_elx') # : r_larm : 26
self.joint_names.append('r_arm_wry') # : r_farm : 27
self.joint_names.append('r_arm_wrx') # : r_hand : 28
self.joint_names.append('r_arm_wry2') # : r_farm : 29
self.joint_states = JointState()
self.pelvis_names=[]
self.joint_names.append('forward') # : 30
self.joint_names.append('lateral') # : 31
self.joint_names.append('height') # : 32
self.joint_names.append('roll') # : 33
self.joint_names.append('pitch') # : 34
self.joint_names.append('yaw') # : 35
self.pelvis_states = JointState()
self.pelvis_states.position = [0.0, 0.0, 0.85, 0.0, 0.0, 0.0] # default pelvis pose
#self._widget = QWidget()
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.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_ghost_target,0)
self.radioGroup.addButton(self.radio_robot_target,1)
self.radio_ghost_target.setChecked(True)
self.commandGroup = QButtonGroup()
self.commandGroup.setExclusive(True)
self.radio_position_command = QRadioButton()
self.radio_position_command.setText("Position")
self.commandGroup.addButton(self.radio_position_command,0)
self.radio_trajectory_command = QRadioButton()
self.radio_trajectory_command.setText("Trajectory")
self.commandGroup.addButton(self.radio_trajectory_command,1)
self.radio_trajectory_command.setChecked(True)
#print "position button is checked: ",self.radio_position_command.isChecked()
#print "trajectory button is checked: ",self.radio_trajectory_command.isChecked()
print "Default group button checked is ",self.radioGroup.checkedId()
print "Default command button checked is ",self.commandGroup.checkedId()
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
#hbox_radio.addWidget(QLabel("Ghost"))
hbox_radio.addStretch()
vbox_robot = QVBoxLayout()
widget_robot = QWidget()
widget_robot_sel = QWidget()
hbox_sel = QHBoxLayout()
hbox_radio.addStretch()
hbox_sel.addWidget(self.radio_robot_target)
#hbox_sel.addWidget(QLabel("Robot"))
hbox_radio.addStretch()
widget_robot_sel.setLayout(hbox_sel)
vbox_robot.addWidget(widget_robot_sel);
widget_cmd = QWidget()
hbox_cmd = QHBoxLayout()
hbox_radio.addStretch()
hbox_cmd.addWidget(self.radio_position_command)
#hbox_cmd.addWidget(QLabel("Position"))
hbox_radio.addStretch()
hbox_cmd.addWidget(self.radio_trajectory_command)
#hbox_cmd.addWidget(QLabel("Trajectory"))
hbox_radio.addStretch()
widget_cmd.setLayout(hbox_cmd)
vbox_robot.addWidget(widget_cmd);
widget_robot.setLayout(vbox_robot)
hbox_radio.addWidget(widget_robot)
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
positions_widget = QWidget()
hbox = QHBoxLayout()
#self.robot = URDF.from_parameter_server()
self.chains=[]
# Subscribe to Joint States update /atlas/atlas_state
#print "Load parameters"
#print rospy.get_param_names()
#fileName = rospy.get_param('positions_file', '/opt/vigir/catkin_ws/src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch/r_arm_positions.txt')
#print "FileName:",fileName
# Left to right layout
numFiles = rospy.get_param("~num_files");
for i in range(1,numFiles+1):
path = "~position_files/file_" + str(i)
foobar = str(rospy.get_param(path))
self.chains.append(ChainData(self, foobar, hbox))
#add stretch at end so all GUI elements are at left of dialog
hbox.addStretch(1)
positions_widget.setLayout(hbox)
vbox.addWidget(positions_widget)
vbox.addStretch(1)
self._widget.setLayout(vbox)
#context.add_widget(self._widget)
self.stateSubscriber = rospy.Subscriber('/atlas/joint_states', JointState, self.stateCallbackFnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghostCallbackFnc)
self.pelvisSubscriber = rospy.Subscriber('/robot_controllers/pelvis_traj_controller/current_states',JointState, self.pelvisCallbackFnc)
def __init__(self, context, add_execute_widget=True):
super(StepInterfaceWidget, self).__init__()
# init signal mapper
self.command_mapper = QSignalMapper(self)
self.command_mapper.mapped.connect(self._publish_step_plan_request)
# start widget
widget = context
error_status_widget = QErrorStatusWidget()
self.logger = Logger(error_status_widget)
vbox = QVBoxLayout()
# start control box
controls_hbox = QHBoxLayout()
# left coloumn
left_controls_vbox = QVBoxLayout()
left_controls_vbox.setMargin(0)
self.add_command_button(left_controls_vbox, "Rotate Left",
PatternParameters.ROTATE_LEFT)
self.add_command_button(left_controls_vbox, "Strafe Left",
PatternParameters.STRAFE_LEFT)
self.add_command_button(left_controls_vbox, "Step Up",
PatternParameters.STEP_UP)
self.add_command_button(left_controls_vbox, "Center on Left",
PatternParameters.FEET_REALIGN_ON_LEFT)
left_controls_vbox.addStretch()
controls_hbox.addLayout(left_controls_vbox, 1)
# center coloumn
center_controls_vbox = QVBoxLayout()
center_controls_vbox.setMargin(0)
self.add_command_button(center_controls_vbox, "Forward",
PatternParameters.FORWARD)
self.add_command_button(center_controls_vbox, "Backward",
PatternParameters.BACKWARD)
self.add_command_button(center_controls_vbox, "Step Over",
PatternParameters.STEP_OVER)
self.add_command_button(center_controls_vbox, "Center Feet",
PatternParameters.FEET_REALIGN_ON_CENTER)
self.add_command_button(center_controls_vbox, "Wide Stance",
PatternParameters.WIDE_STANCE)
center_controls_vbox.addStretch()
controls_hbox.addLayout(center_controls_vbox, 1)
# right coloumn
right_controls_vbox = QVBoxLayout()
right_controls_vbox.setMargin(0)
self.add_command_button(right_controls_vbox, "Rotate Right",
PatternParameters.ROTATE_RIGHT)
self.add_command_button(right_controls_vbox, "Strafe Right",
PatternParameters.STRAFE_RIGHT)
self.add_command_button(right_controls_vbox, "Step Down",
PatternParameters.STEP_DOWN)
self.add_command_button(right_controls_vbox, "Center on Right",
PatternParameters.FEET_REALIGN_ON_RIGHT)
right_controls_vbox.addStretch()
controls_hbox.addLayout(right_controls_vbox, 1)
# end control box
add_layout_with_frame(vbox, controls_hbox, "Commands:")
# start settings
settings_hbox = QHBoxLayout()
settings_hbox.setMargin(0)
# start left column
left_settings_vbox = QVBoxLayout()
left_settings_vbox.setMargin(0)
# frame id
self.frame_id_line_edit = QLineEdit("/world")
add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit,
"Frame ID:")
# do closing step
self.close_step_checkbox = QCheckBox()
self.close_step_checkbox.setText("Do closing step")
self.close_step_checkbox.setChecked(True)
left_settings_vbox.addWidget(self.close_step_checkbox)
# extra seperation
self.extra_seperation_checkbox = QCheckBox()
self.extra_seperation_checkbox.setText("Extra Seperation")
self.extra_seperation_checkbox.setChecked(False)
left_settings_vbox.addWidget(self.extra_seperation_checkbox)
left_settings_vbox.addStretch()
# number of steps
self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.step_number,
"Number Steps:")
# start step index
self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.start_step_index,
"Start Step Index:")
# end left column
settings_hbox.addLayout(left_settings_vbox, 1)
# start center column
center_settings_vbox = QVBoxLayout()
center_settings_vbox.setMargin(0)
# start foot selection
self.start_foot_selection_combo_box = QComboBox()
self.start_foot_selection_combo_box.addItem("AUTO")
self.start_foot_selection_combo_box.addItem("LEFT")
self.start_foot_selection_combo_box.addItem("RIGHT")
add_widget_with_frame(center_settings_vbox,
self.start_foot_selection_combo_box,
"Start foot selection:")
center_settings_vbox.addStretch()
# step Distance
self.step_distance = generate_q_double_spin_box(0.0, 0.0, 0.5, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.step_distance,
"Step Distance (m):")
# side step distance
self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.side_step,
"Side Step (m):")
# rotation per step
self.step_rotation = generate_q_double_spin_box(
0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(center_settings_vbox, self.step_rotation,
"Step Rotation (deg):")
# end center column
settings_hbox.addLayout(center_settings_vbox, 1)
# start right column
right_settings_vbox = QVBoxLayout()
right_settings_vbox.setMargin(0)
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
# use terrain model
self.use_terrain_model_checkbox = QCheckBox()
self.use_terrain_model_checkbox.setText("Use Terrain Model")
self.use_terrain_model_checkbox.setChecked(False)
self.use_terrain_model_checkbox.stateChanged.connect(
self.use_terrain_model_changed)
right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
# override mode
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_settings_vbox.addWidget(self.override_checkbox)
right_settings_vbox.addStretch()
# delta z
self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
add_widget_with_frame(right_settings_vbox, self.dz,
"delta z per step (m):")
# end right column
settings_hbox.addLayout(right_settings_vbox, 1)
# end settings
add_layout_with_frame(vbox, settings_hbox, "Settings:")
# parameter set selection
self.parameter_set_widget = QParameterSetWidget(logger=self.logger)
add_widget_with_frame(vbox, self.parameter_set_widget,
"Parameter Set:")
# execute option
if add_execute_widget:
add_widget_with_frame(
vbox,
QExecuteStepPlanWidget(logger=self.logger,
step_plan_topic="step_plan"),
"Execute:")
# add error status widget
add_widget_with_frame(vbox, error_status_widget, "Status:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init widget
self.parameter_set_widget.param_cleared_signal.connect(
self.param_cleared)
self.parameter_set_widget.param_changed_signal.connect(
self.param_selected)
self.commands_set_enabled(False)
# subscriber
self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet,
self.set_start_feet_callback)
# publisher
self.step_plan_pub = rospy.Publisher("step_plan",
StepPlan,
queue_size=1)
# action clients
self.step_plan_request_client = actionlib.SimpleActionClient(
"step_plan_request", StepPlanRequestAction)
def create_controller_ui(self):
self.controllers = []
if not self.load_robot_model():
# if no groups config is on the parameter server, request the list from controller manager
self.load_controllers()
robot = URDF.from_parameter_server()
joint_list = {}
for ndx, jnt in enumerate(robot.joints):
joint_list[jnt.name] = ndx
for controller in self.controllers:
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel)
frame.setFrameShadow(QFrame.Raised)
vbox = QVBoxLayout()
label = QLabel()
label.setText(controller.label)
vbox.addWidget(label)
controller.snap_to_ghost_button = QPushButton("SnapGhost")
controller.snap_to_ghost_button.pressed.connect(controller.on_snap_ghost_pressed)
vbox.addWidget(controller.snap_to_ghost_button)
controller.snap_to_current_button = QPushButton("SnapCurrent")
controller.snap_to_current_button.pressed.connect(controller.on_snap_current_pressed)
vbox.addWidget(controller.snap_to_current_button)
controller.apply_to_robot_button = QPushButton("ApplyRobot")
controller.apply_to_robot_button.pressed.connect(controller.on_apply_robot_pressed)
vbox.addWidget(controller.apply_to_robot_button)
# Removed because it is hardcoded
# controller.save_joints_to_file_button = QPushButton("SaveJoints")
# controller.save_joints_to_file_button.pressed.connect(controller.on_save_joints_pressed)
# vbox.addWidget(controller.save_joints_to_file_button)
controller.undo_last_action_button = QPushButton("Undo Last")
controller.undo_last_action_button.pressed.connect(controller.on_undo_pressed)
vbox.addWidget(controller.undo_last_action_button)
print 'Loading limits for controller:', controller.topic
for joint in controller.joints:
label = QLabel()
label.setText(joint.name)
vbox.addWidget(label)
try:
robot_joint = robot.joints[joint_list[joint.name]]
except KeyError:
print 'No limits found for', joint.name
limit_lower = -1.0
limit_upper = 1
else:
limit_lower = robot_joint.limit.lower
limit_upper = robot_joint.limit.upper
print " ", joint.name, " limits(", limit_lower, ", ", limit_upper, ") num"
joint.slider = QSlider(Qt.Horizontal)
joint.slider.setRange(int(limit_lower * 10000.0), int(limit_upper * 10000.0))
joint.slider.setValue(int(limit_lower * 10000.0))
joint.slider.setSingleStep((limit_upper - limit_lower) / 20.0)
joint.slider.valueChanged.connect(joint.on_slider_moved)
vbox.addWidget(joint.slider)
joint.progress_bar = QProgressBar()
joint.progress_bar.setRange(int(limit_lower * 10000.0),
int(limit_upper * 10000.0))
joint.progress_bar.setValue(int(limit_lower * 10000.0))
vbox.addWidget(joint.progress_bar)
vbox.addStretch()
frame.setLayout(vbox)
self.widget.addWidget(frame)
def __init__(self, context):
super(PatternGeneratorWidget, self).__init__()
# publisher
self.pattern_generator_params_pub = rospy.Publisher('pattern_generator/set_params', PatternGeneratorParameters, queue_size = 1)
# start widget
widget = context
# start upper part
hbox = QHBoxLayout()
# start left column
left_vbox = QVBoxLayout()
# start button
start_command = QPushButton("Start")
start_command.clicked.connect(self.start_command_callback)
left_vbox.addWidget(start_command)
# simulation checkbox
self.simulation_mode_checkbox = QCheckBox()
self.simulation_mode_checkbox.setText("Simulation Mode")
self.simulation_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.simulation_mode_checkbox)
# realtime checkbox
self.realtime_mode_checkbox = QCheckBox()
self.realtime_mode_checkbox.setText("Realtime Mode")
self.realtime_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.realtime_mode_checkbox)
# joystick checkbox
self.joystick_mode_checkbox = QCheckBox()
self.joystick_mode_checkbox.setText("Joystick Mode")
self.joystick_mode_checkbox.setChecked(False)
self.joystick_mode_checkbox.clicked.connect(self.joystick_mode_check_callback)
left_vbox.addWidget(self.joystick_mode_checkbox)
# foot seperation
self.foot_seperation = generate_q_double_spin_box(0.2, 0.15, 0.3, 2, 0.01)
self.foot_seperation.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.foot_seperation, "Foot Seperation (m):")
# delta x
self.delta_x = generate_q_double_spin_box(0.0, -0.4, 0.4, 2, 0.01)
self.delta_x.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_x, "dX (m):")
# delta y
self.delta_y = generate_q_double_spin_box(0.0, -2.2, 2.2, 2, 0.01)
self.delta_y.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_y, "dY (m):")
# delta yaw
self.delta_yaw = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.delta_yaw.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_yaw, "dYaw (deg):")
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.roll.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.pitch.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.pitch, "Pitch (deg):")
# end left column
left_vbox.addStretch()
hbox.addLayout(left_vbox, 1)
# start right column
right_vbox = QVBoxLayout()
# stop button
stop_command = QPushButton("Stop")
stop_command.clicked.connect(self.stop_command_callback)
right_vbox.addWidget(stop_command)
# ignore collision
self.collision_checkbox = QCheckBox()
self.collision_checkbox.setText("Ignore Collision")
self.collision_checkbox.setChecked(True)
right_vbox.addWidget(self.collision_checkbox)
# override 3D
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_vbox.addWidget(self.override_checkbox)
# end right coloumn
right_vbox.addStretch()
hbox.addLayout(right_vbox, 1)
# add upper part
hbox.setMargin(0)
vbox = QVBoxLayout()
vbox.addLayout(hbox)
# parameter set selection
self.parameter_set_widget = QParameterSetWidget()
add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")
# end widget
widget.setLayout(vbox)
def __init__(self, context):
#super(FootstepParamControlDialog, self).__init__(context)
#self.setObjectName('FootstepParamControlDialog')
super(FootstepParamControlWidget, self).__init__()
self.name = 'FootstepParamControlWidget'
#self._widget = QWidget()
self._widget = context
vbox = QVBoxLayout()
### STEP_COST_ESTIMATOR ########
self.step_cost_vbox = QVBoxLayout()
self.step_cost_groupbox = QGroupBox( "Step Cost Estimator" )
self.step_cost_groupbox.setCheckable( True )
self.step_cost_groupbox.setChecked(False)
self.step_cost_combobox = QComboBox()
self.step_cost_combobox.addItem( "Euclidean" )
self.step_cost_combobox.addItem( "GPR" )
self.step_cost_combobox.addItem( "Map" )
self.step_cost_combobox.addItem( "Boundary" )
self.step_cost_combobox.addItem( "Dynamics" )
self.step_cost_vbox.addWidget( self.step_cost_combobox )
self.step_cost_groupbox.setLayout( self.step_cost_vbox )
vbox.addWidget( self.step_cost_groupbox )
# #HARD ### FOOTSTEP_SET ########
# # parameters for discret footstep planning mode
# vigir_atlas_control_msgs/VigirBehaviorStepData[] footstep_set # set of footsteps (displacement vectors (in meter / rad))
# float32[] footstep_cost # cost for each footstep given in footstep set
#
# #HARD ### LOAD_GPR_STEP_COST ########
# # map step cost file
# std_msgs/String map_step_cost_file
#
# #HARD ### LOAD_MAP_STEP_COST ########
# # destination of gpr file
# std_msgs/String gpr_step_cost_file
### COLLISION_CHECK_TYPE ########
self.collision_check_groupbox = QGroupBox( "Collision Check Type" )
self.collision_check_groupbox.setCheckable( True )
self.collision_check_groupbox.setChecked( False)
self.collision_check_vbox = QVBoxLayout()
self.collision_check_feet_checkbox = QCheckBox( "Feet" )
self.collision_check_vbox.addWidget( self.collision_check_feet_checkbox )
self.collision_check_ub_checkbox = QCheckBox( "Upper Body" )
self.collision_check_vbox.addWidget( self.collision_check_ub_checkbox )
self.collision_check_fc_checkbox = QCheckBox( "Foot Contact Support" )
self.collision_check_vbox.addWidget( self.collision_check_fc_checkbox )
self.collision_check_groupbox.setLayout( self.collision_check_vbox )
vbox.addWidget( self.collision_check_groupbox )
### FOOT_SIZE ########
self.foot_size_vbox = QVBoxLayout()
self.foot_size_groupbox = QGroupBox( "Foot Size" )
self.foot_size_groupbox.setCheckable( True )
self.foot_size_groupbox.setChecked( False )
self.foot_size_hbox = QHBoxLayout()
self.foot_size_label = QLabel("Foot Size")
self.foot_size_hbox.addWidget( self.foot_size_label )
self.foot_size_x = QDoubleSpinBox()
self.foot_size_x.setSingleStep(.01)
self.foot_size_hbox.addWidget(self.foot_size_x)
self.foot_size_y = QDoubleSpinBox()
self.foot_size_y.setSingleStep(.01)
self.foot_size_hbox.addWidget(self.foot_size_y)
self.foot_size_z = QDoubleSpinBox()
self.foot_size_z.setSingleStep(.01)
self.foot_size_hbox.addWidget(self.foot_size_z)
self.foot_size_vbox.addLayout( self.foot_size_hbox )
self.foot_shift_hbox = QHBoxLayout()
self.foot_shift_label = QLabel("Foot Origin Shift")
self.foot_shift_hbox.addWidget( self.foot_shift_label )
self.foot_shift_x = QDoubleSpinBox()
self.foot_shift_x.setRange(-1.0, 1.0)
self.foot_shift_x.setSingleStep(.01)
self.foot_shift_hbox.addWidget(self.foot_shift_x)
self.foot_shift_y = QDoubleSpinBox()
self.foot_shift_y.setRange(-1.0, 1.0)
self.foot_shift_y.setSingleStep(.01)
self.foot_shift_hbox.addWidget(self.foot_shift_y)
self.foot_shift_z = QDoubleSpinBox()
self.foot_shift_z.setRange(-1.0, 1.0)
self.foot_shift_z.setSingleStep(.01)
self.foot_shift_hbox.addWidget(self.foot_shift_z)
self.foot_size_vbox.addLayout( self.foot_shift_hbox )
self.foot_size_groupbox.setLayout( self.foot_size_vbox )
vbox.addWidget( self.foot_size_groupbox )
### UPPER_BODY_SIZE ########
self.upper_vbox = QVBoxLayout()
self.upper_groupbox = QGroupBox( "Upper Body Size" )
self.upper_groupbox.setCheckable( True )
self.upper_groupbox.setChecked( False )
self.upper_size_hbox = QHBoxLayout()
self.upper_size_label = QLabel("Upper Body Size")
self.upper_size_hbox.addWidget( self.upper_size_label )
self.upper_size_x = QDoubleSpinBox()
self.upper_size_x.setSingleStep(.01)
self.upper_size_hbox.addWidget(self.upper_size_x)
self.upper_size_y = QDoubleSpinBox()
self.upper_size_y.setSingleStep(.01)
self.upper_size_hbox.addWidget(self.upper_size_y)
self.upper_size_z = QDoubleSpinBox()
self.upper_size_z.setSingleStep(.01)
self.upper_size_hbox.addWidget(self.upper_size_z)
self.upper_vbox.addLayout( self.upper_size_hbox )
self.upper_origin_hbox = QHBoxLayout()
self.upper_origin_label = QLabel("Upper Body Origin")
self.upper_origin_hbox.addWidget( self.upper_origin_label )
self.upper_origin_x = QDoubleSpinBox()
self.upper_origin_x.setRange(-1.0, 1.0)
self.upper_origin_x.setSingleStep(.01)
self.upper_origin_hbox.addWidget(self.upper_origin_x)
self.upper_origin_y = QDoubleSpinBox()
self.upper_origin_y.setRange(-1.0, 1.0)
self.upper_origin_y.setSingleStep(.01)
self.upper_origin_hbox.addWidget(self.upper_origin_y)
self.upper_origin_z = QDoubleSpinBox()
self.upper_origin_z.setRange(-1.0, 1.0)
self.upper_origin_z.setSingleStep(.01)
self.upper_origin_hbox.addWidget(self.upper_origin_z)
self.upper_vbox.addLayout( self.upper_origin_hbox )
self.upper_groupbox.setLayout( self.upper_vbox )
vbox.addWidget( self.upper_groupbox )
### TERRAIN_MODEL ########
self.terrain_model_groupbox = QGroupBox( "Terrain Model" )
self.terrain_model_groupbox.setCheckable( True )
self.terrain_model_groupbox.setChecked( False )
self.terrain_model_vbox = QVBoxLayout()
self.terrain_model_checkbox = QCheckBox( "Use Terrain Model" )
self.terrain_model_vbox.addWidget( self.terrain_model_checkbox )
self.terrain_min_ssx_hbox = QHBoxLayout()
self.terrain_min_ssx_label = QLabel("Min Sampling Steps x")
self.terrain_min_ssx_hbox.addWidget( self.terrain_min_ssx_label )
self.terrain_min_ssx_val = QDoubleSpinBox()
self.terrain_min_ssx_val.setSingleStep(1)
self.terrain_min_ssx_val.setRange(0,100)
self.terrain_min_ssx_hbox.addWidget( self.terrain_min_ssx_val )
self.terrain_model_vbox.addLayout( self.terrain_min_ssx_hbox )
self.terrain_min_ssy_hbox = QHBoxLayout()
self.terrain_min_ssy_label = QLabel("Min Sampling Steps y")
self.terrain_min_ssy_hbox.addWidget( self.terrain_min_ssy_label )
self.terrain_min_ssy_val = QDoubleSpinBox()
self.terrain_min_ssy_val.setSingleStep(1)
self.terrain_min_ssy_val.setRange(0,100)
self.terrain_min_ssy_hbox.addWidget( self.terrain_min_ssy_val )
self.terrain_model_vbox.addLayout( self.terrain_min_ssy_hbox )
self.terrain_max_ssx_hbox = QHBoxLayout()
self.terrain_max_ssx_label = QLabel("Max Sampling Steps x")
self.terrain_max_ssx_hbox.addWidget( self.terrain_max_ssx_label )
self.terrain_max_ssx_val = QDoubleSpinBox()
self.terrain_max_ssx_val.setSingleStep(1)
self.terrain_max_ssx_val.setRange(0,100)
self.terrain_max_ssx_hbox.addWidget( self.terrain_max_ssx_val )
self.terrain_model_vbox.addLayout( self.terrain_max_ssx_hbox )
self.terrain_max_ssy_hbox = QHBoxLayout()
self.terrain_max_ssy_label = QLabel("Max Sampling Steps y")
self.terrain_max_ssy_hbox.addWidget( self.terrain_max_ssy_label )
self.terrain_max_ssy_val = QDoubleSpinBox()
self.terrain_max_ssy_val.setSingleStep(1)
self.terrain_max_ssy_val.setRange(0,100)
self.terrain_max_ssy_hbox.addWidget( self.terrain_max_ssy_val )
self.terrain_model_vbox.addLayout( self.terrain_max_ssy_hbox )
self.terrain_max_iz_hbox = QHBoxLayout()
self.terrain_max_iz_label = QLabel("Max Intrusion z")
self.terrain_max_iz_hbox.addWidget( self.terrain_max_iz_label )
self.terrain_max_iz_val = QDoubleSpinBox()
self.terrain_max_iz_val.setDecimals(4)
self.terrain_max_iz_val.setSingleStep(.0001)
self.terrain_max_iz_val.setRange(0,.25)
self.terrain_max_iz_hbox.addWidget( self.terrain_max_iz_val )
self.terrain_model_vbox.addLayout( self.terrain_max_iz_hbox )
self.terrain_max_gc_hbox = QHBoxLayout()
self.terrain_max_gc_label = QLabel("Max Ground Clearance")
self.terrain_max_gc_hbox.addWidget( self.terrain_max_gc_label )
self.terrain_max_gc_val = QDoubleSpinBox()
self.terrain_max_gc_val.setDecimals(4)
self.terrain_max_gc_val.setSingleStep(.0001)
self.terrain_max_gc_val.setRange(0,.25)
self.terrain_max_gc_hbox.addWidget( self.terrain_max_gc_val )
self.terrain_model_vbox.addLayout( self.terrain_max_gc_hbox )
self.terrain_ms_hbox = QHBoxLayout()
self.terrain_ms_label = QLabel("Minimal Support")
self.terrain_ms_hbox.addWidget( self.terrain_ms_label )
self.terrain_ms_val = QDoubleSpinBox()
self.terrain_ms_val.setDecimals(2)
self.terrain_ms_val.setSingleStep(.01)
self.terrain_ms_val.setRange(0,1)
self.terrain_ms_hbox.addWidget( self.terrain_ms_val )
self.terrain_model_vbox.addLayout( self.terrain_ms_hbox )
self.terrain_model_groupbox.setLayout( self.terrain_model_vbox )
vbox.addWidget( self.terrain_model_groupbox )
### STANDARD_STEP_PARAMS ########
self.std_step_vbox = QVBoxLayout()
self.std_step_groupbox = QGroupBox( "Standard Step Parameters" )
self.std_step_groupbox.setCheckable( True )
self.std_step_groupbox.setChecked( False )
self.foot_sep_hbox = QHBoxLayout()
self.foot_sep_label = QLabel("Foot Separation")
self.foot_sep_hbox.addWidget( self.foot_sep_label )
self.foot_sep_val = QDoubleSpinBox()
self.foot_sep_val.setSingleStep(.01)
self.foot_sep_hbox.addWidget(self.foot_sep_val)
self.std_step_vbox.addLayout( self.foot_sep_hbox )
self.std_step_step_duration_hbox = QHBoxLayout()
self.std_step_step_duration_label = QLabel("Step Duration")
self.std_step_step_duration_hbox.addWidget( self.std_step_step_duration_label )
self.std_step_step_duration_val = QDoubleSpinBox()
self.std_step_step_duration_val.setSingleStep(.01)
self.std_step_step_duration_hbox.addWidget( self.std_step_step_duration_val )
self.std_step_vbox.addLayout( self.std_step_step_duration_hbox )
self.std_step_sway_duration_hbox = QHBoxLayout()
self.std_step_sway_duration_label = QLabel("Sway Duration")
self.std_step_sway_duration_hbox.addWidget( self.std_step_sway_duration_label )
self.std_step_sway_duration_val = QDoubleSpinBox()
self.std_step_sway_duration_val.setSingleStep(.01)
self.std_step_sway_duration_hbox.addWidget( self.std_step_sway_duration_val )
self.std_step_vbox.addLayout( self.std_step_sway_duration_hbox )
self.std_step_swing_height_hbox = QHBoxLayout()
self.std_step_swing_height_label = QLabel("Swing Height")
self.std_step_swing_height_hbox.addWidget( self.std_step_swing_height_label )
self.std_step_swing_height_val = QDoubleSpinBox()
self.std_step_swing_height_val.setSingleStep(.01)
self.std_step_swing_height_hbox.addWidget( self.std_step_swing_height_val )
self.std_step_vbox.addLayout( self.std_step_swing_height_hbox )
self.std_step_lift_height_hbox = QHBoxLayout()
self.std_step_lift_height_label = QLabel("Lift Height")
self.std_step_lift_height_hbox.addWidget( self.std_step_lift_height_label )
self.std_step_lift_height_val = QDoubleSpinBox()
self.std_step_lift_height_val.setSingleStep(.01)
self.std_step_lift_height_hbox.addWidget( self.std_step_lift_height_val )
self.std_step_vbox.addLayout( self.std_step_lift_height_hbox )
self.std_step_groupbox.setLayout( self.std_step_vbox )
vbox.addWidget( self.std_step_groupbox )
button_hbox = QHBoxLayout()
button_get = QPushButton("Get Current Values")
button_hbox.addWidget( button_get )
button_submit = QPushButton("Send Values")
button_hbox.addWidget( button_submit)
vbox.addLayout( button_hbox )
vbox.addStretch(1)
self._widget.setLayout(vbox)
#context.add_widget(self._widget)
# publishers and subscribers
self.param_pub = rospy.Publisher('/flor/footstep_planner/set_params', FootstepPlannerParams, queue_size=10)
button_submit.pressed.connect(self.sendParams)
self.param_sub = self.stateSubscriber = rospy.Subscriber('/ros_footstep_planner/params', FootstepPlannerParams, self.getParamCallbackFcn)
button_get.pressed.connect(self.getParams)
def __init__(self, context):
super(TrapezoidalTrajectoryDialog, self).__init__(context)
self.setObjectName("TrapezoidalTrajectoryDialog")
# self.updateStateSignal = Signal(object)
self.updateStateSignal.connect(self.on_updateState)
self.robot = URDF.from_parameter_server()
self.joint_list = {}
for ndx, jnt in enumerate(self.robot.joints):
self.joint_list[jnt.name] = ndx
self.chain = []
self.chain_file = rospy.get_param("~chain_file")
self.chain_name = rospy.get_param("~chain_name")
print
print "Define order of splines used to approximate trapezoid"
self.spline_order = rospy.get_param("~spline_order", 5) # 1 # 3 # 5 # linear, cubic, quintic
if (self.spline_order == 1) or (self.spline_order == 3) or (self.spline_order == 5):
print "Spline order=", self.spline_order
else:
print "Invalid spline order! Must be 1, 3, or 5"
print "Spline order=", self.spline_order
sys.exit(-1)
yaml_file = self.chain_file + self.chain_name + "_chain.yaml"
print "Chain definition file:"
print yaml_file
print
# define structures
self.robot_state = JointState()
self.robot_command = JointTrajectory()
stream = open(yaml_file, "r")
jointChain = yaml.load_all(stream)
for ndx, data in enumerate(jointChain):
print ndx, " : ", data
self.delay_time = data["delay_time"]
self.ramp_up_time = data["ramp_up_time"]
self.dwell_time = data["dwell_time"]
self.ramp_down_time = data["ramp_down_time"]
self.hold_time = data["hold_time"]
self.ramp_start_fraction = data["ramp_start_fraction"]
self.ramp_end_fraction = data["ramp_end_fraction"]
self.joint_state_topic = data["joint_state_topic"]
self.trajectory_topic = data["trajectory_topic"]
if rospy.search_param(data["chain_param_name"]):
print "Found ", data["chain_param_name"]
else:
print "Failed to find the ", data["chain_param_name"], " in the parameter server!"
sys.exit(-1)
joint_names = rospy.get_param(data["chain_param_name"])
for joint in joint_names:
print joint
self.robot_state.name.append(joint)
self.chain.append(JointData(self, joint))
self.robot_command.joint_names = self.robot_state.name
stream.close()
self.robot_state.position = [0.0] * len(self.robot_state.name)
self.robot_state.velocity = [0.0] * len(self.robot_state.name)
self.robot_state.effort = [0.0] * len(self.robot_state.name)
self.robot_joint_state = JointState()
print "delay_time =", self.delay_time
print "ramp_up_time =", self.ramp_up_time
print "dwell_time =", self.dwell_time
print "ramp_down_time=", self.ramp_down_time
print "hold_time =", self.hold_time
print "spline order =", self.spline_order
if (
(self.ramp_start_fraction < 0.001)
or (self.ramp_end_fraction > 0.999)
or (self.ramp_start_fraction >= self.ramp_end_fraction)
):
print "Invalid ramp fractions - abort!"
print "0.0 < ", self.ramp_start_fraction, " < ", self.ramp_end_fraction, " < 1.0"
return
print "Robot State Structure", self.robot_state
print "Robot Command Structure", self.robot_command
# initialize structure to hold widget handles
self.cmd_spinbox = []
self.ramp_up_spinbox = []
self.ramp_down_spinbox = []
self._widget = QWidget()
vbox = QVBoxLayout()
# Push buttons
hbox = QHBoxLayout()
snap_command = QPushButton("Snap Position")
snap_command.clicked.connect(self.snap_current_callback)
hbox.addWidget(snap_command)
check_limits = QPushButton("Check Limits Gains")
check_limits.clicked.connect(self.check_limits_callback)
hbox.addWidget(check_limits)
apply_command = QPushButton("Send Trajectory")
apply_command.clicked.connect(self.apply_command_callback)
hbox.addWidget(apply_command)
save_trajectory = QPushButton("Save Trajectory")
save_trajectory.clicked.connect(self.save_trajectory_callback)
hbox.addWidget(save_trajectory)
zero_ramp = QPushButton("Zero Ramps")
zero_ramp.clicked.connect(self.zero_ramp_callback)
hbox.addWidget(zero_ramp)
vbox.addLayout(hbox)
time_hbox = QHBoxLayout()
vbox_delay = QVBoxLayout()
vbox_delay.addWidget(QLabel("Delay"))
self.delay_time_spinbox = QDoubleSpinBox()
self.delay_time_spinbox.setDecimals(5)
self.delay_time_spinbox.setRange(0, 10.0)
self.delay_time_spinbox.setSingleStep(0.1)
self.delay_time_spinbox.valueChanged.connect(self.on_delay_time_value)
self.delay_time_spinbox.setValue(self.delay_time)
vbox_delay.addWidget(self.delay_time_spinbox)
time_hbox.addLayout(vbox_delay)
vbox_ramp_up = QVBoxLayout()
vbox_ramp_up.addWidget(QLabel("Ramp Up"))
self.ramp_up_time_spinbox = QDoubleSpinBox()
self.ramp_up_time_spinbox.setDecimals(5)
self.ramp_up_time_spinbox.setRange(0, 10.0)
self.ramp_up_time_spinbox.setSingleStep(0.1)
self.ramp_up_time_spinbox.valueChanged.connect(self.on_ramp_up_time_value)
self.ramp_up_time_spinbox.setValue(self.ramp_up_time)
vbox_ramp_up.addWidget(self.ramp_up_time_spinbox)
time_hbox.addLayout(vbox_ramp_up)
#
vbox_dwell = QVBoxLayout()
vbox_dwell.addWidget(QLabel("Dwell"))
self.dwell_time_spinbox = QDoubleSpinBox()
self.dwell_time_spinbox.setDecimals(5)
self.dwell_time_spinbox.setRange(0, 10.0)
self.dwell_time_spinbox.setSingleStep(0.1)
self.dwell_time_spinbox.valueChanged.connect(self.on_dwell_time_value)
self.dwell_time_spinbox.setValue(self.dwell_time)
vbox_dwell.addWidget(self.dwell_time_spinbox)
time_hbox.addLayout(vbox_dwell)
vbox_ramp_down = QVBoxLayout()
vbox_ramp_down.addWidget(QLabel("Down"))
self.ramp_down_time_spinbox = QDoubleSpinBox()
self.ramp_down_time_spinbox.setDecimals(5)
self.ramp_down_time_spinbox.setRange(0, 10.0)
self.ramp_down_time_spinbox.setSingleStep(0.1)
self.ramp_down_time_spinbox.valueChanged.connect(self.on_ramp_down_time_value)
self.ramp_down_time_spinbox.setValue(self.ramp_down_time)
vbox_ramp_down.addWidget(self.ramp_down_time_spinbox)
time_hbox.addLayout(vbox_ramp_down)
vbox_hold = QVBoxLayout()
vbox_hold.addWidget(QLabel("Hold"))
self.hold_time_spinbox = QDoubleSpinBox()
self.hold_time_spinbox.setDecimals(5)
self.hold_time_spinbox.setRange(0, 10.0)
self.hold_time_spinbox.setSingleStep(0.1)
self.hold_time_spinbox.valueChanged.connect(self.on_hold_time_value)
self.hold_time_spinbox.setValue(self.hold_time)
vbox_hold.addWidget(self.hold_time_spinbox)
time_hbox.addLayout(vbox_hold)
vbox.addLayout(time_hbox)
# Joints title
title_frame = QFrame()
title_frame.setFrameShape(QFrame.StyledPanel)
title_frame.setFrameShadow(QFrame.Raised)
title_hbox = QHBoxLayout()
title_hbox.addWidget(QLabel("Joints"))
title_hbox.addWidget(QLabel("Start"))
title_hbox.addWidget(QLabel("Ramp Up"))
title_hbox.addWidget(QLabel("Ramp Down"))
title_frame.setLayout(title_hbox)
vbox.addWidget(title_frame)
# Define the widgets for each joint
for i, joint in enumerate(self.chain):
# print i,",",joint
self.joint_widget(vbox, i)
# add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
# Define the connections to the outside world
self.jointSubscriber = rospy.Subscriber(self.joint_state_topic, JointState, self.stateCallbackFnc)
self.commandPublisher = rospy.Publisher(self.trajectory_topic, JointTrajectory, queue_size=10)
# Add the widget
context.add_widget(self._widget)
def __init__(self, context):
super(HeadControlDialog, self).__init__(context)
self.setObjectName('HeadControlDialog')
self.head_max_pitch_deg = 65.0
self.head_min_pitch_deg = -60.0
self.head_pitch_range_deg = self.head_max_pitch_deg - self.head_min_pitch_deg
self.scan_period_secs = 20.0
self.head_pitch_cmd_deg = 0.0
self.scan_offset = rospy.get_rostime()
self.last_publish_time = self.scan_offset
self._timer = None
self._widget = QWidget()
vbox = QVBoxLayout()
spindle_speed_hbox = QHBoxLayout()
#Add input for setting the spindle speed
spindle_label = QLabel("Spindle Speed [deg/s]")
spindle_speed_hbox.addWidget(spindle_label)
#Add a spinbox for setting the spindle speed
spindle_speed_spinbox = QDoubleSpinBox()
spindle_speed_spinbox.setRange(-360 * 4, 360 * 4)
spindle_speed_spinbox.valueChanged.connect(self.handle_spindle_speed)
self.spindle_speed_pub = rospy.Publisher('/multisense/set_spindle_speed', Float64, queue_size=10)
spindle_speed_hbox.addWidget(spindle_speed_spinbox)
vbox.addLayout(spindle_speed_hbox)
#Add input for directly setting the head pitch
head_pitch_hbox = QHBoxLayout()
head_pitch_label = QLabel("Head Pitch [deg]")
head_pitch_hbox.addWidget(head_pitch_label)
#Add a spinbox for setting the head pitch directly
self.head_pitch_spinbox = QDoubleSpinBox()
self.head_pitch_spinbox.setRange(self.head_min_pitch_deg, self.head_max_pitch_deg)
self.head_pitch_spinbox.valueChanged.connect(self.handle_head_pitch)
self.head_pitch_pub = rospy.Publisher('/atlas/pos_cmd/neck_ry', Float64, queue_size=10)
head_pitch_hbox.addWidget(self.head_pitch_spinbox)
vbox.addLayout(head_pitch_hbox)
#Publisher for head trajectory
self.head_trajectory_pub = rospy.Publisher('/trajectory_controllers/neck_traj_controller/command', JointTrajectory, queue_size=10)
#Add checkbox for invoking scanning behavior
self.head_scan_chkbox = QCheckBox("Scanning behavior")
self.head_scan_chkbox.stateChanged.connect(self.handle_scan_chg)
vbox.addWidget(self.head_scan_chkbox)
#Add input for setting the minimum head pitch
head_min_pitch_hbox = QHBoxLayout()
head_min_pitch_label = QLabel("Min Head Pitch [deg] (raise head)")
head_min_pitch_hbox.addWidget(head_min_pitch_label)
head_min_pitch_spinbox = QDoubleSpinBox()
head_min_pitch_spinbox.setRange(self.head_min_pitch_deg, self.head_max_pitch_deg)
head_min_pitch_spinbox.setValue(self.head_min_pitch_deg)
head_min_pitch_spinbox.valueChanged.connect(self.handle_head_min_pitch)
head_min_pitch_hbox.addWidget(head_min_pitch_spinbox)
vbox.addLayout(head_min_pitch_hbox)
#Add input for setting the maximum head pitch
head_max_pitch_hbox = QHBoxLayout()
head_max_pitch_label = QLabel("Max Head Pitch [deg] (lower head)")
head_max_pitch_hbox.addWidget(head_max_pitch_label)
head_max_pitch_spinbox = QDoubleSpinBox()
head_max_pitch_spinbox.setRange(self.head_min_pitch_deg, self.head_max_pitch_deg)
head_max_pitch_spinbox.setValue(self.head_max_pitch_deg)
head_max_pitch_spinbox.valueChanged.connect(self.handle_head_max_pitch)
head_max_pitch_hbox.addWidget(head_max_pitch_spinbox)
vbox.addLayout(head_max_pitch_hbox)
#Add input for setting the scan period
head_period_hbox = QHBoxLayout()
head_period_label = QLabel("Scanning Period [secs]")
head_period_hbox.addWidget(head_period_label)
head_period_spinbox = QDoubleSpinBox()
head_period_spinbox.setRange(0.01, 60.0)
head_period_spinbox.setValue(self.scan_period_secs)
head_period_spinbox.valueChanged.connect(self.handle_head_period)
head_period_hbox.addWidget(head_period_spinbox)
vbox.addLayout(head_period_hbox)
#add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
def __init__(self, context):
super(SensorParamControlDialog, self).__init__(context)
self.setObjectName('SensorParamControlDialog')
self._widget = QWidget()
vbox = QVBoxLayout()
### Multisense ###
ms_groupbox = QGroupBox( "Multisense" )
ms_vbox = QVBoxLayout()
ms_gain_hbox = QHBoxLayout()
self.ms_gain_label = QLabel("Image Gain [1, 1, 8]")
ms_gain_hbox.addWidget( self.ms_gain_label )
self.ms_gain = QDoubleSpinBox()
self.ms_gain.setSingleStep(.01)
self.ms_gain.setRange(1,8)
ms_gain_hbox.addWidget( self.ms_gain )
ms_vbox.addLayout( ms_gain_hbox )
ms_exp_hbox = QHBoxLayout()
self.ms_exp_auto = QCheckBox("Image Exposure [.03, 0.5]")
ms_exp_hbox.addWidget( self.ms_exp_auto )
self.ms_exp = QDoubleSpinBox()
self.ms_exp.setSingleStep( .001 )
self.ms_exp.setRange( .025,.5 )
ms_exp_hbox.addWidget( self.ms_exp )
ms_vbox.addLayout( ms_exp_hbox )
ms_spindle_hbox = QHBoxLayout()
ms_spindle_label = QLabel("Spindle Speed [0, 5.2]")
ms_spindle_hbox.addWidget( ms_spindle_label )
self.ms_spindle = QDoubleSpinBox()
self.ms_spindle.setSingleStep(.01)
self.ms_spindle.setRange( 0,15.2 )
ms_spindle_hbox.addWidget( self.ms_spindle )
ms_vbox.addLayout( ms_spindle_hbox )
ms_light_hbox = QHBoxLayout()
ms_light_label = QLabel("Light Brightness")
ms_light_hbox.addWidget(ms_light_label)
self.ms_light = QSlider(Qt.Horizontal)
self.ms_light.setRange(0,100)
ms_light_hbox.addWidget( self.ms_light )
ms_vbox.addLayout( ms_light_hbox )
ms_button_hbox = QHBoxLayout()
ms_button_hbox.addStretch(1)
ms_button_get = QPushButton("Get Settings")
ms_button_get.pressed.connect(self.ms_get_callback)
#ms_button_hbox.addWidget( ms_button_get )
ms_button_set = QPushButton("Set Settings")
ms_button_set.pressed.connect(self.ms_set_callback)
ms_button_hbox.addWidget( ms_button_set )
ms_vbox.addLayout( ms_button_hbox )
ms_groupbox.setLayout( ms_vbox )
vbox.addWidget( ms_groupbox )
### Left SA ###
sa_left_groupbox = QGroupBox( "Left SA Camera" )
sa_left_vbox = QVBoxLayout()
sa_left_gain_hbox = QHBoxLayout()
sa_left_gain_label = QLabel("Image Gain [0, 0, 25]")
sa_left_gain_hbox.addWidget( sa_left_gain_label )
self.sa_left_gain = QDoubleSpinBox()
self.sa_left_gain.setSingleStep(.01)
self.sa_left_gain.setRange( 0, 25 )
sa_left_gain_hbox.addWidget( self.sa_left_gain )
sa_left_vbox.addLayout( sa_left_gain_hbox )
sa_left_exp_hbox = QHBoxLayout()
sa_left_exp_label = QLabel("Image Shutter [0, 5.5, 25]")
sa_left_exp_hbox.addWidget( sa_left_exp_label )
self.sa_left_exp = QDoubleSpinBox()
self.sa_left_exp.setSingleStep(.01)
self.sa_left_exp.setRange( 0, 25 )
sa_left_exp_hbox.addWidget( self.sa_left_exp )
sa_left_vbox.addLayout( sa_left_exp_hbox )
sa_left_button_hbox = QHBoxLayout()
sa_left_button_hbox.addStretch(1)
sa_left_button_get = QPushButton("Get Settings")
sa_left_button_get.pressed.connect(self.sa_left_get_callback)
#sa_left_button_hbox.addWidget( sa_left_button_get )
sa_left_button_set = QPushButton("Set Settings")
sa_left_button_set.pressed.connect(self.sa_left_set_callback)
sa_left_button_hbox.addWidget( sa_left_button_set )
sa_left_vbox.addLayout( sa_left_button_hbox )
sa_left_groupbox.setLayout( sa_left_vbox )
vbox.addWidget(sa_left_groupbox)
### Right SA ###
sa_right_groupbox = QGroupBox( "Right SA Camera" )
sa_right_vbox = QVBoxLayout()
sa_right_gain_hbox = QHBoxLayout()
sa_right_gain_label = QLabel("Image Gain [0, 0, 25]")
sa_right_gain_hbox.addWidget( sa_right_gain_label )
self.sa_right_gain = QDoubleSpinBox()
self.sa_right_gain.setSingleStep(.01)
self.sa_right_gain.setRange( 0, 25 )
sa_right_gain_hbox.addWidget( self.sa_right_gain )
sa_right_vbox.addLayout( sa_right_gain_hbox )
sa_right_exp_hbox = QHBoxLayout()
sa_right_exp_label = QLabel("Image Shutter [0, 5.5, 25]")
sa_right_exp_hbox.addWidget( sa_right_exp_label )
self.sa_right_exp = QDoubleSpinBox()
self.sa_right_exp.setSingleStep(.01)
self.sa_right_exp.setRange( 0, 25 )
sa_right_exp_hbox.addWidget( self.sa_right_exp )
sa_right_vbox.addLayout( sa_right_exp_hbox )
sa_right_button_hbox = QHBoxLayout()
sa_right_button_hbox.addStretch(1)
sa_right_button_get = QPushButton("Get Settings")
sa_right_button_get.pressed.connect(self.sa_right_get_callback)
#sa_right_button_hbox.addWidget( sa_right_button_get )
sa_right_button_set = QPushButton("Set Settings")
sa_right_button_set.pressed.connect(self.sa_right_set_callback)
sa_right_button_hbox.addWidget( sa_right_button_set )
sa_right_vbox.addLayout( sa_right_button_hbox )
sa_right_groupbox.setLayout( sa_right_vbox )
vbox.addWidget(sa_right_groupbox)
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
# publishers and subscribers
self.ms_cam_pub = rospy.Publisher( '/multisense_sl/set_cam_parameters', Float64MultiArray, queue_size=10)
self.ms_light_pub = rospy.Publisher( '/multisense_sl/set_light_brightness', Float64, queue_size=10)
self.ms_spindle_pub = rospy.Publisher( '/multisense_sl/set_spindle_speed', Float64, queue_size=10)
self.sa_left_cam_pub = rospy.Publisher( '/sa/left/set_cam_parameters', Float64MultiArray, queue_size=10)
self.sa_right_cam_pub = rospy.Publisher( '/sa/right/set_cam_parameters', Float64MultiArray, queue_size=10)
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()
def __init__(self, context):
# super(BDIPelvisPoseWidget, self).__init__(context)
# self.setObjectName('BDIPelvisPoseWidget')
super(BDIPelvisPoseWidget, self).__init__()
self.name = "BDIPelvisPoseWidget"
self.updateStateSignal.connect(self.on_updateState)
# self._widget = QWidget()
self._widget = context
vbox = QVBoxLayout()
self.forward_position = 0.0
self.lateral_position = 0.0
self.height_position = 0.91
self.roll_position = 0.0
self.pitch_position = 0.0
self.yaw_position = 0.0
self.currentForward = 0.0
self.currentLateral = 0.0
self.currentHeight = 0.91
self.currentRoll = 0.0
self.currentPitch = 0.0
self.currentYaw = 0.0
# Define checkboxes
vbox = QVBoxLayout()
label = QLabel()
label.setText("BDI Pelvis Height (Manipulate Mode Only)") # todo - connect controller mode
vbox.addWidget(label)
self.enable_checkbox = QCheckBox("Enable")
self.enable_checkbox.stateChanged.connect(self.on_enable_check)
vbox.addWidget(self.enable_checkbox)
self.snap_to_current_button = QPushButton("Snap to Current")
self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
vbox.addWidget(self.snap_to_current_button)
self.roll_slider = QSlider(Qt.Horizontal)
self.roll_label = QLabel()
self.roll_label.setText("Roll")
vbox.addWidget(self.roll_label)
self.roll_slider.setRange(int(-100), int(101))
self.roll_slider.setValue(int(0))
self.roll_slider.setSingleStep((200) / 50)
self.roll_slider.setTickInterval(25)
self.roll_slider.valueChanged.connect(self.on_rollSliderMoved)
vbox.addWidget(self.roll_slider)
self.roll_progress_bar = QProgressBar()
self.roll_progress_bar.setRange(int(-100), int(101))
self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100)
self.roll_progress_bar.setFormat("%.6f" % self.currentRoll)
vbox.addWidget(self.roll_progress_bar)
self.pitch_slider = QSlider(Qt.Horizontal)
self.pitch_label = QLabel()
self.pitch_label.setText("Pitch")
vbox.addWidget(self.pitch_label)
self.pitch_slider.setRange(int(-100), int(101))
self.pitch_slider.setValue(int(0))
self.pitch_slider.setSingleStep((200) / 50)
self.pitch_slider.setTickInterval(25)
self.pitch_slider.valueChanged.connect(self.on_pitchSliderMoved)
vbox.addWidget(self.pitch_slider)
self.pitch_progress_bar = QProgressBar()
self.pitch_progress_bar.setRange(int(-100), int(101))
self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100)
self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch)
vbox.addWidget(self.pitch_progress_bar)
self.yaw_slider = QSlider(Qt.Horizontal)
self.yaw_label = QLabel()
self.yaw_label.setText("Yaw")
vbox.addWidget(self.yaw_label)
self.yaw_slider.setRange(int(-100), int(101))
self.yaw_slider.setValue(int(0))
self.yaw_slider.setSingleStep((200) / 50)
self.yaw_slider.setTickInterval(25)
self.yaw_slider.valueChanged.connect(self.on_yawSliderMoved)
vbox.addWidget(self.yaw_slider)
self.yaw_progress_bar = QProgressBar()
self.yaw_progress_bar.setRange(int(-100), int(101))
self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100)
self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw)
vbox.addWidget(self.yaw_progress_bar)
self.forward_label = QLabel()
self.forward_label.setText("Forward")
vbox.addWidget(self.forward_label)
widget = QWidget()
hbox = QHBoxLayout()
hbox.addStretch()
self.forward_slider = QSlider(Qt.Vertical)
# self.forward_slider.setText("Height")
self.forward_slider.setRange(int(-101), int(100))
self.forward_slider.setValue(int(0))
self.forward_slider.setSingleStep(1)
self.forward_slider.setTickInterval(10)
self.forward_slider.valueChanged.connect(self.on_forwardSliderMoved)
hbox.addWidget(self.forward_slider)
self.forward_progress_bar = QProgressBar()
self.forward_progress_bar.setOrientation(Qt.Vertical)
self.forward_progress_bar.setRange(int(-100), int(101))
self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100)
self.forward_progress_bar.setTextVisible(False)
hbox.addWidget(self.forward_progress_bar)
self.forward_progress_bar_label = QLabel()
self.forward_progress_bar_label.setText("%.6f" % self.currentForward)
hbox.addWidget(self.forward_progress_bar_label)
hbox.addStretch()
widget.setLayout(hbox)
vbox.addWidget(widget)
self.lateral_label = QLabel()
self.lateral_label.setText("Lateral")
vbox.addWidget(self.lateral_label)
self.lateral_slider = QSlider(Qt.Horizontal)
# self.lateral_slider.setText("Lateral")
self.lateral_slider.setRange(int(-100), int(101))
self.lateral_slider.setValue(int(0))
self.lateral_slider.setSingleStep((200) / 50)
self.lateral_slider.setTickInterval(25)
self.lateral_slider.valueChanged.connect(self.on_lateralSliderMoved)
vbox.addWidget(self.lateral_slider)
self.lateral_progress_bar = QProgressBar()
self.lateral_progress_bar.setRange(int(-100), int(101))
self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100)
self.lateral_progress_bar.setFormat("%.6f" % self.currentLateral)
vbox.addWidget(self.lateral_progress_bar)
self.height_label = QLabel()
self.height_label.setText("Height")
vbox.addWidget(self.height_label)
widget = QWidget()
hbox = QHBoxLayout()
hbox.addStretch()
self.height_slider = QSlider(Qt.Vertical)
# self.height_slider.setText("Height")
self.height_slider.setRange(int(55), int(105))
self.height_slider.setValue(int(91))
self.height_slider.setSingleStep(2)
self.height_slider.setTickInterval(10)
self.height_slider.valueChanged.connect(self.on_heightSliderMoved)
hbox.addWidget(self.height_slider)
self.height_progress_bar = QProgressBar()
self.height_progress_bar.setOrientation(Qt.Vertical)
self.height_progress_bar.setRange(int(55), int(105))
self.height_progress_bar.setValue(self.currentHeight * 100)
self.height_progress_bar.setTextVisible(False)
hbox.addWidget(self.height_progress_bar)
self.height_progress_bar_label = QLabel()
self.height_progress_bar_label.setText("%.6f" % self.currentHeight)
hbox.addWidget(self.height_progress_bar_label)
hbox.addStretch()
widget.setLayout(hbox)
vbox.addWidget(widget)
vbox.addStretch()
self._widget.setLayout(vbox)
# context.add_widget(self._widget)
self.height_position = 0.91
self.lateral_position = 0.0
self.yaw_position = 0.0
self.first_time = True
self.enable_checkbox.setChecked(False)
self.yaw_slider.setEnabled(False)
self.roll_slider.setEnabled(False)
self.pitch_slider.setEnabled(False)
self.forward_slider.setEnabled(False)
self.lateral_slider.setEnabled(False)
self.height_slider.setEnabled(False)
self.pub_robot = rospy.Publisher("/flor/controller/bdi_desired_pelvis_pose", PoseStamped, queue_size=10)
# self.stateSubscriber = rospy.Subscriber('/flor/pelvis_controller/current_states',JointState, self.stateCallbackFnc)
# self.pub_bdi_pelvis = rospy.Publisher('/bdi_manipulate_pelvis_pose_rpy',PoseStamped)
self.pelvis_trajectory_pub = rospy.Publisher(
"/robot_controllers/pelvis_traj_controller/command", JointTrajectory, queue_size=10
)
self.stateSubscriber = rospy.Subscriber(
"/robot_controllers/pelvis_traj_controller/state", JointTrajectoryControllerState, self.stateCallbackFnc
)
def __init__(self, context):
super(NoLimitJointControlDialog, self).__init__(context)
self.setObjectName('NoLimitJointControlDialog')
self.updateStateSignal.connect(self.on_updateState)
self.updateGhostSignal.connect(self.on_updateGhost)
self.joint_states = JointState()
self.ghost_joint_states = JointState()
self._widget = QWidget()
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.addWidget(QLabel("Ghost"))
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_robot_target)
#hbox_radio.addWidget(QLabel("Robot"))
hbox_radio.addStretch()
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
widget = QWidget()
hbox = QHBoxLayout()
# Left to right layout
self.joint_control = NoLimitJointControl(self, roslib.packages.get_pkg_dir('vigir_rqt_no_limit_joint_control') + '/launch/joints.txt',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_snapGhostPressed)
all_box.addWidget(self.snap_to_ghost_button)
self.snap_to_current_button = QPushButton("SnapAllCurrent")
self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
all_box.addWidget(self.snap_to_current_button)
self.apply_to_robot_button = QPushButton("ApplyAllRobot")
self.apply_to_robot_button.pressed.connect(self.on_applyRobotPressed)
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_applyWBCRobotPressed)
all_box.addWidget(self.apply_to_robot_button)
all_widget.setLayout(all_box)
vbox.addWidget(all_widget)
# all_hbox = QHBoxLayout()
# all_hbox.addStretch()
# all_hbox.addWidget(all_widget)
# all_hbox.addStretch()
# bottom_widget=QWidget()
# bottom_widget.setLayout(all_jbox)
# vbox.addWidget(bottom_widget)
vbox.addStretch()
self._widget.setLayout(vbox)
context.add_widget(self._widget)
self.first_time = True
self.stateSubscriber = rospy.Subscriber('/atlas/joint_states', JointState, self.stateCallbackFnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghostCallbackFnc)
self.wbc_robot_pub = rospy.Publisher('/flor/wbc_controller/joint_states',JointState, queue_size=10)
def __init__(self, parent, fileName, widget):
self.controllers = []
self.parent = parent
self.loadFile(fileName)
robot = URDF.from_parameter_server()
joint_list = {}
for ndx,jnt in enumerate(robot.joints):
joint_list[jnt.name] = ndx
for controller in self.controllers:
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel);
frame.setFrameShadow(QFrame.Raised);
vbox = QVBoxLayout()
label = QLabel()
label.setText(controller.label)
vbox.addWidget(label);
print controller.name
controller.snap_to_ghost_button = QPushButton("SnapGhost")
controller.snap_to_ghost_button.pressed.connect(controller.on_snapGhostPressed)
vbox.addWidget(controller.snap_to_ghost_button)
controller.snap_to_current_button = QPushButton("SnapCurrent")
controller.snap_to_current_button.pressed.connect(controller.on_snapCurrentPressed)
vbox.addWidget(controller.snap_to_current_button)
controller.apply_to_robot_button = QPushButton("ApplyRobot")
controller.apply_to_robot_button.pressed.connect(controller.on_applyRobotPressed)
vbox.addWidget(controller.apply_to_robot_button)
controller.save_joints_to_file_button = QPushButton("SaveJoints")
controller.save_joints_to_file_button.pressed.connect(controller.on_saveJointsPressed)
vbox.addWidget(controller.save_joints_to_file_button)
controller.undo_last_action_button = QPushButton("Undo Last")
controller.undo_last_action_button.pressed.connect(controller.on_undoPressed)
vbox.addWidget(controller.undo_last_action_button)
for joint in controller.joints:
label = QLabel()
label.setText(joint.name)
vbox.addWidget(label);
robot_joint = robot.joints[joint_list[joint.name]]
lower = robot_joint.limit.lower - (math.fabs(robot_joint.limit.upper)+math.fabs(robot_joint.limit.lower))*0.2
upper = robot_joint.limit.upper + (math.fabs(robot_joint.limit.upper)+math.fabs(robot_joint.limit.lower))*0.2
print " ",joint.name, " limits(", robot_joint.limit.lower,", ",robot_joint.limit.upper,") num"
joint.slider = QSlider(Qt.Horizontal)
joint.slider.setRange(int(lower*10000.0), int(upper*10000.0))
joint.slider.setValue(int(lower*10000.0))
joint.slider.setSingleStep((upper-lower)/20.0)
joint.slider.valueChanged.connect(joint.on_sliderMoved)
vbox.addWidget(joint.slider)
joint.progress_bar = QProgressBar()
joint.progress_bar.setRange(int(lower*10000.0), int(upper*10000.0))
joint.progress_bar.setValue(int(lower*10000.0))
vbox.addWidget(joint.progress_bar)
vbox.addStretch()
frame.setLayout(vbox)
widget.addWidget(frame)
def __init__(self, context):
super(SteeringInterfaceDialog, self).__init__(context)
self.setObjectName('SteeringInterfaceDialog')
self.time_step = rospy.get_param('~time_step', 1.0)
self.listener = tf.TransformListener()
# Assuming right arm control for now
self.r_arm_pub = rospy.Publisher('/flor/r_arm_controller/trajectory',JointTrajectory, None, False, True, None, queue_size=10)
self.r_leg_pub = rospy.Publisher('/flor/r_leg_controller/trajectory',JointTrajectory, None, False, True, None, queue_size=10)
self.markers_pub = rospy.Publisher('/flor_footstep_planner/footsteps_array',MarkerArray, None, False, True, None, queue_size=10)
self.r_leg_state = RightLegState()
self.r_arm_state = RightArmState()
# External data coming in (current joint states, and template pose (utility vehicle model)
self.joint_data = JointState()
self.template_selection = TemplateSelection()
self.joints_sub = rospy.Subscriber('/atlas/joint_states',JointState, self.joint_states_callback )
self.template_sub = rospy.Subscriber('/template/template_selected',TemplateSelection, self.template_selection_callback )
# ------------------------------------------------------------------------------------
# TODO--- Define these transforms relative to the origin of the large polaris mesh
#Name X Y Z
#Parking Break -0.07 0.48 -0.87
#Parking Break Free -0.07 0.53 -0.85
#FNR Switch 0.02 0.56 -0.90
#FNR Switch F 0.02 0.60 -0.90
#FNR Switch R 0.02 0.55 -0.95
#Gas Pedal -0.10 0.60 -1.50
#Gas Pedal Half -0.10 0.62 -1.53
#Gas Pedal Full -0.10 0.65 -1.56
#Break Pedal -0.27 0.60 -1.50
#Break Pedal Slow -0.27 0.63 -1.53
#Break Pedal Stop -0.27 0.66 -1.55
# Transform brake and throttle to car template (static transforms relative to the car)
# leg
self._c_T_b = [-0.27, 0.60,-1.50] # (b)rake
self._c_T_b_slow = [-0.27, 0.63,-1.53]
self._c_T_b_stop = [-0.27, 0.66,-1.55]
self._c_T_t = [-0.10, 0.60,-1.50] # (t)hrottle
self._c_T_t_slow = [-0.10, 0.62,-1.53]
self._c_T_t_fast = [-0.10, 0.65,-1.56]
self._c_T_n = [-0.185,0.60,-1.50] # (n)eutral // in between neutral gas/brake
# need to add them in order for the transforms to work
#FAST = 0 self._c_T_t_fast = [-0.10, 0.65,-1.56]
#SLOW = 1 self._c_T_t_slow = [-0.10, 0.62,-1.53]
#NEUTRAL = 2 self._c_T_n = [-0.185,0.60,-1.50] # (n)eutral // in between neutral gas/brake
#BRAKE = 3 self._c_T_b_slow = [-0.27, 0.63,-1.53]
#STOP = 4 self._c_T_b_stop = [-0.27, 0.66,-1.55]
self.r_leg_state._c_T_target_positions.append(self._c_T_t_fast)
self.r_leg_state._c_T_target_positions.append(self._c_T_t_slow)
self.r_leg_state._c_T_target_positions.append(self._c_T_n)
self.r_leg_state._c_T_target_positions.append(self._c_T_b_slow)
self.r_leg_state._c_T_target_positions.append(self._c_T_b_stop)
# arm
self._c_T_hl = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_l = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_fwd = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_r = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_hr = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_p = [-0.07, 0.48,-0.87] # (p)arking brake
self._c_T_p_free = [-0.07, 0.53,-0.85]
self._c_T_f = [ 0.02, 0.56,-0.90] # (f)wd_button
self._c_T_f_f = [ 0.02, 0.60,-0.90]
self._c_T_f_r = [ 0.02, 0.55,-0.95]
# need to add them in order
#HLEFT = 0
#LEFT = 1
#FWD = 2
#RIGHT = 3
#HRIGHT = 4
#PRE_FORWARD = 5 self._c_T_f = [ 0.02, 0.56,-0.90] # (f)wd_button
#FORWARD = 6 self._c_T_f_f = [ 0.02, 0.60,-0.90]
#REVERSE = 7 self._c_T_f_r = [ 0.02, 0.55,-0.95]
#PRE_PARKING = 8 self._c_T_p = [-0.07, 0.48,-0.87] # (p)arking brake
#PARKING = 9 self._c_T_p_free = [-0.07, 0.53,-0.85]
self.r_arm_state._c_T_target_positions.append(self._c_T_hl)
self.r_arm_state._c_T_target_positions.append(self._c_T_l)
self.r_arm_state._c_T_target_positions.append(self._c_T_fwd)
self.r_arm_state._c_T_target_positions.append(self._c_T_r)
self.r_arm_state._c_T_target_positions.append(self._c_T_hr)
self.r_arm_state._c_T_target_positions.append(self._c_T_f)
self.r_arm_state._c_T_target_positions.append(self._c_T_f_f)
self.r_arm_state._c_T_target_positions.append(self._c_T_f_r)
self.r_arm_state._c_T_target_positions.append(self._c_T_p)
self.r_arm_state._c_T_target_positions.append(self._c_T_p_free)
# ---------------------------------------------------------------------------------
# Widget layout stuff
# -----------------------------------------------------------------------
self._widget = QWidget()
vbox = QVBoxLayout()
top_widget = QWidget()
top_hbox = QHBoxLayout()
# top row
hard_left_command = QPushButton("H L")
hard_left_command.clicked.connect(self.hard_left_command_callback)
top_hbox.addWidget(hard_left_command)
left_command = QPushButton("Left")
left_command.clicked.connect(self.left_command_callback)
top_hbox.addWidget(left_command)
straight_command = QPushButton("Fwd")
straight_command.clicked.connect(self.straight_command_callback)
top_hbox.addWidget(straight_command)
right_command = QPushButton("Right")
right_command.clicked.connect(self.right_command_callback)
top_hbox.addWidget(right_command)
hard_right_command = QPushButton("H R")
hard_right_command.clicked.connect(self.hard_right_command_callback)
top_hbox.addWidget(hard_right_command)
top_widget.setLayout(top_hbox)
vbox.addWidget(top_widget)
# Center column
bottom_widget = QWidget()
bottom_hbox = QHBoxLayout()
calc_widget = QWidget()
calc_vbox = QVBoxLayout()
calc_vbox.addStretch()
calc_command = QPushButton("Calc")
calc_command.clicked.connect(self.calc_command_callback)
calc_vbox.addWidget(calc_command)
calc_widget.setLayout(calc_vbox)
bottom_hbox.addWidget(calc_widget)
throttle_widget = QWidget()
throttle_vbox = QVBoxLayout()
fast_command = QPushButton("Fast")
fast_command.clicked.connect(self.fast_command_callback)
throttle_vbox.addWidget(fast_command)
slow_command = QPushButton("Slow")
slow_command.clicked.connect(self.slow_command_callback)
throttle_vbox.addWidget(slow_command)
neutral_command = QPushButton("Neutral")
neutral_command.clicked.connect(self.neutral_command_callback)
throttle_vbox.addWidget(neutral_command)
slow_command = QPushButton("Brake")
slow_command.clicked.connect(self.slow_command_callback)
throttle_vbox.addWidget(slow_command)
stop_command = QPushButton("Stop")
stop_command.clicked.connect(self.stop_command_callback)
throttle_vbox.addWidget(stop_command)
throttle_widget.setLayout(throttle_vbox)
bottom_hbox.addWidget(throttle_widget)
right_widget = QWidget()
right_vbox = QVBoxLayout()
right_vbox.addStretch()
parking_command = QPushButton("Parking")
parking_command.clicked.connect(self.parking_command_callback)
right_vbox.addWidget(parking_command)
go_button_command = QPushButton("Fwd")
go_button_command.clicked.connect(self.go_button_command_callback)
right_vbox.addWidget(go_button_command)
reverse_button_command = QPushButton("R")
reverse_button_command.clicked.connect(self.reverse_button_command_callback)
right_vbox.addWidget(reverse_button_command)
right_widget.setLayout(right_vbox)
bottom_hbox.addWidget(right_widget)
bottom_widget.setLayout(bottom_hbox)
vbox.addWidget(bottom_widget)
self._widget.setLayout(vbox)
# --------------------------------------------------- End Widget stuff -------------------------------
context.add_widget(self._widget)
def buildRadioButtons(self,
labelTextArray,
orientation,
alignment,
activeButtons=None,
behavior=CheckboxGroupBehavior.RADIO_BUTTONS):
'''
@param labelTextArray: Names of buttons
@type labelTextArray: [string]
@param orientation: Whether to arrange the buttons vertically or horizontally.
@type orientation: Orientation
@param alignment: whether buttons should be aligned Left/Center/Right for horizontal,
Top/Center/Bottom for vertical:/c
@type alignment: Alignment
@param activeButtons: Name of the buttons that is to be checked initially. Or None.
@type activeButtons: [string]
@param behavior: Indicates whether the button group is to behave like Radio Buttons, or like Checkboxes.
@type behavior: CheckboxGroupBehavior
@return
1. The button group that contains the related buttons. Caller: ensure that
this object does not go out of scope.
2. The button layout, which callers will need to add to
their own layouts.
3. and a dictionary mapping button names to button objects that
were created within this method. This dict is needed by the
controller.
@rtype (QButtonGroup, QLayout, dict<string,QRadioButton>).
'''
# Button control management group. It has no visible
# representation. It allows the radio button bahavior, for instance:
buttonGroup = QButtonGroup();
if behavior == CheckboxGroupBehavior.CHECKBOXES:
buttonGroup.setExclusive(False);
else:
buttonGroup.setExclusive(True);
if orientation == Orientation.VERTICAL:
buttonCompLayout = QVBoxLayout();
# Arrange for the alignment (Part 1):
if (alignment == Alignment.CENTER) or\
(alignment == Alignment.BOTTOM):
buttonCompLayout.addStretch(1);
else:
buttonCompLayout = QHBoxLayout();
# Arrange for the alignment (Part 1):
if (alignment == Alignment.CENTER) or\
(alignment == Alignment.RIGHT):
buttonCompLayout.addStretch(1);
# Build the buttons:
buttonDict = {};
for label in labelTextArray:
button = QRadioButton(label);
buttonDict[label] = button;
# Add button to the button management group:
buttonGroup.addButton(button);
# Add the button to the visual group:
buttonCompLayout.addWidget(button);
if label in activeButtons:
button.setChecked(True);
if orientation == Orientation.VERTICAL:
buttonCompLayout = QVBoxLayout();
# Arrange for the alignment (Part 2):
if (alignment == Alignment.CENTER) or\
(alignment == Alignment.TOP):
buttonCompLayout.addStretch(1);
else:
# Arrange for the alignment (Part 2):
if (alignment == Alignment.CENTER) or\
(alignment == Alignment.LEFT):
buttonCompLayout.addStretch(1);
return (buttonGroup, buttonCompLayout, buttonDict);
def __init__(self, context):
super(PatternGeneratorWidget, self).__init__()
# publisher
self.pattern_generator_params_pub = rospy.Publisher(
'pattern_generator/set_params',
PatternGeneratorParameters,
queue_size=1)
# start widget
widget = context
# start upper part
hbox = QHBoxLayout()
# start left column
left_vbox = QVBoxLayout()
# start button
start_command = QPushButton("Start")
left_vbox.addWidget(start_command)
# simulation checkbox
self.simulation_mode_checkbox = QCheckBox()
self.simulation_mode_checkbox.setText("Simulation Mode")
self.simulation_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.simulation_mode_checkbox)
# realtime checkbox
self.realtime_mode_checkbox = QCheckBox()
self.realtime_mode_checkbox.setText("Realtime Mode")
self.realtime_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.realtime_mode_checkbox)
# joystick checkbox
self.joystick_mode_checkbox = QCheckBox()
self.joystick_mode_checkbox.setText("Joystick Mode")
self.joystick_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.joystick_mode_checkbox)
# ignore invalid steps checkbox
self.ignore_invalid_steps_checkbox = QCheckBox()
self.ignore_invalid_steps_checkbox.setText("Ignore Invalid Steps")
self.ignore_invalid_steps_checkbox.setChecked(False)
left_vbox.addWidget(self.ignore_invalid_steps_checkbox)
# foot seperation
self.foot_seperation = generate_q_double_spin_box(
0.2, 0.15, 0.3, 2, 0.01)
self.foot_seperation.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.foot_seperation,
"Foot Seperation (m):")
# delta x
self.delta_x = generate_q_double_spin_box(0.0, -0.4, 0.4, 2, 0.01)
self.delta_x.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_x, "dX (m):")
# delta y
self.delta_y = generate_q_double_spin_box(0.0, -2.2, 2.2, 2, 0.01)
self.delta_y.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_y, "dY (m):")
# delta yaw
self.delta_yaw = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.delta_yaw.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_yaw, "dYaw (deg):")
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.roll.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.pitch.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.pitch, "Pitch (deg):")
# end left column
left_vbox.addStretch()
hbox.addLayout(left_vbox, 1)
# start right column
right_vbox = QVBoxLayout()
# stop button
stop_command = QPushButton("Stop")
right_vbox.addWidget(stop_command)
# ignore collision
self.collision_checkbox = QCheckBox()
self.collision_checkbox.setText("Ignore Collision")
self.collision_checkbox.setChecked(True)
right_vbox.addWidget(self.collision_checkbox)
# override 3D
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_vbox.addWidget(self.override_checkbox)
# end right coloumn
right_vbox.addStretch()
hbox.addLayout(right_vbox, 1)
# add upper part
hbox.setMargin(0)
vbox = QVBoxLayout()
vbox.addLayout(hbox)
# parameter set selection
self.parameter_set_widget = QParameterSetWidget()
add_widget_with_frame(vbox, self.parameter_set_widget,
"Parameter Set:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# signal connections
start_command.clicked.connect(self.start_command_callback)
stop_command.clicked.connect(self.stop_command_callback)
self.joystick_mode_checkbox.clicked.connect(
self.joystick_mode_check_callback)
self.ignore_invalid_steps_checkbox.clicked.connect(
self._publish_parameters)
def __init__(self, context):
super(FootstepTerrainControlDialog, self).__init__(context)
self.setObjectName('FootstepTerrainControlDialog')
self.request_seq = 0
self._widget = QWidget()
vbox = QVBoxLayout()
self.cube_vbox = QVBoxLayout()
self.cube_groupbox = QGroupBox( "Terrain Box" )
self.cube_min_hbox = QHBoxLayout()
self.cube_min_label = QLabel("Bottom Left")
self.cube_min_hbox.addWidget( self.cube_min_label )
self.cube_min_x = QDoubleSpinBox()
self.cube_min_x.setSingleStep(.01)
self.cube_min_x.setRange(-10.0, 10.0)
self.cube_min_x.setValue(-3.0)
self.cube_min_hbox.addWidget(self.cube_min_x)
self.cube_min_y = QDoubleSpinBox()
self.cube_min_y.setSingleStep(.01)
self.cube_min_y.setRange(-10.0, 10.0)
self.cube_min_y.setValue(-3.0)
self.cube_min_hbox.addWidget(self.cube_min_y)
self.cube_min_z = QDoubleSpinBox()
self.cube_min_z.setSingleStep(.01)
self.cube_min_z.setRange(-10.0, 10.0)
self.cube_min_z.setValue(-1.0)
self.cube_min_hbox.addWidget(self.cube_min_z)
self.cube_vbox.addLayout( self.cube_min_hbox )
self.cube_max_hbox = QHBoxLayout()
self.cube_max_label = QLabel("Top Right")
self.cube_max_hbox.addWidget( self.cube_max_label )
self.cube_max_x = QDoubleSpinBox()
self.cube_max_x.setSingleStep(.01)
self.cube_max_x.setRange(-10.0, 10.0)
self.cube_max_x.setValue(3.0)
self.cube_max_hbox.addWidget(self.cube_max_x)
self.cube_max_y = QDoubleSpinBox()
self.cube_max_y.setSingleStep(.01)
self.cube_max_y.setRange(-10.0, 10.0)
self.cube_max_y.setValue(3.0)
self.cube_max_hbox.addWidget(self.cube_max_y)
self.cube_max_z = QDoubleSpinBox()
self.cube_max_z.setSingleStep(.01)
self.cube_max_z.setRange(-10.0, 10.0)
self.cube_max_z.setValue(1.0)
self.cube_max_hbox.addWidget(self.cube_max_z)
self.cube_vbox.addLayout( self.cube_max_hbox )
self.cube_groupbox.setLayout( self.cube_vbox )
vbox.addWidget( self.cube_groupbox )
self.type_hbox = QHBoxLayout()
# self.type_label = QLabel( "Type" )
# self.type_hbox.addWidget( self.type_label )
#
# self.type_combobox = QComboBox()
#
# self.type_combobox.addItem( "Type 1" )
# self.type_combobox.addItem( "Type 2" )
# self.type_combobox.addItem( "Type 3" )
#
# self.type_hbox.addWidget( self.type_combobox )
#
# vbox.addLayout( self.type_hbox )
self.scan_number_hbox = QHBoxLayout()
self.scan_number_label = QLabel("Number of Scans Used")
self.scan_number_hbox.addWidget( self.scan_number_label )
self.scan_number_val = QDoubleSpinBox()
self.scan_number_val.setDecimals(0)
self.scan_number_val.setRange(0,10000)
self.scan_number_val.setValue(2000)
self.scan_number_hbox.addWidget(self.scan_number_val)
vbox.addLayout( self.scan_number_hbox )
# self.use_terrain_checkbox = QCheckBox( "Use Terrain" )
# vbox.addWidget( self.use_terrain_checkbox )
button_hbox = QHBoxLayout()
# button_get = QPushButton("Get Current Values")
# button_hbox.addWidget( button_get )
button_submit = QPushButton("Send Values")
button_hbox.addWidget( button_submit)
vbox.addLayout( button_hbox )
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
# publishers and subscribers
self.terrain_pub = rospy.Publisher("/flor/terrain_classifier/generate_terrain_model", TerrainModelRequest, queue_size=10 )
button_submit.pressed.connect(self.sendParams)
class LogicalMarkerPlugin(Plugin):
def __init__(self, context):
super(LogicalMarkerPlugin, self).__init__(context)
# Create an image for the original map. This will never change.
try:
self.map_yaml_file_str = rospy.get_param("~map_file")
self.data_directory = rospy.get_param("~data_directory")
except KeyError:
rospy.logfatal("~map_file and ~data_directory need to be set to use the logical marker")
return
map_image_location = getImageFileLocation(self.map_yaml_file_str)
map = loadMapFromFile(self.map_yaml_file_str)
locations_file = getLocationsFileLocationFromDataDirectory(self.data_directory)
doors_file = getDoorsFileLocationFromDataDirectory(self.data_directory)
objects_file = getObjectsFileLocationFromDataDirectory(self.data_directory)
# Give QObjects reasonable names
self.setObjectName('LogicalMarkerPlugin')
# Create QWidget
self.master_widget = QWidget()
self.master_layout = QVBoxLayout(self.master_widget)
# Main Functions - Doors, Locations, Objects
self.function_layout = QHBoxLayout()
self.master_layout.addLayout(self.function_layout)
self.function_buttons = []
self.current_function = None
for button_text in ['Locations', 'Doors', 'Objects']:
button = QPushButton(button_text, self.master_widget)
button.clicked[bool].connect(self.handle_function_button)
button.setCheckable(True)
self.function_layout.addWidget(button)
self.function_buttons.append(button)
self.function_layout.addStretch(1)
self.master_layout.addWidget(self.get_horizontal_line())
# Subfunction toolbar
self.subfunction_layout = QHBoxLayout()
clearLayoutAndFixHeight(self.subfunction_layout)
self.master_layout.addLayout(self.subfunction_layout)
self.current_subfunction = None
self.master_layout.addWidget(self.get_horizontal_line())
self.image = MapImage(map_image_location, self.master_widget)
self.master_layout.addWidget(self.image)
self.master_layout.addWidget(self.get_horizontal_line())
# Configuration toolbar
self.configuration_layout = QHBoxLayout()
clearLayoutAndFixHeight(self.configuration_layout)
self.master_layout.addLayout(self.configuration_layout)
# Add a stretch at the bottom.
self.master_layout.addStretch(1)
self.master_widget.setObjectName('LogicalMarkerPluginUI')
if context.serial_number() > 1:
self.master_widget.setWindowTitle(self.master_widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self.master_widget)
# Activate the functions
self.functions = {}
self.functions['Locations'] = LocationFunction(locations_file,
map,
self.master_widget,
self.subfunction_layout,
self.configuration_layout,
self.image)
self.functions['Doors'] = DoorFunction(doors_file,
map,
self.functions['Locations'],
self.master_widget,
self.subfunction_layout,
self.configuration_layout,
self.image)
self.functions['Objects'] = ObjectFunction(objects_file,
map,
self.functions['Locations'],
self.master_widget,
self.subfunction_layout,
self.configuration_layout,
self.image)
def construct_layout(self):
pass
def get_horizontal_line(self):
"""
http://stackoverflow.com/questions/5671354/how-to-programmatically-make-a-horizontal-line-in-qt
"""
hline = QFrame()
hline.setFrameShape(QFrame.HLine)
hline.setFrameShadow(QFrame.Sunken)
return hline
def handle_function_button(self):
source = self.sender()
if source.text() == self.current_function:
source.setChecked(True)
return
# Depress all other buttons.
for button in self.function_buttons:
if button != source:
button.setChecked(False)
if self.current_function is not None:
self.functions[self.current_function].deactivateFunction()
self.current_function = source.text()
# Clear all subfunction buttons.
clearLayoutAndFixHeight(self.subfunction_layout)
if self.current_function is not None:
self.functions[self.current_function].activateFunction()
def shutdown_plugin(self):
modified = False
for function in self.functions:
if self.functions[function].isModified():
modified = True
if modified:
ret = QMessageBox.warning(self.master_widget, "Save",
"The logical map has been modified.\n"
"Do you want to save your changes?",
QMessageBox.Save | QMessageBox.Discard)
if ret == QMessageBox.Save:
for function in self.functions:
self.functions[function].saveConfiguration()
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class TextSearchFrame(QDockWidget):
'''
A frame to find text in the Editor.
'''
search_result_signal = Signal(str, bool, str, int)
''' @ivar: A signal emitted after search_threaded was started.
(search text, found or not, file, position in text)
for each result a signal will be emitted.
'''
replace_signal = Signal(str, str, int, str)
''' @ivar: A signal emitted to replace string at given position.
(search text, file, position in text, replaced by text)
'''
def __init__(self, tabwidget, parent=None):
QDockWidget.__init__(self, "Find", parent)
self.setObjectName('SearchFrame')
self.setFeatures(QDockWidget.DockWidgetMovable
| QDockWidget.DockWidgetFloatable)
self._dockwidget = QFrame(self)
self.vbox_layout = QVBoxLayout(self._dockwidget)
self.layout().setContentsMargins(0, 0, 0, 0)
self.layout().setSpacing(1)
# frame with two rows for find and replace
find_replace_frame = QFrame(self)
find_replace_vbox_layout = QVBoxLayout(find_replace_frame)
find_replace_vbox_layout.setContentsMargins(0, 0, 0, 0)
find_replace_vbox_layout.setSpacing(1)
find_replace_vbox_layout.addSpacerItem(
QSpacerItem(1, 1, QSizePolicy.Expanding, QSizePolicy.Expanding))
# create frame with find row
find_frame = self._create_find_frame()
find_replace_vbox_layout.addWidget(find_frame)
rplc_frame = self._create_replace_frame()
find_replace_vbox_layout.addWidget(rplc_frame)
# frame for find&replace and search results
self.vbox_layout.addWidget(find_replace_frame)
self.vbox_layout.addWidget(self._create_found_frame())
self.vbox_layout.addStretch(2024)
self.setWidget(self._dockwidget)
# intern search parameters
self._tabwidget = tabwidget
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self._search_result_index = -1
self._search_recursive = False
self._search_thread = None
def _create_find_frame(self):
find_frame = QFrame(self)
find_hbox_layout = QHBoxLayout(find_frame)
find_hbox_layout.setContentsMargins(0, 0, 0, 0)
find_hbox_layout.setSpacing(1)
self.search_field = EnchancedLineEdit(find_frame)
self.search_field.setPlaceholderText('search text')
self.search_field.textChanged.connect(self.on_search_text_changed)
self.search_field.returnPressed.connect(self.on_search)
find_hbox_layout.addWidget(self.search_field)
self.search_result_label = QLabel(find_frame)
self.search_result_label.setText(' ')
find_hbox_layout.addWidget(self.search_result_label)
self.find_button_back = QPushButton("<")
self.find_button_back.setFixedWidth(44)
self.find_button_back.clicked.connect(self.on_search_back)
find_hbox_layout.addWidget(self.find_button_back)
self.find_button = QPushButton(">")
self.find_button.setDefault(True)
# self.find_button.setFlat(True)
self.find_button.setFixedWidth(44)
self.find_button.clicked.connect(self.on_search)
find_hbox_layout.addWidget(self.find_button)
return find_frame
def _create_replace_frame(self):
# create frame with replace row
self.rplc_frame = rplc_frame = QFrame(self)
rplc_hbox_layout = QHBoxLayout(rplc_frame)
rplc_hbox_layout.setContentsMargins(0, 0, 0, 0)
rplc_hbox_layout.setSpacing(1)
self.replace_field = EnchancedLineEdit(rplc_frame)
self.replace_field.setPlaceholderText('replace text')
self.replace_field.returnPressed.connect(self.on_replace)
rplc_hbox_layout.addWidget(self.replace_field)
self.replace_result_label = QLabel(rplc_frame)
self.replace_result_label.setText(' ')
rplc_hbox_layout.addWidget(self.replace_result_label)
self.replace_button = replace_button = QPushButton("> &Replace >")
replace_button.setFixedWidth(90)
replace_button.clicked.connect(self.on_replace_click)
rplc_hbox_layout.addWidget(replace_button)
rplc_frame.setVisible(False)
return rplc_frame
def _create_found_frame(self):
self.found_files_frame = ff_frame = QGroupBox("recursive search")
ff_frame.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
self.found_files_vbox_layout = QVBoxLayout(ff_frame)
self.found_files_vbox_layout.setSpacing(0)
self.found_files_vbox_layout.setContentsMargins(0, 0, 0, 0)
self.found_files_list = QListWidget(ff_frame)
self.found_files_list.setSizePolicy(QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.found_files_list.setFrameStyle(QFrame.StyledPanel)
self.found_files_list.itemActivated.connect(self.on_itemActivated)
self.found_files_list.setStyleSheet("QListWidget {"
"background-color:transparent;"
"}"
"QListWidget::item {"
"background-color:transparent;"
"}"
"QListWidget::item:selected {"
"background-color: darkgray;"
"}")
self.found_files_vbox_layout.addWidget(self.found_files_list)
ff_frame.setCheckable(True)
ff_frame.setChecked(False)
ff_frame.setFlat(True)
self.found_files_list.setVisible(False)
return self.found_files_frame
def keyPressEvent(self, event):
'''
Enable the shortcats for search and replace
'''
self.parent().keyPressEvent(event)
def on_search(self):
'''
Initiate the new search or request a next search result.
'''
if self.current_search_text != self.search_field.text(
) or self._search_recursive != self.found_files_frame.isChecked():
# clear current search results
self._reset()
self.current_search_text = self.search_field.text()
if self.current_search_text:
path_text = {}
self._wait_for_result = True
for i in range(self._tabwidget.count()):
path_text[self._tabwidget.widget(
i).filename] = self._tabwidget.widget(
i).document().toPlainText()
self._search_recursive = self.found_files_frame.isChecked()
self._search_thread = TextSearchThread(
self.current_search_text,
self._tabwidget.currentWidget().filename,
path_text=path_text,
recursive=self._search_recursive)
self._search_thread.search_result_signal.connect(
self.on_search_result)
self._search_thread.warning_signal.connect(
self.on_warning_result)
self._search_thread.start()
elif self.search_results:
self._check_position()
if self.search_results:
if self._search_result_index + 1 >= len(self.search_results):
self._search_result_index = -1
self._search_result_index += 1
self.search_result_signal.emit(
*self.search_results[self._search_result_index])
self.replace_button.setEnabled(True)
self._update_label()
def on_search_back(self):
'''
Slot to handle the search back function.
'''
self._check_position(False)
if self.search_results:
self._search_result_index -= 1
if self._search_result_index < 0:
self._search_result_index = len(self.search_results) - 1
self._update_label()
self.search_result_signal.emit(
*self.search_results[self._search_result_index])
self.replace_button.setEnabled(True)
def _check_position(self, forward=True):
try:
# if the position of the textCursor was changed by the user, move the search index
cur_pos = self._tabwidget.currentWidget().textCursor().position()
st, _f, pa, idx = self.search_results[self._search_result_index]
sear_pos = idx + len(st)
if cur_pos != sear_pos:
first_idx = self._get_current_index_for_current_file()
if first_idx != -1:
st, _f, pa, idx = self.search_results[first_idx]
sear_pos = idx + len(st)
while cur_pos > sear_pos and self._tabwidget.currentWidget(
).filename == pa:
first_idx += 1
st, _f, pa, idx = self.search_results[first_idx]
sear_pos = idx + len(st)
self._search_result_index = first_idx
if forward:
self._search_result_index -= 1
else:
self._reset(True)
except:
pass
def _get_current_index_for_current_file(self):
for index in range(len(self.search_results)):
_st, _f, pa, _idx = self.search_results[index]
if self._tabwidget.currentWidget().filename == pa:
return index
return -1
def on_search_result(self, search_text, found, path, index):
'''
Slot to handle the signals for search result. This signals are forwarded used
search_result_signal.
'''
if found and search_text == self.current_search_text:
self.search_results_fileset.add(path)
item = (search_text, found, path, index)
if item not in self.search_results:
self.search_results.append((search_text, found, path, index))
if self._wait_for_result:
self._search_result_index += 1
if index >= self._tabwidget.currentWidget().textCursor(
).position() or self._tabwidget.currentWidget(
).filename != path:
self._wait_for_result = False
self.search_result_signal.emit(
*self.search_results[self._search_result_index])
self.replace_button.setEnabled(True)
if self.search_results:
if len(self.search_results_fileset) > 1:
for item in self.search_results_fileset:
pkg, path = package_name(os.path.dirname(item))
itemstr = '%s [%s]' % (os.path.basename(item), pkg)
if not self.found_files_list.findItems(
itemstr, Qt.MatchExactly):
list_item = QListWidgetItem(itemstr)
list_item.setToolTip(item)
self.found_files_list.addItem(list_item)
self.found_files_frame.setVisible(True)
self.found_files_list.setVisible(
len(self.search_results_fileset) > 0)
self._update_label()
def on_warning_result(self, text):
rospy.logwarn(text)
def on_replace_click(self):
self.on_replace()
self.on_search()
def on_replace(self):
'''
Emits the replace signal, but only if currently selected text is equal to the searched one.
'''
if self.search_results:
try:
search_text, _found, path, index = self.search_results[
self._search_result_index]
cursor = self._tabwidget.currentWidget().textCursor()
if cursor.selectedText() == search_text:
rptxt = self.replace_field.text()
for rindex in range(self._search_result_index + 1,
len(self.search_results)):
st, _f, pa, idx = self.search_results[rindex]
if path == pa:
self.search_results.pop(rindex)
self.search_results.insert(
rindex,
(st, _f, pa, idx + len(rptxt) - len(st)))
else:
break
self._remove_search_result(self._search_result_index)
self.replace_signal.emit(search_text, path, index, rptxt)
else:
self.replace_button.setEnabled(False)
except:
import traceback
print traceback.format_exc()
pass
def on_itemActivated(self, item):
'''
Go to the results for the selected file entry in the list.
'''
item_path = item.toolTip()
new_search_index = -1
tmp_index = -1
search_index = -1
tmp_search_text = ''
for search_text, found, path, index in self.search_results:
new_search_index += 1
if item_path == path:
if tmp_index == -1:
tmp_index = new_search_index
tmp_search_text = search_text
search_index = index
if new_search_index > self._search_result_index:
self.search_result_signal.emit(search_text, found, path,
index)
self._search_result_index = new_search_index
self._update_label()
return
if tmp_index != -1:
self.search_result_signal.emit(tmp_search_text, True, item_path,
search_index)
self._search_result_index = tmp_index
self._update_label()
def on_search_text_changed(self, _text):
'''
Clear search result if the text was changed.
'''
self._reset()
def _update_label(self, clear_label=False):
'''
Updates the status label for search results. The info is created from search result lists.
'''
msg = ' '
if self.search_results:
count_files = len(self.search_results_fileset)
msg = '%d/%d' % (self._search_result_index + 1,
len(self.search_results))
if count_files > 1:
msg = '%s(%d)' % (msg, count_files)
if self._search_thread is not None and self._search_thread.is_alive():
msg = 'searching..%s' % msg
elif not msg.strip() and self.current_search_text:
msg = '0 found'
self.current_search_text = ''
if clear_label:
msg = ' '
self.search_result_label.setText(msg)
self.find_button_back.setEnabled(len(self.search_results))
def file_changed(self, path):
'''
Clears search results if for changed file are some search results are available
:param path: changed file path
:type path: str
'''
if path in self.search_results_fileset:
self._reset()
def set_replace_visible(self, value):
self.rplc_frame.setVisible(value)
self.raise_()
self.activateWindow()
if value:
self.replace_field.setFocus()
self.replace_field.selectAll()
self.setWindowTitle("Find / Replace")
else:
self.setWindowTitle("Find")
self.search_field.setFocus()
def is_replace_visible(self):
return self.rplc_frame.isVisible()
def _reset(self, force_new_search=False):
# clear current search results
if self._search_thread is not None:
self._search_thread.search_result_signal.disconnect()
self._search_thread.stop()
self._search_thread = None
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self.found_files_list.clear()
self.found_files_list.setVisible(False)
self._update_label(True)
self._search_result_index = -1
self.find_button_back.setEnabled(False)
if force_new_search:
self.on_search()
def enable(self):
self.setVisible(True)
# self.show()
self.raise_()
self.activateWindow()
self.search_field.setFocus()
self.search_field.selectAll()
def _remove_search_result(self, index):
try:
self.search_results.pop(index)
# create new set with files contain the search text
new_path_set = set(path
for _st, _fd, path, _idx in self.search_results)
# remove the file from the list widget
for pp in self.search_results_fileset - new_path_set:
for wi_idx in range(self.found_files_list.count()):
# we have to compare each tooltip of the item
if pp == self.found_files_list.item(wi_idx).toolTip():
self.found_files_list.takeItem(wi_idx)
break
self.search_results_fileset = new_path_set
self.found_files_list.setVisible(
len(self.search_results_fileset) > 0)
except:
import traceback
print traceback.format_exc()
class LogicalMarkerPlugin(Plugin):
def __init__(self, context):
super(LogicalMarkerPlugin, self).__init__(context)
# Create an image for the original map. This will never change.
try:
self.map_yaml_file_str = rospy.get_param("~map_file")
self.data_directory = rospy.get_param("~data_directory")
except KeyError:
rospy.logfatal(
"~map_file and ~data_directory need to be set to use the logical marker"
)
return
map_image_location = getImageFileLocation(self.map_yaml_file_str)
map = loadMapFromFile(self.map_yaml_file_str)
locations_file = getLocationsFileLocationFromDataDirectory(
self.data_directory)
doors_file = getDoorsFileLocationFromDataDirectory(self.data_directory)
objects_file = getObjectsFileLocationFromDataDirectory(
self.data_directory)
# Give QObjects reasonable names
self.setObjectName('LogicalMarkerPlugin')
# Create QWidget
self.master_widget = QWidget()
self.master_layout = QVBoxLayout(self.master_widget)
# Main Functions - Doors, Locations, Objects
self.function_layout = QHBoxLayout()
self.master_layout.addLayout(self.function_layout)
self.function_buttons = []
self.current_function = None
for button_text in ['Locations', 'Doors', 'Objects']:
button = QPushButton(button_text, self.master_widget)
button.clicked[bool].connect(self.handle_function_button)
button.setCheckable(True)
self.function_layout.addWidget(button)
self.function_buttons.append(button)
self.function_layout.addStretch(1)
self.master_layout.addWidget(self.get_horizontal_line())
# Subfunction toolbar
self.subfunction_layout = QHBoxLayout()
clearLayoutAndFixHeight(self.subfunction_layout)
self.master_layout.addLayout(self.subfunction_layout)
self.current_subfunction = None
self.master_layout.addWidget(self.get_horizontal_line())
self.image = MapImage(map_image_location, self.master_widget)
self.master_layout.addWidget(self.image)
self.master_layout.addWidget(self.get_horizontal_line())
# Configuration toolbar
self.configuration_layout = QHBoxLayout()
clearLayoutAndFixHeight(self.configuration_layout)
self.master_layout.addLayout(self.configuration_layout)
# Add a stretch at the bottom.
self.master_layout.addStretch(1)
self.master_widget.setObjectName('LogicalMarkerPluginUI')
if context.serial_number() > 1:
self.master_widget.setWindowTitle(
self.master_widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self.master_widget)
# Activate the functions
self.functions = {}
self.functions['Locations'] = LocationFunction(
locations_file, map, self.master_widget, self.subfunction_layout,
self.configuration_layout, self.image)
self.functions['Doors'] = DoorFunction(
doors_file, map, self.functions['Locations'], self.master_widget,
self.subfunction_layout, self.configuration_layout, self.image)
self.functions['Objects'] = ObjectFunction(
objects_file, map, self.functions['Locations'], self.master_widget,
self.subfunction_layout, self.configuration_layout, self.image)
def construct_layout(self):
pass
def get_horizontal_line(self):
"""
http://stackoverflow.com/questions/5671354/how-to-programmatically-make-a-horizontal-line-in-qt
"""
hline = QFrame()
hline.setFrameShape(QFrame.HLine)
hline.setFrameShadow(QFrame.Sunken)
return hline
def handle_function_button(self):
source = self.sender()
if source.text() == self.current_function:
source.setChecked(True)
return
# Depress all other buttons.
for button in self.function_buttons:
if button != source:
button.setChecked(False)
if self.current_function is not None:
self.functions[self.current_function].deactivateFunction()
self.current_function = source.text()
# Clear all subfunction buttons.
clearLayoutAndFixHeight(self.subfunction_layout)
if self.current_function is not None:
self.functions[self.current_function].activateFunction()
def shutdown_plugin(self):
modified = False
for function in self.functions:
if self.functions[function].isModified():
modified = True
if modified:
ret = QMessageBox.warning(
self.master_widget, "Save",
"The logical map has been modified.\n"
"Do you want to save your changes?",
QMessageBox.Save | QMessageBox.Discard)
if ret == QMessageBox.Save:
for function in self.functions:
self.functions[function].saveConfiguration()
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class ControlModeWidget:
def __init__(self, context):
self.control_mode = 0
self.mode_pub = rospy.Publisher('/flor/controller/mode_command', VigirControlModeCommand, queue_size=10)
self._widget = context
self.vbox = QVBoxLayout()
#Add input for setting the spindle speed
list_label = QLabel("Select Control Mode")
self.vbox.addWidget(list_label)
# Indexed list of allowed control modes from feedback
self.allowed_modes = rospy.get_param("/atlas_controller/allowed_control_modes")
self.allowed_modes=rospy.get_param("/atlas_controller/allowed_control_modes")
self.mode_ids={}
self.mode_ids
for ndx,txt in enumerate(self.allowed_modes):
self.mode_ids[txt] = ndx
# API 2.13 ordering
self.bdi_mode_names=['NONE ', 'FREEZE ', 'STAND_PREP ', \
'STAND ', 'WALK ', 'STEP ', 'MANIPULATE ', \
'USER ', 'CALIBRATE ']
self.list_box = QListWidget(None)
self.list_box.addItems(self.allowed_modes)
self.list_box.currentItemChanged.connect(self.handle_selection_change)
self.vbox.addWidget(self.list_box)
self.selection_label = QLabel("Flor Selected: "+self.allowed_modes[0]+"("+str(self.control_mode)+")")
self.vbox.addWidget(self.selection_label)
self.label = QLabel("Flor Command : "+self.allowed_modes[0]+"("+str(self.control_mode)+")")
self.vbox.addWidget(self.label)
self.flor_mode = 0
self.bdi_current_mode = 0
self.bdi_desired_mode = 0
self.flor_mode_label = QLabel("Flor Mode : "+self.allowed_modes[self.flor_mode]+"("+str(self.flor_mode)+")"+" BDI:("+str(self.bdi_current_mode)+", "+str(self.bdi_desired_mode)+")")
self.vbox.addWidget(self.flor_mode_label)
#Add combo box for available settings
self.available_modes = QComboBox();
self.available_modes.addItem("");
self.available_modes.addItem("BDI");
self.available_modes.addItem("Enable Upper Body");
self.available_modes.addItem("Enable Whole Body");
self.available_modes.currentIndexChanged.connect(self.handle_avail_modes_changed)
self.vbox.addWidget(self.available_modes);
self.vbox.addStretch(1)
#Add Button for sending the behavior mode command
self.push_button = QPushButton("Set Mode")
self.push_button.clicked.connect(self.handle_set_mode)
self.vbox.addWidget(self.push_button)
self.vbox.addStretch(1)
hbox = QHBoxLayout()
hbox.addStretch(1)
self.stop_enable= QCheckBox()
self.stop_enable.setChecked(False)
hbox.addWidget(self.stop_enable)
self.stop_enable.clicked.connect(self.handle_stop_enable)
self.stop_button = QPushButton("STOP!")
self.stop_button.clicked.connect(self.handle_stop)
self.stop_button.setStyleSheet('QPushButton {background-color: gray }')
hbox.addWidget(self.stop_button)
hbox.addStretch(1)
self.vbox.addLayout(hbox)
self._widget.setLayout(self.vbox)
#add stretch at end so all GUI elements are at top of dialog
self.vbox.addStretch(1)
self.flor_mode_cmd_sub = rospy.Subscriber("/flor/controller/mode", FlorControlMode, self.florModeCallback)
def shutdown_plugin(self):
print "Shutting down ..."
self.flor_mode_cmd_sub.unregister()
self.mode_pub.unregister()
print "Done!"
# Update BDI state
def simStateCallback(self, state):
if ((self.bdi_current_state != state.current_behavior) or (self.bdi_desired_state != state.desired_behavior) or (self.bdi_error_code != state.error_code) or (self.bdi_behavior_status != state.behavior_feedback.status_flags) ):
self.bdi_current_state = state.current_behavior
self.bdi_desired_state = state.desired_behavior
self.bdi_error_code = state.error_code
self.bdi_behavior_status = state.behavior_feedback.status_flags
if ((self.bdi_current_state < 0) or (self.bdi_current_state >= length(self.bdi_mode_names))):
self.bdi_state_label.setText( " BDI State : "+self.bdi_mode_names[0]+"("+str(self.bdi_current_state)+", " + str(self.bdi_desired_state) + ") EC:("+str(self.bdi_error_code)+", " + str(self.bdi_behavior_status) +")")
else:
self.bdi_state_label.setText( " BDI State : "+self.bdi_mode_names[self.bdi_current_state]+"("+str(self.bdi_current_state)+", " + str(self.bdi_desired_state) + ") EC:("+str(self.bdi_error_code)+", " + str(self.bdi_behavior_status) +")")
def simCommandCallback(self, bdi_cmd):
if (self.bdi_behavior != bdi_cmd.behavior) :
self.bdi_behavior = bdi_cmd.behavior
self.bdi_command_label.setText(" BDI Cmd : "+self.bdi_mode_names[self.bdi_behavior]+"("+str(self.bdi_behavior)+")")
def florModeCallback(self, cmd):
if ( (self.flor_mode != cmd.control_mode) or (self.bdi_current_mode != cmd.bdi_current_behavior) or (self.bdi_desired_mode != cmd.bdi_desired_behavior) ):
if (self.flor_mode != cmd.control_mode) and (self.control_mode != cmd.control_mode):
print "Flor mode changed externally - clear selection"
self.clearSelections()
self.selection_label.setText("Flor Selected : ")
self.label.setText("Flor Command : ")
self.flor_mode = cmd.control_mode
self.bdi_current_mode = cmd.bdi_current_behavior
self.bdi_desired_mode = cmd.bdi_desired_behavior
print "Flor mode: ", self.flor_mode, " BDI: ",self.bdi_current_mode,", ",self.bdi_desired_mode
if (self.flor_mode > 250):
print "Invalid control mode "
self.flor_mode = 0
#print "Allowed modes:"
#print self.allowed_modes
self.flor_mode_label.setText(" Flor Mode : "+self.allowed_modes[self.flor_mode]+"("+str(self.flor_mode)+") BDI:("+str(self.bdi_current_mode)+","+str(self.bdi_desired_mode)+")")
def clearSelections(self):
for i in range(self.list_box.count()):
item = self.list_box.item(i)
self.list_box.setItemSelected(item, False)
self.list_box.setCurrentRow(-1)
#Slot for selecting
def handle_selection_change(self, curr, prev):
if (curr != None):
self.control_mode = self.mode_ids[curr.text()]
self.selection_label.setText("Flor Selected : "+curr.text()+"("+str(self.control_mode)+")")
#else:
# print "NULL selection"
#self.label.setText(self.allowed_modes[selected])
#self.spindle_speed_pub.publish(data=math.radians(degree_per_sec))
@Slot(bool)
def handle_set_mode(self):
print "Selected=",self.allowed_modes[self.control_mode], ' id=', self.control_mode
self.label.setText("Flor Command : "+self.allowed_modes[self.control_mode]+"("+str(self.control_mode)+")")
mode_msg = VigirControlModeCommand()
mode_msg.header.stamp = rospy.Time.now()
mode_msg.requested_control_mode = self.control_mode
print mode_msg
self.mode_pub.publish(mode_msg)
@Slot(bool)
def handle_stop(self):
# FLOR_STOP command
if (self.stop_enable.isChecked()):
self.clearSelections()
self.selection_label.setText("Flor Selected : ")
self.control_mode = self.mode_ids['stop']
print "Selected=",self.allowed_modes[self.control_mode], ' id=', self.control_mode
#self.list_box
self.selection_label.setText("Flor Selected : "+self.allowed_modes[self.control_mode]+"("+str(self.control_mode)+")")
self.label.setText("Flor Command : "+self.allowed_modes[self.control_mode]+"("+str(self.control_mode)+")")
mode_msg = VigirControlModeCommand()
mode_msg.header.stamp = rospy.Time.now()
mode_msg.requested_control_mode = self.control_mode
print mode_msg
self.mode_pub.publish(mode_msg)
else:
print "Stop disabled!"
@Slot(bool)
def handle_stop_enable(self):
if (self.stop_enable.isChecked()):
self.stop_button.setStyleSheet('QPushButton {background-color: red }')
#self.stop_enable.setChecked(False)
else:
self.stop_button.setStyleSheet('QPushButton {background-color: gray }')
#self.stop_enable.setChecked(True)
@Slot(bool)
def handle_avail_modes_changed(self, index):
print 'no longer mapping allowable modes'
return
