Using noether to generate full surface paths

[rr-tom-noble]

Hi,

I'm looking into generating paths over the face of an entire surface of a mesh, and it seems like noether might be a perfect fit for doing that. I think the process would look something like:

1. Subset of the mesh is selected (would it be possible to pass a mesh to noether via a ROS topic rather than through a file?)
2. Selected sub-mesh is converted to a BSpline representation (already possible in noether I believe? i.e. mesh filtering demo. Unsure if this is a non-tessellated representation, or a modified mesh though)
3. Given the size of a tool, a path is planned over the surface (shape doesn't matter too much as long as it covers the whole area)

I've taken a look at the half edge demo which seems promising. Is it possible to generate paths over a surface rather than just around the edge? If so, is there a demo showcasing that? I'm rather new to the world of tool path planning, so please forgive any silly questions 😄

[rr-tom-noble]

It seems like the plane slice rastering demo might be what I'm looking for, but I'm unsure how to view the output in rviz

[rr-tom-noble]

Upon closer inspection, I think it may be the surface walk path planner that I want. I've written some code to run a demo based on the plane slice demo's files, but I'm getting an error. I'm not entirely sure what all of the parameters do, so I'd much appreciate any help!

#include <ros/ros.h>
#include <XmlRpcException.h>
#include <boost/format.hpp>
#include <boost/filesystem.hpp>
#include <noether_conversions/noether_conversions.h>
#include <tool_path_planner/surface_walk_raster_generator.h>
#include <tool_path_planner/utilities.h>
#include <visualization_msgs/MarkerArray.h>
#include <actionlib/client/simple_action_client.h>
#include <console_bridge/console.h>
#include <noether_msgs/GenerateToolPathsAction.h>

using RGBA = std::tuple<double, double, double, double>;

static const double GOAL_WAIT_PERIOD = 300.0;  // sec
static const std::string DEFAULT_FRAME_ID = "world";
static const std::string GENERATE_TOOL_PATHS_ACTION = "generate_tool_paths";
static const std::string RASTER_LINES_MARKERS_TOPIC = "raster_lines";
static const std::string RASTER_POSES_MARKERS_TOPIC = "raster_poses";
static const std::string RASTER_PATH_NS = "raster_";
static const std::string INPUT_MESH_NS = "input_mesh";
static const RGBA RAW_MESH_RGBA = std::make_tuple(0.6, 0.6, 1.0, 1.0);

std::size_t countPathPoints(const noether_msgs::ToolPaths& tool_paths)
{
    std::size_t point_count = 0;

    for (const auto& path : tool_paths.paths)
        for (const auto& segment : path.segments)
            point_count += segment.poses.size();

    return point_count;
}

class SurfaceWalkExample
{
public:
    SurfaceWalkExample(ros::NodeHandle nh) : nh_(nh), ac_(GENERATE_TOOL_PATHS_ACTION)
    {
        raster_lines_markers_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(RASTER_LINES_MARKERS_TOPIC, 1);
        raster_poses_markers_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(RASTER_POSES_MARKERS_TOPIC, 1);
    }

    ~SurfaceWalkExample() {}

    bool loadConfig(tool_path_planner::SurfaceWalkRasterGenerator::Config& config)
    {
        ros::NodeHandle ph("~");
        XmlRpc::XmlRpcValue cfg;
        if (!ph.getParam("surface_walk_rastering", cfg))
        {
            return false;
        }

        std::vector<std::string> field_names = { "raster_spacing", "point_spacing", "raster_rot_offset",
                                                 "min_hole_size",  "min_segment_size", "intersection_plane_height",
                                                 "tool_offset", "generate_extra_rasters", "cut_direction_x",
                                                 "cut_direction_y", "cut_direction_z", "debug"};
        if (!std::all_of(field_names.begin(), field_names.end(), [&cfg](const std::string& f) { return cfg.hasMember(f); }))
        {
            ROS_ERROR("Failed to find one or more members");
        }

        std::size_t idx = 0;
        try
        {
            config.raster_spacing = static_cast<double>(cfg[field_names[idx++]]);
            config.point_spacing = static_cast<double>(cfg[field_names[idx++]]);
            config.raster_rot_offset = static_cast<double>(cfg[field_names[idx++]]);
            config.min_hole_size = static_cast<double>(cfg[field_names[idx++]]);
            config.min_segment_size = static_cast<double>(cfg[field_names[idx++]]);
            config.intersection_plane_height = static_cast<double>(cfg[field_names[idx++]]);
            config.tool_offset = static_cast<double>(cfg[field_names[idx++]]);
            config.generate_extra_rasters = static_cast<bool>(cfg[field_names[idx++]]);
            for(int i=0; i < 3; i++)
            {
                config.cut_direction[i] = static_cast<double>(cfg[field_names[idx++]]);
            }
            config.debug = static_cast<bool>(cfg[field_names[idx++]]);
        }
        catch (XmlRpc::XmlRpcException& e)
        {
            ROS_ERROR_STREAM(e.getMessage());
        }
        return true;
    }

    bool run()
    {
        using namespace noether_conversions;

        // loading parameters
        ros::NodeHandle ph("~");
        std::string mesh_file;
        if (!ph.getParam("mesh_file", mesh_file))
        {
            ROS_ERROR("Failed to load one or more parameters");
            return false;
        }

        ROS_INFO("Got mesh file %s", mesh_file.c_str());

        // get configuration
        tool_path_planner::SurfaceWalkRasterGenerator::Config config;
        if (!loadConfig(config))
        {
            ROS_WARN("Failed to load configuration, using default parameters");
        }

        markers_publish_timer_ = nh_.createTimer(ros::Duration(0.5), [&](const ros::TimerEvent& e) {
            if (!line_markers_.markers.empty())
            {
                raster_lines_markers_pub_.publish(line_markers_);
            }

            if (!poses_markers_.markers.empty())
            {
                raster_poses_markers_pub_.publish(poses_markers_);
            }
        });

        shape_msgs::Mesh mesh_msg;
        if (!noether_conversions::loadPLYFile(mesh_file, mesh_msg))
        {
            ROS_ERROR("Failed to read file %s", mesh_file.c_str());
            return false;
        }
        line_markers_.markers.push_back(createMeshMarker(mesh_file, INPUT_MESH_NS, DEFAULT_FRAME_ID, RAW_MESH_RGBA));

        // waiting for server
        if (!ac_.waitForServer(ros::Duration(5.0)))
        {
            ROS_ERROR("Action server %s was not found", GENERATE_TOOL_PATHS_ACTION.c_str());
            return false;
        }

        // sending request
        noether_msgs::GenerateToolPathsGoal goal;
        noether_msgs::ToolPathConfig config_msg;
        config_msg.type = noether_msgs::ToolPathConfig::SURFACE_WALK_RASTER_GENERATOR;
        tool_path_planner::toSurfaceWalkConfigMsg(config_msg.surface_walk_generator, config);

        goal.path_configs.push_back(config_msg);
        goal.surface_meshes.push_back(mesh_msg);
        goal.proceed_on_failure = true;
        ac_.sendGoal(goal);
        ros::Time start_time = ros::Time::now();
        if (!ac_.waitForResult(ros::Duration(GOAL_WAIT_PERIOD)))
        {
            ROS_ERROR("Failed to generate edges from mesh");
            return false;
        }

        noether_msgs::GenerateToolPathsResultConstPtr res = ac_.getResult();
        if (!res->success)
        {
            ROS_ERROR("Failed to generate edges from mesh");
            return false;
        }

        const std::vector<noether_msgs::ToolPaths>& raster_paths = res->tool_paths;

        ROS_INFO("Found %lu rasters", raster_paths.size());

        for (std::size_t i = 0; i < raster_paths.size(); i++)
        {
            ROS_INFO("Raster %lu contains %lu points", i, countPathPoints(raster_paths[i]));

            std::string ns = RASTER_PATH_NS + std::to_string(i);
            visualization_msgs::MarkerArray edge_path_axis_markers =
                    convertToAxisMarkers(raster_paths[i], DEFAULT_FRAME_ID, ns);

            visualization_msgs::MarkerArray edge_path_line_markers =
                    convertToArrowMarkers(raster_paths[i], DEFAULT_FRAME_ID, ns);

            poses_markers_.markers.insert(
                    poses_markers_.markers.end(), edge_path_axis_markers.markers.begin(), edge_path_axis_markers.markers.end());
            line_markers_.markers.insert(
                    line_markers_.markers.end(), edge_path_line_markers.markers.begin(), edge_path_line_markers.markers.end());
        }
        return true;
    }

private:
    ros::NodeHandle nh_;
    ros::Timer markers_publish_timer_;
    ros::Publisher raster_lines_markers_pub_;
    ros::Publisher raster_poses_markers_pub_;
    visualization_msgs::MarkerArray line_markers_;
    visualization_msgs::MarkerArray poses_markers_;
    actionlib::SimpleActionClient<noether_msgs::GenerateToolPathsAction> ac_;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "surface_walk_example_node");
    console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG);
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(2);
    spinner.start();
    SurfaceWalkExample surface_walk_example(nh);
    if (!surface_walk_example.run())
    {
        return -1;
    }
    ros::waitForShutdown();
    return 0;
}

<?xml version="1.0"?>
<launch>
  <arg name="mesh_file" default="$(find noether_examples)/data/raw_mesh.ply"/>
  <node launch-prefix="" name="surface_walk_generator_node" pkg="noether_examples" type="surface_walk_example_node" output="screen">
    <param name="mesh_file" value="$(arg mesh_file)"/>
    <rosparam ns="surface_walk_rastering">
      raster_spacing: 0.05
      point_spacing: 0.02
      raster_rot_offset: 0.0
      min_hole_size: 0.02
      min_segment_size: 0.01
      intersection_plane_height: 0.0
      tool_offset: 0.0
      generate_extra_rasters: false
      cut_direction_x: 0.0
      cut_direction_y: 0.0
      cut_direction_z: 0.0
      debug: false
    </rosparam>
  </node>
  <include file="$(find noether)/launch/tool_path_planner_server.launch"/>
</launch>

rob@aa-006:~/catkin_ws$ roslaunch noether_examples surface_walk_rastering_generator_demo.launch 
... logging to /home/rob/.ros/log/6f293914-dfd9-11eb-a592-e5e0fa8e79ef/roslaunch-aa-006-49761.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://aa-006:33971/

SUMMARY
========

PARAMETERS
 * /rosdistro: noetic
 * /rosversion: 1.15.11
 * /surface_walk_generator_node/mesh_file: /home/rob/catkin_...
 * /surface_walk_generator_node/surface_walk_rastering/cut_direction_x: 0.0
 * /surface_walk_generator_node/surface_walk_rastering/cut_direction_y: 0.0
 * /surface_walk_generator_node/surface_walk_rastering/cut_direction_z: 0.0
 * /surface_walk_generator_node/surface_walk_rastering/debug: False
 * /surface_walk_generator_node/surface_walk_rastering/generate_extra_rasters: False
 * /surface_walk_generator_node/surface_walk_rastering/intersection_plane_height: 0.0
 * /surface_walk_generator_node/surface_walk_rastering/min_hole_size: 0.02
 * /surface_walk_generator_node/surface_walk_rastering/min_segment_size: 0.01
 * /surface_walk_generator_node/surface_walk_rastering/point_spacing: 0.02
 * /surface_walk_generator_node/surface_walk_rastering/raster_rot_offset: 0.0
 * /surface_walk_generator_node/surface_walk_rastering/raster_spacing: 0.05
 * /surface_walk_generator_node/surface_walk_rastering/tool_offset: 0.0

NODES
  /
    surface_walk_generator_node (noether_examples/surface_walk_example_node)
    tool_path_planner_server (noether/tool_path_planner_server)

auto-starting new master
process[master]: started with pid [49769]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 6f293914-dfd9-11eb-a592-e5e0fa8e79ef
process[rosout-1]: started with pid [49779]
started core service [/rosout]
process[surface_walk_generator_node-2]: started with pid [49786]
process[tool_path_planner_server-3]: started with pid [49787]
[ INFO] [1625741053.701597069]: Got mesh file /home/rob/catkin_ws/src/noether/noether_examples/data/raw_mesh.ply
[ INFO] [1625741054.010328414]: Starting path generation
[ INFO] [1625741054.012399528]: Planning path
Warning: tool_path_planner::SurfaceWalkRasterGenerator generating normal data
         at line 523 in /home/rob/catkin_ws/src/noether/tool_path_planner/src/surface_walk_raster_generator.cpp
[ERROR] [1625741062.350154201]: Path planning on surface 1 failed
[ERROR] [1625741062.352045486]: Failed to generate edges from mesh
[surface_walk_generator_node-2] process has died [pid 49786, exit code 255, cmd /home/rob/catkin_ws/devel/lib/noether_examples/surface_walk_example_node __name:=surface_walk_generator_node __log:=/home/rob/.ros/log/6f293914-dfd9-11eb-a592-e5e0fa8e79ef/surface_walk_generator_node-2.log].
log file: /home/rob/.ros/log/6f293914-dfd9-11eb-a592-e5e0fa8e79ef/surface_walk_generator_node-2*.log

[rr-tom-noble]

Playing around with the intersection plane height value fixed the issue. I'm guessing the edges are defined by the intersection of the mesh with plane, and having the height set to 0.0 caused the plane to miss the mesh completely?

[marip8]

Subset of the mesh is selected (would it be possible to pass a mesh to noether via a ROS topic rather than through a file?)

You can use the tool path planning server which hosts a service for generating a tool path

Selected sub-mesh is converted to a BSpline representation (already possible in noether I believe? i.e. mesh filtering demo. Unsure if this is a non-tessellated representation, or a modified mesh though)

The b-spline mesh filter doesn't convert a mesh to a b-spline representation (like in a CAD model), it just uses b-splines to approximate the surface and creates a new triangle mesh from that approximation

Given the size of a tool, a path is planned over the surface (shape doesn't matter too much as long as it covers the whole area)

The raster planners (surface walk and plane slice) have this behavior

I've taken a look at the half edge demo which seems promising. Is it possible to generate paths over a surface rather than just around the edge?

No, the half-edge planner (and eigen value planner) is a planner that only creates tool paths around the edges of the mesh

If so, is there a demo showcasing that?

Yes, it seems you found the plane slice demo and copied it to work for the surface walk planner

Playing around with the intersection plane height value fixed the issue. I'm guessing the edges are defined by the intersection of the mesh with plane, and having the height set to 0.0 caused the plane to miss the mesh completely?

Yes the tool paths are generated by intersecting the mesh with a plane. If the plane height is too small it won't cut through the mesh as you've experienced. I believe you can pass the debug parameter to the surface walk planner and it will bring up an OpenGL window that shows you the behavior of the cutting plane.

[marip8]

It seems like the plane slice rastering demo might be what I'm looking for, but I'm unsure how to view the output in rviz

The tool path should be published as a geometry_msgs::PoseArray message, so you can add a PoseArray display in Rviz to see the tool path

[marip8]

I'm going to move this to the Discussions page since it's more of a high-level usage question and less of a specific issue. Feel free to continue posting from there

[rr-tom-noble]

Thanks a bunch for the answers! Have a couple more questions if you don't mind answering them:

• Would you be able to explain some of the purposes of the other parameters for the surface walk planner? The spacing options are pretty self-explanatory, and I'd hazard a guess that cut direction controls the orientation of the paths and min hole size is something used to determine whether to plan paths over a gap in the mesh or not? Not completely sure about the others

• I've noticed that the plane slicer and surface walk planners both generate collections of distinct paths, rather than one continuous path. Is it not possible to do this, or am I missing an option? I was thinking of otherwise generating both an edge path and surface paths, then splicing them together so that the final path follows the edge from the end of one surface path to the start of the next.

• I've seen mention of STLs in some of the issues, but I can't seem to find the function I'd use to load one -- only loadPLYFile(). Am I looking in the wrong place, or would I need to convert from STL to PLY first?

Much appreciated.

[marip8]

• Raster spacing - spacing of raster lines (m)
• Point Spacing - spacing of points along raster lines (m)
• Cut direction - vector (relative to the mesh coordinates) to which the raster lines will be parallel (orthogonal to cutting plane normal). When this is set to [0, 0, 0], the planner cuts along the largest principal axis of the mesh
• Raster rotational offset - rotation of the plane used to cut the mesh and generate raster lines (rad). Typically we use this to fine tune the direction of the raster lines
• Min. segment size - minimum length of segments that get generated by the planner (m)
• Min. hole size - Minimum hole size (m) for which the planner will split a raster line such that the tool path does not travel over the hole
• Intersecting plane height - height of the cutting plane that intersects with the mesh to create the raster lines
• Tool offset - Z-axis offset (m) of the tool waypoints off the surface
• Generate extra rasters - flag for generating two extra rasters, one at the beginning and end of the path (primarily used for paint applications)
• Debug - flag for visualizing the cutting process during planning

FYI, we are planning on revising/simplifying a lot of these parameters in the refactor we are working on. We'll be sure to document them significantly better than before.

[marip8]

I've noticed that the plane slicer and surface walk planners both generate collections of distinct paths, rather than one continuous path. Is it not possible to do this, or am I missing an option? I was thinking of otherwise generating both an edge path and surface paths, then splicing them together so that the final path follows the edge from the end of one surface path to the start of the next.

I don't know that there are tools in this repository for doing that. We typically take care of that at a higher level since the requirements for a finalized tool path vary from application to application

I've seen mention of STLs in some of the issues, but I can't seem to find the function I'd use to load one -- only loadPLYFile(). Am I looking in the wrong place, or would I need to convert from STL to PLY first?

I'm not entirely sure what's being used in the examples, but PCL provides a loadPolygonFile function that is capable of loading .stl, .ply, .obj, and a few other formats. If we're not using that for the examples we probably should be. It's also easy enough to save an .stl to .ply using Meshlab or Blender