def __init__(self): from threading import Lock # Client to HPP self.client = HppClient(context="simulation") self.mutex = Lock() # Initialize configuration of robot and objects from HPP hpp = self.client.hpp() self.q = hpp.problem.getInitialConfig() self.configSize = len(self.q) # Compute ranks of joints in HPP configuration self.computeRanksInConfiguration() self.initializeObjects(self.q) self.initializeSoT2HPPconversion() self.transitionName = "" self.q_rhs = None self.objectPublisher = dict() # Create an object publisher by object prefix = rospy.get_param('tf_prefix', 'sim_') for o in self.objects: self.objectPublisher [o] = PublishObjectPose \ ('world', prefix+o+'/base_link') self.objectPublisher[o].broadcast(self.objectPose[o]) pose = self.objectPose[o] r = self.rankInConfiguration[o + '/root_joint'] self.q[r:r + 7] = pose # Create subscribers self.subscribers = ros_tools.createSubscribers(self, "/agimus", self.subscriberDict) # Create mapping from transition names to graph component id in HPP self.graphDict = createGraphDict(self.client)
def __init__ (self): self.discretization = None rospy.on_shutdown (self._ros_shutdown) super(HppOutputQueue, self).__init__ (connect=False) ## Publication frequency self.dt = rospy.get_param ("/sot_controller/dt") self.frequency = 1. / self.dt # Hz self.subscribers = ros_tools.createSubscribers (self, "", self.subscribersDict) self.services = ros_tools.createServices (self, "", self.servicesDict) self.pubs = ros_tools.createPublishers ("/hpp/target", self.publishersDist) self.times = None
def __init__ (self, topicStateFeedback): super(PlanningRequestAdapter, self).__init__ (connect=False) self.subscribers = ros_tools.createSubscribers (self, "/agimus", self.subscribersDict) self.publishers = ros_tools.createPublishers ("/agimus", self.publishersDict) self.topicStateFeedback = topicStateFeedback self.topicEstimation = "/agimus/estimation/semantic" self.q_init = None self.init_mode = "user_defined" self.get_current_state = None self.get_estimation = None self.tfListener = TransformListener() self.mutexSolve = Lock() if not rospy.has_param ("/motion_planning/tf/world_frame_name"): rospy.set_param ("/motion_planning/tf/world_frame_name", "world") self.robot_name = "" self.robot_base_frame = None
def __init__(self): super(PlayPath, self).__init__( outcomes=_outcomes, input_keys=[ "transitionId", "endStateId", "duration", "queue_initialized" ], output_keys=["queue_initialized"], ) self.targetPub = ros_tools.createPublishers("/hpp/target", self.hppTargetPubDict) self.subscribers = ros_tools.createSubscribers(self, "", self.subscribersDict) self.serviceProxies = ros_tools.createServiceProxies( "", PlayPath.serviceProxiesDict) self.path_published = False self.event_done = False self.event_error = None self.interruption = None self.event_done_count = 0
def __init__(self, status): super(PlayPath, self).__init__( outcomes=["succeeded", "aborted", "preempted"], input_keys=["transitionId", "endStateId", "duration", "currentSection", "queue_initialized"], output_keys=["queue_initialized"], ) self.status = status self.targetPub = ros_tools.createPublishers( "/hpp/target", self.hppTargetPubDict ) self.subscribers = ros_tools.createSubscribers(self, "", self.subscribersDict) self.serviceProxies = ros_tools.createServiceProxies( "", PlayPath.serviceProxiesDict ) self.path_published = False self.event_done = None self.event_error = None self.event_done_min_time = 0 self.interruption = None self.event_done_count = 0