From 1f63b986d9414700fba1fed8ca5e41bc08a0a987 Mon Sep 17 00:00:00 2001
From: zhangwanjie <zhangwanjie@126.com>
Date: Sun, 31 May 2020 07:26:05 +0800
Subject: [PATCH] update for wpr_ simulation

---
 launch/add_waypoint_simulation.launch | 14 ++++++++++++++
 src/wp_navi_server.cpp                |  5 ++++-
 2 files changed, 18 insertions(+), 1 deletion(-)
 create mode 100644 launch/add_waypoint_simulation.launch

diff --git a/launch/add_waypoint_simulation.launch b/launch/add_waypoint_simulation.launch
new file mode 100644
index 0000000..d40a56c
--- /dev/null
+++ b/launch/add_waypoint_simulation.launch
@@ -0,0 +1,14 @@
+<launch>
+
+  <!-- Map server -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find wpr_simulation)/maps/map.yaml"/>
+  
+  <!-- RViz and TF  -->
+  <arg name="rvizconfig" default="$(find waterplus_map_tools)/rviz/editwaypoints.rviz" />
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+  <node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 /map /base_link 100"/>
+
+  <!-- Map tools  -->
+  <node pkg="waterplus_map_tools" type="wp_edit_node" name="wp_edit_node" output="screen" />
+
+</launch>
diff --git a/src/wp_navi_server.cpp b/src/wp_navi_server.cpp
index a4daf97..a6d26ee 100644
--- a/src/wp_navi_server.cpp
+++ b/src/wp_navi_server.cpp
@@ -106,10 +106,13 @@ int main(int argc, char** argv)
             if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
             {
                 ROS_INFO("Arrived at WayPoint !");
+                result_msg.data = "done";
             }
             else
+            {
                 ROS_WARN("Failed to get to WayPoint ..." );
-            result_msg.data = "done";
+                result_msg.data = "failure";
+            }
             result_pub.publish(result_msg);
             bNewCmd = false;
         }