Python JointState.position[i]方法代码示例

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
 def loop(self):
     hz = 20
     r = rospy.Rate(hz)
     msg = JointState()
     for i in range(8):
         msg.name.append("q"+str(i+1))
         
     while not rospy.is_shutdown() and not self.endthread:
         br = tf.TransformBroadcaster()
         msg.header.stamp=rospy.Time.now()
         msg.position = 8 * [0.0]
         msg.effort = 8 * [0.0]
         self.torque[6]=0.1
         for i in range(8):
             msg.position[i]=self.q_desired[i]
             msg.effort[i]=self.torque[i]
         if(self.publishInfo):
             self.publisher.publish(msg)     
             
         if(self.publishDesiredInfo):
             br.sendTransform((self.wTp_desired[0],
                              self.wTp_desired[1],
                              self.wTp_desired[2]),
                              tf.transformations.quaternion_from_euler(self.wTp_desired[3],
                                                                       self.wTp_desired[4],
                                                                       self.wTp_desired[5]),
                              rospy.Time.now(),
                              self.child_frame,
                              self.parent_frame)
             # Add in the tf transform now
         
         self.master.update_idletasks()    
         r.sleep()            

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [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[i] [as 别名]
    def loop(self):
        hz = get_param("rate", 10) # 10hz
        r = rospy.Rate(hz)

        delta = get_param("delta", 0.0)

        # Publish Joint States
        while not rospy.is_shutdown():
            msg = JointState()
            msg.header.stamp = rospy.Time.now()

            if delta > 0:
                self.update(delta)

            # Initialize msg.position, msg.velocity, and msg.effort.
            has_position = len(self.joints.items()) > 0
            has_velocity = False
            has_effort = False
            for (name,joint) in self.joints.items():
                if not has_position and 'position' in joint:
                    has_position = True
                if not has_velocity and 'velocity' in joint:
                    has_velocity = True
                if not has_effort and 'effort' in joint:
                    has_effort = True
            num_joints = len(self.joints.items())
            
            if has_position:
                msg.position = num_joints * [0.0]
            if has_velocity:
                msg.velocity = num_joints * [0.0]
            if has_effort:
                msg.effort = num_joints * [0.0]
            
            for i, name in enumerate(self.joint_list):
                msg.name.append(str(name))
                joint = None

                # Add Free Joint
                if name in self.joints:
                    joint = self.joints[name]
                    factor = 1
                    offset = 0
                
                if has_position and 'position' in joint:
                    pos = self.joint_positions[ name ]
                    msg.position[i] = pos
                    
                if has_velocity and 'velocity' in joint:
                    msg.velocity[i] = joint['velocity'] * factor
                if has_effort and 'effort' in joint:
                    msg.effort[i] = joint['effort']

            self.pub.publish(msg)
            r.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
    def default(self, ci='unused'):
        message = JointState()
        message.name = [''] * 7
        message.position = [0] * 7
        message.velocity = [0] * 7
        message.effort = [0] * 7
        for i in range(7):
            message.name[i] = "arm_%i_joint"%i
            message.position[i] = self.data["kuka_%i"%(i+1)]

        self.publish(message)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
    def __on_packet(self, buf):
        state = RobotState.unpack(buf)
        self.last_state = state
        #import deserialize; deserialize.pstate(self.last_state)

        #log("Packet.  Mode=%s" % state.robot_mode_data.robot_mode)

        if not state.robot_mode_data.real_robot_enabled:
            rospy.logfatal("Real robot is no longer enabled.  Driver is fuxored")
            time.sleep(2)
            sys.exit(1)

        # If the urscript program is not executing, then the driver
        # needs to publish joint states using information from the
        # robot state packet.
        if self.robot_state != self.EXECUTING:
            msg = JointState()
            msg.header.stamp = rospy.get_rostime()
            msg.header.frame_id = "From binary state data"
            msg.name = joint_names
            msg.position = [0.0] * 6
            for i, jd in enumerate(state.joint_data):
                msg.position[i] = jd.q_actual + joint_offsets.get(joint_names[i], 0.0)
            msg.velocity = [jd.qd_actual for jd in state.joint_data]
            msg.effort = [0]*6
            pub_joint_states.publish(msg)
            self.last_joint_states = msg

        # Updates the state machine that determines whether we can program the robot.
        can_execute = (state.robot_mode_data.robot_mode in [RobotMode.READY, RobotMode.RUNNING])
        if self.robot_state == self.CONNECTED:
            if can_execute:
                self.__trigger_ready_to_program()
                self.robot_state = self.READY_TO_PROGRAM
        elif self.robot_state == self.READY_TO_PROGRAM:
            if not can_execute:
                self.robot_state = self.CONNECTED
        elif self.robot_state == self.EXECUTING:
            if not can_execute:
                self.__trigger_halted()
                self.robot_state = self.CONNECTED

        # Report on any unknown packet types that were received
        if len(state.unknown_ptypes) > 0:
            state.unknown_ptypes.sort()
            s_unknown_ptypes = [str(ptype) for ptype in state.unknown_ptypes]
            self.throttle_warn_unknown(1.0, "Ignoring unknown pkt type(s): %s. "
                          "Please report." % ", ".join(s_unknown_ptypes))

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
    def default(self, ci='unused'):
        
        message = JointState()        
        message.header = self.get_ros_header()      
        
        message.name = [''] * 7
        message.position = [0] * 7
        message.velocity = [0] * 7
        message.effort = [0] * 7
        
        # Define name used to export joints
        base_name = "kuka_joint_"
        
        for i in range(7):
            message.name[i] = base_name + ("%d" % (i+1) )
            message.position[i] = self.data[ "kuka_" + ("%d" % (i+1) ) ]

        self.publish(message)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
def main():
    print "INITIALIZING RIGHT ARM NODE IN SIMULATION BY MARCOSOFT..."
    ###Connection with ROS
    rospy.init_node("right_arm")
    br = tf.TransformBroadcaster()
    jointStates = JointState()
    jointStates.name = ["ra_1_joint", "ra_2_joint", "ra_3_joint", "ra_4_joint", "ra_5_joint", "ra_6_joint", "ra_7_joint"]
    jointStates.position = [0, 0, 0, 0, 0, 0, 0]

    subPosition = rospy.Subscriber("right_arm/goal_pose", Float32MultiArray, callbackPos)
    pubArmPose = rospy.Publisher("right_arm/current_pose", Float32MultiArray, queue_size = 1)
    pubGripper = rospy.Publisher("right_arm/current_gripper", Float32, queue_size = 1)
    pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)
    pubArmBattery = rospy.Publisher("/hardware/robot_state/right_arm_battery", Float32, queue_size=1)

    loop = rospy.Rate(10)

    global goalAngles;
    goalAngles = [0, 0, 0, 0, 0, 0, 0]
    angles = [0, 0, 0, 0, 0, 0, 0]
    speeds = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
    msgCurrentPose = Float32MultiArray()
    msgCurrentPose.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    msgCurrentGripper = Float32()
    msgCurrentGripper.data = 0.0
    deltaAngles = [0, 0, 0, 0, 0, 0, 0]
    while not rospy.is_shutdown():
        for i in range(len(deltaAngles)):
            deltaAngles[i] = goalAngles[i] - angles[i]
            if deltaAngles[i] > speeds[i]:
                deltaAngles[i] = speeds[i]
            if deltaAngles[i] < -speeds[i]:
                deltaAngles[i] = -speeds[i]
            angles[i] += deltaAngles[i]
            jointStates.position[i] = angles[i]
            msgCurrentPose.data[i] = angles[i]
        
        jointStates.header.stamp = rospy.Time.now()
        pubJointStates.publish(jointStates)
        pubArmPose.publish(msgCurrentPose)
        msgBattery = Float32()
        msgBattery.data = 11.6
        pubArmBattery.publish(msgBattery);
        loop.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
 def loop(self):
     hz = 200
     r = rospy.Rate(hz)
     msg = JointState()
     for i in range(8):
         msg.name.append("q"+str(i+1))
         
     while not rospy.is_shutdown() and not self.endthread:
         
         msg.header.stamp=rospy.Time.now()
         msg.position = 8 * [0.0]
         msg.effort = 8 * [0.0]
         self.torque[6]=0.1
         for i in range(8):
             msg.position[i]=self.q_desired[i]
             msg.effort[i]=self.torque[i]
         
         if(self.publishInfo==True):
             self.publisher.publish(msg)
         self.master.update_idletasks()    
         r.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
    def on_applyWBCRobotPressed(self):
        print "Send all latest joint values directly to robot as a WBC command"
        if self.first_time:
            print "Uninitialized !"
        else:
            joint_state = JointState()
            joint_state.header.stamp = rospy.Time.now()
            joint_state.name     =  copy.deepcopy(self.joint_states.name)
            joint_state.position =  list(copy.deepcopy(self.joint_states.position))
            joint_state.velocity =  list(copy.deepcopy(self.joint_states.velocity))

            #print "Positions: ",joint_state.position
            for i,name in enumerate(joint_state.name):
                # for each joint, retrieve the data from each controller to populate the WBC vectors
                for controller in self.joint_control.controllers:
                    for joint in controller.joints:
                        if joint.name == self.joint_states.name[i]:
                            print "     fetching value for "+name+" = "+str(joint.position)+"  at ndx="+str(i)
                            joint_state.position[i] = joint.position
                            joint_state.velocity[i] = joint.velocity
                            #joint_state.effort[i]   = joint.effort

            self.wbc_robot_pub.publish(joint_state)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
    def handle(self):
        self.socket_lock = threading.Lock()
        self.last_joint_states = None
        setConnectedRobot(self)
        print "Handling a request"
        try:
            buf = self.recv_more()
            if not buf: return

            while True:
                #print "Buf:", [ord(b) for b in buf]

                # Unpacks the message type
                mtype = struct.unpack_from("!i", buf, 0)[0]
                buf = buf[4:]
                #print "Message type:", mtype

                if mtype == MSG_OUT:
                    # Unpacks string message, terminated by tilde
                    i = buf.find("~")
                    while i < 0:
                        buf = buf + self.recv_more()
                        i = buf.find("~")
                        if len(buf) > 2000:
                            raise Exception("Probably forgot to terminate a string: %s..." % buf[:150])
                    s, buf = buf[:i], buf[i+1:]
                    log("Out: %s" % s)

                elif mtype == MSG_JOINT_STATES:
                    while len(buf) < 3*(6*4):
                        buf = buf + self.recv_more()
                    state_mult = struct.unpack_from("!%ii" % (3*6), buf, 0)
                    buf = buf[3*6*4:]
                    state = [s / MULT_jointstate for s in state_mult]

                    msg = JointState()
                    msg.header.stamp = rospy.get_rostime()
                    msg.name = joint_names
                    msg.position = [0.0] * 6
                    for i, q_meas in enumerate(state[:6]):
                        msg.position[i] = q_meas + joint_offsets.get(joint_names[i], 0.0)
                    msg.velocity = state[6:12]
                    msg.effort = state[12:18]
                    self.last_joint_states = msg
                    pub_joint_states.publish(msg)
                elif mtype == MSG_QUIT:
                    print "Quitting"
                    raise EOF("Received quit")
                elif mtype == MSG_WAYPOINT_FINISHED:
                    while len(buf) < 4:
                        buf = buf + self.recv_more()
                    waypoint_id = struct.unpack_from("!i", buf, 0)[0]
                    buf = buf[4:]
                    print "Waypoint finished (not handled)"
                #gets all IO States and publishes them into a message
                elif mtype == MSG_GET_IO:

                    #gets digital in states
                    while len(buf) < 4:
                        buf = buf + self.recv_more()
                    inp = struct.unpack_from("!i", buf, 0)[0]
                    buf = buf[4:]
                    msg = IOStates()
                    for i in range(0, 10):
                        msg.digital_in_states.append(DigitalIn(i, (inp & (1<<i))>>i))
                    #gets digital out states
                    while len(buf) < 4:
                        buf = buf + self.recv_more()
                    inp = struct.unpack_from("!i", buf, 0)[0]
                    buf = buf[4:]
                    for i in range(0, 10):
                        msg.digital_out_states.append(DigitalOut(i, (inp & (1<<i))>>i))
                    #gets flag states
                    while len(buf) < 4:
                        buf = buf + self.recv_more()
                    inp = struct.unpack_from("!i", buf, 0)[0]
                    buf = buf[4:]
                    for i in range(0, 32):
                        msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
                    #gets analog_out[0] state
                    while len(buf) < 4:
                        buf = buf + self.recv_more()
                    inp = struct.unpack_from("!i", buf, 0)[0]
                    inp /= 1000000.0
                    buf = buf[4:]
                    msg.analog_out_states.append(Analog(0, inp))
                    #gets analog_out[1] state
                    while len(buf) < 4:
                        buf = buf + self.recv_more()
                    inp = struct.unpack_from("!i", buf, 0)[0]
                    inp /= 1000000.0
                    buf = buf[4:]
                    msg.analog_out_states.append(Analog(1, inp))
                    #gets analog_in[0] state
                    while len(buf) < 4:
                        buf = buf + self.recv_more()
                    inp = struct.unpack_from("!i", buf, 0)[0]
                    inp /= 1000000.0
                    buf = buf[4:]
                    msg.analog_in_states.append(Analog(0, inp))
#.........这里部分代码省略.........

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
    def loop(self):
        hz = get_param("rate", 10) # 10hz
        r = rospy.Rate(hz)

        delta = get_param("delta", 0.0)

        msg = JointState()
        msgDisplayRobotState = DisplayRobotState()
        msgRobotState = RobotState()

        # Publish Joint States
        while not rospy.is_shutdown():
            msg.header.stamp = rospy.Time.now()

            if delta > 0:
                self.update(delta)

            # Initialize msg.position, msg.velocity, and msg.effort.
            has_position = len(self.dependent_joints.items()) > 0
            has_velocity = False
            has_effort = False
            for (name,joint) in self.free_joints.items():
                if not has_position and 'position' in joint:
                    has_position = True
                if not has_velocity and 'velocity' in joint:
                    has_velocity = True
                if not has_effort and 'effort' in joint:
                    has_effort = True
            num_joints = (len(self.free_joints.items()) +
                          len(self.dependent_joints.items()))
            if has_position:
                msg.position = num_joints * [0.0]
            if has_velocity:
                msg.velocity = num_joints * [0.0]
            if has_effort:
                msg.effort = num_joints * [0.0]

            
            for i, name in enumerate(self.joint_list):
                msg.name.append(str(name))
                joint = None

                # Add Free Joint
                if name in self.free_joints:
                    joint = self.free_joints[name]
                    factor = 1
                    offset = 0
                # Add Dependent Joint
                elif name in self.dependent_joints:
                    param = self.dependent_joints[name]
                    parent = param['parent']
                    joint = self.free_joints[parent]
                    factor = param.get('factor', 1)
                    offset = param.get('offset', 0)
                
                if has_position and 'position' in joint:
                    pos = self.joint_positions[ name ]
                    msg.position[i] = pos
                    
                if has_velocity and 'velocity' in joint:
                    msg.velocity[i] = joint['velocity'] * factor
                if has_effort and 'effort' in joint:
                    msg.effort[i] = joint['effort']


            msgRobotState.joint_state = msg
            msgDisplayRobotState.state = msgRobotState

            self.pub.publish( msgDisplayRobotState )
            r.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
    def loop(self):
        hz = get_param("rate", 10) # 10hz
        r = rospy.Rate(hz)

        delta = get_param("delta", 0.0)

        # Publish Joint States
        while not rospy.is_shutdown():
            msg = JointState()
            msg.header.stamp = rospy.Time.now()

            if delta > 0:
                self.update(delta)

            # Initialize msg.position, msg.velocity, and msg.effort.
            has_position = len(self.dependent_joints.items()) > 0
            has_velocity = False
            has_effort = False
            for (name,joint) in self.free_joints.items():
                if not has_position and 'position' in joint:
                    has_position = True
                if not has_velocity and 'velocity' in joint:
                    has_velocity = True
                if not has_effort and 'effort' in joint:
                    has_effort = True
            num_joints = (len(self.free_joints.items()) +
                          len(self.dependent_joints.items()))
            if has_position:
                msg.position = num_joints * [0.0]
            if has_velocity:
                msg.velocity = num_joints * [0.0]
            if has_effort:
                msg.effort = num_joints * [0.0]


            for i, name in enumerate(self.joint_list):
                msg.name.append(str(name))
                joint = None

                # Add Free Joint
                if name in self.free_joints:
                    joint = self.free_joints[name]
                    factor = 1
                    offset = 0
                    if has_position and 'position' in joint:
                        msg.position[i] = joint['position'] * factor + offset
                # Add Dependent Joint
                elif name in self.dependent_joints:
                    param = self.dependent_joints[name]
                    if param['name']=='Joint_inter1h' or param['name']=='Joint_inter2h' or param['name']=='Joint_inter3h' or param['name']=='Joint_inter4h':
                        parent = param['parent']
                        joint = self.free_joints[parent]
                        #jointref = self.free_joints[name]
                        factor = param.get('factor', 1)
                        offset = -1.82 # -104.27*pi/180 (about 104.27deg to adjust even if 110.47deg into CAO... ?)

			#L2 = pow(dist_a,2)+pow(dist_b,2)-2*dist_b*dist_a*math.cos(pi/2-joint['position'])
                        #L = pow(L2,0.5)
                        #val1 = math.acos( min(1,max( (pow(dist_a,2)+L2-pow(dist_b,2))/(2*L*dist_a) ,-1)) )
                        #val2 = math.acos( min(1,max( (pow(dist_c,2)+L2-pow(dist_d,2))/(2*L*dist_c) ,-1)) )
                        #pub.publish("%.2f, %.2f, position_inter1h %.2f"%(val1,val2,joint['position']) )

                        # function for dependent movment
                        if has_position and 'position' in joint:
                            L2 = pow(dist_a,2)+pow(dist_b,2)-2*dist_b*dist_a*math.cos(1.57-joint['position'])
                            L = pow(L2,0.5)
                            val1 = math.acos( min(1,max( (pow(dist_a,2)+L2-pow(dist_b,2))/(2*L*dist_a) ,-1)) )
                            val2 = math.acos( min(1,max( (pow(dist_c,2)+L2-pow(dist_d,2))/(2*L*dist_c) ,-1)) )
                            if joint['position']>(-1.57):
                                msg.position[i] = val1 + val2 + offset
                            else:
                                msg.position[i] = -val1 + val2 + offset
                    elif param['name']=='Joint_leg1' or param['name']=='Joint_leg2' or param['name']=='Joint_leg3' or param['name']=='Joint_leg4':
                        parent = param['parent']
                        joint = self.free_joints[parent]
                        #jointref = self.free_joints[name]
                        factor = param.get('factor', 1)
                        offset = -1.72 # (-98.55deg=81.45-180)*pi/180 (about 81.45deg instead 87.61deg into CAO... ?)

			#L2 = pow(dist_a,2)+pow(dist_b,2)-2*dist_b*dist_a*math.cos(1.57-joint['position'])
                        #L = pow(L2,0.5)
                        #val1 = math.acos( min(1,max( (pow(dist_b,2)+L2-pow(dist_a,2))/(2*L*dist_b) ,-1)) )
                        #val2 = math.acos( min(1,max( (pow(dist_d,2)+L2-pow(dist_c,2))/(2*L*dist_d) ,-1)) )
                        #pub.publish("%.2f, %.2f, position_inter1 %.2f, L2=%.2f , L+%.2f"%(val1,val2,joint['position'],L2,L) )

                        # function for dependent movment
                        if has_position and 'position' in joint:
                            L2 = pow(dist_a,2)+pow(dist_b,2)-2*dist_b*dist_a*math.cos(1.57-joint['position'])
                            L = pow(L2,0.5)
                            val1 = math.acos( min(1,max( (pow(dist_b,2)+L2-pow(dist_a,2))/(2*L*dist_b) ,-1)) )
                            val2 = math.acos( min(1,max( (pow(dist_d,2)+L2-pow(dist_c,2))/(2*L*dist_d) ,-1)) )
                            if joint['position']>(-1.57):
                                msg.position[i] = val1 + val2 + offset
                            else:
                                msg.position[i] = -val1 + val2 + offset

                    else:
                        parent = param['parent']
                        joint = self.free_joints[parent]
                        factor = param.get('factor', 1)
#.........这里部分代码省略.........

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
    def handle(self):
        self.socket_lock = threading.Lock()
        self.last_joint_states = None
        setConnectedRobot(self)
        print "Handling a request"
        try:
            buf = self.recv_more()
            if not buf: return

            while True:
                #print "Buf:", [ord(b) for b in buf]

                # Unpacks the message type
                mtype = struct.unpack_from("!i", buf, 0)[0]
                buf = buf[4:]
                #print "Message type:", mtype

                if mtype == MSG_OUT:
                    # Unpacks string message, terminated by tilde
                    i = buf.find("~")
                    while i < 0:
                        buf = buf + self.recv_more()
                        i = buf.find("~")
                        if len(buf) > 2000:
                            raise Exception("Probably forgot to terminate a string: %s..." % buf[:150])
                    s, buf = buf[:i], buf[i+1:]
                    log("Out: %s" % s)

                elif mtype == MSG_JOINT_STATES:
                    while len(buf) < 3*(6*4):
                        buf = buf + self.recv_more()
                    state_mult = struct.unpack_from("!%ii" % (3*6), buf, 0)
                    buf = buf[3*6*4:]
                    state = [s / MULT_jointstate for s in state_mult]

                    msg = JointState()
                    msg.header.stamp = rospy.Time.now()
                    msg.name = joint_names
                    msg.position = [0.0] * 6
                    for i, q_meas in enumerate(state[:6]):
                        msg.position[i] = q_meas + joint_offsets.get(joint_names[i], 0.0)
                    msg.velocity = state[6:12]
                    msg.effort = state[12:18]
                    self.last_joint_states = msg
                    pub_joint_states.publish(msg)
                elif mtype == MSG_QUIT:
                    print "Quitting"
                    raise EOF("Received quit")
                elif mtype == MSG_WAYPOINT_FINISHED:
                    while len(buf) < 4:
                        buf = buf + self.recv_more()
                    waypoint_id = struct.unpack_from("!i", buf, 0)[0]
                    buf = buf[4:]
                    print "Waypoint finished (not handled)"
                else:
                    raise Exception("Unknown message type: %i" % mtype)

                if not buf:
                    buf = buf + self.recv_more()
        except EOF, ex:
            print "Connection closed (command):", ex
            setConnectedRobot(None)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
    def loop(self):
        hz = get_param("rate", 10) # 10hz
        r = rospy.Rate(hz)

        delta = get_param("delta", 0.0)

        # create list of joint controllers
        joint_controller_dict = {}
        for name in self.joint_list:
            dxl_topic = '/'+ str( name ) + '_controller/command'
            joint_controller_dict[ str( name ) ] = rospy.Publisher(dxl_topic, Float64, queue_size=5)
            print( 'Created publisher: ' + dxl_topic )

        # Publish Joint States
        while not rospy.is_shutdown():
            msg = JointState()
            msg.header.stamp = rospy.Time.now()

            if delta > 0:
                self.update(delta)

            # Initialize msg.position, msg.velocity, and msg.effort.
            has_position = len(self.dependent_joints.items()) > 0
            has_velocity = False
            has_effort = False
            for (name,joint) in self.free_joints.items():
                if not has_position and 'position' in joint:
                    has_position = True
                if not has_velocity and 'velocity' in joint:
                    has_velocity = True
                if not has_effort and 'effort' in joint:
                    has_effort = True
            num_joints = (len(self.free_joints.items()) +
                          len(self.dependent_joints.items()))
            if has_position:
                msg.position = num_joints * [0.0]
            if has_velocity:
                msg.velocity = num_joints * [0.0]
            if has_effort:
                msg.effort = num_joints * [0.0]

            
            for i, name in enumerate(self.joint_list):
                msg.name.append(str(name))
                joint = None

                # Add Free Joint
                if name in self.free_joints:
                    joint = self.free_joints[name]
                    factor = 1
                    offset = 0
                # Add Dependent Joint
                elif name in self.dependent_joints:
                    param = self.dependent_joints[name]
                    parent = param['parent']
                    joint = self.free_joints[parent]
                    factor = param.get('factor', 1)
                    offset = param.get('offset', 0)
                
                if has_position and 'position' in joint:
                    pos = self.joint_positions[ name ]
 #                   pos = joint['position'] * factor + offset
                    msg.position[i] = pos
                    
                    dx_msg.data = pos
#                    joint_controller_dict[ str(name) ].publish( dx_msg )
                    
                if has_velocity and 'velocity' in joint:
                    msg.velocity[i] = joint['velocity'] * factor
                if has_effort and 'effort' in joint:
                    msg.effort[i] = joint['effort']

                

            self.pub.publish(msg)
            r.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[i] [as 别名]
    def loop(self):
        hz = get_param("rate", 10)  # 10hz
        r = rospy.Rate(hz)

        delta = get_param("delta", 0.0)

        # Publish Joint States
        while not rospy.is_shutdown():
            msg = JointState()
            msg.header.stamp = rospy.Time.now()

            if delta > 0:
                self.update(delta)

            # Initialize msg.position, msg.velocity, and msg.effort.
            has_position = len(self.dependent_joints.items()) > 0
            has_velocity = False
            has_effort = False
            for name, joint in self.free_joints.items():
                if not has_position and 'position' in joint:
                    has_position = True
                if not has_velocity and 'velocity' in joint:
                    has_velocity = True
                if not has_effort and 'effort' in joint:
                    has_effort = True
            num_joints = (len(self.free_joints.items()) +
                          len(self.dependent_joints.items()))
            if has_position:
                msg.position = num_joints * [0.0]
            if has_velocity:
                msg.velocity = num_joints * [0.0]
            if has_effort:
                msg.effort = num_joints * [0.0]

            for i, name in enumerate(self.joint_list):
                msg.name.append(str(name))
                joint = None

                # Add Free Joint
                if name in self.free_joints:
                    joint = self.free_joints[name]
                    factor = 1
                    offset = 0
                # Add Dependent Joint
                elif name in self.dependent_joints:
                    param = self.dependent_joints[name]
                    parent = param['parent']
                    factor = param.get('factor', 1)
                    offset = param.get('offset', 0)
                    # Handle recursive mimic chain
                    recursive_mimic_chain_joints = [name]
                    while parent in self.dependent_joints:
                        if parent in recursive_mimic_chain_joints:
                            error_message = "Found an infinite recursive mimic chain"
                            rospy.logerr("%s: [%s, %s]", error_message, ', '.join(recursive_mimic_chain_joints), parent)
                            sys.exit(-1)
                        recursive_mimic_chain_joints.append(parent)
                        param = self.dependent_joints[parent]
                        parent = param['parent']
                        offset += factor * param.get('offset', 0)
                        factor *= param.get('factor', 1)
                    joint = self.free_joints[parent]

                if has_position and 'position' in joint:
                    msg.position[i] = joint['position'] * factor + offset
                if has_velocity and 'velocity' in joint:
                    msg.velocity[i] = joint['velocity'] * factor
                if has_effort and 'effort' in joint:
                    msg.effort[i] = joint['effort']

            self.pub.publish(msg)
            try:
                r.sleep()
            except rospy.exceptions.ROSTimeMovedBackwardsException:
                pass