pub tests in stretch_driver

stretch_core/nodes/tests/test_command_groups.py

@@ -50,6 +50,12 @@ def mobile_base_cg():
     virtual_range_m = (-0.5,0.5)
     return cg.MobileBaseCommandGroup(virtual_range_m)
 
+@pytest.fixture  
+def invalid_joints_callback():
+    pass
+
+
+
 
 #Begin tests 
 def test_get_num_valid_commands(head_pan_cg,head_tilt_cg,wrist_yaw_cg,gripper_cg,telescoping_cg,lift_cg,mobile_base_cg): 
@@ -62,4 +68,21 @@ def test_get_num_valid_commands(head_pan_cg,head_tilt_cg,wrist_yaw_cg,gripper_cg
         assert True
     else:
         assert False 
-     
+
+
+''' IN DEVELOPMENT 
+def test_update(head_pan_cg,head_tilt_cg,wrist_yaw_cg,gripper_cg,telescoping_cg,lift_cg,mobile_base_cg,invalid_joints_callback):
+    
+    command_groups = [head_pan_cg,head_tilt_cg,wrist_yaw_cg,gripper_cg,
+                      telescoping_cg,lift_cg,mobile_base_cg]
+
+    commanded_joint_names = [cg.name for cg in command_groups]
+
+
+    updates = [c.update(commanded_joint_names, invalid_joints_callback)
+                for c in command_groups]
+
+    if not all(updates):
+        assert False
+    assert True  
+'''

stretch_core/tests/node_tests/test_stretch_driver.py

@@ -3,7 +3,10 @@ import rospy
 import time 
 
 from std_msgs.msg import String
-from sensor_msgs.msg import JointState
+from sensor_msgs.msg import JointState, Imu, MagneticField
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+
 
 @pytest.fixture
 def node():
@@ -11,7 +14,7 @@ def node():
 
 @pytest.fixture
 def waiter():
-    class Waiter(object):
+    class Waiter:
         def __init__(self):
             self.received = []
             self.condition = lambda x: False
@@ -33,7 +36,8 @@ def waiter():
 
     return Waiter()
 
-def test_listener_receives_something(node, waiter):
+
+def test_joint_states_receives_something(node, waiter):
     waiter.condition = lambda data: True  # any message is good
     
     rospy.Subscriber("/stretch/joint_states", JointState, waiter.callback) 
@@ -41,7 +45,38 @@ def test_listener_receives_something(node, waiter):
 
     assert waiter.success 
 
-def test_hello_works():
-    assert True 
+def test_odom_receives_something(node, waiter):
+    waiter.condition = lambda data: True  # any message is good
+    
+    rospy.Subscriber("/odom", Odometry, waiter.callback) 
+    waiter.wait(10.0)
+
+    assert waiter.success 
+
+
+def test_imu_mobile_base_receives_something(node, waiter):
+    waiter.condition = lambda data: True  # any message is good
+    
+    rospy.Subscriber("/imu_mobile_base", Imu, waiter.callback) 
+    waiter.wait(10.0)
+
+    assert waiter.success 
 
 
+def test_imu_wrist_receives_something(node, waiter):
+    waiter.condition = lambda data: True  # any message is good
+    
+    rospy.Subscriber("/imu_wrist", Imu, waiter.callback) 
+    waiter.wait(10.0)
+
+    assert waiter.success 
+
+
+def test_magnetometer_mobile_base_receives_something(node, waiter):
+    waiter.condition = lambda data: True  # any message is good
+    
+    rospy.Subscriber("/magnetometer_mobile_base", MagneticField, waiter.callback) 
+    waiter.wait(10.0)
+
+    assert waiter.success 
+