def __init__(self):
# Wait on a ROS Occupancy Grid message to set up the map.
self.occupancy_grid_msg = self.get_occupancy_grid_msg()
self.occupancy_grid = plc.OccupancyGrid(self.occupancy_grid_msg)
self.print_map_stats()
self.clicked_point_sub = rospy.Subscriber(
"/clicked_point", PointStamped, self.clicked_pose, queue_size=10
)
self.graph_pub = rospy.Publisher('/visualization/graph', Marker, queue_size=10)
self.wall_pub = rospy.Publisher('/visualization/wall', Marker, queue_size=10)
self.map_dilated_pub = rospy.Publisher('/map_dilated/', OccupancyGrid, queue_size=5)
self.line_trajectory = utils.LineTrajectory('visualization')
# Initially, there is no start or goal.
self.start = None
self.goal = None
# Record the wall
self.wall_start = None
self.wall_end = None
self.wall = False
# NB temporary
self.previous_point = None
self.curr_point = None
self.dilated = False
self.test_pub = rospy.Publisher('/test_data', String)
rospy.loginfo('Initialized path planner node!')
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):
# Define Topics for publishing or subscribing messages
self.init_charging_topic = rospy.get_param("~init_charging_topic")
self.pose_topic = rospy.get_param("~pose_topic")
self.drive_topic = rospy.get_param("~drive_topic")
self.stop_charging_topic = rospy.get_param("~stop_charging_topic")
# Define Adjustable Parameters
self.lookahead = float(rospy.get_param("~lookahead"))
self.max_reacquire = float(rospy.get_param("~max_reacquire"))
self.KV = float(rospy.get_param("~KV"))
self.KW = float(rospy.get_param("~KW"))
# Internal Use Variables - Do not modify without consultation
self.trajectory = utils.LineTrajectory("/charging_followed")
self.do_viz = True
self.d_t = 0.1
self.nearest_pt = None
self.lookahead_pt = None
self.init = False
self.stop_charging = False
# Subscribers
self.init_charging_sub = rospy.Subscriber(self.init_charging_topic, Bool, self.init_chargingCB, queue_size=1)
self.stop_charging_sub = rospy.Subscriber(self.stop_charging_topic, Bool, self.stop_chargingCB, queue_size=1)
self.traj_sub = rospy.Subscriber("/charging_navi/trajectory", PolygonStamped, self.trajCB, queue_size=1)
self.pose_sub = rospy.Subscriber(self.pose_topic, Odometry, self.poseCB, queue_size=1)
# Publishers
self.drive_pub = rospy.Publisher(self.drive_topic, Twist, queue_size=1)
self.nearest_pt_pub = rospy.Publisher("/charging_navi/nearest_pt", Marker, queue_size=1)
self.lookahead_pt_pub = rospy.Publisher("/charging_navi/lookahead_pt", Marker, queue_size=1)
def __init__(self):
self.odom_topic = rospy.get_param("~odom_topic")
self.kdd = MAX_KDD
self.speed = MAX_SPEED # FILL IN #
self.lookahead = self.kdd*self.speed
# if wrap is false, self.trajectory.np_points will have an extra pt, but self.trajectory.points won't
self.wrap = False # FILL IN #
self.stop = False
self.wheelbase_length = 3.0#0.325 FILL IN #
self.trajectory = utils.LineTrajectory("/followed_trajectory")
self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_callback, queue_size=1)
self.drive_pub = rospy.Publisher("/tesse/drive", AckermannDriveStamped, queue_size=1)
# change value to self.odom_topic later
self.pose_sub = rospy.Subscriber(self.odom_topic, Odometry, self.pursuit_callback, queue_size=1)
self.num_pts = None
self.prev_seg = None
# initial pose waiting
self.initialpose = True
self.init_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.init_cb, queue_size=1)
rospy.loginfo("pure pursuit waiting for initialpose")
#rviz crap
self.do_rviz = True
self.nearest_pub = rospy.Publisher("/target_pt", Marker, queue_size=1)
def __init__(self):
self.odom_topic = rospy.get_param("~odom_topic")
self.lookahead = # FILL IN #
self.speed = # FILL IN #
self.wrap = # FILL IN #
self.wheelbase_length = # FILL IN #
self.trajectory = utils.LineTrajectory("/followed_trajectory")
self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_callback, queue_size=1)
self.drive_pub = rospy.Publisher("/drive", AckermannDriveStamped, queue_size=1)
def __init__(self):
self.odom_topic = "/pf/pose/odom"
self.lookahead = 40 # FILL IN #
self.speed = 2.5 # FILL IN #
# self.wrap = 0# FILL IN #
self.wheelbase_length = 2.5 # Guess #
# self.trajectory = utils.LineTrajectory("/followed_trajectory")
# self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_callback, queue_size=1)
#**********************************************************************************************************
# self.path = rospy.get_param("~race_trj")
rospack = rospkg.RosPack()
self.lab6_path = rospack.get_path("lab6")
# self.path = rospy.get_param("~avoid_trj
# self.path = os.path.join(self.lab6_path+"/trajectories/2020-05-06-05-57-52.traj") #fave path 2
# self.path = os.path.join(self.lab6_path+"/trajectories/2020-05-06-08-38-22.traj") #fave path 1 maybe
# self.path = os.path.join(self.lab6_path+"/trajectories/2020-05-06-11-49-02.traj")
self.path = os.path.join(self.lab6_path +
"/trajectories/2020-05-06-14-07-14.traj")
self.trajectory = utils.LineTrajectory("/followed_trajectory")
self.trajectory.load(self.path)
self.segment_num = max(len(self.trajectory.points), 100)
# self.segment_num = max(len(self.trajectory.points), 500)
#Convert trajectory PoseArray to arr (line segment array)
N = self.segment_num // (len(self.trajectory.points) - 1)
rospy.loginfo(N)
self.arr = self.create_segment(self.trajectory.toPoseArray().poses, N)
# self.arr_flag = 1
#rospy.loginfo(self.arr)
#***********************************************************************************************************
self.drive_pub = rospy.Publisher("/tesse/drive",
AckermannDriveStamped,
queue_size=1)
self.drive_msg = AckermannDriveStamped()
self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry,
self.odomCallback)
rospy.Subscriber("/tesse/hood_lidar/scan", LaserScan,
self.scanCallback)
# self.segment_num = 1
# self.arr_flag = 0
self.odom_flag = 0
self.scan_flag = 0
# self.steering_angle_arr = []
self.previous_steering = 0
# self.steering_angle_dt = []
self.time_now = time.time()
self.obstacle_detected = 0
self.obstacle_timer = None
self.avoid_dir_lock = False
self.avoid_direction = 0
def __init__(self):
# Define Topics for Publishing or Subscribing Messages
self.trajectory_topic = rospy.get_param("~trajectory_topic")
self.odom_topic = rospy.get_param("~odom_topic")
self.drive_topic = rospy.get_param("~drive_topic")
self.init_table_topic = rospy.get_param("~init_table_topic")
# Define Adjustable Parameters
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"))
self.KV = float(rospy.get_param("~KV"))
self.KW = float(rospy.get_param("~KW"))
# Internal Use Variables - Do not modify without consultation
self.init = False # This needs to be "False" when integrated with others
self.trajectory = utils.LineTrajectory("/table_followed")
self.do_viz = True
self.odom_timer = utils.Timer(10)
self.iters = 0
self.d_t = float(1 / self.controller_freq)
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 = "/table_navi"
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)
# Publisher
self.drive_pub = rospy.Publisher(self.drive_topic, Twist, queue_size=1)
# Subscribers
self.init_table_sub = rospy.Subscriber(self.init_table_topic,
Bool,
self.init_tableCB,
queue_size=1)
self.traj_sub = rospy.Subscriber(self.trajectory_topic,
PolygonStamped,
self.trajectory_callback,
queue_size=1)
self.odom_sub = rospy.Subscriber(self.odom_topic,
Odometry,
self.odom_callback,
queue_size=1)
print "Initialized. Waiting on messages..."
def __init__(self):
# Define Adjustable Parameters
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"))
# Internal USE Variables - Do not modify without consultation
self.trajectory = utils.LineTrajectory("/followed_trajectory")
self.model = utils.MobileRobotControlModel()
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
self.current_pose = None
self.chosen_lookahead = None
self.chosen_KV = None
self.chosen_KW = None
# Publishers
self.drive_pub = rospy.Publisher(rospy.get_param("~drive_topic"),
Twist,
queue_size=1)
self.nearest_pt_pub = rospy.Publisher("/pure_pursuit/nearest_point",
Marker,
queue_size=1)
self.lookahead_pt_pub = rospy.Publisher(
"/pure_pursuit/lookahead_point", Marker, queue_size=1)
self.lookahead_dist_pub = rospy.Publisher(
"/pure_pursuit/lookahead_dist", Marker, queue_size=1)
# Subscribers
self.traj_sub = rospy.Subscriber(rospy.get_param("~trajectory_topic"),
PolygonStamped,
self.trajectoryCB,
queue_size=1)
self.odom_sub = rospy.Subscriber(rospy.get_param("~odom_topic"),
Odometry,
self.odomCB,
queue_size=1)
self.amcl_sub = rospy.Subscriber(rospy.get_param("~amcl_topic"),
PoseWithCovarianceStamped,
self.odomCB,
queue_size=1)
self.check_drive_sub = rospy.Subscriber(
rospy.get_param("~check_drive_topic"),
Bool,
self.check_driveCB,
queue_size=1)
print "Initialized. Waiting on messages..."
def __init__(self):
#srospy.logerr("WHYYYYYYY")
self.trajectory_topic = rospy.get_param("~trajectory_topic")
self.odom_topic = rospy.get_param("~odom_topic")
self.lookahead = rospy.get_param("~lookahead")
self.speed = float(rospy.get_param("~speed"))
self.wrap = bool(rospy.get_param("~wrap"))
self.wheelbase_length = float(rospy.get_param("~wheelbase"))
#self.drive_topic = rospy.get_param("~drive_topic")
self.drive_topic = "/drive"
self.ct_error_topic = "/ct_error"
self.error = "/error"
self.trajectory = utils.LineTrajectory("/followed_trajectory")
self.traj_sub = rospy.Subscriber(self.trajectory_topic,
PolygonStamped,
self.trajectory_callback,
queue_size=1)
self.odom_sub = rospy.Subscriber(self.odom_topic,
Odometry,
self.odom_callback,
queue_size=10)
self.drive_pub = rospy.Publisher(self.drive_topic,
AckermannDriveStamped,
queue_size=10)
self.viz_pub = rospy.Publisher("/visualization_marker",
Marker,
queue_size=10)
self.viz_pub1 = rospy.Publisher("/visualization_marker1",
Marker,
queue_size=10)
self.viz_pub2 = rospy.Publisher("/visualization_marker2",
Marker,
queue_size=10)
self.ct_error_pub = rospy.Publisher(self.ct_error_topic,
Float64,
queue_size=1)
self.error_pub = rospy.Publisher(self.error, Float64, queue_size=1)
self.trajectoryRecieved = False
self.lookahead = 2
self.good = 0.0
self.drive_on = 0.0
self.listener = tf.TransformListener()
self.priorCross = None
self.end = False
'''
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.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.traj_sub = rospy.Subscriber(self.trajectory_topic,
PolygonStamped,
self.trajectory_callback,
queue_size=1)
'''
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):
################################
# Begin ObstacleDetection init #
################################
# Additional tunable hyperparameter
self.viable_angle_range = np.pi/6. # Amount away from theta = 0 that will be considered
self.in_range = 50. # Number of meters away for a obstacle to be considered
self.laser_sub = rospy.Subscriber(rospy.get_param("~scan_topic", "/tesse/hood_lidar/scan"), LaserScan, self.lidar_cb, queue_size=1)
# For testing
self.vis_pub = rospy.Publisher("/particles", Marker, queue_size = 1)
self.lidar_pts = None
############################
# # Begin PurePursuit init #
############################
self.odom_topic = rospy.get_param("~odom_topic")
self.kdd = MAX_KDD
self.speed = MAX_SPEED # FILL IN #
self.lookahead = self.kdd*self.speed
# if wrap is false, self.trajectory.np_points will have an extra pt, but self.trajectory.points won't
self.wrap = False # FILL IN #
self.stop = False
self.wheelbase_length = 3.0#0.325 FILL IN #
self.trajectory = utils.LineTrajectory("/followed_trajectory")
self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_callback, queue_size=1)
self.drive_pub = rospy.Publisher("/tesse/drive", AckermannDriveStamped, queue_size=1)
# change value to self.odom_topic later
self.pose_sub = rospy.Subscriber(self.odom_topic, Odometry, self.pursuit_callback, queue_size=1)
self.num_pts = None
self.prev_seg = None
# initial pose waiting
self.initialpose = True
self.init_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.init_cb, queue_size=1)
rospy.loginfo("Pure pursuit Waiting 10 seconds for initial pose")
time.sleep(10.0)
#rviz crap
self.do_rviz = True
self.nearest_pub = rospy.Publisher("/target_pt", Marker, queue_size=1)
self.closest_pub = rospy.Publisher("/close_pt", Marker, queue_size=1)
def __init__(self):
self.odom_topic = "/pf/pose/odom"
self.lookahead = 10# FILL IN #
self.speed = 30 # FILL IN #
# self.wrap = 0# FILL IN #
self.wheelbase_length = 2.5# Guess #
# FOR TESTING ----------------------------------------------
#self.save_path = os.path.join(lab6_path+"/trajectories/", time.strftime("%Y-%m-%d-%H-%M-%S") + "-test.txt")
#-----------------------------------------------------------
# self.trajectory = utils.LineTrajectory("/followed_trajectory")
# self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_callback, queue_size=1)
#**********************************************************************************************************
#load trajectory
self.path = rospy.get_param("~race_trj")
rospack = rospkg.RosPack()
self.lab6_path = rospack.get_path("lab6")
self.trajectory = utils.LineTrajectory("/followed_trajectory")
# self.trajectory.publish_trajectory()
self.trajectory.load(self.path)
self.trajectory.load(os.path.join(self.lab6_path+"/trajectories/race_path.traj"))
self.segment_num = max(len(self.trajectory.points), 1000)
#Convert trajectory PoseArray to arr (line segment array)
N = self.segment_num // (len(self.trajectory.points) - 1)
#rospy.loginfo(N)
self.arr = self.create_segment(self.trajectory.toPoseArray().poses, N)
#rospy.loginfo(self.arr)
#***********************************************************************************************************
self.drive_pub = rospy.Publisher("/tesse/drive", AckermannDriveStamped, queue_size=1)
self.drive_msg = AckermannDriveStamped()
self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry, self.odomCallback)
self.last_ix = 0
self.last_lookahead = 0
self.thetas = []
self.steering_angle_arr = []
self.previous_steering = 0
self.time_now = time.time()
rospy.on_shutdown(self.save_arrays)
def __init__(self):
self.odom_topic = "/pf/pose/odom"
self.lookahead = 0.5 # FILL IN #
self.speed = 0.5 # FILL IN #
self.wrap = 0 # FILL IN #
self.wheelbase_length = 0.2 # Guess #
self.trajectory = utils.LineTrajectory("/followed_trajectory")
self.traj_sub = rospy.Subscriber("/trajectory/current",
PoseArray,
self.trajectory_callback,
queue_size=1)
self.drive_pub = rospy.Publisher("/drive",
AckermannDriveStamped,
queue_size=1)
self.drive_msg = AckermannDriveStamped()
rospy.Subscriber(self.odom_topic, Odometry, self.odomCallback)
self.arr_flag = 0
#"""
self.markerPub = rospy.Publisher('robotMarker', Marker, queue_size=10)
self.robotMarker = Marker()
self.robotMarker.header.frame_id = "/map"
self.robotMarker.scale.x = 0.2
self.robotMarker.scale.y = 0.2
self.robotMarker.scale.z = 0.2
self.robotMarker.color.r = 0.0
self.robotMarker.color.g = 1.0
self.robotMarker.color.b = 0.0
self.robotMarker.color.a = 1.0
self.robotMarker.type = 2
self.markerPub = rospy.Publisher('robotMarker2', Marker, queue_size=10)
self.robotMarker2 = Marker()
self.robotMarker2.header.frame_id = "/map"
self.robotMarker2.scale.x = 0.2
self.robotMarker2.scale.y = 0.2
self.robotMarker2.scale.z = 0.2
self.robotMarker2.color.r = 1.0
self.robotMarker2.color.g = 0.0
self.robotMarker2.color.b = 0.0
self.robotMarker2.color.a = 1.0
self.robotMarker2.type = 2
def __init__(self):
self.found_trajectory = utils.LineTrajectory("/found_trajectory")
self.rough_trajectory = utils.LineTrajectory("/rough_trajectory")
self.should_publish = bool(rospy.get_param("~publish"))
self.pub_topic = rospy.get_param("~trajectory_topic")
self.odom_topic = rospy.get_param("~odom_topic")
self.exploration_timeout = rospy.get_param("~exploration_timeout")
self.show_exploration_buffer = rospy.get_param(
"~show_exploration_buffer")
self.save_trajectory = rospy.get_param("~save_trajectory")
self.save_path = rospy.get_param("~save_path")
self.should_refine_trajectory = bool(
rospy.get_param("~refine_trajectory"))
self.refining_timeout = rospy.get_param("~refining_timeout")
self.map = None
self.map_initialized = False
self.last_pose_time = 0.0
self.circle_path = None
# just a few initial poses for debugging purposes
# self.last_pose = np.array([-1., 0., -np.pi/1.3])
# self.last_pose = np.array([-1., 0., 0.0])
self.last_pose = np.array([-3., -1., np.pi])
# self.last_pose = np.array([-1.27, -2.6, np.pi])
# self.last_pose = np.array([-6.27, -2.6, np.pi])
# self.last_pose = np.array([15.3, 25.18, 0])
# self.last_pose = np.array([-10, 33.8, 0])
# self.last_pose = None
self.get_omap()
self.space_explorer = SpaceExploration(self.map)
if self.should_publish:
self.traj_pub = rospy.Publisher(self.pub_topic,
PolygonStamped,
queue_size=1)
if self.should_refine_trajectory:
self.path_planner = PathPlanner(self.map)
self.fast_path = None
self.fast_trajectory = utils.LineTrajectory("/fast_trajectory")
# visualization publishers
self.viz_namespace = "/circle_search"
self.circle_pub = rospy.Publisher(self.viz_namespace +
"/exploration_circles",
MarkerArray,
queue_size=1)
self.fast_circle_pub = rospy.Publisher(self.viz_namespace +
"/fast_circles",
MarkerArray,
queue_size=1)
self.exploration_pub = rospy.Publisher(self.viz_namespace +
"/exploration_buffer",
OccupancyGrid,
queue_size=1)
self.nav_goal_sub = rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.goal_point_callback,
queue_size=1)
self.odom_sum = rospy.Subscriber(self.odom_topic,
Odometry,
self.odom_callback,
queue_size=1)
print "Initialized. Waiting on messages..."
def __init__(self):
# Adjustable Parameters
self.traj_topic = rospy.get_param("~trajectory_topic")
self.odom_topic = rospy.get_param("~odom_topic")
self.state_topic = rospy.get_param("~state_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"))
# Internal USE Variables - Modify with consultation
self.trajectory = utils.LineTrajectory("/followed_trajectory")
self.do_viz = True
self.odom_timer = utils.Timer(10)
self.iters = 0
self.tf2Buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf2Buffer)
self.d_t = float(1 / self.controller_freq)
self.rate = rospy.Rate(self.controller_freq)
self.allow_drive = False
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
# Publishers
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.drive_pub = rospy.Publisher(self.drive_topic, Twist, queue_size=1)
# Subscribers
# - topic to listen for trajectories
self.traj_sub = rospy.Subscriber(self.traj_topic,
PolygonStamped,
self.trajectoryCB,
queue_size=1)
self.state_sub = rospy.Subscriber(self.state_topic,
FSMState,
self.stateCB,
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.odomCB,
queue_size=1)
print "Initialized. Waiting on messages..."
def __init__(self):
# Build list of turn primitives.
assert self.NUM_TURN_DIVISIONS % 2 == 1
turn_angles = np.linspace(-self.MAX_TURN, self.MAX_TURN,
self.NUM_TURN_DIVISIONS)
np.testing.assert_almost_equal(
turn_angles[self.NUM_TURN_DIVISIONS / 2], 0.0)
turn_angles = sorted(turn_angles, key=lambda x: math.fabs(x))
rospy.loginfo("Making turn angles: {}".format(turn_angles))
self.primitives = [Turn_Primitive(angle) for angle in turn_angles]
self.primitives.extend([
Turn_Primitive(a * self.MAX_TURN_POSSIBLE,
total_time=self.MAX_TURN_TOTAL_TIME)
for a in [1, -1]
])
# Setup visualization publishers - latch them for easier debugging.
self.display_tree = []
self.display_path = []
graph_viz_topic = utils.getParamOrFail(
"/obstacle_avoidance/graph_viz_topic")
path_viz_topic = utils.getParamOrFail(
"/obstacle_avoidance/path_viz_topic")
goal_line_viz_topic = utils.getParamOrFail(
"/obstacle_avoidance/goal_line_viz_topic")
self.graph_pub = rospy.Publisher(graph_viz_topic,
Marker,
queue_size=10,
latch=True)
self.path_pub = rospy.Publisher(path_viz_topic,
Marker,
queue_size=10,
latch=True)
self.goal_line_pub = rospy.Publisher(goal_line_viz_topic,
Marker,
queue_size=3,
latch=True)
# Data structure and publisher for the waypoints (PolygonStamped).
self.line_trajectory = utils.LineTrajectory(
viz_namespace='visualization')
waypoint_topic = utils.getParamOrFail(
"/obstacle_avoidance/trajectory_topic")
self.waypoint_pub = rospy.Publisher(waypoint_topic,
PolygonStamped,
queue_size=10,
latch=True)
# Listen on /clicked_point for goal points.
# This will execute a plan from the current pose to specified goal.
self.clicked_point_sub = rospy.Subscriber("/clicked_point",
PointStamped,
self.clicked_point,
queue_size=10)
# Stores a list of points for collision checking.
# self.trajectory_plc = plc.Point2fList()
self.trajectory_list = []
self.current_trajectory_cost = None
# Listen on /initialpose for start and goal poses.
self.clicked_pose_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose,
queue_size=10)
self.pose_update_sub = rospy.Subscriber(
utils.getParamOrFail("obstacle_avoidance/localization_topic"),
PoseStamped,
self.localization_cb,
queue_size=1)
# Setup occupancy grid.
# self.local_grid_sub = rospy.Subscriber(
# utils.getParamOrFail("obstacle_avoidance/local_map_topic"), OGMSG, self.local_map_cb, queue_size=5
# )
self.occupancy_grid_sub = rospy.Subscriber(
utils.getParamOrFail("obstacle_avoidance/dilated_map_topic"),
OGMSG,
self.occupancy_grid_cb,
queue_size=1)
self.occupancy_grid_msg = self.get_static_map(
) # Blocks until static map received.
# self.occupancy_grid = plc.OccupancyGrid(self.occupancy_grid_msg)
self.occupancy_grid = OccupancyGrid(self.occupancy_grid_msg)
self.occupancy_grid.dilateOccupancyGrid(0.2, True)
# Store state: (x, y, theta).
self.previous_pose = None
self.current_pose = None
self.goal_point = None # Set by the clicked point callback.
rospy.loginfo("Initialized Primitive Planner node!")
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
