Smach and OpenCV (passing image data between states)

edit

Hi guys,

currently I'm working with the smach tutorial here: http://wiki.ros.org/smach/Tutorials/S...

I added a new class called getNewFrame where I subscribe to the camera's topic and save image files to filesystem. Now I want to add a new state for detecting a Marker within the frame. By using smach this means that I have to create a new class for the new state - done. Inside the class I need to do all the OpenCV stuff (load last frame, processing). Finally I want to have a state for determing the outcome like if is_marker_found == True: return outcome1 and so on. I think you got it.

But after trying I got a lot of errors... I can not pass the frame from one to another class like you would normally do in python. I'm not really into smach so I hope the community can tell me IF and HOW I can use smach to pass OpenCV Image data so a state called "get new frame" can pass the frame to the next state "detectMarker".

Thanks and hope you can help me :-) Have a nice day!

This code will not be executable without editing the placeholder #####

# define state getNewFrame
class getNewFrame(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['haveNewFrame', 'haveNotFrame'])

        self.image_topic = "/camera/image_raw"
        rospy.Subscriber(self.image_topic, Image, self.image_callback)

        self.is_newFrame = False
        # Instantiate CvBridge
        self.bridge = CvBridge()
        lst_frame = []

    def image_callback(self, msg):
        try:
            # Convert your ROS Image message to OpenCV2
            lst_frame = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            self.is_newFrame = True
        except CvBridgeError, e:
            print(e)
            self.is_newFrame = False
        else:
            # Save your OpenCV2 image as a jpeg 
            cv2.imwrite('#####', lst_frame)

    def execute(self, userdata):
        rospy.loginfo('Executing state GNF')
        if self.is_newFrame == True:          
            return 'haveNewFrame'
        else:
            return 'haveNotFrame'

class #####(object):
    def __init__(self):
        self.i = 0

    def run(self):
        rospy.init_node('#####')
        sm = smach.StateMachine(outcomes=['#####'])

        # Open the container
        with sm:

            smach.StateMachine.add('FMIF', findMarkerInFrame(), transitions={'markerFound':'GMPTS', 'markerNotFound':'GNF'})
            smach.StateMachine.add('GNF', getNewFrame(), transitions={'haveNewFrame':'FMIF', 'haveNotFrame':'GNF'})
            smach.StateMachine.add('GMPTS', getMarkerPts(), transitions={'haveMarkerPts':'AKB', 'haveNoMarkerPts':'GNF'})
            smach.StateMachine.add('AKB', alignKobukiToMarker(), transitions={'isAligned':'DF', 'isNotAligned':'GNF'})
            smach.StateMachine.add('DF', driveBaseForward(), transitions={'is_finishedDrive':'AKB', 'stop':'GNF'})

        sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
        sis.start()

        # Execute SMACH plan
        outcome = sm.execute()

        rospy.spin()
        sis.stop()    

if __name__ == '__main__':
     ##### = #####()
     #####.#####()