Source code for middlewares.ros.sick

#! /usr/bin/env python
"""
This script tests the Sick laser scanner with ROS in MORSE.
"""

import sys
import time
import math
import struct
from morse.testing.ros import RosTestCase
from morse.testing.testing import testlogger

import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan

# Include this import to be able to use your test file as a regular
# builder script, ie, usable with: 'morse [run|exec] base_testing.py
try:
    from morse.builder import *
except ImportError:
    pass

[docs]def pub_vw(topic, v, w): pub = rospy.Publisher(topic, Twist) msg = Twist() msg.linear.x = v msg.angular.z = w # wait 1 second for publisher rospy.sleep(1.0) pub.publish(msg)
[docs]class SickLaserRosTest(RosTestCase):
[docs] def setUpEnv(self): """ Defines the test scenario """ robot = ATRV() motion = MotionVW() robot.append(motion) motion.add_stream('ros') sick = Sick() sick.translate(x = 0.18, z = 0.94) robot.append(sick) # sick.properties(scan_window = 270, resolution = .25) sick.properties(scan_window = 180, resolution = 1) sick.add_stream('ros') # test does not call sick.__del__() so create laser arc manually sick.create_laser_arc() env = Environment('indoors-1/boxes', fastmode=True)
[docs] def test_sick_laser(self): rospy.init_node('morse_ros_laser_testing', log_level=rospy.DEBUG) motion_topic = '/robot/motion' laser_topic = '/robot/sick' pub_vw(motion_topic, 1, 1) old = [] for step in range(5): msg = rospy.wait_for_message(laser_topic, LaserScan, 10) self.assertEqual(len(msg.ranges), 181) # 180 + center ray self.assertNotEqual(msg.ranges, old) old = msg.ranges # assert that : near <= distance <= far for distance in msg.ranges: self.assertGreaterEqual(distance, 0.1) self.assertLessEqual(distance, 30) time.sleep(0.2) # wait for turning ########################## Run these tests ##########################
if __name__ == "__main__": from morse.testing.testing import main main(SickLaserRosTest, time_modes = [TimeStrategies.BestEffort])