コード例 #1
0
ファイル: node.py プロジェクト: pengsongyou/PR_lab
class LocalizationNode(object):
    '''
    Class to hold all ROS related transactions to use split and merge algorithm.
    '''
    
    #===========================================================================
    def __init__(self, x0=0,y0=0,theta0=0, odom_lin_sigma=0.025, odom_ang_sigma=np.deg2rad(2),
                        meas_rng_noise=0.2,  meas_ang_noise=np.deg2rad(10),xs = 0, ys = 0, thetas = 0,
                       rob2sensor = [-0.087, 0.013, np.deg2rad(0)]):
        '''
        Initializes publishers, subscribers and the particle filter.
        '''

        # Transformation from robot to sensor
        self.robotToSensor = np.array([xs,ys,thetas])        
        
        # Publishers
        self.pub_lines = rospy.Publisher("ekf_lines", Marker, queue_size=0)
        self.pub_map = rospy.Publisher("map", Marker, queue_size=0)
        self.pub_map_gt = rospy.Publisher("map_gt", Marker, queue_size=0)
        self.pub_odom = rospy.Publisher("predicted_odom", Odometry, queue_size=0)
        self.pub_uncertainity = rospy.Publisher("uncertainity",  Marker, queue_size=0)
        self.pub_laser = rospy.Publisher("ekf_laser",  LaserScan, queue_size=0)
        
        # Subscribers
        self.sub_scan = rospy.Subscriber("scan", LaserScan, self.laser_callback)
        self.sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback)
        self.sub_scan = rospy.Subscriber("lines", Marker, self.lines_callback)
        
        # TF
        self.tfBroad = tf.TransformBroadcaster()
        
        # Incremental odometry
        self.last_odom = None
        self.odom = None
        
        # Flags
        self.new_odom = False
        self.new_laser = False
        self.pub = False
        
        # Ground truth map
        dataset = rospy.get_param("~dataset",None)
        if dataset == 1:
            self.map = get_map()
            x0 = 0.8-0.1908
            y0 = 0.3+0.08481
            theta0 = -0.034128
        elif dataset == 2:
            self.map = np.array([])
        elif dataset == 3:
            self.map = get_dataset3_map()
        else:
            self.map = np.array([])
        
        # Initial state
        self.x0 = np.array([x0,y0,theta0])        
        
        # Particle filter
        self.ekf = EKF_SLAM(x0, y0, theta0, odom_lin_sigma, odom_ang_sigma, meas_rng_noise, meas_ang_noise)
        
        # Transformation from robot to sensor
        self.robot2sensor = np.array(rob2sensor)
                
            
    #===========================================================================
    def odom_callback(self, msg):
        '''
        Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
        '''
        # Save time
        self.time = msg.header.stamp
        
        # Translation
        trans = (msg.pose.pose.position.x, 
                 msg.pose.pose.position.y, 
                 msg.pose.pose.position.z)
        
        # Rotation
        rot = (msg.pose.pose.orientation.x,
               msg.pose.pose.orientation.y,
               msg.pose.pose.orientation.z,
               msg.pose.pose.orientation.w)
               
        # Publish transform
        self.tfBroad.sendTransform(translation = self.robot2sensor,
                                   rotation = (0, 0, 0, 1), 
                                   time = msg.header.stamp,
                                   child = '/sensor',
                                   parent = '/robot')
        self.tfBroad.sendTransform(translation = self.robot2sensor,
                                   rotation = (0, 0, 0, 1), 
                                   time = msg.header.stamp,
                                   child = '/camera_depth_frame',
                                   parent = '/base_footprint')
        self.tfBroad.sendTransform(translation = trans,
                                   rotation = rot, 
                                   time = msg.header.stamp,
                                   child = '/base_footprint',
                                   parent = '/odom')
        self.tfBroad.sendTransform(translation = (self.x0[0],self.x0[1],0),
                                   rotation = tf.transformations.quaternion_from_euler(0,0,self.x0[2]), 
                                   time = msg.header.stamp,
                                   child = '/odom',
                                   parent = '/world')
                                   
        # Incremental odometry
        if self.last_odom is not None and not self.new_odom:
            
            # Increment computation
            delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
            delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
            yaw = yaw_from_quaternion(msg.pose.pose.orientation)
            lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
            
            # Odometry seen from vehicle frame
            self.uk = np.array([delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
                              - delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
                                angle_wrap(yaw - lyaw)])
            
            # Flag
            self.new_odom = True
        
            # For next loop
            self.last_odom = msg
        if self.last_odom is None:
            self.last_odom = msg
        
    
    #===========================================================================
    def laser_callback(self, msg):
        '''
        Republishes laser scan in the EKF solution frame.
        '''
        msg.header.frame_id = '/sensor'
        self.pub_laser.publish(msg)
    
    #===========================================================================
    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))
    
    #===========================================================================
    def iterate(self):
        '''
        Main loop of the filter.
        '''
        # Prediction
        if self.new_odom:
            
            self.ekf.predict(self.uk.copy())
            self.new_odom = False
            self.pub = True
            
        # Weightimg and resampling
        if self.new_laser:
            Innovk_List, H_k_List, Rk_List,idx_not_associated = self.ekf.data_association(self.lines.copy())
            self.ekf.update_position(Innovk_List, H_k_List, Rk_List)
            self.ekf.state_augmentation(self.lines.copy(),idx_not_associated)
            self.new_laser = False
            self.pub = True
            
        # Publish results
        if self.pub:
            self.publish_results()
            self.pub = False
    
    #===========================================================================
    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')
コード例 #2
0
ファイル: node.py プロジェクト: raabid236/PR
class LocalizationNode(object):
    '''
    Class to hold all ROS related transactions to use split and merge algorithm.
    '''

    #===========================================================================
    def __init__(self,
                 x0=0,
                 y0=0,
                 theta0=0,
                 odom_lin_sigma=0.025,
                 odom_ang_sigma=np.deg2rad(2),
                 meas_rng_noise=0.2,
                 meas_ang_noise=np.deg2rad(10),
                 xs=0,
                 ys=0,
                 thetas=0,
                 rob2sensor=[-0.087, 0.013, np.deg2rad(0)]):
        '''
        Initializes publishers, subscribers and the particle filter.
        '''

        # Transformation from robot to sensor
        self.robotToSensor = np.array([xs, ys, thetas])

        # Publishers
        self.pub_lines = rospy.Publisher("ekf_lines", Marker, queue_size=0)
        self.pub_map = rospy.Publisher("map", Marker, queue_size=0)
        self.pub_map_gt = rospy.Publisher("map_gt", Marker, queue_size=0)
        self.pub_odom = rospy.Publisher("predicted_odom",
                                        Odometry,
                                        queue_size=0)
        self.pub_uncertainity = rospy.Publisher("uncertainity",
                                                Marker,
                                                queue_size=0)
        self.pub_laser = rospy.Publisher("ekf_laser", LaserScan, queue_size=0)

        # Subscribers
        self.sub_scan = rospy.Subscriber("scan", LaserScan,
                                         self.laser_callback)
        self.sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback)
        self.sub_scan = rospy.Subscriber("lines", Marker, self.lines_callback)

        # TF
        self.tfBroad = tf.TransformBroadcaster()

        # Incremental odometry
        self.last_odom = None
        self.odom = None

        # Flags
        self.new_odom = False
        self.new_laser = False
        self.pub = False

        # Ground truth map
        dataset = rospy.get_param("~dataset", None)
        if dataset == 1:
            self.map = get_map()
            x0 = 0.8 - 0.1908
            y0 = 0.3 + 0.08481
            theta0 = -0.034128
        elif dataset == 2:
            self.map = np.array([])
        elif dataset == 3:
            self.map = get_dataset3_map()
        else:
            self.map = np.array([])

        # Initial state
        self.x0 = np.array([x0, y0, theta0])

        # Particle filter
        self.ekf = EKF_SLAM(x0, y0, theta0, odom_lin_sigma, odom_ang_sigma,
                            meas_rng_noise, meas_ang_noise)

        # Transformation from robot to sensor
        self.robot2sensor = np.array(rob2sensor)

    #===========================================================================
    def odom_callback(self, msg):
        '''
        Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
        '''
        # Save time
        self.time = msg.header.stamp

        # Translation
        trans = (msg.pose.pose.position.x, msg.pose.pose.position.y,
                 msg.pose.pose.position.z)

        # Rotation
        rot = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
               msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)

        # Publish transform
        self.tfBroad.sendTransform(translation=self.robot2sensor,
                                   rotation=(0, 0, 0, 1),
                                   time=msg.header.stamp,
                                   child='/sensor',
                                   parent='/robot')
        self.tfBroad.sendTransform(translation=self.robot2sensor,
                                   rotation=(0, 0, 0, 1),
                                   time=msg.header.stamp,
                                   child='/camera_depth_frame',
                                   parent='/base_footprint')
        self.tfBroad.sendTransform(translation=trans,
                                   rotation=rot,
                                   time=msg.header.stamp,
                                   child='/base_footprint',
                                   parent='/odom')
        self.tfBroad.sendTransform(
            translation=(self.x0[0], self.x0[1], 0),
            rotation=tf.transformations.quaternion_from_euler(
                0, 0, self.x0[2]),
            time=msg.header.stamp,
            child='/odom',
            parent='/world')

        # Incremental odometry
        if self.last_odom is not None and not self.new_odom:

            # Increment computation
            delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
            delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
            yaw = yaw_from_quaternion(msg.pose.pose.orientation)
            lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)

            # Odometry seen from vehicle frame
            self.uk = np.array([
                delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
                -delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
                angle_wrap(yaw - lyaw)
            ])

            # Flag
            self.new_odom = True

            # For next loop
            self.last_odom = msg
        if self.last_odom is None:
            self.last_odom = msg

    #===========================================================================
    def laser_callback(self, msg):
        '''
        Republishes laser scan in the EKF solution frame.
        '''
        msg.header.frame_id = '/sensor'
        self.pub_laser.publish(msg)

    #===========================================================================
    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))

    #===========================================================================
    def iterate(self):
        '''
        Main loop of the filter.
        '''
        # Prediction
        if self.new_odom:

            self.ekf.predict(self.uk.copy())
            self.new_odom = False
            self.pub = True

        # Weightimg and resampling
        if self.new_laser:

            Innovk_List, H_k_List, Rk_List, idx_not_associated = self.ekf.data_association(
                self.lines.copy())
            self.ekf.update_position(Innovk_List, H_k_List, Rk_List)
            self.ekf.state_augmentation(self.lines.copy(), idx_not_associated)
            self.new_laser = False
            self.pub = True

        # Publish results
        if self.pub:
            self.publish_results()
            self.pub = False

    #===========================================================================
    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')
コード例 #3
0
ファイル: node.py プロジェクト: evargasv/ekf-slam
class LocalizationNode(object):
    '''
    Class to hold all ROS related transactions to use split and merge algorithm.
    '''
    
    #===========================================================================
    def __init__(self, x0=0,y0=0,theta0=0, odom_lin_sigma=0.025, odom_ang_sigma=np.deg2rad(2),
                        meas_rng_noise=0.2,  meas_ang_noise=np.deg2rad(10),xs = 0, ys = 0, thetas = 0):
        '''
        Initializes publishers, subscribers and the particle filter.
        '''

        # Transformation from robot to sensor
        self.robotToSensor = np.array([xs,ys,thetas])        
        
        # Publishers
        self.pub_lines = rospy.Publisher("lines", Marker)
        self.pub_odom = rospy.Publisher("predicted_odom", Odometry)
        self.pub_uncertainity = rospy.Publisher("uncertainity",  Marker)
        
        # Subscribers
        self.sub_scan = rospy.Subscriber("scan", LaserScan, self.laser_callback)
        self.sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback)
        
        # TF
        self.tfBroad = tf.TransformBroadcaster()
        
        # Incremental odometry
        self.last_odom = None
        self.odom = None
        
        # Flags
        self.new_odom = False
        self.new_laser = False
        self.pub = False
        
        # Ground truth map
#        self.map = get_map(0.8,0.3,-0.03)    
        self.map = get_map()
        
        # Initial state
        self.x0 = np.array([x0,y0,theta0])        
        
        # Particle filter
        self.ekf = EKF_SLAM(x0, y0, theta0, odom_lin_sigma, odom_ang_sigma, meas_rng_noise, meas_ang_noise)
                
            
    #===========================================================================
    def odom_callback(self, msg):
        '''
        Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
        '''
        # Save time
        self.time = msg.header.stamp
        
        # Translation
        trans = (msg.pose.pose.position.x, 
                 msg.pose.pose.position.y, 
                 msg.pose.pose.position.z)
        
        # Rotation
        rot = (msg.pose.pose.orientation.x,
               msg.pose.pose.orientation.y,
               msg.pose.pose.orientation.z,
               msg.pose.pose.orientation.w)
        
        # Publish transform
        self.tfBroad.sendTransform(translation = (self.robotToSensor[0],self.robotToSensor[1],0),
                                   rotation = tf.transformations.quaternion_from_euler(0,0,self.robotToSensor[2]),
                                   time = msg.header.stamp,
                                   child = '/openni_depth_frame',
                                   parent = '/robot')
        
        self.tfBroad.sendTransform(translation = trans,
                                   rotation = rot, 
                                   time = msg.header.stamp,
                                   child = '/robot_original',
                                   parent = '/odom')
        self.tfBroad.sendTransform(translation = (self.x0[0]-0.1908,self.x0[1]+0.08481,self.x0[2]),
                                   rotation = tf.transformations.quaternion_from_euler(0,0,-0.034128),
#                                   rotation = (0,0,0,1), 
                                   time = msg.header.stamp,
                                   child = '/odom',
                                   parent = '/world')
                                   
        # Incremental odometry
        if self.last_odom is not None and not self.new_odom:
            
            # Increment computation
            delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
            delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
            yaw = yaw_from_quaternion(msg.pose.pose.orientation)
            lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
            
            # Odometry seen from vehicle frame
            self.uk = np.array([delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
                              - delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
                                angle_wrap(yaw - lyaw)])
            
            # Flag
            self.new_odom = True
        
            # For next loop
            self.last_odom = msg
        if self.last_odom is None:
            self.last_odom = msg
        
    
    #===========================================================================
    def laser_callback(self, msg):
        '''
        Function called each time a LaserScan message with topic "scan" arrives. 
        '''
        # Save time
        self.time = msg.header.stamp
        
        # 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)))
                            
        # Filter long ranges
        points = points[:, rng < msg.range_max]
        
        # Use split and merge to obtain lines and publish
        self.lines = splitandmerge(points, split_thres=0.1,
                                      inter_thres=0.3,
                                      min_points=6,
                                      dist_thres=0.12,
                                      ang_thres=np.deg2rad(10))

        # Have valid points
        if self.lines is not None:            
            # Transform line to robot frame
            for i in range(0,self.lines.shape[0]):
                point1S = np.append(self.lines[i,0:2], 0)
                point2S = np.append(self.lines[i,2:4], 0)
                point1R = comp(self.robotToSensor, point1S)
                point2R = comp(self.robotToSensor, point2S)
                self.lines[i,:] = np.append(point1R[0:2],point2R[0:2])
            
            # Publish results
            publish_lines(self.lines, self.pub_lines, frame='/robot',
                      time=msg.header.stamp, ns='scan_lines_robot', color=(0,0,1))
            
            # Flag
            self.new_laser = True
    
    #===========================================================================
    def iterate(self):
        '''
        Main loop of the filter.
        '''
        # Prediction
        if self.new_odom:
            
            self.ekf.predict(self.uk.copy())
            self.new_odom = False
            self.pub = True
            
        # Weightimg and resampling
        if self.new_laser:
            
            Innovk_List, H_k_List, S_f_List, Rk_List,idx_not_associated = self.ekf.data_association(self.lines.copy())
            self.ekf.update_position(Innovk_List, H_k_List, S_f_List, Rk_List)
            self.ekf.state_augmentation(self.lines.copy(),idx_not_associated)
            self.new_laser = False
            self.pub = True
            
        # Publish results
        if self.pub:
            self.publish_results()
            self.pub = False
    
    #===========================================================================
    def publish_results(self):
        '''
        Publishes all results from the filter.
        '''
        # Ground turth of the map of the room
        publish_lines(self.map, self.pub_lines, frame='/world', ns='gt_map', color=(0.3,0.3,0.3))
        
        msg_odom, msg_ellipse, trans, rot, room_map = get_ekf_msgs(self.ekf)
        
        publish_lines(room_map, self.pub_lines, frame='/world', ns='map', color=(0,1,0))
        

        self.pub_odom.publish(msg_odom)
        self.pub_uncertainity.publish(msg_ellipse)
        self.tfBroad.sendTransform(translation = trans,
                                   rotation = rot, 
                                   time = self.time,
                                   child = '/robot',
                                   parent = '/world')