How to wait for completion when publishing JointState to /goal_dynamixel_position (dynamixel-workbench-controllers)

edit

I am using dynamixel-workbench for a project and setting joint states manually by publishing directly to the topic /goal_dynamixel_position which is established in position_control.cpp. However, if I want to publish a series of joint states to be executed one after another, they will override each other unless manually waiting with rospy.sleep(), or in my case establishing some subscriber that checks to see if my motors are currently moving. I am doing that now by reading messages from /dynamixel_state. I think this is leading to issues where the program can't restart properly, and I'm wondering if there is a better way to do this?

Code for action server for control of dynamixel arm with an isMoving() function:

#! /usr/bin/env python

import rospy
import actionlib
from open_manipulator_core.msg import ArmAction, ArmResult, ArduinoAction, ArduinoGoal
import time
import rospy
from sensor_msgs.msg import JointState
from dynamixel_workbench_msgs.msg import DynamixelStateList, DynamixelState
from std_msgs.msg import Header
import sys
import tf

class ArmServer:
  def __init__(self):
    self.server = actionlib.SimpleActionServer('arm_server', ArmAction, self.execute, False)
    self.server.start()

    self.pub = rospy.Publisher('/goal_dynamixel_position', JointState, queue_size=1)

    self.listener = tf.TransformListener()

    self.sub = rospy.Subscriber("/joint_states", JointState, self.js_callback)
    self.tag_found = False
    self.scan_complete = False

    #self.jt_client = actionlib.SimpleActionClient('/arm/follow_joint_trajectory',FollowJointTrajectoryAction)

    self.joint_state = rospy.wait_for_message('/joint_states', JointState, timeout=5.0)

    self.joint_states = [[0,0,0,0,0,0,1],
                        [0,-0.2,0.3,0.3,0,0,1],
                        [0,-0.3,1,-0.8,0,0,1],
                        [0,-1.15,1.62,-1.8,-1.55,-0.07,1],
                        [0,-1.27,1.58,-1.75,-1.55,-0.13,1.75],
                        [0,-1.44,1.67,-1.32,-1.56,-1.16,1.75],
                        [0,-0.6,1,-1.1,0,0,1.75]]

    self.ready_state = [0,0,0.4,-0.74,0,0,1]

    self.scan_states = [[0,-0.3,1,-0.8,0,0,1],[2,-0.3,1,-0.8,0,0,1]] #NEED TO TEST THESE JOINT STATES

    self.move_started = False

    self.result = ArmResult()

  def isMoving(self):
    moving = False
    self.dynamixel_state_list = rospy.wait_for_message('/dynamixel_state', DynamixelStateList, timeout=5.0)
    for state in self.dynamixel_state_list.dynamixel_state:
      if state.moving == 1:
        moving = True
    return moving

  def js_callback(self,data):
    if self.tag_found and not self.scan_complete:
      rospy.loginfo("------ Tag Found at id_11:")
      rospy.loginfo(data.position[0])
      self.scan_angle = data.position[0]
      self.result.result = data.position[0]
      self.move_started = False
      self.scan_complete = True

  def setJointState(self,pos):
    new_joint_state = self.joint_state

    h = Header()
    h.stamp = rospy.Time.now()
    h.frame_id = self.joint_state.header.frame_id
    new_joint_state.header = h
    new_joint_state.position = pos

    self.pub.publish(new_joint_state)

    rospy.sleep(3)

  def execute(self, goal):
    rate = rospy.Rate(50)

    if goal.sequence == 1: #Grab Anchor Sequence
      for state in self.joint_states[2:7]:
        self.setJointState(state)

        while not self.move_started:
          if self.isMoving():
            self.move_started = True

        while self.move_started:
          if not self.isMoving():
            self.move_started = False


    if goal.sequence == 2: #Arm Ready Position
      self.setJointState(self.ready_state)

      while not self.move_started:
        if self ...