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