Revision history [back]

for (int index = 0; index < dxl_cnt_; index++)
{
  ROS_INFO("Joint trajectory consists of %ld points.", req.goal_trajectory.points.size());
  ros::Time trajectoryStartTime = ros::Time::now();
  for (int i = 0; i < req.goal_trajectory.points.size(); i++)
  {
    while ( (ros::Time::now() - trajectoryStartTime).toSec() < req.goal_trajectory.points[i].time_from_start.toSec() )
    {
      continue;
    }
    int32_t speed = dxl_wb_->convertVelocity2Value(dxl_id_[index], req.goal_trajectory.points[i].velocities[0]);
    int32_t position = dxl_wb_->convertRadian2Value(dxl_id_[index], req.goal_trajectory.points[i].positions[0]);
    dxl_wb_->itemWrite(dxl_id_[index], "Profile_Velocity", speed);
    dxl_wb_->goalPosition(dxl_id_[index], position);
    ROS_INFO("Writing position to Dynamixel motor #%d: [%d, %d] at time %f.", dxl_id_[index], position, speed, (ros::Time::now() - trajectoryStartTime).toSec());
  }
}

I was able to figure it out for trajectory_msgs/JointTrajectory by using the following:

for (int index = 0; index < dxl_cnt_; index++)
{
  ROS_INFO("Joint trajectory consists of %ld points.", req.goal_trajectory.points.size());
jointTrajectory.points.size());
  ros::Time trajectoryStartTime = ros::Time::now();
  for (int i = 0; i < req.goal_trajectory.points.size(); jointTrajectory.points.size(); i++)
  {
    while ( (ros::Time::now() - trajectoryStartTime).toSec() < req.goal_trajectory.points[i].time_from_start.toSec() jointTrajectory.points[i].time_from_start.toSec() )
    {
      continue;
    }
    int32_t speed = dxl_wb_->convertVelocity2Value(dxl_id_[index], req.goal_trajectory.points[i].velocities[0]);
jointTrajectory.points[i].velocities[0]);
    int32_t position = dxl_wb_->convertRadian2Value(dxl_id_[index], req.goal_trajectory.points[i].positions[0]);
jointTrajectory.points[i].positions[0]);
    dxl_wb_->itemWrite(dxl_id_[index], "Profile_Velocity", speed);
    dxl_wb_->goalPosition(dxl_id_[index], position);
    ROS_INFO("Writing position to Dynamixel motor #%d: [%d, %d] at time %f.", dxl_id_[index], position, speed, (ros::Time::now() - trajectoryStartTime).toSec());
  }
}