def __init__(self):
        """
        Constructor
        """
        self._sm = None

        smach.set_loggers(
            rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr  # hide SMACH transition log spamming
        )

        # set up proxys for sm <--> GUI communication
        # publish topics
        self._pub = ProxyPublisher({"flexbe/behavior_update": String, "flexbe/request_mirror_structure": Int32})

        self._running = False
        self._stopping = False
        self._active_id = 0
        self._starting_path = None
        self._current_struct = None

        self._outcome_topic = "flexbe/mirror/outcome"

        self._struct_buffer = list()

        # listen for mirror message
        self._sub = ProxySubscriberCached()
        self._sub.subscribe(self._outcome_topic, UInt8)
        self._sub.enable_buffer(self._outcome_topic)

        self._sub.subscribe("flexbe/mirror/structure", ContainerStructure, self._mirror_callback)
        self._sub.subscribe("flexbe/status", BEStatus, self._status_callback)
        self._sub.subscribe("flexbe/mirror/sync", BehaviorSync, self._sync_callback)
        self._sub.subscribe("flexbe/mirror/preempt", Empty, self._preempt_callback)
def stepCallback(ga_engine):
    generation = ga_engine.getCurrentGeneration()
    if generation % 10 == 0:
        best = ga.bestIndividual()
        G = convert_chromosome_pygraphviz(best, names_mapping, 
                                          transitions_mapping)
        G.write("/home/pezzotto/tmp/tmp.dot")
        smach.set_loggers(smach.loginfo,
                  smach.logwarn,
                  smach_grasp.null_print,
                  smach.logerr)
        score = eval_func(best)
        print "Score: ", score
        
        
    return False
    def __init__(self):
        '''
        Constructor
        '''
        self.be = None
        Logger.initialize()
        smach.set_loggers (
            rospy.logdebug, # hide SMACH transition log spamming
            rospy.logwarn,
            rospy.logdebug,
            rospy.logerr
        )

        #ProxyPublisher._simulate_delay = True
        #ProxySubscriberCached._simulate_delay = True

        # prepare temp folder
        rp = rospkg.RosPack()
        self._tmp_folder = "/tmp/flexbe_onboard"
        if not os.path.exists(self._tmp_folder):
            os.makedirs(self._tmp_folder)
        sys.path.append(self._tmp_folder)
        
        # prepare manifest folder access
        self._behavior_lib = BehaviorLibrary()
        
        self._pub = ProxyPublisher()
        self._sub = ProxySubscriberCached()

        self.status_topic = 'flexbe/status'
        self.feedback_topic = 'flexbe/command_feedback'

        self._pub.createPublisher(self.status_topic, BEStatus, _latch = True)
        self._pub.createPublisher(self.feedback_topic, CommandFeedback)

        # listen for new behavior to start
        self._running = False
        self._switching = False
        self._sub.subscribe('flexbe/start_behavior', BehaviorSelection, self._behavior_callback)

        # heartbeat
        self._pub.createPublisher('flexbe/heartbeat', Empty)
        self._execute_heartbeat()

        rospy.sleep(0.5) # wait for publishers etc to really be set up
        self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
        rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
Beispiel #4
0
    def __init__(self):
        '''
        Constructor
        '''
        self._sm = None

        smach.set_loggers(
            rospy.logdebug,  # hide SMACH transition log spamming
            rospy.logwarn,
            rospy.logdebug,
            rospy.logerr)

        # set up proxys for sm <--> GUI communication
        # publish topics
        self._pub = ProxyPublisher({
            'flexbe/behavior_update': String,
            'flexbe/request_mirror_structure': Int32
        })

        self._running = False
        self._stopping = False
        self._active_id = 0
        self._starting_path = None
        self._current_struct = None
        self._state_checksums = dict()

        self._outcome_topic = 'flexbe/mirror/outcome'

        self._struct_buffer = list()

        self._is_requesting_mirror = False

        # listen for mirror message
        self._sub = ProxySubscriberCached()
        self._sub.subscribe(self._outcome_topic, UInt8)
        self._sub.enable_buffer(self._outcome_topic)

        self._sub.subscribe('flexbe/mirror/structure', ContainerStructure,
                            self._mirror_callback)
        self._sub.subscribe('flexbe/status', BEStatus, self._status_callback)
        self._sub.subscribe('flexbe/mirror/sync', BehaviorSync,
                            self._sync_callback)
        self._sub.subscribe('flexbe/mirror/preempt', Empty,
                            self._preempt_callback)
def test():
    
    import smach_grasp
    from smach_grasp import (names_mapping, classes_mapping, transitions_mapping,
                         data_mapping, node_degrees, nodes_params)

    
    import graph_genome
    random.seed()
    
    smach.set_loggers(smach.loginfo,
                      smach_grasp.null_print,
                      smach_grasp.null_print,
                      smach_grasp.null_print)
    
    num_nodes = 20
    
    start = time.time()
    chromosome = graph_genome.GraphGenome(num_nodes, 
                                          node_degrees,
                                          nodes_params)
    chromosome.initialize()
    G = convert_chromosome_pygraphviz(chromosome, names_mapping, 
                                      transitions_mapping)
    G.write("/home/pezzotto/tmp/tmp.dot")
    
    sm = convert_chromosome_smach(chromosome, 
                                  names_mapping, 
                                  transitions_mapping, 
                                  classes_mapping,
                                  data_mapping)
    robot_state =  smach_grasp.RobotWorldState()
    sm.userdata.robot_state = robot_state
    
    try:
        outcome = sm.execute()
    except smach.exceptions.InvalidUserCodeError as err:
        print "Error: ", err.message
    else:
        print "Outcome: ", outcome
    print "Distance is: ",  smach_grasp.state_dist_gripper_object(sm.userdata.robot_state)
    print "Time: ", time.time()-start
def stepCallback(ga_engine):
    generation = ga_engine.getCurrentGeneration()
    if generation % freq_stats == 0:
        
        comm = MPI.COMM_WORLD
        if comm.rank == 0:
            migrator = ga_engine.migrationAdapter
            if migrator.all_stars is not None:                
                print "All stars: ", [i.score for i in migrator.all_stars]                
                if ga_engine.getMinimax() == Consts.minimaxType["maximize"]:                
                    best = max(migrator.all_stars, key = lambda x:x.score)
                else:
                    best = min(migrator.all_stars, key = lambda x:x.score)
                    
                dirname = ga_engine.getParam("dirname", "")
                wholepath = os.path.normpath(os.path.join(os.path.abspath(dirname),                                             
                                             "bestgenome.dat")
                                            )
                file = open(wholepath,"wb")
                cPickle.dump((best, all_mappings), 
                             file, 
                             protocol=cPickle.HIGHEST_PROTOCOL)
                file.close()
                smach.set_loggers(smach.loginfo,
                        smach.logwarn,
                        smach_stack.null_print,
                        smach.logerr)
#                score = eval_func(best)
                newbest = best.clone()
                newbest.evaluate()
                score = newbest.score
                print "Fitness: ", best.score
                print "Real Score: ", score
                print
                ga_engine.printTimeElapsed()
        
        
    return False
    def __init__(self):
        self.be = None
        Logger.initialize()
        self._tracked_imports = list()
        # hide SMACH transition log spamming
        smach.set_loggers(rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr)

        # prepare temp folder
        self._tmp_folder = tempfile.mkdtemp()
        sys.path.append(self._tmp_folder)
        rospy.on_shutdown(self._cleanup_tempdir)

        # prepare manifest folder access
        self._behavior_lib = BehaviorLibrary()

        # prepare communication
        self.status_topic = 'flexbe/status'
        self.feedback_topic = 'flexbe/command_feedback'
        self._pub = ProxyPublisher({
            self.feedback_topic: CommandFeedback,
            'flexbe/heartbeat': Empty
        })
        self._pub.createPublisher(self.status_topic, BEStatus, _latch=True)
        self._execute_heartbeat()

        # listen for new behavior to start
        self._running = False
        self._run_lock = threading.Lock()
        self._switching = False
        self._switch_lock = threading.Lock()
        self._sub = ProxySubscriberCached()
        self._sub.subscribe('flexbe/start_behavior', BehaviorSelection, self._behavior_callback)

        rospy.sleep(0.5)  # wait for publishers etc to really be set up
        self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
        rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
Beispiel #8
0
    def __init__(self):
        '''
        Constructor
        '''
        self.be = None
        self._current_behavior = None
        Logger.initialize()
        smach.set_loggers(
            rospy.logdebug,  # hide SMACH transition log spamming
            rospy.logwarn,
            rospy.logdebug,
            rospy.logerr)

        #ProxyPublisher._simulate_delay = True
        #ProxySubscriberCached._simulate_delay = True

        behaviors_package = "flexbe_behaviors"
        if rospy.has_param("~behaviors_package"):
            behaviors_package = rospy.get_param("~behaviors_package")
            rospy.loginfo("Using custom behaviors package: %s" %
                          behaviors_package)
        else:
            rospy.loginfo("Using default behaviors package: %s" %
                          behaviors_package)

        # prepare temp folder
        rp = rospkg.RosPack()
        self._tmp_folder = os.path.join(rp.get_path('flexbe_onboard'), 'tmp/')
        if not os.path.exists(self._tmp_folder):
            os.makedirs(self._tmp_folder)
        sys.path.append(self._tmp_folder)

        # prepare manifest folder access
        manifest_folder = os.path.join(rp.get_path(behaviors_package),
                                       'behaviors/')
        rospy.loginfo("Parsing available behaviors...")
        file_entries = [
            os.path.join(manifest_folder, filename)
            for filename in os.listdir(manifest_folder)
            if not filename.startswith('#')
        ]
        manifests = sorted([
            xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)
        ])
        self._behavior_lib = dict()
        for i in range(len(manifests)):
            try:
                m = ET.parse(manifests[i]).getroot()
            except ET.ParseError:
                rospy.logerr(
                    'Failed to parse behavior description xml file: "%s"' %
                    manifests[i])
            else:
                e = m.find("executable")
                self._behavior_lib[i] = {
                    "name": m.get("name"),
                    "package": e.get("package_path").split(".")[0],
                    "file": e.get("package_path").split(".")[1],
                    "class": e.get("class")
                }
                # rospy.loginfo("+++ " + self._behavior_lib[i]["name"])

        # enable automatic reloading of all subsequent modules on reload
        _reload_importer = ReloadImporter()
        _reload_importer.add_reload_path(self._tmp_folder)
        behaviors_folder = os.path.abspath(
            os.path.join(rp.get_path(self._behavior_lib[0]['package']), '..'))
        _reload_importer.add_reload_path(behaviors_folder)
        _reload_importer.enable()

        self._pub = ProxyPublisher()
        self._sub = ProxySubscriberCached()

        self.status_topic = 'flexbe/status'
        self.feedback_topic = 'flexbe/command_feedback'

        self._pub.createPublisher(self.status_topic, BEStatus, _latch=True)
        self._pub.createPublisher(self.feedback_topic, CommandFeedback)

        # listen for new behavior to start
        self._running = False
        self._switching = False
        self._sub.subscribe('flexbe/start_behavior', BehaviorSelection,
                            self._behavior_callback)

        # heartbeat
        self._pub.createPublisher('flexbe/heartbeat', Empty)
        self._execute_heartbeat()

        rospy.sleep(0.5)  # wait for publishers etc to really be set up
        self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
        rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
import cPickle
from optparse import OptionParser

from pyevolve import GSimpleGA
from pyevolve import G1DList
from pyevolve import Selectors
from pyevolve import Initializators, Mutators,  Consts
from pyevolve import DBAdapters
from pyevolve import Crossovers
from pyevolve import Consts

import graph_evolve
from graph_evolve import graph_genome, smach_stack
import smach
smach.set_loggers(smach_stack.null_print,
          smach_stack.null_print,
          smach_stack.null_print,
          smach_stack.null_print)
from graph_evolve.chromosome_smach import convert_chromosome_smach

import math
import numpy as np

#import graph_evolve

#from graph_evolve import graph_genome
import sys


from graph_evolve.smach_stack import (names_mapping, classes_mapping, transitions_mapping,
                         data_mapping, node_degrees, nodes_params)
from pyevolve import GSimpleGA
from pyevolve import G1DList
from pyevolve import Selectors
from pyevolve import Initializators, Mutators,  Consts
from pyevolve import DBAdapters
from pyevolve import Crossovers

import graph_evolve
from graph_evolve import graph_genome
import smach
from graph_evolve import smach_grasp
from graph_evolve.chromosome_smach import convert_chromosome_smach
from graph_evolve.chromosome_smach import convert_chromosome_pygraphviz

smach.set_loggers(smach.loginfo,
                  smach_grasp.null_print,
                  smach_grasp.null_print,
                  smach_grasp.null_print)

names_mapping = ["RecogniseObject",
                 "MoveToObject",
                 "GraspObject",
                 "CheckSuccess",
                 "MoveGripperToParam"]
classes_mapping = {"RecogniseObject" : smach_grasp.RecogniseObject,
                 "MoveToObject": smach_grasp.MoveGripperTo,
                 "GraspObject": smach_grasp.GraspObject,
                 "CheckSuccess": smach_grasp.CheckSuccess,
                 "MoveGripperToParam" : smach_grasp.MoveGripperToParam}
transitions_mapping = {'RecogniseObject':["success",],
                       'MoveToObject':["success", "failure",],
                       'GraspObject':["success", "failure",],
Beispiel #11
0
Basic finite state machine to search for an object, and approach it.
The robot aligns with the object and move towards the edge of the table,
until it stops.
"""

import roslib; 
roslib.load_manifest('learn_actions')
import smach
import rospy
import pr2_control_utilities
import tabletop_actions.object_detector as object_detector
import tabletop_actions.pushers as pushers
from learn_actions.msg import ObjectDiscovery

smach.set_loggers(rospy.loginfo,
                  rospy.logwarn,
                  lambda s: None,
                  rospy.logerr)
                
def publish_result(publisher, box, table, whicharm, torso_joint):
    
    msg = ObjectDiscovery()
    msg.header.stamp = rospy.Time.now()

    msg.object_pose.x = box.pose.pose.position.x
    msg.object_pose.y = box.pose.pose.position.y
    msg.object_pose.z = box.pose.pose.position.z
    
    msg.table = table
    
    msg.grasping_result = whicharm
    msg.torso_joint = torso_joint
Beispiel #12
0
# SMACH
import smach

__all__ = ['set_preempt_handler',
        'ActionServerWrapper',
        'IntrospectionClient','IntrospectionServer',
        'SimpleActionState',
        'ServiceState',
        'MonitorState',
        'ConditionState']

# Setup smach-ros interface
smach.set_loggers(
        rospy.loginfo,
        rospy.logwarn,
        rospy.logdebug,
        rospy.logerr)

smach.set_shutdown_check(rospy.is_shutdown)

### Core classes
from util import set_preempt_handler

### Top-level Containers / Wrappers
from action_server_wrapper import ActionServerWrapper
from introspection import IntrospectionClient, IntrospectionServer

### State Classes
from simple_action_state import SimpleActionState
from service_state import ServiceState
import cPickle
from optparse import OptionParser

from pyevolve import GSimpleGA
from pyevolve import G1DList
from pyevolve import Selectors
from pyevolve import Initializators, Mutators,  Consts
from pyevolve import DBAdapters
from pyevolve import Crossovers
from pyevolve import Consts

import graph_evolve
from graph_evolve import graph_genome, smach_grasp
import smach
smach.set_loggers(smach_grasp.null_print,
          smach_grasp.null_print,
          smach_grasp.null_print,
          smach_grasp.null_print)
from graph_evolve.chromosome_smach import convert_chromosome_smach

import math
import numpy as np

#import graph_evolve

#from graph_evolve import graph_genome
import sys


from graph_evolve.smach_grasp import (names_mapping, classes_mapping, transitions_mapping,
                         data_mapping, node_degrees, nodes_params)
Beispiel #14
0
#!/usr/bin/env python
import rospy
import rosunit
import unittest
import re

from .logger import Logger
from .test_interface import TestInterface
from .test_context import TestContext, LaunchContext
from .data_provider import DataProvider

import smach
# hide SMACH transition log spamming
smach.set_loggers(rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr)


class Tester(object):
    def __init__(self):
        self._tests = dict()

    def run_test(self, name, config):
        try:
            self._verify_config(config)
        except Exception as e:
            Logger.print_title(name, 'Invalid', None)
            Logger.print_error('invalid test specification!\n\t%s' % str(e))
            Logger.print_result(name, False)
            self._tests['test_%s_pass' % name] = self._test_config_invalid(
                str(e))
            return 0
from graph_evolve import graph_genome
import smach
from graph_evolve import smach_stack
from graph_evolve.chromosome_smach import convert_chromosome_smach

import cPickle
from mpi4py import MPI
from pyevolve import MpiMigration
import math
import random
import time
import os
import datetime

smach.set_loggers(smach.loginfo,
                  smach_stack.null_print,
                  smach_stack.null_print,
                  smach_stack.null_print)

from graph_evolve.smach_stack import (names_mapping, classes_mapping, transitions_mapping,
                         data_mapping, node_degrees, nodes_params)

all_mappings = (names_mapping,
                classes_mapping,
                transitions_mapping,                
                data_mapping,
                node_degrees,
                nodes_params)

def single_try(chromosome, **args):
    sm = convert_chromosome_smach(chromosome, 
                                  names_mapping, 
    def __init__(self):
        '''
        Constructor
        '''
        self.be = None
        Logger.initialize()
        smach.set_loggers (
            rospy.logdebug, # hide SMACH transition log spamming
            rospy.logwarn,
            rospy.logdebug,
            rospy.logerr
        )

        #ProxyPublisher._simulate_delay = True
        #ProxySubscriberCached._simulate_delay = True

        behaviors_package = "flexbe_behaviors"
        if rospy.has_param("~behaviors_package"):
            behaviors_package = rospy.get_param("~behaviors_package")
            rospy.loginfo("Using custom behaviors package: %s" % behaviors_package)
        else:
            rospy.loginfo("Using default behaviors package: %s" % behaviors_package)

        # prepare temp folder
        rp = rospkg.RosPack()
        self._tmp_folder = os.path.join(rp.get_path('flexbe_onboard'), 'tmp/')
        if not os.path.exists(self._tmp_folder):
            os.makedirs(self._tmp_folder)
        sys.path.append(self._tmp_folder)

        # prepare manifest folder access
        manifest_folder = os.path.join(rp.get_path(behaviors_package), 'behaviors/')
        rospy.loginfo("Parsing available behaviors...")
        file_entries = [os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#')]
        manifests = sorted([xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)])
        self._behavior_lib = dict()
        for i in range(len(manifests)):
            m = ET.parse(manifests[i]).getroot()
            e = m.find("executable")
            self._behavior_lib[i] = {"name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class")}
#            rospy.loginfo("+++ " + self._behavior_lib[i]["name"])

        self._pub = ProxyPublisher()
        self._sub = ProxySubscriberCached()

        self.status_topic = 'flexbe/status'
        self.feedback_topic = 'flexbe/command_feedback'

        self._pub.createPublisher(self.status_topic, BEStatus, _latch = True)
        self._pub.createPublisher(self.feedback_topic, CommandFeedback)

        # listen for new behavior to start
        self._running = False
        self._switching = False
        self._sub.subscribe('flexbe/start_behavior', BehaviorSelection, self._behavior_callback)

        # heartbeat
        self._pub.createPublisher('flexbe/heartbeat', Empty)
        self._execute_heartbeat()

        rospy.sleep(0.5) # wait for publishers etc to really be set up
        self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
        rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
Beispiel #17
0
"""
Basic finite state machine to search for an object, and approach it.
The robot aligns with the object and move towards the edge of the table,
until it stops.
"""

import roslib
roslib.load_manifest('learn_actions')
import smach
import rospy
import pr2_control_utilities
import tabletop_actions.object_detector as object_detector
import tabletop_actions.pushers as pushers
from learn_actions.msg import ObjectDiscovery

smach.set_loggers(rospy.loginfo, rospy.logwarn, lambda s: None, rospy.logerr)


def publish_result(publisher, box, table, whicharm, torso_joint):

    msg = ObjectDiscovery()
    msg.header.stamp = rospy.Time.now()

    msg.object_pose.x = box.pose.pose.position.x
    msg.object_pose.y = box.pose.pose.position.y
    msg.object_pose.z = box.pose.pose.position.z

    msg.table = table

    msg.grasping_result = whicharm
    msg.torso_joint = torso_joint
from simulators import multi_svm 
sys.modules["multi_svm"] = multi_svm
from graph_evolve import graph_genome
import smach
from graph_evolve import  smach_svm_grasp

import cPickle
import random
import time
import os
import datetime
from graph_evolve import chromosome_smach

def null_print(msg): pass
smach.set_loggers(null_print,
                  null_print,
                  null_print,
                  null_print)


experiment = smach_svm_grasp.ExperimentSetup()

experiment.max_transitions = 15

#experiment.tableSVR_path = "/home/pezzotto/Logs/TryToPushAndActionConsequence/table_changesSVR.pkl"
#experiment.objectSVR_path = "/home/pezzotto/Logs/TryToPushAndActionConsequence/object_changesSVR.pkl"
#experiment.reachableSVC_path = "/home/pezzotto/Logs/TryToPushAndActionConsequence/reachableSVC.pkl"
#experiment.testingdata_path = "/home/pezzotto/Logs/TryToPushAndActionConsequence/given_fsm_2011-11-25-18-23-28.pkl"

experiment.pop_size = 200
experiment.stop_elitism = False
experiment.poolsize = 20
Beispiel #19
0
import smach

__all__ = ['set_preempt_handler',
        'RosState',
        'SmachNode',
        #'ActionServerWrapper', ### Wraps a SMACH SM into an Action server
        'IntrospectionClient','IntrospectionServer',
        'SimpleActionState',
        'ServiceState',
        'MonitorState',
        'ConditionState']

# Setup smach-ros interface
smach.set_loggers(
        rclpy.logging.get_logger(__name__).info,
        rclpy.logging.get_logger(__name__).warn,
        rclpy.logging.get_logger(__name__).debug,
        rclpy.logging.get_logger(__name__).error)

smach.set_shutdown_check(lambda: not rclpy.ok())

### Core classes
from .util import set_preempt_handler

### Top-level Containers / Wrappers
#from .action_server_wrapper import ActionServerWrapper
from .introspection import IntrospectionClient, IntrospectionServer

### State Classes
from .ros_state import RosState
from .node import SmachNode