def __init__(self):
self.tfListener = tf.TransformListener()
self.lp = lg.LaserProjection()
rospy.sleep(0.1)
# initialize a list for all possible ammobox sites
self.ammobox_list = []
if LOCAL:
for abp in AmmoBoxLocalPos[ROLE]:
self.ammobox_list.append(AmmoBoxLocation(abp))
else:
for abp in AmmoBoxGlobalPos:
self.ammobox_list.append(AmmoBoxLocation(abp))
# process laserscan data
self.sub_top_lidar = rospy.Subscriber("scan2", LaserScan,
self.TopLidarCB)
self.pub_pc = rospy.Publisher("converted_pc", PointCloud, queue_size=1)
# publish the marker array to visualize in rviz
self.pub_ammo_detect = rospy.Publisher("ammo_detect",
AmmoDetect,
queue_size=1)
self.pub_markers = rospy.Publisher("ammobox_markers",
MarkerArray,
queue_size=1)
self.marker_timer = rospy.Timer(rospy.Duration(0.1),
self.MarkerTimerCB)
def __init__(self):
# Start ROS
rospy.init_node('scan_to_file', anonymous=False)
# Get Ros Params
self.world_frame_id = rospy.get_param("~world_frame_id")
self.scan_frame_id = rospy.get_param("~scan_frame_id")
self.scan_topic = rospy.get_param("~scan_topic")
self.save_to = rospy.get_param("~save_to")
# ROS Subs and pubs
rospy.Subscriber(self.scan_topic, LaserScan, self.laser_callback)
# Regual variables
self.mem_file = "memory.txt"
filename = "pointcloud_%d.txt"%(self.get_counter())
self.f = open(self.save_to + filename, "w")
self.update_counter()
# ROS variables
self.point_msg = PointStamped()
self.point_msg.header.frame_id = self.scan_frame_id
self.point_msg.point.z = 0.0
self.tl = tf.TransformListener(True, rospy.Duration(10))
self.lp = lg.LaserProjection()
rospy.loginfo("scan_to_file started, ready to process LaserScan")
rospy.spin()
def callback(scan):
print len(scan.ranges)
lp = lg.LaserProjection()
cloud = lp.projectLaser(scan)
# print cloud
points = pc2.read_points(cloud)
# print type(points)
objPoints = np.array(map(extract, points))
# print len(objPoints)
objPoints = np.reshape(objPoints,
(1, objPoints.shape[0], objPoints.shape[1]))
print objPoints[0]
img_points, _ = cv2.fisheye.projectPoints(objPoints, rvec, tvec, K, D)
# print img_points[0][0]
target_img_points = np.array(
[[img_points[0][0][0], img_points[0][0][1], 1]])
# world = np.dot(np.dot(target_img_points, np.linalg.inv(K)) - np.reshape(tvec,[3,-1]),np.linalg.inv(rot_mat))
world = np.dot(
np.dot(target_img_points, np.linalg.inv(K)) - tvec,
np.linalg.inv(rot_mat))
# print [img_points[0][0][0],img_points[0][0][1],0.0]
# new_obj, _ = cv2.fisheye.projectPoints([img_points[0][0][0],img_points[0][0][1],0.0], rvec, tvec, K, D)
# print new_obj
point_out = cv2.fisheye.undistortPoints(img_points, K=K, D=D)[0]
print point_out[0]
def __init__(self):
# Visualization topics:
self.visual_cart_footprint_estimator = rospy.Publisher(
"/visual_cart_footprint_estimator", PolygonStamped, queue_size=1)
self.visual_laser_msg = rospy.Publisher("/cart_filtered_scan",
LaserScan,
queue_size=1)
self.visual_cart = rospy.Publisher("/isolated_cart_scan",
LaserScan,
queue_size=1)
# The cart status:
self.the_cart_pub = rospy.Publisher("/cart_is_holded",
String,
queue_size=1)
self.ref_cart_legs = [(1.375, 0.225), (0.705, 0.225), (0.705, -0.225),
(1.375, -0.225)]
self.the_cart = False
# Cart footprint estimator message
self.cart_footprint_estimator = PolygonStamped()
self.cart_footprint_estimator.header.frame_id = "base_link"
# Handlers and feedback data:
self.filtered_laser_msg = LaserScan()
self.cart_isolated_laser_msg = LaserScan()
self.laser_projection = lg.LaserProjection()
self.sss = simple_script_server()
def __init__(self):
self.ready = False
rospy.init_node("obstacle_avoidance")
rospy.loginfo("Starting obstacle avoidance node...")
rate = rospy.get_param('~rate', 10)
self.update_rate = rospy.Rate(rate)
self.visualize = rospy.get_param('~visualize', True)
srv = Server(ObstacleAvoidanceConfig, self.paramCB)
self.twist_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
self.odom_sub = rospy.Subscriber("/odom", Odometry, self.odomCB)
self.odom_msg = Odometry()
self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped,
self.goalCB)
self.goal_msg = PoseStamped()
self.laser_sub = rospy.Subscriber("/scan", LaserScan, self.laserCB)
self.laser_msg = LaserScan()
if self.visualize:
self.projections_pub = rospy.Publisher("/projections",
PoseArray,
queue_size=10)
self.best_projection_pub = rospy.Publisher("/best_projection",
Pose,
queue_size=10)
robot_clearance = Rect()
# Assume robot is a 40x40cm box with the center at 0,0
robot_clearance.xmin = rospy.get_param('~xmin', 0.2)
robot_clearance.ymin = rospy.get_param('~ymin', 0.2)
robot_clearance.xmax = rospy.get_param('~xmax', 0.2)
robot_clearance.ymax = rospy.get_param('~ymax', 0.2)
self.config = Config()
self.config.maxSpeed = rospy.get_param('~maxSpeed', 0.50)
self.config.minSpeed = rospy.get_param('~minSpeed', -0.5)
self.config.maxYawrate = rospy.get_param('~maxYawrate', 1.00)
self.config.maxAccel = rospy.get_param('~maxAccel', 1.0)
self.config.maxdYawrate = rospy.get_param('~maxdYawrate', 1.00)
self.config.velocityResolution = rospy.get_param(
'~velocityResolution', 0.00505)
self.config.yawrateResolution = rospy.get_param(
'~yawrateResolution', 0.00505)
self.config.dt = rospy.get_param('~dt', 0.1)
self.config.predictTime = rospy.get_param('~predictTime', 0.1)
self.config.heading = rospy.get_param('~heading', 0.15)
self.config.clearance = rospy.get_param('~clearance', 1.00)
self.config.velocity = rospy.get_param('~velocity', 1.00)
self.config.base = robot_clearance
# Path planner is used to compute next command
self.path_planner = PathPlanner(self.config)
# Used to create pointcloud from laser data
self.lp = lg.LaserProjection()
rospy.loginfo("Obstacle avoidance node setup complete!")
self.ready = True
def callback(scan, image):
lp = lg.LaserProjection()
cloud = lp.projectLaser(scan)
# print cloud
points = pc2.read_points(cloud)
print type(points)
objPoints = np.array(map(extract, points))
print objPoints
def __init__(self):
self.lp = lg.LaserProjection()
print(sys.version)
rospy.init_node('slam_cloud2')
self.scan_sub = rospy.Subscriber('/hector/scanningLidar/laser_scan',
LaserScan,
self.convert_and_publish) # make node
self.pc2_pub = rospy.Publisher('/PointCloud2',
PointCloud2,
queue_size=1)
def __init__(self):
rospy.loginfo("Starting scan_to_pc2 converter")
scan_name = rospy.get_param("~scan_name")
pc2_name = rospy.get_param("~pc2_name")
#rospy.loginfo("%s is %s", rospy.resolve_name('/scan_in'), scan_name)
rospy.Subscriber(scan_name, LaserScan, self.laser_callback)
self.pc2_pub = rospy.Publisher(pc2_name, PointCloud2, queue_size=10)
self.lp = lg.LaserProjection()
rospy.spin()
def __init__(self):
rospy.init_node('location_service')
self.lp = lg.LaserProjection()
self.transformation_matrix = read_transformation_matrix(
rospy.get_param('~laser_calibration'))
self.translation_vector = self.transformation_matrix[:3, 3]
self.rotation_matrix = self.transformation_matrix[:3, :3]
self.rotation_vector, _ = cv2.Rodrigues(self.rotation_matrix)
self.lens, self.K, self.D = read_instrinsic_calibration(
rospy.get_param('~camera_calibration'))
rospy.Service(rospy.get_param('~service_name', 'location'),
LocationSrv, self.project_location)
def __init__(self):
# get parameters
lrs_topic = rospy.get_param("~lrs_topic")
lrs_pc2_topic = rospy.get_param("~lrs_pc2_topic")
# initialize
self.laser_pjojection = lg.LaserProjection()
self.points_num = None
self.lrs_frame = None
self.sub_lrs = rospy.Subscriber(lrs_topic, LaserScan, self.lrs_handler)
self.pub_lrs_pc2 = rospy.Publisher(lrs_pc2_topic,
PointCloud2,
queue_size=1)
self.tf_listener = tf.TransformListener()
self.processing_flag = False
class LaserSubs(object):
laser_coordinate = None
lp = lg.LaserProjection()
def __init__(self):
rospy.Subscriber('/base_scan', LaserScan, self.callback)
# rospy.Subscriber('scan', LaserScan, self.callback)
def callback(self, msg):
# global LOCK
# if (not LOCK):
pc2_msg = self.lp.projectLaser(msg)
point_generator = pc2.read_points_list(pc2_msg)
self.laser_coordinate = np.asarray(point_generator)[:, :2]
def get_laser_data(self):
return np.copy(self.laser_coordinate)
def __init__(self):
# General Variables
self.ref_frame = "/map" # If SLAM got messed up change "/map" to "/odom"
# ROS variables
self.point_msg = PointStamped()
self.point_msg.header.frame_id = "rplidar_laser"
self.point_msg.point.z = 0.0
self.tl = tf.TransformListener(True, rospy.Duration(10))
self.lp = lg.LaserProjection()
# IMPORTANT: For older scans subscribe to "rplidar_scan" instead of "scan1"
rospy.Subscriber("scan1", LaserScan, self.processLaser2)
self.pc_pub = rospy.Publisher("converted_pc",
PointCloud2,
queue_size=1)
rospy.spin()
def __init__(self):
rospy.init_node("person_follower")
rate = rospy.get_param('~rate', 10)
self.update_rate = rospy.Rate(rate)
self.debug = rospy.get_param('~debug', False)
self.kp1 = rospy.get_param('~kp1', 0.5)
self.kp2 = rospy.get_param('~kp2', 1)
self.person_pub = rospy.Publisher("/person_position",
PointStamped,
queue_size=10)
self.twist_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
self.laser_sub = rospy.Subscriber("/scan", LaserScan, self.laserCB)
self.laser_msg = LaserScan()
self.lp = lg.LaserProjection()
def __init__(self):
# Visualization topics:
self.visual_cart_safety_footprint = rospy.Publisher(
"/visual_cart_safety_footprint", PolygonStamped, queue_size=10)
self.visual_cart_footprint_estimator = rospy.Publisher(
"/visual_cart_footprint_estimator", PolygonStamped, queue_size=10)
self.visual_laser_msg = rospy.Publisher("/cart_filtered_scan",
LaserScan,
queue_size=1)
self.visual_cart = rospy.Publisher("/isolated_cart_scan",
LaserScan,
queue_size=1)
# The cart status:
self.ref_cart_legs = [(1.375, 0.225), (0.705, 0.225), (0.705, -0.225),
(1.375, -0.225)]
self.the_cart = False
self.cart_legs = []
self.cart_area = 0.2
self.cart_wheel_diameter = 0.11
# Cart safty footprint message
self.cart_safety_footprint = PolygonStamped()
self.cart_safety_footprint.header.stamp = rospy.Time.now()
self.cart_safety_footprint.header.frame_id = "base_link"
self.cart_safety_footprint.polygon.points = [
Point(1.375, 0.225, 0.0),
Point(1.375, -0.225, 0.0),
Point(0.705, -0.225, 0.0),
Point(0.705, 0.225, 0.0)
]
# Cart footprint estimator message
self.cart_footprint_estimator = PolygonStamped()
self.cart_footprint_estimator.header.frame_id = "base_link"
# Robot status:
self.current_position = Odometry()
self.standing_position = []
# Handlers and feedback data:
self.filtered_laser_msg = LaserScan()
self.cart_isolated_laser_msg = LaserScan()
self.laser_projection = lg.LaserProjection()
self.sss = simple_script_server()
def obstacle_check(scan_msg):
distances = []
lp = lg.LaserProjection()
# convert laser scan to point cloud
pc2_msg = lp.projectLaser(scan_msg)
point_generator = pc2.read_points(pc2_msg)
# calculate how far away each point is
for point in point_generator:
if not math.isnan(point[2]):
distances.append(np.sqrt(point[0]**2 + point[1]**2))
length = len(distances)
mid = length / 2
# only consider points in front of robot
if (length >= 65):
distances = distances[mid - 65 / 2:mid + 65 / 2]
if min(distances) < 0.6:
return True
return False
#!/usr/bin/env python
import sensor_msgs.point_cloud2 as pc2
import rospy
from sensor_msgs.msg import PointCloud2, LaserScan
import laser_geometry.laser_geometry as lg
import math
rospy.init_node("laserscan_to_pointcloud")
lp = lg.LaserProjection()
pc_pub = rospy.Publisher("sync_scan_cloud_filtered", PointCloud2, queue_size=1)
def scan_cb(msg):
# convert the message of type LaserScan to a PointCloud2
pc2_msg = lp.projectLaser(msg)
pc2_msg.header.frame_id = "/camera"
# now we can do something with the PointCloud2 for example:
# publish it
pc_pub.publish(pc2_msg)
# convert it to a generator of the individual points
point_generator = pc2.read_points(pc2_msg)
# we can access a generator in a loop
sum = 0.0
num = 0
for point in point_generator:
def scandata(msg):
global point_list
pointcloud_msg = lasergeometry.LaserProjection().projectLaser(msg)
point_list = point.read_points_list(pointcloud_msg)
class hsvFilter:
def __init__(self):
self.node_name = "image_overlay"
rospy.init_node(self.node_name)
rospy.on_shutdown(self.cleanup)
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.tf_prefix = rospy.get_param('~tf_prefix')
rpy_offset_name = rospy.get_param('~rpy_offset')
self.offset_roll = rospy.get_param(rpy_offset_name + "/roll")
self.offset_pitch = rospy.get_param(rpy_offset_name + "/pitch")
self.offset_yaw = rospy.get_param(rpy_offset_name + "/yaw")
self.image_pub = rospy.Publisher("view_image", Image, queue_size=1)
self.image_sub = rospy.Subscriber("input_image",
Image,
self.image_callback,
queue_size=1)
self.objects_sub = rospy.Subscriber("input_objects",
TrackedObjectArray,
self.objects_callback,
queue_size=1)
self.laser_sub = rospy.Subscriber("scan",
LaserScan,
self.laser_callback,
queue_size=1)
self.last_laser = None
self.last_objects = TrackedObjectArray()
self.focus_sub = rospy.Subscriber("focus",
TrackedInfo,
self.focus_callback,
queue_size=1)
self.last_focus = TrackedInfo()
self.voltage_sub = rospy.Subscriber("voltage",
Float32,
self.voltage_callback,
queue_size=1)
self.last_voltage = 0.0
self.point_sub = rospy.Subscriber("point",
Point,
self.point_callback,
queue_size=1)
self.last_point = Point()
self.info_sub = rospy.Subscriber("camera_info",
CameraInfo,
self.info_callback,
queue_size=1)
self.last_info = CameraInfo
def image_callback(self, ros_image):
try:
frame = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
except CvBridgeError, e:
print e
image = np.array(frame, dtype=np.uint8)
#objects
for object in self.last_objects.objects:
color = (0, 255, 0)
if self.last_focus.category == object.info.category and self.last_focus.id == object.info.id:
color = (255, 100, 100)
r = object.rect
cv2.rectangle(image, (r.x, r.y), (r.x + r.width, r.y + r.height),
color,
thickness=3)
text1 = object.info.category
text2 = "id:" + str(object.info.id)
cv2.putText(image,
text1, (r.x, r.y - 25),
cv2.FONT_HERSHEY_PLAIN,
1.5, (0, 255, 0),
thickness=2)
cv2.putText(image,
text2, (r.x, r.y - 5),
cv2.FONT_HERSHEY_PLAIN,
1.5, (0, 255, 0),
thickness=2)
#laser
lp = lg.LaserProjection()
if self.last_laser is not None:
center_u = 640
center_v = 210
scale = 200 # m -> px
box_pu = 150
box_nu = 150
box_pv = 50
box_nv = 200
(trans, quat) = self.listener.lookupTransform(
self.tf_prefix + '/base_link',
self.tf_prefix + '/sensor0/front_laser_link', rospy.Time(0))
rpy = tf.transformations.euler_from_quaternion(quat)
offset_x = trans[0]
offset_y = trans[1]
rotate_z = rpy[2]
color = (50, 50, 250)
limit_nu = center_u - box_nu
limit_nv = center_v - box_nv
limit_pu = center_u + box_pu
limit_pv = center_v + box_pv
cv2.rectangle(image, (limit_nu, limit_nv), (limit_pu, limit_pv),
color,
thickness=3)
cv2.circle(image, (int(center_u), int(center_v)),
int(scale * 0.12), (0, 0, 255), 3)
pc2_msg = lp.projectLaser(self.last_laser)
point_list = pc2.read_points_list(pc2_msg)
for point0 in point_list:
point_x = math.cos(rotate_z) * point0.x - math.sin(
rotate_z) * point0.y + offset_x
point_y = math.sin(rotate_z) * point0.x + math.cos(
rotate_z) * point0.y + offset_y
point_u = int(center_u - scale * point_y)
point_v = int(center_v - scale * point_x)
if limit_nu <= point_u <= limit_pu and limit_nv <= point_v <= limit_pv:
cv2.circle(image, (point_u, point_v), 3, (0, 0, 255), -1)
# voltage
text_v = "V: " + "{:.1f}".format(self.last_voltage)
cv2.putText(image,
text_v, (10, 50),
cv2.FONT_HERSHEY_PLAIN,
3.0, (0, 0, 255),
thickness=3)
# point
if self.last_point.x > 0.1:
(trans, rot) = self.listener.lookupTransform(
self.tf_prefix + '/sensor0/head_camera_link',
self.tf_prefix + '/gun0/standard', rospy.Time(0))
R = tf.transformations.quaternion_matrix(rot)
T = tf.transformations.translation_matrix(trans)
CR = tf.transformations.euler_matrix(-self.offset_roll,
-self.offset_pitch,
-self.offset_yaw)
px = np.array(
[self.last_point.x, self.last_point.y, self.last_point.z]).T
px1 = px / np.linalg.norm(px) * 0.5
px2 = px / np.linalg.norm(px) * 1.5
px1 = np.mat([px1[0], px1[1], px1[2], 1]).T
out1 = CR * (T * (R * px1))
px2 = np.mat([px2[0], px2[1], px2[2], 1]).T
out2 = CR * (T * (R * px2))
pp = image_geometry.PinholeCameraModel()
pp.fromCameraInfo(self.last_info)
uv1 = pp.project3dToPixel(out1[0:3])
uv2 = pp.project3dToPixel(out2[0:3])
cv2.circle(image, (int(uv1[0]), int(uv1[1])), 30, (0, 0, 255), 2)
cv2.circle(image, (int(uv2[0]), int(uv2[1])), 20, (0, 0, 255), 2)
msg = self.bridge.cv2_to_imgmsg(image, encoding="bgr8")
self.image_pub.publish(msg)
def __init__(self, scan_topic_name):
self.raw = LaserScan()
self.cloud = None
self.lp = lg.LaserProjection()
self.__scan_sub = rospy.Subscriber(scan_topic_name, LaserScan, self.__scanCB)
def run(args):
rospy.init_node('rrt_navigation')
print("START")
# Update control every 100 ms.
rate_limiter = rospy.Rate(100)
publisher = rospy.Publisher('/'+ROBOT_NAME+'/cmd_vel', Twist, queue_size=5)
# path_publisher = rospy.Publisher(
# '/'+ROBOT_NAME+'/path', Path, queue_size=1)
follow_point_publisher = rospy.Publisher('/follow', Marker, queue_size=1)
centroids_publisher = rospy.Publisher('/centroids', Marker, queue_size=1)
slam = SLAM()
goal = GoalPose()
lp = lg.LaserProjection()
laser = SimpleLaser(lp, name=ROBOT_NAME)
frame_id = 0
current_path = []
previous_time = rospy.Time.now().to_sec()
# Stop moving message.
stop_msg = Twist()
stop_msg.linear.x = 0.
stop_msg.angular.z = 0.
# Make sure the robot is stopped.
i = 0
while i < 10 and not rospy.is_shutdown():
publisher.publish(stop_msg)
rate_limiter.sleep()
i += 1
print("START2")
while not rospy.is_shutdown():
slam.update()
current_time = rospy.Time.now().to_sec()
# Make sure all measurements are ready.
# Get map and current position through SLAM:
# > roslaunch exercises slam.launch
if not slam.ready or not laser.ready:
rate_limiter.sleep()
continue
# Follow path using feedback linearization.
position = np.array([
slam.pose[X] + EPSILON * np.cos(slam.pose[YAW]),
slam.pose[Y] + EPSILON * np.sin(slam.pose[YAW])], dtype=np.float32)
# v = get_velocity(position, np.array(current_path, dtype=np.float32))
# follow is relative to robot frame
follow = get_follow_position(position, laser)
# 20cm away from object is good enough
goal_reached = np.linalg.norm(follow) < .2
if goal_reached:
print("Goal Reached")
publisher.publish(stop_msg)
rate_limiter.sleep()
continue
v = cap(0.2*follow, SPEED)
print("v: ", v)
u, w = feedback_linearized(slam.pose, v, epsilon=EPSILON)
vel_msg = Twist()
vel_msg.linear.x = u
vel_msg.angular.z = w
print(vel_msg)
publisher.publish(vel_msg)
# publish point that is being followed
marker = Marker()
marker.header.frame_id = "/"+ROBOT_NAME+"/base_link"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
f = Point()
f.x = follow[X]
f.y = follow[Y]
marker.points = [f]
t = rospy.Duration()
marker.lifetime = t
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 1.0
follow_point_publisher.publish(marker)
rate_limiter.sleep()
frame_id += 1
#!/usr/bin/env python
import rospy
import cv2
from sensor_msgs.msg import CameraInfo, PointCloud2, LaserScan
import sensor_msgs.point_cloud2 as pc2
import laser_geometry.laser_geometry as lg
import yaml
import time
from math import sin, cos, tan, isnan
import image_geometry as geo
lidarProj = lg.LaserProjection()
# Load calibration
f = "/home/ubuntu/FreespaceProject/calib/c920.yaml"
data = None
with open(f, "r") as stream:
data = yaml.load(stream)
calib = CameraInfo()
calib.width = data["image_width"]
calib.height = data["image_height"]
calib.K = data["camera_matrix"]["data"]
calib.D = data["distortion_coefficients"]["data"]
calib.R = data["rectification_matrix"]["data"]
calib.P = data["projection_matrix"]["data"]
calib.distortion_model = data["distortion_model"]
lidarMsg = None
