Work on applying the probed heightmap.

2023-05-18 07:24:05 +03:00
parent f78aa930d6
commit 4e7e059e22
13 changed files with 334 additions and 37 deletions
@@ -26,7 +26,7 @@ set(CMAKE_CXX_VISIBILITY_PRESET hidden)

 set(TESTS grbl_test.cpp heightmap_test.cpp)
 set(TERJE terje/gcode.cpp terje/gcode.h terje/gcode_parser.h terje/gcode_parser.cpp terje/machine.h terje/machine.cpp terje/grbl.h terje/grbl.cpp terje/comms.h terje/comms.cpp terje/telnet.h terje/telnet.cpp terje/measure_view_model.h terje/measure_view_model.cpp)
-set(SENDER_GRBL_SRC grbl.h grbl.cpp grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp heightmap.h heightmap.cpp gcode_parser.h gcode_parser.cpp gcode_commands.h gcode_commands.cpp sender_app.h sender_app.cpp)
+set(SENDER_GRBL_SRC grbl.h grbl.cpp grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp heightmap.h heightmap.cpp gcode_parser.h gcode_parser.cpp gcode_commands.h gcode_commands.cpp sender_app.h sender_app.cpp gcode_file.h gcode_file.cpp)

 add_executable(sender main.cpp ${TESTS} ${TERJE} ${SENDER_GRBL_SRC})
 target_link_libraries(sender nanogui GL gtest gtest_main)
@@ -37,13 +37,13 @@ glm::vec<3, double> grbl::line_motion_cmd::interpolate(double ratio) {
    return start + delta() * ratio;
 }

-std::vector<std::shared_ptr<grbl::motion_cmd>> grbl::line_motion_cmd::split(double lngth) {
+std::vector<grbl::motion_cmd*> grbl::line_motion_cmd::split(double lngth) {
    //don't split up rapid or not fully defined motions
    if (rapid || !start_valid || !position_valid[0] || !position_valid[1] || !position_valid[2]) {
-        return {std::shared_ptr<grbl::motion_cmd>(this)};
+        return {this};
    }

-    std::vector<std::shared_ptr<motion_cmd>> result;
+    std::vector<motion_cmd*> result;
    int divisions = (int) std::ceil(length() / lngth);

    if (divisions < 1) {
@@ -61,7 +61,7 @@ std::vector<std::shared_ptr<grbl::motion_cmd>> grbl::line_motion_cmd::split(doub
        immediate->feed = feed;
        immediate->position_valid[0] = immediate->position_valid[1] = immediate->position_valid[2] = true;
        immediate->start_valid = true;
-        result.push_back(std::shared_ptr<line_motion_cmd>(immediate));
+        result.push_back(immediate);

        last_end = end;
    }
@@ -90,7 +90,7 @@ glm::vec<3, double> grbl::arc_motion_cmd::interpolate(double ratio) {
    return interpolation;
 }

-std::vector<std::shared_ptr<grbl::motion_cmd>> grbl::arc_motion_cmd::split(double lngth) {
+std::vector<grbl::motion_cmd*> grbl::arc_motion_cmd::split(double lngth) {
    int divisions = (int) ceil(length() / lngth);

    if (divisions < 1)
@@ -99,7 +99,7 @@ std::vector<std::shared_ptr<grbl::motion_cmd>> grbl::arc_motion_cmd::split(doubl
    glm::vec3 lastEnd = start;


-    std::vector<std::shared_ptr<grbl::motion_cmd>> result;
+    std::vector<grbl::motion_cmd*> result;

    for (int i = 1; i <= divisions; i++) {
        glm::vec<3, double> end = interpolate(((double) i) / divisions);
@@ -113,7 +113,7 @@ std::vector<std::shared_ptr<grbl::motion_cmd>> grbl::arc_motion_cmd::split(doubl
        immediate->u = u;
        immediate->v = v;

-        result.push_back(std::shared_ptr<arc_motion_cmd>(immediate));
+        result.push_back(immediate);

        lastEnd = end;
    }
@@ -52,7 +52,7 @@ namespace grbl {
        // Total travel distance of tool
        virtual double length() const = 0;
        virtual glm::vec<3, double> interpolate(double ratio) = 0;
-        virtual std::vector<std::shared_ptr<motion_cmd>> split(double length) = 0;
+        virtual std::vector<motion_cmd*> split(double length) = 0;
    };

    struct line_motion_cmd : public motion_cmd {
@@ -65,7 +65,7 @@ namespace grbl {

        double length() const override;
        glm::vec<3, double> interpolate(double ratio) override;
-        std::vector<std::shared_ptr<motion_cmd>> split(double length) override;
+        std::vector<motion_cmd*> split(double length) override;
    };

    struct arc_motion_cmd : public motion_cmd {
@@ -112,7 +112,7 @@ namespace grbl {
        }

        glm::vec<3, double> interpolate(double ratio) override;
-        std::vector<std::shared_ptr<motion_cmd>> split(double length) override;
+        std::vector<motion_cmd*> split(double length) override;

        double length() const override;

@@ -0,0 +1,157 @@
+#include "gcode_file.h"
+#include "gcode_parser.h"
+#include <fstream>
+#include <iomanip>
+#include <utility>
+
+grbl::grbl_file grbl::grbl_file::load(std::string path) {
+    grbl::grbl_parser parser;
+
+    grbl::grbl_file result{};
+
+    std::ifstream in_file{path};
+    if (in_file) {
+        parser.parse(in_file);
+        result = grbl_file(parser.commands);
+    }
+
+    return result;
+}
+
+grbl::grbl_file::grbl_file(std::vector<grbl::command*> cmd)
+        : commands(cmd) {
+
+}
+
+std::vector<std::string> grbl::grbl_file::get_gcode() {
+    std::vector<std::string> result;
+
+    result.emplace_back("G90 G91.1 G21 G17");
+
+    grbl::parser_state state;
+    auto xyz = "XYZ";
+
+    std::stringstream ss;
+    ss << std::fixed << std::setprecision(3);
+
+    for (auto &c: commands) {
+        auto *motion = dynamic_cast<motion_cmd *>(c);
+        bool is_motion = motion != nullptr;
+
+        auto *line = dynamic_cast<line_motion_cmd *>(c);
+        auto *arc = dynamic_cast<arc_motion_cmd *>(c);
+        auto *mcode = dynamic_cast<mcode_cmd *>(c);
+        auto *spindle = dynamic_cast<spindle_cmd *>(c);
+        auto *dwell = dynamic_cast<dwell_cmd *>(c);
+
+        if (is_motion) {
+            if (motion->feed != state.feed) {
+                ss.str("");
+                ss << "F" << motion->feed;
+                result.push_back(ss.str());
+                state.feed = motion->feed;
+            }
+        }
+
+        if (line != nullptr) {
+            std::string code = line->rapid ? "G0" : "G1";
+
+            for (int i = 0; i < 3; i++) {
+                if (!line->position_valid[i])
+                    continue;
+                if (!line->start_valid || state.position[i] != line->end[i]) {
+                    ss.str("");
+                    ss << xyz[i] << line->end[i];
+                    code += ss.str();
+                }
+            }
+
+            result.push_back(code);
+
+            state.position = line->end;
+            continue;
+        }
+
+        if (arc != nullptr) {
+            if (state.plane != arc->plane) {
+                switch (arc->plane) {
+                    case arc_plane::xy:
+                        result.emplace_back("G17");
+                        break;
+                    case arc_plane::yz:
+                        result.emplace_back("G19");
+                        break;
+                    case arc_plane::zx:
+                        result.emplace_back("G18");
+                        break;
+                }
+                state.plane = arc->plane;
+            }
+
+            std::string code = arc->direction == arc_direction::cw ? "G2" : "G3";
+            if (state.position.x != arc->end.x) {
+                ss.str("");
+                ss << "X" << arc->end.x;
+                code += ss.str();
+            }
+            if (state.position.y != arc->end.y) {
+                ss.str("");
+                ss << "Y" << arc->end.y;
+                code += ss.str();
+            }
+            if (state.position.z != arc->end.z) {
+                ss.str("");
+                ss << "Z" << arc->end.z;
+                code += ss.str();
+            }
+
+            glm::vec3 center = roll_components({arc->u, arc->v, 0}, (int) arc->plane) - state.position;
+
+            if (center.x != 0 && arc->plane != arc_plane::yz) {
+                ss.str("");
+                ss << " I" << center.x;
+                code += ss.str();
+            }
+            if (center.y != 0 && arc->plane != arc_plane::zx) {
+                ss.str("");
+                ss << " J" << center.y;
+                code += ss.str();
+            }
+            if (center.z != 0 && arc->plane != arc_plane::xy) {
+                ss.str("");
+                ss << " K" << center.z;
+                code += ss.str();
+            }
+
+            result.push_back(code);
+            state.position = arc->end;
+            continue;
+        }
+
+        if (mcode != nullptr) {
+            int code = mcode->code;
+            if (code == 2 || code == 30)
+                continue;
+
+            result.push_back("M" + std::to_string(code));
+            continue;
+        }
+
+        if (spindle != nullptr) {
+            ss.str("");
+            ss << "S" << spindle->speed;
+            result.push_back(ss.str());
+            continue;
+        }
+
+        if (dwell != nullptr) {
+            ss.str("");
+            ss << "G4 P" << dwell->seconds;
+            result.push_back(ss.str());
+            continue;
+        }
+    }
+
+    return result;
+}
+
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <string>
+#include "gcode_commands.h"
+
+namespace grbl {
+
+    struct grbl_file {
+
+        grbl_file() = default;
+        grbl_file(std::vector<grbl::command *> commands);
+        static grbl_file load(std::string path);
+
+        std::vector<std::string> get_gcode();
+
+        std::vector<grbl::command *> commands;
+    };
+}
@@ -121,7 +121,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) {
                throw parse_exception("M code can only have positive integer parameters", line_number);
            }

-            commands.push_back(std::make_shared<mcode_cmd>(param, line_number));
+            commands.push_back(new mcode_cmd{param, line_number});
            it = words.erase(it);
            continue;
        }
@@ -132,7 +132,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) {
                warnings.push_back("spindle speed must be positive. (line " + std::to_string(line_number) + ")");
            }

-            commands.push_back(std::make_shared<spindle_cmd>(std::abs(param), line_number));
+            commands.push_back(new spindle_cmd{std::abs(param), line_number});
            it = words.erase(it);
            continue;
        }
@@ -203,7 +203,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) {
                        warnings.push_back("dwell time must be positive. (line " + std::to_string(line_number) + ")");
                    }

-                    commands.push_back(std::make_shared<dwell_cmd>(std::abs((*next).parameter), line_number));
+                    commands.push_back(new dwell_cmd{std::abs((*next).parameter), line_number});
                    it = words.erase(it);
                    it = words.erase(it);
                    continue;
@@ -341,7 +341,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) {

            std::memcpy(l->position_valid, state.position_valid, sizeof(bool) * 3);

-            commands.push_back(std::shared_ptr<line_motion_cmd>(l));
+            commands.push_back(l);
            state.position = end_pos;
            return;
        }
@@ -512,7 +512,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) {
            a->line_number = line_number;
            a->plane = state.plane;

-            commands.push_back(std::shared_ptr<arc_motion_cmd>(a));
+            commands.push_back(a);
            state.position = end_pos;
            return;
        }
@@ -52,7 +52,7 @@ namespace grbl {

    struct grbl_parser {
        parser_state state;
-        std::vector<std::shared_ptr<command>> commands;
+        std::vector<command *> commands;
        std::vector<std::string> warnings;

        grbl_parser();
@@ -1,5 +1,7 @@
 #include "grbl.h"
 #include "string_utils.h"
+#include "gcode_parser.h"
+#include "gcode_file.h"

 #include <utility>
 #include <fstream>
@@ -38,7 +40,7 @@ grbl::program::program(std::string filename) {
 static auto comment_re = std::regex(R"(^\s*\(([^)]*)\)\s*$)");
 static auto gcode_re = std::regex(R"(([a-zA-Z0-9\s.\-]+)\s*(;.*|\(([^)]*)\))?)");

-bool grbl::program::load_from_stream(std::istream& in) {
+bool grbl::program::load_from_stream(std::istream &in) {
    instructions.clear();

    is_loaded = true;
@@ -74,7 +76,7 @@ bool grbl::program::load_from_stream(std::istream& in) {
    return is_loaded;
 }

-bool grbl::program::load_from_string(const std::string& content) {
+bool grbl::program::load_from_string(const std::string &content) {
    std::stringstream in_stream;
    in_stream << content;

@@ -95,7 +97,7 @@ bool grbl::program::load_from_file(std::string path) {
    return is_loaded;
 }

-std::ostream& operator<<(std::ostream& out, const grbl::instruction_type& t) {
+std::ostream &operator<<(std::ostream &out, const grbl::instruction_type &t) {
    switch (t) {
        case grbl::instruction_type::gcode:
            out << "gcode";
@@ -110,13 +112,118 @@ std::ostream& operator<<(std::ostream& out, const grbl::instruction_type& t) {
    return out;
 }

-std::ostream& operator<<(std::ostream& out, const grbl::instruction& i) {
-    out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment << " }";
+std::ostream &operator<<(std::ostream &out, const grbl::instruction &i) {
+    out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment
+        << " }";
    return out;
 }

-void grbl::program::dump(std::ostream& out) {
-    for (auto& i: instructions) {
+void grbl::program::dump(std::ostream &out) {
+    for (auto &i: instructions) {
        out << i << std::endl;
    }
 }
+
+grbl::program grbl::program::apply_heightmap(grbl::heightmap& grid) {
+
+    double segmentLength = grid.resolution;
+
+    grbl::program result;
+
+    std::vector<command*> new_commands;
+
+    grbl::grbl_parser parser;
+    std::ifstream in_file{filename};
+    if (in_file) {
+        parser.parse(in_file);
+
+        for (auto &c: parser.commands) {
+            auto line = dynamic_cast<grbl::line_motion_cmd *>(c);
+            auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c);
+            if (line != nullptr) {
+
+                // do not split up or modify any lines that are rapid or not fully defined
+                if (!line->start_valid ||
+                    (!line->position_valid[0] || !line->position_valid[1] || !line->position_valid[2]) || line->rapid) {
+                    new_commands.push_back(line);
+                    continue;
+                }
+
+                for (auto &m: line->split(segmentLength)) {
+                    m->start.z += grid.get_z_at(m->start.x, m->start.y);
+                    m->end.z += grid.get_z_at(m->end.x, m->end.y);
+
+                    new_commands.push_back(m);
+                }
+
+            } else if (arc != nullptr) {
+                if (arc->plane != arc_plane::xy) {
+                    throw std::runtime_error(
+                            "GCode contains arcs in YZ or XZ plane (G18/19), can't apply height map. Use 'Arcs to Lines' if you really need this.");
+                }
+
+                for (auto &m: arc->split(segmentLength)) {
+                    m->start.z += grid.get_z_at(m->start.x, m->start.y);
+                    m->end.z += grid.get_z_at(m->end.x, m->end.y);
+
+                    new_commands.push_back(m);
+                }
+
+            } else {
+                new_commands.push_back(c);
+            }
+        }
+
+        grbl::grbl_file file(new_commands);
+        auto new_lines = file.get_gcode();
+        result.load_from_lines(new_lines);
+    }
+
+    return result;
+
+}
+
+bool grbl::program::load_from_lines(std::vector<std::string> lines) {
+    is_loaded = true;
+    size_t line_number = 0;
+    for (auto &line: lines) {
+        line_number++;
+        if (!line.empty()) {
+
+            std::smatch sm{};
+            if (std::regex_match(line, sm, comment_re)) {
+                auto comment = sm.str(1);
+                instructions.emplace_back(instruction::new_comment(line_number, comment));
+            } else if (std::regex_match(line, sm, gcode_re)) {
+                auto command = trim(sm.str(1));
+                auto comment = trim(sm.str(3));
+                instructions.emplace_back(instruction::new_gcode(line_number, command, comment));
+            } else {
+                std::cerr << "Failed to parse line " << line << std::endl;
+                is_loaded = false;
+            }
+        }
+    }
+
+    return is_loaded;
+}
+
+void grbl::program::save(std::string path) {
+    std::ofstream out_file{path};
+    if (out_file) {
+        for (auto &i: instructions) {
+            switch (i.type) {
+                case instruction_type::gcode:
+                    out_file << i.command;
+                    if (!i.comment.empty())
+                        out_file << " (" << i.comment << ")";
+                    out_file << std::endl;
+                    break;
+                default:
+                    break;
+            }
+
+        }
+    }
+
+}
@@ -3,6 +3,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include "heightmap.h"

 namespace grbl {

@@ -32,6 +33,7 @@ struct program {
    bool load_from_file(std::string filename);
    bool load_from_stream(std::istream& in);
    bool load_from_string(const std::string& content);
+    bool load_from_lines(std::vector<std::string> lines);

    void dump(std::ostream& out);

@@ -42,10 +44,11 @@ struct program {
    instruction instruction_at(size_t index) {
        return instructions.at(index);
    }
-
    bool is_loaded = false;
    std::string filename;
    std::vector<instruction> instructions{};
+    grbl::program apply_heightmap(grbl::heightmap& grid);
+    void save(std::string path);
 };

 }
@@ -81,14 +81,14 @@ G00 X0. Y0. Z0.25
    }

    for (auto &c: parser.commands) {
-        auto line = dynamic_cast<grbl::line_motion_cmd *>(c.get());
+        auto line = dynamic_cast<grbl::line_motion_cmd *>(c);
        if (line != nullptr) {
            std::cout << "Line from " << glm::to_string(line->start) << " to " << glm::to_string(line->end)
                      << std::endl;
            continue;
        }

-        auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c.get());
+        auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c);
        if (arc != nullptr) {
            std::cout << "Arc from " << glm::to_string(arc->start) << " to " << glm::to_string(arc->end) << std::endl;
            continue;
@@ -346,6 +346,9 @@ void grbl::program_renderer::initialize_program_buffers() {
 GLsizei grbl::program_renderer::update_model_vbo(const grbl::program &pgm) {
    if (!pgm.is_loaded) return 0;

+    min_pos = glm::vec3(std::numeric_limits<float>::max());
+    max_pos = glm::vec3(std::numeric_limits<float>::min());
+
    min_pos = max_pos = glm::vec3(0);
    /*
    static auto movement_re = std::regex(R"(([gG]0*1?\s+|[xXyYzZ]\s*[0-9\.\-]+))");
@@ -436,19 +439,19 @@ GLsizei grbl::program_renderer::update_model_vbo(const grbl::program &pgm) {
        auto feed_color = glm::vec4(1.0f, 0, 0, 1);

        for (auto &c: parser.commands) {
-            auto line = dynamic_cast<grbl::line_motion_cmd *>(c.get());
-            auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c.get());
+            auto line = dynamic_cast<grbl::line_motion_cmd *>(c);
+            auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c);
            if (line != nullptr) {
-                if (line->rapid)
+                if (line->rapid) {
                    add_line(buffer_data, line->start, rapid_color, line->end, rapid_color);
-                else
+                } else {
                    add_line(buffer_data, line->start, feed_color, line->end, feed_color);

-                if (!line->rapid) {
                    update_model_extents(line->start);
                    update_model_extents(line->end);
                }

+
            } else if (arc != nullptr) {
                auto pieces = arc->split(0.1);
                for (auto &p: pieces) {
@@ -14,12 +14,13 @@ SenderApp::SenderApp()
        tab_widget->set_selected_index(index);
    });

-    info_layer = tab_widget->add<Widget>();
+    info_vscroll = tab_widget->add<nanogui::VScrollPanel>();
+    tab_widget->append_tab("Info", info_vscroll);
+
+    info_layer = info_vscroll->add<Widget>();
    info_layer->set_layout(new nanogui::GroupLayout(10, 20, 30, 0));
    info_layer->add<nanogui::Label>("");

-    tab_widget->append_tab("Info", info_layer);
-
    auto probe_layer = tab_widget->add<Widget>();
    probe_layer->set_layout(new nanogui::GroupLayout(10, 20, 30, 0));
    probe_layer->add<nanogui::Label>("");
@@ -131,6 +132,8 @@ void SenderApp::add_work_parameters_markup() {
    btn_run_program = work_btn_holder->add<nanogui::Button>("Run");
 //        btnRunProgram->set_enabled(false);
    btn_run_program->set_callback([&] {
+//        auto leveled_pgm = pgm.apply_heightmap(heightmap_grid);
+//        leveled_pgm.save("output_corrected6.nc");
        cnc.run_program(pgm, "G" + std::to_string(cbo_work_offset->selected_index() + 54));
    });
    btn_run_program->set_tooltip("Execute program");
@@ -87,7 +87,6 @@ private:
    void draw_contents() override;

    grbl::machine cnc{};
-private:

    nanogui::TextBox *txt_min_z = nullptr, *txt_feed = nullptr;
    nanogui::TextBox *txt_heightmap_from_x = nullptr, *txt_heightmap_from_y = nullptr;
@@ -101,13 +100,20 @@ private:
    nanogui::Color color_green = nanogui::Color(0, 255, 0, 255);
    nanogui::Button *btn_load_program = nullptr, *btn_check_program = nullptr, *btn_run_program = nullptr;

-    nanogui::Widget *info_layer = nullptr, *heightmap_layer = nullptr;
    nanogui::TabWidget *tab_widget = nullptr;
+
+    nanogui::VScrollPanel *info_vscroll = nullptr;
+    nanogui::Widget *info_layer = nullptr;
+
+    nanogui::Widget *heightmap_layer = nullptr;
+
+
    nanogui::VScrollPanel *settings_vscroll = nullptr;
    nanogui::Widget *settings_layer = nullptr;

    nanogui::VScrollPanel *parameters_vscroll = nullptr;
    nanogui::Widget *parameters_layer = nullptr;
+
    nanogui::TextBox *mpos_x_text = nullptr, *mpos_y_text = nullptr, *mpos_z_text = nullptr;
    nanogui::ComboBox *cbo_work_offset = nullptr, *cbo_tool = nullptr, *cbo_jog_feed_rates = nullptr, *cbo_jog_distance = nullptr;