#!/usr/bin/env python import rospy, math, random import numpy as np from sensor_msgs.msg import LaserScan from laser_geometry import LaserProjection import sensor_msgs.point_cloud2 as pc2 laser_projector = LaserProjection() def listener(scan_topic="/base_scan"): rospy.init_node('listener') rospy.Subscriber(scan_topic, LaserScan, on_scan) rospy.spin() def on_scan(scan): rospy.loginfo("Got scan, projecting") cloud = laser_projector.projectLaser(scan) gen = pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z")) for p in gen: #print " x : %f y: %f z: %f" %(p[0],p[1],p[2]) print(p.size()) rospy.loginfo("Printed cloud") if __name__ == '__main__': listener()
def __init__(self): self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.on_scan) self.laser_projector = LaserProjection() rospy.sleep(1)
def __init__(self): self._cv_bridge = CvBridge() self._laser_projector = LaserProjection() # # Camera rectification?? To be improved: read from .yaml file # Put here the calibration of the camera # self.DIM = (1920, 1208) # self.K = np.array([[693.506921, 0.000000, 955.729444], [0.000000, 694.129349, 641.732500], [0.0, 0.0, 1.0]]) # self.D = np.array([[-0.022636], [ -0.033855], [0.013493], [-0.001831]]) # self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap(self.K, self.D, np.eye(3), self.K, self.DIM, cv2.CV_16SC2) # np.eye(3) here is actually the rotation matrix # OR load it from a yaml file self.cameraModel = PinholeCameraModel() # See https://github.com/ros-perception/camera_info_manager_py/tree/master/tests camera_infomanager = CameraInfoManager( cname='truefisheye', url='package://ros_camera_lidar_calib/cfg/truefisheye800x503.yaml' ) # Select the calibration file camera_infomanager.loadCameraInfo() self.cameraInfo = camera_infomanager.getCameraInfo() # Crete a camera from camera info self.cameraModel.fromCameraInfo(self.cameraInfo) # Create camera model self.DIM = (self.cameraInfo.width, self.cameraInfo.height) # Get rectification maps self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap( self.cameraModel.intrinsicMatrix(), self.cameraModel.distortionCoeffs(), np.eye(3), self.cameraModel.intrinsicMatrix(), (self.cameraInfo.width, self.cameraInfo.height), cv2.CV_16SC2) # np.eye(3) here is actually the rotation matrix # # Declare subscribers to get the latest data cam0_subs_topic = '/gmsl_camera/port_0/cam_0/image_raw' cam1_subs_topic = '/gmsl_camera/port_0/cam_1/image_raw' cam2_subs_topic = '/gmsl_camera/port_0/cam_2/image_raw' #cam3_subs_topic = '/gmsl_camera/port_0/cam_3/image_raw/compressed' lidar_subs_topic = '/Sensor/points' #self.cam0_img_sub = rospy.Subscriber( cam0_subs_topic , Image, self.cam0_img_callback, queue_size=1) #self.cam1_img_sub = rospy.Subscriber( cam1_subs_topic , Image, self.cam1_img_callback, queue_size=1) self.cam2_img_sub = rospy.Subscriber(cam2_subs_topic, Image, self.cam2_img_callback, queue_size=1) #self.cam0_img_sub = rospy.Subscriber( cam0_subs_topic , CompressedImage, self.cam0_img_compre_callback, queue_size=1) #self.cam1_img_sub = rospy.Subscriber( cam1_subs_topic , CompressedImage, self.cam1_img_compre_callback, queue_size=1) #self.cam2_img_sub = rospy.Subscriber( cam2_subs_topic , CompressedImage, self.cam2_img_compre_callback, queue_size=1) #self.cam3_img_sub = rospy.Subscriber( cam3_subs_topic , CompressedImage, self.cam3_img_compre_callback, queue_size=1) self.lidar_sub = rospy.Subscriber(lidar_subs_topic, PointCloud2, self.lidar_callback, queue_size=1) # Get the tfs self.tf_listener = tf.TransformListener() #self.lidar_time = rospy.Subscriber(lidar_subs_topic , PointCloud2, self.readtimestamp) #self.img0_time = rospy.Subscriber(cam0_subs_topic , CompressedImage, self.readtimestamp) # # Declare the global variables we will use to get the latest info self.cam0_image_np = np.empty((self.DIM[1], self.DIM[0], 3)) self.cam0_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3)) self.cam1_image_np = np.empty((self.DIM[1], self.DIM[0], 3)) self.cam1_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3)) self.cam2_image_np = np.empty((self.DIM[1], self.DIM[0], 3)) self.cam2_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3)) self.cam3_image_np = np.empty((self.DIM[1], self.DIM[0], 3)) self.cam3_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3)) self.pcl_cloud = np.empty( (500, 4) ) # Do not know the width of a normal scan. might be variable too...... self.now = rospy.Time.now() # # Main loop: Data projections and alignment on real time self.lidar_angle_range_interest = [ 0, 180 ] # From -180 to 180. Front is 0deg. Put the range of angles we may want to get points from. Depending of camera etc thread.start_new_thread(self.projection_calibrate())
def __init__(self): self.laserProj = LaserProjection()
def __init__(self): self.laserProj = LaserProjection() self.pcPub = rospy.Publisher("/laserPCL", pc2, queue_size=1) self.laserSub = rospy.Subscriber("/scan", LaserScan, self.laserCallback)
def main(): global msg global trigger # Init node -------------------------------------- rospy.init_node("collision_node", anonymous=True) # set frequency 50 Hz rate = rospy.Rate(50) # set_up motor_power publisher motor_power_pub = rospy.Publisher('/motor_power', Bool, queue_size=10) # set_up publisher to coll_warn topic coll_warn_pub = rospy.Publisher('coll_warn', Int16, queue_size=10) # set_up subscriber to laserscan laser_sub = rospy.Subscriber("/scan", LaserScan, callback) motor_power_cmd = Bool() # set coll_warn message coll_warn_msg = [0, 1, 2] msg = LaserScan() laser_projection = LaserProjection() # set_up initial conditions trigger = 'FALSE' warninig_now = 'FALSE' warning_updated = 'FALSE' stop_now = 'FALSE' stop_updated = 'FALSE' # set_motor_power True as initial condition motor_power_cmd.data = 1 # Control loop -------------------------------------------- while not rospy.is_shutdown(): warning_now = 'FALSE' stop_now = 'FALSE' if trigger == 'TRUE': # convert laser_scan data to Point_Cloud2 point_cloud2_msg = laser_projection.projectLaser(msg) # read pc2 as points point_generator = pc2.read_points(point_cloud2_msg) for point in point_generator: x = point[0] y = point[1] # access as co-ordinates p = Point(x * 1000, y * 1000) if p.within(warn_zone): if p.within(stop_zone): stop_now = 'TRUE' else: warning_now = 'TRUE' # check for stop_zone condition -------------------- if stop_now == 'TRUE' or stop_updated == 'TRUE': motor_power_cmd.data = 0 motor_power_pub.publish(motor_power_cmd) coll_warn_pub.publish(coll_warn_msg[2]) print('stop') # check for warn_zone condition -------------------- elif warning_now == 'TRUE': motor_power_cmd.data = 1 coll_warn_pub.publish(coll_warn_msg[1]) print('warning') else: if warning_updated == 'TRUE' or stop_updated == 'TRUE': motor_power_cmd.data = 1 motor_power_pub.publish(motor_power_cmd) print('clear') coll_warn_pub.publish(coll_warn_msg[0]) # update warning and stop warning_updated = warning_now stop_updated = stop_now # set trigger as False trigger = 'FALSE' rate.sleep()
def __init__(self): rospy.init_node("uw_teleop") ##### try laser_assembler ##### rospy.wait_for_service("assemble_scans2") self.assemble_scans = rospy.ServiceProxy('assemble_scans2', AssembleScans2) ############################### self.vel_pub1 = rospy.Publisher('/dataNavigator1', Odometry, queue_size=10) self.vel_pub2 = rospy.Publisher('/dataNavigator2', Odometry, queue_size=10) self.lidar1_pub = rospy.Publisher('/dataNavigatorlidar1', Odometry, queue_size=10) self.lidar2_pub = rospy.Publisher('/dataNavigatorlidar2', Odometry, queue_size=10) self.lidar3_pub = rospy.Publisher('/dataNavigatorlidar3', Odometry, queue_size=10) self.lidar4_pub = rospy.Publisher('/dataNavigatorlidar4', Odometry, queue_size=10) self.lidar5_pub = rospy.Publisher('/dataNavigatorlidar5', Odometry, queue_size=10) self.lidar6_pub = rospy.Publisher('/dataNavigatorlidar6', Odometry, queue_size=10) self.lidar7_pub = rospy.Publisher('/dataNavigatorlidar7', Odometry, queue_size=10) self.lidar8_pub = rospy.Publisher('/dataNavigatorlidar8', Odometry, queue_size=10) self.lidar9_pub = rospy.Publisher('/dataNavigatorlidar9', Odometry, queue_size=10) self.lidar10_pub = rospy.Publisher('/dataNavigatorlidar10', Odometry, queue_size=10) self.lidar11_pub = rospy.Publisher('/dataNavigatorlidar11', Odometry, queue_size=10) self.lidar12_pub = rospy.Publisher('/dataNavigatorlidar12', Odometry, queue_size=10) self.rate = rospy.Rate(20) self.joy_sub = rospy.Subscriber("/joy", Joy, self.joy_msg_callback) self.bf1_odom_sub = rospy.Subscriber("/uwsim/BlueFox1Odom", Odometry, self.pose1_callback) self.bf2_odom_sub = rospy.Subscriber("/uwsim/BlueFox2Odom", Odometry, self.pose2_callback) self.bf1_lidar_sub = rospy.Subscriber("/lidar/multibeam", LaserScan, self.lidar_callback) #self.bf1_lidardup_sub = rospy.Subscriber("/lidar/multibeam", LaserScan, self.lidardup_callback) self.bf1_laser_sub = rospy.Subscriber("/BlueFox1/multibeam11", LaserScan, self.laser1_callback) self.bf2_laser_sub = rospy.Subscriber("/BlueFox2/multibeam", LaserScan, self.laser2_callback) # messages required by /ndt_mapping self.lidar_pc2_pub = rospy.Publisher('velodyne_points', PointCloud2, queue_size=10) self.imu_pub = rospy.Publisher("/imu_raw", Imu, queue_size=10) self.vehicle_odom_pub = rospy.Publisher('/odom_pose', Odometry, queue_size=10) self.joy_data = Joy() self.vel_cmd1 = Odometry() self.vel_cmd2 = Odometry() self.bf1_pose = None self.bf2_pose = None self.odom_hz = 10 self.bf1_vel = None self.bf2_vel = None self.bf1_laser = LaserScan() self.bf2_laser = LaserScan() self.joy_button = None self.lidar_counter = 0 # this is a hack to make lidar spin self.laser_projection = LaserProjection() self.velodyne_mapped_laserscan = LaserScan() self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
def test_project_laser(self): tolerance = 6 # decimal places projector = LaserProjection() ranges = [-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 100.0] intensities = np.arange(1.0, 6.0).tolist() min_angles = -np.pi / np.array([1.0, 1.5, 2.0, 4.0, 8.0]) max_angles = -min_angles angle_increments = np.pi / np.array([180., 360., 720.]) scan_times = [rospy.Duration(1. / i) for i in [40, 20]] for range_val, intensity_val, \ angle_min, angle_max, angle_increment, scan_time in \ product(ranges, intensities, min_angles, max_angles, angle_increments, scan_times): try: scan = build_constant_scan(range_val, intensity_val, angle_min, angle_max, angle_increment, scan_time) except BuildScanException: if (angle_max - angle_min) / angle_increment > 0: self.fail() cloud_out = projector.projectLaser( scan, -1.0, LaserProjection.ChannelOption.INDEX) self.assertEquals( len(cloud_out.fields), 4, "PointCloud2 with channel INDEX: fields size != 4") cloud_out = projector.projectLaser( scan, -1.0, LaserProjection.ChannelOption.INTENSITY) self.assertEquals( len(cloud_out.fields), 4, "PointCloud2 with channel INDEX: fields size != 4") cloud_out = projector.projectLaser(scan, -1.0) self.assertEquals( len(cloud_out.fields), 5, "PointCloud2 with channel INDEX: fields size != 5") cloud_out = projector.projectLaser( scan, -1.0, LaserProjection.ChannelOption.INTENSITY | LaserProjection.ChannelOption.INDEX) self.assertEquals( len(cloud_out.fields), 5, "PointCloud2 with channel INDEX: fields size != 5") cloud_out = projector.projectLaser( scan, -1.0, LaserProjection.ChannelOption.INTENSITY | LaserProjection.ChannelOption.INDEX | LaserProjection.ChannelOption.DISTANCE) self.assertEquals( len(cloud_out.fields), 6, "PointCloud2 with channel INDEX: fields size != 6") cloud_out = projector.projectLaser( scan, -1.0, LaserProjection.ChannelOption.INTENSITY | LaserProjection.ChannelOption.INDEX | LaserProjection.ChannelOption.DISTANCE | LaserProjection.ChannelOption.TIMESTAMP) self.assertEquals( len(cloud_out.fields), 7, "PointCloud2 with channel INDEX: fields size != 7") valid_points = 0 for i in range(len(scan.ranges)): ri = scan.ranges[i] if (PROJECTION_TEST_RANGE_MIN <= ri and ri <= PROJECTION_TEST_RANGE_MAX): valid_points += 1 self.assertEqual(valid_points, cloud_out.width, "Valid points != PointCloud2 width") idx_x = idx_y = idx_z = 0 idx_intensity = idx_index = 0 idx_distance = idx_stamps = 0 i = 0 for f in cloud_out.fields: if f.name == "x": idx_x = i elif f.name == "y": idx_y = i elif f.name == "z": idx_z = i elif f.name == "intensity": idx_intensity = i elif f.name == "index": idx_index = i elif f.name == "distances": idx_distance = i elif f.name == "stamps": idx_stamps = i i += 1 i = 0 for point in pc2.read_points(cloud_out): ri = scan.ranges[i] ai = scan.angle_min + i * scan.angle_increment self.assertAlmostEqual(point[idx_x], ri * np.cos(ai), tolerance, "x not equal") self.assertAlmostEqual(point[idx_y], ri * np.sin(ai), tolerance, "y not equal") self.assertAlmostEqual(point[idx_z], 0, tolerance, "z not equal") self.assertAlmostEqual(point[idx_intensity], scan.intensities[i], tolerance, "Intensity not equal") self.assertAlmostEqual(point[idx_index], i, tolerance, "Index not equal") self.assertAlmostEqual(point[idx_distance], ri, tolerance, "Distance not equal") self.assertAlmostEqual(point[idx_stamps], i * scan.time_increment, tolerance, "Timestamp not equal") i += 1
def laserCb(msg): projector = LaserProjection() cloud_out = projector.projectLaser(msg) pub_ptcl.publish(cloud_out)