def test_jog_arm_joint_command(node):
# Test sending a joint command
assert util.wait_for_jogger_initialization()
received = []
sub = rospy.Subscriber(COMMAND_OUT_TOPIC, JointTrajectory,
lambda msg: received.append(msg))
joint_cmd = JointJogCmd()
TEST_DURATION = 1
PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file
velocities = [0.1]
# Send a command to start the jogger
joint_cmd.send_joint_velocity_cmd(velocities)
start_time = rospy.get_rostime()
received = []
while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION:
joint_cmd.send_joint_velocity_cmd(velocities)
time.sleep(0.1)
# TEST_DURATION/PUBLISH_PERIOD is the expected number of messages in this duration.
# Allow a small +/- window due to rounding/timing errors
assert len(received) >= TEST_DURATION / PUBLISH_PERIOD - 20
assert len(received) <= TEST_DURATION / PUBLISH_PERIOD + 20
def test_drift_dimensions_service(node):
assert util.wait_for_jogger_initialization()
# Service to change drift dimensions
drift_service = rospy.ServiceProxy(SERVICE_NAME, ChangeDriftDimensions)
# Service call to allow drift in all dimensions except y-translation
# The transform is an identity matrix, not used for now
drift_response = drift_service(True, False, True, True, True, True,
Transform())
# Check for successful response
assert drift_response.success == True
def test_vel_limit(node):
# Test sending a joint command
assert util.wait_for_jogger_initialization()
received = []
sub = rospy.Subscriber(
COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg)
)
joint_cmd = JointJogCmd()
TEST_DURATION = 1
PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file
# Panda arm limit, from joint_limits.yaml
VELOCITY_LIMIT = rospy.get_param("/robot_description_planning/joint_limits/panda_joint1/max_velocity")
# Send a velocity command that exceeds the limit
velocities = [10 * VELOCITY_LIMIT]
# Send a command to start the jogger
joint_cmd.send_joint_velocity_cmd(velocities)
start_time = rospy.get_rostime()
received = []
while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION:
joint_cmd.send_joint_velocity_cmd(velocities)
time.sleep(0.1)
# Period of outgoing commands from the jogger, from yaml
JOGGER_COMMAND_PERIOD = rospy.get_param("/jog_server/publish_period")
# Should be no velocities greater than the limit
assert len(received) > 2
for msg_idx in range(1, len(received)):
try:
velocity = \
(received[msg_idx].points[0].positions[0] - received[msg_idx - 1].points[0].positions[0]) / JOGGER_COMMAND_PERIOD
assert abs(velocity) <= VELOCITY_LIMIT
except IndexError:
# Sometimes a message doesn't have any points
pass
def test_jog_arm_halt_msg(node):
assert util.wait_for_jogger_initialization(SERVICE_NAME)
received = []
sub = rospy.Subscriber(HALT_TOPIC, Int8, lambda msg: received.append(msg))
cartesian_cmd = CartesianJogCmd()
# This nonzero command should produce jogging output
# A subscriber in a different thread fills `received`
TEST_DURATION = 1
start_time = rospy.get_rostime()
received = []
while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION:
cartesian_cmd.send_cmd([1, 1, 1], [0, 0, 1])
time.sleep(0.1)
# Check the received messages
# A non-zero value signifies a warning
assert len(received) > 3
assert (received[-1].data != 0) or (received[-2].data !=
0) or (received[-3].data != 0)
def test_jog_arm_cartesian_command(node):
# Test sending a cartesian velocity command
assert util.wait_for_jogger_initialization(SERVICE_NAME)
received = []
sub = rospy.Subscriber(
COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg)
)
cartesian_cmd = CartesianJogCmd()
# Repeated zero-commands should produce no output, other than a few halt messages
# A subscriber in a different thread fills 'received'
cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0])
cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0])
cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0])
cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0])
received = []
rospy.sleep(1)
assert len(received) <= 4 # 'num_outgoing_halt_msgs_to_publish' in the config file
# This nonzero command should produce jogging output
# A subscriber in a different thread fills `received`
TEST_DURATION = 1
PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file
# Send a command to start the jogger
cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1])
start_time = rospy.get_rostime()
received = []
while (rospy.get_rostime() - start_time).to_sec() < TEST_DURATION:
cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1])
time.sleep(0.1)
# TEST_DURATION/PUBLISH_PERIOD is the expected number of messages in this duration.
# Allow a small +/- window due to rounding/timing errors
assert len(received) >= TEST_DURATION/PUBLISH_PERIOD - 20
assert len(received) <= TEST_DURATION/PUBLISH_PERIOD + 20
