本文整理汇总了Python中dynamixel_msgs.msg.JointState类的典型用法代码示例。如果您正苦于以下问题：Python JointState类的具体用法？Python JointState怎么用？Python JointState使用的例子？那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了JointState类的9个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: __init__
def __init__(self):
rospy.init_node('joint_states_publisher')
motors_list = rospy.get_param('~motors_list')
joint_state_publisher_topic = rospy.get_param('~joint_state_topic', '/dynamixel/joint_states')
urdf_file_path = rospy.get_param('~urdf_file_path', '')
self.joint_state = {}
if len(urdf_file_path) == 0:
robot = URDF.from_parameter_server()
else:
robot = URDF.from_xml_string(open(urdf_file_path,'r').read())
for joint in robot.joints:
joint_state = JointState()
joint_state.name = joint.name
joint_state.current_pos = 0
self.joint_state[joint.name] = joint_state
for motor in motors_list:
joint_state_topic = '/dynamixel/' + motor + '/state'
rospy.Subscriber(joint_state_topic, JointState, self.state_callback)
self.publisher = rospy.Publisher(joint_state_publisher_topic, SensorJointState, queue_size=1)
self.fix_publisher = rospy.Publisher('/joint_states', SensorJointState, queue_size=1)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
self.publish_joint_states()
rate.sleep()示例2: _BroadcastJointStateinfo_P1
def _BroadcastJointStateinfo_P1(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
P1 = ((321-float(lineParts[1]))*0.008)-0.15
P2 = self.left_tilt
P3 = float(lineParts[3])# current
P4 = 0#float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P1_MotorPublisher.publish(Motor_State)
self._left_tilt_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "left_arm_tilt_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
self._P1_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:left_arm_tilt_joint" + str(sys.exc_info()[0]))示例3: _BroadcastJointStateinfo_P1
def _BroadcastJointStateinfo_P1(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
P1 = 0-((float(lineParts[1])* 0.00174532925)-(2.68 + self.cal_pan))# position
P2 = 0-((float(lineParts[2])* 0.00174532925)- (2.68 + self.cal_pan))# target
P3 = float(lineParts[3])# current
P4 = float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P2_MotorPublisher.publish(Motor_State)
Joint_State = JointState()
Joint_State.name = "right_arm_pan_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
self._P1_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:pan" + str(sys.exc_info()[0]))示例4: _BroadcastJointStateinfo_P4
def _BroadcastJointStateinfo_P4(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
P1 = 0-((float(lineParts[1])* 0.00174532925)-1.57)
P2 = 0-((float(lineParts[2])* 0.00174532925)-1.57)
P3 = float(lineParts[3])
P4 = float(lineParts[4])
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P4_MotorPublisher.publish(Motor_State)
#rospy.logwarn(Motor_State)
#rospy.logwarn(val)
Joint_State = JointState()
Joint_State.name = "right_arm_rotate_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()
self._P4_JointPublisher.publish(Joint_State)
except:
rospy.logwarn("Unexpected error:4" + str(sys.exc_info()[0]))示例5: _HandleJoint_1_Command
def _HandleJoint_1_Command(self, data):
""" republish position. """
neck =(data)
self.gear_ratio = 0.3
new_pos =float (data.current_pos) * self.gear_ratio
new_goal=float (data.goal_pos) * self.gear_ratio
new_error = new_goal - new_pos
jointstate = JointState()
jointstate.name =("head_tilt_mod_joint")
jointstate.motor_ids =(neck.motor_ids)
jointstate.motor_temps =(neck.motor_temps)
jointstate.goal_pos =(new_goal)
jointstate.current_pos =(new_pos)
jointstate.velocity =(neck.velocity)
jointstate.load =(neck.load)
jointstate.is_moving =(neck.is_moving)
jointstate.error =(new_error)
#rospy.loginfo(jointstate)
self._neckPublisher.publish(jointstate)示例6: msg_callback_left_finger
def msg_callback_left_finger(self, msg):
out_msg = JointState()
out_msg.header = msg.header
out_msg.goal_pos = msg.set_point
out_msg.current_pos = msg.process_value
out_msg.error = msg.error
#we still need to obtain the speed information about the joint...
#motor IDs are filled according to a joint of the arm
out_msg.motor_ids.append(8)
#other stuff that is not known
out_msg.load=0
out_msg.is_moving=0 # we should obtain this information somehow
out_msg.motor_temps.append(0)
self.pub_left_finger.publish(out_msg)示例7: _BroadcastJointStateinfo_P2
def _BroadcastJointStateinfo_P2(self, lineParts):
partsCount = len(lineParts)
# rospy.logwarn(partsCount)
if partsCount < 7:
pass
try:
off = 299
A1 = float(lineParts[1]) - off
P1 = 0 - ((A1 * 0.00174532925) - 0)
A2 = float(lineParts[2]) - off
P2 = 0 - ((A2 * 0.00174532925) - 0)
P3 = float(lineParts[3]) # current
P4 = 0 # float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P2_MotorPublisher.publish(Motor_State)
self._right_tilt_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "right_arm_tilt_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now() # .stamprospy.Time.from_sec(state.timestamp)
self._P2_JointPublisher.publish(Joint_State)
# rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:right_arm_tilt_joint" + str(sys.exc_info()[0]))示例8: _BroadcastJointStateinfo_P2
def _BroadcastJointStateinfo_P2(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
cal = 0
A1 = (1023 + cal) - float(lineParts[1])
P1 = (A1 * 0.003070961)#(3.8-((float(lineParts[1])* 0.004597701)))# position
A2 = (1023 + cal) - float(lineParts[1])
P2 = (A2* 0.003070961)#float(lineParts[2])# target
P3 = float(lineParts[3])# current
P4 = float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P2_MotorPublisher.publish(Motor_State)
Joint_State = JointState()
Joint_State.name = "right_arm_lift_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
self._P2_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:2" + str(sys.exc_info()[0]))示例9: _BroadcastJointStateinfo_P3
def _BroadcastJointStateinfo_P3(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
#offset = Float(-1.57)
P1 = (float(lineParts[1])/1000)-0.6
P2 = self.right_lift #0-((float(lineParts[2])* 0.00174532925)-1.57)
P3 = 0#float(lineParts[3])
P4 = 0
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P3_MotorPublisher.publish(Motor_State)
#rospy.logwarn(Motor_State)
self._left_lift_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "left_arm_lift_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()
self._P3_JointPublisher.publish(Joint_State)
except:
rospy.logwarn("Unexpected error:left_arm_lift_joint" + str(sys.exc_info()[0]))