post_processor.cpp

Go to the documentation of this file.

#include <fanuc_post_processor/post_processor.hpp>

namespace fanuc_post_processor
{

FanucPostProcessor::FanucPostProcessor() :
        program_name_("ros"),
        program_comment_(""),
        permissions_(READ_WRITE),
        applicative_(""),
        line_numbers_(true),
        for_count_(0)
{
}

FanucPostProcessor::~FanucPostProcessor()
{
}

unsigned FanucPostProcessor::generatePrograms(std::vector<std::pair<ProgramName, Program>> &output_programs,
                                              const unsigned lines_per_program)
{
  output_programs.clear();

  if (lines_per_program == 1)
  {
    ROS_ERROR_STREAM("Need at least 2 instructions per program");
    return 0;
  }

  const unsigned number_of_programs(
      lines_per_program == 0 ? 1 : std::ceil((double)commands_.size() / lines_per_program));

  if (number_of_programs > 999)
  {
    ROS_ERROR_STREAM("More than 999 programs would be generated: " << number_of_programs);
    return 0;
  }

  std::string program;
  unsigned program_id(0);
  unsigned line_count(0);

  std::map<unsigned, FanucPose> poses_in_program; // key is the original pose ID, FanucPose contains the 'new' pose ID
  ProgramName program_name;
  for (unsigned cmd_id(0); cmd_id < commands_.size(); ++cmd_id)
  {
    // Initialize program
    if (program.empty())
    {
      // PROG
      program.append("/PROG ");
      program_name = program_name_;
      if (!tweakProgramName(program_name, program_id))
        return 0;
      program.append(program_name);
      program.append("\n");

      // ATTR
      program.append("/ATTR");
      program.append("\n");
      program.append("COMMENT = \"" + program_comment_ + "\";");
      program.append("\n");
      program.append("PROTECT = " + permissionsToString(permissions_) + ";");
      program.append("\n");

      // Default group
      if (!poses_.empty() && poses_.front().groups_.size() > 4)
      {
        ROS_ERROR_STREAM("Poses have more than 4 groups");
        return 0;
      }

      std::string default_group("1,*,*,*,*");

      if (!poses_.empty())
      {
        default_group = "";
        const unsigned number_of_groups(poses_.front().groups_.size());

        // Add group
        for (unsigned i(0); i < number_of_groups; ++i)
        {
          if (default_group.size() == 8) // Last group case
            default_group.append("1");
          else
            default_group.append("1,");
        }

        // Add empty
        for (unsigned i(0); i < 5 - number_of_groups; ++i)
        {
          if (default_group.size() == 8) // Last group case
            default_group.append("*");
          else
            default_group.append("*,");
        }
      }

      program.append("DEFAULT_GROUP = " + default_group + ";");
      program.append("\n");

      // APPL
      if (!applicative_.empty())
      {
        program.append("/APPL\n");
        program.append("  " + applicative_ + ";");
        program.append("\n");
      }

      // MN
      program.append("/MN");
      program.append("\n");
    }

    if (std::shared_ptr<FanucPoseCommand> pose_cmd = std::dynamic_pointer_cast<FanucPoseCommand>(commands_.at(cmd_id)))
    {
      // Check if pose is in the map
      if (poses_in_program.find(pose_cmd->pose_.pose_id_) == poses_in_program.end())
      {
        // Pose is not in the map, we assign it a new ID and add it to the map
        FanucPose modified_pose(pose_cmd->pose_);
        modified_pose.pose_id_ = poses_in_program.size() + 1; // Pose ID starts at 1 (not zero)
        poses_in_program.insert(std::pair<unsigned, FanucPose>(pose_cmd->pose_.pose_id_, modified_pose));
      }
      // If pose is in the map we don't need to do anything

      std::string line(poses_in_program.find(pose_cmd->pose_.pose_id_)->second.getMainLineString());
      prependLineNumber(line, line_count++);
      program.append(line);
    }
    else if (std::shared_ptr<FanucStringCommand> str_cmd = std::dynamic_pointer_cast<FanucStringCommand>(
        commands_.at(cmd_id)))
    {
      std::string line(str_cmd->cmd_);
      prependLineNumber(line, line_count++);
      program.append(line);
    }
    else
    {
      ROS_ERROR_STREAM("Un-recognized FanucCommand!");
      return 0;
    }
    program.append("\n");

    // Program limit hit
    if (lines_per_program != 0 && line_count == lines_per_program - 1)
    {
      // Don't add a CALL if this is also the last command of the program
      if (cmd_id == commands_.size() - 1)
        break;

      ProgramName next_program_name(program_name_);
      if (!tweakProgramName(next_program_name, program_id + 1))
        return 0;

      std::string call_line("CALL " + next_program_name + " ;\n");
      prependLineNumber(call_line, line_count++);
      program.append(call_line);

      // POS
      program.append("/POS");
      program.append("\n");

      for (auto pose_map : poses_in_program)
        program.append(pose_map.second.getPoseLinesString());

      // END
      program.append("/END\n\n");

      output_programs.push_back(std::pair<ProgramName, Program>(program_name, program));
      ++program_id;
      line_count = 0;
      program.clear();
      poses_in_program.clear();
    }
  }

  // End of commands, finish last program generated
  // POS
  program.append("/POS");
  program.append("\n");

  for (auto pose_map : poses_in_program)
    program.append(pose_map.second.getPoseLinesString());

  // END
  program.append("/END\n\n");
  output_programs.push_back(std::pair<ProgramName, Program>(program_name, program));
  return output_programs.size();
}

void FanucPostProcessor::clearProgram()
{
  commands_.clear();
  poses_.clear();
  labels_id_.clear();
}

void FanucPostProcessor::useLineNumbers(bool line_numbers)
{
  line_numbers_ = line_numbers;
}

void FanucPostProcessor::uploadToFtp(const std::vector<std::pair<ProgramName, Program>> programs,
                                     const std::string ip_address,
                                     const std::string port_number,
                                     const std::string username,
                                     const std::string password)
{
  // Write programs on disk
  for (auto &program : programs)
  {
    std::ofstream ls_program;
    ls_program.open("/tmp/" + program.first + ".ls");
    if (!ls_program.is_open())
      throw std::runtime_error("Could not open /tmp/" + program.first + ".ls for writing");

    ls_program << program.second;
    ls_program.close();

    // Upload to the robot
    ROS_INFO_STREAM("Trying to upload " + program.first + ".ls at " + ip_address);
    try
    {
      curlite::Easy curlite;
      curlite.set(CURLOPT_URL, "ftp://@" + ip_address + ":" + port_number + "/\\" + program.first + ".ls");
      curlite.set(CURLOPT_USERNAME, username);
      curlite.set(CURLOPT_PASSWORD, password);
      curlite.set(CURLOPT_UPLOAD, true);

      // Open input file stream
      std::ifstream ifs("/tmp/" + program.first + ".ls", std::ios::binary);
      ifs >> curlite;

      double total_secs = curlite.getInfo<double>(CURLINFO_TOTAL_TIME);
      ROS_INFO_STREAM("Upload time: " << total_secs << " s");
    }
    catch (std::exception &e)
    {
      throw std::runtime_error(e.what());
    }
  }
}

void FanucPostProcessor::setProgramName(std::string name)
{
  if (name.empty())
    throw std::runtime_error("Program name cannot be empty");
  if (name.find(' ') != std::string::npos)
    throw std::runtime_error("Program name cannot contain a space");
  if (name.size() > program_name_max_size_)
    name.resize(program_name_max_size_);

  program_name_ = name;
  std::transform(program_name_.begin(), program_name_.end(), program_name_.begin(), ::toupper);
}

void FanucPostProcessor::setProgramComment(const std::string comment)
{
  program_comment_ = comment;
}

void FanucPostProcessor::setPermissions(const FanucPostProcessor::Permissions perms)
{
  permissions_ = perms;
}

void FanucPostProcessor::setApplicative(const std::string appl)
{
  applicative_ = appl;
}

void FanucPostProcessor::appendPose(FanucPose &pose)
{
  if (!pose.isValid())
    throw std::runtime_error("Pose is not valid");

  if (!poses_.empty() && !pose.hasSameGroupsAxes(poses_.front()))
    throw std::runtime_error("Pose does not have the same groups/axes of the first pose in the program");

  if (poses_.empty())
    pose.pose_id_ = 1;
  else
    pose.pose_id_ = poses_.back().pose_id_ + 1;

  poses_.push_back(pose);
  std::shared_ptr<FanucPoseCommand> cmd = std::make_shared<FanucPoseCommand>(pose);
  commands_.push_back(cmd);
}

void
FanucPostProcessor::appendPoseId(FanucPose &pose)
{
  if (pose.pose_id_ == 0)
    throw std::runtime_error("Invalid pose id (0)");

  for (auto pose_tmp : poses_)
  {
    if (pose_tmp.pose_id_ == pose.pose_id_)
    {
      pose.groups_.clear(); // Remove these fields to avoid confusion
      std::shared_ptr<FanucPoseCommand> cmd = std::make_shared<FanucPoseCommand>(pose);
      cmd->pose_.groups_ = pose_tmp.groups_; // Copy robot pose

      if (!cmd->pose_.isValid())
        throw std::runtime_error("Pose is not valid");

      commands_.push_back(cmd);
      return;
    }
  }

  throw std::runtime_error("Pose ID " + std::to_string(pose.pose_id_) + " was not found");
}

void FanucPostProcessor::appendComment(const std::string comment)
{
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("!" + comment + ";");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendEmptyLine()
{
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>(";");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendDigitalOutput(const unsigned digital_out_id,
                                             const bool state)
{
  const std::string DO_id(std::to_string(digital_out_id));
  const std::string on_off(state == true ? "ON" : "OFF");
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("DO[" + DO_id + "]=" + on_off + ";");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendDigitalOutput(const unsigned digital_out_id,
                                             const double pulse_time)
{
  if (pulse_time <= 0)
    throw std::runtime_error("Pulse time must be > 0");

  const std::string digital_output_id(std::to_string(digital_out_id));
  std::ostringstream ss;
  ss.imbue(std::locale("C")); // To ensure dot as a decimal separator
  ss << std::fixed << std::setprecision(2);
  ss << pulse_time;
  const std::string time = ss.str();
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>(
      "DO[" + digital_output_id + "]=PULSE," + time + "sec;");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendWait(const double time)
{
  if (time <= 0)
    throw std::runtime_error("Time must be >= 0");

  std::ostringstream ss;
  ss.imbue(std::locale("C")); // To ensure dot as a decimal separator
  ss << std::fixed << std::setprecision(2);
  ss << time;
  const std::string duration = ss.str();
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("WAIT  " + duration + "(sec);");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendWait(const unsigned digital_in_id,
                                    bool state)
{
  const std::string DI_id(std::to_string(digital_in_id));
  const std::string DI_state(state ? "ON" : "OFF");
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>(
      "WAIT DI[" + DI_id + "]=" + DI_state + ";");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendUFrame(const unsigned uf_id)
{
  if (uf_id > 9)
    throw std::runtime_error("UFrame number exceeds maximum of 9, UFrame ID given = " + std::to_string(uf_id));

  const std::string uframe(std::to_string(uf_id));
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("UFRAME_NUM=" + uframe + ";");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendUTool(const unsigned ut_id)
{
  if (ut_id == 0 || ut_id > 9)
    throw std::runtime_error("UTool ID must be between 1 and 9. UTool ID given = " + std::to_string(ut_id));

  const std::string utool(std::to_string(ut_id));
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("UTOOL_NUM=" + utool + ";");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendGroupOutput(const unsigned id,
                                           const unsigned value)
{
  const std::string group_id(std::to_string(id));
  const std::string group_value(std::to_string(value));
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>(
      "GO[" + group_id + "]=" + group_value + ";");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendSetRegister(const unsigned r_id,
                                           const double value)
{
  if (r_id == 0)
    throw std::runtime_error("Register ID starts at 1. 0 Given register ID = 0");

  const std::string id(std::to_string(r_id));
  std::ostringstream ss;
  ss.imbue(std::locale("C")); // To ensure dot as a decimal separator
  ss << std::fixed << std::setprecision(2);
  ss << value;
  const std::string r_value = ss.str();
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("R[" + id + "]=" + r_value + ";");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendCall(const std::string program_name)
{
  std::string name(program_name);
  std::transform(name.begin(), name.end(), name.begin(), ::toupper);
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("CALL " + name + ";");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendRun(const std::string program_name)
{
  std::string name(program_name);
  std::transform(name.begin(), name.end(), name.begin(), ::toupper);
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("RUN " + name + ";");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendLabel(const unsigned id)
{
  // Search if label is used, if not return false
  for (std::vector<unsigned>::iterator iter(labels_id_.begin()); iter != labels_id_.end(); ++iter)
    if (*iter == id)
      throw std::runtime_error("Label ID does not exist. Given label ID = " + std::to_string(id));
  labels_id_.push_back(id); // Label "id" is now available in appendJumpLabel

  const std::string lbl_id(std::to_string(id));
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("LBL[" + lbl_id + "];");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendJumpLabel(const unsigned id)
{
  // Do not check if label is available, it might be declared after the JUMP
  const std::string lbl_id(std::to_string(id));
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("JMP LBL[" + lbl_id + "];");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendDataMonitorStart(const unsigned id)
{
  const std::string datamon_id(std::to_string(id));
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("Sample Start[" + datamon_id + "];");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendDataMonitorStop()
{
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("Sample End;");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendFor(const unsigned reg_id,
                                   const std::string start,
                                   const std::string stop)
{
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>(
      "FOR R[" + std::to_string(reg_id) + "]=" + start + " TO " + stop + ";");
  commands_.push_back(cmd);
  ++for_count_;
}

void FanucPostProcessor::appendFor(const unsigned reg_id,
                                   const unsigned start,
                                   const unsigned stop)
{
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>(
      "FOR R[" + std::to_string(reg_id) + "]=" + std::to_string(start)
          + " TO " + std::to_string(stop) + ";");
  ++for_count_;
}

void FanucPostProcessor::appendEndFor()
{
  if (for_count_ == 0)
    throw std::runtime_error("Cannot append ENDFOR without appending a FOR first.");

  --for_count_;
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>("ENDFOR;");
  commands_.push_back(cmd);
}

void FanucPostProcessor::appendCustomCommand(const std::string command)
{
  std::shared_ptr<FanucStringCommand> cmd = std::make_shared<FanucStringCommand>(command + ";");
  commands_.push_back(cmd);
}

bool FanucPostProcessor::getPoseFromId(const unsigned pose_id,
                                       FanucPose &pose)
{
  bool found = false;
  for (auto pose_tmp : poses_)
  {
    if (pose_tmp.pose_id_ == pose_id)
    {
      pose = pose_tmp;
      found = true;
      break;
    }
  }

  return found;
}

void FanucPostProcessor::prependLineNumber(std::string &line,
                                           const unsigned number)
{
  if (!line_numbers_)
  {
    line.insert(0, ":");
    return;
  }

  if (number > 99999)
    throw std::runtime_error("Line number exceeds 99999! Line number = " + std::to_string(number));

  std::string line_header(std::to_string(number));
  line_header.append(":");

  int characters_to_be_added(6 - line_header.size());
  if (characters_to_be_added < 0)
    throw std::runtime_error("Error computing line number characters");

  for (unsigned i(0); i < (unsigned)characters_to_be_added; ++i)
    line_header.insert(0, " ");

  line.insert(0, line_header);
}

bool FanucPostProcessor::tweakProgramName(ProgramName &name,
                                          const unsigned number)
{
  std::string twk_name(name);
  unsigned digit_count(std::to_string(number).size());
  if (digit_count > program_name_max_size_)
    return false;

  int offset(0);
  while (twk_name.size() + digit_count + offset > program_name_max_size_)
    --offset;

  if (offset < 0)
    twk_name.erase(twk_name.size() + offset, -offset);
  twk_name.append(std::to_string(number));

  name = twk_name;
  return true;
}

}