bool LoCoBotController::jointCommandMsgCallback()

in robots/LoCoBot/locobot_control/src/locobot_controller.cpp [992:1091]

• 84 lines of code
• 21 McCabe index (conditional complexity)

bool LoCoBotController::jointCommandMsgCallback(locobot_control::JointCommand::Request &req,
                                                locobot_control::JointCommand::Response &res) {
  /*1 to 5 would be each arm joint, 6 for gripper and 7,8 for pan and tilt respectively*/

  int32_t goal_position = 0;
  bool ret;
  const char *log;

  if (req.id == 1) {
    if (torque_control_) {
      if (req.unit == "newt") {
        goal_position = int32_t(req.goal_position*torque_to_current_value_ratio);
      } else {
        goal_position = int32_t(req.goal_position);
      }
      ret = dxl_wb_->itemWrite(req.id, "Goal_Current", goal_position, &log);
    } else {
      if (req.unit == "rad") {
        goal_position = dxl_wb_->convertRadian2Value(req.id, req.goal_position);
      } else {
        goal_position = req.goal_position;
      }

      ret = dxl_wb_->goalPosition(req.id, goal_position, &log);
    }

  } else if (req.id == 2) {
    if (torque_control_) {
      if (req.unit == "newt") {
        goal_position = int32_t(req.goal_position*torque_to_current_value_ratio);
      } else {
        goal_position = req.goal_position;
      }
      goal_position = goal_position / 2;         // as there are 2 motors
      ret = dxl_wb_->itemWrite(req.id, "Goal_Current", goal_position, &log);

      ret = ret && dxl_wb_->itemWrite(req.id + 1, "Goal_Current", -1 * goal_position, &log);
    } else {
      if (req.unit == "rad") {
        goal_position = dxl_wb_->convertRadian2Value(req.id, req.goal_position);
      } else {
        goal_position = req.goal_position;
      }

      ret = dxl_wb_->goalPosition(req.id, goal_position, &log);

      // For the other motor operating for the same joint
      if (req.unit == "rad") {
        goal_position = dxl_wb_->convertRadian2Value(req.id, -1 * req.goal_position);
      } else {
        goal_position = -1 * req.goal_position;
      }

      ret = ret && dxl_wb_->goalPosition(req.id + 1, goal_position, &log);
    }

  } else if ((req.id >= 3) && (req.id < 5)) {
    if (torque_control_) {
      if (req.unit == "newt") {
        goal_position = -1 * int32_t(req.goal_position*torque_to_current_value_ratio);
      } else {
        goal_position = -1 * req.goal_position;
      }

      ret = dxl_wb_->itemWrite(req.id + 1, "Goal_Current", goal_position, &log);
    } else {
      if (req.unit == "rad") {
        goal_position = dxl_wb_->convertRadian2Value(req.id + 1, -1 * req.goal_position);
      } else {
        goal_position = -1 * req.goal_position;
      }

      ret = dxl_wb_->goalPosition(req.id + 1, goal_position, &log);
    }
  } else if (req.id == 5) {
    if (torque_control_) {
      if (req.unit == "newt") {
        ROS_INFO("This motor does not support direct torque... You can write PWM[which is voltage control]");
        ret = false;
      } else {
        goal_position = -1 * req.goal_position;
      }

      ret = dxl_wb_->itemWrite(req.id + 1, "Goal_PWM", goal_position, &log);
    } else {
      if (req.unit == "rad") {
        goal_position = dxl_wb_->convertRadian2Value(req.id + 1, -1 * req.goal_position);
      } else {
        goal_position = -1 * req.goal_position;
      }

      ret = dxl_wb_->goalPosition(req.id + 1, goal_position, &log);
    }
  } else {
    ROS_INFO("Invalid joint id %d", req.id);
    ret = false;
  }

  res.result = ret;
}