def test_link_pose(self):
print "LNK\n"
rospy.init_node(NAME, anonymous=True)
self.link_pose_topic = rospy.get_param(self.link_pose_topic_name,
self.link_pose_topic)
self.valid_pose_topic = rospy.get_param(self.valid_pose_topic_name,
self.valid_pose_topic)
self.min_link_samples = rospy.get_param(self.min_link_samples_topic,
self.min_link_samples)
self.min_valid_samples = rospy.get_param(self.min_valid_samples_topic,
self.min_valid_samples)
self.tolerance = rospy.get_param(self.tolerance_topic, self.tolerance)
self.max_error = rospy.get_param(self.max_error_topic, self.max_error)
self.test_duration = rospy.get_param(self.test_duration_topic,
self.test_duration)
self.test_start_time = rospy.get_param(self.test_start_time_topic,
self.test_start_time)
while not rospy.is_shutdown() and time.time() < self.test_start_time:
rospy.stdinfo("Waiting for test to start at time [%s]" %
self.test_start_time)
time.sleep(0.1)
print "subscribe"
rospy.Subscriber(self.link_pose_topic, Odometry, self.linkP3dInput)
rospy.Subscriber(self.valid_pose_topic, Odometry, self.validP3dInput)
start_t = time.time()
timeout_t = start_t + self.test_duration
while not rospy.is_shutdown(
) and not self.success and time.time() < timeout_t:
self.checkPose()
time.sleep(0.1)
self.assert_(self.success)
def test_bumper(self):
print "LNK\n"
rospy.init_node(NAME, anonymous=True)
self.bumper_topic = rospy.get_param(self.bumper_topic_name,
self.bumper_topic)
self.min_samples = rospy.get_param(self.min_samples_topic,
self.min_samples)
self.test_duration = rospy.get_param(self.test_duration_topic,
self.test_duration)
self.test_start_time = rospy.get_param(self.test_start_time_topic,
self.test_start_time)
while not rospy.is_shutdown() and time.time() < self.test_start_time:
rospy.stdinfo("Waiting for test to start at time [%s]" %
self.test_start_time)
time.sleep(0.1)
print "subscribe"
rospy.Subscriber(self.bumper_topic, ContactsState,
self.bumperStateInput)
start_t = time.time()
timeout_t = start_t + self.test_duration
while not rospy.is_shutdown(
) and not self.success and time.time() < timeout_t:
self.checkContact()
time.sleep(0.1)
self.assert_(self.success)
def test_link_pose(self):
print "LNK\n"
rospy.init_node(NAME, anonymous=True)
self.link_pose_topic = rospy.get_param(self.link_pose_topic_name,self.link_pose_topic);
self.valid_pose_topic = rospy.get_param(self.valid_pose_topic_name,self.valid_pose_topic);
self.min_link_samples = rospy.get_param(self.min_link_samples_topic,self.min_link_samples);
self.min_valid_samples = rospy.get_param(self.min_valid_samples_topic,self.min_valid_samples);
self.tolerance = rospy.get_param(self.tolerance_topic,self.tolerance);
self.max_error = rospy.get_param(self.max_error_topic,self.max_error);
self.test_duration = rospy.get_param(self.test_duration_topic,self.test_duration);
self.test_start_time = rospy.get_param(self.test_start_time_topic,self.test_start_time);
while not rospy.is_shutdown() and time.time() < self.test_start_time:
rospy.stdinfo("Waiting for test to start at time [%s]"% self.test_start_time)
time.sleep(0.1)
print "subscribe"
rospy.Subscriber(self.link_pose_topic, Odometry, self.linkP3dInput)
rospy.Subscriber(self.valid_pose_topic, Odometry, self.validP3dInput)
start_t = time.time()
timeout_t = start_t + self.test_duration
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
self.checkPose()
time.sleep(0.1)
self.assert_(self.success)
def test_bumper(self):
print "LNK\n"
rospy.init_node(NAME, anonymous=True)
self.bumper_topic = rospy.get_param(self.bumper_topic_name,self.bumper_topic);
self.min_samples = rospy.get_param(self.min_samples_topic,self.min_samples);
self.test_duration = rospy.get_param(self.test_duration_topic,self.test_duration);
self.test_start_time = rospy.get_param(self.test_start_time_topic,self.test_start_time);
while not rospy.is_shutdown() and time.time() < self.test_start_time:
rospy.stdinfo("Waiting for test to start at time [%s]"% self.test_start_time)
time.sleep(0.1)
print "subscribe"
rospy.Subscriber(self.bumper_topic, ContactsState, self.bumperStateInput)
start_t = time.time()
timeout_t = start_t + self.test_duration
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
self.checkContact()
time.sleep(0.1)
self.assert_(self.success)
