def __init__(self): self.app = ConsoleApp() self.view = self.app.createView() self.robotSystem = robotsystem.create(self.view) self.config = drcargs.getDirectorConfig() jointGroups = self.config['teleopJointGroups'] self.jointTeleopPanel = JointTeleopPanel(self.robotSystem, jointGroups) self.jointCommandPanel = JointCommandPanel(self.robotSystem) self.jointCommandPanel.ui.speedSpinBox.setEnabled(False) self.jointCommandPanel.ui.mirrorArmsCheck.setChecked(self.jointTeleopPanel.mirrorArms) self.jointCommandPanel.ui.mirrorLegsCheck.setChecked(self.jointTeleopPanel.mirrorLegs) self.jointCommandPanel.ui.resetButton.connect('clicked()', self.resetJointTeleopSliders) self.jointCommandPanel.ui.mirrorArmsCheck.connect('clicked()', self.mirrorJointsChanged) self.jointCommandPanel.ui.mirrorLegsCheck.connect('clicked()', self.mirrorJointsChanged) self.widget = QtGui.QWidget() gl = QtGui.QGridLayout(self.widget) gl.addWidget(self.app.showObjectModel(), 0, 0, 4, 1) # row, col, rowspan, colspan gl.addWidget(self.view, 0, 1, 1, 1) gl.addWidget(self.jointCommandPanel.widget, 1, 1, 1, 1) gl.addWidget(self.jointTeleopPanel.widget, 0, 2, -1, 1) gl.setRowStretch(0,1) gl.setColumnStretch(1,1) #self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan) lcmUtils.addSubscriber('STEERING_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) lcmUtils.addSubscriber('THROTTLE_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
def __init__(self): self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan) lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause) self.animationTimer = None
def initDrakeTimeDisplay(): def onViewerDraw(msg): t = msg.timestamp * 1e-3 vis.updateText('sim time: %.3f' % t, 'sim time') lcmUtils.addSubscriber('DRAKE_VIEWER_DRAW', lcmbotcore.viewer_draw_t, onViewerDraw)
def __init__(self): self.app = ConsoleApp() self.view = self.app.createView() self.robotSystem = robotsystem.create(self.view) self.config = drcargs.getDirectorConfig() jointGroups = self.config['teleopJointGroups'] self.jointTeleopPanel = JointTeleopPanel(self.robotSystem, jointGroups) self.jointCommandPanel = JointCommandPanel(self.robotSystem) self.jointCommandPanel.ui.speedSpinBox.setEnabled(False) self.jointCommandPanel.ui.mirrorArmsCheck.setChecked(self.jointTeleopPanel.mirrorArms) self.jointCommandPanel.ui.mirrorLegsCheck.setChecked(self.jointTeleopPanel.mirrorLegs) self.jointCommandPanel.ui.resetButton.connect('clicked()', self.resetJointTeleopSliders) self.jointCommandPanel.ui.mirrorArmsCheck.connect('clicked()', self.mirrorJointsChanged) self.jointCommandPanel.ui.mirrorLegsCheck.connect('clicked()', self.mirrorJointsChanged) self.widget = QtGui.QWidget() gl = QtGui.QGridLayout(self.widget) gl.addWidget(self.app.showObjectModel(), 0, 0, 4, 1) # row, col, rowspan, colspan gl.addWidget(self.view, 0, 1, 1, 1) gl.addWidget(self.jointCommandPanel.widget, 1, 1, 1, 1) gl.addWidget(self.jointTeleopPanel.widget, 0, 2, -1, 1) gl.setRowStretch(0,1) gl.setColumnStretch(1,1) #self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan) lcmUtils.addSubscriber('STEERING_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) lcmUtils.addSubscriber('THROTTLE_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
def initSubscriber(self): self.subscriber = lcmUtils.addSubscriber( self.channel, optitrack_frame_t, self.onMessage) self.subscriber.setSpeedLimit(10) self.desc_subscriber = lcmUtils.addSubscriber( self.desc_channel, optitrack_data_descriptions_t, self.onDescMessage)
def setEnabled(self, enabled): if enabled and not self.subscriber: #self.subscriber = lcmUtils.addSubscriber('LCMGL.*', callback=self.onMessage) self.subscriber = lcmUtils.addSubscriber('OCTOMAP', callback=self.onMessage) self.subscriber = lcmUtils.addSubscriber('OCTOMAP_REF', callback=self.onMessage) self.subscriber = lcmUtils.addSubscriber('OCTOMAP_IN', callback=self.onMessage) elif not enabled and self.subscriber: lcmUtils.removeSubscriber(self.subscriber) self.subscriber = None
def setEnabled(self, enabled): if enabled and not self.subscriber: #self.subscriber = lcmUtils.addSubscriber('LCMGL.*', callback=self.onMessage) self.subscriber = lcmUtils.addSubscriber('OCTOMAP', callback=self.onMessage) self.subscriber = lcmUtils.addSubscriber('OCTOMAP_REF', callback=self.onMessage) self.subscriber = lcmUtils.addSubscriber('OCTOMAP_IN', callback=self.onMessage) elif not enabled and self.subscriber: lcmUtils.removeSubscriber(self.subscriber) self.subscriber = None
def start(self): sub = lcmUtils.addSubscriber('PLAN_EXECUTION_STATUS', lcmdrc.plan_status_t, self.handleStatus) sub.setSpeedLimit(0.2) self.subs.append(sub) self.subs.append( lcmUtils.addSubscriber('FOOTSTEP_PLAN_RESPONSE', lcmdrc.footstep_plan_t, self.handleFootstepPlan))
def _setupSubscriptions(self): lcmUtils.addSubscriber('PLAN_EXECUTION_STATUS', lcmdrc.plan_status_t, self.onControllerStatus) lcmUtils.addSubscriber('ATLAS_BATTERY_DATA', atlas.battery_data_t, self.onAtlasBatteryData) lcmUtils.addSubscriber('ATLAS_ELECTRIC_ARM_STATUS', atlas.electric_arm_status_t, self.onAtlasElectricArmStatus) lcmUtils.addSubscriber('CONTROLLER_RATE', lcmdrc.message_rate_t, self.onControllerRate) sub = lcmUtils.addSubscriber('ATLAS_STATUS', atlas.status_t, self.onAtlasStatus) sub.setSpeedLimit(60) sub = lcmUtils.addSubscriber('ATLAS_STATE_EXTRA', atlas.state_extra_t, self.onAtlasStateExtra) sub.setSpeedLimit(5)
def _addSubscribers(self): self.subscribers.append( lcmUtils.addSubscriber('DRAKE_VIEWER_LOAD_ROBOT', lcmdrake.lcmt_viewer_load_robot, self.onViewerLoadRobot)) self.subscribers.append( lcmUtils.addSubscriber('DRAKE_VIEWER_DRAW', lcmdrake.lcmt_viewer_draw, self.onViewerDraw)) self.subscribers.append( lcmUtils.addSubscriber('DRAKE_PLANAR_LIDAR_.*', lcmbot.planar_lidar_t, self.onPlanarLidar, callbackNeedsChannel=True))
def _addSubscribers(self): self.subscribers.append( lcmUtils.addSubscriber("DRAKE_VIEWER_LOAD_ROBOT", lcmrl.viewer_load_robot_t, self.onViewerLoadRobot) ) self.subscribers.append(lcmUtils.addSubscriber("DRAKE_VIEWER_DRAW", lcmrl.viewer_draw_t, self.onViewerDraw)) self.subscribers.append( lcmUtils.addSubscriber( "DRAKE_PLANAR_LIDAR_.*", lcmbot.planar_lidar_t, self.onPlanarLidar, callbackNeedsChannel=True ) ) self.subscribers.append( lcmUtils.addSubscriber( "DRAKE_POINTCLOUD_.*", lcmbot.pointcloud_t, self.onPointCloud, callbackNeedsChannel=True ) )
def __init__(self): self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmbotcore.robot_state_t, self.onJointPositionGoal) self.sub = lcmUtils.addSubscriber('SINGLE_JOINT_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause) self.debug = False if self.debug: self.app = ConsoleApp() self.view = self.app.createView() self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view) self.jointController.setPose('ATLAS_COMMAND', commandStream._currentCommandedPose) self.view.show() self.timer = TimerCallback(targetFps=30) self.timer.callback = self.onDebug
def __init__(self): self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmbotcore.robot_state_t, self.onJointPositionGoal) self.sub = lcmUtils.addSubscriber('SINGLE_JOINT_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause) self.debug = False if self.debug: self.app = ConsoleApp() self.view = self.app.createView() self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view) self.jointController.setPose('ATLAS_COMMAND', commandStream._currentCommandedPose) self.view.show() self.timer = TimerCallback(targetFps=30) self.timer.callback = self.onDebug
def _setupSubscriptions(self): lcmUtils.addSubscriber('PLAN_EXECUTION_STATUS', lcmdrc.plan_status_t, self.onControllerStatus) lcmUtils.addSubscriber('ATLAS_BATTERY_DATA', atlas.battery_data_t, self.onAtlasBatteryData) lcmUtils.addSubscriber('ATLAS_ELECTRIC_ARM_STATUS', atlas.electric_arm_status_t, self.onAtlasElectricArmStatus) lcmUtils.addSubscriber('CONTROLLER_RATE', lcmdrc.message_rate_t, self.onControllerRate) sub = lcmUtils.addSubscriber('ATLAS_STATUS', atlas.status_t, self.onAtlasStatus) sub.setSpeedLimit(60) sub = lcmUtils.addSubscriber('ATLAS_STATE_EXTRA', atlas.state_extra_t, self.onAtlasStateExtra) sub.setSpeedLimit(5)
def addLCMUpdater(self, channelName): ''' adds an lcm subscriber to update the joint positions from lcm robot_state_t messages ''' def onRobotStateMessage(msg): if self.ignoreOldStateMessages and self.lastRobotStateMessage is not None and msg.utime < self.lastRobotStateMessage.utime: return poseName = channelName pose = robotstate.convertStateMessageToDrakePose(msg) self.lastRobotStateMessage = msg # use joint name/positions from robot_state_t and append base_{x,y,z,roll,pitch,yaw} jointPositions = np.hstack((msg.joint_position, pose[:6])) jointNames = msg.joint_name + robotstate.getDrakePoseJointNames( )[:6] self.setPose(poseName, pose, pushToModel=False) for model in self.models: model.model.setJointPositions(jointPositions, jointNames) self.subscriber = lcmUtils.addSubscriber(channelName, bot_core.robot_state_t, onRobotStateMessage) self.subscriber.setSpeedLimit(60)
def __init__(self, ikPlanner): lcmUtils.addSubscriber('CANDIDATE_MANIP_PLAN', lcmdrc.robot_plan_w_keyframes_t, self.onManipPlan) lcmUtils.addSubscriber('CANDIDATE_ROBOT_PLAN_WITH_SUPPORTS',lcmdrc.robot_plan_with_supports_t, self.onManipPlan) self.lastManipPlan = None self.committedPlans = [] self.callbacks = callbacks.CallbackRegistry([self.PLAN_RECEIVED, self.PLAN_COMMITTED, self.USE_SUPPORTS]) self.ikPlanner = ikPlanner self.publishPlansWithSupports = False self.plansWithSupportsAreQuasistatic = True self.leftFootSupportEnabled = True self.rightFootSupportEnabled = True self.leftHandSupportEnabled = False self.rightHandSupportEnabled = False self.pelvisSupportEnabled = False
def __init__(self, channel): self.leftFootLink = drcargs.getDirectorConfig()['leftFootLink'] self.rightFootLink = drcargs.getDirectorConfig()['rightFootLink'] footContactSub = lcmUtils.addSubscriber( channel, lcmdrc.foot_contact_estimate_t, self.onFootContact) footContactSub.setSpeedLimit(60)
def changeSubscriptionAlt(self, newAltChannel="POSE_BODY_ALT"): # used to monitor a different pose e.g. POSE_BODY_LOGGED in playback self.altChannel = newAltChannel lcmUtils.removeSubscriber ( self.altSubscribe ) self.altSubscribe = lcmUtils.addSubscriber( self.altChannel , pose_t, self.onPoseAlt) self.altSubscribe.setSpeedLimit(60)
def changeSubscriptionBDI(self, newBDIChannel="POSE_BDI"): # used to monitor a different pose e.g. POSE_BODY_LOGGED in playback self.bdiChannel = newBDIChannel lcmUtils.removeSubscriber ( self.bdiSubcribe ) self.bdiSubcribe = lcmUtils.addSubscriber( self.bdiChannel , pose_t, self.onPoseBDI) self.bdiSubcribe.setSpeedLimit(60)
def add_subscriber(self): if (self._subscriber is not None): return self._subscriber = lcmUtils.addSubscriber( 'DRAKE_VIEWER_DRAW', messageClass=lcmbotcore.viewer_draw_t, callback=self.handle_message)
def __init__(self, jointController, footstepParams, request_channel='IRIS_REGION_REQUEST', response_channel='IRIS_REGION_RESPONSE'): self.jointController = jointController self.params = footstepParams self.map_mode_map = [ lcmdrc.footstep_plan_params_t.FOOT_PLANE, lcmdrc.footstep_plan_params_t.TERRAIN_HEIGHTS_AND_NORMALS, lcmdrc.footstep_plan_params_t.TERRAIN_HEIGHTS_Z_NORMALS, lcmdrc.footstep_plan_params_t.HORIZONTAL_PLANE ] self.request_channel = request_channel self.response_channel = response_channel self.regions = {} self.sub = lcmUtils.addSubscriber(self.response_channel, lcmdrc.iris_region_response_t, self.onIRISRegionResponse) self.segmentation_sub = lcmUtils.addSubscriber('IRIS_SEGMENTATION_RESPONSE', lcmdrc.iris_region_response_t, self.onIRISRegionResponse)
def __init__(self, channel=DEFAULT_CHANNEL, frame_names=[]): self._channel = channel self._frame_names = frame_names self._handlers = {} for frame_name in self._frame_names: self._handlers[frame_name] = LcmImageHandler() self._subscriber = lcmUtils.addSubscriber(channel, rl.image_array_t, self._on_message)
def add_subscriber(self): if (self._subscriber is not None): return self._subscriber = lcmUtils.addSubscriber( 'CASSIE_STATE', messageClass=dairlib.lcmt_robot_output, callback=self.handle_message)
def add_subscriber(self): if (self._subscriber is not None): return self._subscriber = lcmUtils.addSubscriber( 'DRAKE_VIEWER_DRAW', messageClass=lcmbotcore.viewer_draw_t, callback=self.handle_message)
def _add_subscriber(self): if (self._subscriber is not None): return self._subscriber = lcmUtils.addSubscriber( 'DRAKE_DRAW_FRAMES', messageClass = lcmrobotlocomotion.viewer_draw_t, callback = self._handle_message)
def __init__(self, jointController, footstepParams, request_channel='IRIS_REGION_REQUEST', response_channel='IRIS_REGION_RESPONSE'): self.jointController = jointController self.params = footstepParams self.map_mode_map = [ lcmdrc.footstep_plan_params_t.FOOT_PLANE, lcmdrc.footstep_plan_params_t.TERRAIN_HEIGHTS_AND_NORMALS, lcmdrc.footstep_plan_params_t.TERRAIN_HEIGHTS_Z_NORMALS, lcmdrc.footstep_plan_params_t.HORIZONTAL_PLANE ] self.request_channel = request_channel self.response_channel = response_channel self.regions = {} self.sub = lcmUtils.addSubscriber(self.response_channel, lcmdrc.iris_region_response_t, self.onIRISRegionResponse) self.segmentation_sub = lcmUtils.addSubscriber('IRIS_SEGMENTATION_RESPONSE', lcmdrc.iris_region_response_t, self.onIRISRegionResponse)
def __init__(self): self.widget = QtGui.QLabel('') self.widget.setFixedWidth(60) self.widget.setToolTip('Simulation time') self.widget.setAlignment(QtCore.Qt.AlignRight) self.setTime(0) self.subscriber = lcmUtils.addSubscriber(timeChannel, callback=self.onMessage)
def _add_subscriber(self): if (self._subscriber is not None): return self._subscriber = lcmUtils.addSubscriber( 'DRAKE_DRAW_FRAMES', messageClass=lcmrobotlocomotion.viewer_draw_t, callback=self._handle_message)
def __init__(self, channel=DEFAULT_CHANNEL, frame_names=[]): self._channel = channel self._frame_names = frame_names self._handlers = {} for frame_name in self._frame_names: self._handlers[frame_name] = LcmImageHandler() self._subscriber = lcmUtils.addSubscriber( channel, rl.image_array_t, self._on_message)
def __init__(self, statusBar): self.label = QtGui.QLabel('') statusBar.addPermanentWidget(self.label) self.sub = lcmUtils.addSubscriber( 'IIWA_STATUS', lcmdrake.lcmt_iiwa_status, self.onIiwaStatus) self.sub.setSpeedLimit(30) self.label.text = '[waiting for sim status]'
def _add_subscriber(self): if (self._subscriber is not None): return self._subscriber = lcmUtils.addSubscriber( 'DRAKE_POINT_CLOUD_.*', messageClass=lcmt_point_cloud, callback=self._handle_message, callbackNeedsChannel=True) self._subscriber.setNotifyAllMessagesEnabled(True)
def add_subscriber(self): if self._sub is not None: return self._sub = lcmUtils.addSubscriber( 'MARKER_ARROWS', messageClass=lcmdrakemsg.lcmt_arbitrary_arrow_collection, callback=self.handle_message) print self._name + " subscriber added."
def __init__(self): self.app = ConsoleApp() self.view = self.app.createView() self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view) self.jointController.setZeroPose() self.view.show() self.sub = lcmUtils.addSubscriber('ATLAS_COMMAND', lcmbotcore.atlas_command_t, self.onAtlasCommand) self.sub.setSpeedLimit(60)
def __init__(self): self.app = ConsoleApp() self.view = self.app.createView() self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view) self.jointController.setZeroPose() self.view.show() self.sub = lcmUtils.addSubscriber('ATLAS_COMMAND', lcmbotcore.atlas_command_t, self.onAtlasCommand) self.sub.setSpeedLimit(60)
def _addSubscribers(self): self.subscribers.append( lcmUtils.addSubscriber('DRAKE_VIEWER_LOAD_ROBOT', lcmrl.viewer_load_robot_t, self.onViewerLoadRobot)) self.subscribers.append( lcmUtils.addSubscriber('DRAKE_VIEWER_DRAW', lcmrl.viewer_draw_t, self.onViewerDraw)) self.subscribers.append( lcmUtils.addSubscriber('DRAKE_PLANAR_LIDAR_.*', lcmbot.planar_lidar_t, self.onPlanarLidar, callbackNeedsChannel=True)) self.subscribers.append( lcmUtils.addSubscriber('DRAKE_POINTCLOUD_.*', lcmbot.pointcloud_t, self.onPointCloud, callbackNeedsChannel=True))
def add_subscriber(self): if self._sub is not None: return self._sub = lcmUtils.addSubscriber( 'CONTACT_RESULTS', messageClass=lcmdrakemsg.lcmt_contact_results_for_viz, callback=self.handle_message) print self._name + " subscriber added."
def add_subscriber(self): if self._sub is not None: return self._sub = lcmUtils.addSubscriber( 'CONTACT_RESULTS', messageClass=lcmdrakemsg.lcmt_contact_results_for_viz, callback=self.handle_message) print(self._name + ' subscriber added.')
def _add_subscriber(self): if (self._subscriber is not None): return self._subscriber = lcmUtils.addSubscriber( 'DRAKE_DRAW_FRAMES.*', messageClass=lcmrobotlocomotion.viewer_draw_t, callback=self._handle_message, callbackNeedsChannel=True) self._subscriber.setNotifyAllMessagesEnabled(True)
def add_subscriber(self): if (self._subscriber is not None): return channel = "INPUT_SUPERVISOR_STATUS" self._subscriber = lcmUtils.addSubscriber( channel, messageClass=dairlib.lcmt_input_supervisor_status, callback=self.handle_message)
def __init__(self, statusBar): self.label = QtGui.QLabel('') statusBar.addPermanentWidget(self.label) self.sub = lcmUtils.addSubscriber('IIWA_STATUS', lcmdrake.lcmt_iiwa_status, self.onIiwaStatus) self.sub.setSpeedLimit(30) self.label.text = '[waiting for sim status]'
def setEnabled(self, enabled): if enabled and not self.subscriber0: self.subscriber0 = lcmUtils.addSubscriber('OBJECT_COLLECTION', callback=self.on_obj_collection_data) self.subscriber1 = lcmUtils.addSubscriber('LINK_COLLECTION', callback=self.on_link_collection_data) self.subscriber2 = lcmUtils.addSubscriber('POINTS_COLLECTION', callback=self.on_points_collection_data) self.subscriber3 = lcmUtils.addSubscriber('RESET_COLLECTIONS', callback=self.on_reset_collections_data) self.subscriber0.setNotifyAllMessagesEnabled(True) self.subscriber1.setNotifyAllMessagesEnabled(True) self.subscriber2.setNotifyAllMessagesEnabled(True) self.subscriber3.setNotifyAllMessagesEnabled(True) elif not enabled and self.subscriber0: lcmUtils.removeSubscriber(self.subscriber0) lcmUtils.removeSubscriber(self.subscriber1) lcmUtils.removeSubscriber(self.subscriber2) lcmUtils.removeSubscriber(self.subscriber3) self.subscriber0 = None self.subscriber1 = None self.subscriber2 = None self.subscriber3 = None
def __init__(self, ikPlanner): lcmUtils.addSubscriber('CANDIDATE_MANIP_PLAN', lcmdrc.robot_plan_w_keyframes_t, self.onManipPlan) lcmUtils.addSubscriber('CANDIDATE_ROBOT_PLAN_WITH_SUPPORTS', lcmdrc.robot_plan_with_supports_t, self.onManipPlan) self.lastManipPlan = None self.committedPlans = [] self.callbacks = callbacks.CallbackRegistry( [self.PLAN_RECEIVED, self.PLAN_COMMITTED, self.USE_SUPPORTS]) self.ikPlanner = ikPlanner self.publishPlansWithSupports = False self.plansWithSupportsAreQuasistatic = True self.leftFootSupportEnabled = True self.rightFootSupportEnabled = True self.leftHandSupportEnabled = False self.rightHandSupportEnabled = False self.pelvisSupportEnabled = False
def _add_subscriber(self): if (self._subscriber is not None): return self._subscriber = lcmUtils.addSubscriber( 'DRAKE_DRAW_FRAMES.*', messageClass=lcmrobotlocomotion.viewer_draw_t, callback=self._handle_message, callbackNeedsChannel=True) self._subscriber.setNotifyAllMessagesEnabled(True)
def add_subscriber(self): if self._sub is not None: return self._sub = lcmUtils.addSubscriber( 'CONTACT_RESULTS', messageClass=lcmdrakemsg.lcmt_contact_results_for_viz, callback=self.handle_message) # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) print self._name + " subscriber added."
def __init__(self, channel, statusBar=None): self.sub = lcmUtils.addSubscriber(channel, lcmbotcore.robot_state_t, self.onRobotState) self.label = QtGui.QLabel('') statusBar.addPermanentWidget(self.label) self.timer = TimerCallback(targetFps=10) self.timer.callback = self.showRate self.timer.start() self.l_foot_force_z = 0 self.r_foot_force_z = 0
def add_subscriber(self): if self._sub is not None: return self._sub = lcmUtils.addSubscriber( 'CONTACT_RESULTS', messageClass=lcmdrakemsg.lcmt_contact_results_for_viz, callback=self.handle_message) # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) print self._name + " subscriber added."
def __init__(self, channel, statusBar=None): self.sub = lcmUtils.addSubscriber(channel, lcmbotcore.robot_state_t, self.onRobotState) self.label = QtGui.QLabel('') statusBar.addPermanentWidget(self.label) self.timer = TimerCallback(targetFps=10) self.timer.callback = self.showRate self.timer.start() self.l_foot_force_z = 0 self.r_foot_force_z = 0
def _setupSubscriptions(self): useHistoricalLoader = False historicalLoader = lcmUtils.HistoricalLCMLoader('drc', 'software/drc_lcmtypes/lcmtypes', os.getenv('DRC_BASE')) if useHistoricalLoader else None lcmUtils.addSubscriber('FOOTSTEP_PLAN_RESPONSE', lcmdrc.footstep_plan_t, self.onFootstepPlan, historicalLoader) lcmUtils.addSubscriber('WALKING_TRAJ_RESPONSE', lcmdrc.robot_plan_t, self.onWalkingPlan) lcmUtils.addSubscriber('WALKING_SIMULATION_TRAJ_RESPONSE', lcmdrc.robot_plan_t, self.onWalkingPlan) ### Related to BDI-frame adjustment: self.bdiSubcribe = lcmUtils.addSubscriber( self.bdiChannel , pose_t, self.onPoseBDI) self.bdiSubcribe.setSpeedLimit(60) sub2 = lcmUtils.addSubscriber('BDI_ADJUSTED_FOOTSTEP_PLAN', lcmdrc.footstep_plan_t, self.onBDIAdjustedFootstepPlan) sub2.setSpeedLimit(1) # was 5 but was slow rendering
def add_subscriber(self): if self._subs is not None: return self._subs = [] self._subs.append( lcmUtils.addSubscriber( "DEFORMABLE_MESHES_INIT", messageClass=lcmdrakemsg. experimental_lcmt_deformable_tri_meshes_init, callback=self.handle_init_message, )) self._subs.append( lcmUtils.addSubscriber( "DEFORMABLE_MESHES_UPDATE", messageClass=lcmdrakemsg. experimental_lcmt_deformable_tri_meshes_update, callback=self.handle_update_message, )) print(self._name + " subscribers added.")
def __init__(self, channel): self.collection = OrderedDict() self.collectionId = newUUID() self.sentCommands = set() self.sentRequest = None self.channel = channel self.callbacks = callbacks.CallbackRegistry([self.DESCRIPTION_UPDATED_SIGNAL, self.DESCRIPTION_REMOVED_SIGNAL]) self.sub = lcmUtils.addSubscriber(self.channel, messageClass=lcmdrc.affordance_collection_t, callback=self._onCommandMessage) self.sub.setNotifyAllMessagesEnabled(True) self._modified()
def __init__(self, teleopPanel, teleopJointController, ikPlanner, view): lcmUtils.addSubscriber("GAMEPAD_CHANNEL", lcmdrc.gamepad_cmd_t, self.onGamepadCommand) self.speedMultiplier = 1.0 self.maxSpeedMultiplier = 3.0 self.minSpeedMultiplier = 0.2 self.travel = 1 self.angularTravel = 180 self.baseFrame = None self.resetDeltas() self.timer = TimerCallback() self.timer.callback = self.tick self.teleopPanel = teleopPanel self.teleopJointController = teleopJointController self.ikPlanner = ikPlanner self.view = view self.camera = view.camera() self.cameraTarget = np.asarray(self.camera.GetFocalPoint()) self.endEffectorFrames = [] self.endEffectorIndex = 0 self.teleopOn = False self.keyFramePoses = list() self.cameraMode = False self.timerStarted = False
def _addSubscriber(self): self.subscriber = lcmUtils.addSubscriber( 'DIRECTOR_TREE_VIEWER_REQUEST.*', lcmrl.viewer2_comms_t, self.onViewerRequest, callbackNeedsChannel=True) # Note: from discussion with @patmarion, there's a bug in the lcmUtils subscriber # when dealing with regex channels: # > If you subscribe to MY_CHANNEL_*, and two messages arrive back to back # > (MY_CHANNEL_FOO, foo_data) and (MY_CHANNEL_BAR, bar_data), then the default # > behavior in the lcm subscriber is to only call your callback once (if notify # > all messages = false), and it calls callback(MY_CHANNEL_FOO, bar_data) # # However, this can be avoided by notifying for *all* messages, which is what we # want anyway: self.subscriber.setNotifyAllMessagesEnabled(True)
def __init__(self, name, topic, sensorLocations, model, windowSize = 10): self.name = name self.sensorLocations = sensorLocations self.frameNames = self.getFrameNames() self.robotStateModel = model sub = lcmUtils.addSubscriber(topic, takktile.state_t, self.drawSpheres) sub.setSpeedLimit(60) self.active = False self.doTare = True self.taredSensorValues = {} self.tareCount = 0 self.tareWindow = windowSize self.frames = {}
def __init__(self, model, jointController, view): self.model = model self.jointController = jointController self.view = view self.colorNoHighlight = QtGui.QColor(190, 190, 190) self.colorHighlight = QtCore.Qt.red self.timer = TimerCallback() self.timer.callback = self.update self.timer.targetFps = 60 #self.timer.start() self.collisionStateIds = [] self.lastRobotStateMessage = None self.lastPlanMessage = None self.lastPlanCheckMessage = None lcmUtils.addSubscriber('EST_ROBOT_STATE', lcmdrc.robot_state_t, self.onRobotState) lcmUtils.addSubscriber('ROBOT_COLLISIONS', lcmdrc.robot_collision_array_t, self.onPlanCheck) lcmUtils.addSubscriber('CANDIDATE_MANIP_PLAN', lcmdrc.robot_plan_w_keyframes_t, self.onManipPlan) w = QtGui.QWidget() l = QtGui.QHBoxLayout(w) self.slider = QtGui.QSlider(QtCore.Qt.Horizontal) self.clearButton = QtGui.QPushButton('clear') self.zeroButton = QtGui.QPushButton('zero') l.addWidget(self.clearButton) l.addWidget(self.zeroButton) l.addWidget(self.slider) self.slider.connect(self.slider, 'valueChanged(int)', self.onSlider) self.slider.connect(self.zeroButton, 'clicked()', self.onZeroButtonClicked) self.slider.connect(self.clearButton, 'clicked()', self.onClearButtonClicked) ww = QtGui.QWidget() ll = QtGui.QVBoxLayout(ww) ll.addWidget(self.view) ll.addWidget(w) ll.setMargin(0) ww.show() ww.resize(800, 600) self.widget = ww
def addLCMUpdater(self, channelName): ''' adds an lcm subscriber to update the joint positions from lcm robot_state_t messages ''' def onRobotStateMessage(msg): if self.ignoreOldStateMessages and self.lastRobotStateMessage is not None and msg.utime < self.lastRobotStateMessage.utime: return poseName = channelName pose = robotstate.convertStateMessageToDrakePose(msg) self.lastRobotStateMessage = msg # use joint name/positions from robot_state_t and append base_{x,y,z,roll,pitch,yaw} jointPositions = np.hstack((msg.joint_position, pose[:6])) jointNames = msg.joint_name + robotstate.getDrakePoseJointNames()[:6] self.setPose(poseName, pose, pushToModel=False) for model in self.models: model.model.setJointPositions(jointPositions, jointNames) self.subscriber = lcmUtils.addSubscriber(channelName, lcmdrc.robot_state_t, onRobotStateMessage) self.subscriber.setSpeedLimit(60)
def __init__(self): self.aprilTagSubsciber = lcmUtils.addSubscriber('APRIL_TAG_TO_CAMERA_LEFT', lcmbotcore.rigid_transform_t, self.onAprilTag) pose = transformUtils.poseFromTransform(vtk.vtkTransform()) desc = dict(classname='MeshAffordanceItem', Name='polaris', Filename='software/models/polaris/polaris_cropped.vtp', pose=pose) self.pointcloudAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc) self.originFrame = self.pointcloudAffordance.getChildFrame() self.originToAprilTransform = transformUtils.transformFromPose(np.array([-0.038508 , -0.00282131, -0.01000079]), np.array([ 9.99997498e-01, -2.10472556e-03, -1.33815696e-04, 7.46246794e-04])) # offset for . . . who knows why # t = transformUtils.transformFromPose(np.array([ 0.14376024, 0.95920689, 0.36655712]), np.array([ 0.28745842, 0.90741428, -0.28822068, 0.10438304])) t = transformUtils.transformFromPose(np.array([ 0.10873244, 0.93162364, 0.40509084]), np.array([ 0.32997378, 0.88498408, -0.31780588, 0.08318602])) self.leftFootEgressStartFrame = vis.updateFrame(t, 'left foot start', scale=0.2,visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([ 0.265, 0.874, 0.274]), np.array([ 0.35290731, 0.93443693, -0.04181263, 0.02314636])) self.leftFootEgressMidFrame = vis.updateFrame(t, 'left foot mid', scale=0.2,visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([ 0.54339115, 0.89436275, 0.26681047]), np.array([ 0.34635985, 0.93680077, -0.04152008, 0.02674412])) self.leftFootEgressOutsideFrame = vis.updateFrame(t, 'left foot outside', scale=0.2,visible=True, parent=self.pointcloudAffordance) # pose = [np.array([-0.78962299, 0.44284877, -0.29539116]), np.array([ 0.54812954, 0.44571517, -0.46063251, 0.53731713])] #old location # pose = [np.array([-0.78594663, 0.42026626, -0.23248139]), np.array([ 0.54812954, 0.44571517, -0.46063251, 0.53731713])] # updated location pose = [np.array([-0.78594663, 0.42026626, -0.23248139]), np.array([ 0.53047159, 0.46554963, -0.48086192, 0.52022615])] # update orientation desc = dict(classname='CapsuleRingAffordanceItem', Name='Steering Wheel', uuid=newUUID(), pose=pose, Color=[1, 0, 0], Radius=float(0.18), Segments=20) self.steeringWheelAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc) pose = [np.array([-0.05907324, 0.80460545, 0.45439687]), np.array([ 0.14288327, 0.685944 , -0.703969 , 0.11615873])] desc = dict(classname='BoxAffordanceItem', Name='pedal', Dimensions=[0.12, 0.33, 0.04], pose=pose, Color=[0,1,0]) self.pedalAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc) # t = transformUtils.transformFromPose(np.array([ 0.04045136, 0.96565326, 0.25810111]), # np.array([ 0.26484648, 0.88360091, -0.37065556, -0.10825996])) # t = transformUtils.transformFromPose(np.array([ -4.34908919e-04, 9.24901627e-01, 2.65614116e-01]), # np.array([ 0.25022251, 0.913271 , -0.32136359, -0.00708626])) t = transformUtils.transformFromPose(np.array([ 0.0384547 , 0.89273742, 0.24140762]), np.array([ 0.26331831, 0.915796 , -0.28055337, 0.11519963])) self.leftFootPedalSwingFrame = vis.updateFrame(t,'left foot pedal swing', scale=0.2, visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([-0.9012598 , -0.05709763, 0.34897024]), np.array([ 0.03879584, 0.98950919, 0.03820214, 0.13381721])) self.leftFootDrivingFrame = vis.updateFrame(t,'left foot driving', scale=0.2, visible=True, parent=self.pointcloudAffordance) # t = transformUtils.transformFromPose(np.array([-0.12702725, 0.92068409, 0.27209386]), # np.array([ 0.2062255 , 0.92155886, -0.30781119, 0.11598529])) # t = transformUtils.transformFromPose(np.array([-0.14940375, 0.90347275, 0.23812658]), # np.array([ 0.27150909, 0.91398724, -0.28877386, 0.0867167 ])) # t = transformUtils.transformFromPose(np.array([-0.17331227, 0.87879312, 0.25645152]), # np.array([ 0.26344489, 0.91567196, -0.28089824, 0.11505581])) # self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([-0.12702725, 0.92068409, 0.27209386]), np.array([ 0.2062255 , 0.92155886, -0.30781119, 0.11598529])) self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([-0.13194951, 0.89871423, 0.24956246]), np.array([ 0.21589082, 0.91727326, -0.30088849, 0.14651633])) self.leftFootOnPedal = vis.updateFrame(t,'left foot on pedal', scale=0.2, visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([ 0.17712239, 0.87619935, 0.27001509]), np.array([ 0.33484372, 0.88280787, -0.31946488, 0.08044963])) self.leftFootUpFrame = vis.updateFrame(t,'left foot up frame', scale=0.2, visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([ 0.47214939, -0.04856998, 0.01375837]), np.array([ 6.10521653e-03, 4.18621358e-04, 4.65520611e-01, 8.85015882e-01])) self.rightHandGrabFrame = vis.updateFrame(t,'right hand grab bar', scale=0.2, visible=True, parent=self.pointcloudAffordance) self.frameSync = vis.FrameSync() self.frameSync.addFrame(self.originFrame) self.frameSync.addFrame(self.pointcloudAffordance.getChildFrame(), ignoreIncoming=True) self.frameSync.addFrame(self.leftFootEgressStartFrame, ignoreIncoming=True) self.frameSync.addFrame(self.leftFootEgressMidFrame, ignoreIncoming=True) self.frameSync.addFrame(self.leftFootEgressOutsideFrame, ignoreIncoming=True) self.frameSync.addFrame(self.steeringWheelAffordance.getChildFrame(), ignoreIncoming=True) self.frameSync.addFrame(self.pedalAffordance.getChildFrame(), ignoreIncoming=True) self.frameSync.addFrame(self.leftFootPedalSwingFrame, ignoreIncoming=True) self.frameSync.addFrame(self.leftFootDrivingFrame, ignoreIncoming=True) self.frameSync.addFrame(self.leftFootDrivingKneeInFrame, ignoreIncoming=True) self.frameSync.addFrame(self.leftFootOnPedal, ignoreIncoming=True) self.frameSync.addFrame(self.leftFootUpFrame, ignoreIncoming=True) self.frameSync.addFrame(self.rightHandGrabFrame, ignoreIncoming=True)
def start(self): sub = lcmUtils.addSubscriber('PLAN_EXECUTION_STATUS', lcmdrc.plan_status_t, self.handleStatus) sub.setSpeedLimit(0.2) self.subs.append(sub) self.subs.append(lcmUtils.addSubscriber('FOOTSTEP_PLAN_RESPONSE', lcmdrc.footstep_plan_t, self.handleFootstepPlan))
def __init__(self, channel): self.leftFootLink = drcargs.getDirectorConfig()['leftFootLink'] self.rightFootLink = drcargs.getDirectorConfig()['rightFootLink'] footContactSub = lcmUtils.addSubscriber(channel, lcmdrc.foot_contact_estimate_t, self.onFootContact) footContactSub.setSpeedLimit(60)
def initSubscriber(self): self.subscriber = lcmUtils.addSubscriber(self.channel, lcmvicon.vicon_t, self.onMessage) self.subscriber.setSpeedLimit(10)