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