Work on applying the probed heightmap.

CMakeLists.txt

@@ -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)

gcode_commands.cpp

@@ -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;
     }

gcode_commands.h

@@ -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;
 

gcode_file.cpp

@@ -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;
+}
+

gcode_file.h

@@ -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;
+    };
+}

gcode_parser.cpp

@@ -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;
         }

gcode_parser.h

@@ -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();

grbl.cpp

@@ -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) {
+std::ostream &operator<<(std::ostream &out, const grbl::instruction &i) {
-    out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment << " }";
+    out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment
+        << " }";
     return out;
 }
 
-void grbl::program::dump(std::ostream& out) {
+void grbl::program::dump(std::ostream &out) {
-    for (auto& i: instructions) {
+    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;
+            }
+
+        }
+    }
+
+}

grbl.h

@@ -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);
 };
 
 }

grbl_test.cpp

@@ -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;

render.cpp

@@ -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 line = dynamic_cast<grbl::line_motion_cmd *>(c);
-            auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c.get());
+            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) {

sender_app.cpp

@@ -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");

sender_app.h

@@ -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;