Trajectory execution in Gazebo works, still messing with obstacle avoidance