Trajectory Splicing Error

edit

Hello,

I am trying to send my Staubli TX90 robot arm a Joint Trajectory command which publishes to /joint_path_command, which the motion_streaming_interface node subscribes to. My code complies and build correctly, but when I run the node, there is an error saying "Trajectory splicing not yet implemented, stopping current motion." I looked online and noticed that:

// read current state value (should be atomic)
int state = this->state_;

ROS_DEBUG("Current state is: %d", state);

if (TransferStates::IDLE != state)
{

if (msg->points.empty())

ROS_INFO("Empty trajectory received, canceling current trajectory");

else

ROS_ERROR("Trajectory splicing not yet implemented, stopping current motion.");

     this->mutex_.lock();

trajectoryStop();

     this->mutex_.unlock();

return;

This is my code for the node that I am running to communicate with the robot arm and subscribe to /joint_states and publish to /joint_path_command:

using namespace std;

sensor_msgs::JointState JS;

void jointStateCallback(const sensor_msgs::JointState msg) { JS = msg; }

int main(int argc, char** argv) { 
  ros::init(argc, argv, "mover_node");

  ros::NodeHandle n;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  bool success;

  ros::Publisher joint_pub = n.advertise<trajectory_msgs::jointtrajectory>("/joint_path_command", 1);
  ros::Subscriber joint_sub = n.subscribe<sensor_msgs::jointstate>("/joint_states", 1, jointStateCallback);
  ros::Rate rate(10.0);

  trajectory_msgs::JointTrajectory JT;

  JT.joint_names.clear();
  JT.joint_names.push_back("joint_1");
  JT.joint_names.push_back("joint_2");
  JT.joint_names.push_back("joint_3");
  JT.joint_names.push_back("joint_4");
  JT.joint_names.push_back("joint_5");
  JT.joint_names.push_back("joint_6");

  JT.points.resize(2);
  JT.points[0].positions.resize(JT.joint_names.size());
  JT.points[1].positions.resize(JT.joint_names.size());

  while(ros::ok()) {

    if(JS.position.size() != 0)
    {
      cout << JT << endl;

      JT.points[0].time_from_start = ros::Duration(0.0001);
      JT.points[0].positions = JS.position;

      JT.points[1].positions[0] = 0.0;
      JT.points[1].positions[1] = 0.0;
      JT.points[1].positions[2] = 0.0;
      JT.points[1].positions[3] = 0.0;
      JT.points[1].positions[4] = 0.0;
      JT.points[1].positions[5] = 0.0;
      JT.points[1].time_from_start = ros::Duration(1);

      joint_pub.publish(JT);
    }
    rate.sleep();
  } 
  ros::shutdown();

  return 0;
}

The values of the JT object are:

header:

seq: 0

stamp: 0.000000000

frame_id:

joint_names[]

joint_names[0]: joint_1

joint_names[1]: joint_2

joint_names[2]: joint_3

joint_names[3]: joint_4

joint_names[4]: joint_5

joint_names[5]: joint_6

points[]

points[0]:

positions[]

  positions[0]: 0.883191

  positions[1]: -0.00796466

  positions[2]: -0.0596276

  positions[3]: 3.05678

  positions[4]: -0.00680966

  positions[5]: 0.0617737

velocities[]

accelerations[]

effort[]

time_from_start: 0.000100000

points[1]:

positions[]

  positions[0]: 0

  positions[1]: 0

  positions[2]: 0

  positions[3]: 0

  positions[4]: 0

  positions[5]: 0

velocities[]

accelerations[]

effort[]

time_from_start: 1.000000000