Replace pr2_ethercat main loop with one based on ros_control