def __init__(self,
continuous_estimation=False,
joint_states_topic="/joint_states",
visual_tags_enabled=True):
super(Estimation, self).__init__(context="estimation")
self.locked_joints = []
self.tf_pub = TransformBroadcaster()
self.tf_root = "world"
self.mutex = Lock()
self.robot_name = rospy.get_param("~robot_name", "")
self.last_stamp_is_ready = False
self.last_stamp = rospy.Time.now()
self.last_visual_tag_constraints = list()
self.current_stamp = rospy.Time.now()
self.current_visual_tag_constraints = list()
self.visual_tags_enabled = visual_tags_enabled
self.continuous_estimation(SetBoolRequest(continuous_estimation))
self.estimation_rate = 50 # Hz
self.subscribers = ros_tools.createSubscribers(self, "/agimus",
self.subscribersDict)
self.publishers = ros_tools.createPublishers("/agimus",
self.publishersDict)
self.services = ros_tools.createServices(self, "/agimus",
self.servicesDict)
self.joint_state_subs = rospy.Subscriber(joint_states_topic,
JointState,
self.get_joint_state)
def _createTopics (self, namespace, topics, subscribe):
"""
\param subscribe boolean whether this node should subscribe to the topics.
If False, this node publishes to the topics.
"""
if subscribe:
return ros_tools.createSubscribers (self, namespace, topics)
else:
return ros_tools.createPublishers (namespace, topics)
def __init__(self, topicStateFeedback):
super(PlanningRequestAdapter, self).__init__()
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.setHppUrl()
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(HppOutputQueue, self).__init__()
## Publication frequency
self.frequency = 1. / rospy.get_param("/sot_controller/dt") # Hz
## Queue size should be adapted according to the queue size in SoT
self.queue_size = 1024
self.queue = Queue.Queue(self.queue_size)
self.setJointNames(
SetJointNamesRequest(self._hpp().robot.getJointNames()))
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.reading = False
self.firstMsgs = None
self.resetTopics()