ROS SMACH: Change goal in each iteration step

edit

Hello,

I am trying to grasp objects from a bin using SMACH in ROS-Melodic on Ubuntu 18.04. Everything is working smoothly. The only issue I am having is the following: I have a demo program in which I am removing a bin from a shelf, placing it on the desk of my mobile platform and then I am starting my object-detection pipeline in order to remove screws from the bin (classical bin-picking-scenario). Another component on my platform is a setplate in which I want to place grasped screws in preformed cavities. Each cavity can contain one screw (you can call this sort of a pre-montage).

For all of this, I am using SMACH. In order to pick the 3 screws, I am using a SMACH-Iterator, as described in the tutorial (http://wiki.ros.org/smach/Tutorials/I...). So what I want to do is to place each screw in a different cavity, but when using the iterator, my program obviously always places each screw in the same cavity.

My question is: Is there any way I can change the goal of my ROS-action, which I am calling from SMACH and in which I am navigating to a cavity, for each loop-iteration (3 in total) )in my SMACH-Iterator ? I've been trying to use a callback from outside of the iterator and within in combination with a simple global "counter"-variable, but this always results in the THIRD cavity to be selected. So I guess the three iterations are constructed before execution, but then there still has to be a way .. maybe my problem is quite simple, any sort of help is appreciated!

Here is an extraction from my code, which I am using to pick 3 screws. I am using ROS-Actions below:

# Execution:
def main():

# Register state machine in roscore
rospy.init_node('forobotics_state_machine')

# Add outcommented state-machine here
forobotics_sm = StateMachine(outcomes=['succeeded','aborted','preempted'])

counter = 0

with forobotics_sm:

    StateMachine.add('Roboter_in_scanPosition_fahren', SimpleActionState('absolute_joints_action', Absolute_JointsAction, 
                    goal=param_scan), {'succeeded':'ITERATOR'})

    # Iteration: Grip 3 times, is added to the main state-machine below
    forobotics_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
                             input_keys = ['dummy_in'],
                             it = lambda:range(0,3),
                             output_keys = ['dummy_out'],
                             it_label = 'part',
                             exhausted_outcome = 'succeeded')

    with forobotics_it:

        forobotics_container = StateMachine(outcomes=['succeeded', 'aborted', 'preempted', 'continue'])

        with forobotics_container:

            print("counter: ", counter)

            def legen_cb(userdata, goal):

                legen_goal = Absolute_JointsGoal()

                if counter == 0:

                    legen_goal = param_legen0

                elif counter == 1:

                    legen_goal = param_legen1

                elif counter  == 2:

                    legen_goal = param_legen2

                return legen_goal


            StateMachine.add('Pose_von_pointnet_holen', Pointnet_tf(),
                            transitions={'succeeded':'Zu_ober_greifpose_fahren', 'aborted':'aborted'},
                            remapping={'point1':'data1', 'point2':'data2', 'point3':'data3', 'distance1':'data4', 'distance2':'data5', 'distance3':'data6', 'direction1':'data7', 'direction2':'data8', 'direction3':'data9', 'count1':'data10', 'count2':'data11', 'count3':'data12'})

            StateMachine.add('Zu_ober_greifpose_fahren', ServiceState('/linear_pose_server/linear_pose', LinearPose,
                            # request=LinearPoseRequest(direction = point, distance=distance, richtung = direction, size=anzahl)), 
                            request_slots = ['direction', 'distance', 'richtung', 'size']),
                            transitions={'succeeded':'Greifen_oeffnen'},
                            remapping = {'direction':'data1', 'distance':'data4', 'richtung':'data7', 'size':'data10'})

            StateMachine.add('Greifen_oeffnen', SimpleActionState('ec_36_12_34_01_ptm_gripper_1/retain', retainAction, 
                            goal=param_retain0), {'succeeded':'zu_objekt'} )

            StateMachine.add('zu_objekt', ServiceState('/linear_pose_server/linear_pose', LinearPose,
                            #request=LinearPoseRequest(direction = point_1, distance=distance_1, richtung = direction_1, size=anzahl_1))    
                            request_slots = ['direction', 'distance', 'richtung', 'size']),
                            transitions={'succeeded':'Objekt_Greifen'},
                            remapping ...