def __init__(self):
pipe_type = 1
pipe_classes = [
trr_l1.Contour1Pipeline, trr_l2.Contour2Pipeline,
trr_l3.Contour3Pipeline, trr_l4.Foo4Pipeline
]
trr_rpu.TrrSimpleVisionPipeNode.__init__(self, pipe_classes[pipe_type],
self.pipe_cbk)
roi_y_min = rospy.get_param('~roi_y_min', 0)
tl, br = (0, roi_y_min), (self.cam.w, self.cam.h)
self.pipeline.set_roi(tl, br)
# Image publishing
self.img_pub = trr_rpu.CompressedImgPublisher(
self.cam, '/trr_vision/lane/image_debug')
# Model publishing
self.lane_model_pub = trr_rpu.LaneModelPublisher(
'/trr_vision/lane/detected_model', who='trr_vision_lane_node')
self.lane_model = trru.LaneModel()
self.cfg_srv = dynamic_reconfigure.server.Server(
two_d_guidance.cfg.trr_vision_laneConfig, self.cfg_callback)
self.status_pub = trr_rpu.VisionLaneStatusPublisher(
'/trr/vision/lane/status', who='trr_vision_lane_node')
# start image subscription only here
self.start()
def __init__(self, cam, robot_name):
trr_vu.Pipeline.__init__(self)
be_param = trr_vu.NamedBirdEyeParam(robot_name)
self.cam = cam
self.set_roi((0, 0), (cam.w, cam.h))
self.bird_eye = trr_vu.BirdEyeTransformer(cam, be_param)
self.line_finder = trr_vu.HoughLinesFinder(self.bird_eye.mask_unwraped)
self.lane_model = trr_u.LaneModel()
def __init__(self, cam, robot_name):
trr_vu.Pipeline.__init__(self)
be_param = trr_vu.NamedBirdEyeParam(robot_name)
self.bird_eye = trr_vu.BirdEyeTransformer(cam, be_param)
self.line_finder = trr_vu.HoughLinesFinder(self.bird_eye.mask_unwraped)
self.lane_model = trr_u.LaneModel()
self.img = None
self.undistorted_img = None
self.display_mode = Foo4Pipeline.show_be
def __init__(self, cam, robot_name):
trr_vu.Pipeline.__init__(self)
self.thresholder = trr_vu.BinaryThresholder()
self.contour_finder = trr_vu.ContourFinder(min_area=500)
self.floor_plane_injector = trr_vu.FloorPlaneInjector()
be_param = trr_vu.NamedBirdEyeParam(robot_name)
self.bird_eye = trr_vu.BirdEyeTransformer(cam, be_param)
self.lane_model = trru.LaneModel()
self.display_mode = Contour1Pipeline.show_contour
self.img = None
def __init__(self, cam, robot_name):
trr_vu.Pipeline.__init__(self)
be_param = trr_vu.NamedBirdEyeParam(robot_name)
self.cam = cam
self.set_roi((0, 0), (cam.w, cam.h))
self.bird_eye = trr_vu.BirdEyeTransformer(cam, be_param)
self.thresholder = trr_vu.BinaryThresholder()
self.contour_finder = trr_vu.ContourFinder(min_area=500)
self.lane_model = trr_u.LaneModel()
self.display_mode = Contour3Pipeline.show_none
self.img = None
def __init__(self):
self.low_freq = 20
self.robot_name = rospy.get_param('~robot_name', "caroline")
self.ref_frame = rospy.get_param(
'~ref_frame', '{}/base_link_footprint'.format(self.robot_name))
self.fake_line_detector = trr_u.FakeLineDetector()
self.lane_model = trr_u.LaneModel()
self.lane_model_pub = trr_u.LaneModelPublisher()
self.lane_model_marker_pub = trr_u.LaneModelMarkerPublisher(
ref_frame=self.ref_frame,
topic='/follow_line/detected_lane_fake',
color=(1., 1., 0., 0.))
def __init__(self, lookahead=0.4, vel_sp=1.5, path_fname=None):
self.lookaheads = [CstLookahead(lookahead), AdaptiveLookahead()]
self.lookahead_mode = Guidance.mode_lookahead_cst
self.lookahead_dist = lookahead
self.lookahead_time = 0.1
self.lane = trr_u.LaneModel()
self.carrot = [self.lookahead_dist, 0]
self.R = np.inf
self.vel_ctl = VelCtl(path_fname, vel_sp)
self.calculated_ang_sp = 0.
self.lin_sp, self.ang_sp = 0., 0.
self.est_vel = 0.
self.set_mode(Guidance.mode_idle)
self.understeering_comp = 0
self.compensate = False #True
def __init__(self):
trr_rpu.PeriodicNode.__init__(self, 'trr_guidance_display_node')
robot_name = rospy.get_param('~robot_name', '')
def prefix(robot_name, what):
return what if robot_name == '' else '{}/{}'.format(
robot_name, what)
cam_name = rospy.get_param('~camera',
prefix(robot_name, 'camera_road_front'))
ref_frame = rospy.get_param('~ref_frame',
prefix(robot_name, 'base_link_footprint'))
rospy.loginfo(' using ref_frame: {}'.format(ref_frame))
self.lane_model = trr_u.LaneModel()
self.mark_pub = MarkerPublisher(ref_frame)
self.img_pub = ImgPublisher("/trr_guidance/image_debug", cam_name)
self.guid_stat_sub = trr_rpu.GuidanceStatusSubscriber()
self.odom_sub = trr_rpu.OdomListener('/odom', 'guidance_display_node')
def __init__(self):
name = 'trr_state_estimator_node'
trr_rpu.PeriodicNode.__init__(self, name)
tdg_dir = rospkg.RosPack().get_path('two_d_guidance')
default_path_filename = os.path.join(
tdg_dir, 'paths/demo_z/track_trr_real.npz')
path_filename = rospy.get_param('~path_filename',
default_path_filename)
self.estimator = trr_se.StateEstimator(path_filename,
self.on_lm_passed)
self.lane_model = trr_u.LaneModel()
self.state_est_pub = trr_rpu.TrrStateEstimationPublisher(
'trr_state_est/status')
# we call race manager's LandmarkPassed service to notify start/finish line crossing
srv_topic = 'LandmarkPassed'
print('Waiting for service: {}'.format(srv_topic))
rospy.wait_for_service(srv_topic)
print '##service available'
self.lm_crossed_srv_proxy = rospy.ServiceProxy(
srv_topic, two_d_guidance.srv.LandmarkPassed)
self.start_finish_sub = trr_rpu.TrrStartFinishSubscriber(
what=name, user_callback=self.on_start_finish)
self.traffic_light_sub = trr_rpu.TrrTrafficLightSubscriber()
#odom_topic = '/caroline_robot_hardware/diff_drive_controller/odom' # real
#odom_topic = '/caroline/diff_drive_controller/odom' # sim
odom_topic = '/odom' # external remaping
self.odom_sub = trr_rpu.OdomListener(odom_topic, name, 0.1,
self.on_odom)
# unused for now
#self.lane_model_sub = trr_rpu.LaneModelSubscriber('/trr_vision/lane/detected_model', 'state_estimator', user_cbk=self.on_vision_lane)
self.dyn_cfg_srv = dynamic_reconfigure.server.Server(
two_d_guidance.cfg.trr_state_estimatorConfig,
self.dyn_cfg_callback)
# we expose a service for loading a velocity profile
self.lm_service = rospy.Service('StateEstimatorLoadPath',
two_d_guidance.srv.GuidanceLoadVelProf,
self.on_load_path)
def __init__(self):
pipe_type = 0
pipe_classes = [
cv_l1.Contour1Pipeline, trr_l2.Contour2Pipeline,
trr_l3.Contour3Pipeline, trr_l4.Foo4Pipeline
]
#trr_rpu.TrrSimpleVisionPipeNode.__init__(self, pipe_classes[pipe_type], self.pipe_cbk)
cv_rpu.SimpleVisionPipeNode.__init__(self, pipe_classes[pipe_type],
self.pipe_cbk)
# Image publishing
self.img_pub = trr_rpu.CompressedImgPublisher(
self.cam, '/vision/lane/image_debug')
# Model publishing
self.lane_model_pub = trr_rpu.LaneModelPublisher(
'/vision/lane/detected_model', who='trr_vision_lane_node')
self.lane_model = trru.LaneModel()
self.cfg_srv = dynamic_reconfigure.server.Server(
two_d_guidance.cfg.trr_vision_laneConfig, self.cfg_callback)
self.status_pub = trr_rpu.VisionLaneStatusPublisher(
'/vision/lane/status', who='vision_lane_node')
#self.pipeline.thresholder.set_threshold(200)
# start image subscription only here
self.start()