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(InitializePath, self).__init__(
outcomes=_outcomes,
input_keys=[
"pathId",
"times",
"transitionIds",
"endStateIds",
"currentSection",
],
output_keys=[
"transitionId", "endStateId", "currentSection", "duration"
],
)
self.targetPub = ros_tools.createPublishers("/hpp/target",
self.hppTargetPubDict)
self.serviceProxies = ros_tools.createServiceProxies(
"", InitializePath.serviceProxiesDict)
self.hppclient = HppClient(context="corbaserver")
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, hppclient):
super(InitializePath, self).__init__(
outcomes=["finished", "move_base", "next"],
input_keys=[
"pathId",
"times",
"transitionIds",
"endStateIds",
"currentSection",
],
output_keys=[
"currentSection", "times", "pathId", "transitionId",
"endStateId", "duration"
],
)
self.status = status
self.targetPub = ros_tools.createPublishers("/hpp/target",
self.hppTargetPubDict)
self.serviceProxies = ros_tools.createServiceProxies(
"", InitializePath.serviceProxiesDict)
self.hppclient = hppclient
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