Towards Robotic Harvesting: Mapping, Localization, Planning and Control for a Legged Tree Harvester