diff --git a/.github/deps.repos b/.github/deps.repos
index ef88206..873c139 100644
--- a/.github/deps.repos
+++ b/.github/deps.repos
@@ -1,5 +1,13 @@
 repositories:
-  Fields2Cover:
+  navigation2:
     type: git
-    url: https://github.com/Fields2Cover/Fields2Cover.git
+    url: https://github.com/ros-navigation/navigation2.git
     version: main
+  fields2cover:
+    type: git
+    url: https://github.com/aosmw/Fields2Cover.git
+    version: 9c3c352ba1dc71f3f48706c298b82cd4641e536e
+  ortools_vendor:
+    type: git
+    url: https://github.com/Fields2Cover/ortools_vendor.git
+    version: c28f53e50164645fa8cee0a7a307061ac6f83aa7
diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml
index 7e2a3e6..563c256 100644
--- a/.github/workflows/test.yml
+++ b/.github/workflows/test.yml
@@ -1,3 +1,4 @@
+---
 name: Test
 on:
   pull_request:
@@ -6,29 +7,46 @@ on:
       - main
 
 jobs:
-  build_and_test:
+  noble_rolling:
     runs-on: ubuntu-latest
     env:
       ROS_DISTRO: ${{ matrix.ros_distro }}
-      RMW_IMPLEMENTATION: rmw_cyclonedds_cpp
     container:
-      image: rostooling/setup-ros-docker:ubuntu-jammy-latest
+      image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest
     strategy:
       fail-fast: false
       matrix:
-          ros_distro: [iron]
+        ros_distro: [rolling]
     steps:
-    - uses: actions/checkout@v2
-    - name: Install Cyclone DDS
-      run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y
-    - name: Build and run tests
-      id: action-ros-ci
-      uses: ros-tooling/action-ros-ci@v0.3
-      with:
-        import-token: ${{ secrets.GITHUB_TOKEN }}
-        target-ros2-distro: ${{ matrix.ros_distro }}
-        vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos"
-    - uses: actions/upload-artifact@v1
-      with:
-        name: colcon-logs
-        path: ros_ws/log
+      - name: Install node, libunwind-dev, libgoogle-glog-dev
+        run: |
+          sudo apt-get update
+          sudo apt-get -y install nodejs
+          sudo apt-get -y install libunwind-dev
+          sudo apt-get -y install libgoogle-glog-dev
+          sudo apt-get -y install libceres-dev
+      - name: checkout source
+        uses: actions/checkout@v4
+      - name: Build and run tests
+        id: action-ros-ci
+        uses: ros-tooling/action-ros-ci@v0.3
+        with:
+          package-name: "opennav_coverage_demo"
+          target-ros2-distro: ${{ matrix.ros_distro }}
+          vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos"
+          colcon-defaults: |
+            {
+              "test": {
+                "packages-select": [
+                    "opennav_coverage",
+                    "opennav_coverage_bt",
+                    "opennav_row_coverage",
+                    "opennav_coverage_navigator",
+                    "opennav_coverage_demo"
+                ]
+              }
+            }
+      - uses: actions/upload-artifact@v1
+        with:
+          name: noble-colcon-logs
+          path: ros_ws/log
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..5e1b386
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,59 @@
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+# Colcon output
+build
+log
+install
+
+# Visual Studio Code files
+.vscode
+
+# Eclipse project files
+.cproject
+.project
+.pydevproject
+
+# Python artifacts
+__pycache__/
+*.py[cod]
+.ipynb_checkpoints
+
+sphinx_doc/_build
+
+# CLion artifacts
+.idea
+cmake-build-debug/
+
+# doxygen docs
+doc/html/
+
+#Vim
+*.swp
diff --git a/README.md b/README.md
index 5503441..3080a0b 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
 # Open Navigation's Nav2 Complete Coverage
 
-This package contains the Complete Coverage Task Server & auxiliary tools utilizing the [Fields2Cover](https://github.com/Fields2Cover/Fields2Cover) complete coverage planning system which includes a great deal of options in headland, swath, route, and final path planning. You can find more information about Fields2Cover (F2C) in its [ReadTheDocs Documentation](https://fields2cover.github.io/index.html). It can accept both GPS and Cartesian coordinates and publishes the field, headland, swaths, and route as separate topics in cartesian coordinates for debugging and visualization. It can also compute coverage paths based on open-field polygons **or** based on annotated rows as might exist in a tree farm or other applications with both irregular and regular pre-established rows.
+This package contains the Complete Coverage Task Server & auxiliary tools utilizing the [Fields2Cover](https://github.com/Fields2Cover/Fields2Cover) complete coverage planning system which includes a great deal of options in headland, swath, route, and final path planning. You can find more information about Fields2Cover (F2C) in its [ReadTheDocs Documentation](https://fields2cover.github.io/index.html). It can accept both GPS and Cartesian coordinates and publishes the field, headland, swaths, and route as separate topics in cartesian coordinates for debugging and visualization (topics have Transient Local durability for late-joining visualization tools). It can also compute coverage paths based on open-field polygons **or** based on annotated rows as might exist in a tree farm or other applications with both irregular and regular pre-established rows.
 
 This capability was created by [Open Navigation LLC](https://www.opennav.org/) in partnership with [Bonsai Robotics](https://www.bonsairobotics.ai/). Bonsai Robotics builds autonomous software for machines in adverse and GPS degraded conditions utilizing vision. Bonsai Robotics funded the development of this work for their own product and has graciously allowed Open Navigation to open-source it for the community to leverage in their own systems. Please thank Bonsai Robotics for their commendable donation to the ROS community! Bonsai is hiring [here](https://www.bonsairobotics.ai/careers).
 
diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt
index 2e1ff4e..2ffaf99 100644
--- a/opennav_coverage/CMakeLists.txt
+++ b/opennav_coverage/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(opennav_coverage)
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
@@ -42,6 +43,7 @@ set(dependencies
   builtin_interfaces
   tf2_ros
   opennav_coverage_msgs
+  ament_index_cpp
 )
 
 add_library(${library_name} SHARED
diff --git a/opennav_coverage/include/opennav_coverage/robot_params.hpp b/opennav_coverage/include/opennav_coverage/robot_params.hpp
index 35ba0a2..10078ce 100644
--- a/opennav_coverage/include/opennav_coverage/robot_params.hpp
+++ b/opennav_coverage/include/opennav_coverage/robot_params.hpp
@@ -44,19 +44,19 @@ class RobotParams
   {
     nav2_util::declare_parameter_if_not_declared(
       node, "robot_width", rclcpp::ParameterValue(2.1));
-    robot_.robot_width = node->get_parameter("robot_width").as_double();
+    robot_.setWidth(node->get_parameter("robot_width").as_double());
 
     nav2_util::declare_parameter_if_not_declared(
       node, "operation_width", rclcpp::ParameterValue(2.5));
-    robot_.op_width = node->get_parameter("operation_width").as_double();
+    robot_.setCovWidth(node->get_parameter("operation_width").as_double());
 
     nav2_util::declare_parameter_if_not_declared(
       node, "min_turning_radius", rclcpp::ParameterValue(0.4));
-    robot_.setMinRadius(node->get_parameter("min_turning_radius").as_double());
+    robot_.setMinTurningRadius(node->get_parameter("min_turning_radius").as_double());
 
     nav2_util::declare_parameter_if_not_declared(
       node, "linear_curv_change", rclcpp::ParameterValue(2.0));
-    robot_.linear_curv_change = node->get_parameter("linear_curv_change").as_double();
+    robot_.setMaxDiffCurv(node->get_parameter("linear_curv_change").as_double());
   }
 
   /**
@@ -65,16 +65,16 @@ class RobotParams
    */
   double getWidth()
   {
-    return robot_.robot_width;
+    return robot_.getWidth();
   }
 
   /**
-   * @brief Get the robot's operational width
+   * @brief Get the robot's operational/coverage width
    * @return robot's operational width in m
    */
   double getOperationWidth()
   {
-    return robot_.op_width;
+    return robot_.getCovWidth();
   }
 
   /**
diff --git a/opennav_coverage/include/opennav_coverage/utils.hpp b/opennav_coverage/include/opennav_coverage/utils.hpp
index 43ead77..faad2d5 100644
--- a/opennav_coverage/include/opennav_coverage/utils.hpp
+++ b/opennav_coverage/include/opennav_coverage/utils.hpp
@@ -128,7 +128,7 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg(
   msg.swaths_ordered = true;
   msg.header = header;
 
-  if (raw_path.states.size() == 0) {
+  if (raw_path.size() == 0) {
     return msg;
   }
 
@@ -142,54 +142,55 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg(
     path.moveTo(field.getRefPoint());
   }
 
-  PathSectionType curr_state = path.states[0].type;
-  if (curr_state == PathSectionType::SWATH) {
-    curr_swath_start = path.states[0].point;
-  } else if (curr_state == PathSectionType::TURN) {
+  PathSectionType curr_section_type = path.getState(0).type;
+  if (curr_section_type == PathSectionType::SWATH) {
+    curr_swath_start = path.getState(0).point;
+  } else if (curr_section_type == PathSectionType::TURN) {
     msg.turns.push_back(nav_msgs::msg::Path());
     msg.turns.back().header = header;
     curr_turn = &msg.turns.back();
   }
 
-  for (unsigned int i = 0; i != path.states.size(); i++) {
-    if (curr_state == path.states[i].type && path.states[i].type == PathSectionType::SWATH) {
+  for (unsigned int i = 0; i != path.size(); i++) {
+    auto idx_type = path.getState(i).type;
+    if (curr_section_type == idx_type && idx_type == PathSectionType::SWATH) {
       // Continuing swath so...
       // (1) no action required.
-    } else if (curr_state == path.states[i].type && path.states[i].type == PathSectionType::TURN) {
+    } else if (curr_section_type == idx_type && idx_type == PathSectionType::TURN) {
       // Continuing a turn so...
       // (1) keep populating
-      curr_turn->poses.push_back(toMsg(path.states[i]));
-    } else if (curr_state != path.states[i].type && path.states[i].type == PathSectionType::TURN) {
+      curr_turn->poses.push_back(toMsg(path.getState(i)));
+    } else if (curr_section_type != idx_type && idx_type == PathSectionType::TURN) {
       // Transitioning from a swath to a turn so...
       // (1) Complete the existing swath
       opennav_coverage_msgs::msg::Swath swath;
       swath.start = toMsg(curr_swath_start);
-      swath.end = toMsg(path.states[i - 1].point);
+      swath.end = toMsg(path.getState(i - 1).point);
       msg.swaths.push_back(swath);
       // (2) Start a new turn path
       msg.turns.push_back(nav_msgs::msg::Path());
       msg.turns.back().header = header;
       curr_turn = &msg.turns.back();
-      curr_turn->poses.push_back(toMsg(path.states[i]));
-    } else if (curr_state != path.states[i].type && path.states[i].type == PathSectionType::SWATH) {
+      curr_turn->poses.push_back(toMsg(path.getState(i)));
+    } else if (curr_section_type != idx_type && idx_type == PathSectionType::SWATH) {
       // Transitioning from a turn to a swath so...
       // (1) Update new swath starting point
-      curr_swath_start = path.states[i].point;
+      curr_swath_start = path.getState(i).point;
     }
 
-    curr_state = path.states[i].type;
+    curr_section_type = idx_type;
 
-    if (path.states[i].type != PathSectionType::SWATH &&
-      path.states[i].type != PathSectionType::TURN)
+    if (idx_type != PathSectionType::SWATH &&
+      idx_type != PathSectionType::TURN)
     {
       throw std::runtime_error("Unknown type of path state detected, cannot obtain path!");
     }
   }
 
-  if (curr_state == PathSectionType::SWATH) {
+  if (curr_section_type == PathSectionType::SWATH) {
     opennav_coverage_msgs::msg::Swath swath;
     swath.start = toMsg(curr_swath_start);
-    swath.end = toMsg(path.states.back().point);
+    swath.end = toMsg(path.back().point);
     msg.swaths.push_back(swath);
   }
 
@@ -215,7 +216,7 @@ inline nav_msgs::msg::Path toNavPathMsg(
   nav_msgs::msg::Path msg;
   msg.header = header;
 
-  if (raw_path.states.size() == 0) {
+  if (raw_path.size() == 0) {
     return msg;
   }
 
@@ -226,15 +227,15 @@ inline nav_msgs::msg::Path toNavPathMsg(
     path.moveTo(field.getRefPoint());
   }
 
-  for (unsigned int i = 0; i != path.states.size(); i++) {
+  for (unsigned int i = 0; i != path.size(); i++) {
     // Swaths come in pairs of start-end sequentially
-    if (i > 0 && path.states[i].type == PathSectionType::SWATH &&
-      path.states[i - 1].type == PathSectionType::SWATH)
+    if (i > 0 && path.getState(i).type == PathSectionType::SWATH &&
+      path.getState(i - 1).type == PathSectionType::SWATH)
     {
-      const float & x0 = path.states[i - 1].point.getX();
-      const float & y0 = path.states[i - 1].point.getY();
-      const float & x1 = path.states[i].point.getX();
-      const float & y1 = path.states[i].point.getY();
+      const float & x0 = path.getState(i - 1).point.getX();
+      const float & y0 = path.getState(i - 1).point.getY();
+      const float & x1 = path.getState(i).point.getX();
+      const float & y1 = path.getState(i).point.getY();
 
       const float dist = hypotf(x1 - x0, y1 - y0);
       const float ux = (x1 - x0) / dist;
@@ -243,10 +244,10 @@ inline nav_msgs::msg::Path toNavPathMsg(
 
       geometry_msgs::msg::PoseStamped pose;
       pose.pose.orientation =
-        nav2_util::geometry_utils::orientationAroundZAxis(path.states[i].angle);
+        nav2_util::geometry_utils::orientationAroundZAxis(path.getState(i).angle);
       pose.pose.position.x = x0;
       pose.pose.position.y = y0;
-      pose.pose.position.z = path.states[i].point.getZ();
+      pose.pose.position.z = path.getState(i).point.getZ();
 
       while (curr_dist < dist) {
         pose.pose.position.x += pt_dist * ux;
@@ -256,7 +257,7 @@ inline nav_msgs::msg::Path toNavPathMsg(
       }
     } else {
       // Turns are already dense paths
-      msg.poses.push_back(toMsg(path.states[i]));
+      msg.poses.push_back(toMsg(path.getState(i)));
     }
   }
 
diff --git a/opennav_coverage/include/opennav_coverage/visualizer.hpp b/opennav_coverage/include/opennav_coverage/visualizer.hpp
index 67afd8f..5e30638 100644
--- a/opennav_coverage/include/opennav_coverage/visualizer.hpp
+++ b/opennav_coverage/include/opennav_coverage/visualizer.hpp
@@ -46,16 +46,16 @@ class Visualizer
   {
     nav_plan_pub_ = rclcpp::create_publisher<nav_msgs::msg::Path>(
       node->get_node_topics_interface(),
-      "coverage_server/coverage_plan", rclcpp::QoS(1));
+      "coverage_server/coverage_plan", rclcpp::QoS(1).transient_local());
     headlands_pub_ = rclcpp::create_publisher<geometry_msgs::msg::PolygonStamped>(
       node->get_node_topics_interface(),
-      "coverage_server/field_boundary", rclcpp::QoS(1));
+      "coverage_server/field_boundary", rclcpp::QoS(1).transient_local());
     planning_field_pub_ = rclcpp::create_publisher<geometry_msgs::msg::PolygonStamped>(
       node->get_node_topics_interface(),
-      "coverage_server/planning_field", rclcpp::QoS(1));
+      "coverage_server/planning_field", rclcpp::QoS(1).transient_local());
     swaths_pub_ = rclcpp::create_publisher<visualization_msgs::msg::Marker>(
       node->get_node_topics_interface(),
-      "coverage_server/swaths", rclcpp::QoS(1));
+      "coverage_server/swaths", rclcpp::QoS(1).transient_local());
   }
 
   void deactivate();
diff --git a/opennav_coverage/src/coverage_server.cpp b/opennav_coverage/src/coverage_server.cpp
index 0399e71..3481c16 100644
--- a/opennav_coverage/src/coverage_server.cpp
+++ b/opennav_coverage/src/coverage_server.cpp
@@ -175,7 +175,7 @@ void CoverageServer::computeCoveragePath()
     std::string frame_id;
     if (goal->use_gml_file) {
       master_field = f2c::Parser::importFieldGml(goal->gml_field, true);
-      frame_id = master_field.coord_sys;
+      frame_id = master_field.getCRS();
     } else {
       master_field = util::getFieldFromGoal(goal);
       master_field.setCRS(goal->frame_id);
@@ -185,7 +185,7 @@ void CoverageServer::computeCoveragePath()
     if (!cartesian_frame_) {
       f2c::Transform::transformToUTM(master_field);
     }
-    field = master_field.field.getGeometry(0);
+    field = master_field.getField().getGeometry(0);
 
     RCLCPP_INFO(
       get_logger(),
@@ -269,10 +269,10 @@ CoverageServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramet
         path_gen_->setTurnPointDistance(parameter.as_double());
       } else if (name == "robot_width") {
         auto & robot = robot_params_->getRobot();
-        robot.robot_width = parameter.as_double();
+        robot.setWidth(parameter.as_double());
       } else if (name == "operation_width") {
         auto & robot = robot_params_->getRobot();
-        robot.op_width = parameter.as_double();
+        robot.setCovWidth(parameter.as_double());
       }
     } else if (type == ParameterType::PARAMETER_STRING) {
       if (name == "default_headland_type") {
diff --git a/opennav_coverage/src/path_generator.cpp b/opennav_coverage/src/path_generator.cpp
index 8febd21..063b938 100644
--- a/opennav_coverage/src/path_generator.cpp
+++ b/opennav_coverage/src/path_generator.cpp
@@ -26,17 +26,16 @@ Path PathGenerator::generatePath(
   PathType action_type = toType(settings.mode);
   PathContinuityType action_continuity_type = toContinuityType(settings.continuity_mode);
   std::shared_ptr<f2c::pp::TurningBase> curve{nullptr};
-  float turn_point_distance;
 
   // If not set by action, use default mode
   if (action_type == PathType::UNKNOWN || action_continuity_type == PathContinuityType::UNKNOWN) {
     action_type = default_type_;
     action_continuity_type = default_continuity_type_;
     curve = default_curve_;
-    turn_point_distance = default_turn_point_distance_;
+    curve->setDiscretization(default_turn_point_distance_);
   } else {
     curve = createCurve(action_type, action_continuity_type);
-    turn_point_distance = settings.turn_point_distance;
+    curve->setDiscretization(settings.turn_point_distance);
   }
 
   if (!curve) {
@@ -46,8 +45,7 @@ Path PathGenerator::generatePath(
   RCLCPP_DEBUG(
     logger_,
     "Generating path with curve: %s", toString(action_type, action_continuity_type).c_str());
-  generator_->turn_point_dist = turn_point_distance;
-  return generator_->searchBestPath(robot_params_->getRobot(), swaths, *curve);
+  return generator_->planPath(robot_params_->getRobot(), swaths, *curve);
 }
 
 void PathGenerator::setPathMode(const std::string & new_mode)
diff --git a/opennav_coverage/src/swath_generator.cpp b/opennav_coverage/src/swath_generator.cpp
index 3a1d231..2007577 100644
--- a/opennav_coverage/src/swath_generator.cpp
+++ b/opennav_coverage/src/swath_generator.cpp
@@ -51,7 +51,7 @@ Swaths SwathGenerator::generateSwaths(
       if (!objective) {
         throw CoverageException("No valid swath mode set! Options: LENGTH, NUMBER, COVERAGE.");
       }
-      generator_->step_angle = step_angle;
+      generator_->setStepAngle(step_angle);
       return generator_->generateBestSwaths(*objective, robot_params_->getOperationWidth(), field);
     case SwathAngleType::SET_ANGLE:
       return generator_->generateSwaths(swath_angle, robot_params_->getOperationWidth(), field);
diff --git a/opennav_coverage/src/visualizer.cpp b/opennav_coverage/src/visualizer.cpp
index e3b47c9..3e6d66a 100644
--- a/opennav_coverage/src/visualizer.cpp
+++ b/opennav_coverage/src/visualizer.cpp
@@ -46,6 +46,7 @@ void Visualizer::visualize(
     for (unsigned int i = 0; i != utm_path->poses.size(); i++) {
       utm_path->poses[i].pose.position.x += ref_pt.getX();
       utm_path->poses[i].pose.position.y += ref_pt.getY();
+      utm_path->poses[i].header.frame_id = GLOBAL_FRAME;
     }
     nav_plan_pub_->publish(std::move(utm_path));
   }
diff --git a/opennav_coverage/test/test_headland.cpp b/opennav_coverage/test/test_headland.cpp
index 84b928c..119bbb2 100644
--- a/opennav_coverage/test/test_headland.cpp
+++ b/opennav_coverage/test/test_headland.cpp
@@ -80,13 +80,15 @@ TEST(HeadlandTests, TestheadlandGeneration)
 
   // Generate some toy field
   f2c::Random rand;
-  auto field = rand.generateRandField(5, 1e5);
+  double area = 1e5;
+  int sides = 5;
+  auto field = rand.generateRandField(area, sides);
 
   // Shouldn't throw, results in valid output
   opennav_coverage_msgs::msg::HeadlandMode settings;
-  auto field_out = generator.generateHeadlands(field.field.getGeometry(0), settings);
+  auto field_out = generator.generateHeadlands(field.getField().getGeometry(0), settings);
   settings.mode = "CONSTANT";
-  auto field_out2 = generator.generateHeadlands(field.field.getGeometry(0), settings);
+  auto field_out2 = generator.generateHeadlands(field.getField().getGeometry(0), settings);
 }
 
 }  // namespace opennav_coverage
diff --git a/opennav_coverage/test/test_path.cpp b/opennav_coverage/test/test_path.cpp
index e0b7563..81e5a12 100644
--- a/opennav_coverage/test/test_path.cpp
+++ b/opennav_coverage/test/test_path.cpp
@@ -114,9 +114,11 @@ TEST(PathTests, TestpathGeneration)
 
   // Generate some toy route
   f2c::Random rand;
-  auto field = rand.generateRandField(5, 1e5);
+  double area = 1e5;
+  int sides = 5;
+  auto field = rand.generateRandField(area, sides);
   opennav_coverage_msgs::msg::SwathMode sw_settings;
-  auto swaths = swath_gen.generateSwaths(field.field.getGeometry(0), sw_settings);
+  auto swaths = swath_gen.generateSwaths(field.getField().getGeometry(0), sw_settings);
   opennav_coverage_msgs::msg::RouteMode rt_settings;
   auto route = route_gen.generateRoute(swaths, rt_settings);
 
diff --git a/opennav_coverage/test/test_robot.cpp b/opennav_coverage/test/test_robot.cpp
index 227f600..6905e21 100644
--- a/opennav_coverage/test/test_robot.cpp
+++ b/opennav_coverage/test/test_robot.cpp
@@ -37,7 +37,7 @@ TEST(RobotTests, Testrobot)
 
   EXPECT_EQ(robot.getWidth(), 2.1);
   EXPECT_EQ(robot.getOperationWidth(), 2.5);
-  EXPECT_EQ(robot.getRobot().linear_curv_change, 2.0);
+  EXPECT_EQ(robot.getRobot().getMaxDiffCurv(), 2.0);
 }
 
 }  // namespace opennav_coverage
diff --git a/opennav_coverage/test/test_route.cpp b/opennav_coverage/test/test_route.cpp
index ac11d3e..71b9894 100644
--- a/opennav_coverage/test/test_route.cpp
+++ b/opennav_coverage/test/test_route.cpp
@@ -99,8 +99,10 @@ TEST(RouteTests, TestrouteGeneration)
 
   // Generate some toy field
   f2c::Random rand;
-  auto field = rand.generateRandField(5, 1e5);
-  auto swaths = swath_gen.generateSwaths(field.field.getGeometry(0), sw_settings);
+  double area = 1e5;
+  int sides = 5;
+  auto field = rand.generateRandField(area, sides);
+  auto swaths = swath_gen.generateSwaths(field.getField().getGeometry(0), sw_settings);
 
   // Shouldn't throw, results in valid output
   opennav_coverage_msgs::msg::RouteMode settings;
diff --git a/opennav_coverage/test/test_swath.cpp b/opennav_coverage/test/test_swath.cpp
index 997cec1..2eab672 100644
--- a/opennav_coverage/test/test_swath.cpp
+++ b/opennav_coverage/test/test_swath.cpp
@@ -108,17 +108,19 @@ TEST(SwathTests, TestswathGeneration)
 
   // Generate some toy field
   f2c::Random rand;
-  auto field = rand.generateRandField(5, 1e5);
+  double area = 1e5;
+  int sides = 5;
+  auto field = rand.generateRandField(area, sides);
 
   // Shouldn't throw, results in valid output
   opennav_coverage_msgs::msg::SwathMode settings;
-  auto swaths1 = generator.generateSwaths(field.field.getGeometry(0), settings);
+  auto swaths1 = generator.generateSwaths(field.getField().getGeometry(0), settings);
   settings.mode = "BRUTE_FORCE";
   settings.objective = "LENGTH";
-  auto swaths2 = generator.generateSwaths(field.field.getGeometry(0), settings);
+  auto swaths2 = generator.generateSwaths(field.getField().getGeometry(0), settings);
   settings.mode = "SET_ANGLE";
   settings.objective = "NUMBER";
-  auto swaths3 = generator.generateSwaths(field.field.getGeometry(0), settings);
+  auto swaths3 = generator.generateSwaths(field.getField().getGeometry(0), settings);
 }
 
 }  // namespace opennav_coverage
diff --git a/opennav_coverage/test/test_utils.cpp b/opennav_coverage/test/test_utils.cpp
index fd8b63f..80a1943 100644
--- a/opennav_coverage/test/test_utils.cpp
+++ b/opennav_coverage/test/test_utils.cpp
@@ -105,12 +105,15 @@ TEST(UtilsTests, TesttoNavPathMsg)
   std_msgs::msg::Header header_in;
   header_in.frame_id = "test";
   Path path_in;
-  path_in.states.resize(10);
+  path_in.getStates().resize(10);
+  EXPECT_EQ(path_in.size(), 10u);
   F2CField field;
 
   auto msg = util::toNavPathMsg(path_in, field, header_in, true, 0.1);
   EXPECT_EQ(msg.header.frame_id, "test");
-  EXPECT_EQ(msg.poses.size(), 10u);
+
+  // Path goes nowhere so compressed into a dense path
+  EXPECT_EQ(msg.poses.size(), 1u);
 }
 
 TEST(UtilsTests, TesttoCoveragePathMsg2)
@@ -126,38 +129,39 @@ TEST(UtilsTests, TesttoCoveragePathMsg2)
   EXPECT_EQ(msg.contains_turns, true);
   EXPECT_EQ(msg.swaths_ordered, true);
 
-  // states are not valid (e.g. non-marked as turn or swath)
-  path_in.states.resize(10);
-  EXPECT_THROW(util::toCoveragePathMsg(path_in, field, header_in, true), std::runtime_error);
+  // states are initalised as a valid swath
+  path_in.getStates().resize(10);
+  msg = util::toCoveragePathMsg(path_in, field, header_in, true);
+  util::toCoveragePathMsg(path_in, field, header_in, true);
 
   // Now lets make it valid
   using f2c::types::PathSectionType;
-  path_in.states[0].type = PathSectionType::SWATH;
-  path_in.states[1].type = PathSectionType::SWATH;
-  path_in.states[2].type = PathSectionType::TURN;
-  path_in.states[3].type = PathSectionType::TURN;
-  path_in.states[4].type = PathSectionType::SWATH;
-  path_in.states[5].type = PathSectionType::SWATH;
-  path_in.states[6].type = PathSectionType::TURN;
-  path_in.states[7].type = PathSectionType::TURN;
-  path_in.states[8].type = PathSectionType::SWATH;
-  path_in.states[9].type = PathSectionType::SWATH;
+  path_in.getState(0).type = PathSectionType::SWATH;
+  path_in.getState(1).type = PathSectionType::SWATH;
+  path_in.getState(2).type = PathSectionType::TURN;
+  path_in.getState(3).type = PathSectionType::TURN;
+  path_in.getState(4).type = PathSectionType::SWATH;
+  path_in.getState(5).type = PathSectionType::SWATH;
+  path_in.getState(6).type = PathSectionType::TURN;
+  path_in.getState(7).type = PathSectionType::TURN;
+  path_in.getState(8).type = PathSectionType::SWATH;
+  path_in.getState(9).type = PathSectionType::SWATH;
 
   msg = util::toCoveragePathMsg(path_in, field, header_in, true);
   EXPECT_EQ(msg.swaths.size(), 3u);
   EXPECT_EQ(msg.turns.size(), 2u);
   EXPECT_EQ(msg.turns[0].poses.size(), 2u);
 
-  path_in.states[0].type = PathSectionType::TURN;
-  path_in.states[1].type = PathSectionType::TURN;
-  path_in.states[2].type = PathSectionType::SWATH;
-  path_in.states[3].type = PathSectionType::SWATH;
-  path_in.states[4].type = PathSectionType::TURN;
-  path_in.states[5].type = PathSectionType::TURN;
-  path_in.states[6].type = PathSectionType::SWATH;
-  path_in.states[7].type = PathSectionType::SWATH;
-  path_in.states[8].type = PathSectionType::TURN;
-  path_in.states[9].type = PathSectionType::TURN;
+  path_in.getState(0).type = PathSectionType::TURN;
+  path_in.getState(1).type = PathSectionType::TURN;
+  path_in.getState(2).type = PathSectionType::SWATH;
+  path_in.getState(3).type = PathSectionType::SWATH;
+  path_in.getState(4).type = PathSectionType::TURN;
+  path_in.getState(5).type = PathSectionType::TURN;
+  path_in.getState(6).type = PathSectionType::SWATH;
+  path_in.getState(7).type = PathSectionType::SWATH;
+  path_in.getState(8).type = PathSectionType::TURN;
+  path_in.getState(9).type = PathSectionType::TURN;
 
   msg = util::toCoveragePathMsg(path_in, field, header_in, true);
   EXPECT_EQ(msg.swaths.size(), 2u);
@@ -181,7 +185,7 @@ TEST(UtilsTests, TestgetFieldFromGoal)
   // Should work now, with a trivial polygon of 3 nodes of (0, 0)
   goal->polygons[0].coordinates[2].axis1 = 0.0;
   auto field = util::getFieldFromGoal(goal);
-  EXPECT_EQ(field.field.getGeometry(0).getGeometry(0).size(), 3u);
+  EXPECT_EQ(field.getField().getGeometry(0).getGeometry(0).size(), 3u);
 
   // Test with inner polygons, first invalid
   goal->polygons.resize(2);
@@ -193,7 +197,7 @@ TEST(UtilsTests, TestgetFieldFromGoal)
   goal->polygons[0].coordinates[0].axis1 = 0.0;
   goal->polygons[1].coordinates[0].axis1 = 1.0;
   auto field2 = util::getFieldFromGoal(goal);
-  EXPECT_EQ(field2.field.getGeometry(0).getGeometry(1).size(), 3u);
+  EXPECT_EQ(field2.getField().getGeometry(0).getGeometry(1).size(), 3u);
 }
 
 TEST(UtilsTests, TestPathComponentsIterator)
diff --git a/opennav_coverage_bt/CMakeLists.txt b/opennav_coverage_bt/CMakeLists.txt
index 7844ee6..3d3aade 100644
--- a/opennav_coverage_bt/CMakeLists.txt
+++ b/opennav_coverage_bt/CMakeLists.txt
@@ -12,7 +12,7 @@ find_package(nav2_behavior_tree REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(opennav_coverage_msgs REQUIRED)
-find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
 
 # potentially replace with nav2_common, nav2_package()
 set(CMAKE_CXX_STANDARD 17)
@@ -33,7 +33,7 @@ set(dependencies
   nav_msgs
   geometry_msgs
   opennav_coverage_msgs
-  behaviortree_cpp_v3
+  behaviortree_cpp
 )
 
 add_library(opennav_compute_complete_coverage_action_bt_node SHARED src/compute_complete_coverage_path.cpp)
diff --git a/opennav_coverage_bt/behavior_trees/navigate_w_basic_row_complete_coverage.xml b/opennav_coverage_bt/behavior_trees/navigate_w_basic_row_complete_coverage.xml
index 7a2fc52..7743d92 100644
--- a/opennav_coverage_bt/behavior_trees/navigate_w_basic_row_complete_coverage.xml
+++ b/opennav_coverage_bt/behavior_trees/navigate_w_basic_row_complete_coverage.xml
@@ -11,7 +11,7 @@
   This BT shows field file usage with the row coverage server
 -->
 
-<root main_tree_to_execute="MainTree">
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
     <RateController hz="0.0000001"> <!-- once, for demo -->
       <Sequence name="NavigateWithoutReplanning">
diff --git a/opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp b/opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp
index dee58a2..1f9eabd 100644
--- a/opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp
+++ b/opennav_coverage_bt/include/opennav_coverage_bt/utils.hpp
@@ -16,7 +16,7 @@
 #define OPENNAV_COVERAGE_BT__UTILS_HPP_
 
 #include <charconv>
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 
 namespace BT
 {
diff --git a/opennav_coverage_bt/package.xml b/opennav_coverage_bt/package.xml
index e8c96db..f207c3b 100644
--- a/opennav_coverage_bt/package.xml
+++ b/opennav_coverage_bt/package.xml
@@ -18,7 +18,7 @@
   <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>opennav_coverage_msgs</depend>
-  <depend>behaviortree_cpp_v3</depend>
+  <depend>behaviortree_cpp</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/opennav_coverage_bt/src/cancel_complete_coverage_path.cpp b/opennav_coverage_bt/src/cancel_complete_coverage_path.cpp
index f0658ca..9eee337 100644
--- a/opennav_coverage_bt/src/cancel_complete_coverage_path.cpp
+++ b/opennav_coverage_bt/src/cancel_complete_coverage_path.cpp
@@ -31,7 +31,7 @@ CoverageCancel::CoverageCancel(
 
 }  // namespace opennav_coverage_bt
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   BT::NodeBuilder builder =
diff --git a/opennav_coverage_bt/src/compute_complete_coverage_path.cpp b/opennav_coverage_bt/src/compute_complete_coverage_path.cpp
index 307aca1..6f988e2 100644
--- a/opennav_coverage_bt/src/compute_complete_coverage_path.cpp
+++ b/opennav_coverage_bt/src/compute_complete_coverage_path.cpp
@@ -46,6 +46,7 @@ void ComputeCoveragePathAction::on_tick()
     // Convert from vector of Polygons to coverage sp. message
     std::vector<geometry_msgs::msg::Polygon> polys;
     getInput("polygons", polys);
+    goal_.polygons.clear();
     goal_.polygons.resize(polys.size());
     for (unsigned int i = 0; i != polys.size(); i++) {
       for (unsigned int j = 0; j != polys[i].points.size(); j++) {
@@ -98,7 +99,7 @@ void ComputeCoveragePathAction::halt()
 
 }  // namespace opennav_coverage_bt
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   BT::NodeBuilder builder =
diff --git a/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp
index 8ffb87c..d6fe48d 100644
--- a/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp
+++ b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp
@@ -17,7 +17,7 @@
 #include <set>
 #include <string>
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 #include "nav2_behavior_tree/utils/test_action_server.hpp"
 #include "opennav_coverage_bt/cancel_complete_coverage_path.hpp"
@@ -125,7 +125,7 @@ TEST_F(CancelCoverageActionTestFixture, test_ports)
 {
   std::string xml_txt =
     R"(
-      <root main_tree_to_execute = "MainTree" >
+      <root BTCPP_format="4" main_tree_to_execute="MainTree" >
         <BehaviorTree ID="MainTree">
              <CancelCoverage name="CoverageCancel"/>
         </BehaviorTree>
diff --git a/opennav_coverage_bt/test/test_compute_coverage_path.cpp b/opennav_coverage_bt/test/test_compute_coverage_path.cpp
index 09ba0b8..ed79d4c 100644
--- a/opennav_coverage_bt/test/test_compute_coverage_path.cpp
+++ b/opennav_coverage_bt/test/test_compute_coverage_path.cpp
@@ -20,7 +20,7 @@
 #include "nav_msgs/msg/path.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 #include "nav2_behavior_tree/utils/test_action_server.hpp"
 #include "opennav_coverage_bt/compute_complete_coverage_path.hpp"
@@ -123,7 +123,7 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick)
   // create tree
   std::string xml_txt =
     R"(
-      <root main_tree_to_execute = "MainTree" >
+      <root BTCPP_format="4" main_tree_to_execute="MainTree" >
         <BehaviorTree ID="MainTree">
             <ComputeCoveragePath nav_path="{path}"/>
         </BehaviorTree>
@@ -141,13 +141,13 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_tick)
 
   // check if returned path is correct
   nav_msgs::msg::Path path;
-  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  [[maybe_unused]] auto res = config_->blackboard->get("path", path);
   EXPECT_EQ(path.poses.size(), 2u);
   EXPECT_EQ(path.poses[0].pose.position.x, 0.0);
   EXPECT_EQ(path.poses[1].pose.position.x, 1.0);
 
   // halt node so another goal can be sent
-  tree_->rootNode()->halt();
+  tree_->haltTree();
   EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
 }
 
diff --git a/opennav_coverage_demo/launch/coverage_demo_launch.py b/opennav_coverage_demo/launch/coverage_demo_launch.py
index 1214c19..1db1a33 100644
--- a/opennav_coverage_demo/launch/coverage_demo_launch.py
+++ b/opennav_coverage_demo/launch/coverage_demo_launch.py
@@ -63,10 +63,12 @@ def generate_launch_description():
             '-R', '0.0', '-P', '0.0', '-Y', '0.0'])
 
     # start the visualization
+    rviz_config = os.path.join(coverage_demo_dir, 'opennav_coverage_demo.rviz')
+    # print("rviz_config:", rviz_config)
     rviz_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
-        launch_arguments={'namespace': ''}.items())
+        launch_arguments={'namespace': '', 'rviz_config': rviz_config}.items())
 
     # start navigation
     bringup_cmd = IncludeLaunchDescription(
diff --git a/opennav_coverage_demo/launch/row_coverage_demo_launch.py b/opennav_coverage_demo/launch/row_coverage_demo_launch.py
index 2161b78..a9ee6b3 100644
--- a/opennav_coverage_demo/launch/row_coverage_demo_launch.py
+++ b/opennav_coverage_demo/launch/row_coverage_demo_launch.py
@@ -63,10 +63,11 @@ def generate_launch_description():
             '-R', '0.0', '-P', '0.0', '-Y', '-1.5708'])
 
     # start the visualization
+    rviz_config = os.path.join(coverage_demo_dir, 'opennav_coverage_demo.rviz')
     rviz_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
-        launch_arguments={'namespace': ''}.items())
+        launch_arguments={'namespace': '', 'rviz_config': rviz_config}.items())
 
     # start navigation
     bringup_cmd = IncludeLaunchDescription(
diff --git a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py b/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py
index 5435451..fe4d020 100644
--- a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py
+++ b/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py
@@ -150,26 +150,33 @@ def main():
     navigator.navigateCoverage(field)
 
     i = 0
-    while not navigator.isTaskComplete():
-        # Do something with the feedback
-        i = i + 1
-        feedback = navigator.getFeedback()
-        if feedback and i % 5 == 0:
-            print('Estimated time of arrival: ' + '{0:.0f}'.format(
-                  Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
-                  + ' seconds.')
-        time.sleep(1)
-
-    # Do something depending on the return code
-    result = navigator.getResult()
-    if result == TaskResult.SUCCEEDED:
-        print('Goal succeeded!')
-    elif result == TaskResult.CANCELED:
+    try:
+        while not navigator.isTaskComplete():
+            # Do something with the feedback
+            i = i + 1
+            feedback = navigator.getFeedback()
+            if feedback and i % 5 == 0:
+                print('Estimated time of arrival: ' + '{0:.0f}'.format(
+                    Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
+                    + ' seconds.')
+            time.sleep(1)
+
+        # Do something depending on the return code
+        result = navigator.getResult()
+        if result == TaskResult.SUCCEEDED:
+            print('Goal succeeded!')
+        elif result == TaskResult.CANCELED:
+            print('Goal was canceled!')
+        elif result == TaskResult.FAILED:
+            print('Goal failed!')
+        else:
+            print('Goal has an invalid return status!')
+
+    except KeyboardInterrupt:
+        print('\nCtrl-C detected. Cancelling goal')
+        cancel_future = navigator.goal_handle.cancel_goal_async()
+        rclpy.spin_until_future_complete(navigator, cancel_future)
         print('Goal was canceled!')
-    elif result == TaskResult.FAILED:
-        print('Goal failed!')
-    else:
-        print('Goal has an invalid return status!')
 
 
 if __name__ == '__main__':
diff --git a/opennav_coverage_demo/rviz/opennav_coverage_demo.rviz b/opennav_coverage_demo/rviz/opennav_coverage_demo.rviz
new file mode 100644
index 0000000..e35f648
--- /dev/null
+++ b/opennav_coverage_demo/rviz/opennav_coverage_demo.rviz
@@ -0,0 +1,703 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /TF1/Frames1
+        - /TF1/Tree1
+        - /Controller1
+        - /Polygon1
+        - /Polygon2
+        - /Path1
+        - /Path2
+      Splitter Ratio: 0.5833333134651184
+    Tree Height: 575
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: nav2_rviz_plugins/Navigation 2
+    Name: Navigation 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: false
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: false
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        EPSG:4258:
+          Value: true
+        base_footprint:
+          Value: true
+        base_link:
+          Value: true
+        base_scan:
+          Value: true
+        camera_depth_frame:
+          Value: true
+        camera_depth_optical_frame:
+          Value: true
+        camera_link:
+          Value: true
+        camera_rgb_frame:
+          Value: true
+        camera_rgb_optical_frame:
+          Value: true
+        caster_back_left_link:
+          Value: true
+        caster_back_right_link:
+          Value: true
+        imu_link:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+        wheel_left_link:
+          Value: true
+        wheel_right_link:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        EPSG:4258:
+          map:
+            odom:
+              base_footprint:
+                base_link:
+                  base_scan:
+                    {}
+                  camera_link:
+                    camera_depth_frame:
+                      camera_depth_optical_frame:
+                        {}
+                    camera_rgb_frame:
+                      camera_rgb_optical_frame:
+                        {}
+                  caster_back_left_link:
+                    {}
+                  caster_back_right_link:
+                    {}
+                  imu_link:
+                    {}
+                  wheel_left_link:
+                    {}
+                  wheel_right_link:
+                    {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: -999999
+      Min Color: 0; 0; 0
+      Min Intensity: 999999
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Bumper Hit
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.07999999821186066
+      Style: Spheres
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /mobile_base/sensors/bumper_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 1
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /map
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /map_updates
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: nav2_rviz_plugins/ParticleCloud
+      Color: 0; 180; 0
+      Enabled: true
+      Max Arrow Length: 0.30000001192092896
+      Min Arrow Length: 0.019999999552965164
+      Name: Amcl Particle Swarm
+      Shape: Arrow (Flat)
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /particle_cloud
+      Value: true
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 0.30000001192092896
+          Class: rviz_default_plugins/Map
+          Color Scheme: costmap
+          Draw Behind: false
+          Enabled: true
+          Name: Global Costmap
+          Topic:
+            Depth: 1
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /global_costmap/costmap
+          Update Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /global_costmap/costmap_updates
+          Use Timestamp: false
+          Value: true
+        - Alpha: 0.30000001192092896
+          Class: rviz_default_plugins/Map
+          Color Scheme: costmap
+          Draw Behind: false
+          Enabled: true
+          Name: Downsampled Costmap
+          Topic:
+            Depth: 1
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /downsampled_costmap
+          Update Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /downsampled_costmap_updates
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz_default_plugins/Path
+          Color: 255; 0; 0
+          Enabled: true
+          Head Diameter: 0.019999999552965164
+          Head Length: 0.019999999552965164
+          Length: 0.30000001192092896
+          Line Style: Lines
+          Line Width: 0.029999999329447746
+          Name: Path
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: Arrows
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.004999999888241291
+          Shaft Length: 0.019999999552965164
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /plan
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 125; 125; 125
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: VoxelGrid
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.05000000074505806
+          Style: Boxes
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /global_costmap/voxel_marked_cloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Class: rviz_default_plugins/Polygon
+          Color: 25; 255; 0
+          Enabled: false
+          Name: Polygon
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /global_costmap/published_footprint
+          Value: false
+      Enabled: true
+      Name: Global Planner
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 0.699999988079071
+          Class: rviz_default_plugins/Map
+          Color Scheme: costmap
+          Draw Behind: false
+          Enabled: true
+          Name: Local Costmap
+          Topic:
+            Depth: 1
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /local_costmap/costmap
+          Update Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /local_costmap/costmap_updates
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz_default_plugins/Path
+          Color: 0; 12; 255
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.30000001192092896
+          Line Style: Lines
+          Line Width: 0.029999999329447746
+          Name: Local Plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /local_plan
+          Value: true
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: Trajectories
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /marker
+          Value: false
+        - Alpha: 1
+          Class: rviz_default_plugins/Polygon
+          Color: 25; 255; 0
+          Enabled: true
+          Name: Polygon
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /local_costmap/published_footprint
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: VoxelGrid
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /local_costmap/voxel_marked_cloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: Controller
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_default_plugins/Image
+          Enabled: true
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: RealsenseCamera
+          Normalize Range: true
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /intel_realsense_r200_depth/image_raw
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: RealsenseDepthImage
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /intel_realsense_r200_depth/points
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: false
+      Name: Realsense
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep All
+        Reliability Policy: Reliable
+        Value: /waypoints
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Polygon
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /coverage_server/field_boundary
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/Polygon
+      Color: 255; 85; 0
+      Enabled: true
+      Name: Polygon
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /coverage_server/planning_field
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 255; 85; 255
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.07999999821186066
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /coverage_server/coverage_plan
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 255; 170; 127
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Billboards
+      Line Width: 0.20000000298023224
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /received_global_plan
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+    - Class: nav2_rviz_plugins/GoalTool
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Angle: -0.004999999888241291
+      Class: rviz_default_plugins/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 47.66288757324219
+      Target Frame: <Fixed Frame>
+      Value: TopDownOrtho (rviz_default_plugins)
+      X: 11.359171867370605
+      Y: 9.817364692687988
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1072
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Navigation 2:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001c8000003dafc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000027a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002bb0000015a0000012300fffffffb0000001e005200650061006c00730065006e0073006500430061006d006500720061000000037e0000009f0000002800ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003da000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000495000003da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RealsenseCamera:
+    collapsed: false
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1912
+  X: 1082
+  Y: 2
diff --git a/opennav_coverage_demo/setup.py b/opennav_coverage_demo/setup.py
index b13c308..76d11f8 100644
--- a/opennav_coverage_demo/setup.py
+++ b/opennav_coverage_demo/setup.py
@@ -17,6 +17,7 @@
         (os.path.join('share', package_name), glob('launch/*')),
         (os.path.join('share', package_name), glob('world/*')),
         (os.path.join('share', package_name), glob('params/*')),
+        (os.path.join('share', package_name), glob('rviz/*')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
diff --git a/opennav_coverage_navigator/src/coverage_navigator.cpp b/opennav_coverage_navigator/src/coverage_navigator.cpp
index d4e46a7..952dd93 100644
--- a/opennav_coverage_navigator/src/coverage_navigator.cpp
+++ b/opennav_coverage_navigator/src/coverage_navigator.cpp
@@ -122,7 +122,7 @@ CoverageNavigator::onLoop()
   try {
     // Get current path points
     nav_msgs::msg::Path current_path;
-    blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path);
+    [[maybe_unused]] auto res = blackboard->get(path_blackboard_id_, current_path);
 
     // Find the closest pose to current pose on global path
     auto find_closest_pose_idx =
@@ -165,7 +165,7 @@ CoverageNavigator::onLoop()
   }
 
   int recovery_count = 0;
-  blackboard->get<int>("number_recoveries", recovery_count);
+  [[maybe_unused]] auto result = blackboard->get("number_recoveries", recovery_count);
   feedback_msg->number_of_recoveries = recovery_count;
   feedback_msg->current_pose = current_pose;
   feedback_msg->navigation_time = clock_->now() - start_time_;
diff --git a/opennav_row_coverage/CMakeLists.txt b/opennav_row_coverage/CMakeLists.txt
index 2125c62..bf313e9 100644
--- a/opennav_row_coverage/CMakeLists.txt
+++ b/opennav_row_coverage/CMakeLists.txt
@@ -9,7 +9,8 @@ find_package(nav2_util REQUIRED)
 find_package(Fields2Cover REQUIRED)
 find_package(opennav_coverage REQUIRED)
 find_package(opennav_coverage_msgs REQUIRED)
-find_package(tinyxml2 REQUIRED)
+find_package(tinyxml2_vendor REQUIRED)
+find_package(TinyXML2 REQUIRED)
 
 # potentially replace with nav2_common, nav2_package()
 set(CMAKE_CXX_STANDARD 17)
@@ -35,6 +36,7 @@ set(dependencies
   std_msgs
   opennav_coverage
   opennav_coverage_msgs
+  tinyxml2_vendor
 )
 
 add_library(${library_name} SHARED
@@ -46,7 +48,11 @@ ament_target_dependencies(${library_name}
   ${dependencies}
 )
 
-target_link_libraries(${library_name} Fields2Cover tinyxml2::tinyxml2)
+#Fields2Cover not packaged with ament so have to do it manually
+#and grab the vendored TINYXML2_LIBRARY too
+target_link_libraries(${library_name}
+    Fields2Cover
+    tinyxml2::tinyxml2)
 
 add_executable(${executable_name}
   src/main.cpp
@@ -58,8 +64,6 @@ ament_target_dependencies(${executable_name}
   ${dependencies}
 )
 
-target_link_libraries(${executable_name} Fields2Cover tinyxml2::tinyxml2)
-
 rclcpp_components_register_nodes(${library_name} "opennav_row_coverage::RowCoverageServer")
 
 install(TARGETS ${library_name}
diff --git a/opennav_row_coverage/package.xml b/opennav_row_coverage/package.xml
index 9538a82..d30d9ba 100644
--- a/opennav_row_coverage/package.xml
+++ b/opennav_row_coverage/package.xml
@@ -14,7 +14,7 @@
   <depend>fields2cover</depend>
   <depend>opennav_coverage</depend>
   <depend>opennav_coverage_msgs</depend>
-  <depend>tinyxml2</depend>
+  <depend>tinyxml2_vendor</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/opennav_row_coverage/src/row_coverage_server.cpp b/opennav_row_coverage/src/row_coverage_server.cpp
index 9533013..dc6839e 100644
--- a/opennav_row_coverage/src/row_coverage_server.cpp
+++ b/opennav_row_coverage/src/row_coverage_server.cpp
@@ -180,7 +180,7 @@ void RowCoverageServer::computeCoveragePath()
     // (0) Obtain field and rows to use
     F2CField master_field = f2c::Parser::importFieldGml(goal->gml_field, true);
     Rows rows = util::parseRows(goal->gml_field, order_ids_);
-    std::string frame_id = master_field.coord_sys;
+    std::string frame_id = master_field.getCRS();
 
     if (!cartesian_frame_) {
       f2c::Transform::transformToUTM(master_field);
@@ -188,7 +188,7 @@ void RowCoverageServer::computeCoveragePath()
     } else {
       rows = util::removeRowsRefPoint(rows, master_field);
     }
-    Field field = master_field.field.getGeometry(0);
+    Field field = master_field.getField().getGeometry(0);
 
     RCLCPP_INFO(
       get_logger(),
@@ -263,10 +263,10 @@ RowCoverageServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
         path_gen_->setTurnPointDistance(parameter.as_double());
       } else if (name == "robot_width") {
         auto & robot = robot_params_->getRobot();
-        robot.robot_width = parameter.as_double();
+        robot.setWidth(parameter.as_double());
       } else if (name == "operation_width") {
         auto & robot = robot_params_->getRobot();
-        robot.op_width = parameter.as_double();
+        robot.setCovWidth(parameter.as_double());
       } else if (name == "default_offset") {
         swath_gen_->setOffset(parameter.as_double());
       }
diff --git a/opennav_row_coverage/src/row_swath_generator.cpp b/opennav_row_coverage/src/row_swath_generator.cpp
index c3af7e5..4ec50c3 100644
--- a/opennav_row_coverage/src/row_swath_generator.cpp
+++ b/opennav_row_coverage/src/row_swath_generator.cpp
@@ -176,7 +176,7 @@ Rows RowSwathGenerator::adjustRowOrientations(const Rows & input_rows)
   Rows rows = input_rows;
 
   auto unitVectorize = [&](const Row & row) {
-      Point v = row.first.EndPoint() - row.first.StartPoint();
+      Point v = row.first.endPoint() - row.first.startPoint();
       double normalize = 1.0 / std::sqrt(
         v.getX() * v.getX() + v.getY() * v.getY());
       return v * normalize;