Ask Your Question
0

How to subsribe to a ROS topic for several seperate times?

asked 2014-08-25 16:48:32 -0500

guodi gravatar image

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. Here is my code:

def addMarkerCallback(msg):
    draw_functions = DrawFunctions('visualisation_marker')
    if msg.data:
        draw_functions.draw_rviz_sphere(0.02)
    else:
        print 'no data'
rospy.init_node("sensor_marker", anonymous = True)

for item in scanlist:    
    moveit_cmd.go(item, wait=True)
    sub1 = rospy.Subscriber('sensor/right', SensorData, addMarkerCallback)   
    rospy.spin()
    print 'go finished'

However when I run the code, the problem is the loop will always stay in the first iteration, so the robot will not go to the other positions in the scanlist. I guess it is the problem of rospy.spin(). Could anyone please tell me how to solve this problem...Thanks a lot!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2014-08-26 14:10:25 -0500

gvdhoorn gravatar image

updated 2014-08-27 01:50:06 -0500

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.

edit flag offensive delete link more

Comments

As a possible refinement, you could just go for 3 states and a counter: init, pre_p, at_p and scan_list_pointer_. In init, set scan_list_pointer_ = 0, go to pre_p. Now base transition to at_p on proximity to scan_list[scan_list_pointer_]. In at_p, publish RViz marker, increment scan_list_pointer_ and transition immediately to pre_p. Return, repeat (but now scan_list[scan_list_pointer_] points to the next point automatically).

gvdhoorn gravatar image gvdhoorn  ( 2014-08-26 14:13:32 -0500 )edit

Thanks a lot! Actually I am trying to let the robot gripper pass the five positions in the scanlist successively. And the sensor embedded in the finger are giving sensor data continuously in 500Hz. I would like to check the sensor data in each position and draw a marker in the rviz...so do you know how to do that then....Thank you!

guodi gravatar image guodi  ( 2014-08-26 23:55:37 -0500 )edit

Ah, so sensor/right is the sensor data you want to selectively record? I was under the impression that sensor/right provided you with a position. I'll update my answer accordingly.

gvdhoorn gravatar image gvdhoorn  ( 2014-08-27 01:43:21 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2014-08-25 16:48:32 -0500

Seen: 160 times

Last updated: Aug 27 '14