#!/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)
