Python JointState.header方法代码示例

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
    def default(self, ci="unused"):
        """ Publish the data of the posture sensor as a ROS JointState message """
        js = JointState()
        js.header = self.get_ros_header()

        js.name = [
            "kuka_arm_0_joint",
            "kuka_arm_1_joint",
            "kuka_arm_2_joint",
            "kuka_arm_3_joint",
            "kuka_arm_4_joint",
            "kuka_arm_5_joint",
            "kuka_arm_6_joint",
            "head_pan_joint",
            "head_tilt_joint",
        ]
        js.position = [
            self.data["seg0"],
            self.data["seg1"],
            self.data["seg2"],
            self.data["seg3"],
            self.data["seg4"],
            self.data["seg5"],
            self.data["seg6"],
            self.data["pan"],
            self.data["tilt"],
        ]
        # js.velocity = [1, 1, 1, 1, 1, 1, 1]
        # js.effort = [50, 50, 50, 50, 50, 50, 50]

        self.publish(js)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
def sim_robot():

    rospy.init_node("simulated_robot")
    pub = rospy.Publisher("joint_states", JointState, queue_size=5)

    rospy.Subscriber("joint_1/command", Float64, lambda msg: joint_cb(msg, 0))
    rospy.Subscriber("joint_2/command", Float64, lambda msg: joint_cb(msg, 1))
    rospy.Subscriber("joint_3/command", Float64, lambda msg: joint_cb(msg, 2))
    rospy.Subscriber("joint_4/command", Float64, lambda msg: joint_cb(msg, 3))
    rospy.Subscriber("joint_5/command", Float64, lambda msg: joint_cb(msg, 4))
    rospy.Subscriber("joint_7/command", Float64, lambda msg: joint_cb(msg, 5))

    msg = JointState()
    msg.header = Header(stamp=rospy.Time.now())
    msg.name = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_7"]
    msg.position = [0] * 6
    msg.velocity = [0, 0, 0, 0, 0, 0]
    msg.effort = [0, 0, 0, 0, 0, 0]

    def joint_cb(msg_in, id):
        msg.header = Header(stamp=rospy.Time.now())
        msg.position[id] = msg_in.data

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        pub.publish(msg)
        rate.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
    def arm_joint_state(self, robot_state=None, fail_if_joint_not_found=True):
        '''
        Returns the joint state of the arm in the current planning scene state or the passed in state.

        **Args:**

            *robot_state (arm_navigation_msgs.msg.RobotState):* robot state from which to find the joints.  
            If None, will use the current robot state in the planning scene interface
            
            *fail_if_joint_not_found (boolean):* Raise a value error if an arm joint is not found in the robot state

        **Returns:**
            A sensor_msgs.msg.JointState containing just the arm joints as they are in robot_state

        **Raises:**

            **ValueError:** if fail_if_joint_not_found is True and an arm joint is not found in the robot state
        '''
        if not robot_state:
            robot_state = self._psi.get_robot_state()
        joint_state = JointState()
        joint_state.header = robot_state.joint_state.header
        for n in self.joint_names:
            found = False
            for i in range(len(robot_state.joint_state.name)):
                if robot_state.joint_state.name[i] == n:
                    joint_state.name.append(n)
                    joint_state.position.append(robot_state.joint_state.position[i])
                    found = True
                    break
            if not found and fail_if_joint_not_found:
                raise ValueError('Joint '+n+' is missing from robot state')
        return joint_state

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
def sorted_joint_state_msg(msg, joint_names):
  """
  Returns a sorted C{sensor_msgs/JointState} for the given joint names
  @type  msg: sensor_msgs/JointState
  @param msg: The input message
  @type  joint_names: list
  @param joint_names: The sorted joint names
  @rtype: sensor_msgs/JointState
  @return: The C{JointState} message with the fields in the order given by joint names
  """
  valid_names = set(joint_names).intersection(set(msg.name))
  valid_position = len(msg.name) == len(msg.position)
  valid_velocity = len(msg.name) == len(msg.velocity)
  valid_effort = len(msg.name) == len(msg.effort)
  num_joints = len(valid_names)
  retmsg = JointState()
  retmsg.header = copy.deepcopy(msg.header)
  for name in joint_names:
    if name not in valid_names:
      continue
    idx = msg.name.index(name)
    retmsg.name.append(name)
    if valid_position:
      retmsg.position.append(msg.position[idx])
    if valid_velocity:
      retmsg.velocity.append(msg.velocity[idx])
    if valid_effort:
      retmsg.effort.append(msg.effort[idx])
  return retmsg

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
 def gripper_cb(self, data):
     js = JointState()
     js.header = data.header
     js.name = ["Gripper"]
     js.position = [data.current_pos]
     js.velocity = [data.velocity]
     self.gripper_pub.publish(js)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [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 header [as 别名]
    def _get_neutral_joint_state(self, njs):
        js = JointState()
        js.header = Header(stamp=rospy.Time.now(), frame_id='base')

        for j in njs:
            js.name.append(j)
            js.position.append(njs[j])

        return js

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
    def default(self, ci='unused'):
        js = JointState()
        js.header = self.get_ros_header()

        joints =  fill_missing_pr2_joints(self.data)
        js.name = joints.keys()
        js.position = joints.values()

        self.publish(js)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
def talker():
    global joy_value
    global last_joy
    global joint_state
    rospy.init_node('vsv_arm_teleop_raw')
    sub = rospy.Subscriber('~joy', Joy, joy_cb)
    jsub = rospy.Subscriber('~joint_state', JointState, joint_cb)
    joint = rospy.Publisher('~joint_command', JointState)
    axis={}
    axis["ArmPan"] = [int(s) for s in str(rospy.get_param("~axis_pan","6")).split(",")]
    axis["ArmTilt"] = [int(s) for s in str(rospy.get_param("~axis_tilt","7")).split(",")]
    axis["ArmFold"] = [int(s) for s in str(rospy.get_param("~axis_fold","4,5")).split(",")]
    axis["ArmExtend"] = [int(s) for s in str(rospy.get_param("~axis_extend","0,3")).split(",")]
    axis["ToolRotate"] = [int(s) for s in str(rospy.get_param("~axis_rotate","2,1")).split(",")]
    step_value = {"ArmPan":1*pi/180., "ArmTilt":5*pi/180.,
            "ArmFold":2*pi/180., "ArmExtend":0.05, "ToolRotate":5*pi/180.}
    timeout = True
    rate = rospy.Rate(10)
    while not rospy.is_shutdown() and not joint_state:
        rate.sleep()
    if rospy.is_shutdown():
        return
    command = JointState()
    command.header = joint_state.header
    command.name = joint_state.name
    command.velocity = [0.0] * len(joint_state.name)
    command.effort = [0.0] * len(joint_state.name)
    command.position = [x for x in joint_state.position]
    index = dict(zip(command.name,range(0,len(command.name))))
    while not rospy.is_shutdown():
        if (rospy.rostime.get_time() - last_joy) < 0.5: 
            if timeout:
                timeout = False
                rospy.loginfo("Accepting joystick commands")
            for k in axis.keys():
                if len(axis[k])==1:
                    # Axis
                    command.position[index[k]] = sat(command.position[index[k]]+joy_value.axes[axis[k][0]]*step_value[k],
                            joint_state.position[index[k]],step_value[k])
                else:
                    # Buttons
                    if joy_value.buttons[axis[k][0]]:
                        command.position[index[k]] = sat(command.position[index[k]]-step_value[k],
                            joint_state.position[index[k]],step_value[k])
                    elif joy_value.buttons[axis[k][1]]:
                        command.position[index[k]] = sat(command.position[index[k]]+step_value[k],
                            joint_state.position[index[k]],step_value[k])

            # if twist.linear.x < 0:
            #     twist.angular.z = - twist.angular.z
        else:
            if not timeout:
                timeout = True
                rospy.loginfo("Timeout: ignoring joystick commands")
        joint.publish(command)
        rate.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
 def sync_joint_cb(self,*args):
     self.connected = True
     js = JointState()
     js.header = args[0].header
     for j in args:
         js.name+=j.name
         js.position+=j.position
         js.velocity+=j.velocity
         js.effort+=j.effort
     self.joint_pub.publish(js)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
    def convertJointTrajectorySensorMsg(self, msg):

        # Generate fake JointState msg
        sim_data_msg = JointState()
        sim_data_msg.header = msg.header
        sim_data_msg.name = msg.joint_names
        sim_data_msg.position = msg.actual.positions
        sim_data_msg.velocity = msg.actual.velocities
        sim_data_msg.effort = msg.actual.effort
        return sim_data_msg

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
    def default(self, ci='unused'):
        """ Publish the data of the posture sensor as a ROS JointState message """
        js = JointState()
        js.header = self.get_ros_header()

        joints =  fill_missing_pr2_joints(self.data)
        js.name = joints.keys()
        js.position = joints.values()

        self.publish(js)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
 def receieveJointStates(self, msg):
     if (time.time() - self.lastJointSaveTime > self.streamSavingPeriod):
         armState = JointState()
         armState.header = msg.header
         for i in range(len(msg.name)):
             if (msg.name[i] in self.allJoints):
                 armState.name.append(msg.name[i])
                 armState.position.append(msg.position[i])
         self.jointStateBag.write('arm_states', armState)
         self.lastJointSaveTime = time.time()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
    def default(self, ci='unused'):
        js = JointState()
        js.header = self.get_ros_header()
        js.name = ['head_pan_joint', 'head_tilt_joint']

        js.position = [
            self.data['pan'],
            self.data['tilt']
        ]

        self.publish(js)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import header [as 别名]
    def default(self, ci='unused'):
        js = JointState()
        js.header = self.get_ros_header()

        # collect name and positions of jointstates from sensor
        js.name = list(self.data.keys())[1:]
        js.position = list(self.data.values())[1:]
        # for now leaving out velocity and effort
        #js.velocity = [1, 1, 1, 1, 1, 1, 1]
        #js.effort = [50, 50, 50, 50, 50, 50, 50]

        self.publish(js)