sensor_msgs JointState

JointState

sensor_msgs

sensro_msgs

sensor_msgs/JointState Message

JointState

# This is a message that holds data to describe the state of a set of torque controlled joints. 
#
# The state of each joint (revolute or prismatic) is defined by:
#  * the position of the joint (rad or m),
#  * the velocity of the joint (rad/s or m/s) and 
#  * the effort that is applied in the joint (Nm or N).
#
# Each joint is uniquely identified by its name
# The header specifies the time at which the joint states were recorded. All the joint states
# in one message have to be recorded at the same time.
#
# This message consists of a multiple arrays, one for each part of the joint state. 
# The goal is to make each of the fields optional. When e.g. your joints have no
# effort associated with them, you can leave the effort array empty. 
#
# All arrays in this message should have the same size, or be empty.
# This is the only way to uniquely associate the joint name with the correct
# states.

Header header

string[] name
float64[] position
float64[] velocity
float64[] effort

Compact Message Definition

std_msgs/Header header
string[] name
float64[] position
float64[] velocity
float64[] effort

订阅 JointState

import rclpy
from rclpy.node import Node
import array

from sensor_msgs.msg import JointState

import json
import serial

ser = serial.Serial("/dev/ttyUSB0",115200)

class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__('serial_ctrl') # 初始化一个Node
        
        self.position = []
        
        self.subscription = self.create_subscription( # 订阅 Topic JointState
            JointState,
            'joint_states',
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning
    
    def posGet(self, radInput, direcInput, multiInput):
        if radInput == 0:
            return 2047
        else:
            getPos = int(2047 + (direcInput * radInput / 3.1415926 * 2048 * multiInput) + 0.5)
            return getPos

    def listener_callback(self, msg):  # 回调函数
        a = msg.position # 获取话题消息
        
        join1 = self.posGet(a[0], -1, 1)
        join2 = self.posGet(a[1], -1, 3)
        join3 = self.posGet(a[2], -1, 1)
        join4 = self.posGet(a[3],  1, 1)
        join5 = self.posGet(a[4], -1, 1)
        
        data = json.dumps({'T':3,'P1':join1,'P2':join2,'P3':join3,'P4':join4,'P5':join5,'S1':0,'S2':0,'S3':0,'S4':0,'S5':0,'A1':60,'A2':60,'A3':60,'A4':60,'A5':60}) # json 封装
        
        ser.write(data.encode())  # 串口输出
        
        print(data) # 打印日志


def main(args=None):
    rclpy.init(args=args)
    minimal_subscriber = MinimalSubscriber()
    rclpy.spin(minimal_subscriber)
    
    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()