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')
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')
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",],
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
# 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)
#!/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')
"""
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
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
