example.cpp

Go to the documentation of this file.

#include <fanuc_post_processor/post_processor.hpp>
#include <ros/ros.h>

enum Code
{
  FG_BLACK = 30,
  FG_RED = 31,
  FG_GREEN = 32,
  FG_YELLOW = 33,
  FG_BLUE = 34,
  FG_MAGENTA = 35,
  FG_CYAN = 36,
  FG_LIGHT_GRAY = 37,
  FG_DEFAULT = 39,
  BG_RED = 41,
  BG_GREEN = 42,
  BG_BLUE = 44,
  BG_DEFAULT = 49,
  FG_DARK_GRAY = 90,
  FG_LIGHT_RED = 91,
  FG_LIGHT_GREEN = 92,
  FG_LIGHT_YELLOW = 93,
  FG_LIGHT_BLUE = 94,
  FG_LIGHT_MAGENTA = 95,
  FG_LIGHT_CYAN = 96,
  FG_WHITE = 97
};

std::ostream& operator<<(std::ostream& os,
                         Code code)
{
  return os << "\033[" << static_cast<int>(code) << "m";
}

using namespace fanuc_post_processor;

int main(int argc,
         char **argv)
{
  ros::init(argc, argv, "fanuc_post_processor_application");
  ros::NodeHandle node;

  try
  {
    FanucPostProcessor fanuc_pp;
    fanuc_pp.setProgramName("ROS_TP_program_");
    fanuc_pp.setProgramComment("ROS generated");
    fanuc_pp.setApplicative("ARC Welding Equipment : 1,*,*,*,*");

    fanuc_pp.appendComment("This is a ROS generated TP program");
    fanuc_pp.appendEmptyLine();
    fanuc_pp.appendDigitalOutput(5, true);
    fanuc_pp.appendWait(0.5);
    fanuc_pp.appendDigitalOutput(5, false);
    fanuc_pp.appendDigitalOutput(3, 0.5);
    fanuc_pp.appendWait(1);
    fanuc_pp.appendUFrame(9);
    fanuc_pp.appendUTool(2);
    fanuc_pp.appendGroupOutput(3, 5);
    fanuc_pp.appendSetRegister(4, 50);
    fanuc_pp.appendRun("MY_OTHER_TP_PROGRAM");
    fanuc_pp.appendLabel(1);
    fanuc_pp.appendDataMonitorStart(19);

    FanucPose pose;
    // Add external linear axis
    FanucAxis ext_axis(FanucAxis::AxisType::LINEAR, "E1", M_PI);
    pose.groups_.at(0).axis_.push_back(ext_axis);

    // Add extra group with 2 rotative axis, J1 and J2
    std::vector<FanucAxis> axes;
    axes.push_back(FanucAxis(FanucAxis::AxisType::ROTATIVE, "J1", 1));
    axes.push_back(FanucAxis(FanucAxis::AxisType::ROTATIVE, "J2", 2));

    FanucGroup group_positionner(axes);
    pose.groups_.push_back(group_positionner);

    FanucPose::Config config;
    std::get<0>(config) = FanucPose::FlipNonFlip::Flip;
    std::get<1>(config) = FanucPose::Elbow::Up;
    std::get<2>(config) = FanucPose::BackFront::Front;
    pose.configuration_ = config;

    // With C++14, gcc 6 or higher you can do this instead:
    //pose.configuration_ =
    //{ FanucPose::FlipNonFlip::Flip, FanucPose::Elbow::Up, FanucPose::BackFront::Front};

    for (unsigned i(0); i < 10; ++i)
    {
      pose.move_type_ = FanucPose::MovementType::LINEAR;
      pose.speed_register_ = false;
      pose.speed_ = 20;
      pose.speed_type_ = FanucPose::SpeedType::CM_MIN;
      pose.cnt_ = 100;
      pose.option_ = "INC";
      Eigen::Isometry3d pose_1(Eigen::Isometry3d::Identity());
      pose_1.translation() << 0.1, 0.2, 0;
      pose.setRobotPose(pose_1);
      pose.groups_.at(0).user_frame_ = 2;
      pose.groups_.at(0).tool_frame_ = 6;
      pose.groups_.at(1).user_frame_ = 8;
      pose.groups_.at(1).tool_frame_ = 9;
      fanuc_pp.appendPose(pose);
    }

    pose.move_type_ = FanucPose::MovementType::JOINT;
    pose.speed_register_ = true; // Use register for speed
    pose.speed_ = 4; // Register 4
    pose.speed_type_ = FanucPose::SpeedType::PERCENTAGE;
    pose.cnt_ = 60;
    pose.option_.clear();
    Eigen::Isometry3d pose_2(Eigen::Isometry3d::Identity());
    pose_2.translation() << 0.12, 0.1, 0.1;
    pose.setRobotPose(pose_2);
    pose.groups_.at(0).user_frame_ = 1;
    pose.groups_.at(0).tool_frame_ = 1;
    pose.groups_.at(1).user_frame_ = 6;
    pose.groups_.at(1).tool_frame_ = 4;
    fanuc_pp.appendPose(pose);

    FanucPose pose_id;
    pose_id.is_pose_id_ = true; // We will ignore the groups_ of this pose and look for pose_id_ to get it
    pose_id.pose_id_ = 1; // Set the pose ID to be used to 1 (first pose added)
    pose_id.move_type_ = FanucPose::MovementType::LINEAR;
    pose_id.speed_ = 10;
    pose_id.speed_type_ = FanucPose::SpeedType::CM_MIN;
    pose_id.cnt_ = 100;
    fanuc_pp.appendPoseId(pose_id);

    fanuc_pp.appendDataMonitorStop();
    fanuc_pp.appendJumpLabel(1);

    std::vector<std::pair<FanucPostProcessor::ProgramName, FanucPostProcessor::Program>> programs;
    fanuc_pp.generatePrograms(programs, 20);
    ROS_INFO_STREAM("These are the generated programs:");
    for (unsigned i(0); i < programs.size(); ++i)
      ROS_INFO_STREAM(FG_GREEN << programs.at(i).first << FG_DEFAULT << std::endl << programs.at(i).second);

    // Try to upload program
    fanuc_pp.uploadToFtp(programs, "192.168.1.1");
  }
  catch (std::runtime_error &e)
  {
    ROS_ERROR_STREAM(e.what());
    return 1;
  }

  return 0;
}