class FootstepParamControlWidget(QObject):
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 shutdown_plugin(self):
print "Shutdown ..."
self.param_pub.unregister()
print "Done!"
def getParams( self ):
msg = FootstepPlannerParams()
msg.change_mask = 0
self.param_pub.publish( msg )
def sendParams( self ):
msg = FootstepPlannerParams()
msg.change_mask = 0
if self.step_cost_groupbox.isChecked():
msg.change_mask = msg.change_mask | 0x001
if self.collision_check_groupbox.isChecked():
msg.change_mask = msg.change_mask | 0x010
if self.foot_size_groupbox.isChecked():
msg.change_mask = msg.change_mask | 0x020
if self.upper_groupbox.isChecked():
msg.change_mask = msg.change_mask | 0x040
if self.std_step_groupbox.isChecked():
msg.change_mask = msg.change_mask | 0x080
if self.terrain_model_groupbox.isChecked():
msg.change_mask = msg.change_mask | 0x100
### STEP_COST_ESTIMATOR ########
msg.step_cost_type = self.step_cost_combobox.currentIndex()
# #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 ########
msg.collision_check_type = 0
if self.collision_check_feet_checkbox.isChecked():
msg.collision_check_type = msg.collision_check_type | 0x01
if self.collision_check_ub_checkbox.isChecked():
msg.collision_check_type = msg.collision_check_type | 0x02
if self.collision_check_fc_checkbox.isChecked():
msg.collision_check_type = msg.collision_check_type | 0x04
### FOOT_SIZE ########
msg.foot_size.x = self.foot_size_x.value()
msg.foot_size.y = self.foot_size_y.value()
msg.foot_size.z = self.foot_size_z.value()
msg.foot_origin_shift.x = self.foot_shift_x.value()
msg.foot_origin_shift.y = self.foot_shift_y.value()
msg.foot_origin_shift.z = self.foot_shift_z.value()
### UPPER_BODY_SIZE ########
msg.upper_body_size.x = self.upper_size_x.value()
msg.upper_body_size.y = self.upper_size_y.value()
msg.upper_body_size.z = self.upper_size_z.value()
msg.upper_body_origin_shift.x = self.upper_origin_x.value()
msg.upper_body_origin_shift.y = self.upper_origin_y.value()
msg.upper_body_origin_shift.z = self.upper_origin_z.value()
### TERRAIN_MODEL ########
msg.use_terrain_model = self.terrain_model_checkbox.isChecked()
msg.min_sampling_steps_x = self.terrain_min_ssx_val.value()
msg.min_sampling_steps_y = self.terrain_min_ssy_val.value()
msg.max_sampling_steps_x = self.terrain_max_ssx_val.value()
msg.max_sampling_steps_y = self.terrain_max_ssy_val.value()
msg.max_intrusion_z = self.terrain_max_iz_val.value()
msg.max_ground_clearance = self.terrain_max_gc_val.value()
msg.minimal_support = self.terrain_ms_val.value()
### STANDARD_STEP_PARAMS ########
msg.foot_seperation = self.foot_sep_val.value()
msg.step_duration = self.std_step_step_duration_val.value()
msg.sway_duration = self.std_step_sway_duration_val.value()
msg.swing_height = self.std_step_swing_height_val.value()
msg.lift_height = self.std_step_lift_height_val.value()
print msg
self.param_pub.publish( msg )
def getParamCallbackFcn(self, msg):
### STEP_COST_ESTIMATOR ########
if msg.change_mask & 0x001:
self.step_cost_combobox.setCurrentIndex( msg.step_cost_type )
# #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 ########
if msg.change_mask & 0x010:
self.collision_check_feet_checkbox.setChecked( msg.collision_check_type & 0x01 )
self.collision_check_ub_checkbox.setChecked( msg.collision_check_type & 0x02 )
self.collision_check_fc_checkbox.setChecked( msg.collision_check_type & 0x04 )
### FOOT_SIZE ########
if msg.change_mask & 0x020:
self.foot_size_x.setValue( msg.foot_size.x )
self.foot_size_y.setValue( msg.foot_size.y )
self.foot_size_z.setValue( msg.foot_size.z )
self.foot_shift_x.setValue( msg.foot_origin_shift.x )
self.foot_shift_y.setValue( msg.foot_origin_shift.y )
self.foot_shift_z.setValue( msg.foot_origin_shift.z )
### UPPER_BODY_SIZE ########
if msg.change_mask & 0x040:
self.upper_size_x.setValue( msg.upper_body_size.x )
self.upper_size_y.setValue( msg.upper_body_size.y )
self.upper_size_z.setValue( msg.upper_body_size.z )
self.upper_origin_x.setValue( msg.upper_body_origin_shift.x )
self.upper_origin_y.setValue( msg.upper_body_origin_shift.y )
self.upper_origin_z.setValue( msg.upper_body_origin_shift.z )
### STANDARD_STEP_PARAMS ########
if msg.change_mask & 0x080:
self.foot_sep_val.setValue( msg.foot_seperation )
self.std_step_step_duration_val.setValue( msg.step_duration )
self.std_step_sway_duration_val.setValue( msg.sway_duration )
self.std_step_swing_height_val.setValue( msg.swing_height )
self.std_step_lift_height_val.setValue( msg.lift_height )
### TERRAIN_MODEL ########
if msg.change_mask & 0x100:
self.terrain_model_checkbox.setChecked( msg.use_terrain_model )
self.terrain_min_ssx_val.setValue( msg.min_sampling_steps_x )
self.terrain_min_ssy_val.setValue( msg.min_sampling_steps_y )
self.terrain_max_ssx_val.setValue( msg.max_sampling_steps_x )
self.terrain_max_ssy_val.setValue( msg.max_sampling_steps_y )
self.terrain_max_iz_val.setValue( msg.max_intrusion_z )
self.terrain_max_gc_val.setValue( msg.max_ground_clearance )
self.terrain_ms_val.setValue( msg.minimal_support )