minor: to_string func for geometry_msgs::Point

src/homer_navigation_node.cpp

@@ -8,6 +8,13 @@
 #include <opencv2/opencv.hpp>
 #endif
 
+std::string to_string(const geometry_msgs::Point& p)
+{
+    std::stringstream ss;
+    ss << "[" << p.x << ", " << p.y << ", " << p.z << "]";
+    return ss.str();
+}
+
 //----------------------------------------------------------------------------------------------------------------------
 void filter_scan_points(std::vector<geometry_msgs::Point>& points)
 {
@@ -241,6 +248,8 @@ void HomerNavigationNode::setExplorerMap()
 bool HomerNavigationNode::planPath(
     const geometry_msgs::Point& A, const geometry_msgs::Point& B, nav_msgs::Path& path)
 {
+    ROS_INFO_STREAM(
+        "Planning path from map point " << to_string(A) << " to " << to_string(B) << "...");
     path.header.frame_id = "map";
     path.header.stamp = ros::Time::now();