Added EDT querying

Author: deepak-1530

CMakeLists.txt

@@ -48,6 +48,8 @@ target_link_libraries(pubCamPose ${catkin_LIBRARIES})
 add_executable(pcNoise src/pcNoise.cpp)
 target_link_libraries(pcNoise ${catkin_LIBRARIES})
 
+add_executable(queryPt src/pubQueryPoint.cpp)
+target_link_libraries(queryPt ${catkin_LIBRARIES})
 
 
 

include/FastPlannerOctomap/Map.h

@@ -28,6 +28,7 @@ namespace Map3D
             bool ifUpdateMap(Eigen::Vector3d pt1);
             void setStartPosition(Eigen::Vector3d pt);
             void setMapRange(Eigen::Vector3d pt);
+            bool isInMap(octomap::point3d pt);
             void setMinMax();
             void erase();
             void getCostMapMarker(visualization_msgs::MarkerArray m, DynamicEDTOctomap *ptr, ros::Publisher pub);
@@ -137,6 +138,16 @@ void Map3D::OctoMapEDT::setMapRange(Eigen::Vector3d pt)
 
 }
 
+/////////////////////////////////////////////////////////////////////////////////////////
+/** check if a point lies in map or not **/
+bool Map3D::OctoMapEDT::isInMap(octomap::point3d pt)
+{
+    if ( start.x() <= pt.x() <= end.x() && start.y() <= pt.y() <= end.y() && start.z() <= pt.z() <= end.z())
+        return true;
+    else
+        return false;
+}
+
 
 //////////////////////////////////////////////////////////////////////////////////////////
 /** publish the costmap obtained from the octree **/

src/Planner.cpp

@@ -57,6 +57,8 @@ std::vector<Eigen::Vector3d> prevTraj;
 visualization_msgs::MarkerArray costMap_vis;
 
 ros::Publisher queryPointEdt;
+octomap::point3d qPt;
+bool queryPtUpdated = false;
 
 /**********************************************************************************************************************************************************
  * -------------------------------------------------------------------Callbacks---------------------------------------------------------------------------*
@@ -103,14 +105,30 @@ void edt_cb(const geometry_msgs::PoseStamped msg)
 {
     octomap::point3d queryPt(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z);
 
-    float dist = costMap3D.costMap->getDistance(queryPt);
-    std::cout<<"Distance of queried point "<<queryPt<<" is :-> "<<dist<<std::endl;
-
+    std::cout<<"Here "<<std::endl;
+    qPt = queryPt;
+    queryPtUpdated = true;
+    DynamicEDTOctomap costMap(5.0, costMap3D.tree, costMap3D.start, costMap3D.end, false);
+    
+    // check if the point is in the map
     std_msgs::Float64 edt;
-    edt.data = dist;
+    if(costMap3D.isInMap(queryPt))
+    {
+        std::cout<<costMap3D.start<<"  "<<costMap3D.end<<std::endl;
+        std::cout<<"Inside Map"<<std::endl;
+        std::cout<<queryPt<<std::endl;
+        float dist = costMap.getDistance(queryPt);
+        std::cout<<"Distance of queried point "<<queryPt<<" is :-> "<<dist<<std::endl;
 
+        edt.data = dist;
+    }
+    
+    else
+    {
+        edt.data = 1000;
+    }
     queryPointEdt.publish(edt);
-
+    
 }
 
 ///////////////////////////////////////////////////////////////////
@@ -166,9 +184,23 @@ void plan(ros::Publisher path,  ros::Publisher map, ros::Publisher pathEDT)
             DynamicEDTOctomap DistMap(5.0, costMap3D.tree, costMap3D.start, costMap3D.end, false); // take unknwon region as unoccupied
             DistMap.update();
 
+            octomap::point3d qp(24.23, 0.0 ,1.25);
+
+
             // set this as the costMap in the costMap3D object
             costMap3D.costMap = &DistMap;
 
+            if(queryPtUpdated)
+            {std::cout<<"--------------------------------------------------------------- Distance is "<<costMap3D.costMap->getDistance(qp)<<std::endl;
+                std_msgs::Float64 edt;
+                edt.data = costMap3D.costMap->getDistance(qPt);
+                queryPointEdt.publish(edt);
+                queryPtUpdated = false;
+
+            }
+
+
+
             // set planning range and pass cost map to planner
             kAstar.init(costMap3D.start, costMap3D.end, currPose);
             kAstar.setEnvironment(&DistMap);
@@ -183,10 +215,12 @@ void plan(ros::Publisher path,  ros::Publisher map, ros::Publisher pathEDT)
                 {
                     status = kAstar.search(startPose, startVel, startAcc, goalPose, goalVel, true, false, 0.0);
                     startOver = 0;
+                    ros::spinOnce();
                 }                
             else
                 {
                     status = kAstar.search(startPose, startVel, startAcc, goalPose, goalVel, false, false, 0.0);
+                    ros::spinOnce();
                 }
 
             std::cout<<"Planner output status is >>>>> "<<status<<std::endl;
@@ -237,7 +271,7 @@ void plan(ros::Publisher path,  ros::Publisher map, ros::Publisher pathEDT)
                 generatedPathEDT.header.frame_id = "map";
                 generatedPathEDT.poses.push_back(pEdt);
 
-                std::cout<<"Waypoint in current trajectory ..."<<pos<<std::endl;
+              //  std::cout<<"Waypoint in current trajectory ..."<<pos<<std::endl;
 
                 if(-INF<pos(0)<INF && -INF<pos(1)<INF && -INF<pos(2)<INF)
                 { 
@@ -323,8 +357,8 @@ int main(int argc, char **argv)
     ros::Publisher pathEDT       = n.advertise<nav_msgs::Path>("/fastPlanner_path_EDT",1);
 
     /** EDT Subscriber and distance publisher **/
-    ros::Subscriber queryPoint    = n.subscribe<geometry_msgs::PoseStamped>("/query_point_topic", 1, edt_cb);
-    queryPointEdt = n.advertise<std_msgs::Float64>("/query_point_distance", 1);
+    ros::Subscriber queryPoint   = n.subscribe<geometry_msgs::PoseStamped>("/query_point_topic", 1, edt_cb);
+    queryPointEdt                = n.advertise<std_msgs::Float64>("/query_point_distance", 1);
 
     ros::Rate rate(20);
 
@@ -333,7 +367,7 @@ int main(int argc, char **argv)
 
     while(!goalReceived)
     {
-        std::cout<<"Waiting for goal ..."<<std::endl;
+        //std::cout<<"Waiting for goal ..."<<std::endl;
         ros::spinOnce();
         rate.sleep();
 

src/pubQueryPoint.cpp

@@ -0,0 +1,44 @@
+// publish query point for which edt is needed on tje topic "/query_point_topic"
+// calculate EDT and publish the distance on the topic "/query_point_distance"
+
+#include"ros/ros.h"
+#include"geometry_msgs/PoseStamped.h"
+#include"std_msgs/Float64.h"
+#include<iostream>
+
+ros::Subscriber edtDistSub;
+ros::Publisher  edtQueryPub;
+
+void edt_query_cb(std_msgs::Float64 msg)
+{
+    std::cout<<msg.data<<std::endl;
+}
+
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "EDT_query");
+    ros::NodeHandle nh;
+
+    edtQueryPub = nh.advertise<geometry_msgs::PoseStamped>("/query_point_topic", 1);
+    edtDistSub = nh.subscribe<std_msgs::Float64>("/query_point_distance", 1, edt_query_cb);
+
+    while(ros::ok())
+    {
+        if (!ros::ok())
+        {
+            break;
+        }
+        geometry_msgs::PoseStamped queryPt;
+        std::cout<<"Enter the query point ";
+        std::cin>>queryPt.pose.position.x>>queryPt.pose.position.y>>queryPt.pose.position.z;
+
+        edtQueryPub.publish(queryPt);
+        ros::spinOnce();
+
+        
+    }
+
+    return 0;
+    
+}