void LoCoBotController::execute_joint_trajectory()

in robots/LoCoBot/locobot_control/src/locobot_controller.cpp [415:487]

• 56 lines of code
• 10 McCabe index (conditional complexity)

void LoCoBotController::execute_joint_trajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {

  ros::Time trajectoryStartTime = ros::Time::now();
  ros::Rate loop_rate(25);

  if (goal->trajectory.points.size() < 2) {
    control_msgs::FollowJointTrajectoryResult result;
    result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
    smooth_joint_trajectory_server_->setSucceeded(result);
    return;
  }

  bool result = false;
  const char *log = NULL;

  uint8_t id_array[group_motors_["arm"].size()];
  uint8_t id_cnt = 0;

  int32_t dynamixel_position[group_motors_["arm"].size()];

  static uint32_t point_cnt = 0;
  static uint32_t position_cnt = 0;

  for (auto const &motor:group_motors_["arm"]) {
    id_array[id_cnt++] = dynamixel_name_2ids_[motor];
  }

  for (int i = 0; i < goal->trajectory.points.size(); i++) {
    //check for preempt requests from clients
    if (smooth_joint_trajectory_server_->isPreemptRequested()) {

      //preempt action server
      smooth_joint_trajectory_server_->setPreempted();
      ROS_INFO("Joint trajectory server preempted by client");
      return;
    }

    while ((ros::Time::now() - trajectoryStartTime).toSec() < goal->trajectory.points[i].time_from_start.toSec()) {
      continue;
    }

    for (uint8_t index = 0; index < id_cnt; index++) {
      uint8_t id = id_array[index];
      if (id <= 2) {
        dynamixel_position[index] =
            dxl_wb_->convertRadian2Value(id, goal->trajectory.points[i].positions[id - 1]);
      } else if (id == 3) {
        dynamixel_position[index] = dxl_wb_->convertRadian2Value(id,
                                                                 -goal->trajectory.points[i].positions[1]);
      } else {
        dynamixel_position[index] =
            dxl_wb_->convertRadian2Value(id, -goal->trajectory.points[i].positions[id - 2]);
      }
    }

    {
      std::unique_lock<std::mutex> lock(dynamixel_mutex_);
      result = dxl_wb_->syncWrite(SYNC_WRITE_HANDLER_FOR_GOAL_POSITION,
                                  id_array, id_cnt, dynamixel_position, 1, &log);
    }
    if (!result) {
      ROS_ERROR("%s", log);
    }

    loop_rate.sleep();

  }

  control_msgs::FollowJointTrajectoryResult actionlib_result;
  actionlib_result.error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
  smooth_joint_trajectory_server_->setSucceeded(actionlib_result);
  ROS_INFO("Trajectory goal execution took %fs.", (ros::Time::now() - trajectoryStartTime).toSec());
}