Rename controlnode -> control_node

src/controlnode.py → src/control_node.py

@@ -7,12 +7,11 @@ from nav_msgs.msg import Odometry
 from std_msgs.msg import Empty
 from std_msgs.msg import Header, Float32
 from std_srvs.srv import Empty as SrvEmpty
-from lab2.msg import XYHVPath
-from lab2.srv import FollowPath
+from ta_lab2.msg import XYHVPath
+from ta_lab2.srv import FollowPath
 from visualization_msgs.msg import Marker
 
 import mpc
-import nonlinear
 import pid
 import purepursuit
 import utils
@@ -20,7 +19,6 @@ import utils
 controllers = {
     "PID": pid.PIDController,
     "PP": purepursuit.PurePursuitController,
-    "NL": nonlinear.NonLinearController,
     "MPC": mpc.ModelPredictiveController,
 }
 
@@ -49,7 +47,7 @@ class ControlNode:
             self.path_event.wait()
             self.reset_lock.acquire()
             ip = self.inferred_pose
-            if ip is not None and self.controller.ready():
+            if ip is not None and self.controller.is_ready():
                 index = self.controller.get_reference_index(ip)
                 pose = self.controller.get_reference_pose(index)
                 error = self.controller.get_error(ip, index)
@@ -60,6 +58,7 @@ class ControlNode:
                 if next_ctrl is not None:
                     self.publish_ctrl(next_ctrl)
                 if self.controller.path_complete(ip, error):
+                    print("Path Completed!")
                     self.path_event.clear()
             self.reset_lock.release()
             rate.sleep()
@@ -86,13 +85,16 @@ class ControlNode:
 
         rospy.Service("/controller/follow_path", FollowPath, self.cb_path)
 
-        rospy.Subscriber(rospy.get_param("~pose_topic", "/sim_car_pose/pose"),
-                         PoseStamped, self.cb_pose, queue_size=10)
+        car_pose_topic = \
+            ('/car_pose' if int(rospy.get_param('/controller/use_sim_pose')) == 1
+            else '/pf/inferred_pose')
+        rospy.Subscriber(car_pose_topic, PoseStamped,
+                         self.cb_pose, queue_size=10)
 
         self.rp_ctrls = rospy.Publisher(
             rospy.get_param(
                 "~ctrl_topic",
-                default="/vesc/high_level/ackermann_cmd_mux/input/nav_0"
+                default="/mux/ackermann_cmd_mux/input/navigation"
             ),
             AckermannDriveStamped, queue_size=2
         )
@@ -162,9 +164,6 @@ class ControlNode:
         rospy.loginfo("End state reset")
         return []
 
-    def cb_odom(self, msg):
-        self.inferred_pose = utils.rospose_to_posetup(msg.pose.pose)
-
     def cb_path(self, msg):
         print "Got path!"
         path = msg.path.waypoints

src/main.py

 #!/usr/bin/env python
 
-import controlnode
+import control_node
 import threading
 import signal
 
 if __name__ == '__main__':
-    node = controlnode.ControlNode("controller")
+    node = control_node.ControlNode("controller")
 
     signal.signal(signal.SIGINT, node.shutdown)