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 :smile:

[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