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.