Exemple #1
0
    def __add_scan_msg(self, topic, frame, measurements):
        """
        Publish a LaserScan message to a ROSBag file with the given measurement ranges.

        :param measurements: (ndarray) 2D Array of measurements to be published.
                                       Measured ranges must be in measurements[:, 1]

        :return: None
        """

        if self._bag is None:
            return

        meas_msg = LaserScan()

        meas_msg.header.frame_id = frame  # topic_prefix + self._params['laser_frame']
        meas_msg.header.stamp = self._current_time
        meas_msg.header.seq = self._laser_msg_seq

        meas_msg.angle_min = self._params['start_ray']
        meas_msg.angle_max = self._params['end_ray']
        if self._params['num_rays'] > 1:
            meas_msg.angle_increment = (meas_msg.angle_max - meas_msg.angle_min
                                        ) / (self._params['num_rays'] - 1)
        else:
            meas_msg.angle_increment = 0.0

        meas_msg.range_min = 0.0
        meas_msg.range_max = self._params['max_range']

        meas_msg.ranges = measurements[:, 1]

        self._bag.write(topic, meas_msg, self._current_time)
        self._bag.flush()
Exemple #2
0
def laserscan_err_inj(tb3_name):
    #Create error-injected topic
    rospy.init_node('laser_err_inj')

    #########################################
    #Create new message
    laserscan_msg = LaserScan() 

    #Fill message with values
    laserscan_msg.header.seq = 0 
    laserscan_msg.header.stamp.secs = 0
    laserscan_msg.header.stamp.nsecs = 0
    laserscan_msg.header.frame_id = ""

    laserscan_msg.angle_min = 0.0
    laserscan_msg.angle_max = 0.0
    laserscan_msg.angle_increment = 0.0
    laserscan_msg.time_increment = 0.0
    laserscan_msg.scan_time = 0.0
    laserscan_msg.range_min = 0.0
    laserscan_msg.range_max = 0.0

    laserscan_msg.ranges = [0.0] * 360
    laserscan_msg.intensities = [0.0] * 360

    #########################################

    rate = rospy.Rate(50)

    #Publish message into new topic
    while not rospy.is_shutdown():
        my_sub = rospy.Subscriber(turtlebot_dict[tb3_name] + 'scan', LaserScan, listener) 
        my_pub = rospy.Publisher(turtlebot_dict[tb3_name] + 'laser_err_inj', LaserScan, queue_size = 10) 

        #########################################
        #INJECT ERRORS HERE
        laserscan_msg.header.seq = actual_seq 
        laserscan_msg.header.stamp.secs = actual_secs
        laserscan_msg.header.stamp.nsecs = actual_nsecs
        laserscan_msg.header.frame_id = actual_frameid

        laserscan_msg.angle_min = actual_anglemin
        laserscan_msg.angle_max = actual_anglemax
        laserscan_msg.angle_increment = actual_angleincrement
        laserscan_msg.time_increment = actual_timeincrement
        laserscan_msg.scan_time = actual_scantime
        laserscan_msg.range_min = actual_rangemin
        laserscan_msg.range_max = actual_rangemax

        for i in range(len(actual_ranges)):
            laserscan_msg.ranges[i] = actual_ranges[i] #inject error here (i just made everything = 0 because it's easy to see when testing)

        for j in range(len(actual_intensities)):
            laserscan_msg.intensities[i] = actual_intensities[i] #inject error here (i just made everything = 0 because it's easy to see when testing)
        #########################################
            
        my_pub.publish(laserscan_msg)
        rate.sleep()
    
    rospy.spin()
Exemple #3
0
    def timer_callback(self, timer):
        ts = rospy.Time.now()

        # pub scan
        scan = LaserScan()
        scan.header.stamp = ts
        scan.header.frame_id = 'ego_racecar/laser'
        scan.angle_min = self.angle_min
        scan.angle_max = self.angle_max
        scan.angle_increment = self.angle_inc
        scan.range_min = 0.
        scan.range_max = 30.
        scan.ranges = self.ego_scan
        self.ego_scan_pub.publish(scan)

        scan = LaserScan()
        scan.header.stamp = ts
        scan.header.frame_id = 'ego_racecar/laser'
        scan.angle_min = self.angle_min
        scan.angle_max = self.angle_max
        scan.angle_increment = self.angle_inc
        scan.range_min = 0.
        scan.range_max = 30.
        scan.ranges = self.opp_scan
        self.opp_scan_pub.publish(scan)

        # pub tf
        self.publish_odom(ts)
        self.publish_transforms(ts)
        self.publish_laser_transforms(ts)
        self.publish_wheel_transforms(ts)

        # pub race info
        self.publish_race_info(ts)
def create_messages(params, words):
    #    0   |      1       | 2 - 1+num_readings | 2+num_readings | 3+num_readings |
    # xLASER | num_readings |  [range_readings]  |       x        |        y       |
    #-------------------------------------------------------------------------------
    # 4+num_readings | 5+num_readings | 6+num_readings | 7+num_readings |
    #      theta     |     odom_x     |     odom_y     |   odom_theta   |
    #-------------------------------------------------------------------------------
    # 8+num_readings | 9+num_readings | 10+num_readings
    # ipc_timestamp | ipc_hostname  |logger_timestamp

    laser_msg = LaserScan()
    laser_msg.header.frame_id = laser_tf_child_frame

    num_readings = int(words[1])
    laser_msg.ranges = list(map(float, words[2:1 + num_readings + 1]))

    str_laser_fov = "laser_" + param_names[words[0]] + "_fov"
    if str_laser_fov in params:
        a_range = float(params[str_laser_fov][0])
    else:
        a_range = radians(180.0)

    str_laser_res = "laser_" + param_names[words[0]] + "_resolution"
    if str_laser_res in params:
        laser_msg.angle_increment = radians(float(params[str_laser_res][0]))
    else:
        laser_msg.angle_increment = a_range / float(num_readings)

    laser_msg.angle_min = -a_range / 2.0
    laser_msg.angle_max = a_range / 2.0

    str_max_range = "robot_" + param_names[words[0]] + "_max"
    if str_max_range in params:
        laser_msg.range_max = float(params[str_max_range][0])
    else:
        laser_msg.range_max = 20.0
    laser_msg.range_min = 0

    laser_msg.header.stamp = rospy.Time(float(words[8 + num_readings]))

    TS_odom_robot_msg = TransformStamped()
    TS_odom_robot_msg.header.stamp = laser_msg.header.stamp + rospy.Duration(
        0.01)
    TS_odom_robot_msg.header.frame_id = laser_tf_parrent_frame
    TS_odom_robot_msg.child_frame_id = laser_tf_child_frame
    TS_odom_robot_msg.transform.translation = Point(
        float(words[5 + num_readings]), float(words[6 + num_readings]), 0.0)
    quaternion = tf.transformations.quaternion_from_euler(
        0.0, 0.0, float(words[7 + num_readings]))
    TS_odom_robot_msg.transform.rotation.x = quaternion[0]
    TS_odom_robot_msg.transform.rotation.y = quaternion[1]
    TS_odom_robot_msg.transform.rotation.z = quaternion[2]
    TS_odom_robot_msg.transform.rotation.w = quaternion[3]
    tf_msg = TFMessage()
    tf_msg.transforms.append(TS_odom_robot_msg)

    return [(laser_topic_name, laser_msg, laser_msg.header.stamp),
            (tf_topic_name, tf_msg, TS_odom_robot_msg.header.stamp)]
Exemple #5
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 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)
    def to_laserscan(self, frame):
        """Returns a LaserScan message correspon2ding to slice.

        Args:
            frame: Frame ID.

        Returns:
            A sensor_msgs.msg.LaserScan.
        """

        scan = LaserScan()
        scan.header.frame_id = frame
        scan.header.stamp = self.timestamp

        scan.angle_min = self.angle_min
        scan.angle_max = self.angle_max
        scan.angle_increment = self.step
        #scan.ranges = self.bins
        scan.ranges = [b * 1.45 / 2000 for b in self.bins]

        # points out if this range are discarded
        scan.range_min = 0.0
        scan.range_max = 70000.0

        return scan
def tfmini_laserscan_publisher(frame_id):

    scan_pup = rospy.Publisher('tfmini_laser', LaserScan, queue_size=0)

    scan = LaserScan()

    #-- Convention: counter clockwise is positive (left positive, right negative)
    tfminiscanner = TfminiServoScanner(SERVO_GPIO, SRV_ANGLE_MIN,
                                       SRV_ANGLE_MAX, SRV_DUTY_ANGLE_MIN,
                                       SRV_DUTY_ANGLE_MAX, LASER_ANGLE_SAMPLES,
                                       SRV_TIME_MIN_MAX)

    #-- Initialize the message
    scan.header.frame_id = frame_id
    scan.range_min = tfminiscanner.laser.distance_min * 0.01
    scan.range_max = tfminiscanner.laser.distance_max * 0.01

    tfminiscanner.reset_servo()
    time.sleep(1)

    counter = 0

    while not rospy.is_shutdown():
        ini_angle, end_angle, time_increment, angle_increment, ranges = tfminiscanner.scan(
            scale_factor=0.01, reset=True)
        scan.angle_min = ini_angle
        scan.angle_max = end_angle
        scan.angle_increment = angle_increment
        scan.time_increment = time_increment
        scan.ranges = ranges

        scan_pup.publish(scan)
Exemple #9
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)
    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
Exemple #11
0
def fillUpOldLaserMessage(words):

    laser_msg = LaserScan()

    laser_msg.header.frame_id = "base_link"
    ranges = []
    num_range_readings = int(words[1])
    last_range_reading = num_range_readings + 1
    for word in words[2:last_range_reading + 1]:
        ranges.append(float(word))

    ang_range = float(180)

    # Get angular resolution
    ang_res = (radians(ang_range) / (num_range_readings - 0.0))

    # Get max reading
    max_reading = 20.

    # init laser msg
    ang_min = radians(-ang_range / 2.)
    ang_max = radians(+ang_range / 2.) - ang_res

    laser_msg.angle_min = ang_min
    laser_msg.angle_max = ang_max
    laser_msg.angle_increment = ang_res

    laser_msg.range_min = 0
    laser_msg.range_max = max_reading

    laser_msg.ranges = ranges
    laser_msg.header.stamp = rospy.Time(float(words[last_range_reading + 7]))

    return laser_msg
def LaserReceiver():
    pub = rospy.Publisher('scanPi', LaserScan, queue_size=10)
    rospy.init_node('LaserReceiver', anonymous=True)
    rate = rospy.Rate(10)  # 10hz
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  # UDP
    sock.bind(("192.168.1.65", 4001))
    while not rospy.is_shutdown():
        data, addr = sock.recvfrom(15024)
        data = json.loads(data)
        laser = LaserScan()
        laser.header.stamp.secs = data["header"]["stamp"]["secs"]
        laser.header.stamp.nsecs = data["header"]["stamp"]["nsecs"]
        laser.header.frame_id = data["header"]["frame_id"]
        laser.header.seq = data["header"]["seq"]
        laser.angle_min = data["angle_min"]
        laser.angle_max = data["angle_max"]
        laser.angle_increment = data["angle_increment"]
        laser.time_increment = data["time_increment"]
        laser.range_min = data["range_min"]
        laser.range_max = data["range_max"]
        laser.scan_time = data["scan_time"]
        laser.ranges = data["ranges"]
        laser.intensities = data["intensities"]
        pub.publish(laser)
        rate.sleep()
Exemple #13
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 handleScanMsg(data):
    rospy.loginfo("\n/scan is now being handled...")
    #### Setup Corrected-LaserScan Publisher
    correctedScan_publisher = rospy.Publisher("/scanCorrected",
                                              LaserScan,
                                              queue_size=10)
    correctedScan_msg = LaserScan()

    #### Start copying the variables
    correctedScan_msg.angle_min = data.angle_min
    correctedScan_msg.angle_max = data.angle_max
    correctedScan_msg.angle_increment = data.angle_increment
    correctedScan_msg.time_increment = data.time_increment
    correctedScan_msg.scan_time = data.scan_time
    correctedScan_msg.range_min = data.range_min
    correctedScan_msg.range_max = data.range_max
    correctedScan_msg.intensities = data.intensities

    #### Start copying 'ranges' using the corrected values if necessary
    # if (+inf) -> change it to (-1)

    correctedScan_msg.ranges = [0] * len(data.ranges)

    for i in range(len(data.ranges)):
        if numpy.isinf(data.ranges[i]):
            correctedScan_msg.ranges[i] = -100
        else:
            correctedScan_msg.ranges[i] = data.ranges[i]

    #### Publish msg
    rate = rospy.Rate(1800)  # 1800hz
    correctedScan_publisher.publish(correctedScan_msg)
    rospy.loginfo("\n/scanCorrected is now being published...")
    rate.sleep()
Exemple #15
0
def laser_test():
    pub = rospy.Publisher('laser_scan', LaserScan)
    rospy.init_node('laser_test')
    laser_msg = LaserScan()

    laser_msg.header.frame_id = 'laser'
    laser_msg.angle_min = -1.5
    laser_msg.angle_max = 1.5
    laser_msg.angle_increment = 0.1
    laser_msg.time_increment = 0.1
    laser_msg.scan_time = 0.1
    laser_msg.range_min = 0.5
    laser_msg.range_max = 1.5
    laser_msg.ranges = [
        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 9.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
        1.0, 0.1, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
        5.0, 1.0
    ]
    laser_msg.intensities = laser_msg.ranges

    r = rospy.Rate(10)  # 10hz
    while not rospy.is_shutdown():
        laser_msg.header.stamp = rospy.get_rostime()
        pub.publish(laser_msg)
        r.sleep()
	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)
Exemple #17
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()
Exemple #18
0
def merge_scans(rf, sg):
    rf.ranges = list(rf.ranges)
    for i in range(40):
        rf.ranges[len(rf.ranges)-i-1] = 0

    if not sg:
        rf.header.frame_id = 'laser'
        return rf
    else:
        global angle_min
        global angle_max
        global angle_increment
        global last_scan_time
        if not last_scan_time:
            last_scan_time = time.time()

        scan = LaserScan()
        scan.header.frame_id = 'laser'
        scan.header.stamp = get_most_recent_timestamp(rf, sg)    
        scan.angle_min = angle_min
        scan.angle_max = angle_max
        scan.angle_increment = angle_increment
        scan.scan_time = time.time() - last_scan_time
        scan.time_increment = scan.scan_time / 541
        scan.range_min = rf.range_min
        scan.range_max = rf.range_max
        scan.ranges = rf.ranges
        for i in range(180*2):
            if sg.ranges[i] < scan.ranges[90 + i] or scan.ranges[90 + i] == 0:
                scan.ranges[90 + i] = sg.ranges[i]

    return scan
    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
Exemple #20
0
def main():
    rospy.init_node('laserscan_generator', anonymous = True)
    rate = rospy.Rate(30) # 30hz
    pub = rospy.Publisher('scan_generated', LaserScan, queue_size=2)

    no_points = 300

    scan = LaserScan()
    scan.header.frame_id = "map"
    scan.angle_min = -math.pi+(40*math.pi/180)
    scan.angle_max = math.pi-(15*math.pi/180)
    scan.angle_increment = 2*math.pi/no_points
    scan.time_increment = (1 / 30) / no_points;
    scan.range_min = 0.0
    scan.range_max = 100.0
    #scan.set_ranges_size(100)
    #scan.set_intensities_size(100)

    for i in range(1, no_points):
        if i < (no_points/2):
            if i/(no_points/2)*math.pi < abs(scan.angle_min):
                scan.ranges.append(10+ i%10 * 0.1)
                scan.intensities.append(no_points)
        elif i > (no_points/2):
            if i/no_points*math.pi < abs(scan.angle_max):
                scan.ranges.append(10- i%10 * 0.1)
                scan.intensities.append(no_points)

    rospy.loginfo("laserscan generated")

    while not rospy.is_shutdown():

        pub.publish(scan)
        rate.sleep()
def getLaserScan(frame_id, laser_scan_line):
    # Timestamp [seconds.microseconds]
    # # of ranges [unitless]
    # Angular offset [1/4 degree]
    # R1..R181 Ranges (zero padded to 181 ranges) [m]
    #
    # 1235603336.30835, 181, 0, 11.360, 11.360, 11.390, 11.410, 81.910, 81.910, 11.380, 11.400, 11.430, 6.450, 6.170, 6.030, 5.880, 5.740, 5.600, 5.470, 5.360, 5.370, 5.390, 5.430, 5.470, 5.500, 5.530, 5.580, 5.610, 5.410, 5.230, 5.130, 5.180, 5.230, 5.280, 5.350, 6.040, 6.110, 6.180, 6.250, 6.330, 6.400, 6.490, 5.950, 5.750, 5.640, 5.520, 5.440, 5.330, 5.220, 5.280, 5.040, 5.490, 5.590, 5.690, 5.810, 5.930, 6.080, 6.210, 6.360, 6.530, 6.690, 6.870, 13.930, 13.770, 13.650, 13.650, 13.530, 13.430, 13.300, 13.190, 13.040, 12.870, 12.780, 12.700, 12.630, 12.550, 12.480, 12.410, 12.360, 12.310, 12.240, 12.200, 12.150, 12.110, 12.070, 12.040, 12.010, 11.990, 11.970, 11.560, 11.930, 11.920, 11.920, 11.910, 11.930, 11.920, 11.920, 11.940, 11.930, 12.830, 12.840, 12.300, 12.130, 12.120, 13.000, 12.250, 12.230, 12.270, 12.330, 12.390, 12.440, 12.520, 12.580, 12.810, 13.640, 13.740, 13.830, 13.940, 13.640, 6.410, 6.220, 6.010, 5.810, 5.640, 5.080, 4.180, 4.090, 4.250, 4.070, 4.050, 3.700, 3.560, 3.510, 3.510, 3.570, 3.430, 3.520, 3.590, 4.940, 4.650, 4.630, 5.050, 5.040, 5.080, 4.890, 2.790, 2.710, 2.660, 2.620, 2.590, 2.600, 2.660, 2.650, 2.630, 2.690, 2.790, 2.900, 4.250, 4.150, 2.510, 2.480, 2.390, 2.360, 2.330, 2.320, 2.300, 2.410, 2.270, 3.930, 2.290, 2.390, 3.850, 3.830, 3.830, 3.710, 4.060, 4.050, 4.040, 4.030, 4.020, 4.010, 4.010, 4.010, 4.010
    str_timestamp, str_num_readings, str_angular_offset, str_ranges = laser_scan_line.split(', ', 3)
    num_readings = int(str_num_readings)
    angular_offset = float(str_angular_offset)
    ranges = map(float, str_ranges.split(', ')) #convert array of readings
    laser_frequency = 50
    angle_range_rad = 180 * np.pi / 180

    #populate the LaserScan message
    msg = LaserScan()
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = frame_id
    msg.angle_min = - (angle_range_rad / 2)
    msg.angle_max = (angle_range_rad / 2)
    msg.angle_increment = angle_range_rad / num_readings
    msg.time_increment = (1 / laser_frequency) / (num_readings)
    msg.range_min = 0.0
    msg.range_max = 40.0
    msg.ranges = ranges
    msg.intensities = [0.0] * len(ranges)

    return msg
def fake_scan_publisher():
    rospy.init_node('fake_scan_publisher')
    pub = rospy.Publisher(rospy.get_param("~pubs_topic", "fake_scan"),
                          LaserScan,
                          queue_size=10)
    rate = rospy.Rate(rospy.get_param("~pubs_rate", 20))  # 20hz
    while not rospy.is_shutdown():
        current_time = rospy.Time.now()
        scan = LaserScan()
        scan.header.stamp = current_time
        scan.header.frame_id = "base_link"
        scan.angle_min = rospy.get_param("~angle_min", -2 * math.pi / 3)
        scan.angle_max = rospy.get_param("~angle_max", 2 * math.pi / 3)
        scan.angle_increment = rospy.get_param("~angle_increment",
                                               math.pi / 300)
        scan.scan_time = 1.0 / 20
        scan.range_min = rospy.get_param("~range_min", 1.0)
        scan.range_max = rospy.get_param("~range_max", 10.0)
        num_readings = int(
            (scan.angle_max - scan.angle_min) // scan.angle_increment) + 1
        scan.ranges = [0] * num_readings
        for i in range(0, num_readings):
            scan.ranges[i] = random.uniform(scan.range_min, scan.range_max)
    #rospy.loginfo(scan)
        pub.publish(scan)
        rate.sleep()
def checker(fake_laser_param, realtime_lasers, nonrealtime_lasers):
    r = rospy.Rate(RATE)
    seq = 0
    laser_scan = LaserScan()
    laser_scan.header.seq = seq
    laser_scan.header.frame_id = fake_laser_param['frame_name']
    laser_scan.angle_min = fake_laser_param['angle_min']
    laser_scan.angle_max = fake_laser_param['angle_max']
    laser_scan.angle_increment = fake_laser_param['angle_increment']
    laser_scan.range_min = fake_laser_param['range_min']
    laser_scan.range_max = fake_laser_param['range_max']
    laser_scan.scan_time = 0
    laser_scan.time_increment = 0
    pub = rospy.Publisher('/scan', LaserScan, queue_size=10)
    while not rospy.is_shutdown():
        fake_laser_data = realtime_lasers[0].get_range_data()
        for laser_scanner in realtime_lasers[1:]:
            new_laser_data = laser_scanner.get_range_data()
            fake_laser_data = [min(r1, r2) for r1, r2 in zip(fake_laser_data, new_laser_data)]
        for laser_scanner in nonrealtime_lasers:
            laser_data = laser_scanner.get_range_data()
            #fake_laser_data = [r1 if r1 < 1000 else min(r1, r2) for r1, r2 in zip(fake_laser_data, laser_data)]
            fake_laser_data = [min(r1, r2) for r1, r2 in zip(fake_laser_data, laser_data)]
        laser_scan.ranges = fake_laser_data
        laser_scan.header.stamp = rospy.Time.now()
        pub.publish(laser_scan)
        seq += 1
        r.sleep()
Exemple #24
0
def mergescans(lidarscandata):
    global camscans, scansmerged

    mergedscan = LaserScan()
    mergedscan.header.stamp = lidarscandata.header.stamp
    mergedscan.header.frame_id = 'laser_frame'
    mergedscan.angle_min = 0.0
    mergedscan.angle_max = ANGINC * NUMPOINTS
    mergedscan.angle_increment = ANGINC
    mergedscan.time_increment = 0.0
    mergedscan.scan_time = lidarscandata.scan_time
    mergedscan.range_min = lidarscandata.range_min
    mergedscan.range_max = lidarscandata.range_max
    mergedscan.ranges = []

    angle = 0.0
    while angle <= mergedscan.angle_max:

        distance = 0.0
        distance = getlidarscan(angle, lidarscandata)
        distcam = getcamscan(angle)
        if not distcam == 0.0:
            distance = distcam

        mergedscan.ranges.append(distance)
        angle += ANGINC

    scan_pub.publish(mergedscan)
    camscans = []
Exemple #25
0
 def ls_scan_pub(self, pub, laser_distance1, laser_angle1):
     # distance data is from -pi to pi
     laser_distance1 = np.array(laser_distance1)
     scan_msg = LaserScan()
     point_num = 360.0
     scan_msg.angle_max = self.angle_max  # LS-lidar rotates clockwise, which is disobey right-hand rules for coordinate system
     scan_msg.angle_min = self.angle_min
     scan_msg.header.frame_id = 'base_laser_link'
     scan_msg.angle_increment = 2 * math.pi / point_num
     scan_msg.range_min = 0.15
     scan_msg.range_max = 3.0
     scan_msg.header.stamp = rospy.Time.now()
     # print("begin to select")
     if (self.angle_min > 0) or (self.angle_max <= 0):
         range_part = laser_distance1[(self.angle_range >= self.angle_min) &
                                      (self.angle_range <= self.angle_max)]
         scan_msg.ranges = np.flipud(range_part)
     else:
         range_part1 = laser_distance1[(self.angle_range >= self.angle_min)
                                       & (self.angle_range <= 0)]
         range_part2 = laser_distance1[(self.angle_range <= self.angle_max)
                                       & (self.angle_range > 0)]
         p1 = np.flipud(range_part1)
         p2 = np.flipud(range_part2)
         scan_msg.ranges = np.append(p1, p2)
     scan_msg.intensities = [255] * len(laser_distance1)
     pub.publish(scan_msg)
Exemple #26
0
    def handle_scanner_msg(self, args):
        # Broadcast scanner transform first
        time_now = rospy.Time.now()        
        pos = self.laser_tf.transform.translation
        rot = self.laser_tf.transform.rotation        
        self.tf_broadcaster.sendTransform((pos.x, pos.y, pos.z),
                                      (rot.x, rot.y, rot.z, rot.w),
                                      time_now,
                                      self.laser_tf.child_frame_id,
                                      self.laser_tf.header.frame_id)
        
        scan = LaserScan()
        scan.header.stamp = time_now
        scan.header.frame_id = self.base_laser_frame                            
        for (name, par) in args.items():
            if name == 'Range':
                scan.ranges = [float(s) for s in par.split(',')]
            if name == 'FOV':
                fov = float(par)
                scan.angle_min = -fov/2.0
                scan.angle_max = fov/2.0
            if name == 'Resolution':
                scan.angle_increment = float(par)
        scan.range_min = 0.0
        scan.range_max = 20.0

        self.scan_pub.publish(scan)
Exemple #27
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)
Exemple #28
0
def talker():
    pub = rospy.Publisher('scan', LaserScan, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(10)  # 10hz

    br = tf.TransformBroadcaster()

    sequence = 1
    while not rospy.is_shutdown():
        br.sendTransform((0.1, 0.1, 0),
                         tf.transformations.quaternion_from_euler(0, 0, 0),
                         rospy.Time.now(), 'base_laser', 'base_link')

        br.sendTransform(
            (sequence * 0.01, sequence * 0.01, 0),
            tf.transformations.quaternion_from_euler(0, 0, sequence * 0.01),
            rospy.Time.now(), 'base_link', 'odom')

        scan = LaserScan()
        scan.header.seq = sequence
        scan.header.stamp = rospy.get_rostime()
        scan.header.frame_id = 'base_laser'
        scan.angle_min = -0.1
        scan.angle_max = 0.1
        scan.angle_increment = 0.1
        scan.range_max = 5.0
        scan.ranges = [4.0, 4.0, 4.0]
        scan.intensities = [0.1, 0.1, 0.1]

        rospy.logdebug(scan)
        pub.publish(scan)
        sequence += 1
        rate.sleep()
    def republish_laser_scan(self,
                             data,
                             fov_adjustment=1.0,
                             publisher=None,
                             combined_publisher=None):
        """Publish range as laser scan."""
        if not self._running:
            return

        laser_scan_message = LaserScan()
        laser_scan_message.range_max = data.max_range
        laser_scan_message.range_min = data.min_range

        laser_scan_message.angle_min = -(data.field_of_view /
                                         (2 / fov_adjustment))
        laser_scan_message.angle_max = data.field_of_view / (2 /
                                                             fov_adjustment)
        laser_scan_message.angle_increment = data.field_of_view / (
            2 / fov_adjustment)

        laser_scan_message.ranges = [data.range] * 3

        laser_scan_message.header.frame_id = data.header.frame_id
        laser_scan_message.header.stamp = data.header.stamp

        if publisher is not None:
            publisher.publish(laser_scan_message)

        if combined_publisher is not None:
            combined_publisher.publish(laser_scan_message)
Exemple #30
0
def read(pub):
    if (ser.in_waiting >= 9):
        a = rospy.Time.now()
        #	print("read")
        Dist_Total = -1
        if ((b'Y' == ser.read()) and (b'Y' == ser.read())):
            Dist_L = ser.read()
            Dist_H = ser.read()
            Strength_L = ser.read()
            Strength_H = ser.read()
            Strength = ord(Strength_L) + 256 * ord(Strength_H)
            Mode = ser.read()
            tmp = ser.read()
            CheckSum = ser.read()
            Dist_Total = (ord(Dist_H) * 256) + (ord(Dist_L))
            scan = LaserScan()
            #	    print(rospy.Time.now(),Dist_Total)

            scan.header.stamp = a
            scan.header.frame_id = 'laser_frame'
            scan.angle_min = 0.
            scan.angle_max = 0.
            scan.angle_increment = 0
            scan.time_increment = 0
            scan.range_min = 30.
            scan.range_max = 1200.
            scan.ranges = []
            scan.intensities = []
            scan.ranges.append(Dist_Total)
            scan.intensities.append(Strength)
            pub.publish(scan)
Exemple #31
0
    def handleScanner(self):

        scan = LaserScan()
        scan.header.stamp = self.current_time

        self.ser.write("0")
        line = self.ser.readline()
        if len(line) <= 3:
            return
        direction = line[0:2]
        if direction == '+:':
            self.direction = 1.0
        elif direction == '-:':
            self.direction = -1.0
        else:
            return

        rospy.logdebug(line)

        scan.header.frame_id = 'laser_frame'
        scan.angle_min = -1.57 * self.direction
        scan.angle_max = 1.57 * self.direction
        scan.angle_increment = 3.14 / self.num_readings * self.direction
        scan.time_increment = 0.003012 * self.num_readings
        scan.range_min = 0.02  # [m]
        scan.range_max = 4.00  # [m]

        scan.ranges = map(lambda n: 1.0 * int(n) / 1000, line[2:].split(','))

        self.scan_pub.publish(scan)
Exemple #32
0
def main():
    rospy.init_node('VL53L0X_node', anonymous=True)

    timing = tof.get_timing()

    while not rospy.is_shutdown():
        current_time = rospy.Time.now()
        distance = tof.get_distance()  # Nuskaityti duomenis
        rospy.loginfo("dist={}".format(distance))

        # Atmesti negerus duomenis
        if distance > 3000 or distance < 0:
            distance = 0

        # Headeris
        laser_scan = LaserScan()
        laser_scan.header.stamp = current_time
        laser_scan.header.frame_id = "radar"

        laser_scan.angle_min = -0.15
        laser_scan.angle_max = 0.15
        laser_scan.angle_increment = 0.05
        laser_scan.time_increment = 0
        laser_scan.scan_time = timing
        laser_scan.range_min = 0.030
        laser_scan.range_max = 2.500
        laser_scan.ranges = [float32(distance / 1000)] * 7

        pub_scan.publish(laser_scan)

        rospy.Rate(1 / (timing / 1000000.00)).sleep()
def create_lidar_msg(L):

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

    num_readings = 1440
    scan = LaserScan()
    scan.header.stamp = rospy.Time.now()
    scan.header.frame_id = "base_scan"
    scan.angle_min = math.radians(float(array_lidar[0]))
    scan.angle_max = math.radians(float(array_lidar[1]))
    scan.angle_increment = math.radians(.25)  #get from lidar
    scan.time_increment = float(25. / num_readings)  #time in ms / measurements
    scan.scan_time = float(array_lidar[2])
    scan.range_min = float(array_lidar[4]) / 1000  #sent in mm, needs m
    scan.range_max = float(array_lidar[5]) / 1000  #sent in mm, needs m
    #    string_array = array_lidar[3].strip("[").strip("]").split(",")
    array_string = array_lidar[3].translate(None, '[]')
    string_array = array_string.split(",")
    scan.ranges = [float(r) / 1000 for r in string_array]  #better way?

    scanPub.publish(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 prepare_laserscan_msg(self):
        '''
        Fill laser scan message
        '''
        laser_scan_msg = LaserScan()

        #Step 1: 
        num_readings = 100
        laser_frequency = 40
        ranges = []
        intensities = []
        count = 0
        i = 0
        
        #generate some fake data for laser scan
        while (i < num_readings):
            ranges.append(count)
            intensities.append(4 + count)
            i = i + 1

        #Step 2
        now = rospy.get_rostime()
        laser_scan_msg.header.stamp = now
        laser_scan_msg.header.frame_id = "laser_frame"
        laser_scan_msg.angle_min = -1.57
        laser_scan_msg.angle_max = 1.57
        laser_scan_msg.angle_increment = 3.14 / num_readings
        laser_scan_msg.time_increment = (1 / laser_frequency) / (num_readings)
        laser_scan_msg.range_min = 0.0
        laser_scan_msg.range_max = 4.0

        laser_scan_msg.ranges = ranges
        laser_scan_msg.intensities = intensities
Exemple #36
0
def callback(data):
    n = int(rospy.get_param('~reduction', '2'))
    pub=rospy.Publisher('/scan_reduced', LaserScan)
    newScan=LaserScan()
    newScan=data
    newScan.angle_increment=n*data.angle_increment
    newScan.ranges=data.ranges[1::n]    
    pub.publish(newScan)
Exemple #37
0
    def spin(self):        
        encoders = [0,0]

        self.x = 0                  # position in xy plane
        self.y = 0
        self.th = 0
        then = rospy.Time.now()

        # things that don't ever change
        scan_link = rospy.get_param('~frame_id','neato_link')
        scan = LaserScan(header=rospy.Header(frame_id=scan_link)) 
        scan.angle_min = 0
        scan.angle_max = 6.26
        scan.angle_increment = 0.017437326
        scan.range_min = 0.020
        scan.range_max = 5.0

        # The LiDAR spins counterclockwise at 10 revolutions per second.
        # Each revolution yields 90 packets.
        # Each packet contains 22 bytes.
        # Within these 22 bytes are 4 distance measurements and more
        # (4 data points/packet * 90 packets = 360 data points). 
        # So there is one data measurement per degree turn.
        # Byte 01, "FA" is a starting byte which appears between the ending 
        # and beginning of two packets.
        # Byte 02 is a hex value from A0-F9, representing the 90 packets 
        # outputted per revolution.
        # Byte 03 and 04 are a 16bit (combined) value representing the speed 
        # at which the LiDAR is rotating.
        # Bytes 06 and 05 are 1st  distance measurement in this packet.
        # Bytes 10 and 09 are 2nd  distance measurement in this packet.
        # Bytes 14 and 13 are 3rd  distance measurement in this packet.
        # Bytes 18 and 17 are the 4th distance measurement in this packet. 
        # Bytes 08:07, 12:11, 16:15, and 20:19 represent information about 
        # the surface off of which the laser has bounced to be detected by 
        # the LiDAR.
        # Bytes 22 and 21 are checksum and are used for determining the 
        # validity of the received packet.
    
        # main loop of driver
        # r = rospy.Rate(10)
        rospy.loginfo("0")
        # requestScan()
        data = []
        i = 0
        while not rospy.is_shutdown():
            # string = self.port.readline()
            byte = self.port.read()
            b = ord(byte)
            data.append(b)
            i = i +1
            if i > 1000:
              for j in range(0,999):
                rospy.loginfo("%d", j);
                rospy.loginfo(": {:02X}".format(data[j]));
                i = 0
                data = []
Exemple #38
0
	def sonarsCallback(self, event):
		sonars = self.rh.sensors.getSonarsMeasurements()['sonars']
		laser_msg = LaserScan()
		laser_msg.ranges.append(sonars['front_right'])
		laser_msg.ranges.append(sonars['front_left'])
		laser_msg.range_max = 1.00
		laser_msg.angle_increment = 0.785398185253
		laser_msg.angle_min = -0.392699092627
		self.pub.publish(laser_msg)
Exemple #39
0
 def createLaserMessage(self, frameID, keyPrefix, scanNum):
     laserScanMsg = LaserScan()
     laserScanMsg.header.frame_id = frameID
     laserScanMsg.angle_min = self.PEPPER_LASER_MIN_ANGLE
     laserScanMsg.angle_max = self.PEPPER_LASER_MAX_ANGLE
     laserScanMsg.angle_increment = self.PEPPER_LASER_FOV/scanNum
     laserScanMsg.range_min = self.PEPPER_LASER_MIN_RANGE
     laserScanMsg.range_max = self.PEPPER_LASER_MAX_RANGE
     return laserScanMsg
Exemple #40
0
    def update(self):
        #############################################################################
        now = rospy.Time.now()
        if now > self.t_next:
            elapsed = now - self.then
            self.then = now
            elapsed = elapsed.to_sec()

            # this approximation works (in radians) for small angles
            th = self.th - self.th_pre

            self.dr = th / elapsed

            # publish the odom information
            quaternion = Quaternion()
            quaternion.x = self.qx
            quaternion.y = self.qy
            quaternion.z = self.qz
            quaternion.w = self.qw
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),
                (0, 0, quaternion.z, quaternion.w),
                rospy.Time.now(),
                self.base_frame_id,
                self.odom_frame_id,
            )

            self.laserBroadcaster.sendTransform(
                (0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), self.laser_frame_id, self.base_frame_id
            )

            odom = Odometry()
            odom.header.stamp = now
            odom.header.frame_id = self.odom_frame_id
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.child_frame_id = self.base_frame_id
            odom.twist.twist.linear.x = self.dx
            odom.twist.twist.linear.y = 0
            odom.twist.twist.angular.z = self.dr
            self.odomPub.publish(odom)
            laser = LaserScan()
            laser.header.stamp = now
            laser.header.frame_id = self.laser_frame_id
            laser.angle_min = self.laser.angle_min
            laser.angle_max = self.laser.angle_max
            laser.angle_increment = self.laser.angle_increment
            laser.time_increment = self.laser.time_increment
            laser.scan_time = self.laser.scan_time
            laser.range_min = self.laser.range_min
            laser.range_max = self.laser.range_max
            laser.ranges = self.laser.ranges
            laser.intensities = self.laser.intensities
            self.laserPub.publish(laser)
Exemple #41
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)
Exemple #42
0
 def publish_laser_msg(self, ranges):
     msg = LaserScan()
     msg.angle_min = self.robot.laser.min_angle
     msg.angle_max = self.robot.laser.max_angle
     msg.angle_increment = self.robot.laser.resolution
     msg.range_min = 0.0
     msg.range_max = self.robot.laser.range
     msg.ranges = ranges
     msg.header.stamp = rospy.Time.now()
     self.laser_publisher.publish(msg)
	def make_wallscan(self, data):
		num_readings = len(data)
		wall_scan = LaserScan()
		wall_scan.header.frame_id = "base_laser_link"
		wall_scan.ranges = data
		wall_scan.angle_min = -3.14;
		wall_scan.angle_max = 3.14;
		wall_scan.angle_increment = (3.14*2) / num_readings;
		wall_scan.range_min = 0.0;
		wall_scan.range_max = 5;

		return wall_scan
Exemple #44
0
 def build_laser_scan(self, ranges):
     result = LaserScan()
     result.header.stamp = rospy.Time.now()
     result.angle_min = -almath.PI
     result.angle_max = almath.PI
     if len(ranges[1]) > 0:
         result.angle_increment = (result.angle_max - result.angle_min) / len(ranges[1])
     result.range_min = 0.0
     result.range_max = ranges[0]
     for range_it in ranges[1]:
         result.ranges.append(range_it[1])
     return result
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
Exemple #46
0
 def inputCallback(self, msg):
 #########################################################################
     # rospy.loginfo("-D- range_filter inputCallback")
     cur_val = msg.value
 
     if cur_val <= self.max_valid and cur_val >= self.min_valid:
         self.prev.append(cur_val)
         del self.prev[0]
     
         p = array(self.prev)
         self.rolling_ave = p.mean()
         self.rolling_std = p.std()
     
         self.rolling_meters = ((self.b * self.rolling_ave) ** self.m) / 100
     
         self.filtered_pub.publish( self.rolling_meters )
         self.std_pub.publish( self.rolling_std )
         
         rng = Range()
         rng.radiation_type = 1
         rng.min_range = self.min_range
         rng.max_range = self.max_range
         rng.range = self.rolling_meters
         rng.header.frame_id = self.frame
         rng.field_of_view = 0.1
         rng.header.stamp = rospy.Time.now()
         
         self.range_pub.publish( rng )
        
         ranges = [] 
         intensities = []
         angle_start = 0.0 - self.field_of_view
         angle_stop = self.field_of_view
         for angle in linspace( angle_start, angle_stop, 10):
             ranges.append( self.rolling_meters )
             intensities.append( 1.0 )
         scan = LaserScan()
         scan.ranges = ranges
         scan.header.frame_id = self.frame
         scan.time_increment = 0;
         scan.range_min = self.min_range
         scan.range_max = self.max_range
         scan.angle_min = angle_start
         scan.angle_max = angle_stop
         scan.angle_increment = (angle_stop - angle_start) / 10
         scan.intensities = intensities
         scan.scan_time = self.scan_time
         scan.header.stamp = rospy.Time.now()
         self.scan_pub.publish( scan )
Exemple #47
0
 def publishLaserScan(self,data=None):
     scan = LaserScan()
     scan.header.seq = 1
     scan.header.stamp = rospy.Time.now()
     num_readings = 100
     laser_frequency = 40
     scan.header.frame_id = "base_laser"
     scan.angle_min = radians(-30)
     scan.angle_max = radians(30)
     scan.angle_increment = radians(60) / num_readings
     scan.time_increment = (1 / laser_frequency) / (num_readings)
     scan.range_min = 0.5
     scan.range_max = 6
     scan.ranges = [5]*num_readings
     self.laserScanPublisher.publish(scan)
Exemple #48
0
    def publishZoneScores(self,data, prh_resolution):
        """
        Publishes the zone scores
        """
        pc = LaserScan()
        pc.header.frame_id = "/base_link"
        pc.header.stamp = rospy.get_rostime()

        pc.angle_min = -np.pi
        pc.angle_max = np.pi
        pc.angle_increment = prh_resolution
        pc.range_min = 0.00
        pc.range_max = 5.0

        for r in data:
            pc.ranges.append(r)

        self.zone_score_pub.publish(pc)
Exemple #49
0
def co_callback(coa): # coa = CastObstacleArray

    scan = LaserScan()
    scan.header.frame_id = '/base_link'
    scan.angle_min = 0

    n = len(co_msg.obstacles)
    scan.angle_min = coa.obstacles[0].theta
    scan.angle_max = coa.obstacles[n-1].theta
    scan.angle_increment = coa.obstacles[1].theta - coa.obstacles[0].theta

    for obs in coa.obstacles:
        distance = rho2range(obs.rho)
        scan.ranges.append(distance)
        scan.range_min = min(scan.range_min, distance)
        scan.range_max = max(scan.range_min, distance)

    scan_publisher.publish(scan)
Exemple #50
0
def handle_laser_scan(laserMsg,currTime):
	scan = LaserScan()
	scan.header.stamp = currTime
	scan.header.frame_id = "base_link"
	scan.angle_min = -1.57
	scan.angle_max = 1.57
	scan.angle_increment = 3.14/181
	scan.time_increment = (1/0.5)/181
	scan.range_min = 0.0
	scan.range_max = 10.0
	scan.ranges = [0.0 for i in xrange(len(laserMsg)-1)]
	scan.intensities = [0.0 for i in xrange(len(laserMsg)-1)]
	for i in xrange(len(laserMsg)-1):
		scan.ranges[i] =laserMsg[i]
		scan.intensities[i]= 100
	laserPub =  rospy.Publisher('scan',LaserScan,queue_size = 1)
	laserPub.publish(scan)
	rospy.sleep(4.0)
    def Cb(self, msg):
        n_ranges = list()
        pv = msg.data[0]
        for i in range(50):
            if pv>0.0 and msg.data[i]>0.0:
                n_ranges.append((pv+msg.data[i])/2)
            else:
                n_ranges.append(0.0)
            n_ranges.append(msg.data[i])
            pv = msg.data[i]

        laserMsg = LaserScan()
        laserMsg.ranges = n_ranges
        laserMsg.angle_increment = 0.2; 
        laserMsg.time_increment = 1/50
        laserMsg.header.frame_id = '/ultra'
        laserMsg.header.stamp = rospy.Time.now()
        self.scanPub.publish(laserMsg)
Exemple #52
0
    def publishPolarHistogram(self):
        """
        Publishes the polar histogram
        """
        pc = LaserScan()
        pc.header.frame_id = "/base_link"
        pc.header.stamp = rospy.get_rostime()

        pc.angle_min = -np.pi
        pc.angle_max = np.pi
        pc.angle_increment = self.prh_resolution
        pc.range_min = 0.00
        pc.range_max = 5.0

        self.polar_range_hist_lock.acquire()
        for r in self.polar_range_hist:
            pc.ranges.append(r)

        self.polar_range_hist_lock.release()
        self.polar_hist_pub.publish(pc)
def lidar_listener():
	port = 5560
	kill = False
	
	#init
	rospy.loginfo("Initializing LIDAR Listener...");
	sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
	sock.bind(('', port))
	laserpub = rospy.Publisher('/scan', LaserScan)
	rospy.init_node('lidar_listener')
	
	message = LaserScan();
	message.angle_max = 4.1887
	message.angle_increment = .0062832
	message.scan_time = .1
	message.range_min = .02
	message.range_max = 4.0
	message.header.frame_id = "/base_laser"
	
	#Loop
	rospy.loginfo("LIDAR Listener initialized")
	while not rospy.is_shutdown():
		try:
			raw_data =sock.recv(4096, socket.MSG_DONTWAIT)
			message.header.stamp=rospy.Time.now()
			if not raw_data:
				rospy.logwarn("No Raw Data")
			else:
				message.ranges=decode(raw_data)
				print message.ranges
				rospy.logdebug(message.ranges)
				laserpub.publish(message)
		except socket.error as ex:
			if (ex[0] != 11):
				rospy.logwarn( "Lidar Listener Socket Exception: ",ex)
			else:
				rospy.logdebug("Lidar Listener Timed Out");
		except Exception as ex:
			raise ex
	rospy.loginfo("Shutting down LIDAR listener...")
	sock.close()		
Exemple #54
0
    def __init__(self):
        rospy.init_node('fake_laser')

        # parameters
        self.rate = rospy.get_param("~rate", 1.0)
        self.frame_id = rospy.get_param("~frame", "base_laser")
        self.range_min = rospy.get_param("~range_min", 0.2)
        self.range_max = rospy.get_param("~range_max", 5.5)
        self.angle_min = rospy.get_param("~angle_min", -1.57)
        self.angle_max = rospy.get_param("~angle_max", 1.57)
        self.num_readings = rospy.get_param("~num_readings", 100)
        self.fixed_reading = rospy.get_param("~fixed_reading", 2.0)
        self.scan_time = rospy.get_param("~scan_time", 0.1)
        
        self.angle_increment = (self.angle_max - self.angle_min) / self.num_readings
        self.time_increment = self.scan_time / (self.num_readings)

        self.scanPub = rospy.Publisher('scan', LaserScan)

        rospy.loginfo("Started fake laser node publishing to /scan topic.")

        r = rospy.Rate(self.rate)
        
        while not rospy.is_shutdown():
            ranges = list()
            # Generate the fake data.
            for i in range(self.num_readings):
                ranges.append(self.fixed_reading * sin(i) * random.uniform(0, 1))
                
            scan = LaserScan()
            scan.header.stamp = rospy.Time.now()     
            scan.header.frame_id = self.frame_id
            scan.angle_min = self.angle_min
            scan.angle_max = self.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 = ranges    
            self.scanPub.publish(scan)
            r.sleep()
def build_constant_scan(
        range_val, intensity_val,
        angle_min, angle_max, angle_increment, scan_time):
    count = np.uint(np.ceil((angle_max - angle_min) / angle_increment))
    if count < 0:
        raise BuildScanException

    scan = LaserScan()
    scan.header.stamp = rospy.rostime.Time.from_sec(10.10)
    scan.header.frame_id = "laser_frame"
    scan.angle_min = angle_min
    scan.angle_max = angle_max
    scan.angle_increment = angle_increment
    scan.scan_time = scan_time.to_sec()
    scan.range_min = PROJECTION_TEST_RANGE_MIN
    scan.range_max = PROJECTION_TEST_RANGE_MAX
    scan.ranges = [range_val for _ in range(count)]
    scan.intensities = [intensity_val for _ in range(count)]
    scan.time_increment = scan_time.to_sec()/count

    return scan
Exemple #56
0
def imu_serial():
    pub = rospy.Publisher('laser_scan', LaserScan)
    rospy.init_node('imu_serial')
    laser_msg = LaserScan()

    laser_msg.header.frame_id = 'laser'
    laser_msg.angle_min = -1.5
    laser_msg.angle_max = 1.5
    laser_msg.angle_increment = 0.1
    laser_msg.time_increment = 0.1
    laser_msg.scan_time = 0.1
    laser_msg.range_min = 0.5
    laser_msg.range_max = 1.5    
    laser_msg.ranges = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 9.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.1, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 1.0]
    laser_msg.intensities = laser_msg.ranges 

    r = rospy.Rate(100) # 10hz
    while not rospy.is_shutdown():
        laser_msg.header.stamp = rospy.get_rostime()
        pub.publish(laser_msg)
        r.sleep()
 def publish_laser_data(self, publisher):
     scan = LaserScan()
     scan.header.stamp = rospy.Time.now()
     scan.header.frame_id = "base_laser_link"
     scan.angle_min = -30 * math.pi / 180
     scan.angle_max = 30 * math.pi / 180
     scan.angle_increment = 1 * math.pi / 180 
     scan.range_min = 0.03 
     scan.range_max = 3.00
     scan.ranges = []
     # read the ranges from left to right
     for i in range(0,15):
         scan.ranges.append(self.ranges["ultrasonic_3"])
     for i in range(15,30):
         scan.ranges.append(self.ranges["ultrasonic_1"])
     for i in range(30,45):
         scan.ranges.append(self.ranges["ultrasonic_2"])
     for i in range(45,60):
         scan.ranges.append(self.ranges["ultrasonic_4"])
     publisher.publish(scan)
     # clear out the scans
     self.resetRanges()
Exemple #58
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