expose std service to call global, add UI panel inside Rviz

Author: tungdanganh

gbplanner/config/rviz/m100_sim.rviz

@@ -25,7 +25,7 @@ Panels:
         - /POST1/MarkerArray2
         - /POST1/MarkerArray3
       Splitter Ratio: 0.544668615
-    Tree Height: 828
+    Tree Height: 656
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -45,6 +45,8 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: VLP
+  - Class: gbplanner_ui/GbPlanner Control
+    Name: GbPlanner Control
 Visualization Manager:
   Class: ""
   Displays:
@@ -206,13 +208,13 @@ Visualization Manager:
           Queue Size: 100
           Value: true
         - Class: rviz/MarkerArray
-          Enabled: false
+          Enabled: true
           Marker Topic: /vis/planning_homing_path
           Name: HomingPath
           Namespaces:
             {}
           Queue Size: 100
-          Value: false
+          Value: true
         - Class: rviz/MarkerArray
           Enabled: false
           Marker Topic: /vis/planning_graph
@@ -290,8 +292,8 @@ Visualization Manager:
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: 2.7149744
-            Min Value: 0.138104916
+            Max Value: 2.72305822
+            Min Value: 0.138391018
             Value: true
           Axis: Z
           Channel Name: intensity
@@ -520,35 +522,37 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 103.752136
+      Distance: 98.0074921
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 67.5463791
-        Y: 19.115593
-        Z: -33.9160614
+        X: 119.164963
+        Y: 9.18015099
+        Z: -12.1816568
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 1.49979651
+      Pitch: 1.33979666
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.11538696
+      Yaw: 3.25038838
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: true
+    collapsed: false
+  GbPlanner Control:
+    collapsed: false
   Height: 1028
-  Hide Left Dock: true
+  Hide Left Dock: false
   Hide Right Dock: true
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000037dfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000037d000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000190000002310000001600fffffffb0000000c00430061006d00650072006100000003affffffff60000000000000000fb0000000a0049006d00610067006501000002dc000000c90000000000000000000000010000010f00000399fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000399000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a00000003bfc0100000002fb0000000800540069006d00650100000000000003a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003a00000037d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000037dfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002d1000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000190000002310000001600fffffffb0000000c00430061006d00650072006100000003affffffff60000000000000000fb0000000a0049006d00610067006501000002dc000000c90000000000000000fb00000022004700620050006c0061006e006e0065007200200043006f006e00740072006f006c01000002ff000000a6000000a600ffffff000000010000010f00000399fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000399000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a00000003bfc0100000002fb0000000800540069006d00650100000000000003a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002300000037d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

gbplanner/launch/m100/gbplanner_sim.launch

@@ -75,8 +75,6 @@
     <rosparam command="load" file="$(arg pci_file)" />
   </node>
 
-  <group if="$(arg rviz_en)">
-    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find gbplanner)/config/rviz/m100_sim.rviz" />
-  </group>
+  <node pkg="rviz" type="rviz" name="gbplanner_ui" output="screen" args="-d $(find gbplanner)/config/rviz/m100_sim.rviz"/>
 
 </launch>

gbplanner/src/rrg.cpp

@@ -3264,10 +3264,7 @@ bool Rrg::isRemainingTimeSufficient(const double& time_cost,
                                     double& time_spare) {
   const double kTimeDelta = 20;  // magic number, extra safety
   time_spare = getTimeRemained() - time_cost;
-  if (time_spare < kTimeDelta) {
-    ROS_WARN("REACHED TIME LIMIT: BE CAREFUL.");
-    return false;
-  }
+  if (time_spare < kTimeDelta) return false;
   return true;
 }
 
@@ -3398,8 +3395,7 @@ std::vector<geometry_msgs::Pose> Rrg::runGlobalPlanner(int vertex_id,
     double time_spare = 0;
     if (!isRemainingTimeSufficient(time_cost, time_spare)) {
       ROS_WARN("[Global] Not enough time to go the vertex [%d]", vertex_id);
-      ROS_WARN(
-          "[Global] Consider change to another ID or set ignore_time to True");
+      ROS_WARN("[Global] Consider another ID or set ignore_time to True");
       return ret_path;
     }
     best_frontier = global_graph_->getVertex(vertex_id);
@@ -3442,12 +3438,10 @@ std::vector<geometry_msgs::Pose> Rrg::runGlobalPlanner(int vertex_id,
         }
       }
     }
-    ROS_INFO("Get %d feasible frontiers from global frontiers.",
-             (int)feasible_global_frontiers.size());
+    ROS_INFO("There are %d potential frontiers, get %d feasible frontiers.",
+        (int)global_frontiers.size(), (int)feasible_global_frontiers.size());
     if (feasible_global_frontiers.size() <= 0) {
-      ROS_INFO(
-          "No feasible frontier exists --> Call HOMING instead if fully "
-          "explored.");
+      ROS_WARN("No feasible frontier-->Call HOMING instead if fully explored.");
       return ret_path;
     }
 

planner_control_interface/include/planner_control_interface/planner_control_interface.h

@@ -74,6 +74,7 @@ class PlannerControlInterface {
   ros::ServiceServer pci_set_homing_pos_server_;
   ros::ServiceServer pci_initialization_server_;
   ros::ServiceServer pci_global_server_;
+  ros::ServiceServer pci_std_global_server_;
   ros::ServiceServer pci_stop_server_;
   ros::ServiceServer pci_std_stop_server_;
   ros::ServiceServer pci_geofence_server_;
@@ -131,6 +132,8 @@ class PlannerControlInterface {
 
   bool globalPlannerCallback(planner_msgs::pci_global::Request &req,
                              planner_msgs::pci_global::Response &res);
+  bool stdSrvGlobalPlannerCallback(std_srvs::Trigger::Request &req,
+                                   std_srvs::Trigger::Response &res);
 
   bool stopPlannerCallback(planner_msgs::pci_stop::Request &req,
                            planner_msgs::pci_stop::Response &res);

planner_control_interface/src/planner_control_interface.cpp

@@ -65,6 +65,10 @@ PlannerControlInterface::PlannerControlInterface(
       "pci_global", &PlannerControlInterface::globalPlannerCallback, this);
   planner_global_client_ =
       nh.serviceClient<planner_msgs::planner_global>("gbplanner/global");
+  pci_std_global_server_ = nh_.advertiseService(
+      "planner_control_interface/std_srvs/global_planning",
+      &PlannerControlInterface::stdSrvGlobalPlannerCallback, this);
+
   pci_stop_server_ = nh_.advertiseService(
       "pci_stop", &PlannerControlInterface::stopPlannerCallback, this);
   pci_std_stop_server_ = nh_.advertiseService(
@@ -238,6 +242,22 @@ bool PlannerControlInterface::homingCallback(
   return true;
 }
 
+bool PlannerControlInterface::stdSrvGlobalPlannerCallback(
+    std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
+  planner_msgs::pci_global::Request global_trigger_request;
+  planner_msgs::pci_global::Response global_trigger_response;
+
+  global_trigger_request.not_exe_path = false;
+  global_trigger_request.id = 0;
+  global_trigger_request.not_check_frontier = false;
+  global_trigger_request.ignore_time = false;
+
+  res.success =
+      globalPlannerCallback(global_trigger_request, global_trigger_response);
+  res.success &= global_trigger_response.success;
+  return true;
+}
+
 bool PlannerControlInterface::stdSrvHomingCallback(
     std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
   planner_msgs::pci_homing_trigger::Request homing_trigger_request;