Example #1
0
def main():
    rospy.init_node('fake_scan', anonymous=True)
    scan_subscriber = rospy.Subscriber('/remapped_scan', LaserScan,
                                       scan_callback)
    scan_publisher = rospy.Publisher('/scan', LaserScan, queue_size=10)
    rate = rospy.Rate(3)
    m = 720
    empty_scan_msg = LaserScan()
    empty_scan_msg.angle_min = -2.3561899662
    empty_scan_msg.angle_max = 2.3561899662
    empty_scan_msg.angle_increment = 0.0065540750511
    empty_scan_msg.time_increment = 0.0
    empty_scan_msg.scan_time = 0.0
    empty_scan_msg.range_min = 0.10000000149
    empty_scan_msg.range_max = 30.0
    empty_scan_msg.ranges = [0.0 for i in range(m)]
    empty_scan_msg.intensities = [float('inf') for i in range(m)]
    h = std_msgs.msg.Header()
    h.frame_id = "base_laser"
    for i in range(10):
        h.stamp = rospy.Time.now()
        h.seq += 1
        empty_scan_msg.header = h
        scan_publisher.publish(empty_scan_msg)
        rate.sleep()
    #infinite range, zero intensity --> can't detect anyting
    empty_scan_msg.ranges = [float('inf') for i in range(m)]
    empty_scan_msg.intensities = [0.0 for i in range(m)]
    while not rospy.is_shutdown():
        h.stamp = rospy.Time.now()
        h.seq += 1
        empty_scan_msg.header = h
        scan_publisher.publish(empty_scan_msg)
        rate.sleep()
Example #2
0
    def create_lidar_msg(self, L):

        raw_lidar = L.data
        #stripped_lidar = raw_lidar.translate(None, '[]').translate(None, '"').translate(None, '\'')
        array_lidar = raw_lidar.split(";")

        lidar_msg = LaserScan()
        lidar_msg.header = self.create_header()  #self?
        lidar_msg.angle_min = math.radians(float(array_lidar[0]))
        lidar_msg.angle_max = math.radians(float(array_lidar[1]))
        lidar_msg.angle_increment = math.radians(0.25)  #MAKE PARAM
        lidar_msg.time_increment = 0.025 / (
            270 * 4)  #time in ms / measurements YOYOYOYO CHECK THIS
        lidar_msg.scan_time = float(array_lidar[2]) / 1000  #time in ms
        lidar_msg.range_min = float(
            array_lidar[4]) / 1000  #sent in mm, should be meters
        lidar_msg.range_max = float(
            array_lidar[5]) / 1000  #sent in mm, should be meters

        array_string = array_lidar[3]  #.translate(None, '[]')
        string_array = array_string.split(",")
        lidar_msg.ranges = [float(r) / 1000
                            for r in string_array]  #better way?

        self.scanPub.publish(lidar_msg)
    def _processLaserscan(self, readingType, remappedTimestamp, content):
        # FIXME: For type ROBOTLASER, we should publish the robot/sensor pose using TF and use the correct TF frame
        laserscan = LaserScan()
        laserscan.header = Header(stamp=remappedTimestamp, frame_id="odom", seq=self._laserscanCounter)

        if readingType.startswith("RAWLASER") or readingType.startswith("ROBOTLASER"):
            laserscan.angle_min = float(content[1])
            laserscan.angle_max = laserscan.angle_min + float(content[2])
            laserscan.angle_increment = float(content[3])
            laserscan.time_increment = 0
            laserscan.scan_time = 0.0  # FIXME
            laserscan.range_min = 0
            laserscan.range_max = float(content[4])

            numRanges = int(content[7])
            for i in xrange(0, numRanges):
                laserscan.ranges.append( float(content[8 + i]) )

            numRemissions = int(content[8 + numRanges])
            for i in xrange(0, numRemissions):
                laserscan.intensities.append( float(content[9 + numRanges + i]) )

        else:
            rospy.logwarn("Unsupported laser of type %s in line %d" % (readingType, self._lineCounter) )

        publisher = self._laserscanPublishers[ self._getLaserID(readingType, content) ]
        publisher.publish(laserscan)
        self._laserscanCounter += 1
Example #4
0
 def scanCB(self, msg):
     range_list = list(msg.ranges)
     intensity_list = list(msg.intensities)
     for i in xrange(len(msg.ranges)):
         current_angle = msg.angle_min + msg.angle_increment * i
         # Delete the first element
         if (current_angle < np.deg2rad(self.ang_min)):
             del range_list[0]
             del intensity_list[0]
         # Delete the last element
         elif (current_angle > np.deg2rad(self.ang_max)):
             del range_list[-1]
             del intensity_list[-1]
     new_scan = LaserScan()
     new_scan.header = msg.header
     new_scan.angle_min = np.deg2rad(self.ang_min)
     new_scan.angle_max = np.deg2rad(self.ang_max)
     new_scan.angle_increment = msg.angle_increment
     new_scan.time_increment = msg.time_increment
     new_scan.scan_time = msg.scan_time
     new_scan.range_min = msg.range_min
     new_scan.range_max = msg.range_max
     new_scan.ranges = range_list
     if (self.intensity == False):
         new_scan.intensities = []
     else:
         new_scan.intensities = intensity_list
     self.scan_pub.publish(new_scan)
def callback(data):
    laser_angular_range = 100
    laser = LaserScan()
    print type(data)
    middle_index = int(
        (data.angle_max - data.angle_min) / data.angle_increment) / 2
    angle_increment_degree = data.angle_increment * (180 / PI)
    print 'middle_index ', middle_index
    print 'angle_increment_degree ', angle_increment_degree
    for i in range(int(laser_angular_range / angle_increment_degree)):
        laser.ranges.append(
            data.ranges[middle_index -
                        int(laser_angular_range / angle_increment_degree / 2) +
                        i])

        #laser.intensities.append(data.intensities[middle_index-int(laser_angular_range/angle_increment_degree/2)+i])

    ####################################################
    laser.header = data.header
    laser.header.frame_id = 'laser'
    laser.angle_min = -laser_angular_range / 2 * (PI / 180)

    laser.angle_max = laser_angular_range / 2 * (PI / 180)
    laser.angle_increment = data.angle_increment
    laser.time_increment = data.time_increment
    laser.scan_time = data.scan_time
    laser.range_min = data.range_min
    laser.range_max = data.range_max
    print laser.angle_increment
    print(laser.angle_max - laser.angle_min) / laser.angle_increment - len(
        laser.ranges)
    #####################################################

    print time.time()
    laser_pub.publish(laser)
    def publish_filtered_laser_scan(self, laser_original_data,
                                    new_filtered_laser_range):

        length_range = len(laser_original_data.ranges)
        length_intensities = len(laser_original_data.intensities)

        laser_filtered_object = LaserScan()

        h = Header()
        h.stamp = rospy.Time.now(
        )  # Note you need to call rospy.init_node() before this will work
        h.frame_id = "chassis"

        laser_filtered_object.header = h
        laser_filtered_object.angle_min = laser_original_data.angle_min
        laser_filtered_object.angle_max = laser_original_data.angle_max
        laser_filtered_object.angle_increment = laser_original_data.angle_increment
        laser_filtered_object.time_increment = laser_original_data.time_increment
        laser_filtered_object.scan_time = laser_original_data.scan_time
        laser_filtered_object.range_min = laser_original_data.range_min
        laser_filtered_object.range_max = laser_original_data.range_max

        laser_filtered_object.ranges = []
        laser_filtered_object.intensities = []
        for item in new_filtered_laser_range:
            laser_filtered_object.ranges.append(item)
            laser_filtered_object.intensities.append(item)

        self.laser_filtered_pub.publish(laser_filtered_object)
Example #7
0
    def callback(self, msg):
        rate = rospy.Rate(10)

        scan_msg = LaserScan()
        scan_msg.header = msg.header
        scan_msg.angle_min = msg.angle_min
        scan_msg.angle_max = msg.angle_max
        scan_msg.angle_increment = msg.angle_increment
        scan_msg.time_increment = msg.time_increment
        scan_msg.scan_time = msg.scan_time
        scan_msg.range_min = msg.range_min
        scan_msg.range_max = msg.range_max
        #print np.shape(msg.ranges)
        count = int(msg.scan_time / msg.time_increment)
        scan_msg.ranges = np.zeros((count, ), dtype=np.float)
        scan_msg.intensities = np.zeros((count, ), dtype=np.float)

        for i in xrange(count):
            #if i < 359 or i > 1079:
            if i < 489 or i > 949:
                scan_msg.ranges[i] = msg.ranges[i]
                scan_msg.intensities[i] = msg.intensities[i]
            else:
                scan_msg.ranges[i] = float('inf')
                scan_msg.intensities[i] = 0.0
        #print "here"
        #rate.sleep()
        self.scan_pub.publish(scan_msg)
Example #8
0
    def publish_filtered_laser_scan(self, laser_original_data, new_filtered_laser_range):

        rospy.logdebug("new_filtered_laser_range==>" + str(new_filtered_laser_range))

        laser_filtered_object = LaserScan()

        h = Header()
        h.stamp = rospy.Time.now()  # Note you need to call rospy.init_node() before this will work
        h.frame_id = laser_original_data.header.frame_id

        laser_filtered_object.header = h
        laser_filtered_object.angle_min = laser_original_data.angle_min
        laser_filtered_object.angle_max = laser_original_data.angle_max

        new_angle_incr = abs(laser_original_data.angle_max - laser_original_data.angle_min) / len(
            new_filtered_laser_range)

        # laser_filtered_object.angle_increment = laser_original_data.angle_increment
        laser_filtered_object.angle_increment = new_angle_incr
        laser_filtered_object.time_increment = laser_original_data.time_increment
        laser_filtered_object.scan_time = laser_original_data.scan_time
        laser_filtered_object.range_min = laser_original_data.range_min
        laser_filtered_object.range_max = laser_original_data.range_max

        laser_filtered_object.ranges = []
        laser_filtered_object.intensities = []
        for item in new_filtered_laser_range:
            if item == 0.0:
                laser_distance = 0.1
            else:
                laser_distance = item
            laser_filtered_object.ranges.append(laser_distance)
            laser_filtered_object.intensities.append(item)

        self.laser_filtered_pub.publish(laser_filtered_object)
Example #9
0
    def laser_callback(self, msg):
        filtered_values = LaserScan()
        distance = np.array(msg.ranges)
        filtered_values.header = msg.header
        filtered_values.angle_increment = msg.angle_increment
        filtered_values.time_increment = msg.time_increment
        filtered_values.scan_time = msg.scan_time
        filtered_values.range_min = msg.range_min
        filtered_values.range_max = msg.range_max
        filtered_values.intensities = msg.intensities
        angle = filtered_values.angle_increment
        min_angle = msg.angle_min
        max_angle = msg.angle_max

        laser_noise_variance = rospy.get_param('laser_noise_variance')
        if laser_noise_variance <= 0:
            laser_noise_variance = 0.1

        filtered_values_ranges = np.zeros(len(distance))
        noise_values_ranges = np.random.normal(loc=0,
                                               scale=laser_noise_variance,
                                               size=len(distance))

        for i in range(len(distance)):
            filtered_values_ranges[i] = noise_values_ranges[i] + distance[i]

        filtered_values.ranges = filtered_values_ranges
        filtered_values.angle_min = min_angle
        filtered_values.angle_max = max_angle
        self.scan_pub.publish(filtered_values)
Example #10
0
    def data(self,a):
	angle = []
	for i in reversed(range(90)):
	    angle.append(i)
	for i in range(270,360):
	    angle.append(i)
	Range = [a.ranges[i] for i in angle]
#	rospy.loginfo_throttle(2,a.ranges[0])

	pub=rospy.Publisher('/CloudPoint',LaserScan,queue_size=10)
	msg=LaserScan()
	msg.header=a.header
	msg.header.frame_id = 'laser'
	msg.angle_min=a.angle_min
	msg.angle_max=a.angle_max
	msg.angle_increment=a.angle_increment
	msg.time_increment=a.time_increment
	msg.scan_time=a.scan_time
	msg.range_min=a.range_min
	msg.range_max=a.range_max
	List = self.ranges 
	New = numpy.hstack([List,Range])

	self.ranges = New
	if len(self.ranges) >=4500:
	    self.ranges = self.ranges[900:]
	msg.ranges=self.ranges
	msg.intensities=a.intensities
	pub.publish(msg)
	print len(msg.ranges)
	def scanCallback(self, scan):
		rmin = [0 , 315]
		rmax = [45, 360]
		reduced_scan = LaserScan()
		
		reduced_scan.header = scan.header
		reduced_scan.angle_min = scan.angle_min
		reduced_scan.angle_max = scan.angle_max	
		reduced_scan.angle_increment = scan.angle_increment
		reduced_scan.time_increment = scan.time_increment
		reduced_scan.scan_time = scan.scan_time
		reduced_scan.range_min = scan.range_min
		reduced_scan.range_max = scan.range_max
		reduced_scan.intensities = scan.intensities

		for i in range(len(scan.ranges)):
			if   i >= rmin[0] and i <= rmax[0]-1:
				reduced_scan.ranges.append(scan.ranges[i])
			elif i >= rmin[1] and i <= rmax[1]-1:
				reduced_scan.ranges.append(scan.ranges[i])
			else:
				reduced_scan.ranges.append(0)
			rospy.loginfo("val[%d]:%f", i, reduced_scan.ranges[i])

 		self.pub_reduced_scan.publish(reduced_scan)
Example #12
0
def main():

    rospy.init_node('test')  # to initiate and create an unique node
    rospy.Subscriber(
        '/tim561_scan', LaserScan,
        callback)  # subscribing  to the topic from lidar_test_data.bag
    pub = rospy.Publisher('/tim561_scan_filtered', LaserScan,
                          queue_size=10)  # Publisher for filtered data

    while laserscan is None and not rospy.is_shutdown(
    ):  # For when data is not available keeping the node on hold
        rospy.sleep(1)
    rate = rospy.Rate(10)
    new_data = LaserScan()  # Object for filtered data
    while not rospy.is_shutdown():
        new_data.header = laserscan.header  # Header is not changed in new data
        new_data.header.frame_id = 'laser'  # ID changed for rviz visualization
        new_data.angle_min = laserscan.angle_min  # Doesn't change after filtering
        new_data.angle_max = laserscan.angle_max  # Doesn't change after filtering
        new_data.angle_increment = laserscan.angle_increment  # Doesn't change after filtering
        new_data.time_increment = laserscan.time_increment  # Doesn't change after filtering

        new_data.ranges = lowpass(laserscan.ranges, laserscan.angle_increment,
                                  RC)  # Filtering the data
        new_data.range_min = min(
            new_data.ranges)  # Finding new minimum of filtered data
        new_data.range_max = max(
            new_data.ranges)  # Finding new maximum of filtered data

        new_data.intensities = laserscan.intensities  # Doesn't change after filtering(empty)
        pub.publish(new_data)  # Publishing filtered data
        rate.sleep()
Example #13
0
def callback(msg, ir_sensors):
    # Receive raw data and publish a value
    # The raw_data variable should be an array of numbers
    raw_data = msg.ranges
    distances_avg = ir_sensors.rawDataToDistance(raw_data)
    front_distances = distances_avg[0:6]
    obstacle_distances = distances_avg[6:]
    theta_p, distance, num_sensors, rotation_direction, pose = ir_sensors.distanceToPose(
        front_distances, msg.header
    )  # only take first 6 distances, those rorrespond to the front sensors
    rospy.loginfo(
        'Angle: {:.0f}, Distance: {:.2f}, Num Sensors: {:d}, Rot Dir: {:d}'.
        format(theta_p, distance, num_sensors, rotation_direction))
    rospy.loginfo(pose)
    pub_human_angle.publish(Float64(theta_p))
    pub_distance.publish(Float64(distance))
    pub_num_sensors.publish(Int8(num_sensors))
    pub_rotation_dir.publish(Int8(rotation_direction))
    pub_ir_pose.publish(pose)
    other_sensors = LaserScan()
    other_sensors.header = msg.header
    other_sensors.ranges = obstacle_distances
    pub_ir_other_sensors_distance.publish(other_sensors)
    obstacles = [0, 0, 0, 0, 0, 0, 0]
    for i in range(len(obstacle_distances)):
        if obstacle_distances[i] < ir_sensors.obstacle_distance_min:
            obstacles[i] = 1
    rospy.set_param_raw('obstacles', obstacles)
Example #14
0
def data_fusion(asus_data, laser_data, LaserData, transform):
    if asus_data != None and laser_data != None:
        data = LaserScan()
        data.angle_min = laser_data.angle_min
        data.angle_max = laser_data.angle_max
        data.header = laser_data.header
        data.time_increment = laser_data.time_increment
        data.angle_increment = laser_data.angle_increment
        data.scan_time = laser_data.scan_time
        data.range_min = laser_data.range_min
        data.range_max = laser_data.range_max
        data.ranges = []
        angle = laser_data.angle_min
        for i in laser_data.ranges:
            if -0.510 <= angle and angle <= 0.510:
                num = asus_data.angle_max
                for j in asus_data.ranges:
                    if abs(num - angle) <= asus_data.angle_increment:
                        if i > j:
                            i = j
                    num -= asus_data.angle_increment
            data.ranges.append(i)
            angle -= laser_data.angle_increment
        if len(data.ranges) == len(laser_data.ranges):
            LaserData.append(data)
    else:
        if asus_data == None:
            rospy.loginfo('wait for asus data')
        if laser_data == None:
            rospy.loginfo('wait for rplidar data')
    def _processLaserscan(self, readingType, remappedTimestamp, content):
        # FIXME: For type ROBOTLASER, we should publish the robot/sensor pose using TF and use the correct TF frame
        laserscan = LaserScan()
        laserscan.header = Header(stamp=remappedTimestamp, frame_id="odom", seq=self._laserscanCounter)

        if readingType.startswith("RAWLASER") or readingType.startswith("ROBOTLASER"):
            laserscan.angle_min = float(content[1])
            laserscan.angle_max = laserscan.angle_min + float(content[2])
            laserscan.angle_increment = float(content[3])
            laserscan.time_increment = 0
            laserscan.scan_time = 0.0  # FIXME
            laserscan.range_min = 0
            laserscan.range_max = float(content[4])

            numRanges = int(content[7])
            for i in xrange(0, numRanges):
                laserscan.ranges.append( float(content[8 + i]) )

            numRemissions = int(content[8 + numRanges])
            for i in xrange(0, numRemissions):
                laserscan.intensities.append( float(content[9 + numRanges + i]) )

        else:
            rospy.logwarn("Unsupported laser of type %s in line %d" % (readingType, self._lineCounter) )

        publisher = self._laserscanPublishers[ self._getLaserID(readingType, content) ]
        publisher.publish(laserscan)
        self._laserscanCounter += 1
        if (self.publishTracks):
            self._currentLaserMsgs[self._getLaserID(readingType, content)] = laserscan
Example #16
0
    def create_lidar_msg(lidar_string):
        lidar_msg = LaserScan()    
        data = lidar_string.split(";")
        #num_readings = 1440 --------------------------------
        #data[0] = min angle (degrees)
        #data[1] = max angle (degrees)
        #data[2] = timestep (ms)
        #data[3] = lidar scan array
        #data[4] = min range
        #data[5] = max range	

        #print data

        lidar_msg.header = create_header() #self?
        lidar_msg.angle_min = math.radians(float(data[0]))
        lidar_msg.angle_max = math.radians(float(data[1]))
        lidar_msg.angle_increment = math.radians(.25) #get from lidar
        lidar_msg.time_increment = float(25. / self.num_readings) #time in ms / measurements YOYOYOYO CHECK THIS
        lidar_msg.scan_time = float(data[2])
        lidar_msg.range_min = float(data[4]) / 1000 #sent in mm, should be meters
        lidar_msg.range_max = float(data[5]) / 1000 #sent in mm, should be meters


        array_string = data[3].translate(None, '[]')
        string_array = array_string.split(",")
        lidar_msg.ranges = [float(r) / 1000 for r in string_array] #better way?
    #    string_array = data[3].strip("[").strip("]").split(",")
        # string_array = data[3].split(",")
        # try:
        #     lidar_msg.ranges = [float(r) for r in string_array]
        #     lidar_msg.intensities = []
        # except ValueError:
        #     print "range vals failed"

        return lidar_msg
    def laser_callback(self, msg):
        self.laser = msg
        self.get_transform()
        #rospy.loginfo(msg)
        transformed_laser = LaserScan()
        transformed_laser.header = msg.header

        self.laser_pub.publish(msg)
Example #18
0
 def publish_scan(self, angles, ranges):
     ls = LaserScan()
     ls.header = Utils.make_header("laser", stamp=self.last_stamp)
     ls.angle_min = np.min(angles)
     ls.angle_max = np.max(angles)
     ls.angle_increment = np.abs(angles[0] - angles[1])
     ls.range_min = 0
     ls.range_max = np.max(ranges)
     ls.ranges = ranges
     self.pub_fake_scan.publish(ls)
Example #19
0
 def publish_scan(self, angles, ranges):
   ls = LaserScan()
   ls.header = Utils.make_header("inferred_scan_frame") # This fills in current time for us.
   ls.angle_min = np.min(angles)
   ls.angle_max = np.max(angles)
   ls.angle_increment = np.abs(angles[0] - angles[1])
   ls.range_min = 0
   ls.range_max = np.max(ranges)
   ls.ranges = ranges
   self.pub_fake_scan.publish(ls)
Example #20
0
 def publish_scan(self, angles, ranges):
     ls = LaserScan()
     ls.header = Utils.make_header("laser", stamp=self.last_stamp)
     ls.angle_min = np.min(angles)
     ls.angle_max = np.max(angles)
     ls.angle_increment = np.abs(angles[0] - angles[1])
     ls.range_min = 0
     ls.range_max = np.max(ranges)
     ls.ranges = ranges
     self.pub_fake_scan.publish(ls)
Example #21
0
 def publish_scan(self, angles, ranges):
     '''publish fake laserscan generated from the mean expected pose of particles distribution'''
     ls = LaserScan()
     ls.header = self.create_header("base_laser_link")
     ls.angle_min = np.min(angles)
     ls.angle_max = np.max(angles)
     ls.angle_increment = np.abs(angles[0] - angles[1])
     ls.range_min = 0
     ls.range_max = np.max(ranges)
     ls.ranges = ranges
     self.pub_fake_scan.publish(ls)
Example #22
0
 def publish_scan(self, angles, ranges):
     # publish the given angels and ranges as a laser scan message
     ls = LaserScan()
     ls.header = make_header("laser", stamp=self.last_stamp) # Utils
     ls.angle_min = np.min(angles)
     ls.angle_max = np.max(angles)
     ls.angle_increment = np.abs(angles[0] - angles[1])
     ls.range_min = 0
     ls.range_max = np.max(ranges)
     ls.ranges = ranges
     self.pub_fake_scan.publish(ls)
Example #23
0
    def laserCallback(self, msg):
        pos = self.controllerPosition

        # angle = -math.atan2(pos.point.y, pos.point.x)
        # distance = math.sqrt((pos.point.x) ** 2 + (pos.point.y) ** 2 + (pos.point.z) ** 2)
        # BUFFER = 2 # arc length in meters
        # arc = BUFFER / distance # in radians
        # minAngle = max(angle - (arc / 2), msg.angle_min)
        # minAngleIndex = int(minAngle / msg.angle_increment + abs(msg.angle_min / msg.angle_increment))
        # maxAngle = min(angle + (arc / 2), msg.angle_max)
        # maxAngleIndex = int(maxAngle / msg.angle_increment + abs(msg.angle_min / msg.angle_increment))

        laserPoints = list(msg.ranges) # [minAngleIndex:maxAngleIndex]

        collisionScan = LaserScan()
        collisionScan.header = msg.header
        collisionScan.angle_min = msg.angle_min #minAngle
        collisionScan.angle_max = msg.angle_max #maxAngle
        collisionScan.angle_increment = msg.angle_increment
        collisionScan.time_increment = msg.time_increment
        collisionScan.scan_time = msg.scan_time
        collisionScan.range_min = msg.range_min
        collisionScan.range_max = msg.range_max

        minDist = 999999
        minDistAngle = 0
        for i, point in enumerate(laserPoints):
            #pointAngle = (i + minAngleIndex) * msg.angle_increment + msg.angle_min
            pointAngle = i * msg.angle_increment + msg.angle_min
            if self.controllerPosition is not None:
                distanceFromController = math.sqrt((math.cos(pointAngle) * point - pos.x) ** 2 + (math.sin(pointAngle) * point - pos.y) ** 2)
                if distanceFromController < 1:
                    laserPoints[i] = 5
            if point > msg.range_min and point < minDist:
                minDist = point
                minDistAngle = pointAngle
        if minDist < 0.6:
            rospy.loginfo("Minimum distance is {} at {} degrees".format(round(minDist, 3), round(math.degrees(minDistAngle))))
        if minDist < 0.3:
            self.safeToTrack = False
            cmd = Twist()
            cmd.linear.x = -0.3
            self.cmdvel.publish(cmd)
            if self.controllerID is not None or self.controllerPosition is not None:
                rospy.loginfo("Temporarily killed controller due to obstacle")
                # For permanent killing of controller
                # self.controllerID = None
                # self.controllerPosition = None
        else:
            self.safeToTrack = True

        collisionScan.ranges = laserPoints
        self.laser_pub.publish(collisionScan)
Example #24
0
def publish_scan(scan_pub):
    scan_msg = LaserScan()
    scan_msg.header = Header()
    scan_msg.header.frame_id = "world"
    scan_msg.header.stamp = rospy.Time.now()
    scan_msg.angle_min = -3.1400001049
    scan_msg.angle_max = 3.1400001049
    scan_msg.angle_increment = 0.0174930356443
    scan_msg.range_min = 0.20000000298
    scan_msg.range_max = 60.0
    scan_msg.ranges = np.random.rand(360)*(60.0 - 0.2) + 0.2

    scan_pub.publish(scan_msg)
def create_laser_msg(range_data_array):
    ls = LaserScan()
    ls.angle_increment = 0.006283185307179586 # 0.36 deg
    ls.angle_max = 2.0943951023931953 # 120.0 deg
    ls.angle_min = -2.0943951023931953 # -120.0 deg
    ls.range_max = 4.0
    ls.range_min = 0.02
    ls.scan_time = 0.001 # No idea
    ls.time_increment = 1.73611115315e-05 # No idea, took from http://comments.gmane.org/gmane.science.robotics.ros.user/5192
    ls.header = Header()
    ls.header.frame_id = 'laser_link'
    ls.ranges = range_data_array
    return ls
def create_laser_msg(range_data_array):
    ls = LaserScan()
    ls.angle_increment = 0.006283185307179586  # 0.36 deg
    ls.angle_max = 2.0943951023931953  # 120.0 deg
    ls.angle_min = -2.0943951023931953  # -120.0 deg
    ls.range_max = 4.0
    ls.range_min = 0.02
    ls.scan_time = 0.001  # No idea
    ls.time_increment = 1.73611115315e-05  # No idea, took from http://comments.gmane.org/gmane.science.robotics.ros.user/5192
    ls.header = Header()
    ls.header.frame_id = 'laser_link'
    ls.ranges = range_data_array
    return ls
Example #27
0
 def reading(self, msg):
     resp = LaserScan()
     resp.header = msg.header
     resp.angle_min = msg.angle_min
     resp.angle_max = msg.angle_max
     resp.angle_increment = msg.angle_increment
     resp.time_increment = msg.time_increment
     resp.scan_time = msg.scan_time
     resp.range_min = msg.range_min
     resp.range_max = msg.range_max
     resp.ranges = [self.fix(x, msg.range_max) for x in msg.ranges]
     resp.intensities = msg.intensities
     self.pub.publish(resp)
Example #28
0
    def __publish_scan(self):
        scan = LaserScan()
        scan.header = self.header
        scan.header.frame_id = self.tf_id
        scan.angle_min = self.roi_angle_min
        scan.angle_max = self.roi_angle_max
        scan.angle_increment = self.angle_increment
        scan.time_increment = self.time_increment
        scan.range_min = self.range_min
        scan.range_max = self.range_max
        scan.ranges = self.ranges
        scan.intensities = self.roi_intensities

        self.pub.publish(scan)
 def publish_laser_scan(self, ranges, depth_msg):
     new_msg = LaserScan()
     # TODO: Add timing information
     new_msg.header = depth_msg.header
     new_msg.angle_min = -self._to_rad(self.fov / 2)
     new_msg.angle_max = self._to_rad(self.fov / 2)
     new_msg.angle_increment = self._to_rad(self.fov) / self.NUM_BUCKETS
     new_msg.time_increment = 0.0
     new_msg.range_min = 0.0
     new_msg.range_max = float('inf')
     new_msg.ranges = ranges
     new_msg.intensities = [0.0] * len(ranges)
     #print(new_msg)
     self.laser_pub.publish(new_msg)
 def ranges_to_laserscan(self):
     # TODO: optimize if bottleneck
     msg = LaserScan()
     msg.header = Utils.make_header('laser')
     msg.angle_min = self.laser_params["angle_min"]
     msg.angle_max = self.laser_params["angle_max"]
     msg.angle_increment = self.laser_params["angle_increment"]
     msg.time_increment = self.laser_params["time_increment"]
     msg.scan_time = self.laser_params["scan_time"]
     msg.range_min = self.laser_params["range_min"]
     msg.range_max = self.laser_params["range_max"]
     msg.ranges = self.ranges.tolist()
     msg.intensities = self.ranges.tolist()
     return msg
Example #31
0
    def initRangeFinder(self, frame, angle_min, angle_max, range_min,
                        range_max, ranges_dim):
        result = LaserScan()
        result.header = Header()

        result.header.frame_id = frame
        result.header.stamp = self.get_clock().now().to_msg()

        result.angle_min = angle_min
        result.angle_max = angle_max
        result.angle_increment = (angle_max - angle_min) / ranges_dim
        result.range_min = range_min
        result.range_max = range_max
        result.ranges = [0.0] * ranges_dim

        return result
Example #32
0
    def pub_laser(self, msg):
        laser = LaserScan()

        laser.header = msg.header
        laser.header.frame_id = self.frame_id
        laser.angle_min = 0
        laser.angle_max = 3.14
        laser.angle_increment = 0.01
        laser.time_increment = 0.01
        laser.scan_time = 0.1
        laser.range_min = 0.2
        laser.range_max = 4.5
        laser.ranges = [msg.range, msg.range]
        laser.intensities = [1, 1]

        self.laserPub.publish(laser)
Example #33
0
    def filterScanData(self, data):
        self.lidarScan = data
        newRanges = self.filterRanges()

        msg = LaserScan()
        msg.header = data.header
        msg.angle_min = data.angle_min
        msg.angle_max = data.angle_max
        msg.angle_increment = data.angle_increment
        msg.time_increment = data.time_increment
        msg.scan_time = data.scan_time
        msg.range_min = data.range_min
        msg.range_max = data.range_max
        msg.ranges = newRanges
        msg.intensities = data.intensities
        self.filterScanPub.publish(msg)
Example #34
0
    def convert_array_to_laser_scan(self, vision_raw_scan):

        if vision_raw_scan.size < 100:
            return None

        header = Header()
        header.frame_id = "vision_scan"
        #header.stamp = time()

        laser_scan = LaserScan()
        laser_scan.angle_min = 0.0
        laser_scan.angle_max = self.angle_max
        laser_scan.angle_increment = self.angle_increment
        laser_scan.range_min = 0.0
        laser_scan.range_max = self.range_max
        #laser_scan.ranges = [0]*360

        image_size = vision_raw_scan.shape

        if len(image_size) == 3:
            vision_raw_scan = cv2.cvtColor(vision_raw_scan, cv2.COLOR_BGR2GRAY)
            image_size = vision_raw_scan.shape

        if self.init is False:
            self._init_lines(image_size)
            self.init = True


        tasks = list()
        for line in range(self.number_lines):
            tasks.append((vision_raw_scan, self.lines[line]))

        laser_scan.ranges = self.pool.map(_getObstacle, tasks)

        #pool.close()
        laser_scan.header = header
        #laser_scan.scan_time = 1.0/5.0
        #laser_scan.time_increment = 1.0/5.0
        return laser_scan
Example #35
0
    def default(self, ci='unused'):
        laserscan = LaserScan()
        laserscan.header = self.get_ros_header()

        # Note: Scan time and laser frequency are chosen as standard values
        laser_frequency = 40 # TODO ? component_instance.frequency()
        scan_window = self.component_instance.bge_object['scan_window']
        num_readings = scan_window / self.component_instance.bge_object['resolution']

        laserscan.angle_max = scan_window * math.pi / 360
        laserscan.angle_min = laserscan.angle_max * -1
        laserscan.angle_increment = scan_window / num_readings * math.pi / 180
        laserscan.time_increment = 1 / laser_frequency / num_readings
        laserscan.scan_time = 1.0
        laserscan.range_min = 0.3
        laserscan.range_max = self.component_instance.bge_object['laser_range']
        # ROS expect the ranges to be sorted clockwise.
        # see morse.builder.sensor.LaserSensorWithArc.create_laser_arc
        # where we create the ray from -window / 2.0 to +window / 2.0
        laserscan.ranges = self.data['range_list']

        self.publish(laserscan)
Example #36
0
    def create_lidar_msg(self, L):
 
        raw_lidar = L.data
        #stripped_lidar = raw_lidar.translate(None, '[]').translate(None, '"').translate(None, '\'')
        array_lidar = raw_lidar.split(";")
        
        lidar_msg = LaserScan()
        lidar_msg.header = self.create_header() #self?
        lidar_msg.angle_min = math.radians(float(array_lidar[0]))
        lidar_msg.angle_max = math.radians(float(array_lidar[1]))
        lidar_msg.angle_increment = math.radians(0.25) #MAKE PARAM
        lidar_msg.time_increment = 0.025/(270*4) #time in ms / measurements YOYOYOYO CHECK THIS
        lidar_msg.scan_time = float(array_lidar[2]) / 1000 #time in ms
        lidar_msg.range_min = float(array_lidar[4]) / 1000 #sent in mm, should be meters
        lidar_msg.range_max = float(array_lidar[5]) / 1000 #sent in mm, should be meters


        array_string = array_lidar[3]   #.translate(None, '[]')
        string_array = array_string.split(",")
        lidar_msg.ranges = [float(r) / 1000 for r in string_array] #better way?

        self.scanPub.publish(lidar_msg)