rostopic pub Python
$ rostopic type /ur_driver/joint_speed
trajectory_msgs/JointTrajectory

$ rosmsg show trajectory_msgs/JointTrajectory
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string[] joint_names
trajectory_msgs/JointTrajectoryPoint[] points
  float64[] positions
  float64[] velocities
  float64[] accelerations
  float64[] effort
  duration time_from_start


Python publish topic:


from multiprocessing import Process, Queue
import os, time, random

import rospy
from std_msgs.msg import String

from trajectory_msgs.msg import JointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint

import threading
import time

import numpy as np

from ddpg01 import DDPG

agent = DDPG( "env" )

qvel_global = [0, 0, 0, 0, 0, 0]
mutex = threading.Lock()

def talker_4():

    try:

        pub = rospy.Publisher('/ur_driver/joint_speed', JointTrajectory, queue_size=10)

        rate = rospy.Rate(125)

        qvel = [0, 0, 0, 0, 0, 0]

        i_seq = 0

        global qvel_global, mutex

        trajectory = JointTrajectory()

        trajectory.joint_names = ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']

        while not rospy.is_shutdown():

            if mutex.acquire():

                acc_max = 0.01
                for i in range(6):
                    if ( qvel_global[i] - qvel[i] ) > acc_max:
                        qvel[i] += acc_max
                    elif ( qvel_global[i] - qvel[i] ) < -acc_max:
                        qvel[i] -= acc_max
                    else:
                        qvel[i] = qvel_global[i]

                mutex.release()

            trajectory.header.seq = i_seq
            i_seq += 1

            current_time = rospy.Time.now()
            trajectory.header.stamp = current_time

            trajectory.points = [JointTrajectoryPoint(velocities=qvel)]
            pub.publish(trajectory)

            rate.sleep()

    except rospy.ROSInterruptException:
        pass


深度学习推荐
深度学习推荐

墨之科技,版权所有 © Copyright 2017-2027

湘ICP备14012786号     邮箱:ai@inksci.com