Ejemplo n.º 1
0
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()
Ejemplo n.º 2
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
Ejemplo n.º 3
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
Ejemplo n.º 4
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]
Ejemplo n.º 6
0
    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()
Ejemplo n.º 7
0
    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()
Ejemplo n.º 8
0
 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)
Ejemplo n.º 12
0
    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
Ejemplo n.º 13
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()
Ejemplo n.º 14
0
    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)
Ejemplo n.º 15
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()
Ejemplo n.º 16
0
    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)
Ejemplo n.º 18
0
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)
Ejemplo n.º 19
0
    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
Ejemplo n.º 20
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)
Ejemplo n.º 21
0
 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
Ejemplo n.º 22
0
    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)
Ejemplo n.º 23
0
 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)
Ejemplo n.º 24
0
 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)
Ejemplo n.º 25
0
 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
Ejemplo n.º 27
0
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
Ejemplo n.º 28
0
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")
Ejemplo n.º 29
0
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
Ejemplo n.º 30
0
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
Ejemplo n.º 32
0
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
Ejemplo n.º 33
0
    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 )
Ejemplo n.º 34
0
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)
Ejemplo n.º 35
0
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)
Ejemplo n.º 36
0
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)
Ejemplo n.º 37
0
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()
Ejemplo n.º 38
0
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)
Ejemplo n.º 39
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(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
Ejemplo n.º 41
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(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()
Ejemplo n.º 42
0
    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 )
Ejemplo n.º 43
0
    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)
Ejemplo n.º 44
0
    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
Ejemplo n.º 45
0
    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)
Ejemplo n.º 47
0
    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)