Waypoint nav interface

scripts/waypoint_nav.py

@@ -0,0 +1,69 @@
+#!/usr/bin/env python
+import time
+import rospy
+import argparse
+import roslib
+roslib.load_manifest('amrl_msgs')
+from amrl_msgs.msg import NavStatusMsg, Localization2DMsg
+from std_msgs.msg import Empty
+
+
+class WaypointNav:
+    def __init__(self, stop_time):
+        self.stop_time = stop_time
+        self.traj = []  # list of tuples (x, y, theta) waypoints
+        self.next_goal = 0
+        self.stop_detected_first_time = -1
+        self.INITIAL = True
+        rospy.Subscriber('/navigation_goal_status', NavStatusMsg, self.nav_callback, queue_size=1)
+        rospy.Subscriber('/reset_nav_goals', Empty, self.reset_callback, queue_size=1)
+        rospy.Subscriber('/set_waypoint', Localization2DMsg, self.set_waypoint_callback, queue_size=1)
+        self.pub = rospy.Publisher('/move_base_simple/goal_amrl', Localization2DMsg, queue_size=1)
+
+    def reset_callback(self, msg):
+        self.next_goal = 0
+        self.stop_detected_first_time = -1
+        self.traj = []
+        self.INITIAL = True
+
+    def set_waypoint_callback(self, msg):
+        self.traj.append((msg.pose.x, msg.pose.y, msg.pose.theta))
+
+    def nav_callback(self, msg):
+        if len(self.traj) == 0:
+            return
+        cur_traj = self.traj  # to protect against asynchronous updates to self.traj
+        pub_msg = Localization2DMsg()
+        if msg.status == 0:  # stopped
+            if self.stop_detected_first_time == -1:
+                self.stop_detected_first_time = rospy.get_rostime().secs
+            else:
+                if (rospy.get_rostime().secs - self.stop_detected_first_time > self.stop_time) or self.INITIAL:
+                    if self.INITIAL:
+                        time.sleep(20)  # wait for everything to setup properly and waypoints to be set
+                    self.INITIAL = False
+                    self.stop_detected_first_time = -1
+                    # publish next goal
+                    pub_msg.pose.x = cur_traj[self.next_goal][0]
+                    pub_msg.pose.y = cur_traj[self.next_goal][1]
+                    pub_msg.pose.theta = cur_traj[self.next_goal][2]
+                    self.pub.publish(pub_msg)
+                    time.sleep(4)
+                    self.next_goal += 1
+                    self.next_goal %= len(cur_traj)
+        else:
+            self.stop_detected_first_time = -1
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--stop_time", type=int, default=10, help="Time to stop at each waypoint")
+    args = parser.parse_args(rospy.myargv()[1:])
+
+    rospy.init_node('waypoint_nav', anonymous=False)
+    obj = WaypointNav(stop_time=args.stop_time)
+    time.sleep(1)
+    try:
+        rospy.spin()
+    except KeyboardInterrupt:
+        print("Shutting down ROS waypoint nav node")

src/navigation/navigation.cc

@@ -189,6 +189,13 @@ void Navigation::SetNavGoal(const Vector2f& loc, float angle) {
   plan_path_.clear();
 }
 
+void Navigation::ResetNavGoals() {
+  nav_state_ = NavigationState::kStopped;
+  nav_goal_loc_ = robot_loc_;
+  nav_goal_angle_ = robot_angle_;
+  local_target_.setZero();
+  plan_path_.clear();
+}
 
 void Navigation::SetOverride(const Vector2f& loc, float angle) {
   nav_state_ = NavigationState::kOverride;

src/navigation/navigation.h

@@ -98,6 +98,7 @@ class Navigation {
                          float* clearance,
                          Eigen::Vector2f* obstruction);
   void SetNavGoal(const Eigen::Vector2f& loc, float angle);
+  void ResetNavGoals();
   void SetOverride(const Eigen::Vector2f& loc, float angle);
   void Resume();
   bool PlanStillValid();

src/navigation/navigation_main.cc

@@ -65,6 +65,7 @@
 #include "shared/util/helpers.h"
 #include "shared/ros/ros_helpers.h"
 #include "std_msgs/Bool.h"
+#include "std_msgs/Empty.h"
 #include "tf/transform_broadcaster.h"
 #include "tf/transform_datatypes.h"
 #include "visualization/visualization.h"
@@ -237,6 +238,10 @@ void GoToCallbackAMRL(const amrl_msgs::Localization2DMsg& msg) {
   navigation_.Resume();
 }
 
+void ResetNavGoalsCallback(const std_msgs::Empty& msg) {
+  navigation_.ResetNavGoals();
+}
+
 bool PlanServiceCb(graphNavSrv::Request &req,
                  graphNavSrv::Response &res) {
   const Vector2f start(req.start.x, req.start.y);
@@ -838,6 +843,8 @@ int main(int argc, char** argv) {
       n.subscribe("/move_base_simple/goal", 1, &GoToCallback);
   ros::Subscriber goto_amrl_sub =
       n.subscribe("/move_base_simple/goal_amrl", 1, &GoToCallbackAMRL);
+  ros::Subscriber reset_nav_goals_sub =
+      n.subscribe("/reset_nav_goals", 1, &ResetNavGoalsCallback);
   ros::Subscriber enabler_sub =
       n.subscribe(CONFIG_enable_topic, 1, &EnablerCallback);
   ros::Subscriber halt_sub =
@@ -870,8 +877,10 @@ int main(int argc, char** argv) {
       // Publish Visualizations
       PublishForwardPredictedPCL(navigation_.GetPredictedCloud());
       DrawRobot();
-      DrawTarget();
-      DrawPathOptions();
+      if (navigation_.GetNavStatusUint8() != static_cast<uint8_t>(navigation::NavigationState::kStopped)) {
+        DrawTarget();
+        DrawPathOptions();
+      }
       PublishVisualizationMarkers();
       PublishPath();
       local_viz_msg_.header.stamp = ros::Time::now();