def __init__(self):
# Define Adjustaable Parameters
self.min_speed = rospy.get_param("~min_speed")
self.max_speed = rospy.get_param("~max_speed")
self.max_acceleration = rospy.get_param("~max_acceleration")
self.max_decceleration = rospy.get_param("~max_decceleration")
# Internal USE Variables - Do not modify without consultation
self.dynamic_model = utils.ApproximateDynamicsModel()
self.ackermann_model = utils.AckermannModel(0.36)
self.trajectory = LineTrajectory("/speed_track")
self.counts = 0
# Publishers
self.viz_track_pub = rospy.Publisher("/speed_track/viz",
MarkerArray,
queue_size=1)
self.traj_pub = rospy.Publisher(rospy.get_param("~pub_topic"),
PolygonStamped,
queue_size=1)
# Subscriber
self.traj_sub = rospy.Subscriber(rospy.get_param("~sub_topic"),
String,
self.trajCB,
queue_size=1)
print "Initialized. Waiting on messages..."
# need to wait a short period of time before publishing the first message
time.sleep(0.5)
def __init__(self):
self.trajectory_topic = rospy.get_param("~trajectory_topic")
self.odom_topic = rospy.get_param("~odom_topic")
self.check_drive_topic = rospy.get_param("~check_drive_topic")
self.drive_topic = rospy.get_param("~drive_topic")
self.lookahead = rospy.get_param("~lookahead")
self.max_reacquire = rospy.get_param("~max_reacquire")
self.controller_freq = float(
rospy.get_param("~controller_freq", "10.0"))
self.wrap = bool(rospy.get_param("~wrap"))
wheelbase_length = float(rospy.get_param("~wheelbase"))
self.KV = float(rospy.get_param("~KV"))
self.KW = float(rospy.get_param("~KW"))
self.trajectory = utils.LineTrajectory("/followed_trajectory")
self.model = utils.AckermannModel(wheelbase_length)
self.do_viz = True
self.odom_timer = utils.Timer(10)
self.iters = 0
self.d_t = float(1 / self.controller_freq)
self.check_drive = True
self.nearest_point = None
self.lookahead_point = None
# set up the visualization topic to show the nearest point on the trajectory, and the lookahead point
self.viz_name = "/pure_pursuit"
self.nearest_point_pub = rospy.Publisher(self.viz_name +
"/nearest_point",
Marker,
queue_size=1)
self.lookahead_point_pub = rospy.Publisher(self.viz_name +
"/lookahead_point",
Marker,
queue_size=1)
# topic to send drive commands to
self.control_pub = rospy.Publisher(self.drive_topic,
Twist,
queue_size=1)
# topic to listen for trajectories
self.traj_sub = rospy.Subscriber(self.trajectory_topic,
PolygonStamped,
self.trajectory_callback,
queue_size=1)
# topic to listen for odometry messages, either from particle filter or robot localization
self.odom_sub = rospy.Subscriber(self.odom_topic,
Odometry,
self.odom_callback,
queue_size=1)
# topic to listen whether drive the vehicle or not
self.check_drive_sub = rospy.Subscriber(self.check_drive_topic,
Bool,
self.check_driveCB,
queue_size=1)
print "Initialized. Waiting on messages..."
def __init__(self):
self.trajectory_topic = rospy.get_param("~trajectory_topic")
self.odom_topic = rospy.get_param("~odom_topic")
self.lookahead = rospy.get_param("~lookahead")
self.max_reacquire = rospy.get_param("~max_reacquire")
self.speed = float(rospy.get_param("~speed"))
self.wrap = bool(rospy.get_param("~wrap"))
wheelbase_length = float(rospy.get_param("~wheelbase"))
self.drive_topic = rospy.get_param("~drive_topic")
self.trajectory = utils.LineTrajectory("/followed_trajectory")
self.model = utils.AckermannModel(wheelbase_length)
self.do_viz = True
self.odom_timer = utils.Timer(10)
self.iters = 0
self.nearest_point = None
self.lookahead_point = None
# set up the visualization topic to show the nearest point on the trajectory, and the lookahead point
self.viz_namespace = "/pure_pursuit"
self.nearest_point_pub = rospy.Publisher(self.viz_namespace +
"/nearest_point",
Marker,
queue_size=1)
self.lookahead_point_pub = rospy.Publisher(self.viz_namespace +
"/lookahead_point",
Marker,
queue_size=1)
# topic to send drive commands to
self.control_pub = rospy.Publisher(self.drive_topic,
AckermannDriveStamped,
queue_size=1)
# topic to listen for trajectories
self.traj_sub = rospy.Subscriber(self.trajectory_topic,
PolygonStamped,
self.trajectory_callback,
queue_size=1)
# topic to listen for odometry messages, either from particle filter or the simulator
self.odom_sub = rospy.Subscriber(self.odom_topic,
Odometry,
self.odom_callback,
queue_size=1)
print "Initialized. Waiting on messages..."
def __init__(self):
self.lookahead = rospy.get_param("~lookahead", default=1.5)
self.max_reacquire = rospy.get_param("~max_reacquire", default=3.0)
self.speed = float(rospy.get_param("~speed", default=1.5))
self.wrap = bool(rospy.get_param("~wrap", default=0))
wheelbase_length = float(rospy.get_param("~wheelbase", default=0.335))
self.max_twiddle_step = rospy.get_param("~max_twiddle_step",
default=500)
self.viz = rospy.get_param("~viz", default=True)
self.margin = rospy.get_param("~margin", default=0.3)
self.max_angle = rospy.get_param("~max_angle", default=0.4538)
self.need_smooth = rospy.get_param("~need_smooth", default=True)
self.angle_noise = rospy.get_param("~angle_noise", default=0.03)
self.load_or_not = rospy.get_param("~load_or_not", default=False)
self.trajectory = utils.LineTrajectory("/followed_trajectory")
self.model = utils.AckermannModel(wheelbase_length)
self.do_viz = True
self.odom_timer = utils.Timer(10)
self.iters = 0
self.nearest_point = None
self.lookahead_point = None
self.tau_p = rospy.get_param('~tau_p', default=1.0)
self.tau_i = rospy.get_param('~tau_i', default=0.0)
self.tau_d = rospy.get_param('~tau_d', default=0.0)
self.cte = 0.
self.diff_cte = 0.
self.int_cte = 0.
self.prev_angle = 0.
self.first_angle = True
self.already_twiddled = True
# set up the visualization topic to show the nearest point on the trajectory, and the lookahead point
self.viz_namespace = "/pure_pursuit"
self.nearest_point_pub = rospy.Publisher(self.viz_namespace +
"/nearest_point",
Marker,
queue_size=1)
self.lookahead_point_pub = rospy.Publisher(self.viz_namespace +
"/lookahead_point",
Marker,
queue_size=1)
self.pub_smoothed_path_ = rospy.Publisher('/smoothed_path',
Path,
queue_size=2)
# topic to send drive commands to
self.control_pub = rospy.Publisher("/ackermann_cmd_mux",
AckermannDriveStamped,
queue_size=1)
self.cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# topic to listen for odometry messages, either from particle filter or the simulator
self.odom_sub = rospy.Subscriber("/odom",
Odometry,
self.odom_callback,
queue_size=1)
self.cur_pose_sub = rospy.Subscriber("/current_pose",
PoseStamped,
self.cur_pose_callback,
queue_size=1)
#self.pose_sub = rospy.Subscriber(
# "/initialpose",
# PoseWithCovarianceStamped,
# self.poseCB,
# queue_size=1)
if self.viz:
self.pub_move_point = rospy.Publisher("/move_point",
PoseArray,
queue_size=1)
self.pub_move_point_1 = rospy.Publisher("/move_point/no_pid",
PoseArray,
queue_size=1)
self.map_data = None
# rospy.wait_for_service('/static_map')
# try:
# get_map = rospy.ServiceProxy('/static_map', GetMap)
# resp = get_map()
# self.map_data = resp.map
# except rospy.ServiceException, e:
# print "Service call failed: ", e
if self.load_or_not:
rospy.wait_for_service('/get_raw_path')
try:
get_raw_path = rospy.ServiceProxy('/get_raw_path', GetRawPath)
resp = get_raw_path()
raw_path = resp.raw_path
self.trajectory.clear()
self.trajectory.fromPath(raw_path)
self.trajectory.publish_viz(duration=0.0)
print "Get path."
except rospy.ServiceException, e:
print "Service call failed: ", e