How to visualize Smac Planner's path calculation

edit

Hi,

On the Navigation2 / Smac Planner GitHub page it is described, that the path calculation process can be visualized by adding the following piece(s) of code:

// In createPath()
static auto node = std::make_shared<rclcpp::Node>("test");
static auto pub = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions", 1);
geometry_msgs::msg::PoseArray msg;
geometry_msgs::msg::Pose msg_pose;
msg.header.stamp = node->now();
msg.header.frame_id = "map";

...

// Each time we expand a new node 
msg_pose.position.x = _costmap->getOriginX() + (current_node->pose.x * _costmap->getResolution());
msg_pose.position.y = _costmap->getOriginY() + (current_node->pose.y * _costmap->getResolution());
msg.poses.push_back(msg_pose);

... 

// On backtrace or failure
pub->publish(msg);

My question is, where these should be exactly inserted? My goal is to test the Smac State Lattice Planner with own custom control sets. I used the main branch version of Navigation2 repository on ROS2 Rolling Ridley (as the State Lattice Planner is not existing in Galactic)