def talker(): torso_pub = rospy.Publisher('/torso_controller/command', Float64MultiArray, queue_size=10) left_leg_pub = rospy.Publisher('/left_leg_controller/command', Float64MultiArray, queue_size=10) right_leg_pub = rospy.Publisher('/right_leg_controller/command', Float64MultiArray, queue_size=10) rospy.init_node('publish_to_controller', anonymous=True) rate = rospy.Rate(1) msg = Float64MultiArray() # hipyaw, hiproll, hippitch, kneepitch, anklepitch, ankleroll msg.data = [0.0, 0.0, -0.63, 1.03, -0.44, 0.0] # msg.data = [0.0, 0.0, 0., 0.0, 0., 0.0] dim = MultiArrayDimension() dim.size = len(msg.data) dim.label = "command" dim.stride = len(msg.data) msg.layout.dim.append(dim) msg.layout.data_offset = 0 torso_msg = Float64MultiArray() torso_msg.data = [0.0, 0.2, 0.0] dim = MultiArrayDimension() dim.size = len(torso_msg.data) dim.label = "command" dim.stride = len(torso_msg.data) torso_msg.layout.dim.append(dim) torso_msg.layout.data_offset = 0 while not rospy.is_shutdown(): print msg left_leg_pub.publish(msg) right_leg_pub.publish(msg) torso_pub.publish(torso_msg) rate.sleep()
def __init__(self): # Define the dimensions of the message rightHandGoalDim = MultiArrayDimension() rightHandGoalDim.size = NUM_DOFS rightHandGoalDim.label = "rightHandGoal" rightHandGoalDim.stride = 1 # Define the goal messages self.rightHandGoalMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): self.rightHandGoalMsg.data.append(0) self.rightHandGoalMsg.layout.dim.append(rightHandGoalDim) self.rightHandGoalMsg.layout.data_offset = 0 leftHandGoalDim = MultiArrayDimension() leftHandGoalDim.size = NUM_DOFS leftHandGoalDim.label = "leftHandGoal" leftHandGoalDim.stride = 1 self.leftHandGoalMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): self.leftHandGoalMsg.data.append(0) self.leftHandGoalMsg.layout.dim.append(leftHandGoalDim) self.leftHandGoalMsg.layout.data_offset = 0
def __init__(self): # Define the dimensions of the message rightHandGoalDim = MultiArrayDimension() rightHandGoalDim.size = NUM_DOFS rightHandGoalDim.label = "rightHandGoal" rightHandGoalDim.stride = 1 # Define the goal messages self.rightHandGoalMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): self.rightHandGoalMsg.data.append(0) self.rightHandGoalMsg.layout.dim.append(rightHandGoalDim) self.rightHandGoalMsg.layout.data_offset = 0 leftHandGoalDim = MultiArrayDimension() leftHandGoalDim.size = NUM_DOFS leftHandGoalDim.label = "leftHandGoal" leftHandGoalDim.stride = 1 self.leftHandGoalMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): self.leftHandGoalMsg.data.append(0) self.leftHandGoalMsg.layout.dim.append(leftHandGoalDim) self.leftHandGoalMsg.layout.data_offset = 0
def default(self, ci='unused'): arrayList = [] arrayList.append(self.data['w_fl']) arrayList.append(self.data['w_fr']) arrayList.append(self.data['w_rl']) arrayList.append(self.data['w_rr']) dim_fl = MultiArrayDimension() dim_fl.size = sys.getsizeof(self.data['w_fl']) dim_fl.label = 'FrontLeftWheelSpeed' dim_fl.stride = sys.getsizeof(self.data['w_fl']) dim_fr = MultiArrayDimension() dim_fr.size = sys.getsizeof(self.data['w_fr']) dim_fr.label = 'FrontRightWheelSpeed' dim_fr.stride = sys.getsizeof(self.data['w_fr']) dim_rl = MultiArrayDimension() dim_rl.size = sys.getsizeof(self.data['w_rl']) dim_rl.label = 'RearLeftWheelSpeed' dim_rl.stride = sys.getsizeof(self.data['w_rl']) dim_rr = MultiArrayDimension() dim_rr.size = sys.getsizeof(self.data['w_rr']) dim_rr.label = 'RearRightWheelSpeed' dim_rr.stride = sys.getsizeof(self.data['w_rr']) wheelspeed = Float64MultiArray() wheelspeed.data = arrayList wheelspeed.layout.dim.append(dim_fl) wheelspeed.layout.dim.append(dim_fr) wheelspeed.layout.dim.append(dim_rl) wheelspeed.layout.dim.append(dim_rr) wheelspeed.layout.data_offset = 0 self.publish(wheelspeed)
def __request_callback(self, msg): ''' Callback if image is received, faces are detected and added to the global array of people A message is sent with the ids, locations and face widths in a multiarray. ''' cv_image = self.bridge.imgmsg_to_cv2(msg, 'rgb8') if SHOW_RECEIVED_IMAGE: cv2.imwrite('rec_img.png', cv_image) ret = self.fr.recognize_people(cv_image) simple_msg = Float32MultiArray() if ret: print('new face detected') locs, ids = ret for i, loc in zip(ids, locs): if i in self.recognized_ids: index = self.recognized_ids.index(i) self.recognized_locs[index] = loc self.recognized_times[index] = datetime.datetime.now() else: self.recognized_ids.append(i) self.recognized_locs.append(loc) self.recognized_times.append(datetime.datetime.now()) else: print('No faces detected') # Publish the computed info on the simple_people topic # as a multiarray. The structure is defined as [id, xpos, ypos, width] # in a repeated fashion for multiple faces. # The width might be used as a metric for how far the face is away dim = MultiArrayDimension() dim.label = 'id_and_pos' dim.size = 3 dim.stride = 0 dim2 = MultiArrayDimension() dim2.label = 'length_faces' dim2.size = len(self.recognized_ids) dim2.stride = 0 simple_msg.layout.dim = [dim, dim2] simple_msg.layout.data_offset = 0 data = [] # Add the recognized faces around the table to the message for i, loc in zip(self.recognized_ids, self.recognized_locs): width = self.__get_width(loc) x, y, valid = self.__calculate_people_positions(width) data = np.concatenate((data, [i, x, y, width]), axis=0) simple_msg.data = data print(simple_msg) # Publish the message self.recognition_pub.publish(simple_msg) # Check if person is seen in the last TIME_BEFORE_FORGOTTEN seconds and otherwise remove for index, time in enumerate(self.recognized_times): if abs((time - datetime.datetime.now() ).total_seconds()) > TIME_BEFORE_FORGOTTEN: del self.recognized_ids[index] del self.recognized_locs[index] del self.recognized_times[index]
def __init__(self): pub1 = rospy.Publisher('tactile', UBI0All) pub2 = rospy.Publisher('palm_extras', Float64MultiArray) pub3 = rospy.Publisher('tactile_mid_prox', MidProxDataAll) pub4 = rospy.Publisher('tactile_aux_spi', AuxSpiData) ubiall = UBI0All() ubiall.tactiles = [] ubi = UBI0() ubi.distal = [ 200, 300, 200, 300, 400, 500, 600, 700, 800, 900, 1000, 900 ] midproxall = MidProxDataAll() midproxall.sensors = [] midprox = MidProxData() midprox.middle = [1500, 2400, 0, 0] midprox.proximal = [100, 400, 800, 0] for iFinger in range(0, 5): ubiall.tactiles.append(ubi) midproxall.sensors.append(midprox) palmextras = Float64MultiArray() dim = MultiArrayDimension() dim.label = "accelerometer" dim.size = 3 palmextras.layout.dim.append(dim) dim = MultiArrayDimension() dim.label = "gyrometer" dim.size = 3 palmextras.layout.dim.append(dim) dim = MultiArrayDimension() dim.label = "analog_inputs" dim.size = 4 palmextras.layout.dim.append(dim) palmextras.data = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 250.0, 550.0, 850.0, 1150.0 ] auxspi = AuxSpiData() auxspi.sensors = [ 100, 400, 700, 1000, 1300, 1700, 2000, 2300, 100, 200, 300, 400, 500, 600, 700, 800 ] rospy.init_node('agni_tactile_test_sensors') r = rospy.Rate(100) # 100hz while not rospy.is_shutdown(): pub1.publish(ubiall) pub2.publish(palmextras) pub3.publish(midproxall) pub4.publish(auxspi) r.sleep()
def __init__(self): pub1 = rospy.Publisher('tactile', UBI0All) pub2 = rospy.Publisher('palm_extras', Float64MultiArray) pub3 = rospy.Publisher('tactile_mid_prox', MidProxDataAll) pub4 = rospy.Publisher('tactile_aux_spi', AuxSpiData) ubiall = UBI0All() ubiall.tactiles=[] ubi= UBI0() ubi.distal = [200, 300, 200, 300, 400, 500, 600, 700, 800, 900, 1000, 900] midproxall = MidProxDataAll() midproxall.sensors=[] midprox = MidProxData() midprox.middle = [1500, 2400, 0, 0] midprox.proximal = [100, 400, 800, 0] for iFinger in range(0,5): ubiall.tactiles.append(ubi) midproxall.sensors.append(midprox) palmextras = Float64MultiArray() dim = MultiArrayDimension() dim.label = "accelerometer" dim.size = 3 palmextras.layout.dim.append(dim) dim = MultiArrayDimension() dim.label = "gyrometer" dim.size = 3 palmextras.layout.dim.append(dim) dim = MultiArrayDimension() dim.label = "analog_inputs" dim.size = 4 palmextras.layout.dim.append(dim) palmextras.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 250.0, 550.0,850.0, 1150.0] auxspi = AuxSpiData() auxspi.sensors = [100, 400, 700, 1000, 1300, 1700, 2000, 2300, 100, 200, 300, 400, 500, 600, 700, 800] rospy.init_node('agni_tactile_test_sensors') r = rospy.Rate(100) # 100hz while not rospy.is_shutdown(): pub1.publish(ubiall) pub2.publish(palmextras) pub3.publish(midproxall) pub4.publish(auxspi) r.sleep()
def __init__(self): rospy.init_node('save_hitting_sound', anonymous=True) self.length = rospy.get_param('/mini_microphone/length') self.rate = rospy.get_param('/mini_microphone/rate') self.cutoff_rate = rospy.get_param('~cutoff_rate') self.wave_strength_thre = 2.0 self.visualize_data_length = min( int(self.length * self.cutoff_rate / self.rate), self.length / 2) self.time_to_listen = rospy.get_param('~time_to_listen') self.queue_size = int(self.time_to_listen * (self.rate / self.length)) self.wave_spec_queue = np.zeros( (self.queue_size, self.visualize_data_length)) # remove folding noise # publish self.hitting_sound_pub = rospy.Publisher( '/mini_microphone/hitting_sound', Float32MultiArray) self.sound_image_pub = rospy.Publisher('/mini_microphone/sound_image', Image) self.hit_sound_image_pub = rospy.Publisher( '/mini_microphone/hit_sound_image', Image) self.hitting_sound = Float32MultiArray() self.hitting_sound.layout = [] dim = MultiArrayDimension() dim.label = 'time' dim.size = self.queue_size dim.stride = self.queue_size * self.length self.hitting_sound.layout.append(dim) dim.label = 'wave_spec' dim.size = self.length dim.stride = self.length self.hitting_sound.layout.append(dim) self.bridge = CvBridge() self.count_from_last_hitting = 0 # subscribe rospy.Subscriber('/mini_microphone/sound_spec', Float32MultiArray, self.sound_spec_cb) # save data self.save_image = rospy.get_param('~save_image') self.save_spectrum = rospy.get_param('~save_spectrum') self.target_class = rospy.get_param('~target_class', 'unspecified_data') self.save_dir = osp.join(os.environ['HOME'], 'hitting_sound_data') self.spectrum_save_dir = osp.join(self.save_dir, 'spectrum', self.target_class) if self.save_spectrum and not os.path.exists(self.spectrum_save_dir): os.makedirs(self.spectrum_save_dir) self.image_save_dir = osp.join(self.save_dir, 'image', 'origin', self.target_class) if self.save_image and not os.path.exists(self.image_save_dir): os.makedirs(self.image_save_dir)
def callback(msg,prevMarkers): detectedMarkers = msg.markers now = rospy.get_time() #the current time in seconds for detectedMarker in detectedMarkers: measuredTime = detectedMarker.header.stamp.secs markerID = detectedMarker.id prevMarkers[markerID] = measuredTime detected_markers = [] for marker in prevMarkers.keys(): if abs(prevMarkers[marker]-now) > 5: #if the measurement has been stale for 5 seconds del prevMarkers[marker] else: detected_markers.append(marker) array1 = MultiArrayDimension() array1.label = 'numMarkers' array1.size = len(detected_markers) array1.size = len(detected_markers) layout = MultiArrayLayout() layout.dim = [array1,] layout.data_offset = 0 msg = Int16MultiArray() msg.layout = layout msg.data = detected_markers pub.publish(msg)
def numpyArray2MultiArray(na, ma): s = na.shape for i in np.arange(len(s)): d = MultiArrayDimension() d.size = s[i] ma.layout.dim.append(d) ma.data = np.reshape(na, -1)
def publish(self): ''' ''' PathMsg = PathDisplay() PathMsg.xGlbPath = self.dataPlanning['xGlbPath'] PathMsg.yGlbPath = self.dataPlanning['yGlbPath'] # different from the usage in std_msgs dim = MultiArrayDimension() dim.stride = int(self.dataPlanning['indOptimal']) dim.size = self.dataPlanning['pathCandidate'].shape[0] PathMsg.xLocPath.data = np.array(self.dataPlanning['pathCandidate'][:,0]).flatten() PathMsg.xLocPath.layout.dim = [dim] PathMsg.yLocPath.data = np.array(self.dataPlanning['pathCandidate'][:,1]).flatten() PathMsg.yLocPath.layout.dim = [dim] PathMsg.header.stamp = rospy.Time.now() basePathPub.publish(PathMsg) PlanMsg = PlanOP() PlanMsg.prePoint.x = self.prePoint[0] PlanMsg.prePoint.y = self.prePoint[1] PlanMsg.prePoint.z = 0. PlanMsg.xCtrPath = self.dataPlanning['xCtrPath'] PlanMsg.yCtrPath = self.dataPlanning['yCtrPath'] PlanMsg.distObst = self.dataPlanning['distObst'] PlanMsg.desireDirDrive = self.desireDirDrive PlanMsg.curDirDrive = self.curDirDrive PlanMsg.flagIsShifting = self.flagIsShifting PlanMsg.distYClose = self.distYClose PlanMsg.modeDrive = self.modeDrive PlanMsg.vbrMode = self.vbrMode PlanMsg.startedFlag = self.toCloudStartedFlag PlanMsg.endTaskFlag = self.toCloudEndTaskFlag PlanMsg.header.stamp = rospy.Time.now() planPub.publish(PlanMsg)
def __init__(self, rosTopic, minAmplitude, maxAmplitude, numDoFs, period): """ The constructor. Keyword arguments: rosTopic -- The ROS topic on which to publish the goal. minAmplitude -- The minimum amplitude in radians. maxAmplitude -- The maximum amplitude in radians. numDoFs -- The number of DoFs. period -- The period of the square wave in seconds. """ self.period = period self.rosTopic = rosTopic # Define the dimensions of the message dim = MultiArrayDimension() dim.size = numDoFs dim.label = "goalMsg" dim.stride = 1 # Define the goal message containing the upper bound of the square wave self.goalMsgHigh = Float64MultiArray() for ii in range(0, numDoFs): self.goalMsgHigh.data.append(maxAmplitude) self.goalMsgHigh.layout.dim.append(dim) self.goalMsgHigh.layout.data_offset = 0 # Define the goal message containing the lower bound of the square wave self.goalMsgLow = Float64MultiArray() for ii in range(0, numDoFs): self.goalMsgLow.data.append(minAmplitude) self.goalMsgLow.layout.dim.append(dim) self.goalMsgLow.layout.data_offset = 0
def publish(self): #pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=10) pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=1) rate = rospy.Rate(50) #10Hz a = [0.0, 0.0] dim = MultiArrayDimension() dim.size = len(a) dim.label = "REMOTEmsg" dim.stride = len(a) apub = Float64MultiArray() apub.data = a apub.layout.dim.append(dim) apub.layout.data_offset = 0 while not rospy.is_shutdown(): #UART_read = ser.readline() #UART_read = ser.Serial.readline() UART_read = self.dev.readline() steer_cmd = int(UART_read[0:4]) throt_cmd = int(UART_read[4:8]) a = [throt_cmd, steer_cmd] rospy.loginfo(a) apub.data = a pub.publish(apub) rate.sleep()
def publish(self): # compose the multiarray message jointVelocities = Float32MultiArray() myLayout = MultiArrayLayout() myMultiArrayDimension = MultiArrayDimension() myMultiArrayDimension.label = "joint_velocities" myMultiArrayDimension.size = 1 myMultiArrayDimension.stride = self.numOfWheels myLayout.dim = [myMultiArrayDimension] myLayout.data_offset = 0 jointVelocities.layout = myLayout if rospy.get_time() - self.lastTwistTime < self.timeout: self.right = 1.0 * self.linearVelocity + self.angularVelocity * self.baseWidth / 2 self.left = 1.0 * self.linearVelocity - self.angularVelocity * self.baseWidth / 2 rospy.loginfo("wheels velocities vl, vr [{0:.3f},{1:.3f}]".format(self.left, self.right)) if self.numOfWheels == 2: # first item is left and second is right jointVelocities.data = [self.left, self.right] elif self.numOfWheels == 4: jointVelocities.data = [self.left, self.right, self.left, self.right] else: if self.numOfWheels == 2: jointVelocities.data = [0.0, 0.0] # first item is left and second is right elif self.numOfWheels == 4: jointVelocities.data = [0.0, 0.0, 0.0, 0.0] self.joint_velocities_Pub.publish(jointVelocities)
def publish(self): #pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=10) pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=1) rate = rospy.Rate(50) #10Hz a = [0.0, 0.0] dim = MultiArrayDimension() dim.size = len(a) dim.label = "REMOTEmsg" dim.stride = len(a) apub = Float64MultiArray() apub.data = a apub.layout.dim.append(dim) apub.layout.data_offset = 0 while not rospy.is_shutdown(): #UART_read = ser.readline() #UART_read = ser.Serial.readline() UART_read = self.dev.readline() steer_cmd = int(UART_read[0:4]) throt_cmd = int(UART_read[4:8]) a = [throt_cmd, steer_cmd] rospy.loginfo(a) apub.data = a pub.publish(apub) rate.sleep()
def enableGravityCompMode(self): index = raw_input("Put robot into gravity compensation mode? Y/n\n") if index == "N" or index == "n": return True else: # Define the dimensions of the message dim = MultiArrayDimension() dim.size = NUM_DOFS dim.label = "goalMsg" dim.stride = 1 kpMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): kpMsg.data.append(0) # Kp gains for gravity compensation mode kpMsg.layout.dim.append(dim) kpMsg.layout.data_offset = 0 print "Setting posture task Kp gains to be zero..." self.postureTaskKpPublisher.publish(kpMsg) kdMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): kdMsg.data.append(5) # Kd gains for gravity compensation mode kdMsg.layout.dim.append(dim) kdMsg.layout.data_offset = 0 print "Setting posture task Kd gains to be zero..." self.postureTaskKdPublisher.publish(kdMsg) print "Done setting robot into gravity compensation mode." return not rospy.is_shutdown()
def callback(msg, prevMarkers): detectedMarkers = msg.markers # The current time in seconds now = rospy.get_time() for detectedMarker in detectedMarkers: measuredTime = detectedMarker.header.stamp.secs markerID = detectedMarker.id prevMarkers[markerID] = measuredTime detected_markers = list() for marker in prevMarkers.keys(): if abs(prevMarkers[marker] - now) > 5: del prevMarkers[marker] else: detected_markers.append(marker) array = MultiArrayDimension() array.label = 'numMarkers' array.size = len(detected_markers) array.size = len(detected_markers) layout = MultiArrayLayout() layout.dim = [ array, ] layout.data_offset = 0 msg = Int16MultiArray() msg.layout = layout msg.data = detected_markers pub.publish(msg)
def motor_joint_positions_publisher(): #Initiate node for controlling joint1 and joint2 positions. rospy.init_node('motor_positions_node', anonymous=True) #Define publishers for each joint position controller commands. pub = rospy.Publisher('bicopter_motor_controller/command', Float64MultiArray, queue_size=10) rate = rospy.Rate(100) #100 Hz #Defining msg as float64MultiArray msg = Float64MultiArray() msg.layout.data_offset = 0 myMultiArrayDimensions = MultiArrayDimension() myMultiArrayDimensions.label = '' myMultiArrayDimensions.size = 2 myMultiArrayDimensions.stride = 1 msg.layout.dim.append(myMultiArrayDimensions) #While loop to have joints follow a certain position, while rospy is not shutdown. i = 0 while not rospy.is_shutdown(): #Have each joint follow a sine movement of sin(i/100). msg.data = [0.00, 0.00] #Publish the same sine movement to each joint. pub.publish(msg) i = i + 1 #increment i rate.sleep() #sleep for rest of rospy.Rate(100)
def __init__(self, rosTopic, minAmplitude, maxAmplitude, numDoFs, period): """ The constructor. Keyword arguments: rosTopic -- The ROS topic on which to publish the goal. minAmplitude -- The minimum amplitude in radians. maxAmplitude -- The maximum amplitude in radians. numDoFs -- The number of DoFs. period -- The period of the square wave in seconds. """ self.period = period self.rosTopic = rosTopic # Define the dimensions of the message dim = MultiArrayDimension() dim.size = numDoFs dim.label = "goalMsg" dim.stride = 1 # Define the goal message containing the upper bound of the square wave self.goalMsgHigh = Float64MultiArray() for ii in range(0, numDoFs): self.goalMsgHigh.data.append(maxAmplitude) self.goalMsgHigh.layout.dim.append(dim) self.goalMsgHigh.layout.data_offset = 0 # Define the goal message containing the lower bound of the square wave self.goalMsgLow = Float64MultiArray() for ii in range(0, numDoFs): self.goalMsgLow.data.append(minAmplitude) self.goalMsgLow.layout.dim.append(dim) self.goalMsgLow.layout.data_offset = 0
def numpyArray2MultiArray(na, ma): s = na.shape for i in np.arange(len(s)): d = MultiArrayDimension() d.size = s[i] ma.layout.dim.append(d) ma.data = np.reshape(na, -1)
def publish_state(self, state, labels): message = Int32MultiArray() #message.header.frame_id = REFERENCE_FRAME #message.header.stamp = rospy.Time(0) dim1 = MultiArrayDimension() dim2 = MultiArrayDimension() dim1.label = labels[0] dim1.size = state.shape[0] dim1.stride = state.size message.layout.dim.append(dim1) dim2.label = labels[1] dim2.size = state.shape[1] dim2.stride = state.shape[1] message.layout.dim.append(dim2) for a in state.flat[:]: message.data.append(a) self._state_pub.publish(message) pass
def PublishtoPurePursuit(self, waypoints): pub_points = Float32MultiArray() wp_len = len(waypoints) dim0 = MultiArrayDimension() dim1 = MultiArrayDimension() dim0.label = "height" dim0.size = wp_len/3 dim1.label = "width" dim1.size = 3 dimensions = MultiArrayLayout() dimensions.dim = [dim0, dim1] pub_points.layout = dimensions pub_points.data = waypoints self.pursuit_pub.publish(pub_points)
def pubLabelMsg(self, known_labels): msg = Int8MultiArray() dim = MultiArrayDimension() dim.label = "length" dim.size = len(known_labels) dim.stride = len(known_labels) msg.layout.dim.append(dim) msg.layout.data_offset = 0 msg.data = known_labels self.labels_pub.publish(msg)
def sendCommand(self, ts_rostime): msg = Float64MultiArray() dim0 = MultiArrayDimension() dim0.label = 'ts_rostime, numOfsamples, dt' dim0.size = 3 layout_var = MultiArrayLayout() layout_var.dim = [dim0] msg.layout = layout_var msg.data = [ts_rostime, self.numOfSamples, self.dt] self.sampleParticalTrajectory_pub.publish(msg)
def make_hand_configuration(self, points): dim = MultiArrayDimension() dim.label = 'fingers' dim.size = 5 dim.stride = 5 msg = Float64MultiArray() msg.layout.dim = [dim] msg.layout.data_offset = 0 msg.data = points return msg
def gen_msg(self, val): fa = Float64MultiArray() mad = MultiArrayDimension() mad.label = 'commands' mad.size = 2 mad.stride = 2 fa.layout.dim = [mad] fa.layout.data_offset = 0 fa.data = [val, val] return fa
def pack_data(): msg = Float32MultiArray() msg.data = MySensors.get_meters() msg.layout.data_offset = 0 dim = MultiArrayDimension() dim.label = 'num_sensors' dim.size = 3 dim.stride = 1 msg.layout.dim = [dim] return msg
def callback(data): # tempolary hardcode the parameter x_lim = -10 y_lim = -10 x_width = 20 y_width = 20 resol = 0.1 map_meta = data.info N = int(x_width / resol) + 1 temp = np.ones((N, N)) * 50 dimh = MultiArrayDimension() dimw = dimh dimh.size = data.info.height dimh.label = 'height' dimh.stride = 48 dimw.size = data.info.width dimw.label = 'width' dimw.stride = 64 datanp = np.array(data.data, dtype=float) datanp = datanp datanp[datanp < 0] = 50 for i in range(data.info.height): for j in range(data.info.width): idx_x = round( N / 2 - map_meta.height / 2) #-data.info.origin.position.x/0.1) idx_y = round(N / 2 - -data.info.origin.position.x / 0.1) # temp[int(idx_x + i),int(idx_y + j)] = datanp[(i-1)*data.info.width+j] for i in range(80): for j in range(60): temp[int(N / 2 - 40 + i), int(N / 2 - 30 + j)] = 0.1 map = OccupancyGrid() map.data = temp.flatten() map.info.width = int(N) map.info.height = int(N) map.info.resolution = 0.1 map.info.origin = map_meta.origin # data.data pub = rospy.Publisher('/TheMap', Float64MultiArray, queue_size=10) pub_map = rospy.Publisher('/map_fixed', OccupancyGrid, queue_size=10) a = Float64MultiArray() a.layout.dim.append(dimh) a.layout.dim.append(dimw) a.layout.data_offset = 0 a.data = temp.flatten() / 100. pub_map.publish(map) pub.publish(a) br = tf.TransformBroadcaster() br.sendTransform((-map_meta.origin.position.x - 10, -map_meta.origin.position.y - 10, 0.0), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "map", "world")
def matrixListToMultiarray(matrix): rows, columns = matrix.shape msg = Float64MultiArray() # Set up layout msg.layout.data_offset = 0 row_dim = MultiArrayDimension() row_dim.label = "rows" row_dim.size = rows row_dim.stride = columns * rows col_dim = MultiArrayDimension() col_dim.label = "cols" col_dim.size = columns col_dim.stride = columns msg.layout.dim = [row_dim, col_dim] msg.data = matrix.flatten().tolist() return msg
def cb_score(event): global solveResult ret=Bool();ret.data=True;pub_Y2.publish(ret) cb_master(event) score=Float32MultiArray() for sc in solveResult: d=MultiArrayDimension() d.label=sc d.size=len(solveResult[sc]) d.stride=1 print "score",d.label
def create_array_message(): array_dim_0 = MultiArrayDimension() array_dim_0.label = 'dim_0' array_dim_0.size = 4 array_dim_0.stride = 0 array_dim_1 = MultiArrayDimension() array_dim_1.label = 'dim_1' array_dim_1.size = len_faces array_dim_1.stride = 0 array_layout = MultiArrayDimension() array_layout.label = "face_array" array_message = Float32MultiArray() array_message.layout.dim = [array_dim_0, array_dim_1] array_message.layout.data_offset = 0 face_0 = [0.0, 1.0, 1.0, 200.0] face_1 = [1.0, 0.0, 1.0, 100.0] face_2 = [2.0, 1.0, 0.0, 300.0] face_3 = [3.0, 0.0, 0.0, 400.0] array_message.data = face_0 + face_1 + face_2 + face_3 return array_message
def create_depth_msg(depth_img): command = Float64MultiArray() layout = MultiArrayLayout() dimension = MultiArrayDimension() dimension.label = "depth_img" dimension.size = len(depth_img) dimension.stride = len(depth_img) layout.data_offset = 0 layout.dim = [dimension] command.layout = layout command.data = depth_img return command
def sa_right_set_callback(self): sa_right_cam = Float64MultiArray() sa_right_cam_dim_h = MultiArrayDimension() sa_right_cam_dim_h.label = "height" sa_right_cam_dim_h.size = 1 sa_right_cam_dim_h.stride = 2 sa_right_cam.layout.dim.append( sa_right_cam_dim_h ) sa_right_cam_dim_w = MultiArrayDimension() sa_right_cam_dim_w.label = "width" sa_right_cam_dim_w.size = 2 sa_right_cam_dim_w.stride = 2 sa_right_cam.layout.dim.append(sa_right_cam_dim_w) sa_right_cam.data.append( self.sa_right_gain.value() ) sa_right_cam.data.append( self.sa_right_exp.value() ) self.sa_right_cam_pub.publish( sa_right_cam )
def adjustLeftHand(position): Lmsg = Float64MultiArray() dim = MultiArrayDimension() dim.label = 'fingers' dim.size = 5 dim.stride = 5 Lmsg.layout.dim = [dim] Lmsg.layout.data_offset = 0 Lmsg.data = position left_hand_publisher.publish(Lmsg)
def adjustRightHand(position): Rmsg = Float64MultiArray() dim = MultiArrayDimension() dim.label = 'fingers' dim.size = 5 dim.stride = 5 Rmsg.layout.dim = [dim] Rmsg.layout.data_offset = 0 Rmsg.data = position right_hand_publisher.publish(Rmsg)
def publish(vals): command = Float64MultiArray() layout = MultiArrayLayout() dimension = MultiArrayDimension() dimension.label = "keyboard_control" dimension.size = 4 dimension.stride = 4 layout.data_offset = 0 layout.dim = [dimension] command.layout = layout command.data = vals control_pub.publish(command)
def main(): thread1 = myThread() thread1.start() rospy.init_node('reconstruction_node', anonymous=True) # node_name: reconstruction_node pub = rospy.Publisher('topic_name', Float32MultiArray, queue_size=10) # topic_name: topic_name instance_multi_array = Float32MultiArray() instance_multi_array.layout.data_offset = 0 temp_a = MultiArrayDimension() temp_b = MultiArrayDimension() temp_a.label = 'LED' temp_a.size = np.size(thread1.reconstruction_data_array, 0) temp_a.stride = np.size(thread1.reconstruction_data_array, 1) * np.size( thread1.reconstruction_data_array, 0) temp_b.label = 'Sensor' temp_b.size = np.size(thread1.reconstruction_data_array, 1) temp_b.stride = 1 * np.size(thread1.reconstruction_data_array, 1) instance_multi_array.layout.dim.append(temp_a) instance_multi_array.layout.dim.append(temp_b) temp_c = np.reshape(thread1.reconstruction_data_array, (1, thread1.reconstruction_data_array.size), 'C') instance_multi_array.data = temp_c[0] rate = rospy.Rate(1) rospy.loginfo("Node_Publishes") while not rospy.is_shutdown(): # rospy.loginfo(instance_multi_array) pub.publish(instance_multi_array) rate.sleep()
def publisher(recieved): pub = rospy.Publisher('joint_goals', Float64MultiArray, queue_size=10) rospy.loginfo('Publishing the goal') rospy.loginfo(recieved) published_msg = Float64MultiArray() multi_dim = MultiArrayDimension() multi_dim.label = 'foo' multi_dim.size = 1 multi_dim.stride = 1 published_msg.layout.dim = [multi_dim] published_msg.layout.data_offset = 1 published_msg.data = recieved.position pub.publish(published_msg)
def publish(self): #pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=10) pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=1) rate = rospy.Rate(20) #10Hz a = [0.0, 0.0] dim = MultiArrayDimension() dim.size = len(a) dim.label = "REMOTEmsg" dim.stride = len(a) apub = Float64MultiArray() apub.data = a apub.layout.dim.append(dim) apub.layout.data_offset = 0 cnt = 0 while not rospy.is_shutdown(): begin = time.time() #UART_read = ser.readline() #UART_read = ser.Serial.readline() #self.dev.flushInput() while self.dev.inWaiting() < 40: pass UART_read = self.dev.readline() self.dev.flushInput() #if self.dev.inWaiting() > 80: # self.dev.flushInput() #if self.dev.inWaiting() > 50: # self.dev.flushInput() # cnt=0 #self.dev.flushInput() #rospy.loginfo(UART_read) steer_cmd = int(UART_read[0:4]) throt_cmd = int(UART_read[4:8]) #self.dev.flushInput() self.dev1.pulsewidth_us(throt_cmd) self.dev2.pulsewidth_us(steer_cmd) a = [throt_cmd, steer_cmd] rospy.loginfo(a) apub.data = a pub.publish(apub) buffer = self.dev.inWaiting() #rospy.loginfo(buffer) #cnt=cnt+1 #self.dev.flushInput() #length = time.time() - begin #rospy.loginfo(length) rate.sleep()
def makeMultiArray(iterable, label): arrayList = [] for el in iterable: arrayList.append(el) dim = MultiArrayDimension() dim.size = len(arrayList) dim.label = label dim.stride = len(arrayList) tempArray = Float64MultiArray() tempArray.data = arrayList tempArray.layout.dim.append(dim) tempArray.layout.data_offset = 0 return tempArray
def publish(self): #pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=10) pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=1) rate = rospy.Rate(20) #10Hz a = [0.0, 0.0] dim = MultiArrayDimension() dim.size = len(a) dim.label = "REMOTEmsg" dim.stride = len(a) apub = Float64MultiArray() apub.data = a apub.layout.dim.append(dim) apub.layout.data_offset = 0 cnt=0 while not rospy.is_shutdown(): begin = time.time() #UART_read = ser.readline() #UART_read = ser.Serial.readline() #self.dev.flushInput() while self.dev.inWaiting() < 40: pass UART_read = self.dev.readline() self.dev.flushInput() #if self.dev.inWaiting() > 80: # self.dev.flushInput() #if self.dev.inWaiting() > 50: # self.dev.flushInput() # cnt=0 #self.dev.flushInput() #rospy.loginfo(UART_read) steer_cmd = int(UART_read[0:4]) throt_cmd = int(UART_read[4:8]) #self.dev.flushInput() self.dev1.pulsewidth_us(throt_cmd) self.dev2.pulsewidth_us(steer_cmd) a = [throt_cmd, steer_cmd] rospy.loginfo(a) apub.data = a pub.publish(apub) buffer = self.dev.inWaiting() #rospy.loginfo(buffer) #cnt=cnt+1 #self.dev.flushInput() #length = time.time() - begin #rospy.loginfo(length) rate.sleep()
def ms_set_callback(self): ms_cam = Float64MultiArray() ms_cam_dim_h = MultiArrayDimension() ms_cam_dim_h.label = "height" ms_cam_dim_h.size = 1 ms_cam_dim_h.stride = 2 ms_cam.layout.dim.append( ms_cam_dim_h ) ms_cam_dim_w = MultiArrayDimension() ms_cam_dim_w.label = "width" ms_cam_dim_w.size = 2 ms_cam_dim_w.stride = 2 ms_cam.layout.dim.append(ms_cam_dim_w) ms_cam.data.append( self.ms_gain.value() ) if self.ms_exp_auto.isChecked() : ms_cam.data.append( 1 ) else: ms_cam.data.append( self.ms_exp.value() ) self.ms_cam_pub.publish( ms_cam ) light = Float64() light.data = self.ms_light.value()/100.0 self.ms_light_pub.publish( light ) spindle_speed = Float64() spindle_speed.data = self.ms_spindle.value() self.ms_spindle_pub.publish( spindle_speed )
def __init__(self): """ The constructor. """ # Define the joint info self.jointSpecs = [] self.jointSpecs.append(NeckJointDetails("lower_neck_pitch", 0, -40, 15)) self.jointSpecs.append(NeckJointDetails("upper_neck_yaw", 1, -80, 80)) self.jointSpecs.append(NeckJointDetails("upper_neck_roll", 2, -14, 14)) self.jointSpecs.append(NeckJointDetails("upper_neck_pitch", 3, -7, 37)) self.jointSpecs.append(NeckJointDetails("eye_pitch", 4, -34, 34)) self.jointSpecs.append(NeckJointDetails("right_eye_yaw", 5, -34, 34)) self.jointSpecs.append(NeckJointDetails("left_eye_yaw", 6, -34, 34)) # Define the goal message # Create the goal message. Store the current joint states into the messages dimPos = MultiArrayDimension() dimPos.size = len(self.jointSpecs) dimPos.label = "goalPosMsg" dimPos.stride = 1 self.goalPosMsg = Float64MultiArray() for ii in range(0, len(self.jointSpecs)): self.goalPosMsg.data.append(0) self.goalPosMsg.layout.dim.append(dimPos) self.goalPosMsg.layout.data_offset = 0 # dimVel = MultiArrayDimension() # dimVel.size = len(self.jointSpecs) # dimVel.label = "goalVelMsg" # dimVel.stride = 1 # self.goalVelMsg = Float64MultiArray() # for ii in range(0, len(self.jointSpecs)): # self.goalVelMsg.data.append(0) # self.goalVelMsg.layout.dim.append(dimVel) # self.goalVelMsg.layout.data_offset = 0 self.cmdPublisher = rospy.Publisher("/dreamer_controller/controlit/head/position_cmd", Float64MultiArray, queue_size=1)
def __init__(self): # Define the dimensions of the message dim = MultiArrayDimension() dim.size = NUM_DOFS dim.label = "rightHandOrientationGoal" dim.stride = 1 # Define the goal messages self.rightHandGoalMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): self.rightHandGoalMsg.data.append(0) self.rightHandGoalMsg.layout.dim.append(dim) self.rightHandGoalMsg.layout.data_offset = 0 self.rightHandGoalMsg.data[2] = 1 # initial goal is [0, 0, 1] in world frame self.leftHandGoalMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): self.leftHandGoalMsg.data.append(0) self.leftHandGoalMsg.layout.dim.append(dim) self.leftHandGoalMsg.layout.data_offset = 0 self.leftHandGoalMsg.data[2] = 1 # initial goal is [0, 0, 1] in world frame
def __init__(self, rosTopic, amplitude, offset, initGoal, numDoFs, jointIndex, period, updateFreq): """ The constructor. Keyword arguments: rosTopic -- The ROS topic on which to publish the goal. initGoal -- The initial goal (the value the sine wave starts at) amplitude -- The amplitude. offset -- The offset. numDoFs -- The number of DoFs. jointIndex -- The index of the joint being controlled. period -- The period of the sine wave in seconds. updateFreq -- The frequency at which the reference position should be sent """ self.period = period self.frequency = 1.0 / period self.rosTopic = rosTopic self.amplitude = amplitude self.offset = offset self.numDoFs = numDoFs self.jointIndex = jointIndex self.updateFreq = updateFreq self.updatePeriod = 1.0 / updateFreq # Define the dimensions of the message dim = MultiArrayDimension() dim.size = numDoFs dim.label = "goalMsg" dim.stride = 1 # Define the goal message self.goalMsg = Float64MultiArray() for ii in range(0, numDoFs): self.goalMsg.data.append(initGoal) self.goalMsg.layout.dim.append(dim) self.goalMsg.layout.data_offset = 0
def __init__(self): # Define the goal messages rightHandCartesianGoalMsgDim = MultiArrayDimension() rightHandCartesianGoalMsgDim.size = NUM_CARTESIAN_DOFS rightHandCartesianGoalMsgDim.label = "rightHandCartesianGoal" rightHandCartesianGoalMsgDim.stride = 1 self.rightHandCartesianGoalMsg = Float64MultiArray() for ii in range(0, NUM_CARTESIAN_DOFS): self.rightHandCartesianGoalMsg.data.append(0) self.rightHandCartesianGoalMsg.layout.dim.append(rightHandCartesianGoalMsgDim) self.rightHandCartesianGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' rightHandOrientationGoalMsgDim = MultiArrayDimension() rightHandOrientationGoalMsgDim.size = NUM_ORIENTATION_DOFS rightHandOrientationGoalMsgDim.label = "rightHandOrientationGoal" rightHandOrientationGoalMsgDim.stride = 1 self.rightHandOrientationGoalMsg = Float64MultiArray() for ii in range(0, NUM_ORIENTATION_DOFS): self.rightHandOrientationGoalMsg.data.append(0) self.rightHandOrientationGoalMsg.layout.dim.append(rightHandOrientationGoalMsgDim) self.rightHandOrientationGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' postureGoalMsgDim = MultiArrayDimension() postureGoalMsgDim.size = NUM_ROBOT_DOFS postureGoalMsgDim.label = "postureGoal" postureGoalMsgDim.stride = 1 self.postureGoalMsg = Float64MultiArray() for ii in range(0, NUM_ROBOT_DOFS): self.postureGoalMsg.data.append(0) self.postureGoalMsg.layout.dim.append(postureGoalMsgDim) self.postureGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' self.rightHandCmdMsg = Bool() self.rightHandCmdMsg.data = False # relax hand self.rightIndexFingerCmdMsg = Bool() self.rightIndexFingerCmdMsg.data = True # include index finger in power grasp self.rightMiddleFingerCmdMsg = Bool() self.rightMiddleFingerCmdMsg.data = True # include middle finger in power grasp self.enableMsg = Int32() self.enableMsg.data = 1 #-----------------------------------------------------------------------------' # Initialize member variables self.currentPosture = None self.postureError = None self.currentRightCartesianPos = None self.rightCartesianPosError = None self.currentRightOrientation = None self.rightOrientationError = None # Create the ROS topic subscriptions self.postureTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/JPosTask/actualPosition", Float64MultiArray, self.postureTaskActualCallback) self.postureTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/JPosTask/error", Float64MultiArray, self.postureTaskErrorCallback) self.rightCartesianTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandPosition/actualWorldPosition", Float64MultiArray, self.rightCartesianTaskActualCallback) self.rightCartesianTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/RightHandPosition/error", Float64MultiArray, self.rightCartesianTaskErrorCallback) self.rightOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/actualHeading", Float64MultiArray, self.rightOrientationTaskActualCallback) self.rightOrientationTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/errorAngle", Float64, self.rightOrientationTaskErrorCallback) # Create the ROS topic publishers self.postureTaskGoalPublisher = rospy.Publisher("/dreamer_controller/JPosTask/goalPosition", Float64MultiArray, queue_size=1) self.rightCartesianTaskGoalPublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/goalPosition", Float64MultiArray, queue_size=1) self.rightCartesianTaskEnablePublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/enabled", Int32, queue_size=1) self.rightOrientationTaskGoalPublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/goalVector", Float64MultiArray, queue_size=1) self.rightOrientationTaskEnablePublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/enabled", Int32, queue_size=1) self.rightHandCmdPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/powerGrasp", Bool, queue_size=1) self.selectIndexFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightIndexFinger", Bool, queue_size=1) self.selectMiddleFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightMiddleFinger", Bool, queue_size=1) self.selectPinkyFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightPinkyFinger", Bool, queue_size=1)
def __init__(self, enableUserPrompts = False, useQuaternionControl = False): ''' The constructor. Keyword arguments: - enableUserPrompts: Whether to enable user prompts (default is False) - userQuaternionControl: Whether to use quaternions for orientation control (default is False) ''' self.enableUserPrompts = enableUserPrompts self.useQuaternionControl = useQuaternionControl if self.useQuaternionControl: NUM_ORIENTATION_DOFS = 4 # Orientation is defined using a w, x, y, z vector else: NUM_ORIENTATION_DOFS = 3 # Orientation is defined using a x, y, z vector # Define the goal messages rightHandCartesianGoalMsgDim = MultiArrayDimension() rightHandCartesianGoalMsgDim.size = NUM_CARTESIAN_DOFS rightHandCartesianGoalMsgDim.label = "rightHandCartesianGoal" rightHandCartesianGoalMsgDim.stride = 1 self.rightHandCartesianGoalMsg = Float64MultiArray() for ii in range(0, NUM_CARTESIAN_DOFS): self.rightHandCartesianGoalMsg.data.append(0) self.rightHandCartesianGoalMsg.layout.dim.append(rightHandCartesianGoalMsgDim) self.rightHandCartesianGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' leftHandCartesianGoalMsgDim = MultiArrayDimension() leftHandCartesianGoalMsgDim.size = NUM_CARTESIAN_DOFS leftHandCartesianGoalMsgDim.label = "leftHandCartesianGoal" leftHandCartesianGoalMsgDim.stride = 1 self.leftHandCartesianGoalMsg = Float64MultiArray() for ii in range(0, NUM_CARTESIAN_DOFS): self.leftHandCartesianGoalMsg.data.append(0) self.leftHandCartesianGoalMsg.layout.dim.append(leftHandCartesianGoalMsgDim) self.leftHandCartesianGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' if not self.useQuaternionControl: rightHandOrientationGoalMsgDim = MultiArrayDimension() rightHandOrientationGoalMsgDim.size = NUM_ORIENTATION_DOFS rightHandOrientationGoalMsgDim.label = "rightHandOrientationGoal" rightHandOrientationGoalMsgDim.stride = 1 self.rightHandOrientationGoalMsg = Float64MultiArray() for ii in range(0, NUM_ORIENTATION_DOFS): self.rightHandOrientationGoalMsg.data.append(0) self.rightHandOrientationGoalMsg.layout.dim.append(rightHandOrientationGoalMsgDim) self.rightHandOrientationGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' leftHandOrientationGoalMsgDim = MultiArrayDimension() leftHandOrientationGoalMsgDim.size = NUM_ORIENTATION_DOFS leftHandOrientationGoalMsgDim.label = "leftHandOrientationGoal" leftHandOrientationGoalMsgDim.stride = 1 self.leftHandOrientationGoalMsg = Float64MultiArray() for ii in range(0, NUM_ORIENTATION_DOFS): self.leftHandOrientationGoalMsg.data.append(0) self.leftHandOrientationGoalMsg.layout.dim.append(leftHandOrientationGoalMsgDim) self.leftHandOrientationGoalMsg.layout.data_offset = 0 else: self.orientationUpdateMsg = QuatInterpMsg() #-----------------------------------------------------------------------------' postureGoalMsgDim = MultiArrayDimension() postureGoalMsgDim.size = NUM_ROBOT_DOFS postureGoalMsgDim.label = "postureGoal" postureGoalMsgDim.stride = 1 self.postureGoalMsg = Float64MultiArray() for ii in range(0, NUM_ROBOT_DOFS): self.postureGoalMsg.data.append(0) self.postureGoalMsg.layout.dim.append(postureGoalMsgDim) self.postureGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' self.rightHandCmdMsg = Bool() self.rightHandCmdMsg.data = False # relax hand self.rightIndexFingerCmdMsg = Bool() self.rightIndexFingerCmdMsg.data = True # include index finger in power grasp self.rightMiddleFingerCmdMsg = Bool() self.rightMiddleFingerCmdMsg.data = True # include middle finger in power grasp self.rightPinkyFingerCmdMsg = Bool() self.rightPinkyFingerCmdMsg.data = True # include pinky finger in power grasp self.leftGripperCmdMsg = Bool() self.leftGripperCmdMsg.data = False # relax gripper self.tareMsg = Int32() self.tareMsg.data = 1 self.enableMsg = Int32() self.enableMsg.data = 1 #-----------------------------------------------------------------------------' # Initialize member variables self.currentPosture = None self.postureError = None self.currentRightCartesianPos = None self.rightCartesianPosError = None self.currentLeftCartesianPos = None self.leftCartesianPosError = None self.currentRightOrientation = None self.rightOrientationError = None self.currentLeftOrientation = None self.leftOrientationError = None # Create the ROS topic subscriptions self.postureTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/Posture/actualPosition", Float64MultiArray, self.postureTaskActualCallback) self.postureTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/Posture/error", Float64MultiArray, self.postureTaskErrorCallback) self.rightCartesianTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandPosition/actualWorldPosition", Float64MultiArray, self.rightCartesianTaskActualCallback) self.rightCartesianTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/RightHandPosition/error", Float64MultiArray, self.rightCartesianTaskErrorCallback) self.leftCartesianTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandPosition/actualWorldPosition", Float64MultiArray, self.leftCartesianTaskActualCallback) self.leftCartesianTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandPosition/error", Float64MultiArray, self.leftCartesianTaskErrorCallback) if self.useQuaternionControl: self.rightOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/actualWorldOrientation", Float64MultiArray, self.rightOrientationTaskActualCallback) else: self.rightOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/actualHeading", Float64MultiArray, self.rightOrientationTaskActualCallback) self.rightOrientationTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/errorAngle", Float64, self.rightOrientationTaskErrorCallback) if self.useQuaternionControl: self.leftOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandOrientation/actualWorldOrientation", Float64MultiArray, self.leftOrientationTaskActualCallback) else: self.leftOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandOrientation/actualHeading", Float64MultiArray, self.leftOrientationTaskActualCallback) self.leftOrientationTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandOrientation/errorAngle", Float64, self.leftOrientationTaskErrorCallback) # Create the ROS topic publishers self.postureTaskGoalPublisher = rospy.Publisher("/dreamer_controller/Posture/goalPosition", Float64MultiArray, queue_size=1) self.postureTaskTarePublisher = rospy.Publisher("/dreamer_controller/Posture/tare", Int32, queue_size=1) self.rightCartesianTaskGoalPublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/goalPosition", Float64MultiArray, queue_size=1) self.rightCartesianTaskEnablePublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/enableState", Int32, queue_size=1) self.rightCartesianTaskTarePublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/tare", Int32, queue_size=1) self.leftCartesianTaskGoalPublisher = rospy.Publisher("/dreamer_controller/LeftHandPosition/goalPosition", Float64MultiArray, queue_size=1) self.leftCartesianTaskEnablePublisher = rospy.Publisher("/dreamer_controller/LeftHandPosition/enableState", Int32, queue_size=1) self.leftCartesianTaskTarePublisher = rospy.Publisher("/dreamer_controller/LeftHandPosition/tare", Int32, queue_size=1) if not self.useQuaternionControl: self.rightOrientationTaskGoalPublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/goalVector", Float64MultiArray, queue_size=1) self.rightOrientationTaskEnablePublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/enableState", Int32, queue_size=1) self.rightOrientationTaskTarePublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/tare", Int32, queue_size=1) if not self.useQuaternionControl: self.leftOrientationTaskGoalPublisher = rospy.Publisher("/dreamer_controller/LeftHandOrientation/goalVector", Float64MultiArray, queue_size=1) self.leftOrientationTaskEnablePublisher = rospy.Publisher("/dreamer_controller/LeftHandOrientation/enableState", Int32, queue_size=1) self.leftOrientationTaskTarePublisher = rospy.Publisher("/dreamer_controller/LeftHandOrientation/tare", Int32, queue_size=1) if self.useQuaternionControl: self.orientationGoalPublisher = rospy.Publisher("/dreamer_controller/trajectory_generator/update_orientation", QuatInterpMsg, queue_size=1) self.rightHandCmdPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/powerGrasp", Bool, queue_size=1) self.selectIndexFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightIndexFinger", Bool, queue_size=1) self.selectMiddleFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightMiddleFinger", Bool, queue_size=1) self.selectPinkyFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightPinkyFinger", Bool, queue_size=1) self.leftGripperCmdPublisher = rospy.Publisher("/dreamer_controller/controlit/leftGripper/powerGrasp", Bool, queue_size=1)