def main(): robot = turtlebot.robot.build_real() db = location_db.build_real() api = RobotApi(robot, db) rospy.Service('code_it/api/display_message', DisplayMessage, api.on_display_message) rospy.Service('code_it/api/ask_multiple_choice', AskMultipleChoice, api.on_ask_multiple_choice) rospy.Service('code_it/api/go_to', GoTo, api.on_go_to) rospy.Service('code_it/api/go_to_dock', GoToDock, api.on_go_to_dock) rospy.Subscriber("code_it/is_program_running", Bool, api.on_is_program_running)
return GetPoseByNameResponse() else: return GetPoseByNameResponse(exists=True, pose_stamped=pose_stamped) def on_list(self, request): return ListPosesResponse(names=self._db.list()) def on_upsert(self, request): self._db.upsert(request.name, request.pose_stamped) return UpsertPoseResponse() def on_save_current(self, request): pose_stamped = get_current_location(self._tf_listener, 'base_footprint', 'map') if pose_stamped is None: raise rospy.ServiceException('Failed to save current location.') self._db.upsert(request.name, pose_stamped) return SaveCurrentPoseResponse() if __name__ == '__main__': rospy.init_node('location_server') db = build_real() server = Server(db) rospy.Service('location_db/delete', DeletePose, server.on_delete) rospy.Service('location_db/get_by_name', GetPoseByName, server.on_get_by_name) rospy.Service('location_db/list', ListPoses, server.on_list) rospy.Service('location_db/upsert', UpsertPose, server.on_upsert) rospy.Service('location_db/save_current', SaveCurrentPose, server.on_save_current) rospy.spin()