def test_expected(self):
pub_pose_msg = PoseStamped()
pub_pose_msg.header.stamp = rospy.Time.now()
pub_pose_msg.pose.position.x = 1
pub_pose_msg.pose.position.y = 1
pub_pose_msg.pose.position.z = 1
pub_pose_msg.pose.orientation.w = 1
self.pub_3.publish(pub_pose_msg)
# pub start signal
pub_signal = BoolStamped()
pub_signal.header.stamp = rospy.Time.now()
pub_signal.data = True
self.pub_1.publish(pub_signal)
rospy.sleep(1.0)
pub_pose_msg = PoseStamped()
pub_pose_msg.header.stamp = rospy.Time.now()
pub_pose_msg.pose.position.x = 4
pub_pose_msg.pose.position.y = 5
pub_pose_msg.pose.orientation.w = 1
pub_pose_msg.pose.orientation.z = 1
self.pub_3.publish(pub_pose_msg)
pub_signal.header.stamp = rospy.Time.now()
pub_signal.data = True
self.pub_2.publish(pub_signal)
while self.delta_pose is None and self.delta_angle is None:
time.sleep(0.1)
self.assertEqual(self.delta_pose, 5.0,
'{} is not equal to 5.0'.format(self.delta_pose))
self.assertEqual(self.delta_angle, 90.0,
'{} is not equal to 5.0'.format(self.delta_angle))
def test_brake(self):
while self.counter < 3:
self.counter += 1
pub_msg = BoolStamped()
pub_msg.data = True
pub_msg.header.stamp = rospy.Time.now()
# pub /signal/ui/start_test
self.pub_start.publish(pub_msg)
# testing if robot did not really reach max speed
if self.counter == 1:
while self.result_max is None or self.result_max.data == False:
rospy.sleep(0.01)
vel = Twist()
if self.result_max.data == True:
vel.linear.x = 0.7
for i in range(100):
rospy.sleep(0.01)
self.pub_vel.publish(vel)
# wait for result
while self.result_time is None:
rospy.sleep(0.01)
self.assertAlmostEqual(self.result_time, \
self.result_time, \
msg = 'Test{}: braking time: {}, expect {}'.format(self.counter, self.result_time, self.result_time), \
delta= 0.1)
self.assertNotAlmostEqual(self.result_time, \
0, \
msg = 'Test{}: braking time: {}, expect not ZERO'.format(self.counter, self.result_time), \
delta= 0.0)
while self.result_distance is None:
rospy.sleep(0.01)
self.assertAlmostEqual(self.result_distance, \
self.result_distance, \
msg = 'Test{}: braking distance: {}, expect {}'.format(self.counter, self.result_distance, self.result_distance), \
delta= 0.1)
self.assertNotAlmostEqual(self.result_distance, \
0, \
msg = 'Test{}: braking distance: {}, expect not ZERO'.format(self.counter, self.result_distance), \
delta= 0.0)
# pub /signal/ui/reset_test
rospy.sleep(10)
pub_msg.data = True
pub_msg.header.stamp = rospy.Time.now()
self.pub_reset.publish(pub_msg)
rospy.sleep(1)
self.result_distance = None
self.result_time = None
def test_expected(self):
while self.counter < 2:
self.counter += 1
pub_msg = BoolStamped()
pub_msg.data = True
pub_msg.header.stamp = rospy.Time.now()
# pub /signal/ui/start_test
self.pub_start.publish(pub_msg)
# wait for result
while self.result_start is None:
rospy.sleep(0.01)
self.assertTrue(self.result_start,
msg='Test {}: did not start'.format(self.counter))
while self.result_time is None or self.result_completed is None:
rospy.sleep(0.01)
self.assertAlmostEqual(self.result_time, \
self.result_time, \
msg='Test {}: reached goal time: {}, expect {}'.format(self.counter, self.result_time, self.result_time), \
delta= 0.01)
self.assertNotAlmostEqual(self.result_time, \
0, \
msg = 'Test{}: reached goal time: {}, expect not ZERO'.format(self.counter, self.result_time), \
delta= 0.0)
self.assertTrue(self.result_completed,
msg='Test {}: did not complete'.format(
self.counter))
rospy.sleep(1)
# move robot away from goal
goal_msg = PoseStamped()
goal_msg.header.frame_id = "map"
goal_msg.pose.position.x = -0.26
goal_msg.pose.position.y = -1.6
goal_msg.pose.orientation.w = 1
self.pub_goal.publish(goal_msg)
rospy.sleep(7)
# pub /signal/ui/reset_test
pub_msg.data = True
pub_msg.header.stamp = rospy.Time.now()
self.pub_reset.publish(pub_msg)
rospy.sleep(1)
self.assertFalse(self.result_completed,
msg='Test {}: is not reset'.format(self.counter +
1))
self.result_time = None
self.result_completed = None
self.result_start = None
def test_expected(self):
while self.counter < 3:
self.counter += 1
pub_msg_signal = BoolStamped()
pub_msg_signal.data = True
pub_msg_signal.header.stamp = rospy.Time.now()
# pub /signal/ui/start_test
self.pub_start.publish(pub_msg_signal)
# pub /emergency_stop_state
rospy.sleep(8)
pub_msg_emergency = EmergencyStopState()
pub_msg_emergency.emergency_state = 1
self.pub_emergency.publish(pub_msg_emergency)
# wait for result
while self.result_time is None:
rospy.sleep(0.01)
self.assertAlmostEqual(self.result_time, \
self.result_time, \
msg = 'Test{}: braking time: {}, expect {}'.format(self.counter, self.result_time, self.result_time), \
delta= 0.1)
self.assertNotAlmostEqual(self.result_time, \
0, \
msg = 'Test{}: braking time: {}, expect not ZERO'.format(self.counter, self.result_time), \
delta= 0.0)
while self.result_distance is None:
rospy.sleep(0.01)
self.assertAlmostEqual(self.result_distance, \
self.result_distance, \
msg = 'Test{}: braking distance: {}, expect {}'.format(self.counter, self.result_distance, self.result_distance), \
delta= 0.1)
self.assertNotAlmostEqual(self.result_distance, \
0, \
msg = 'Test{}: braking distance: {}, expect not ZERO'.format(self.counter, self.result_distance), \
delta= 0.0)
# pub /signal/ui/reset_test
rospy.sleep(4)
pub_msg_emergency.emergency_state = 0
self.pub_emergency.publish(pub_msg_emergency)
rospy.sleep(1)
pub_msg_signal.data = True
pub_msg_signal.header.stamp = rospy.Time.now()
self.pub_reset.publish(pub_msg_signal)
rospy.sleep(1)
self.result_distance = None
self.result_time = None
def test_expected(self):
while self.count < 3:
self.count += 1
# pub stop signal first
pub_signal = BoolStamped()
pub_signal.header.stamp = rospy.Time.now()
pub_signal.data = True
self.pub_stop.publish(pub_signal)
rospy.sleep(1)
# pub start signal
pub_signal.header.stamp = rospy.Time.now()
pub_signal.data = True
self.pub_start.publish(pub_signal)
rospy.sleep(0.1)
# reset stop signal
pub_signal.data = False
self.pub_stop.publish(pub_signal)
rospy.sleep(0.3)
pub_signal.header.stamp = rospy.Time.now()
pub_signal.data = True
self.pub_stop.publish(pub_signal)
while self.duration is None:
time.sleep(0.1)
self.assertAlmostEqual(self.duration, 0.4, None, msg='test{}: {} is not equal to 0.3'.format(self.count, self.duration), delta=0.05)
self.assertEqual(self.msg_seq, self.count, 'msg published {} times'.format(self.msg_seq))
# reset
self.duration = None
pub_signal.data = False
pub_signal.header.stamp = rospy.Time.now()
self.pub_start.publish(pub_signal)
rospy.sleep(0.1)
pub_signal.header.stamp = rospy.Time.now()
self.pub_stop.publish(pub_signal)
rospy.sleep(0.1)
# pub start signal
pub_signal.header.stamp = rospy.Time.now()
pub_signal.data = True
self.pub_start.publish(pub_signal)
rospy.sleep(0.1)
rospy.sleep(0.3)
pub_signal.header.stamp = rospy.Time.now()
pub_signal.data = True
self.pub_stop.publish(pub_signal)
while self.duration is None:
time.sleep(0.1)
self.assertAlmostEqual(self.duration, 0.4, None, msg='test{}: {} is not equal to 0.3'.format(self.count+1, self.duration), delta=0.05)
self.assertEqual(self.msg_seq, self.count+1, 'msg published {} times'.format(self.msg_seq))
def __init__(self,
sub_dict={_sub_topic: BoolStamped},
pub_dict={
_pub_topic_high: BoolStamped,
_pub_topic_low: BoolStamped
},
rate=None):
self.pre_state = None
self.msg_high = BoolStamped()
self.msg_low = BoolStamped()
super(edgeDetector, self).__init__(sub_dict=sub_dict,
pub_dict=pub_dict,
rate=rate)
def __init__(self, sub_dict={}, result_list=[], param_list=[], rate=100):
self._rate = rospy.Rate(rate)
self._input_interface = {
"start": "/start",
"stop": "/stop",
}
self._input_interface.update(sub_dict)
self._config_param = []
self._config_param += param_list
# get ros param:
self.config_param = {}
self.getConfig(self._config_param)
self._locks = {}
self._flag = {}
for key in self._input_interface:
rospy.Subscriber(self._input_interface[key], BoolStamped,
self.callback, key)
self._locks[key] = threading.Lock()
self._flag[key] = BoolStamped()
self._flag[key].data = False
self.result_list = result_list
for topic in self.result_list:
rospy.Subscriber(topic, Float64Stamped, self.callback, topic)
self._locks[topic] = threading.Lock()
self._flag[topic] = Float64Stamped()
self._flag[topic].data = float("inf")
self.root_folder = get_root_folder()
def test_expected(self):
# pub start signal
pub_signal = BoolStamped()
pub_signal.header.stamp = rospy.Time.now()
pub_signal.data = True
self.pub_1.publish(pub_signal)
rospy.sleep(2.0)
pub_signal.header.stamp = rospy.Time.now()
pub_signal.data = True
self.pub_2.publish(pub_signal)
while self.duration is None:
time.sleep(0.1)
self.assertAlmostEqual(self.duration, 2.0, None, msg='{} is not equal to 2.0'.format(self.duration), delta=0.05)
def test_expected3(self):
pub_msg = BoolStamped()
pub_msg.header.stamp = rospy.Time().now()
pub_msg.data = True
self.result_high = None
self.result_low = None
self.pub_1.publish(pub_msg)
while not rospy.is_shutdown():
with self.mutex:
l_result_high = self.result_high
l_result_low = self.result_low
if l_result_high != None and l_result_low != None:
break
self.assertTrue(self.result_high,
msg='Test3 rising edge - High failed')
self.assertFalse(self.result_low, msg='Test3 rising edge - Low failed')
def test_expected(self):
while self.counter < 5:
self.counter += 1
pub_msg = BoolStamped()
pub_msg.data = True
pub_msg.header.stamp = rospy.Time.now()
# pub /signal/ui/start_test
self.pub_start.publish(pub_msg)
rospy.sleep(5)
# pub /signal/runner/stop_robot
pub_msg.header.stamp = rospy.Time.now()
self.pub_stop.publish(pub_msg)
# wait for result
while self.result_completed is None or self.result_gap is None:
rospy.sleep(0.01)
self.assertAlmostEqual(self.result_gap, \
self.result_gap, \
msg='Test {}: robot stop at {}m from the obstacle, expect {}m'.format(self.counter, self.result_gap, self.result_gap), \
delta= 0.01)
self.assertNotAlmostEqual(self.result_gap, \
0, \
msg = 'Test{}: robot should not stop at {}m from the obstacle'.format(self.counter, self.result_gap), \
delta= 0.0)
self.assertTrue(self.result_completed,
msg='Test {}: did not complete'.format(
self.counter))
rospy.sleep(5)
# pub /signal/ui/reset_test
pub_msg.data = True
pub_msg.header.stamp = rospy.Time.now()
self.pub_reset.publish(pub_msg)
rospy.sleep(1)
self.result_time = None
self.result_completed = None
self.result_start = None
def test_connect_to_action(self):
pub_msg = BoolStamped()
pub_msg.header.stamp = rospy.Time.now()
pub_msg.data = True
self.pub_start.publish(pub_msg)
while self.pose is None:
rospy.sleep(0.1)
self.assertEqual(self.pose.position.x,
self.goal["position"]["x"],
msg='test1: The goal should be {}, but get {}'.format(
self.goal, self.pose))
self.assertAlmostEqual(self.pose.orientation.w, \
0.707, \
msg='test1: The goal should be {}, but get {}'.format(self.goal, self.pose), \
delta = 0.01)
rospy.sleep(0.5)
self.assertEqual(1, self.times, \
msg='test2: The msg should ony publish {}, but published {}'.format(1, self.times))
def pub_init(self):
self.pub_diag_goal = rospy.Publisher(self.ppub_topic_goal,
PoseStamped,
queue_size=10,
latch=True)
self.goal_msg = PoseStamped()
self.goal_msg = self.action_goal.target_pose
self.pub_diag_result = rospy.Publisher(self.pub_topic_result,
BoolStamped,
queue_size=10,
latch=True)
self.action_result_msg = BoolStamped()
self.action_result_msg.header.stamp = rospy.Time.now()
self.action_result_msg.data = False
def test_expected(self):
print(self.robot_type)
self.path = self.dest_dir + "/" + self.robot_type + "/" + self.test_type + "/"
while self.i < 3:
pub_msg = BoolStamped()
pub_msg.header.stamp = rospy.Time.now()
pub_msg.data = True
self.pub_start.publish(pub_msg)
rospy.sleep(3)
if os.listdir(self.path):
self.result = True
else:
self.result = False
self.assertTrue(self.result, msg='there is no folder')
rospy.sleep(2)
pub_msg.header.stamp = rospy.Time.now()
pub_msg.data = False
self.pub_start.publish(pub_msg)
rospy.sleep(1)
pub_msg.header.stamp = rospy.Time.now()
pub_msg.data = True
self.pub_stop.publish(pub_msg)
rospy.sleep(1)
pub_msg.header.stamp = rospy.Time.now()
pub_msg.data = False
self.pub_stop.publish(pub_msg)
rospy.sleep(1)
pub_msg.header.stamp = rospy.Time.now()
pub_msg.data = True
self.pub_success.publish(pub_msg)
rospy.sleep(3)
for i in os.listdir(self.path):
try:
os.rmdir(self.path + i)
except:
files = os.listdir(self.path + i)
for f in files:
os.remove(os.path.join(self.path + i, f))
os.rmdir(self.path + i)
self.i += 1
def test_expected(self):
pub_msg = BoolStamped()
pub_msg.header.stamp = rospy.Time.now()
pub_msg.data = True
self.pub_1.publish(pub_msg)
pub_msg = BoolStamped()
pub_msg.header.stamp = rospy.Time.now()
pub_msg.data = True
self.pub_2.publish(pub_msg)
while self.result is None:
time.sleep(0.1)
self.assertTrue(self.result, msg='Compare with Tolerance Failed')
def test_interrupted(self):
while self.counter < 3:
self.counter += 1
pub_msg = BoolStamped()
pub_msg.data = True
pub_msg.header.stamp = rospy.Time.now()
# pub /signal/ui/start_test
self.pub_start.publish(pub_msg)
rospy.sleep(3.01)
if self.counter == 1:
pub_msg.data = True
pub_msg.header.stamp = rospy.Time.now()
self.pub_interrupt.publish(pub_msg)
if self.counter == 2:
rospy.sleep(0.1)
pub_msg.data = True
pub_msg.header.stamp = rospy.Time.now()
self.pub_interrupt.publish(pub_msg)
if self.counter == 3:
rospy.sleep(2.1)
pub_msg.data = True
pub_msg.header.stamp = rospy.Time.now()
self.pub_interrupt.publish(pub_msg)
# wait for result
while self.result_failed is None or self.result_failed is False:
rospy.sleep(0.01)
self.assertTrue(
self.result_failed,
msg='Test{}: Test_Failed should be True, but get {}'.format(
self.counter, self.result_failed))
# pub /signal/ui/reset_test
rospy.sleep(10)
pub_msg.header.stamp = rospy.Time.now()
self.pub_reset.publish(pub_msg)
rospy.sleep(1)
self.result_failed = None
rospy.sleep(2)
def test_expected(self):
pub_pose_msg_start = PoseStamped()
pub_pose_msg_start.header.stamp = rospy.Time.now()
pub_pose_msg_start.pose.position.x = 1
pub_pose_msg_start.pose.position.y = 1
pub_pose_msg_start.pose.position.z = 1
pub_pose_msg_start.pose.orientation.w = 1
self.pub_pose.publish(pub_pose_msg_start)
rospy.sleep(0.5)
# pub start signal
pub_signal = BoolStamped()
pub_signal.header.stamp = rospy.Time.now()
pub_signal.data = True
self.pub_start.publish(pub_signal)
rospy.sleep(0.5)
pub_pose_msg_stop = PoseStamped()
pub_pose_msg_stop.header.stamp = rospy.Time.now()
pub_pose_msg_stop.pose.position.x = 4
pub_pose_msg_stop.pose.position.y = 5
pub_pose_msg_stop.pose.position.z = 2
pub_pose_msg_stop.pose.orientation.w = 1
pub_pose_msg_stop.pose.orientation.z = 1
self.pub_pose.publish(pub_pose_msg_stop)
rospy.sleep(0.5)
pub_signal.header.stamp = rospy.Time.now()
pub_signal.data = True
self.pub_stop.publish(pub_signal)
while self.delta_pose is None and self.delta_angle is None:
time.sleep(0.1)
self.assertEqual(
self.delta_pose, 5.0,
'test 1:{} is not equal to 5.0'.format(self.delta_pose))
self.assertEqual(
self.delta_angle, 90.0,
'test 1: {} is not equal to 5.0'.format(self.delta_angle))
self.assertEqual(
self.msg_seq_pose, 1,
'test 1: msg_pose published {} times'.format(self.msg_seq_pose))
self.assertEqual(
self.msg_seq_angle, 1,
'test 1: msg_angle published {} times'.format(self.msg_seq_angle))
# reset signal , test agains
pub_signal.data = False
self.pub_start.publish(pub_signal)
rospy.sleep(0.5)
self.pub_stop.publish(pub_signal)
rospy.sleep(0.5)
self.delta_angle = None
self.delta_pose = None
self.pub_pose.publish(pub_pose_msg_start)
pub_signal.data = True
self.pub_start.publish(pub_signal)
rospy.sleep(0.5)
self.pub_pose.publish(pub_pose_msg_stop)
self.pub_stop.publish(pub_signal)
while self.delta_pose is None and self.delta_angle is None:
time.sleep(0.1)
self.assertEqual(
self.delta_pose, 5.0,
'test 2: {} is not equal to 5.0'.format(self.delta_pose))
self.assertEqual(
self.delta_angle, 90.0,
'test 2: {} is not equal to 5.0'.format(self.delta_angle))
self.assertEqual(
self.msg_seq_angle, 2,
'test 2: msg_angle published {} times'.format(self.msg_seq_angle))
self.assertEqual(
self.msg_seq_pose, 2,
'test 2: msg_pose published {} times'.format(self.msg_seq_pose))
def test_expected(self):
# start true, stop false
pub_msg_start = BoolStamped()
pub_msg_start.header.stamp = rospy.Time.now()
pub_msg_start.data = True
self.pub_start.publish(pub_msg_start)
while self.vel is None:
time.sleep(0.1)
self.assertEqual(
self.vel,
self.max_vel,
msg='test1: The velocity should be {}, but get {}'.format(
self.max_vel, self.vel))
self.assertAlmostEqual(self.transfer_timestamp(pub_msg_start.header.stamp), self.transfer_timestamp(self.time), \
msg='test1: Send signal at {}, send velocity at {}'.format(self.transfer_timestamp(pub_msg_start.header.stamp), self.transfer_timestamp(self.time)), \
delta=0.5)
# start false, stop true
pub_msg_start.header.stamp = rospy.Time.now()
pub_msg_start.data = False
self.pub_start.publish(pub_msg_start)
rospy.sleep(0.1)
pub_msg_stop = BoolStamped()
pub_msg_stop.header.stamp = rospy.Time.now()
pub_msg_stop.data = True
self.pub_stop.publish(pub_msg_stop)
self.vel = None
while self.vel is None:
time.sleep(0.1)
self.assertEqual(
self.vel,
0,
msg='test2: The velocity should be {}, but get {}'.format(
0, self.vel))
self.assertAlmostEqual(self.transfer_timestamp(pub_msg_stop.header.stamp), self.transfer_timestamp(self.time), \
msg='test2: Send signal at {}, send velocity at {}'.format(self.transfer_timestamp(pub_msg_stop.header.stamp), self.transfer_timestamp(self.time)), \
delta=0.5)
# reset start false, stop false
pub_msg_start.header.stamp = rospy.Time.now()
pub_msg_start.data = False
self.pub_start.publish(pub_msg_start)
rospy.sleep(0.1)
pub_msg_stop = BoolStamped()
pub_msg_stop.header.stamp = rospy.Time.now()
pub_msg_stop.data = False
self.pub_stop.publish(pub_msg_stop)
rospy.sleep(0.1)
# start true, stop false
self.vel = None
pub_msg_start.header.stamp = rospy.Time.now()
pub_msg_start.data = True
self.pub_start.publish(pub_msg_start)
rospy.sleep(0.1)
while self.vel is None:
time.sleep(0.1)
self.assertEqual(
self.vel,
self.max_vel,
msg='test4: The velocity should be {}, but get {}'.format(
self.max_vel, self.vel))
self.assertAlmostEqual(self.transfer_timestamp(pub_msg_start.header.stamp), self.transfer_timestamp(self.time), \
msg='test4: Send signal at {}, send velocity at {}'.format(self.transfer_timestamp(pub_msg_start.header.stamp), self.transfer_timestamp(self.time)), \
delta=0.5)
def buildNewBoolStamped(self, data=True):
msg = BoolStamped()
msg.header.stamp = rospy.Time.now()
msg.data = data
return msg
def __init__(self, sub_dict={}, pub_dict={}, param_list={}, rate=100):
self._rate = rospy.Rate(rate)
self._input_interface = {
"robot_has_stopped": "/signal/calc/robot_has_stopped",
"start_test": "/signal/ui/start_test",
"interrupt_test": "/signal/ui/interrupt_test",
"reset_test": "/signal/ui/reset_test",
"began_recording": "/signal/logger/begin_write"
}
self._input_interface.update(sub_dict)
self._output_interface = {
"start_robot": "/signal/runner/start_robot",
"stop_robot": "/signal/runner/stop_robot",
"test_completed": "/signal/runner/test_completed",
"test_failed": "/signal/runner/test_failed",
"test_succeeded": "/signal/runner/test_succeeded",
# TODO: The next 3 signals should be removed once UI is fully integrated
"start_test": "/signal/ui/start_test",
"reset_test": "/signal/ui/reset_test",
"interrupt_test": "/signal/ui/interrupt_test"
}
self._output_interface.update(pub_dict)
self._config_param = []
self._config_param += param_list
# TODO: Make this enums or constants or something
self._RETURN_CASE_INTERRUPTED = -1
self._RETURN_CASE_TIMED_OUT = -2
self._locks = {}
self._flag = {}
self._publishers = {}
# get ros param:
self.config_param = {}
self.get_config(self._config_param)
# sub_init
for key in self._input_interface:
rospy.Subscriber(self._input_interface[key], BoolStamped,
self.callback_for_all_bool_topics, key)
self._locks[key] = threading.Lock()
self._flag[key] = BoolStamped()
self._flag[key].data = False
# pub_init
for key in self._output_interface:
self._publishers[key] = rospy.Publisher(
self._output_interface[key],
BoolStamped,
queue_size=10,
latch=True)
self._publishers[key].publish(self.buildNewBoolStamped(False))
try:
while not rospy.is_shutdown():
self.main()
self._rate.sleep()
except rospy.ROSException:
pass
def test_expected(self):
pub_pose_msg = PoseStamped()
pub_pose_msg.header.stamp = rospy.Time.now()
pub_pose_msg.pose.position.x = 1
pub_pose_msg.pose.position.y = 1
pub_pose_msg.pose.position.z = 1
pub_pose_msg.pose.orientation.w = 1
self.pub_1.publish(pub_pose_msg)
pub_pose_msg = PoseStamped()
pub_pose_msg.header.stamp = rospy.Time.now()
pub_pose_msg.pose.position.x = 4
pub_pose_msg.pose.position.y = 5
pub_pose_msg.pose.position.z = 5
pub_pose_msg.pose.orientation.w = 1
pub_pose_msg.pose.orientation.z = 1
self.pub_2.publish(pub_pose_msg)
pub_signal_msg = BoolStamped()
pub_signal_msg.header.stamp = rospy.Time.now()
pub_signal_msg.data = False
self.pub_signal.publish(pub_signal_msg)
rospy.sleep(0.5)
pub_signal_msg.data = True
self.pub_signal.publish(pub_signal_msg)
while self.delta_pose is None and self.delta_angle is None:
time.sleep(0.1)
self.assertEqual(
self.delta_pose, 5.0,
'test1: {} is not equal to 5.0'.format(self.delta_pose))
self.assertEqual(
self.delta_angle, 90.0,
'test1: {} is not equal to 5.0'.format(self.delta_angle))
self.assertEqual(
self.msg_seq_angle, 1,
'test1: msg published {} times'.format(self.msg_seq_angle))
self.assertEqual(
self.msg_seq_pose, 1,
'test1: msg published {} times'.format(self.msg_seq_pose))
rospy.sleep(0.2)
pub_signal_msg.data = False
self.pub_signal.publish(pub_signal_msg)
self.assertEqual(
self.msg_seq_angle, 1,
'test2: msg published {} times'.format(self.msg_seq_angle))
self.assertEqual(
self.msg_seq_pose, 1,
'test2: msg published {} times'.format(self.msg_seq_pose))
rospy.sleep(0.2)
pub_signal_msg.data = True
self.pub_signal.publish(pub_signal_msg)
rospy.sleep(0.2)
self.assertEqual(
self.msg_seq_angle, 2,
'test3: msg published {} times'.format(self.msg_seq_angle))
self.assertEqual(
self.msg_seq_pose, 2,
'test3: msg published {} times'.format(self.msg_seq_pose))
def test_expected(self):
pub_msg_1 = BoolStamped()
pub_msg_1.header.stamp = rospy.Time.now()
pub_msg_1.data = False
self.pub_1.publish(pub_msg_1)
pub_msg_2 = BoolStamped()
pub_msg_2.header.stamp = rospy.Time.now()
pub_msg_2.data = False
self.pub_2.publish(pub_msg_2)
while self.result is None:
time.sleep(0.1)
self.assertFalse(self.result, msg='Compare with Tolerance Failed')
self.assertEqual(self.msg_seq, 1,
'msg published {} times'.format(self.msg_seq))
pub_msg_1.data = True
pub_msg_2.data = True
self.result = None
self.pub_1.publish(pub_msg_1)
self.pub_2.publish(pub_msg_2)
while self.result is None:
time.sleep(0.1)
self.assertTrue(self.result, msg='Compare with Tolerance Failed')
self.assertEqual(self.msg_seq, 2,
'msg published {} times'.format(self.msg_seq))
pub_msg_1.data = False
pub_msg_2.data = True
self.result = None
self.pub_1.publish(pub_msg_1)
self.pub_2.publish(pub_msg_2)
while self.result is None:
time.sleep(0.1)
self.assertFalse(self.result, msg='Compare with Tolerance Failed')
self.assertEqual(self.msg_seq, 3,
'msg published {} times'.format(self.msg_seq))
pub_msg_1.data = True
pub_msg_2.data = True
self.result = None
self.pub_1.publish(pub_msg_1)
self.pub_2.publish(pub_msg_2)
while self.result is None:
time.sleep(0.1)
self.assertTrue(self.result, msg='Compare with Tolerance Failed')
self.assertEqual(self.msg_seq, 4,
'msg published {} times'.format(self.msg_seq))
pub_msg_1.data = True
pub_msg_2.data = False
self.result = None
self.pub_1.publish(pub_msg_1)
self.pub_2.publish(pub_msg_2)
while self.result is None:
time.sleep(0.1)
self.assertFalse(self.result, msg='Compare with Tolerance Failed')
self.assertEqual(self.msg_seq, 5,
'msg published {} times'.format(self.msg_seq))