def sendElectricArmEnabledState(self, enabledState): msg = lcmdrc.atlas_electric_arm_enable_t() msg.utime = getUtime() msg.num_electric_arm_joints = 6 assert len(enabledState) == msg.num_electric_arm_joints msg.enable = enabledState lcmUtils.publish("ATLAS_ELECTRIC_ARM_ENABLE", msg)
def sendControlMessage(command, data=None, channel='DRILL_CONTROL'): m = drill_control_t() m.utime = getUtime() m.control_type = command m.data = data if data is not None else [] m.data_length = len(m.data) lcmUtils.publish(channel, m)
def sendDesiredPumpPsi(self, desiredPsi): msg = lcmdrc.atlas_pump_command_t() msg.utime = getUtime() msg.k_psi_p = 0.0 # Gain on pressure error (A/psi) msg.k_psi_i = 0.0 # Gain on the integral of the pressure error (A/(psi/s) msg.k_psi_d = 0.0 # Gain on the derivative of the pressure error (A/(psi s) msg.k_rpm_p = 0.0 # Gain on rpm error (A / rpm) msg.k_rpm_i = 0.0 # Gain on the integral of the rpm error (A / (rpm s)) msg.k_rpm_d = 0.0 # Gain on the derivative of the rpm error (A / (rpm/s) msg.ff_rpm_d = 0.0 # Feed-forward gain on the desired rpm (A / rpm) msg.ff_psi_d = 0.0 # Feed-forward gain on the desired pressure (A / psi) msg.ff_const = 0.0 # Constant current term (Amps) msg.psi_i_max = 0.0 # Max. abs. value to which the integral psi error is clamped (psi s) msg.rpm_i_max = 0.0 # Max. abs. value to which the integral rpm error is clamped (rpm s) # Max. command output (A). Default is 60 Amps. # This value may need to be lower than the default in order to avoid # causing the motor driver to fault. msg.cmd_max = 60 msg.desired_psi = desiredPsi # default should be 1500 msg.desired_rpm = 5000 # default should be 5000 lcmUtils.publish("ATLAS_PUMP_COMMAND", msg)
def sendFOVRequest(channel, imagePoints): channelToImageType = { 'CAMERA_LEFT' : lcmdrc.data_request_t.CAMERA_IMAGE_HEAD_LEFT, 'CAMERACHEST_LEFT' : lcmdrc.data_request_t.CAMERA_IMAGE_LCHEST, 'CAMERACHEST_RIGHT' : lcmdrc.data_request_t.CAMERA_IMAGE_RCHEST, } dataRequest = lcmdrc.data_request_t() dataRequest.type = channelToImageType[channel] message = lcmdrc.subimage_request_t() message.data_request = dataRequest imagePoints = np.array([[pt[0], pt[1]] for pt in imagePoints]) minX, maxX = imagePoints[:,0].min(), imagePoints[:,0].max() minY, maxY = imagePoints[:,1].min(), imagePoints[:,1].max() message.x = minX message.y = minY message.w = maxX - minX message.h = maxY - minY #print message.x, message.y, message.w, message.h requestChannel = 'SUBIMAGE_REQUEST' lcmUtils.publish(requestChannel, message)
def publish(self): messages = self.reader.getMessages() for message in messages: channel = message[2] val = message[3] / 127.0 if channel is 102: self.msg.slider1 = val elif channel is 103: self.msg.slider2 = val elif channel is 104: self.msg.slider3 = val elif channel is 105: self.msg.slider4 = val elif channel is 106: self.msg.knob1 = val elif channel is 107: self.msg.knob2 = val elif channel is 108: self.msg.knob3 = val elif channel is 109: self.msg.knob4 = val elif channel is 110: self.msg.knob5 = val elif channel is 111: self.msg.knob6 = val elif channel is 112: self.msg.knob7 = val elif channel is 113: self.msg.knob8 = val if len(messages) is not 0: self.msg.utime = getUtime() lcmUtils.publish(self.lcmChannel, self.msg)
def sendWalkingPlanRequest(self, footstepPlan, startPose, waitForResponse=False, waitTimeout=5000, req_type='traj'): msg = lcmdrc.walking_plan_request_t() msg.utime = getUtime() state_msg = robotstate.drakePoseToRobotState(startPose) msg.initial_state = state_msg msg.new_nominal_state = msg.initial_state msg.use_new_nominal_state = True msg.footstep_plan = footstepPlan if req_type == 'traj': requestChannel = 'WALKING_TRAJ_REQUEST' responseChannel = 'WALKING_TRAJ_RESPONSE' response_type = lcmdrc.robot_plan_t elif req_type == 'controller': requestChannel = 'WALKING_CONTROLLER_PLAN_REQUEST' responseChannel = 'WALKING_CONTROLLER_PLAN_RESPONSE' response_type = lcmdrc.walking_plan_t elif req_type == 'simulate_drake': requestChannel = 'WALKING_SIMULATION_DRAKE_REQUEST' responseChannel = 'WALKING_SIMULATION_TRAJ_RESPONSE' response_type = lcmdrc.robot_plan_t else: raise ValueError("Invalid request type: {:s}".format(req_type)) if waitForResponse: if waitTimeout == 0: helper = lcmUtils.MessageResponseHelper(responseChannel, response_type) lcmUtils.publish(requestChannel, msg) return helper return lcmUtils.MessageResponseHelper.publishAndWait(requestChannel, msg, responseChannel, response_type, waitTimeout) else: lcmUtils.publish(requestChannel, msg)
def autoIRISSegmentation(self, xy_lb=[-2, -2], xy_ub=[2, 2], max_num_regions=10, default_yaw=np.nan, max_slope_angle=np.nan, max_height_variation=np.nan, plane_distance_tolerance=np.nan, plane_angle_tolerance=np.nan): msg = lcmdrc.auto_iris_segmentation_request_t() msg.utime = getUtime() A = np.vstack((np.eye(2), -np.eye(2))) b = np.hstack((xy_ub, -np.asarray(xy_lb))) msg.xy_bounds = irisUtils.encodeLinCon(A, b) msg.initial_state = robotstate.drakePoseToRobotState(self.jointController.q) msg.map_mode = self.map_mode_map[self.params.properties.map_mode] msg.num_seed_poses = 0 msg.max_num_regions = max_num_regions msg.region_id = self.getNewUIDs(max_num_regions) msg.default_yaw = default_yaw msg.max_slope_angle = max_slope_angle msg.max_height_variation = max_height_variation msg.plane_distance_tolerance = plane_distance_tolerance msg.plane_angle_tolerance = plane_angle_tolerance lcmUtils.publish('AUTO_IRIS_SEGMENTATION_REQUEST', msg)
def planFootDownAndCenterWeight(self): leftFootDownPlan, leftFootDownEndPose = self.planLeftFootDown() centerWeightPlan = self.planCenterWeight(startPose=leftFootDownEndPose) # now we need to combine these plans footDownEndTime = leftFootDownPlan.plan.plan[-1].utime robotPlan = leftFootDownPlan robotPlan.plan.plan_info = list(robotPlan.plan.plan_info) for state, info in zip(centerWeightPlan.plan.plan, centerWeightPlan.plan.plan_info): state.utime += footDownEndTime robotPlan.plan.plan.append(state) robotPlan.plan.plan_info.append(info) robotPlan.plan.num_states = len(robotPlan.plan.plan) # make support sequence for support, t in zip(centerWeightPlan.support_sequence.supports, centerWeightPlan.support_sequence.ts): t += footDownEndTime robotPlan.support_sequence.ts.append(t) robotPlan.support_sequence.supports.append(support) robotPlan.is_quasistatic = True self.addPlan(robotPlan) lcmUtils.publish('CANDIDATE_ROBOT_PLAN_WITH_SUPPORTS', robotPlan) return robotPlan
def sendFOVRequest(channel, imagePoints): channelToImageType = { 'CAMERA_LEFT': lcmdrc.data_request_t.CAMERA_IMAGE_HEAD_LEFT, 'CAMERACHEST_LEFT': lcmdrc.data_request_t.CAMERA_IMAGE_LCHEST, 'CAMERACHEST_RIGHT': lcmdrc.data_request_t.CAMERA_IMAGE_RCHEST, } dataRequest = lcmdrc.data_request_t() dataRequest.type = channelToImageType[channel] message = lcmdrc.subimage_request_t() message.data_request = dataRequest imagePoints = np.array([[pt[0], pt[1]] for pt in imagePoints]) minX, maxX = imagePoints[:, 0].min(), imagePoints[:, 0].max() minY, maxY = imagePoints[:, 1].min(), imagePoints[:, 1].max() message.x = minX message.y = minY message.w = maxX - minX message.h = maxY - minY #print message.x, message.y, message.w, message.h requestChannel = 'SUBIMAGE_REQUEST' lcmUtils.publish(requestChannel, message)
def processTraj(self, constraints, endPoseName="", nominalPoseName="", seedPoseName="", additionalTimeSamples=None): # Temporary fix / HACK / TODO (should be done in exotica_json) largestTspan = [0, 0] for constraintIndex, _ in enumerate(constraints): # Get tspan extend to normalise time-span if np.isfinite(constraints[constraintIndex].tspan[0]) and np.isfinite(constraints[constraintIndex].tspan[1]): largestTspan[0] = constraints[constraintIndex].tspan[0] if (constraints[constraintIndex].tspan[0] < largestTspan[0]) else largestTspan[0] largestTspan[1] = constraints[constraintIndex].tspan[1] if (constraints[constraintIndex].tspan[1] > largestTspan[1]) else largestTspan[1] # Temporary fix / HACK/ TODO to normalise time spans for constraintIndex, _ in enumerate(constraints): if np.isfinite(constraints[constraintIndex].tspan[0]) and np.isfinite(constraints[constraintIndex].tspan[1]): if largestTspan[1] != 0: constraints[constraintIndex].tspan[0] = constraints[constraintIndex].tspan[0] / largestTspan[1] constraints[constraintIndex].tspan[1] = constraints[constraintIndex].tspan[1] / largestTspan[1] listener = self.ikPlanner.getManipPlanListener() msg = self.setupMessage(constraints, endPoseName, nominalPoseName, seedPoseName, additionalTimeSamples) lcmUtils.publish('PLANNER_REQUEST', msg) lastManipPlan = listener.waitForResponse(timeout=20000) listener.finish() if lastManipPlan: # HACK: need to multiply by original trajectory length again (the one in the plan is planning and not real time), otherwise jumps to real hardware for state in lastManipPlan.plan: state.utime = state.utime * 50 self.ikPlanner.ikServer.infoFunc(lastManipPlan.plan_info[0]) return lastManipPlan, lastManipPlan.plan_info[0]
def autoIRISSegmentation(self, xy_lb=[-2, -2], xy_ub=[2, 2], max_num_regions=10, default_yaw=np.nan, max_slope_angle=np.nan, max_height_variation=np.nan, plane_distance_tolerance=np.nan, plane_angle_tolerance=np.nan): msg = lcmdrc.auto_iris_segmentation_request_t() msg.utime = getUtime() A = np.vstack((np.eye(2), -np.eye(2))) b = np.hstack((xy_ub, -np.asarray(xy_lb))) msg.xy_bounds = irisUtils.encodeLinCon(A, b) msg.initial_state = robotstate.drakePoseToRobotState( self.jointController.q) msg.map_mode = self.map_mode_map[self.params.properties.map_mode] msg.num_seed_poses = 0 msg.max_num_regions = max_num_regions msg.region_id = self.getNewUIDs(max_num_regions) msg.default_yaw = default_yaw msg.max_slope_angle = max_slope_angle msg.max_height_variation = max_height_variation msg.plane_distance_tolerance = plane_distance_tolerance msg.plane_angle_tolerance = plane_angle_tolerance lcmUtils.publish('AUTO_IRIS_SEGMENTATION_REQUEST', msg)
def sendFingerControl(self, positionA, positionB, positionC, force, velocity, scissor, mode): assert 0.0 <= positionA <= 254.0 assert 0.0 <= positionB <= 254.0 assert 0.0 <= positionC <= 254.0 assert 0.0 <= force <= 100.0 assert 0.0 <= velocity <= 100.0 assert 0 <= int(mode) <= 4 if not scissor is None: assert 0.0 <= scissor <= 254.0 msg = lcmrobotiq.command_t() msg.utime = getUtime() msg.activate = 1 msg.emergency_release = 0 msg.do_move = 1 msg.ifc = 1 msg.positionA = int(positionA) msg.positionB = int(positionB) msg.positionC = int(positionC) msg.force = int(254 * (force/100.0)) msg.velocity = int(254 * (velocity/100.0)) msg.mode = int(mode) if not scissor is None: msg.isc = 1 msg.positionS = int(scissor) channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper() lcmUtils.publish(channel, msg)
def publishPlanWithSupports(self, keyFramePlan, supportsList, ts, isQuasistatic): manipPlanner = self.robotSystem.manipPlanner msg_robot_plan_t = manipPlanner.convertKeyframePlan(keyFramePlan) supports = manipPlanner.getSupportLCMFromListOfSupports(supportsList,ts) msg_robot_plan_with_supports_t = manipPlanner.convertPlanToPlanWithSupports(msg_robot_plan_t, supports, ts, isQuasistatic) lcmUtils.publish('CANDIDATE_ROBOT_PLAN_WITH_SUPPORTS', msg_robot_plan_with_supports_t) return msg_robot_plan_with_supports_t
def onUseNewMapButton(self): ''' Send a trigger to the GPF to use the newly created map ''' msg = lcmdrc.utime_t() msg.utime = getUtime() lcmUtils.publish('STATE_EST_USE_NEW_MAP', msg)
def sendFingerControl(self, positionA, positionB, positionC, force, velocity, scissor, mode): assert 0.0 <= positionA <= 254.0 assert 0.0 <= positionB <= 254.0 assert 0.0 <= positionC <= 254.0 assert 0.0 <= force <= 100.0 assert 0.0 <= velocity <= 100.0 assert 0 <= int(mode) <= 4 if not scissor is None: assert 0.0 <= scissor <= 254.0 msg = lcmrobotiq.command_t() msg.utime = getUtime() msg.activate = 1 msg.emergency_release = 0 msg.do_move = 1 msg.ifc = 1 msg.positionA = int(positionA) msg.positionB = int(positionB) msg.positionC = int(positionC) msg.force = int(254 * (force / 100.0)) msg.velocity = int(254 * (velocity / 100.0)) msg.mode = int(mode) if not scissor is None: msg.isc = 1 msg.positionS = int(scissor) channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper() lcmUtils.publish(channel, msg)
def onStartNewMapButton(self): ''' Send a trigger to the Create Octomap process to start a new map ''' msg = lcmdrc.utime_t() msg.utime = getUtime() lcmUtils.publish('STATE_EST_START_NEW_MAP', msg)
def sendDesiredPumpPsi(self, desiredPsi): msg = lcmdrc.atlas_pump_command_t() msg.utime = getUtime() msg.k_psi_p = 0.0 # Gain on pressure error (A/psi) msg.k_psi_i = 0.0 # Gain on the integral of the pressure error (A/(psi/s) msg.k_psi_d = 0.0 # Gain on the derivative of the pressure error (A/(psi s) msg.k_rpm_p = 0.0 # Gain on rpm error (A / rpm) msg.k_rpm_i = 0.0 # Gain on the integral of the rpm error (A / (rpm s)) msg.k_rpm_d = 0.0 # Gain on the derivative of the rpm error (A / (rpm/s) msg.ff_rpm_d = 0.0 # Feed-forward gain on the desired rpm (A / rpm) msg.ff_psi_d = 0.0 # Feed-forward gain on the desired pressure (A / psi) msg.ff_const = 0.0 # Constant current term (Amps) msg.psi_i_max = 0.0 # Max. abs. value to which the integral psi error is clamped (psi s) msg.rpm_i_max = 0.0 # Max. abs. value to which the integral rpm error is clamped (rpm s) # Max. command output (A). Default is 60 Amps. # This value may need to be lower than the default in order to avoid # causing the motor driver to fault. msg.cmd_max = 60 msg.desired_psi = desiredPsi # default should be 1500 msg.desired_rpm = 5000 # default should be 5000 lcmUtils.publish('ATLAS_PUMP_COMMAND', msg)
def sendElectricArmEnabledState(self, enabledState): msg = lcmdrc.atlas_electric_arm_enable_t() msg.utime = getUtime() msg.num_electric_arm_joints = 6 assert len(enabledState) == msg.num_electric_arm_joints msg.enable = enabledState lcmUtils.publish('ATLAS_ELECTRIC_ARM_ENABLE', msg)
def onFootstepPlanContinuous(self, msg): if msg.num_steps <= 2: return if self.navigationPanel.automaticContinuousWalkingEnabled: print "Committing Footstep Plan for AUTOMATIC EXECUTION" lcmUtils.publish('COMMITTED_FOOTSTEP_PLAN', msg)
def applyAllButtonClicked(self, event): msg = data_request_list_t() msg.utime = getUtime() msg.requests = [] for name in self.groups: msg.requests.append(self.groups[name].getMessage()) msg.num_requests = len(msg.requests) lcmUtils.publish('DATA_REQUEST', msg)
def applyAllButtonClicked(self,event): msg = data_request_list_t() msg.utime = getUtime() msg.requests = [] for name in self.groups: msg.requests.append(self.groups[name].getMessage()) msg.num_requests = len(msg.requests) lcmUtils.publish('DATA_REQUEST',msg)
def immediateRequestClicked(self, event): req = self.getMessage() req.period = 0 msg = data_request_list_t() msg.utime = getUtime() msg.requests = [req] msg.num_requests = len(msg.requests) lcmUtils.publish('DATA_REQUEST', msg)
def immediateRequestClicked(self,event): req = self.getMessage() req.period = 0 msg = data_request_list_t() msg.utime = getUtime() msg.requests = [req] msg.num_requests = len(msg.requests) lcmUtils.publish('DATA_REQUEST',msg)
def sendClose(self, percentage=100.0): assert 0.0 <= percentage <= 100.0 msg = lcmirobot.position_control_close_t() msg.utime = getUtime() msg.valid = [True, True, True, False] msg.close_fraction = percentage / 100.0 channel = 'IROBOT_%s_POSITION_CONTROL_CLOSE' % self.side.upper() lcmUtils.publish(channel, msg)
def loadSDFFileAndRunSim(self): from ddapp import sceneloader filename= os.environ['DRC_BASE'] + '/../drc-testing-data/terrain/terrain_simple.sdf' sc=sceneloader.SceneLoader() sc.loadSDF(filename) import ipab msg=ipab.scs_api_command_t() msg.command="loadSDF "+filename+"\nsimulate" lcmUtils.publish('SCS_API_CONTROL', msg)
def apply3DFit(self): if om.findObjectByName('drill') is None: self.log('No 3D fit of drill. Click Spawn Drill button to provide a fit.') msg = lcmdrc.pfgrasp_command_t() msg.command = lcmdrc.pfgrasp_command_t.RUN_ONE_ITER_W_3D_PRIOR affordanceReach = om.findObjectByName('grasp frame') affordanceReach.actor.GetUserTransform().GetPosition(msg.pos) lcmUtils.publish('PFGRASP_CMD', msg)
def updateDescription(self, desc, publish=True, notify=True): self.collection[self.getDescriptionId(desc)] = desc self._modified() if publish: msg = self._newCommandMessage('update', description=desc) lcmUtils.publish(self.channel, msg) if notify: self.callbacks.process(self.DESCRIPTION_UPDATED_SIGNAL, self, self.getDescriptionId(desc))
def _updateAndSendCommandMessage(self): self._currentCommandedPose = self.clipPoseToJointLimits(self._currentCommandedPose) if self._baseFlag: msg = robotstate.drakePoseToRobotState(self._currentCommandedPose) else: msg = drakePoseToAtlasCommand(self._currentCommandedPose) if self.useControllerFlag: msg = drakePoseToQPInput(self._currentCommandedPose) lcmUtils.publish(self.publishChannel, msg)
def sendDataRequest(requestType, repeatTime=0.0): msg = lcmdrc.data_request_t() msg.type = requestType msg.period = int(repeatTime*10) # period is specified in tenths of a second msgList = lcmdrc.data_request_list_t() msgList.utime = getUtime() msgList.requests = [msg] msgList.num_requests = len(msgList.requests) lcmUtils.publish('DATA_REQUEST', msgList)
def publishCorrection(self, channel='POSE_YAW_LOCK'): pelvisToWorld = self.getPelvisEstimate() position, quat = transformUtils.poseFromTransform(pelvisToWorld) msg = lcmbot.pose_t() msg.utime = robotStateJointController.lastRobotStateMessage.utime msg.pos = [0.0, 0.0, 0.0] msg.orientation = quat.tolist() lcmUtils.publish(channel, msg)
def requestIRISRegion(self, tform, uid, bounding_box_width=2): start = np.asarray(tform.GetPosition()) A_bounds, b_bounds = self.getXYBounds(start, bounding_box_width) msg = lcmdrc.iris_region_request_t() msg.utime = getUtime() msg.initial_state = robotstate.drakePoseToRobotState(self.jointController.q) msg.map_mode = self.map_mode_map[self.params.properties.map_mode] msg.num_seed_poses = 1 msg.seed_poses = [transformUtils.positionMessageFromFrame(tform)] msg.region_id = [uid] msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)] lcmUtils.publish(self.request_channel, msg)
def sendDrop(self): msg = lcmrobotiq.command_t() msg.utime = getUtime() msg.activate = 1 msg.emergency_release = 1 msg.do_move = 0 msg.mode = 0 msg.position = 0 msg.force = 0 msg.velocity = 0 channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper() lcmUtils.publish(channel, msg)
def sendClose(self, percentage=100.0): assert 0.0 <= percentage <= 100.0 msg = lcmrobotiq.command_t() msg.utime = getUtime() msg.activate = 1 msg.emergency_release = 0 msg.do_move = 1 msg.position = int(254 * (percentage / 100.0)) msg.force = 254 msg.velocity = 254 channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper() lcmUtils.publish(channel, msg)
def requestIRISRegion(self, tform, uid, bounding_box_width=2): start = np.asarray(tform.GetPosition()) A_bounds, b_bounds = self.getXYBounds(start, bounding_box_width) msg = lcmdrc.iris_region_request_t() msg.utime = getUtime() msg.initial_state = robotstate.drakePoseToRobotState( self.jointController.q) msg.map_mode = self.map_mode_map[self.params.properties.map_mode] msg.num_seed_poses = 1 msg.seed_poses = [transformUtils.positionMessageFromFrame(tform)] msg.region_id = [uid] msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)] lcmUtils.publish(self.request_channel, msg)
def sendClose(self, percentage=100.0): assert 0.0 <= percentage <= 100.0 msg = lcmrobotiq.command_t() msg.utime = getUtime() msg.activate = 1 msg.emergency_release = 0 msg.do_move = 1 msg.position = int(254 * (percentage/100.0)) msg.force = 254 msg.velocity = 254 channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper() lcmUtils.publish(channel, msg)
def onStartMappingButton(self): msg = map_command_t() msg.timestamp = getUtime() msg.command = 0 lcmUtils.publish('KINECT_MAP_COMMAND', msg) utime = self.queue.getCurrentImageTime('KINECT_RGB') self.cameraToLocalInit = vtk.vtkTransform() self.queue.getTransform('KINECT_RGB', 'local', utime, self.cameraToLocalInit) vis.updateFrame(self.cameraToLocalInit, 'initial cam' ) print "starting mapping", utime print self.cameraToLocalInit.GetPosition() print self.cameraToLocalInit.GetOrientation()
def publishConstraints(self, constraints, messageName='PLANNER_REQUEST'): poses = getPlanPoses(constraints, self.ikPlanner) #poseJsonStr = json.dumps(poses, indent=4) #constraintsJsonStr = encodeConstraints(constraints, indent=4) poseJsonStr = json.dumps(poses) constraintsJsonStr = encodeConstraints(constraints) msg = lcmdrc.planner_request_t() msg.utime = getUtime() msg.poses = poseJsonStr msg.constraints = constraintsJsonStr lcmUtils.publish(messageName, msg)
def publishConstraints(self,constraints,messageName='PLANNER_REQUEST'): poses = getPlanPoses(constraints, self.ikPlanner) #poseJsonStr = json.dumps(poses, indent=4) #constraintsJsonStr = encodeConstraints(constraints, indent=4) poseJsonStr = json.dumps(poses) constraintsJsonStr = encodeConstraints(constraints) msg = lcmdrc.planner_request_t() msg.utime = getUtime() msg.poses = poseJsonStr msg.constraints = constraintsJsonStr lcmUtils.publish(messageName, msg)
def removeDescription(self, descriptionId, publish=True, notify=True): try: del self.collection[descriptionId] self._modified() except KeyError: pass if publish: msg = self._newCommandMessage('remove', descriptionId=descriptionId,) lcmUtils.publish(self.channel, msg) if notify: self.callbacks.process(self.DESCRIPTION_REMOVED_SIGNAL, self, descriptionId)
def setMode(self, mode): assert 0 <= int(mode) <= 4 msg = lcmrobotiq.command_t() msg.utime = getUtime() msg.activate = 1 msg.emergency_release = 0 msg.do_move = 0 msg.position = 0 msg.force = 0 msg.velocity = 0 msg.mode = int(mode) channel = 'ROBOTIQ_%s_COMMAND' % self.side.upper() lcmUtils.publish(channel, msg)
def start(self, autoMode): # start pfgrasp c++ program startPfgraspHere = False # initialize pfgrasp particles msg = lcmdrc.pfgrasp_command_t() msg.command = lcmdrc.pfgrasp_command_t.START lcmUtils.publish('PFGRASP_CMD', msg) self.autoMode = autoMode if self.autoMode: self.runoneiter()
def sendFootstepPlanCheckRequest(self, request, waitForResponse=False, waitTimeout=5000): assert isinstance(request, lcmdrc.footstep_check_request_t) requestChannel = 'FOOTSTEP_CHECK_REQUEST' responseChannel = 'FOOTSTEP_PLAN_RESPONSE' if waitForResponse: if waitTimeout == 0: helper = lcmUtils.MessageResponseHelper(responseChannel, lcmdrc.footstep_plan_t) lcmUtils.publish(requestChannel, request) return helper return lcmUtils.MessageResponseHelper.publishAndWait(requestChannel, request, responseChannel, lcmdrc.footstep_plan_t, waitTimeout) else: lcmUtils.publish(requestChannel, request)