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