I have a scanlist of 5 positions for the robot to go. And for each position, I hope to subscribe to a topic to get the sensor data and add a marker in the rviz [..]
This is admittedly more of a design question, but I would argue for a different approach to get to what you want to do.
Essentially, you want to store some data from a sensor, based on the rest of the state of your system (ie: its position relative to some reference position). This to me sounds like a state machine, in which the transitions are taken based on whether or not a position in your scan list has been reached.
Going for a smach state machine might be a bit overkill - a simple state_ integer member variable in a class would probably be enough.
The sequence of states could be something like: init -> pre_p1 -> at_pre1 -> post_pre1 -> pre_p2 -> at_p2 -> post_p2 -> .. etc (I'm assuming your robot is visiting the positions in sequence).
The callback registered for the sensor/right topic you are interested in is always active, but only handles incoming messages whenever the state machine is in the right states (so in at_pN states). If your program is in the right state, it "[..] get(s) the sensor data and add(s) a marker in the rviz", and then immediately transitions to the post_N state. In all other states, it does nothing but compare the current eef position to the scan list.
The decision to go from pre_p1 to at_p1 is dependent on the position of your manipulator's eef link. You could probably use MoveIts RobotState class for that, or alternatively, do the FK yourself, based on a joint_states topic. Then you update the state_ variable based on whether the robot is close enough to the current target point in the list.
As your state machine does not define any backward transitions, you probably don't need any filtering on the eef position: once you are in state at_pN you cannot go back to pre_pN. You could however add a proximity threshold which has to be reached before taking the transition to at_pN.
PS: for a sensor at 500Hz, it might make sense to disable your sensor/right subscriber when you are not in a at_pN state. You could use the pre_pN and post_pN states for that. Then take your measurements in at_pN. This would also remove the need to check the state_ variable in the sensor/right callback (as it is only active when needed).
Edit: updated based on clarification by OP in the comments.
