Python JointState.position方法代码示例

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
def motor_pub():
        p1 = rospy.Publisher('/gBot/left_wheel/cmd', JointState)
	p2 = rospy.Publisher('/gBot/right_wheel/cmd', JointState)
       
        rospy.init_node('motorPub', anonymous=True)
 	rospy.on_shutdown(motor_stop)
        #Message for left_wheel
	msg1 = JointState()
        msg1.name = ["left_wheel"]
        msg1.position = [0.0]
        msg1.velocity = [-100]

        #Message for right_wheel
	msg2 = JointState()
        msg2.name = ["right_wheel"]
        msg2.position = [0.0]
        msg2.velocity = [-100]
	

        while not rospy.is_shutdown():

                msg1.header.stamp = rospy.Time.now()
                p1.publish(msg1)
		msg2.header.stamp = rospy.Time.now()
                p2.publish(msg2)
		rospy.loginfo('Left Motor velocity')
		rospy.loginfo(msg1.velocity)
		rospy.loginfo('Right Motor velocity')
		rospy.loginfo(msg2.velocity)
                rospy.sleep(0.1)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
def talker():
    joint1 = radians(90)
    pub = rospy.Publisher('/kukaAllegroHand/robot_cmd', JointState)
    rospy.init_node('talker', anonymous=True)
    r = rospy.Rate(10000) # 10hz
    msg = JointState()
    msg.position = [0,joint1,0,0,0,0,0]
    pub.publish(msg)
    r.sleep()
    time.sleep(.4)
    
    while True:
        for acc in range(600,601):
            print '************************* acceleratioin: %d *************************' % acc
            # acc = 200
            trjFile = '../trj/acc_new_' + str(acc) + '.dat'
            trj = np.genfromtxt(trjFile, delimiter="\t")
            done = True
            i = 1
            while not rospy.is_shutdown() and i < len(trj):
                print i
                msg = JointState()
                msg.position = [0,joint1,0,0,0,0,0]
                msg.position[0] = radians(trj[i][0])
                msg.position[2] = radians(trj[i][0])
                msg.position[3] = radians(trj[i][0])
                msg.position[4] = radians(trj[i][0])
                msg.position[5] = radians(trj[i][0])
                msg.position[6] = radians(trj[i][0])
                pub.publish(msg)
                r.sleep()
                time.sleep((trj[i][2] - trj[i-1][2])/10)
                i += 1

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
    def execute(self, userdata):
        # rospy.loginfo('Executing state %s', self.__class__.__name__)
        # self.active = True
        # self.first_call = True

        # # wait for some time to read from the topic
        # # TODO: replace with a service call
        # rospy.sleep(3)
        # userdata.obj_list = self.obj_list

        # self.active = False
        # return 'succeeded'
        rospy.loginfo("Executing state %s", self.__class__.__name__)

        self.obj_list = []

        i = 1
        for view in userdata.view_list:

            rospy.loginfo("%i view: set PTU to %s", i, view)

            joint_state = JointState()
            joint_state.header.frame_id = "tessdaf"
            joint_state.name = ["pan", "tilt"]
            joint_state.position = [float(view[0]), float(view[1])]
            joint_state.velocity = [float(1.0), float(1.0)]
            joint_state.effort = [float(1.0), float(1.0)]

            self.ptu_cmd.publish(joint_state)

            # wait until PTU has finished or point cloud get screwed up
            rospy.sleep(2)

            rospy.loginfo("%i view: receive from semantic camera", i)
            self.active = True
            self.first_call = True

            # wait for some time to read once from the topic and store it onto self.pointcloud
            # TODO: replace with a service call
            rospy.sleep(2)

            self.active = False

            i = i + 1

        userdata.obj_list = self.obj_list

        # back to original position
        joint_state = JointState()
        joint_state.header.frame_id = "tessdaf"
        joint_state.name = ["pan", "tilt"]
        joint_state.position = [float(0.0), float(0.0)]
        joint_state.velocity = [float(1.0), float(1.0)]
        joint_state.effort = [float(1.0), float(1.0)]

        self.ptu_cmd.publish(joint_state)

        return "succeeded"

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
def main(ra):
    # Create Pose Publisher and Subscriber
    joint_pub= rospy.Publisher('/dvrk/PSM1/set_torque_joint', JointState, queue_size=10)
    rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, callback_joint)
    rospy.init_node('Talkity_talk_talk',anonymous=True)
    rate = rospy.Rate(ra) # 1hz
    
    #hardcode home to zero 
    joint_msg= JointState()
    joint_msg.header = Header()
    joint_msg.header.stamp = rospy.Time.now()
    #joint_msg.name = ['outer_yaw','o', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
    joint_msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
    joint_msg.position=[]
    joint_msg.velocity = []  
    joint_msg.effort = []
    
  
    #Excitation Trajectory Position Coordinates
    i=0
    q_data1=[-0.2, -0.2, -0.2, -0.2]
    q_data2=[-0.086355,-0.095648,-0.10506,-0.1145,-0.12387,-0.13311,-0.14214,-0.15087,-0.15925,-0.16721,-0.17471,-0.18172,-0.18821,-0.19416,-0.19958,-0.20446,-0.20883,-0.21268,-0.21606,-0.21898,-0.22147,-0.22357,-0.2253,-0.22669,-0.22779,-0.22861,-0.22919,-0.22954,-0.2297,-0.22968,-0.2295,-0.22914,-0.22862,-0.22795,-0.22713,-0.22617,-0.22511,-0.22388,-0.22247,-0.22082,-0.21888,-0.21662,-0.214,-0.21095,-0.20744,-0.20342,-0.19885,-0.19368,-0.18788,-0.1814,-0.17424,-0.16635,-0.15775,-0.14843,-0.13843,-0.12778,-0.11651,-0.10471,-0.092438,-0.079795,-0.066886,-0.053804,-0.040655,-0.027542,-0.014584,-0.0018928,0.010422,0.022271,0.033597,0.044314,0.05438,0.063868,0.072648,0.080705,0.088029,0.09461,0.10049,0.10573,0.11037,0.11443,0.11795,0.12097,0.12353,0.12566,0.12736,0.12866,0.12956,0.13007,0.13023,0.13005,0.12955,0.12874,0.12764,0.12629,0.12472,0.12297,0.12106,0.11903,0.11691,0.11471,0.11247,0.11021,0.10793,0.10564,0.10335,0.10109,0.098863,0.096666,0.094485,0.092317,0.09015,0.087973,0.085786,0.083574,0.081318,0.079023,0.076691,0.074343,0.071992,0.069651,0.067341,0.065096,0.06294,0.060911,0.05905,0.057395,0.055962,0.05478,0.053876,0.053265,0.052956,0.052948,0.053206,0.053699,0.054398,0.055243,0.056172,0.057127,0.058038,0.05881,0.059392,0.059703,0.05959,0.059125,0.058272,0.057011,0.055343,0.053232,0.050668,0.047655,0.044237,0.040451,0.036343,0.031955,0.027341,0.022586,0.017764,0.012952,0.0082075,0.0035806 -0.00086291,-0.0050626,-0.0089787,-0.012584,-0.015859,-0.018781,-0.021356,-0.023586,-0.025488,-0.027074,-0.028361,-0.029398,-0.030214,-0.030849,-0.031337,-0.031706,-0.031991,-0.032234,-0.032468,-0.032704,-0.032954,-0.033233,-0.033555,-0.033912,-0.034301,-0.034697,-0.035081,-0.035422,-0.035697,-0.035867,-0.035877,-0.035676,-0.035207,-0.03442,-0.033253,-0.031662,-0.029576,-0.026944,-0.023725,-0.019887,-0.015407,-0.010266,-0.0044619,0.0020043,0.0091073,0.016811,0.025072,0.033839,0.04304,0.052649,0.062524,0.072569,0.082709,0.092857,0.10293,0.11281,0.12243,0.13173,0.14065,0.14913,0.15713,0.16459,0.1715,0.17786,0.18366,0.1889,0.19362,0.19783,0.20158,0.20492,0.2079,0.21056,0.21295,0.21512,0.21715,0.21904,0.22085,0.22259,0.2243,0.22599,0.22766,0.22932,0.23095,0.23255,0.23409,0.23556,0.23692,0.23812,0.23907,0.2398,0.24025,0.24034,0.24004,0.23926,0.23797,0.23611,0.23359,0.23033,0.22627,0.22136,0.21557,0.20886,0.20118,0.19255,0.18296,0.17243,0.16101,0.14864,0.13538,0.12151,0.10701,0.091903,0.07631,0.060357,0.044198,0.02793,0.011667,-0.0044587,-0.020288,-0.035661,-0.050484,-0.064586,-0.07786,-0.090235,-0.10163,-0.11199,-0.12125,-0.12941,-0.13646,-0.14241,-0.14728,-0.15105,-0.15373,-0.15539,-0.15605,-0.15578,-0.15459,-0.15257,-0.14976,-0.14623,-0.1421,-0.13743,-0.1323,-0.12678,-0.12094,-0.11485,-0.10862,-0.10231,-0.095985,-0.089689,-0.083481,-0.077412,-0.071513,-0.065797,-0.060263,-0.054899,-0.04968,-0.044575,-0.039542,-0.034456,-0.029335,-0.024135,-0.018785,-0.013241,-0.0074516,-0.0014066,0.0049231,0.011561,0.018514,0.025767,0.033291,0.041039,0.048948,0.056956,0.06499,0.072953,0.080753,0.088337,0.095636,0.10252,0.10875,0.11438,0.11943,0.12382,0.12749,0.13041,0.13264,0.1342,0.13509,0.13531,0.13486,0.13379,0.13214,0.12997,0.12732,0.12423,0.12075,0.11694,0.11286,0.10857,0.1041,0.099486,0.09475,0.08992,0.085004,0.080014,0.074935,0.069724,0.064326,0.058826,0.053265,0.047541,0.04157,0.035322,0.028833,0.02214,0.015199,0.0079713,0.00039706,-0.007471,-0.015506,-0.023615,-0.031787,-0.039995,-0.048156,-0.056162,-0.063921,-0.071375,-0.078458,-0.0851,-0.091259,-0.096872,-0.10188,-0.10625,-0.10998,-0.1131,-0.11564,-0.11763,-0.11913,-0.1202,-0.12096,-0.12153,-0.122,-0.12247,-0.12303,-0.12377,-0.12477,-0.12608,-0.1278,-0.1299,-0.13235,-0.13516,-0.13832,-0.14178,-0.14548,-0.14934,-0.15332,-0.15736,-0.16139,-0.16534,-0.16914,-0.17275,-0.17613,-0.1793,-0.18218,-0.18474,-0.18697,-0.18885,-0.19036,-0.19152,-0.19237,-0.19292,-0.19317,-0.19312,-0.19282,-0.19229,-0.19154,-0.19058,-0.18941,-0.18806,-0.18664,-0.18523,-0.18373,-0.18213,-0.18048,-0.17888,-0.17739,-0.17596,-0.17458,-0.17322,-0.17194,-0.17084,-0.16996,-0.16924,-0.16864,-0.16818,-0.16787,-0.16771,-0.16764,-0.16761,-0.1676,-0.16757,-0.1675,-0.16736,-0.16713,-0.16679,-0.1664,-0.16587,-0.16524,-0.16451,-0.1637,-0.16282,-0.16188,-0.16092,-0.15998,-0.15907,-0.15823,-0.15746,-0.15675,-0.1561,-0.15549,-0.15491,-0.15431,-0.15365,-0.15287,-0.15191,-0.15069,-0.14918,-0.14729,-0.14498,-0.14218,-0.13885,-0.13497,-0.13052,-0.12547,-0.11987,-0.11377,-0.10721,-0.10028,-0.093075,-0.085649]
    q_data3=[-0.079852,-0.082139,-0.083106,-0.082646,-0.080594,-0.076762,-0.071134,-0.063814,-0.054815,-0.044135,-0.031832,-0.01809,-0.0031553,0.012866,0.029824,0.047487,0.065601,0.083926,0.10234,0.12065,0.1387,0.15626,0.17319,0.18942,0.20498,0.21978,0.23366,0.24665,0.2588,0.27022,0.28092,0.29103,0.30017,0.30862,0.31667,0.32424,0.33127,0.33778,0.34374,0.34927,0.35453,0.35941,0.36388,0.368,0.37184,0.37538,0.37857,0.38148,0.38422,0.38678,0.38903,0.39112,0.39306,0.39486,0.39652,0.39814,0.39971,0.40112,0.40243,0.40374,0.40498,0.40614,0.40727,0.40825,0.40922,0.41017,0.41094,0.41154,0.41218,0.41281,0.41336,0.4141,0.41439,0.4145,0.41497,0.41573,0.41643,0.41699,0.41767,0.41851,0.41953,0.42069,0.42196,0.42346,0.42504,0.42669,0.42843,0.43035,0.43243,0.43441,0.43623,0.43786,0.43943,0.44093,0.44214,0.44285,0.44304,0.44289,0.4424,0.44132,0.43955,0.43711,0.43405,0.4307,0.42677,0.4221,0.41688,0.41125,0.40524,0.39895,0.39239,0.38549,0.37853,0.37166,0.3646,0.35757,0.35082,0.34449,0.33832,0.33227,0.32652,0.32142,0.31679,0.31253,0.30862,0.30518,0.30219,0.29973,0.29775,0.2961,0.29478,0.29393,0.29349,0.29333,0.29348,0.29395,0.29472,0.29585,0.29723,0.29875,0.30057,0.30268,0.30472,0.30738,0.31034,0.31319,0.31598,0.31898,0.32227,0.32572,0.32922,0.33272,0.33621,0.33977,0.34334,0.34694,0.35048,0.35394,0.35726,0.36057,0.36358,0.36645,0.36937,0.37217,0.37462,0.37681,0.3788,0.38067,0.3824,0.38385,0.38505,0.386,0.38683,0.3876,0.3882,0.38855,0.38878,0.38903,0.38924,0.3894,0.38954,0.38964,0.38973,0.38987,0.38996,0.39024,0.39045,0.39056,0.39054,0.39057,0.39061,0.39054,0.39022,0.38975,0.38923,0.38866,0.38787,0.3868,0.38548,0.38407,0.38257,0.38089,0.37898,0.37685,0.37461,0.37221,0.36953,0.3665,0.36309,0.3591,0.35474,0.3498,0.34393,0.33692,0.32875,0.31955,0.30912,0.29724,0.28371,0.26859,0.25196,0.23391,0.21438,0.19339,0.17107,0.14772,0.12357,0.098775,0.073515,0.04809,0.022717,-0.0019257,-0.025823,-0.04901,-0.071158,-0.091765,-0.11066,-0.12781,-0.14327,-0.157,-0.16883,-0.17863,-0.18654,-0.19287,-0.19781,-0.20126,-0.20332,-0.20431,-0.2044,-0.20379,-0.20265,-0.20106,-0.19905,-0.19676,-0.19444,-0.19206,-0.18958,-0.18694,-0.18415,-0.18108,-0.17764,-0.17373,-0.16921,-0.16387,-0.15758,-0.15023,-0.1417,-0.13161,-0.11999,-0.10746,-0.093137,-0.076579,-0.05832,-0.038796,-0.018042,0.0040629,0.027463,0.051969,0.077223,0.10292,0.12895,0.15479,0.18029,0.20545,0.23013,0.25388,0.27614,0.29682,0.31604,0.33383,0.34993,0.36403,0.37608,0.38615,0.39409,0.40046,0.40509,0.4075,0.40754,0.40552,0.40177,0.39601,0.38801,0.37773,0.36534,0.35088,0.33431,0.31538,0.29427,0.27131,0.24652,0.2198,0.19133,0.16153,0.13065,0.098926,0.066691,0.034325,0.0020559,-0.029857,-0.060853,-0.090528,-0.11874,-0.14525,-0.16976,-0.19203,-0.21181,-0.22901,-0.24362,-0.25566,-0.26511,-0.27201,-0.27647,-0.2787,-0.27896,-0.27737,-0.27409,-0.26965,-0.26386,-0.25687,-0.24871,-0.2406,-0.23276,-0.22456,-0.21582,-0.20693,-0.19843,-0.19045,-0.18292,-0.17577,-0.16904,-0.16281,-0.15715,-0.15211,-0.14776,-0.14411,-0.14096,-0.13819,-0.13587,-0.13402,-0.13255,-0.13125,-0.12989,-0.12842,-0.12654,-0.1239,-0.12097,-0.11708,-0.11161,-0.10524,-0.098217,-0.089336,-0.077418,-0.063179,-0.04798,-0.031811,-0.01369,0.0068895,0.029129,0.052422,0.076567,0.1018,0.12787,0.15402,0.17991,0.20572,0.23111,0.25527,0.27779,0.29889,0.3186,0.33655,0.35244,0.36616,0.37776,0.3874,0.39513,0.40098,0.40494,0.40711,0.40757,0.40651,0.40409,0.40054,0.39578,0.38979,0.38254,0.37412,0.36496,0.35435,0.34238,0.32931,0.3152,0.29976,0.28285,0.26448,0.24487,0.22398,0.20186,0.17857,0.15437,0.12942,0.10371,0.076993,0.050524,0.024549,-0.0014408,-0.027645,-0.05315,-0.077226,-0.10005,-0.12186,-0.14242,-0.16106,-0.17779,-0.19311,-0.20692,-0.21881,-0.22894,-0.23753,-0.24408,-0.24874,-0.25271,-0.25664,-0.2598,-0.26109,-0.26095,-0.26058,-0.26071,-0.26048,-0.25967,-0.25854,-0.2577,-0.25711,-0.25636,-0.2554,-0.25472,-0.25436,-0.25387,-0.25307,-0.25219,-0.2515,-0.25087,-0.25005,-0.24893,-0.24764,-0.24649,-0.24529,-0.24406,-0.24276,-0.24139,-0.23999,-0.2387,-0.23762,-0.23689,-0.23632,-0.23591,-0.23565,-0.23566,-0.23601,-0.23656,-0.23719,-0.23776,-0.2382,-0.23847,-0.23848,-0.23807,-0.23715,-0.2354,-0.2328,-0.22933,-0.225,-0.21972,-0.21329,-0.20536,-0.19699,-0.18807,-0.17793,-0.16645,-0.15449,-0.14255]

    q_output=[None]*len(q_data1)
    qd_output=[None]*len(q_data1)
    torque_output=[None]*len(q_data1)
    while not rospy.is_shutdown():
      if i>len(q_data1)-1:
        i=0;
      joint_msg.position = []
      joint_msg.velocity = []  
      joint_msg.effort=[q_data1[i],0,0,0,0,0,0]
      #joint_msg.effort = [joint_sub.effort[0],joint_sub.effort[1],joint_sub.effort[2],joint_sub.effort[3],joint_sub.effort[4],joint_sub.effort[5],joint_sub.effort[6],joint_sub.effort[7]]
      print(joint_msg)
      joint_pub.publish(joint_msg)

      q_output[i]=joint_sub.position
      qd_output[i]=joint_sub.velocity
      torque_output[i]=joint_sub.effort

      rospy.sleep(1/float(ra))
      i+=1
    rate.sleep()
    
    #Print Code
    with open('data_position.csv', 'wb') as myfile:
        wr = csv.writer(myfile, quoting=csv.QUOTE_NONE)
        for i in range(0,len(q_data1)):
          wr.writerow(q_output[i])
    with open('data_velocity.csv', 'wb') as myfile:
        wr = csv.writer(myfile, quoting=csv.QUOTE_NONE)
        for i in range(0,len(q_data1)):
          wr.writerow(qd_output[i])
    with open('data_torque.csv', 'wb') as myfile:
        wr = csv.writer(myfile, quoting=csv.QUOTE_NONE)
        for i in range(0,len(q_data1)):
          wr.writerow(torque_output[i])    

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
def calibration(portName1, simulated):
    print "INITIALIZING TORSO CALIBRATION..."
    rospy.init_node("torso")

    jointStates = JointState()
    jointStates.name = ["spine_connect","waist_connect","shoulders_connect", "shoulders_left_connect", "shoulders_right_connect"]
    jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0]

    pubTorsoPos = rospy.Publisher("/hardware/torso/current_pose",Float32MultiArray, queue_size = 1) 
    pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)

    msgCurrentPose = Float32MultiArray()
    msgCurrentPose.data = [0,0,0]

    rate = rospy.Rate(30)
    if not simulated:
        print "Torso.-> Trying to open serial port on \"" + portName1 + "\""
        Roboclaw1.Open(portName1, 38400) #ttyACM0  --- M1: front  --- M2: rear
        address = 0x80
        print "Torso.-> Serial port openned on \"" + portName1 + "\" at 38400 bps (Y)"
        print "Torso.-> Clearing previous encoders readings"
        a, bumper , b = Roboclaw1.ReadEncM1(address)
        print "Torso.-> bumper ", bumper
        Roboclaw1.BackwardM2(address, 127) #Abajo-
        
        while bumper == 0 or bumper==1:
            a, bumper , b = Roboclaw1.ReadEncM1(address)
            print "Torso.->bumper ", bumper
        print "Torso.->bumper ", bumper
        Roboclaw1.BackwardM2(address, 0)
        torsoPos = 0
        Roboclaw1.SetEncM2(address, torsoPos)
        Roboclaw1.WriteNVM(address)

        Roboclaw1.ForwardM2(address, 127) #Arriba+
        a, torsoPos , b = Roboclaw1.ReadEncM2(address)
        a, bumper , b = Roboclaw1.ReadEncM1(address)
        while  torsoPos < 84866 or bumper==-1:
            a, torsoPos , b = Roboclaw1.ReadEncM2(address)
            a, bumper , b = Roboclaw1.ReadEncM1(address)
            print "Torso.->bumper ", bumper
        Roboclaw1.BackwardM2(address, 0)
        a, torsoPos , b = Roboclaw1.ReadEncM2(address)
        #print "Torso.-> MAX Torso Pos: ", torsoPos
        Roboclaw1.SetEncM2(address, torsoPos)
        Roboclaw1.WriteNVM(address)
        Roboclaw1.Close()
    else:
        torsoPos = 84866;

    msgCurrentPose.data[0] = (torsoPos*0.352)/169733 #-------------------pulsos a metros
    msgCurrentPose.data[1] = 0.0
    msgCurrentPose.data[2] = 0.0
    
    pubTorsoPos.publish(msgCurrentPose)
    jointStates.header.stamp = rospy.Time.now()
    jointStates.position = [(torsoPos*0.352)/169733, 0.0, 0.0, 0.0, 0.0]
    pubJointStates.publish(jointStates)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
def rotatehead(x):
     pub = rospy.Publisher('/head/commanded_state', JointState)       
     head_command = JointState() 
     head_command.name=["HeadPan"]
     if x > 0:
         head_command.position=[0.0] #forwards    
     else:
         head_command.position=[-180.0] #backwards
     pub.publish(head_command)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
 def initialize_psms(self):
     if self.__AUTOCAMERA_MODE__ == self.MODE.simulation : 
         msg = JointState()
         msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw']
         msg.position = [0.84 , -0.65, 0.10, 0.00, 0.00, 0.00, 0.00]
         self.psm1_pub.publish(msg)
         msg.position = [-0.84 , -0.53, 0.10, 0.00, 0.00, 0.00, 0.00]
         self.psm2_pub.publish(msg)
         self.logerror('psms initialized!')

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
    def execute(self, goal):
        pickle.dump(goal, open("trajecoryGoalMsgVrep.data", "wb"))
        joint_names = goal.trajectory.joint_names
        trajectory_points = goal.trajectory.points
        num_points = len(trajectory_points)
        rospy.loginfo("Received %i trajectory points" % num_points)
        pprint(goal.trajectory)

        end_time = trajectory_points[-1].time_from_start.to_sec()
        control_rate = rospy.Rate(self._control_rate)
        i=num_points
        rospy.loginfo("the num of point is %i" % i )
        #self.jointTrajectoryPublisher.publish(goal.trajectory)

        for trajectory_point in trajectory_points:
            #Publish for vrepp
            jsVrep = JointState()
            jsVrep.name = copy.deepcopy(joint_names)
            jsVrep.position = trajectory_point.positions
            for j in range(len(joint_names)):
                jsVrep.name[j] = self.vrep_joint_names[joint_names[j]]
                vrep_joint_index = int(jsVrep.name[j][-1]) - 1
                print vrep_joint_index
                if vrep_joint_index == 4:
                     self.jointStatePublisherForVrep[vrep_joint_index].publish((-1*jsVrep.position[j]) + self.vrep_position_offset[vrep_joint_index])
                else:
                    self.jointStatePublisherForVrep[vrep_joint_index].publish(jsVrep.position[j] + self.vrep_position_offset[vrep_joint_index])

            #Publisher for real robot
            js = JointState()
            #rospy.loginfo("the length of js.name is %i" %len(js.name))
            js.name=copy.deepcopy(joint_names)
            for j in range(len(joint_names)):
                #rospy.loginfo("j is %i" % j)
                js.name[j] = js.name[j]+'_'+str(i)
            i=i-1
            js.position = trajectory_point.positions
            self.jointStatePublisher.publish(js)


            control_rate.sleep()
            
        start_time = rospy.get_time()
        now_from_start = rospy.get_time() - start_time
        
        last_idx = -1
        self._result.error_code = self._result.SUCCESSFUL
        self.server.set_succeeded(self._result)

        rospy.sleep(5)
        self.execution.publish("OK")

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
 def publishJointState(self):
     msg = JointState()
     msg.header.stamp = rospy.Time.now()
     msg.name = self.jointNames
     msg.position = self.getJointAngles()
     msg.effort = self.getLoads()
     self.jointStatePub.publish(msg)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
 def generate_views(self):        
     rospy.loginfo('Generate views (%s views at pose)' % self.views_at_pose)
     try:
         resp = self.nav_goals(10, self.inflation_radius, self.roi)
         
         if not resp.goals.poses:
             self.views = []
             return
         for j in range(len(resp.goals.poses)):
             for i in range(0,self.views_at_pose):
                 pose = resp.goals.poses[j]
                 pan = random.uniform(self.min_pan, self.max_pan)
                 tilt = random.uniform(self.min_tilt, self.max_tilt)
                 jointstate = JointState()
                 jointstate.name = ['pan', 'tilt']
                 jointstate.position = [pan, tilt]
                 jointstate.velocity = [self.velocity, self.velocity]
                 jointstate.effort = [float(1.0), float(1.0)]
                 resp_ptu_pose = self.ptu_fk(pan,tilt,pose)
                 p = Pose()
                 view = ScitosView(self.next_id(), pose,jointstate, resp_ptu_pose.pose)
                 self.views.append(view)
             
     except rospy.ServiceException, e:
         rospy.logerr("Service call failed: %s" % e)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
 def set_stiffness(self):
     stiffness_message = JointState()
     stiffness_message.name = ["HeadYaw", "HeadPitch"]
     stiffness_message.position = [0,0]
     stiffness_message.velocity = [0,0]
     stiffness_message.effort   = [1,1]
     self.stiffness_publisher.publish(stiffness_message)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
def head_wander():
    rospy.init_node('head_wander')
    joints_pub = rospy.Publisher('/cmd_joints', JointState)
    say = rospy.ServiceProxy('/qbo_talk/festival_say', Text2Speach)

    seq = 0
    while not rospy.is_shutdown():
        seq += 1

        msg = JointState()
        msg.header.seq = seq
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = 'base_footprint'

        msg.name = ['head_pan_joint','head_tilt_joint']
        msg.position = [random.uniform(-0.9,0.9), random.uniform(-0.8,0.3)]

        joints_pub.publish(msg)

        if random.random() < 0.3:
            try:
                say(random.choice(UTTERANCES))
            except rospy.ServiceException:
                # Ignore errors
                pass

        rospy.sleep(2.0)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
 def __on_packet(self, buf):
     global last_joint_states, last_joint_states_lock
     now = rospy.get_rostime()
     #stateRT = RobotStateRT.unpack(buf)
     stateRT = RobotStateRT_V32.unpack(buf)
     self.last_stateRT = stateRT
     
     msg = JointState()
     msg.header.stamp = now
     msg.header.frame_id = "From real-time state data"
     msg.name = joint_names
     msg.position = [0.0] * 6
     for i, q in enumerate(stateRT.q_actual):
         msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0)
     msg.velocity = stateRT.qd_actual
     msg.effort = [0]*6
     pub_joint_states.publish(msg)
     with last_joint_states_lock:
         last_joint_states = msg
     
     wrench_msg = WrenchStamped()
     wrench_msg.header.stamp = now
     wrench_msg.wrench.force.x = stateRT.tcp_force[0]
     wrench_msg.wrench.force.y = stateRT.tcp_force[1]
     wrench_msg.wrench.force.z = stateRT.tcp_force[2]
     wrench_msg.wrench.torque.x = stateRT.tcp_force[3]
     wrench_msg.wrench.torque.y = stateRT.tcp_force[4]
     wrench_msg.wrench.torque.z = stateRT.tcp_force[5]
     pub_wrench.publish(wrench_msg)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
def publish_position():
    '''joint position publisher'''    
    msg = JointState()
    msg.header.stamp = rospy.Time.now()
    msg.name = JOINT_NAME_ARRAY
    msg.position = joints_pos
    pub_js.publish(msg)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position [as 别名]
 def test_fk_easy4(self):
     chain_state = JointState()
     chain_state.position = [0, 0, 0]
     eef = self.chain.fk(chain_state, -1)
     print eef
     eef_expected = numpy.matrix([[1, 0, 0, 3], [0, 1, 0, 0], [0, 0, 1, 2], [0, 0, 0, 1]])
     self.assertAlmostEqual(numpy.linalg.norm(eef - eef_expected), 0.0, 6)