Exemplo n.º 1
0
    def laser_callback(self, msg):
        '''
        Function called each time a LaserScan message with topic "scan" arrives. 
        '''
        
        # Project LaserScan to points in space
        rng = np.array(msg.ranges)
        ang = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
        points = np.vstack((rng * np.cos(ang),
                            rng * np.sin(ang)))
        
        msg.range_max = 3
        self.pub_laser.publish(msg)
                            
        # Filter long ranges
        points = points[:, rng < msg.range_max]
        
        # Use split and merge to obtain lines and publish
        lines = splitandmerge(points)

        self.counter += 1
        new_scan = 'scan_line' + str(self.counter)

        #publish_lines(lines, self.pub_line, frame=msg.header.frame_id,
                      #time=msg.header.stamp, ns=new_scan, color=(1,0,0))
        
        # Publish results
        publish_lines(lines, self.pub_line, frame=msg.header.frame_id,
                      time=msg.header.stamp, ns='scan_line', color=(1,0,0))
Exemplo n.º 2
0
    def lines_callback(self, msg):
        """Callback to a LaserScan message with topic "scan"."""
        # Save time
        self.linestime = msg.header.stamp

        # Get the lines
        if len(msg.points) > 0:

            # Structure for the lines
            self.lines = np.zeros((len(msg.points) / 2, 4))

            for i in range(0, len(msg.points) / 2):
                # Get start and end points
                pt1 = msg.points[2 * i]
                pt2 = msg.points[2 * i + 1]

                # Transform to robot frame
                pt1R = comp(self.robot2sensor, [pt1.x, pt1.y, 0.0])
                pt2R = comp(self.robot2sensor, [pt2.x, pt2.y, 0.0])

                # Append to line list
                self.lines[i, :2] = pt1R[:2]
                self.lines[i, 2:] = pt2R[:2]

            # Flag
            self.new_laser = True

            # Publish
            publish_lines(self.lines, self.pub_lines,
                          frame='/robot',
                          time=msg.header.stamp,
                          ns='lines_robot', color=(0, 0, 1))
Exemplo n.º 3
0
Arquivo: node.py Projeto: raabid236/PR
    def publish_results(self):
        '''
        Publishes all results from the filter.
        '''
        # Ground turth of the map of the room
        publish_lines(self.map,
                      self.pub_map_gt,
                      frame='/world',
                      ns='gt_map',
                      color=(0.3, 0.3, 0.3))

        odom, ellipse, trans, rot, room_map = get_ekf_msgs(self.ekf)

        publish_lines(room_map,
                      self.pub_map,
                      frame='/world',
                      ns='map',
                      color=(0, 1, 0))

        self.pub_odom.publish(odom)
        self.pub_uncertainity.publish(ellipse)
        self.tfBroad.sendTransform(translation=trans,
                                   rotation=rot,
                                   time=self.time,
                                   child='/robot',
                                   parent='/world')
Exemplo n.º 4
0
    def cbk_lines(self, msg):
        """Republish the laser scam in the /robot frame."""
        # Save time
        self.linestime = msg.header.stamp

        # Get the lines
        if len(msg.points) > 0:

            # Structure for the lines
            self.lines = np.zeros((len(msg.points) / 2, 4))

            for i in range(0, len(msg.points)/2):
                # Get start and end points
                pt1 = msg.points[2*i]
                pt2 = msg.points[2*i+1]

                # Transform to robot frame
                pt1R = funcs.comp(self.robot2sensor, [pt1.x, pt1.y, 0.0])
                pt2R = funcs.comp(self.robot2sensor, [pt2.x, pt2.y, 0.0])

                # Append to line list
                self.lines[i, :2] = pt1R[:2]
                self.lines[i, 2:] = pt2R[:2]

            # Flag
            self.new_laser = True

            # Publish
            funcs.publish_lines(self.lines, self.pub_lines, frame='robot',
                                time=msg.header.stamp, ns='lines_robot',
                                color=(0, 0, 1))
Exemplo n.º 5
0
 def publish_results(self, lines):
     '''
     Publishes all results from the filter.
     '''
     if self.time is not None:
         
         time = rospy.Time.now()
         
         # Map of the room
         map_lines = get_dataset3_map()
         publish_lines(map_lines, self.pub_map, frame='/world', ns='map',
                       color=(0,1,0))
         
         # Particles and biggest weighted particle
         msg, msg_mean, msg_odom, trans, rot, msg_wei = get_particle_msgs(self.part_filter,
                                                                 time)
         self.pub_particles.publish(msg)
         self.pub_big_particle.publish(msg_mean)
         self.pub_odom.publish(msg_odom)
         self.pub_wei.publish(msg_wei)
         self.tfBroad.sendTransform(translation = trans,
                                    rotation = rot, 
                                    time = time,
                                    parent = 'world',
                                    child = 'mean_particle')
                                    
         # Publish scanned lines
         if lines is not None:
             publish_lines(lines, self.pub_lines_mean, frame='mean_particle',
                       time=time, ns='scan_lines_mean', color=(0,0,1))
Exemplo n.º 6
0
    def lines_callback(self, msg):
        '''
        Function called each time a LaserScan message with topic "scan" arrives. 
        '''
        # Save time
        self.linestime = msg.header.stamp

        # Get the lines
        if len(msg.points) > 0:
            
            # Structure for the lines
            self.lines = np.zeros((len(msg.points) / 2, 4))
                  
            for i in range(0, len(msg.points)/2):
                # Get start and end points
                pt1 = msg.points[2*i]
                pt2 = msg.points[2*i+1]
                
                # Transform to robot frame
                pt1R = comp(self.robot2sensor, [pt1.x, pt1.y, 0.0])
                pt2R = comp(self.robot2sensor, [pt2.x, pt2.y, 0.0])
                
                # Append to line list
                self.lines[i, :2] = pt1R[:2]
                self.lines[i, 2:] = pt2R[:2]
            
            # Flag
            self.new_laser = True
            
            # Publish
            publish_lines(self.lines, self.pub_lines,
                          frame = '/robot',
                          time = msg.header.stamp,
                          ns = 'lines_robot', color = (0,0,1))
Exemplo n.º 7
0
 def laser_callback(self, msg):
     '''
     Function called each time a LaserScan message with topic "scan" arrives. 
     '''
     
     # Project LaserScan to points in space
     rng = np.array(msg.ranges)
     ang = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
     points = np.vstack((rng * np.cos(ang),
                         rng * np.sin(ang)))
     
     msg.range_max = 3
     self.pub_laser.publish(msg)
                         
     # Filter long ranges
     points = points[:, rng < msg.range_max]
     
     # Use split and merge to obtain lines and publish
     lines = splitandmerge(points)
     
     # # Publish results
     
     # publish_lines(lines, self.pub_line, frame=msg.header.frame_id,
     #              time=msg.header.stamp, ns='scan_line', color=(1,0,0))
     self.i = self.i + 1
     nss = 'scan_line' + str(self.i)
     #msg.header.frame_id = "d" + str(self.i)
     #msg.header.frame_id = 'world'
     publish_lines(lines, self.pub_line, frame=msg.header.frame_id,
                  time=msg.header.stamp, ns=nss, color=(1,0,0))
Exemplo n.º 8
0
    def publish_results(self):
        """Publishe results from the filter."""
        # Map of the room (ground truth)
        funcs.publish_lines(self.ekf.map, self.pub_lines, frame='world',
                            ns='map', color=(0, 1, 0))

        # Get filter data
        odom, ellipse, trans, rot, dummy = funcs.get_ekf_msgs(self.ekf)

        # Publish results
        self.pub_odom.publish(odom)
        self.pub_uncertainity.publish(ellipse)
        self.tfBroad.sendTransform(translation=trans,
                                   rotation=rot,
                                   time=self.time,
                                   child='robot',
                                   parent='world')
Exemplo n.º 9
0
    def publish_results(self):
        '''
        Publishes all results from the filter.
        '''
        # Ground turth of the map of the room
        publish_lines(self.map, self.pub_map_gt, frame='/world', ns='gt_map', color=(0.3,0.3,0.3))
        
        odom, ellipse, trans, rot, room_map = get_ekf_msgs(self.ekf)
        
        publish_lines(room_map, self.pub_map, frame='/world', ns='map', color=(0,1,0))
        

        self.pub_odom.publish(odom)
        self.pub_uncertainity.publish(ellipse)
        self.tfBroad.sendTransform(translation = trans,
                                   rotation = rot, 
                                   time = self.time,
                                   child = '/robot',
                                   parent = '/world')
Exemplo n.º 10
0
    def publish_results(self, lines):
        '''
        Publishes all results from the filter.
        '''
        if self.time is not None:

            time = rospy.Time.now()

            # Map of the room
            map_lines = get_dataset3_map()
            publish_lines(map_lines,
                          self.pub_map,
                          frame='/world',
                          ns='map',
                          color=(0, 1, 0))

            # Particles and biggest weighted particle
            msg, msg_mean, msg_odom, trans, rot, msg_wei = get_particle_msgs(
                self.part_filter, time)
            self.pub_particles.publish(msg)
            self.pub_big_particle.publish(msg_mean)
            self.pub_odom.publish(msg_odom)
            self.pub_wei.publish(msg_wei)
            self.tfBroad.sendTransform(translation=trans,
                                       rotation=rot,
                                       time=time,
                                       parent='world',
                                       child='mean_particle')

            # Publish scanned lines
            if lines is not None:
                publish_lines(lines,
                              self.pub_lines_mean,
                              frame='mean_particle',
                              time=time,
                              ns='scan_lines_mean',
                              color=(0, 0, 1))