Beispiel #1
0
 def source_odom_callback(self, msg):
     with self.lock:
         self.source_odom = msg
         self.calculate_odometry(self.odom, self.source_odom)
         self.publish_odometry()
         if self.publish_tf:
             broadcast_transform(self.broadcast, self.odom, self.invert_tf)
Beispiel #2
0
 def publish_odometry(self):
     # relfect source_odom information
     self.odom.header.stamp = self.source_odom.header.stamp
     self.odom.twist = self.source_odom.twist
     # use only important particels
     combined_prt_weight = zip(self.particles, self.weights)
     selected_prt_weight = zip(
         *sorted(combined_prt_weight, key=itemgetter(1),
                 reverse=True)[:int(self.valid_particle_num)]
     )  # [(p0, w0), (p1, w1), ..., (pN, wN)] -> [(sorted_p0, sorted_w0), (sorted_p1, sorted_w1), ..., (sorted_pN', sorted_wN')] -> [(sorted_p0, ..., sorted_pN'), (sorted_w0, ..., sorted_wN')]
     # estimate gaussian distribution for Odometry msg
     mean, cov = self.guess_normal_distribution(selected_prt_weight[0],
                                                selected_prt_weight[1])
     self.odom.pose.pose = self.convert_list_to_pose(mean)
     self.odom.pose.covariance = list(itertools.chain(*cov))
     self.pub.publish(self.odom)
     if self.publish_tf:
         broadcast_transform(self.broadcast, self.odom, self.invert_tf)
     # update prev_rpy to prevent jump of angles
     self.prev_rpy = transform_quaternion_to_euler([
         self.odom.pose.pose.orientation.x,
         self.odom.pose.pose.orientation.y,
         self.odom.pose.pose.orientation.z,
         self.odom.pose.pose.orientation.w
     ], self.prev_rpy)
     # diagnostics
     self.update_diagnostics(self.particles, self.weights,
                             self.odom.header.stamp)
 def source_odom_callback(self, msg):
     with self.lock:
         self.source_odom = msg
         self.calculate_odometry(self.odom, self.source_odom)
         self.publish_odometry()
         if self.publish_tf:
             broadcast_transform(self.broadcast, self.odom, self.invert_tf)
Beispiel #4
0
    def source_odom_callback(self, msg):
        with self.lock:
            if self.offset_matrix != None:
                source_odom_matrix = make_homogeneous_matrix([getattr(msg.pose.pose.position, attr) for attr in ["x", "y", "z"]], [getattr(msg.pose.pose.orientation, attr) for attr in ["x", "y", "z", "w"]])
                new_odom = copy.deepcopy(msg)
                new_odom.header.frame_id = self.odom_frame
                new_odom.child_frame_id = self.base_link_frame
                twist_list = [new_odom.twist.twist.linear.x, new_odom.twist.twist.linear.y, new_odom.twist.twist.linear.z, new_odom.twist.twist.angular.x, new_odom.twist.twist.angular.y, new_odom.twist.twist.angular.z]
                
                # use median filter to cancel spike noise of twist when use_twist_filter is true
                if self.use_twist_filter:                    
                    twist_list = self.median_filter(twist_list)
                    new_odom.twist.twist = Twist(Vector3(*twist_list[0:3]), Vector3(*twist_list[3: 6]))
                    
                # overwrite twist covariance when calculate_covariance flag is True
                if self.overwrite_pdf:
                    if not all([abs(x) < 1e-3 for x in twist_list]):
                        # shift twist according to error mean when moving (stopping state is trusted)
                        twist_list = [x + y for x, y in zip(twist_list, self.v_err_mean)]
                        new_odom.twist.twist = Twist(Vector3(*twist_list[0:3]), Vector3(*twist_list[3: 6]))
                    # calculate twist covariance according to standard diviation
                    if self.twist_proportional_sigma:
                        current_sigma = [x * y for x, y in zip(twist_list, self.v_err_sigma)]
                    else:
                        if all([abs(x) < 1e-3 for x in twist_list]):
                            current_sigma = [1e-6] * 6 # trust stopping state
                        else:
                            current_sigma = self.v_err_sigma
                    new_odom.twist.covariance = update_twist_covariance(new_odom.twist, current_sigma)
                    
                # offset coords
                new_odom_matrix = self.offset_matrix.dot(source_odom_matrix)
                new_odom.pose.pose.position = Point(*list(new_odom_matrix[:3, 3]))
                new_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(new_odom_matrix)))

                if self.overwrite_pdf:
                    if self.prev_odom != None:
                        dt = (new_odom.header.stamp - self.prev_odom.header.stamp).to_sec()
                        global_twist_with_covariance = TwistWithCovariance(transform_local_twist_to_global(new_odom.pose.pose, new_odom.twist.twist),
                                                                           transform_local_twist_covariance_to_global(new_odom.pose.pose, new_odom.twist.covariance))
                        new_odom.pose.covariance = update_pose_covariance(self.prev_odom.pose.covariance, global_twist_with_covariance.covariance, dt)
                    else:
                        new_odom.pose.covariance = numpy.diag([0.01**2] * 6).reshape(-1,).tolist() # initial covariance is assumed to be constant
                else:
                    # only offset pose covariance
                    new_pose_cov_matrix = numpy.matrix(new_odom.pose.covariance).reshape(6, 6)
                    rotation_matrix = self.offset_matrix[:3, :3]
                    new_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(new_pose_cov_matrix[:3, :3].dot(rotation_matrix))
                    new_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(new_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix))
                    new_odom.pose.covariance = numpy.array(new_pose_cov_matrix).reshape(-1,).tolist()

                # publish
                self.pub.publish(new_odom)
                if self.publish_tf:
                    broadcast_transform(self.broadcast, new_odom, self.invert_tf)
                self.prev_odom = new_odom
                
            else:
                self.offset_matrix = self.calculate_offset(msg)
Beispiel #5
0
 def publish_odometry(self):
     # refrect source_odom informations
     self.pub.publish(self.odom)
     if self.publish_tf:
         broadcast_transform(self.broadcast, self.odom, self.invert_tf)
     # diagnostics
     self.update_diagnostics(self.particles, self.weights, self.odom.header.stamp)
     # update prev_rpy to prevent jump of angles
     self.prev_rpy = transform_quaternion_to_euler([self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w], self.prev_rpy)
Beispiel #6
0
 def publish_odometry(self):
     # relfect source_odom information
     self.odom.header.stamp = self.source_odom.header.stamp
     self.odom.twist = self.source_odom.twist
     # estimate gaussian distribution for Odometry msg 
     mean, cov = self.guess_normal_distribution(self.particles, self.weights)
     self.odom.pose.pose = self.convert_list_to_pose(mean)
     self.odom.pose.covariance = list(itertools.chain(*cov))
     self.pub.publish(self.odom)
     if self.publish_tf:
         broadcast_transform(self.broadcast, self.odom, self.invert_tf)
     # update prev_rpy to prevent jump of angles
     self.prev_rpy = transform_quaternion_to_euler([self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w], self.prev_rpy)
 def publish_odometry(self):
     # relfect source_odom information
     self.odom.header.stamp = self.source_odom.header.stamp
     self.odom.twist = self.source_odom.twist
     # use only important particels
     combined_prt_weight = zip(self.particles, self.weights)
     selected_prt_weight = zip(*sorted(combined_prt_weight, key = itemgetter(1), reverse = True)[:int(self.valid_particle_num)]) # [(p0, w0), (p1, w1), ..., (pN, wN)] -> [(sorted_p0, sorted_w0), (sorted_p1, sorted_w1), ..., (sorted_pN', sorted_wN')] -> [(sorted_p0, ..., sorted_pN'), (sorted_w0, ..., sorted_wN')]
     # estimate gaussian distribution for Odometry msg 
     mean, cov = self.guess_normal_distribution(selected_prt_weight[0], selected_prt_weight[1])
     self.odom.pose.pose = self.convert_list_to_pose(mean)
     self.odom.pose.covariance = list(itertools.chain(*cov))
     self.pub.publish(self.odom)
     if self.publish_tf:
         broadcast_transform(self.broadcast, self.odom, self.invert_tf)
     # update prev_rpy to prevent jump of angles
     self.prev_rpy = transform_quaternion_to_euler([self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w], self.prev_rpy)
Beispiel #8
0
 def publish_odometry(self):
     # relfect source_odom information
     self.odom.header.stamp = self.source_odom.header.stamp
     self.odom.twist = self.source_odom.twist
     # estimate gaussian distribution for Odometry msg
     mean, cov = self.guess_normal_distribution(self.particles,
                                                self.weights)
     self.odom.pose.pose = self.convert_list_to_pose(mean)
     self.odom.pose.covariance = list(itertools.chain(*cov))
     self.pub.publish(self.odom)
     if self.publish_tf:
         broadcast_transform(self.broadcast, self.odom, self.invert_tf)
     # update prev_rpy to prevent jump of angles
     self.prev_rpy = transform_quaternion_to_euler([
         self.odom.pose.pose.orientation.x,
         self.odom.pose.pose.orientation.y,
         self.odom.pose.pose.orientation.z,
         self.odom.pose.pose.orientation.w
     ], self.prev_rpy)
Beispiel #9
0
    def source_odom_callback(self, msg):
        with self.lock:
            if self.offset_matrix is not None:
                source_odom_matrix = make_homogeneous_matrix([
                    getattr(msg.pose.pose.position, attr)
                    for attr in ["x", "y", "z"]
                ], [
                    getattr(msg.pose.pose.orientation, attr)
                    for attr in ["x", "y", "z", "w"]
                ])
                new_odom = copy.deepcopy(msg)
                new_odom.header.frame_id = self.odom_frame
                new_odom.child_frame_id = self.base_link_frame
                twist_list = [
                    new_odom.twist.twist.linear.x,
                    new_odom.twist.twist.linear.y,
                    new_odom.twist.twist.linear.z,
                    new_odom.twist.twist.angular.x,
                    new_odom.twist.twist.angular.y,
                    new_odom.twist.twist.angular.z
                ]

                # use median filter to cancel spike noise of twist when use_twist_filter is true
                if self.use_twist_filter:
                    twist_list = self.median_filter(twist_list)
                    new_odom.twist.twist = Twist(Vector3(*twist_list[0:3]),
                                                 Vector3(*twist_list[3:6]))

                # overwrite twist covariance when calculate_covariance flag is True
                if self.overwrite_pdf:
                    if not all([abs(x) < 1e-3 for x in twist_list]):
                        # shift twist according to error mean when moving (stopping state is trusted)
                        twist_list = [
                            x + y for x, y in zip(twist_list, self.v_err_mean)
                        ]
                        new_odom.twist.twist = Twist(Vector3(*twist_list[0:3]),
                                                     Vector3(*twist_list[3:6]))
                    # calculate twist covariance according to standard diviation
                    if self.twist_proportional_sigma:
                        current_sigma = [
                            x * y for x, y in zip(twist_list, self.v_err_sigma)
                        ]
                    else:
                        if all([abs(x) < 1e-3 for x in twist_list]):
                            current_sigma = [1e-6] * 6  # trust stopping state
                        else:
                            current_sigma = self.v_err_sigma
                    new_odom.twist.covariance = update_twist_covariance(
                        new_odom.twist, current_sigma)

                # offset coords
                new_odom_matrix = self.offset_matrix.dot(source_odom_matrix)
                new_odom.pose.pose.position = Point(*list(new_odom_matrix[:3,
                                                                          3]))
                new_odom.pose.pose.orientation = Quaternion(*list(
                    tf.transformations.quaternion_from_matrix(
                        new_odom_matrix)))

                if self.overwrite_pdf:
                    if self.prev_odom is not None:
                        dt = (new_odom.header.stamp -
                              self.prev_odom.header.stamp).to_sec()
                        global_twist_with_covariance = TwistWithCovariance(
                            transform_local_twist_to_global(
                                new_odom.pose.pose, new_odom.twist.twist),
                            transform_local_twist_covariance_to_global(
                                new_odom.pose.pose, new_odom.twist.covariance))
                        new_odom.pose.covariance = update_pose_covariance(
                            self.prev_odom.pose.covariance,
                            global_twist_with_covariance.covariance, dt)
                    else:
                        new_odom.pose.covariance = numpy.diag(
                            [0.01**2] * 6).reshape(-1, ).tolist(
                            )  # initial covariance is assumed to be constant
                else:
                    # only offset pose covariance
                    new_pose_cov_matrix = numpy.matrix(
                        new_odom.pose.covariance).reshape(6, 6)
                    rotation_matrix = self.offset_matrix[:3, :3]
                    new_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(
                        new_pose_cov_matrix[:3, :3].dot(rotation_matrix))
                    new_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(
                        new_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix))
                    new_odom.pose.covariance = numpy.array(
                        new_pose_cov_matrix).reshape(-1, ).tolist()

                # publish
                self.pub.publish(new_odom)
                if self.publish_tf:
                    broadcast_transform(self.broadcast, new_odom,
                                        self.invert_tf)
                self.prev_odom = new_odom

            else:
                self.offset_matrix = self.calculate_offset(msg)