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
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
......
#!/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)
......
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