Commit 79e1a521 authored by Kay Ke's avatar Kay Ke
Browse files

Rename controlnode -> control_node

parent b68ed6c3
...@@ -7,12 +7,11 @@ from nav_msgs.msg import Odometry ...@@ -7,12 +7,11 @@ from nav_msgs.msg import Odometry
from std_msgs.msg import Empty from std_msgs.msg import Empty
from std_msgs.msg import Header, Float32 from std_msgs.msg import Header, Float32
from std_srvs.srv import Empty as SrvEmpty from std_srvs.srv import Empty as SrvEmpty
from lab2.msg import XYHVPath from ta_lab2.msg import XYHVPath
from lab2.srv import FollowPath from ta_lab2.srv import FollowPath
from visualization_msgs.msg import Marker from visualization_msgs.msg import Marker
import mpc import mpc
import nonlinear
import pid import pid
import purepursuit import purepursuit
import utils import utils
...@@ -20,7 +19,6 @@ import utils ...@@ -20,7 +19,6 @@ import utils
controllers = { controllers = {
"PID": pid.PIDController, "PID": pid.PIDController,
"PP": purepursuit.PurePursuitController, "PP": purepursuit.PurePursuitController,
"NL": nonlinear.NonLinearController,
"MPC": mpc.ModelPredictiveController, "MPC": mpc.ModelPredictiveController,
} }
...@@ -49,7 +47,7 @@ class ControlNode: ...@@ -49,7 +47,7 @@ class ControlNode:
self.path_event.wait() self.path_event.wait()
self.reset_lock.acquire() self.reset_lock.acquire()
ip = self.inferred_pose 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) index = self.controller.get_reference_index(ip)
pose = self.controller.get_reference_pose(index) pose = self.controller.get_reference_pose(index)
error = self.controller.get_error(ip, index) error = self.controller.get_error(ip, index)
...@@ -60,6 +58,7 @@ class ControlNode: ...@@ -60,6 +58,7 @@ class ControlNode:
if next_ctrl is not None: if next_ctrl is not None:
self.publish_ctrl(next_ctrl) self.publish_ctrl(next_ctrl)
if self.controller.path_complete(ip, error): if self.controller.path_complete(ip, error):
print("Path Completed!")
self.path_event.clear() self.path_event.clear()
self.reset_lock.release() self.reset_lock.release()
rate.sleep() rate.sleep()
...@@ -86,13 +85,16 @@ class ControlNode: ...@@ -86,13 +85,16 @@ class ControlNode:
rospy.Service("/controller/follow_path", FollowPath, self.cb_path) rospy.Service("/controller/follow_path", FollowPath, self.cb_path)
rospy.Subscriber(rospy.get_param("~pose_topic", "/sim_car_pose/pose"), car_pose_topic = \
PoseStamped, self.cb_pose, queue_size=10) ('/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( self.rp_ctrls = rospy.Publisher(
rospy.get_param( rospy.get_param(
"~ctrl_topic", "~ctrl_topic",
default="/vesc/high_level/ackermann_cmd_mux/input/nav_0" default="/mux/ackermann_cmd_mux/input/navigation"
), ),
AckermannDriveStamped, queue_size=2 AckermannDriveStamped, queue_size=2
) )
...@@ -162,9 +164,6 @@ class ControlNode: ...@@ -162,9 +164,6 @@ class ControlNode:
rospy.loginfo("End state reset") rospy.loginfo("End state reset")
return [] return []
def cb_odom(self, msg):
self.inferred_pose = utils.rospose_to_posetup(msg.pose.pose)
def cb_path(self, msg): def cb_path(self, msg):
print "Got path!" print "Got path!"
path = msg.path.waypoints path = msg.path.waypoints
......
#!/usr/bin/env python #!/usr/bin/env python
import controlnode import control_node
import threading import threading
import signal import signal
if __name__ == '__main__': if __name__ == '__main__':
node = controlnode.ControlNode("controller") node = control_node.ControlNode("controller")
signal.signal(signal.SIGINT, node.shutdown) signal.signal(signal.SIGINT, node.shutdown)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment