Syncing Arduino motor movements with Rviz "joint_states"

edit

Hi, I am using rosserial_arduino to connect to Rviz running Moveit plugin. In Rviz, I can move the URDF and execute motion planning, by subscribing to ROS topic "joint_states" on Arduino, the NEMA17 motors run as expected.

in my case shown this video: Robot Experiment with ROS Moveit, the motor movements don't sync with the continuous movements RVIZ, resulting in chopped up movements instead of one smooth flow.

How can I change the way Arduino process "joint_states" in order for the stepper motors to move in sync with Rviz? Or is the default rviz "joint_states" topic an ideal way to directly drive a robot?

I'm a total beginner in ROS and doing my reading in other ROS nodes and methods which can be applied to my scenario. I wish to seek help or referral to any reading links. Thank you very much.

Here is the codes I use on my Arduino driving 3 stepper motors:

#include "pinout.h"

#include <ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Bool.h>
#include <sensor_msgs/JointState.h>
#include <AccelStepper.h>
#include <MultiStepper.h>

//Baud Rate Declarations
#define BAUD 115200

//Stepper Configurations
#define NEMA_17_COUNT 3
#define STEPS_REVOLUTE 3200
#define STEPPER_SPEED 1000

int pulley_teeth_count = 20;
int gear_teeth_count = 90;
double gearRatio = (double)gear_teeth_count / (double)pulley_teeth_count;

int radianToSteps(double radian){
  double radian_360 = 4 * acos(0.0);
  int steps = STEPS_REVOLUTE * gearRatio * radian / radian_360;
  return steps;
}
//Stepper Instances & Functions
int stepsRotation[NEMA_17_COUNT] = {STEPS_REVOLUTE, STEPS_REVOLUTE, STEPS_REVOLUTE};
int stepperDirection[NEMA_17_COUNT] = {1, 1, 1};

AccelStepper joint1(1, ROT_STEP_PIN, ROT_DIR_PIN);
AccelStepper joint2(1, LOW_ARM_STEP_PIN, LOW_ARM_DIR_PIN);
AccelStepper joint3(1, UP_ARM_STEP_PIN, UP_ARM_DIR_PIN);

void pinActivation(){
  pinMode(ROT_ENABLE_PIN,OUTPUT);
  pinMode(UP_ARM_ENABLE_PIN,OUTPUT);
  pinMode(LOW_ARM_ENABLE_PIN,OUTPUT);
  digitalWrite(ROT_ENABLE_PIN, LOW);
  digitalWrite(LOW_ARM_ENABLE_PIN, LOW);
  digitalWrite(UP_ARM_ENABLE_PIN, LOW);    
  pinMode(FAN_PIN, OUTPUT);
  digitalWrite(FAN_PIN, HIGH);
}

MultiStepper steppers;
int joint_step[3] = {};
int joint_status = 0;

void motors_cb(const sensor_msgs::JointState& arm_radian) {
  joint_status = 1;
  joint_step[0] = radianToSteps(arm_radian.position[0]);
  joint_step[1] = radianToSteps(arm_radian.position[1]);
  joint_step[2] = radianToSteps(arm_radian.position[3]); // skipping lower_virtualupper joint
  }

//ROS Declarations
ros::NodeHandle node_handle;
ros::Subscriber<sensor_msgs::JointState> joint_states("joint_states", motors_cb);

void setup(){
  pinActivation();
  node_handle.getHardware()->setBaud(BAUD);
  node_handle.initNode();
  node_handle.subscribe(joint_states);
  joint1.setMaxSpeed(STEPPER_SPEED);
  joint2.setMaxSpeed(STEPPER_SPEED);
  joint3.setMaxSpeed(STEPPER_SPEED);
  steppers.addStepper(joint1);
  steppers.addStepper(joint2);
  steppers.addStepper(joint3);
}

void loop(){
  if (joint_status == 1){  
    long position[3];
    position[0] = stepperDirection[0] * joint_step[0];
    position[1] = stepperDirection[1] * joint_step[1];
    position[2] = stepperDirection[2] * joint_step[2];
    steppers.moveTo(position);
    node_handle.spinOnce();
    steppers.runSpeedToPosition();
  }
  joint_status = 0;
  node_handle.spinOnce();
  delay(1);
}