def test_inital_localization_side(self):
# Check if we heard from the localization
sub = MockSubscriber(
"/pose_with_covariance",
PoseWithCovarianceStamped,
callback=lambda msg: setattr(self, 'localization_estimation', msg))
self.with_assertion_grace_period(lambda: sub.assertMessageReceived(),
t=5000)
self.with_assertion_grace_period(lambda: self.assertRobotPosition(
self.localization_estimation.pose.pose.position),
t=15000)
def test_walk_runs(self):
sub = MockSubscriber('walking_motor_goals', JointCommand)
self.with_assertion_grace_period(
lambda: self.assertNodeRunning("walking"), 10)
pub = rospy.Publisher('cmd_vel', Twist, queue_size=1, latch=True)
goal = Twist()
goal.linear.x = 0.3
pub.publish(goal)
sub.wait_until_connected()
rospy.sleep(5)
self.with_assertion_grace_period(sub.assertMessageReceived, 100)
def test_walk_odometry(self):
"""
Test if the walk odometry is correct.
This means also that the robot is walking with the correct speed.
"""
def odom_cb(msg):
nonlocal current_odom
current_odom = msg
# setup
self.initialize_everything()
current_odom = None
sub = MockSubscriber("walk_engine_odometry",
Odometry,
odom_cb,
tcp_nodelay=True)
sub.wait_until_connected()
pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
# remember start odom pose
rospy.sleep(1)
start_odom = current_odom
# execution
cmd_vel = Twist()
cmd_vel.linear.x = 0.1
cmd_vel.linear.y = 0.05
cmd_vel.angular.z = 0.1
pub.publish(cmd_vel)
rospy.sleep(10)
cmd_vel = Twist()
cmd_vel.angular.x = -1
pub.publish(cmd_vel)
rospy.sleep(2)
# verification
end_odom = current_odom
odom_diff = Pose()
odom_diff.position.x = end_odom.pose.pose.position.x - start_odom.pose.pose.position.x
odom_diff.position.y = end_odom.pose.pose.position.y - start_odom.pose.pose.position.y
odom_diff.position.z = end_odom.pose.pose.position.z - start_odom.pose.pose.position.z
# difference between quaternions is annoying, so just use end pose, other tests did not turn
odom_diff.orientation.w = end_odom.pose.pose.orientation.w
odom_diff.orientation.x = end_odom.pose.pose.orientation.x
odom_diff.orientation.y = end_odom.pose.pose.orientation.y
odom_diff.orientation.z = end_odom.pose.pose.orientation.z
# robot should be at odom position
self.assertRobotPose(odom_diff, lin_threshold=1, ang_threshold=1)
self.assertRobotStanding()
def test_start(self):
""" test if node starts correctly without warnings/errors/criticals
and if it is still there after some time (did not crash by itself)"""
# setup
self.initialize_everything()
# wait to make sure node is up
sub = MockSubscriber("DynamixelController/command",
JointCommand,
tcp_nodelay=True)
sub.wait_until_connected()
# execution
time.sleep(2)
# verification
self.assertNoNegativeRosLogs(node="walking")
def test_joint_goals(self):
# setup
self.initialize_everything()
pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
sub = MockSubscriber("DynamixelController/command",
JointCommand,
tcp_nodelay=True)
sub.wait_until_connected()
# wait for support state to make sure the walking is started
sub_support_state = MockSubscriber("walk_support_state",
Phase,
tcp_nodelay=True)
sub_support_state.wait_until_connected()
time.sleep(10)
# execution
cmd_vel = Twist()
cmd_vel.linear.x = 0.1
pub.publish(cmd_vel)
rospy.sleep(5)
# verification
# make sure something is published
sub.assertMessageReceived()
def test_sent_message_gets_received_over_bridge(self):
# setup
send_topic = rospy.get_param("/udp_bridge/topics")[0]
receive_topic = f"{gethostname()}/{send_topic}".replace("//", "/")
subscriber = MockSubscriber(receive_topic, msg.String)
# execution
publisher = rospy.Publisher(send_topic,
msg.String,
latch=True,
queue_size=1)
publisher.publish(msg.String("Hello World"))
# verification
self.with_assertion_grace_period(subscriber.assertOneMessageReceived,
1000 * 5)
def test_no_joint_goals(self):
"""test if joint goals are published when walking is activated and only then"""
# setup
self.initialize_everything()
sub = MockSubscriber("DynamixelController/command",
JointCommand,
tcp_nodelay=True)
sub.wait_until_connected()
# execution
# wait some time
time.sleep(1)
# verification
sub.assertNothingReceived()
def single_run(self, goal):
def done_cb(state, result):
assert state == GoalStatus.SUCCEEDED, "Kick was not successful"
self.kick_succeeded = True
self.kick_succeeded = False
sub = MockSubscriber('kick_motor_goals', JointCommand)
self.with_assertion_grace_period(
lambda: self.assertNodeRunning("dynamic_kick"), 20)
client = ActionClient(self, KickAction, 'dynamic_kick')
assert client.wait_for_server(), "Kick action server not running"
client.send_goal_async(goal)
client.done_cb = done_cb
client.wait_for_result()
sub.wait_until_connected()
sub.assertMessageReceived()
assert self.kick_succeeded, "Kick was not successful"
def one_run(self, direction):
def done_cb(state, result):
assert state == GoalStatus.SUCCEEDED, "Dynup was not successful"
self.kick_succeeded = True
self.kick_succeeded = False
sub = MockSubscriber('dynup_motor_goals', JointCommand)
self.with_assertion_grace_period(
lambda: self.assertNodeRunning("dynup"), 20)
client = ActionClient(self, DynUpAction, 'dynup')
assert client.wait_for_server(), "Dynup action server not running"
goal = DynUpGoal()
goal.direction = direction
client.send_goal_async(goal)
client.done_cb = done_cb
client.wait_for_result()
sub.wait_until_connected()
sub.assertMessageReceived()
assert self.kick_succeeded, "Dynup was not successful"
class MockSubscriberTestCase(RosNodeTestCase):
sub: MockSubscriber
pub: rospy.Publisher
def setUp(self) -> None:
super().setUp()
self.pub = rospy.Publisher(self.topic, String, queue_size=10)
self.sub = MockSubscriber(self.topic, String, queue_size=10)
self.sub.wait_until_connected()
def tearDown(self) -> None:
super().tearDown()
self.sub.unregister()
self.pub.unregister()
def test_assert_message_received(self):
# execution
self.pub.publish(String("this is a test"))
# verification
self.with_assertion_grace_period(t=200,
f=self.sub.assertMessageReceived)
self.sub.assertMessageReceived(String("this is a test"))
self.assertRaises(
AssertionError,
lambda: self.sub.assertMessageReceived(String("something else")))
def test_assert_messages_received(self):
# execution
self.pub.publish(String("test 1"))
self.pub.publish(String("test 2"))
self.pub.publish(String("test 3"))
# verification
self.with_assertion_grace_period(t=200,
f=self.sub.assertMessageReceived)
self.sub.assertMessagesReceived(
[String("test 1"),
String("test 2"),
String("test 3")])
self.sub.assertMessagesReceived(
[String("test 2"),
String("test 1"),
String("test 3")],
any_order=True)
self.assertRaises(
AssertionError, lambda: self.sub.assertMessagesReceived(
[String("test2"), String("test1")]))
def test_assert_nothing_received(self):
# verification
self.sub.assertNothingReceived()
def test_assert_one_message_received(self):
# execution
self.pub.publish(String("test"))
# verification
self.with_assertion_grace_period(t=200,
f=self.sub.assertOneMessageReceived)
self.sub.assertOneMessageReceived()
self.sub.assertOneMessageReceived(String("test"))
self.pub.publish(String("another test"))
time.sleep(0.2)
self.assertRaises(AssertionError,
lambda: self.sub.assertOneMessageReceived())
def test_reset(self):
# setup
self.pub.publish(String("test"))
self.with_assertion_grace_period(t=200,
f=self.sub.assertOneMessageReceived)
# execution
self.sub.reset()
# verification
self.sub.assertNothingReceived()
self.assertRaises(AssertionError,
lambda: self.sub.assertOneMessageReceived())
self.assertRaises(AssertionError,
lambda: self.sub.assertMessageReceived())
def setUp(self) -> None:
super().setUp()
self.pub = rospy.Publisher(self.topic, String, queue_size=10)
self.sub = MockSubscriber(self.topic, String, queue_size=10)
self.sub.wait_until_connected()