Update robot state if time since last command exceeds timeout (#3251)

* Update robot state if time since last command exceeds timeout

* update last_commanded_state_, when the command is staled

* resolve conflicts

* delete space

* chore: add new line

* refactor: if next_joint_state is nullopt, update robot state with blocking

* non-blocking update of robot status

* remove unused include file

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -398,6 +398,7 @@ void ServoNode::servoLoop()
       else
       {
         // if no new command was created, use current robot state
+        last_commanded_state_ = current_state = servo_->getCurrentRobotState(false);
         updateSlidingWindow(current_state, joint_cmd_rolling_window_, servo_params_.max_expected_latency, cur_time);
         servo_->resetSmoothing(current_state);
       }