Updated gotoNode function with CAN communication (issue #25)