Simple node for surface raster planner for RViz visualization (similar to edgepaths)

[shubhamwagh]

I tried to create my own node for toolpath_finder (similar to halfedge_finder) which uses surface_raster_planner_server to request planners for a mesh. I am getting different results with this as compared to running using

roslaunch noether_examples surf_raster_planner_demo.launch.

Kindly find my toolapth_finder.cpp code. Any help will be appreciated.
Note : For now I have kept same config both in tool.yaml and RasterPathGenerator::generateDefaultToolConfig() functio. Also, using the same surafce_raster_planner_server.cpp with a change as mentioned in this issue to avoid segfault.

tool.yaml config file

pt_spacing: 0.05
line_spacing: 0.15
tool_offset: 0.0 # currently unused
intersecting_plane_height: 0.2 # 0.5 works best, not sure if this should be included in the tool
min_hole_size: 0.01
min_segment_size: 0.01
raster_angle: 0.0
raster_wrt_global_axes: false
generate_extra_rasters: true
#cut_norm_x: -8.0
#cut_norm_y: 0.0
#cut_norm_z: 0.0
#centroid_x: 5.0
#centroid_y: 43
#centroid_z: 6.0
debug_on: false

tool_path_planner::ProcessTool RasterPathGenerator::generateDefaultToolConfig()
{
  tool_path_planner::ProcessTool tool;
  tool.pt_spacing = 0.05;
  tool.line_spacing = 0.15;
  tool.tool_offset = 0.0; // currently unused
  tool.intersecting_plane_height = 0.2; // 0.5 works best, not sure if this should be included in the tool
  tool.min_hole_size = 0.01;
  tool.min_segment_size = 0.01;
  tool.raster_angle = 0.0;
  tool.raster_wrt_global_axes = false;
  tool.generate_extra_rasters = true;
  return tool;
}

toolpath_finder.cpp

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

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

static const double GOAL_WAIT_PERIOD = 300.0; //sec
static const std::string GENERATE_TOOL_PATHS_ACTION = "generate_tool_paths";
static const std::string DEFAULT_FRAME_ID = "world";
static const std::string BOUNDARY_LINES_MARKERS_TOPIC ="boundary_lines";
static const std::string BOUNDARY_POSES_MARKERS_TOPIC ="boundary_poses";
static const std::string TOOL_PATH_NS = "toolpath_";
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);
static const std::size_t MAX_MARKERS_ON_DISPLAY = 300;

std::size_t countPathPoints(const noether_msgs::ToolRasterPath& rasters)
{
    std::size_t point_count = 0;

    for(const auto& p : rasters.paths)
    {
        point_count += p.poses.size();
    }

    return point_count;
}

class ToolPathFinder
{
public:

    ToolPathFinder(ros::NodeHandle nh):
            nh_(nh),
            ac_(GENERATE_TOOL_PATHS_ACTION)
    {
        boundary_lines_markers_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(BOUNDARY_LINES_MARKERS_TOPIC,1);
        boundary_poses_markers_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(BOUNDARY_POSES_MARKERS_TOPIC,1);
    }

    ~ToolPathFinder()
    {

    }

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

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

            if(!poses_markers_.markers.empty())
            {
                boundary_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;
        goal.proceed_on_failure = true;
        goal.surface_meshes.push_back(mesh_msg);
        ac_.sendGoal(goal);
        ros::Time start_time = ros::Time::now();
        if(!ac_.waitForResult(ros::Duration(GOAL_WAIT_PERIOD)))
        {
            ROS_ERROR("Failed to generate tool paths from mesh");
            return false;
        }

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

        const auto& boundary_poses = res->tool_raster_paths;

        ROS_INFO("Found %lu edges",boundary_poses.size());

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

            std::string ns = TOOL_PATH_NS + std::to_string(i);
            visualization_msgs::MarkerArray tool_path_axis_markers = convertToAxisMarkers(boundary_poses[i],
                                                                                          DEFAULT_FRAME_ID,
                                                                                          ns);

            visualization_msgs::MarkerArray tool_path_line_markers = convertToDottedLineMarker(boundary_poses[i],
                                                                                               DEFAULT_FRAME_ID,
                                                                                               ns);

            if(poses_markers_.markers.size() > MAX_MARKERS_ON_DISPLAY)
            {
                // prevents buffer overruns
                poses_markers_.markers.clear();
            }

            if(line_markers_.markers.size() > MAX_MARKERS_ON_DISPLAY) // prevents buffer overruns
            {
                // prevents buffer overruns
                line_markers_.markers.clear();
            }

            poses_markers_.markers.insert( poses_markers_.markers.end(),
                                           tool_path_axis_markers.markers.begin(), tool_path_axis_markers.markers.end());
            line_markers_.markers.insert( line_markers_.markers.end(),
                                          tool_path_line_markers.markers.begin(), tool_path_line_markers.markers.end());
        }
        return true;
    }

private:
    ros::NodeHandle nh_;
    ros::Timer markers_publish_timer_;
    ros::Publisher boundary_lines_markers_pub_;
    ros::Publisher boundary_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,"toolpath_finder");
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(2);
    spinner.start();
    console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO);
    ToolPathFinder toolpath_finder(nh);
    if(!toolpath_finder.run())
    {
        return -1;
    }
    ros::waitForShutdown();
    return 0;
}

[shubhamwagh]

I think I had made a simple mistake. Fixed it. Now I get the same results. I had set generate_extra_rasters to true. It is false by default.