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