From 31a2b1e19e4b3da9d32b2f51b23bc45dd7ddd8fc Mon Sep 17 00:00:00 2001 From: iori Date: Mon, 14 Dec 2015 17:08:22 +0900 Subject: [PATCH 01/15] [jsk_robot_startup] Calculate imu rotation when imu coordinate is not same as global --- .../src/jsk_robot_startup/ParticleOdometry.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py index 79265d570a..cd25e89c38 100644 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py @@ -79,6 +79,7 @@ def __init__(self): self.source_odom = None self.measure_odom = None self.imu = None + self.imu_rotation = None self.particles = None self.weights = [] self.measurement_updated = False @@ -127,6 +128,7 @@ def initialize_odometry(self): self.measure_odom = None self.measurement_updated = False self.imu = None + self.imu_rotation = None ## particle filter functions # input: particles(list of pose), source_odom(control input) output: list of sampled particles(pose) @@ -207,7 +209,7 @@ def imu_error_pdf(self, prt): rospy.logwarn("[%s]: use_imu is True but imu is not subscribed", rospy.get_name()) return 1.0 # multiply 1.0 make no effects to weight prt_euler = self.convert_pose_to_list(prt)[3:6] - imu_euler = transform_quaternion_to_euler([self.imu.orientation.x, self.imu.orientation.y, self.imu.orientation.z, self.imu.orientation.w]) # imu.orientation is assumed to be global + imu_euler = self.imu_rotation.dot(numpy.array(transform_quaternion_to_euler([self.imu.orientation.x, self.imu.orientation.y, self.imu.orientation.z, self.imu.orientation.w]))) roll_pitch_pdf = scipy.stats.norm.pdf(prt_euler[0] - imu_euler[0], loc = 0.0, scale = self.roll_error_sigma) * scipy.stats.norm.pdf(prt_euler[1] - imu_euler[1], loc = 0.0, scale = self.pitch_error_sigma) if self.use_imu_yaw: return roll_pitch_pdf * scipy.stats.norm.pdf(prt_euler[2] - imu_euler[2], loc = 0.0, scale = self.yaw_error_sigma) @@ -259,6 +261,15 @@ def init_signal_callback(self, msg): def imu_callback(self, msg): with self.lock: + try: + (trans,rot) = self.listener.lookupTransform(self.base_link_frame, msg.header.frame_id, msg.header.stamp) + except: + try: + (trans,rot) = self.listener.lookupTransform(self.base_link_frame, msg.header.frame_id, rospy.Time(0)) # retry to get newest tf data + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): + rospy.logwarn("[%s] failed to solve tf in calculate imu_transorm: %s to %s", rospy.get_name(), self.base_link_frame, msg.header.frame_id) + return # imu is not updated when imu_rotation cannot be calculated + self.imu_rotation = tf.transformations.quaternion_matrix(rot)[:3, :3] # trans does not affects to orientation self.imu = msg # main functions From 0cffca04256ae49e781beb0ea568f05aa86b0959 Mon Sep 17 00:00:00 2001 From: JSK User Date: Mon, 14 Dec 2015 19:54:41 +0900 Subject: [PATCH 02/15] [jsk_robot_startup] Fix calculation for imu rotation and modify base coordinate from base_link to odom --- .../src/jsk_robot_startup/ParticleOdometry.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py index cd25e89c38..d99edb9fbd 100644 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py @@ -209,7 +209,8 @@ def imu_error_pdf(self, prt): rospy.logwarn("[%s]: use_imu is True but imu is not subscribed", rospy.get_name()) return 1.0 # multiply 1.0 make no effects to weight prt_euler = self.convert_pose_to_list(prt)[3:6] - imu_euler = self.imu_rotation.dot(numpy.array(transform_quaternion_to_euler([self.imu.orientation.x, self.imu.orientation.y, self.imu.orientation.z, self.imu.orientation.w]))) + imu_matrix = tf.transformations.quaternion_matrix([self.imu.orientation.x, self.imu.orientation.y, self.imu.orientation.z, self.imu.orientation.w])[:3, :3] + imu_euler = tf.transformations.euler_from_matrix(numpy.dot(self.imu_rotation, imu_matrix)) roll_pitch_pdf = scipy.stats.norm.pdf(prt_euler[0] - imu_euler[0], loc = 0.0, scale = self.roll_error_sigma) * scipy.stats.norm.pdf(prt_euler[1] - imu_euler[1], loc = 0.0, scale = self.pitch_error_sigma) if self.use_imu_yaw: return roll_pitch_pdf * scipy.stats.norm.pdf(prt_euler[2] - imu_euler[2], loc = 0.0, scale = self.yaw_error_sigma) @@ -262,10 +263,10 @@ def init_signal_callback(self, msg): def imu_callback(self, msg): with self.lock: try: - (trans,rot) = self.listener.lookupTransform(self.base_link_frame, msg.header.frame_id, msg.header.stamp) + (trans,rot) = self.listener.lookupTransform(msg.header.frame_id, self.odom_frame, msg.header.stamp) except: try: - (trans,rot) = self.listener.lookupTransform(self.base_link_frame, msg.header.frame_id, rospy.Time(0)) # retry to get newest tf data + (trans,rot) = self.listener.lookupTransform(msg.header.frame_id, self.odom_frame, rospy.Time(0)) # retry to get newest tf data except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn("[%s] failed to solve tf in calculate imu_transorm: %s to %s", rospy.get_name(), self.base_link_frame, msg.header.frame_id) return # imu is not updated when imu_rotation cannot be calculated From fb53f8a33d722bfd759c6c0ae72a31cc7dcd8462 Mon Sep 17 00:00:00 2001 From: JSK User Date: Mon, 14 Dec 2015 20:03:45 +0900 Subject: [PATCH 03/15] [jsk_robot_startup] Fix calculation for initial offset of viso camera offset to reduce linalg.inv --- .../scripts/CameraToBaseOffset.py | 50 ++++++++++--------- 1 file changed, 26 insertions(+), 24 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py index c85e827510..09f03a4bcc 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -44,34 +44,36 @@ def source_odom_callback(self, msg): with self.lock: # calculate camera transform current_camera_to_base = self.calculate_camera_to_base_transform(msg.header.stamp) + if current_camera_to_base == None: + return + if self.initial_matrix == None: - self.initial_matrix = current_camera_to_base + self.initial_matrix = numpy.linalg.inv(current_camera_to_base) - if self.initial_matrix != None: - camera_relative_base_transformation = numpy.dot(numpy.linalg.inv(self.initial_matrix), current_camera_to_base) # base_link transformation in camera coords - - # calculate offseted odometry - source_odom_matrix = self.make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z], - [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, - msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) - new_odom_matrix = camera_relative_base_transformation.dot(source_odom_matrix) + camera_relative_base_transformation = numpy.dot(self.initial_matrix, current_camera_to_base) # base_link transformation in camera coords + + # calculate offseted odometry + source_odom_matrix = self.make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z], + [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) + new_odom_matrix = camera_relative_base_transformation.dot(source_odom_matrix) - # make odometry msg. twist is copied from source_odom - new_odom = copy.deepcopy(msg) - new_odom.header.frame_id = self.odom_frame - new_odom.child_frame_id = self.base_link_frame - 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))) - new_pose_cov_matrix = numpy.matrix(new_odom.pose.covariance).reshape(6, 6) - rotation_matrix = camera_relative_base_transformation[: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() + # make odometry msg. twist is copied from source_odom + new_odom = copy.deepcopy(msg) + new_odom.header.frame_id = self.odom_frame + new_odom.child_frame_id = self.base_link_frame + 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))) + new_pose_cov_matrix = numpy.matrix(new_odom.pose.covariance).reshape(6, 6) + rotation_matrix = camera_relative_base_transformation[: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: - self.broadcast_transform(new_odom) + # publish + self.pub.publish(new_odom) + if self.publish_tf: + self.broadcast_transform(new_odom) def make_homogeneous_matrix(self, trans, rot): homogeneous_matrix = tf.transformations.quaternion_matrix(rot) From 814f2770b813a1e8ea0f93fe47a91c94e9cb4b00 Mon Sep 17 00:00:00 2001 From: JSK User Date: Mon, 14 Dec 2015 20:04:43 +0900 Subject: [PATCH 04/15] [jsk_robot_startup] Remove use_imu option from launch files and describe in config file --- .../jsk_robot_startup/config/HRP2JSKNT_odometry_params.yaml | 1 + .../jsk_robot_startup/launch/biped_localization.launch | 2 -- .../jsk_robot_startup/launch/particle_odometry.launch | 2 -- 3 files changed, 1 insertion(+), 4 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/config/HRP2JSKNT_odometry_params.yaml b/jsk_robot_common/jsk_robot_startup/config/HRP2JSKNT_odometry_params.yaml index d2b7b70319..bb1d639880 100644 --- a/jsk_robot_common/jsk_robot_startup/config/HRP2JSKNT_odometry_params.yaml +++ b/jsk_robot_common/jsk_robot_startup/config/HRP2JSKNT_odometry_params.yaml @@ -14,6 +14,7 @@ viso_odom_calculator: biped_particle_odometry: particle_num: 80 rate: 50 + use_imu: true biped_odometry_offset: use_twist_filter: true diff --git a/jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch b/jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch index d49735db44..0292a9d275 100644 --- a/jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch +++ b/jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch @@ -1,7 +1,6 @@ - @@ -34,7 +33,6 @@ - diff --git a/jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch b/jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch index dea6df122d..d22c907cd8 100644 --- a/jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch +++ b/jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch @@ -1,5 +1,4 @@ - @@ -59,7 +58,6 @@ odom_frame: biped_odom_particle base_link_frame: BODY publish_tf: false - use_imu: $(arg use_imu) From 17599061bfe947d5da069d02757ed015fd00dd19 Mon Sep 17 00:00:00 2001 From: iori Date: Sat, 19 Dec 2015 17:50:07 +0900 Subject: [PATCH 05/15] [jsk_robot_startup] Separate commonly used utilities for odometry calculation --- .../src/jsk_robot_startup/odometry_utils.py | 110 ++++++++++++++++++ 1 file changed, 110 insertions(+) create mode 100644 jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py new file mode 100644 index 0000000000..c0e3b89790 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py @@ -0,0 +1,110 @@ +#! /usr/bin/env python + +import tf +import numpy +from geometry_msgs.msg import PoseWithCovariance, TwistWithCovariance, Twist, Pose, Point, Quaternion, Vector3 + +# twist transformation +def transform_local_twist_to_global(pose, local_twist): + trans = [pose.position.x, pose.position.y, pose.position.z] + rot = [pose.orientation.x, pose.orientation.y, + pose.orientation.z, pose.orientation.w] + rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3] + global_velocity = numpy.dot(rotation_matrix, numpy.array([[local_twist.linear.x], + [local_twist.linear.y], + [local_twist.linear.z]])) + global_omega = numpy.dot(rotation_matrix, numpy.array([[local_twist.twist.angular.x], + [local_twist.twist.angular.y], + [local_twist.twist.angular.z]])) + return Twist(Vector3(*global_velocity[:, 0]), Vector3(*global_omega[:, 0])) + +def transform_local_twist_covariance_to_global(pose, local_twist_with_covariance): + trans = [pose.position.x, pose.position.y, pose.position.z] + rot = [pose.orientation.x, pose.orientation.y, + pose.orientation.z, pose.orientation.w] + rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3] + twist_cov_matrix = numpy.matrix(local_twist_with_covariance.covariance).reshape(6, 6) + global_twist_cov_matrix = numpy.zeros((6, 6)) + global_twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(twist_cov_matrix[:3, :3].dot(rotation_matrix)) + global_twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(twist_cov_matrix[3:6, 3:6].dot(rotation_matrix)) + return global_twist_cov_matrix.reshape(-1,).tolist() + +# pose calculation +def update_pose(pose, global_twist, dt): + ret_pose = Pose() + # calculate current pose as integration + ret_pose.position.x = pose.position.x + global_twist.linear.x * dt + ret_pose.position.y = pose.position.y + global_twist.linear.y * dt + ret_pose.position.z = pose.position.z + global_twist.linear.z * dt + ret_pose.orientation = update_quaternion(pose.orientation, global_twist.angular, dt) + return ret_pose + +def update_quaternion(orientation, angular, dt): # angular is assumed to be global + # quaternion calculation + quat_vec = numpy.array([[orientation.x], + [orientation.y], + [orientation.z], + [orientation.w]]) + skew_omega = numpy.matrix([[0, -angular.z, angular.y, angular.x], + [angular.z, 0, -angular.x, angular.y], + [-angular.y, angular.x, 0, angular.z], + [-angular.x, -angular.y, -angular.z, 0]]) + new_quat_vec = quat_vec + 0.5 * numpy.dot(skew_omega, quat_vec) * dt + norm = numpy.linalg.norm(new_quat_vec) + if norm == 0: + rospy.logwarn("norm of quaternion is zero") + else: + new_quat_vec = new_quat_vec / norm # normalize + return Quaternion(*numpy.array(new_quat_vec).reshape(-1,).tolist()) + +# covariance calculation +def update_twist_covariance(twist, v_sigma, twist_proportional_sigma): # twist is assumed to be local + twist_list = [twist.twist.linear.x, twist.twist.linear.y, twist.twist.linear.z, twist.twist.angular.x, twist.twist.angular.y, twist.twist.angular.z] + if twist_proportional_sigma == True: + current_sigma = [x * y for x, y in zip(twist_list, v_sigma)] + else: + if all([abs(x) < 1e-6 for x in twist_list]): + current_sigma = [1e-6] * 6 # trust "completely stopping" state + else: + current_sigma = v_sigma + return numpy.diag([max(x**2, 0.001*0.001) for x in current_sigma]).reshape(-1,).tolist() # covariance should be singular + +def update_pose_covariance(pose_cov, global_twist_cov, dt): + ret_pose_cov = [] + # make matirx from covariance array + prev_pose_cov_matrix = numpy.matrix(pose_cov).reshape(6, 6) + global_twist_cov_matrix = numpy.matrix(global_twist_cov).reshape(6, 6) + # jacobian matrix + # elements in pose and twist are assumed to be independent on global coordinates + jacobi_pose = numpy.diag([1.0] * 6) + jacobi_twist = numpy.diag([dt] * 6) + # covariance calculation + pose_cov_matrix = jacobi_pose.dot(prev_pose_cov_matrix.dot(jacobi_pose.T)) + jacobi_twist.dot(global_twist_cov_matrix.dot(jacobi_twist.T)) + # update covariances as array type (twist is same as before) + ret_pose_cov = numpy.array(pose_cov_matrix).reshape(-1,).tolist() + return ret_pose_cov + +# tf broadcast +def broadcast_transform(broadcast, odom, invert_tf): + if not self.odom: + return + position = [odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z] + orientation = [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w] + if invert_tf: + homogeneous_matrix = make_homogeneous_matrix(position, orientation) + homogeneous_matrix_inv = numpy.linalg.inv(homogeneous_matrix) + position = list(homogeneous_matrix_inv[:3, 3]) + orientation = list(tf.transformations.quaternion_from_matrix(homogeneous_matrix_inv)) + parent_frame = odom.child_frame_id + target_frame = odom.header.frame_id + else: + parent_frame = odom.header.frame_id + target_frame = odom.child_frame_id + broadcast.sendTransform(position, orientation, rospy.Time.now(), target_frame, parent_frame) + +# mathmatical tools +def make_homogeneous_matrix(trans, rot): + homogeneous_matrix = tf.transformations.quaternion_matrix(rot) + homogeneous_matrix[:3, 3] = numpy.array(trans).reshape(1, 3) + return homogeneous_matrix + From f52b8174f7f874b86a7282023c849adc9fd9f3ce Mon Sep 17 00:00:00 2001 From: iori Date: Sat, 19 Dec 2015 17:50:38 +0900 Subject: [PATCH 06/15] [jsk_robot_startup] Remove lookup transforms using odometry topic information --- .../OdometryFeedbackWrapper.py | 161 +++--------------- 1 file changed, 25 insertions(+), 136 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py index 386112d030..3dff3fdbee 100755 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py @@ -16,6 +16,8 @@ from dynamic_reconfigure.server import Server from jsk_robot_startup.cfg import OdometryFeedbackWrapperReconfigureConfig +from odometry_utils import transform_twist_to_global, transform_twist_covariance_to_global, update_pose, update_twist_covariance, update_pose_covariance, broadcast_transform, make_homogeneous_matrix + class OdometryFeedbackWrapper(object): def __init__(self): rospy.init_node("OdometryFeedbackWrapper", anonymous=True) @@ -86,7 +88,7 @@ def source_odom_callback(self, msg): self.calculate_odometry(self.odom, self.source_odom) self.publish_odometry() if self.publish_tf: - self.broadcast_transform() + broadcast_transform(self.broadcast, self.odom, self.invert_tf) def feedback_odom_callback(self, msg): if not self.odom: @@ -102,9 +104,10 @@ def feedback_odom_callback(self, msg): nearest_dt = dt nearest_odom = copy.deepcopy(hist) # get approximate pose at feedback_odom timestamp (probably it is past) of nearest_odom - self.update_pose(nearest_odom.pose, nearest_odom.twist, - nearest_odom.header.frame_id, nearest_odom.child_frame_id, - nearest_odom.header.stamp, nearest_dt) + global_nearest_odom_twist = transform_local_twist_to_global(nearest_odom.pose.pose, nearest_odom.twist.twist) + nearest_odom.pose.pose = update_pose(nearest_odom.pose.pose, global_nearest_odom_twist, nearest_dt) + global_nearest_odom_twist_covariance = transform_local_twist_covariance_to_global(nearest_odom.pose.pose, nearest_odom.twist.covariance) + nearest_odom.pose.covariance = update_pose_covariance(nearest_odom.pose.covariance, global_nearest_odom_twist_covariance, nearest_dt) enable_feedback = self.check_covariance(nearest_odom) or self.check_distribution_difference(nearest_odom, self.feedback_odom) or self.check_feedback_time() # update feedback_odom to approximate odom at current timestamp using previsous velocities if enable_feedback: @@ -115,20 +118,18 @@ def feedback_odom_callback(self, msg): if dt > 0.0: # update pose and twist according to the history self.update_twist(self.feedback_odom.twist, hist.twist) - self.update_pose(self.feedback_odom.pose, - hist.twist, self.feedback_odom.header.frame_id, hist.child_frame_id, - hist.header.stamp, dt) # update feedback_odom according to twist of hist + global_hist_twist = transform_local_twist_to_global(hist.pose.pose, hist.twist.twist) + self.feedback_odom.pose.pose = update_pose(self.feedback_odom.pose, global_hist_twist, dt) # update feedback_odom according to twist of hist # update covariance - # this wrapper do not upgrade twist.covariance to trust feedback_odom.covariance - self.update_twist_covariance(self.feedback_odom.twist) - self.update_pose_covariance(self.feedback_odom.pose, self.feedback_odom.twist, - self.feedback_odom.header.frame_id, hist.child_frame_id, - hist.header.stamp, dt) + self.feedback_odom.twist.covariance = hist.twist.covariance + global_hist_twist_covariance = transform_local_twist_covariance_to_global(self.feedback_odom.pose.pose, hist.twist.covariance) + self.feedback_odom.pose.covariance = update_pose_covariance(self.feedback_odom.pose.covariance, global_hist_twist_covariance, dt) self.feedback_odom.header.stamp = hist.header.stamp dt = (self.odom.header.stamp - self.feedback_odom.header.stamp).to_sec() - self.update_pose(self.feedback_odom.pose, self.feedback_odom.twist, - self.feedback_odom.header.frame_id, self.feedback_odom.child_frame_id, - self.feedback_odom.header.stamp, dt) + global_feedback_odom_twist = transform_local_twist_to_global(self.feedback_odom.pose.pose, self.feedback_odom.twist.twist) + self.feedback_odom.pose.pose = update_pose(self.feedback_odom.pose.pose, global_feedback_odom_twist, dt) + global_feedback_odom_twist_covariance = transform_local_twist_covariance_to_global(self.feedback_odom.pose.pose, self.feedback_odom.twist.covariance) + self.feedback_odom.pose.covariance = update_pose_covariance(self.feedback_odom.pose.covariance, global_feedback_odom_twist_covariance, dt) self.feedback_odom.header.stamp = self.odom.header.stamp # integrate feedback_odom and current odom new_odom_pose, new_odom_cov = self.calculate_mean_and_covariance(self.odom.pose, self.feedback_odom.pose) @@ -139,10 +140,10 @@ def feedback_odom_callback(self, msg): self.prev_feedback_time = self.odom.header.stamp self.odom_history = [] # update offset - new_pose_homogeneous_matrix = self.make_homogeneous_matrix([self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z], - [self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w]) - source_homogeneous_matrix = self.make_homogeneous_matrix([self.source_odom.pose.pose.position.x, self.source_odom.pose.pose.position.y, self.source_odom.pose.pose.position.z], - [self.source_odom.pose.pose.orientation.x, self.source_odom.pose.pose.orientation.y, self.source_odom.pose.pose.orientation.z, self.source_odom.pose.pose.orientation.w]) + new_pose_homogeneous_matrix = make_homogeneous_matrix([self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z], + [self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w]) + source_homogeneous_matrix = make_homogeneous_matrix([self.source_odom.pose.pose.position.x, self.source_odom.pose.pose.position.y, self.source_odom.pose.pose.position.z], + [self.source_odom.pose.pose.orientation.x, self.source_odom.pose.pose.orientation.y, self.source_odom.pose.pose.orientation.z, self.source_odom.pose.pose.orientation.w]) # Hnew = Hold * T -> T = Hold^-1 * Hnew self.offset_homogeneous_matrix = numpy.dot(numpy.linalg.inv(source_homogeneous_matrix), new_pose_homogeneous_matrix) # self.odom.header.stamp is assumed to be same as self.source_odom.header.stamp @@ -215,138 +216,26 @@ def calculate_odometry(self, odom, source_odom): result_odom.header.frame_id = self.odom_frame result_odom.child_frame_id = self.base_link_frame - # overwrite twist covariance (twist is copied from source_odom) - self.update_twist_covariance(result_odom.twist) - - # consider only pose because twist is local + # consider only pose because twist is local and copied from source_odom position = [source_odom.pose.pose.position.x, source_odom.pose.pose.position.y, source_odom.pose.pose.position.z] orientation = [source_odom.pose.pose.orientation.x, source_odom.pose.pose.orientation.y, source_odom.pose.pose.orientation.z, source_odom.pose.pose.orientation.w] # calculate pose (use odometry source) - source_homogeneous_matrix = self.make_homogeneous_matrix(position, orientation) + source_homogeneous_matrix = make_homogeneous_matrix(position, orientation) result_homogeneous_matrix = numpy.dot(source_homogeneous_matrix, self.offset_homogeneous_matrix) result_odom.pose.pose.position = Point(*list(result_homogeneous_matrix[:3, 3])) result_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(result_homogeneous_matrix))) - # calculate covariance (do not use odometry source) + # calculate pose covariance (do not use odometry source) if self.odom != None: result_odom.pose.covariance = self.odom.pose.covariance # do not use source_odom covariance in pose dt = (self.source_odom.header.stamp - self.odom.header.stamp).to_sec() else: # initial covariance of pose is defined as same value of source_odom dt = 0.0 - self.update_pose_covariance(result_odom.pose, result_odom.twist, result_odom.header.frame_id, result_odom.child_frame_id, result_odom.header.stamp, dt) + global_twist_covariance = transform_local_twist_covariance_to_global(result_odom.pose.pose, result_odom.twist.covariance) + result_odom.pose.pose.covariance = update_pose_covariance(result_odom.pose.covariance, global_twist_covariance, dt) self.odom = result_odom def update_twist(self, twist, new_twist): twist.twist = new_twist.twist - - def convert_local_twist_to_global_twist(self, local_twist, pose_frame, twist_frame, stamp): - try: - (trans,rot) = self.listener.lookupTransform(pose_frame, twist_frame, stamp) - except: - try: - # rospy.loginfo("timestamp %f of tf (%s to %s) is not correct. use rospy.Time(0).", stamp.to_sec(), pose_frame, twist_frame) - (trans,rot) = self.listener.lookupTransform(pose_frame, twist_frame, rospy.Time(0)) - except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): - rospy.logwarn("failed to solve tf: %s to %s", pose_frame, twist_frame) - return None - rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3] - global_velocity = numpy.dot(rotation_matrix, numpy.array([[local_twist.twist.linear.x], - [local_twist.twist.linear.y], - [local_twist.twist.linear.z]])) - global_omega = numpy.dot(rotation_matrix, numpy.array([[local_twist.twist.angular.x], - [local_twist.twist.angular.y], - [local_twist.twist.angular.z]])) - return Twist(Vector3(*global_velocity[:3, 0]), Vector3(*global_omega[:3, 0])) - - def update_pose(self, pose, twist, pose_frame, twist_frame, stamp, dt): - global_twist = self.convert_local_twist_to_global_twist(twist, pose_frame, twist_frame, stamp) - if global_twist == None: - rospy.logwarn("Cannot calculate global twist. return.") - return - # calculate trapezoidal integration - pose.pose.position.x += global_twist.linear.x * dt - pose.pose.position.y += global_twist.linear.y * dt - pose.pose.position.z += global_twist.linear.z * dt - pose.pose.orientation = self.calculate_quaternion_transform(pose.pose.orientation, twist.twist.angular, dt) - - def calculate_quaternion_transform(self, orientation, angular, dt): # angular is assumed to be global - # quaternion calculation - quat_vec = numpy.array([[orientation.x], - [orientation.y], - [orientation.z], - [orientation.w]]) - # skew_omega = numpy.matrix([[0, angular.z, -angular.y, angular.x], - # [-angular.z, 0, angular.x, angular.y], - # [angular.y, -angular.x, 0, angular.z], - # [-angular.x, -angular.y, -angular.z, 0]]) - skew_omega = numpy.matrix([[0, -angular.z, angular.y, angular.x], - [angular.z, 0, -angular.x, angular.y], - [-angular.y, angular.x, 0, angular.z], - [-angular.x, -angular.y, -angular.z, 0]]) - new_quat_vec = quat_vec + 0.5 * numpy.dot(skew_omega, quat_vec) * dt - norm = numpy.linalg.norm(new_quat_vec) - if norm == 0: - rospy.logwarn("norm of quaternion is zero") - else: - new_quat_vec = new_quat_vec / norm # normalize - return Quaternion(*numpy.array(new_quat_vec).reshape(-1,).tolist()) - - def update_twist_covariance(self, twist): - twist_list = [twist.twist.linear.x, twist.twist.linear.y, twist.twist.linear.z, twist.twist.angular.x, twist.twist.angular.y, twist.twist.angular.z] - if self.twist_proportional_sigma == True: - current_sigma = [x * y for x, y in zip(twist_list, self.v_sigma)] - else: - current_sigma = self.v_sigma - twist.covariance = numpy.diag([max(x**2, 0.001*0.001) for x in current_sigma]).reshape(-1,).tolist() # covariance should be singular - - def update_pose_covariance(self, pose, twist, pose_frame, twist_frame, stamp, dt): - # make matirx from covariance array - prev_pose_cov_matrix = numpy.matrix(pose.covariance).reshape(6, 6) - twist_cov_matrix = numpy.matrix(twist.covariance).reshape(6, 6) - # twist is described in child_frame_id coordinates - try: - (trans,rot) = self.listener.lookupTransform(pose_frame, twist_frame, stamp) - except: - try: - # rospy.logwarn("timestamp %f of tf (%s to %s) is not correct. use rospy.Time(0).", stamp.to_sec(), pose_frame, twist_frame) - (trans,rot) = self.listener.lookupTransform(pose_frame, twist_frame, rospy.Time(0)) # todo: lookup odom.header.stamp - except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): - rospy.logwarn("failed to solve tf: %s to %s", pose_frame, twist_frame) - return - rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3] - global_twist_cov_matrix = numpy.zeros((6, 6)) - global_twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(twist_cov_matrix[:3, :3].dot(rotation_matrix)) - global_twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(twist_cov_matrix[3:6, 3:6].dot(rotation_matrix)) - # jacobian matrix - # elements in pose and twist are assumed to be independent on global coordinates - jacobi_pose = numpy.diag([1.0] * 6) - jacobi_twist = numpy.diag([dt] * 6) - # covariance calculation - pose_cov_matrix = jacobi_pose.dot(prev_pose_cov_matrix.dot(jacobi_pose.T)) + jacobi_twist.dot(global_twist_cov_matrix.dot(jacobi_twist.T)) - # update covariances as array type (twist is same as before) - pose.covariance = numpy.array(pose_cov_matrix).reshape(-1,).tolist() - - def broadcast_transform(self): - if not self.odom: - return - position = [self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z] - orientation = [self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w] - if self.invert_tf: - homogeneous_matrix = self.make_homogeneous_matrix(position, orientation) - homogeneous_matrix_inv = numpy.linalg.inv(homogeneous_matrix) - position = list(homogeneous_matrix_inv[:3, 3]) - orientation = list(tf.transformations.quaternion_from_matrix(homogeneous_matrix_inv)) - parent_frame = self.odom.child_frame_id - target_frame = self.odom.header.frame_id - else: - parent_frame = self.odom.header.frame_id - target_frame = self.odom.child_frame_id - self.broadcast.sendTransform(position, orientation, self.odom.header.stamp, target_frame, parent_frame) - - def make_homogeneous_matrix(self, trans, rot): - homogeneous_matrix = tf.transformations.quaternion_matrix(rot) - homogeneous_matrix[:3, 3] = numpy.array(trans).reshape(1, 3) - return homogeneous_matrix - From ff02efcf9b61e9f94f9d6951beae49df96566a8a Mon Sep 17 00:00:00 2001 From: iori Date: Sat, 19 Dec 2015 19:29:36 +0900 Subject: [PATCH 07/15] [jsk_robot_startup] Fix bags related to feedback wrapper and odoemtry utils --- .../jsk_robot_startup/OdometryFeedbackWrapper.py | 6 +++--- .../src/jsk_robot_startup/odometry_utils.py | 13 +++++++------ 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py index 3dff3fdbee..a2e77ca492 100755 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py @@ -16,7 +16,7 @@ from dynamic_reconfigure.server import Server from jsk_robot_startup.cfg import OdometryFeedbackWrapperReconfigureConfig -from odometry_utils import transform_twist_to_global, transform_twist_covariance_to_global, update_pose, update_twist_covariance, update_pose_covariance, broadcast_transform, make_homogeneous_matrix +from odometry_utils import transform_local_twist_to_global, transform_local_twist_covariance_to_global, update_pose, update_twist_covariance, update_pose_covariance, broadcast_transform, make_homogeneous_matrix class OdometryFeedbackWrapper(object): def __init__(self): @@ -119,7 +119,7 @@ def feedback_odom_callback(self, msg): # update pose and twist according to the history self.update_twist(self.feedback_odom.twist, hist.twist) global_hist_twist = transform_local_twist_to_global(hist.pose.pose, hist.twist.twist) - self.feedback_odom.pose.pose = update_pose(self.feedback_odom.pose, global_hist_twist, dt) # update feedback_odom according to twist of hist + self.feedback_odom.pose.pose = update_pose(self.feedback_odom.pose.pose, global_hist_twist, dt) # update feedback_odom according to twist of hist # update covariance self.feedback_odom.twist.covariance = hist.twist.covariance global_hist_twist_covariance = transform_local_twist_covariance_to_global(self.feedback_odom.pose.pose, hist.twist.covariance) @@ -234,7 +234,7 @@ def calculate_odometry(self, odom, source_odom): # initial covariance of pose is defined as same value of source_odom dt = 0.0 global_twist_covariance = transform_local_twist_covariance_to_global(result_odom.pose.pose, result_odom.twist.covariance) - result_odom.pose.pose.covariance = update_pose_covariance(result_odom.pose.covariance, global_twist_covariance, dt) + result_odom.pose.covariance = update_pose_covariance(result_odom.pose.covariance, global_twist_covariance, dt) self.odom = result_odom def update_twist(self, twist, new_twist): diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py index c0e3b89790..42610f7559 100644 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py @@ -1,5 +1,6 @@ #! /usr/bin/env python +import rospy import tf import numpy from geometry_msgs.msg import PoseWithCovariance, TwistWithCovariance, Twist, Pose, Point, Quaternion, Vector3 @@ -13,9 +14,9 @@ def transform_local_twist_to_global(pose, local_twist): global_velocity = numpy.dot(rotation_matrix, numpy.array([[local_twist.linear.x], [local_twist.linear.y], [local_twist.linear.z]])) - global_omega = numpy.dot(rotation_matrix, numpy.array([[local_twist.twist.angular.x], - [local_twist.twist.angular.y], - [local_twist.twist.angular.z]])) + global_omega = numpy.dot(rotation_matrix, numpy.array([[local_twist.angular.x], + [local_twist.angular.y], + [local_twist.angular.z]])) return Twist(Vector3(*global_velocity[:, 0]), Vector3(*global_omega[:, 0])) def transform_local_twist_covariance_to_global(pose, local_twist_with_covariance): @@ -23,7 +24,7 @@ def transform_local_twist_covariance_to_global(pose, local_twist_with_covariance rot = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3] - twist_cov_matrix = numpy.matrix(local_twist_with_covariance.covariance).reshape(6, 6) + twist_cov_matrix = numpy.matrix(local_twist_with_covariance).reshape(6, 6) global_twist_cov_matrix = numpy.zeros((6, 6)) global_twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(twist_cov_matrix[:3, :3].dot(rotation_matrix)) global_twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(twist_cov_matrix[3:6, 3:6].dot(rotation_matrix)) @@ -86,7 +87,7 @@ def update_pose_covariance(pose_cov, global_twist_cov, dt): # tf broadcast def broadcast_transform(broadcast, odom, invert_tf): - if not self.odom: + if not odom or not broadcast: return position = [odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z] orientation = [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w] @@ -100,7 +101,7 @@ def broadcast_transform(broadcast, odom, invert_tf): else: parent_frame = odom.header.frame_id target_frame = odom.child_frame_id - broadcast.sendTransform(position, orientation, rospy.Time.now(), target_frame, parent_frame) + broadcast.sendTransform(position, orientation, odom.header.stamp, target_frame, parent_frame) # mathmatical tools def make_homogeneous_matrix(trans, rot): From 929c77919f520c6f1a31e96274c185b5992d28b3 Mon Sep 17 00:00:00 2001 From: iori Date: Sat, 19 Dec 2015 20:05:34 +0900 Subject: [PATCH 08/15] [jsk_robot_startup] Put odometry calculation together in OdometryOffset and OdometryFeedbackWrapper is only calculate feedback --- .../jsk_robot_startup/CMakeLists.txt | 1 + .../OdometryFeedbackWrapperReconfigure.cfg | 6 - .../config/OdometryOffsetReconfigure.cfg | 21 +++ .../OdometryFeedbackWrapper.py | 12 +- .../src/jsk_robot_startup/OdometryOffset.py | 159 +++++++----------- 5 files changed, 86 insertions(+), 113 deletions(-) create mode 100755 jsk_robot_common/jsk_robot_startup/config/OdometryOffsetReconfigure.cfg diff --git a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt index 151a4fecb9..c2e8cb1a26 100644 --- a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt +++ b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_python_setup() generate_dynamic_reconfigure_options( + config/OdometryOffsetReconfigure.cfg config/OdometryFeedbackWrapperReconfigure.cfg config/ConstantHeightFramePublisherReconfigure.cfg ) diff --git a/jsk_robot_common/jsk_robot_startup/config/OdometryFeedbackWrapperReconfigure.cfg b/jsk_robot_common/jsk_robot_startup/config/OdometryFeedbackWrapperReconfigure.cfg index 980b335f31..bc64beca39 100755 --- a/jsk_robot_common/jsk_robot_startup/config/OdometryFeedbackWrapperReconfigure.cfg +++ b/jsk_robot_common/jsk_robot_startup/config/OdometryFeedbackWrapperReconfigure.cfg @@ -5,12 +5,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("sigma_x", double_t, 0, "sigma of distribution on x velocity", 0.3, 0.0001, 10.0) -gen.add("sigma_y", double_t, 0, "sigma of distribution on y velocity", 0.3, 0.0001, 10.0) -gen.add("sigma_z", double_t, 0, "sigma of distribution on z velocity", 0.3, 0.0001, 10.0) -gen.add("sigma_roll", double_t, 0, "sigma of distribution on roll angular velocity", 0.3, 0.0001, 10.0) -gen.add("sigma_pitch", double_t, 0, "sigma of distribution on pitch angular velocity", 0.3, 0.0001, 10.0) -gen.add("sigma_yaw", double_t, 0, "sigma of distribution on yaw angular velocity", 0.3, 0.0001, 10.0) gen.add("force_feedback_sigma", double_t, 0, "Odometry feedback is forcely enabled when sigma is larger than this value", 0.0001, 0.3, 10.0) gen.add("distribution_feedback_minimum_sigma", double_t, 0, "Distribution check is enabled when sigma is larger than this value", 0.0001, 0.3, 10.0) diff --git a/jsk_robot_common/jsk_robot_startup/config/OdometryOffsetReconfigure.cfg b/jsk_robot_common/jsk_robot_startup/config/OdometryOffsetReconfigure.cfg new file mode 100755 index 0000000000..a0a864b83b --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/config/OdometryOffsetReconfigure.cfg @@ -0,0 +1,21 @@ +#!/usr/bin/env python +PACKAGE = "jsk_robot_startup" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("mean_x", double_t, 0, "Average for linear x", -10.0, 0.0, 10.0) +gen.add("mean_y", double_t, 0, "Average for linear y", -10.0, 0.0, 10.0) +gen.add("mean_z", double_t, 0, "Average for linear z", -10.0, 0.0, 10.0) +gen.add("mean_roll", double_t, 0, "Average for angular x", -10.0, 0.0, 10.0) +gen.add("mean_pitch", double_t, 0, "Average for angular y", -10.0, 0.0, 10.0) +gen.add("mean_yaw", double_t, 0, "Average for angular z", -10.0, 0.0, 10.0) +gen.add("sigma_x", double_t, 0, "Standard diviation for linear x", 1e-6, 0.05, 10.0) +gen.add("sigma_y", double_t, 0, "Standard diviation for linear y", 1e-6, 0.05, 10.0) +gen.add("sigma_z", double_t, 0, "Standard diviation for linear z", 1e-6, 0.05, 10.0) +gen.add("sigma_roll", double_t, 0, "Standard diviation for angular x", 1e-6, 0.05, 10.0) +gen.add("sigma_pitch", double_t, 0, "Standard diviation for angular y", 1e-6, 0.05, 10.0) +gen.add("sigma_yaw", double_t, 0, "Standard diviation for angular z", 1e-6, 0.05, 10.0) + +exit(gen.generate(PACKAGE, "constant_height_frame_publisher_reconfigure", "OdometryOffsetReconfigure")) diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py index a2e77ca492..3f476df1ff 100755 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py @@ -16,7 +16,7 @@ from dynamic_reconfigure.server import Server from jsk_robot_startup.cfg import OdometryFeedbackWrapperReconfigureConfig -from odometry_utils import transform_local_twist_to_global, transform_local_twist_covariance_to_global, update_pose, update_twist_covariance, update_pose_covariance, broadcast_transform, make_homogeneous_matrix +from odometry_utils import transform_local_twist_to_global, transform_local_twist_covariance_to_global, update_pose, update_pose_covariance, broadcast_transform, make_homogeneous_matrix class OdometryFeedbackWrapper(object): def __init__(self): @@ -40,12 +40,6 @@ def __init__(self): self.r = rospy.Rate(self.rate) self.lock = threading.Lock() self.odom_history = [] - self.v_sigma = [rospy.get_param("~sigma_x", 0.05), - rospy.get_param("~sigma_y", 0.1), - rospy.get_param("~sigma_z", 0.0001), - rospy.get_param("~sigma_roll", 0.0001), - rospy.get_param("~sigma_pitch", 0.0001), - rospy.get_param("~sigma_yaw", 0.01)] self.force_feedback_sigma = rospy.get_param("~force_feedback_sigma", 0.5) self.distribution_feedback_minimum_sigma = rospy.get_param("~distribution_feedback_minimum_sigma", 0.5) self.pub = rospy.Publisher("~output", Odometry, queue_size = 1) @@ -60,10 +54,6 @@ def execute(self): self.r.sleep() def reconfigure_callback(self, config, level): - with self.lock: - for i, sigma in enumerate(["sigma_x", "sigma_y", "sigma_z", "sigma_roll", "sigma_pitch", "sigma_yaw"]): - self.v_sigma[i] = config[sigma] - rospy.loginfo("[%s]" + "velocity sigma updated: x: {0}, y: {1}, z: {2}, roll: {3}, pitch: {4}, yaw: {5}".format(*self.v_sigma), rospy.get_name()) self.force_feedback_sigma = config["force_feedback_sigma"] rospy.loginfo("[%s]: force feedback sigma is %f", rospy.get_name(), self.force_feedback_sigma) self.distribution_feedback_minimum_sigma = config["distribution_feedback_minimum_sigma"] diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py index 2ab1eed98d..9e0477378d 100644 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py @@ -9,12 +9,21 @@ import time import threading import copy +from dynamic_reconfigure.server import Server +from jsk_robot_startup.cfg import OdometryOffsetReconfigureConfig +from odometry_utils import make_homogeneous_matrix, update_twist_covariance, update_pose, update_pose_covariance, broadcast_transform, transform_local_twist_to_global, transform_local_twist_covariance_to_global class OdometryOffset(object): def __init__(self): rospy.init_node("OdometryFeedbackWrapper", anonymous=True) + self.broadcast = tf.TransformBroadcaster() + self.listener = tf.TransformListener(True, rospy.Duration(120)) + self.offset_matrix = None + self.prev_odom = None + self.lock = threading.Lock() # execute rate self.rate = float(rospy.get_param("~rate", 100)) + self.r = rospy.Rate(self.rate) # tf parameters self.publish_tf = rospy.get_param("~publish_tf", True) self.invert_tf = rospy.get_param("~invert_tf", True) @@ -24,27 +33,40 @@ def __init__(self): self.tf_duration = rospy.get_param("~tf_duration", 1) # for filter (only used when use_twist_filter is True) self.use_twist_filter = rospy.get_param("~use_twist_filter", False) - self.filter_buffer_size = rospy.get_param("~filter_buffer_size", 5) - self.filter_buffer = [] - # for covariance calculation (only used when calculate_covariance is True) - self.calculate_covariance = rospy.get_param("~calculate_covariance", False) - self.twist_proportional_sigma = rospy.get_param("~twist_proportional_sigma", False) - self.v_sigma = [rospy.get_param("~sigma_x", 0.05), - rospy.get_param("~sigma_y", 0.1), - rospy.get_param("~sigma_z", 0.0001), - rospy.get_param("~sigma_roll", 0.0001), - rospy.get_param("~sigma_pitch", 0.0001), - rospy.get_param("~sigma_yaw", 0.01)] - self.broadcast = tf.TransformBroadcaster() - self.listener = tf.TransformListener(True, rospy.Duration(120)) - self.r = rospy.Rate(self.rate) - self.offset_matrix = None - self.prev_odom = None - self.lock = threading.Lock() + if self.use_twist_filter: + self.filter_buffer_size = rospy.get_param("~filter_buffer_size", 5) + self.filter_buffer = [] + # to overwrite probability density function (only used when overwrite_pdf is True) + self.overwrite_pdf = rospy.get_param("~overwrite_pdf", False) + if self.overwrite_pdf: + self.twist_proportional_sigma = rospy.get_param("~twist_proportional_sigma", False) + self.v_err_mean = [rospy.get_param("~mean_x", 0.0), + rospy.get_param("~mean_y", 0.0), + rospy.get_param("~mean_z", 0.0), + rospy.get_param("~mean_roll", 0.0), + rospy.get_param("~mean_pitch", 0.0), + rospy.get_param("~mean_yaw", 0.0)] + self.v_err_sigma = [rospy.get_param("~sigma_x", 0.05), + rospy.get_param("~sigma_y", 0.1), + rospy.get_param("~sigma_z", 0.0001), + rospy.get_param("~sigma_roll", 0.0001), + rospy.get_param("~sigma_pitch", 0.0001), + rospy.get_param("~sigma_yaw", 0.01)] + self.reconfigure_server = Server(OdometryOffsetReconfigureConfig, self.reconfigure_callback) self.source_odom_sub = rospy.Subscriber("~source_odom", Odometry, self.source_odom_callback) self.init_signal_sub = rospy.Subscriber("~init_signal", Empty, self.init_signal_callback) self.pub = rospy.Publisher("~output", Odometry, queue_size = 1) + def reconfigure_callback(self, config, level): + with self.lock: + for i, mean in enumerate(["mean_x", "mean_y", "mean_z", "mean_roll", "mean_pitch", "mean_yaw"]): + self.v_err_mean[i] = config[mean] + for i, sigma in enumerate(["sigma_x", "sigma_y", "sigma_z", "sigma_roll", "sigma_pitch", "sigma_yaw"]): + self.v_err_sigma[i] = config[sigma] + rospy.loginfo("[%s]" + "velocity mean updated: x: {0}, y: {1}, z: {2}, roll: {3}, pitch: {4}, yaw: {5}".format(*self.v_err_mean), rospy.get_name()) + rospy.loginfo("[%s]" + "velocity sigma updated: x: {0}, y: {1}, z: {2}, roll: {3}, pitch: {4}, yaw: {5}".format(*self.v_err_sigma), rospy.get_name()) + return config + def execute(self): while not rospy.is_shutdown(): self.r.sleep() @@ -56,10 +78,10 @@ def calculate_offset(self, odom): except: rospy.logwarn("[%s] failed to solve tf in initialize_odometry: %s to %s", rospy.get_name(), self.base_odom_frame, odom.child_frame_id) return None - base_odom_to_base_link = self.make_homogeneous_matrix(trans, rot) # base_odom -> base_link - base_link_to_odom = numpy.linalg.inv(self.make_homogeneous_matrix([odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z], - [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, - odom.pose.pose.orientation.z, odom.pose.pose.orientation.w])) # base_link -> odom + base_odom_to_base_link = make_homogeneous_matrix(trans, rot) # base_odom -> base_link + base_link_to_odom = numpy.linalg.inv(make_homogeneous_matrix([odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z], + [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, + odom.pose.pose.orientation.z, odom.pose.pose.orientation.w])) # base_link -> odom return base_odom_to_base_link.dot(base_link_to_odom) # base_odom -> odom def init_signal_callback(self, msg): @@ -67,14 +89,15 @@ def init_signal_callback(self, msg): with self.lock: self.offset_matrix = None self.prev_odom = None - self.filter_buffer = [] + if self.use_twist_filter: + self.filter_buffer = [] def source_odom_callback(self, msg): with self.lock: if self.offset_matrix != None: - source_odom_matrix = self.make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z], - [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, - msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) + source_odom_matrix = make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z], + [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) new_odom = copy.deepcopy(msg) new_odom.header.frame_id = self.odom_frame new_odom.child_frame_id = self.base_link_frame @@ -86,19 +109,28 @@ def source_odom_callback(self, msg): new_odom.twist.twist = Twist(Vector3(*vel[0:3]), Vector3(*vel[3: 6])) # overwrite twist covariance when calculate_covariance flag is True - if self.calculate_covariance: - self.update_twist_covariance(new_odom.twist) + if self.overwrite_pdf: + # shift twist according to error mean + new_odom.twist.twist.linear.x += self.v_err_mean[0] + new_odom.twist.twist.linear.y += self.v_err_mean[1] + new_odom.twist.twist.linear.z += self.v_err_mean[2] + new_odom.twist.twist.angular.x += self.v_err_mean[3] + new_odom.twist.twist.angular.y += self.v_err_mean[4] + new_odom.twist.twist.angular.z += self.v_err_mean[5] + # calculate twist covariance according to standard diviation + new_odom.twist.covariance = update_twist_covariance(new_odom.twist, self.v_err_sigma, self.twist_proportional_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.calculate_covariance: + 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 = self.transform_twist_with_covariance_to_global(new_odom.pose, new_odom.twist) - new_odom.pose.covariance = self.update_pose_covariance(self.prev_odom.pose.covariance, global_twist_with_covariance.covariance, dt) + 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: @@ -112,7 +144,7 @@ def source_odom_callback(self, msg): # publish self.pub.publish(new_odom) if self.publish_tf: - self.broadcast_transform(new_odom) + broadcast_transform(self.broadcast, new_odom, self.invert_tf) self.prev_odom = new_odom @@ -121,74 +153,9 @@ def source_odom_callback(self, msg): if current_offset_matrix != None: self.offset_matrix = current_offset_matrix - def make_homogeneous_matrix(self, trans, rot): - homogeneous_matrix = tf.transformations.quaternion_matrix(rot) - homogeneous_matrix[:3, 3] = numpy.array(trans).reshape(1, 3) - return homogeneous_matrix - - def broadcast_transform(self, odom): - position = [odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z] - orientation = [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w] - if self.invert_tf: - homogeneous_matrix = self.make_homogeneous_matrix(position, orientation) - homogeneous_matrix_inv = numpy.linalg.inv(homogeneous_matrix) - position = list(homogeneous_matrix_inv[:3, 3]) - orientation = list(tf.transformations.quaternion_from_matrix(homogeneous_matrix_inv)) - parent_frame = odom.child_frame_id - target_frame = odom.header.frame_id - else: - parent_frame = odom.header.frame_id - target_frame = odom.child_frame_id - self.broadcast.sendTransform(position, orientation, odom.header.stamp, target_frame, parent_frame) - def median_filter(self, data): self.filter_buffer.append(data) ret = numpy.median(self.filter_buffer, axis = 0) if len(self.filter_buffer) >= self.filter_buffer_size: self.filter_buffer.pop(0) # filter_buffer has at least 1 member return ret - - def update_twist_covariance(self, twist): - twist_list = [twist.twist.linear.x, twist.twist.linear.y, twist.twist.linear.z, twist.twist.angular.x, twist.twist.angular.y, twist.twist.angular.z] - if self.twist_proportional_sigma == True: - current_sigma = [x * y for x, y in zip(twist_list, self.v_sigma)] - else: - if all([abs(x) < 1e-6 for x in twist_list]): - current_sigma = [1e-6] * 6 # trust "completely stopping" state - else: - current_sigma = self.v_sigma - twist.covariance = numpy.diag([max(x**2, 0.001*0.001) for x in current_sigma]).reshape(-1,).tolist() # covariance should be singular - - def update_pose_covariance(self, pose_cov, global_twist_cov, dt): - ret_pose_cov = [] - # make matirx from covariance array - prev_pose_cov_matrix = numpy.matrix(pose_cov).reshape(6, 6) - global_twist_cov_matrix = numpy.matrix(global_twist_cov).reshape(6, 6) - # jacobian matrix - # elements in pose and twist are assumed to be independent on global coordinates - jacobi_pose = numpy.diag([1.0] * 6) - jacobi_twist = numpy.diag([dt] * 6) - # covariance calculation - pose_cov_matrix = jacobi_pose.dot(prev_pose_cov_matrix.dot(jacobi_pose.T)) + jacobi_twist.dot(global_twist_cov_matrix.dot(jacobi_twist.T)) - # update covariances as array type (twist is same as before) - ret_pose_cov = numpy.array(pose_cov_matrix).reshape(-1,).tolist() - return ret_pose_cov - - def transform_twist_with_covariance_to_global(self, pose, twist): - trans = [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z] - rot = [pose.pose.orientation.x, pose.pose.orientation.y, - pose.pose.orientation.z, pose.pose.orientation.w] - rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3] - twist_cov_matrix = numpy.matrix(twist.covariance).reshape(6, 6) - global_velocity = numpy.dot(rotation_matrix, numpy.array([[twist.twist.linear.x], - [twist.twist.linear.y], - [twist.twist.linear.z]])) - global_omega = numpy.dot(rotation_matrix, numpy.array([[twist.twist.angular.x], - [twist.twist.angular.y], - [twist.twist.angular.z]])) - global_twist_cov_matrix = numpy.zeros((6, 6)) - global_twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(twist_cov_matrix[:3, :3].dot(rotation_matrix)) - global_twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(twist_cov_matrix[3:6, 3:6].dot(rotation_matrix)) - - return TwistWithCovariance(Twist(Vector3(*global_velocity[:, 0]), Vector3(*global_omega[:, 0])), - global_twist_cov_matrix.reshape(-1,).tolist()) From bccfb1aa46c8e924f46fa4a398e331f54d32b35c Mon Sep 17 00:00:00 2001 From: iori Date: Sat, 19 Dec 2015 20:32:53 +0900 Subject: [PATCH 09/15] [jsk_robot_startup] Use common odometry utilities in ParticleOdometry --- .../src/jsk_robot_startup/ParticleOdometry.py | 139 ++---------------- .../src/jsk_robot_startup/odometry_utils.py | 41 ++++++ 2 files changed, 50 insertions(+), 130 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py index d99edb9fbd..46fafddcdc 100644 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py @@ -3,7 +3,6 @@ import rospy import numpy import scipy.stats -import math import random import threading import itertools @@ -15,46 +14,8 @@ from std_msgs.msg import Empty from sensor_msgs.msg import Imu from geometry_msgs.msg import PoseWithCovariance, TwistWithCovariance, Twist, Pose, Point, Quaternion, Vector3 +from odometry_utils import norm_pdf_multivariate, transform_quaternion_to_euler, transform_local_twist_to_global, transform_local_twist_covariance_to_global, update_pose, update_pose_covariance -# scipy.stats.multivariate_normal only can be used after SciPy 0.14.0 -# input: x(array), mean(array), cov_inv(matrix) output: probability of x -# covariance has to be inverted to reduce calculation time -def norm_pdf_multivariate(x, mean, cov_inv): - size = len(x) - if size == len(mean) and (size, size) == cov_inv.shape: - inv_det = numpy.linalg.det(cov_inv) - if not inv_det > 0: - rospy.logwarn("Determinant of inverse cov matrix {0} is equal or smaller than zero".format(inv_det)) - return 0.0 - norm_const = math.pow((2 * numpy.pi), float(size) / 2) * math.pow(1 / inv_det, 1.0 / 2) # determinant of inverse matrix is reciprocal - if not norm_const > 0 : - rospy.logwarn("Norm const {0} is equal or smaller than zero".format(norm_const)) - return 0.0 - x_mean = numpy.matrix(x - mean) - exponent = -0.5 * (x_mean * cov_inv * x_mean.T) - if exponent > 0: - rospy.logwarn("Exponent {0} is larger than zero".format(exponent)) - exponent = 0 - result = math.pow(math.e, exponent) - return result / norm_const - else: - rospy.logwarn("The dimensions of the input don't match") - return 0.0 - -# tf.transformations.euler_from_quaternion is slow because the function calculates matrix inside. -# cf. https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles -def transform_quaternion_to_euler(quat): - zero_thre = numpy.finfo(float).eps * 4.0 # epsilon for testing whether a number is close to zero - roll_numerator = 2 * (quat[3] * quat[0] + quat[1] * quat[2]) - if abs(roll_numerator) < zero_thre: - roll_numerator = numpy.sign(roll_numerator) * 0.0 - yaw_numerator = 2 * (quat[3] * quat[2] + quat[0] * quat[1]) - if abs(yaw_numerator) < zero_thre: - yaw_numerator = numpy.sign(yaw_numerator) * 0.0 - return (numpy.arctan2(roll_numerator, 1 - 2 * (quat[0] ** 2 + quat[1] ** 2)), - numpy.arcsin(2 * (quat[3] * quat[1] - quat[2] * quat[0])), - numpy.arctan2(yaw_numerator, 1 - 2 * (quat[1] ** 2 + quat[2] ** 2))) - class ParticleOdometry(object): ## initialize def __init__(self): @@ -136,8 +97,7 @@ def sampling(self, particles, source_odom): global_twist_with_covariance = self.transform_twist_with_covariance_to_global(source_odom.pose, source_odom.twist) sampled_velocities = self.state_transition_probability_rvs(global_twist_with_covariance.twist, global_twist_with_covariance.covariance) # make sampeld velocity at once because multivariate_normal calculates invert matrix and it is slow dt = (source_odom.header.stamp - self.odom.header.stamp).to_sec() - # return self.calculate_pose_transform(prev_x, Twist(Vector3(*vel_list[0:3]), Vector3(*vel_list[3:6])), dt) - return [self.calculate_pose_transform(prt, Twist(Vector3(*vel[0:3]), Vector3(*vel[3:6])), dt) for prt, vel in zip(particles, sampled_velocities)] + return [update_pose(prt, Twist(Vector3(*vel[0:3]), Vector3(*vel[3:6])), dt) for prt, vel in zip(particles, sampled_velocities)] # input: particles(list of pose), min_weight(float) output: list of weights def weighting(self, particles, min_weight): @@ -244,7 +204,7 @@ def publish_odometry(self): self.odom.pose.covariance = list(itertools.chain(*cov)) self.pub.publish(self.odom) if self.publish_tf: - self.broadcast_transform() + broadcast_transform(self.broadcast, self.odom, self.invert_tf) ## callback functions def source_odom_callback(self, msg): @@ -324,94 +284,13 @@ def guess_normal_distribution(self, particles, weights): return (mean.tolist(), cov.tolist()) - def broadcast_transform(self): - if not self.odom: - return - position = [self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z] - orientation = [self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w] - if self.invert_tf: - homogeneous_matrix = tf.transformations.quaternion_matrix(orientation) - homogeneous_matrix[:3, 3] = numpy.array(position).reshape(1, 3) - homogeneous_matrix_inv = numpy.linalg.inv(homogeneous_matrix) - position = list(homogeneous_matrix_inv[:3, 3]) - orientation = list(tf.transformations.quaternion_from_matrix(homogeneous_matrix_inv)) - parent_frame = self.odom.child_frame_id - target_frame = self.odom.header.frame_id - else: - parent_frame = self.odom.header.frame_id - target_frame = self.odom.child_frame_id - self.broadcast.sendTransform(position, orientation, rospy.Time.now(), target_frame, parent_frame) - - def transform_twist_with_covariance_to_global(self, pose, twist): - trans = [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z] - rot = [pose.pose.orientation.x, pose.pose.orientation.y, - pose.pose.orientation.z, pose.pose.orientation.w] - rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3] - twist_cov_matrix = numpy.matrix(twist.covariance).reshape(6, 6) - global_velocity = numpy.dot(rotation_matrix, numpy.array([[twist.twist.linear.x], - [twist.twist.linear.y], - [twist.twist.linear.z]])) - global_omega = numpy.dot(rotation_matrix, numpy.array([[twist.twist.angular.x], - [twist.twist.angular.y], - [twist.twist.angular.z]])) - global_twist_cov_matrix = numpy.zeros((6, 6)) - global_twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(twist_cov_matrix[:3, :3].dot(rotation_matrix)) - global_twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(twist_cov_matrix[3:6, 3:6].dot(rotation_matrix)) - - return TwistWithCovariance(Twist(Vector3(*global_velocity[:, 0]), Vector3(*global_omega[:, 0])), - global_twist_cov_matrix.reshape(-1,).tolist()) + def transform_twist_with_covariance_to_global(self, pose_with_covariance, twist_with_covariance): + global_twist = transform_local_twist_to_global(pose_with_covariance.pose, twist_with_covariance.twist) + global_twist_cov = transform_local_twist_covariance_to_global(pose_with_covariance.pose, twist_with_covariance.covariance) + return TwistWithCovariance(global_twist, global_twist_cov) def update_pose_with_covariance(self, pose_with_covariance, twist_with_covariance, dt): global_twist_with_covariance = self.transform_twist_with_covariance_to_global(pose_with_covariance, twist_with_covariance) - # calculate current pose as integration - ret_pose = self.calculate_pose_transform(pose_with_covariance.pose, global_twist_with_covariance.twist, dt) - # update covariance - ret_pose_cov = self.calculate_pose_covariance_transform(pose_with_covariance.covariance, global_twist_with_covariance.covariance, dt) + ret_pose = update_pose(pose_with_covariance.pose, global_twist_with_covariance.twist, dt) + ret_pose_cov = update_pose_covariance(pose_with_covariance.covariance, global_twist_with_covariance.covariance, dt) return PoseWithCovariance(ret_pose, ret_pose_cov) - - def calculate_pose_transform(self, pose, global_twist, dt): - ret_pose = Pose() - # calculate current pose as integration - ret_pose.position.x = pose.position.x + global_twist.linear.x * dt - ret_pose.position.y = pose.position.y + global_twist.linear.y * dt - ret_pose.position.z = pose.position.z + global_twist.linear.z * dt - ret_pose.orientation = self.calculate_quaternion_transform(pose.orientation, global_twist.angular, dt) - return ret_pose - - def calculate_quaternion_transform(self, orientation, angular, dt): # angular is assumed to be global - # quaternion calculation - quat_vec = numpy.array([[orientation.x], - [orientation.y], - [orientation.z], - [orientation.w]]) - # skew_omega = numpy.matrix([[0, angular.z, -angular.y, angular.x], - # [-angular.z, 0, angular.x, angular.y], - # [angular.y, -angular.x, 0, angular.z], - # [-angular.x, -angular.y, -angular.z, 0]]) - skew_omega = numpy.matrix([[0, -angular.z, angular.y, angular.x], - [angular.z, 0, -angular.x, angular.y], - [-angular.y, angular.x, 0, angular.z], - [-angular.x, -angular.y, -angular.z, 0]]) - new_quat_vec = quat_vec + 0.5 * numpy.dot(skew_omega, quat_vec) * dt - norm = numpy.linalg.norm(new_quat_vec) - if norm == 0: - rospy.logwarn("norm of quaternion is zero") - else: - new_quat_vec = new_quat_vec / norm # normalize - return Quaternion(*numpy.array(new_quat_vec).reshape(-1,).tolist()) - - def calculate_pose_covariance_transform(self, pose_cov, global_twist_cov, dt): - ret_pose_cov = [] - # make matirx from covariance array - prev_pose_cov_matrix = numpy.matrix(pose_cov).reshape(6, 6) - global_twist_cov_matrix = numpy.matrix(global_twist_cov).reshape(6, 6) - # jacobian matrix - # elements in pose and twist are assumed to be independent on global coordinates - jacobi_pose = numpy.diag([1.0] * 6) - jacobi_twist = numpy.diag([dt] * 6) - # covariance calculation - pose_cov_matrix = jacobi_pose.dot(prev_pose_cov_matrix.dot(jacobi_pose.T)) + jacobi_twist.dot(global_twist_cov_matrix.dot(jacobi_twist.T)) - # update covariances as array type (twist is same as before) - ret_pose_cov = numpy.array(pose_cov_matrix).reshape(-1,).tolist() - return ret_pose_cov - diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py index 42610f7559..091fd41a51 100644 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py @@ -3,6 +3,8 @@ import rospy import tf import numpy +import scipy.stats +import math from geometry_msgs.msg import PoseWithCovariance, TwistWithCovariance, Twist, Pose, Point, Quaternion, Vector3 # twist transformation @@ -109,3 +111,42 @@ def make_homogeneous_matrix(trans, rot): homogeneous_matrix[:3, 3] = numpy.array(trans).reshape(1, 3) return homogeneous_matrix +# scipy.stats.multivariate_normal only can be used after SciPy 0.14.0 +# input: x(array), mean(array), cov_inv(matrix) output: probability of x +# covariance has to be inverted to reduce calculation time +def norm_pdf_multivariate(x, mean, cov_inv): + size = len(x) + if size == len(mean) and (size, size) == cov_inv.shape: + inv_det = numpy.linalg.det(cov_inv) + if not inv_det > 0: + rospy.logwarn("Determinant of inverse cov matrix {0} is equal or smaller than zero".format(inv_det)) + return 0.0 + norm_const = math.pow((2 * numpy.pi), float(size) / 2) * math.pow(1 / inv_det, 1.0 / 2) # determinant of inverse matrix is reciprocal + if not norm_const > 0 : + rospy.logwarn("Norm const {0} is equal or smaller than zero".format(norm_const)) + return 0.0 + x_mean = numpy.matrix(x - mean) + exponent = -0.5 * (x_mean * cov_inv * x_mean.T) + if exponent > 0: + rospy.logwarn("Exponent {0} is larger than zero".format(exponent)) + exponent = 0 + result = math.pow(math.e, exponent) + return result / norm_const + else: + rospy.logwarn("The dimensions of the input don't match") + return 0.0 + +# tf.transformations.euler_from_quaternion is slow because the function calculates matrix inside. +# cf. https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles +def transform_quaternion_to_euler(quat): + zero_thre = numpy.finfo(float).eps * 4.0 # epsilon for testing whether a number is close to zero + roll_numerator = 2 * (quat[3] * quat[0] + quat[1] * quat[2]) + if abs(roll_numerator) < zero_thre: + roll_numerator = numpy.sign(roll_numerator) * 0.0 + yaw_numerator = 2 * (quat[3] * quat[2] + quat[0] * quat[1]) + if abs(yaw_numerator) < zero_thre: + yaw_numerator = numpy.sign(yaw_numerator) * 0.0 + return (numpy.arctan2(roll_numerator, 1 - 2 * (quat[0] ** 2 + quat[1] ** 2)), + numpy.arcsin(2 * (quat[3] * quat[1] - quat[2] * quat[0])), + numpy.arctan2(yaw_numerator, 1 - 2 * (quat[1] ** 2 + quat[2] ** 2))) + From f51cd0867b6afee1ef12d8cf9bec796ff5bb6901 Mon Sep 17 00:00:00 2001 From: iori Date: Sat, 19 Dec 2015 21:10:43 +0900 Subject: [PATCH 10/15] [jsk_robot_startup] Update default odometry paremeter set to overwrite viso covariance in OdometryOffset --- .../config/default_odometry_params.yaml | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/config/default_odometry_params.yaml b/jsk_robot_common/jsk_robot_startup/config/default_odometry_params.yaml index f1907b97e7..4c12e2e565 100644 --- a/jsk_robot_common/jsk_robot_startup/config/default_odometry_params.yaml +++ b/jsk_robot_common/jsk_robot_startup/config/default_odometry_params.yaml @@ -1,11 +1,15 @@ -viso_odom_calculator: - rate: 10 +viso_odometry_offset: + overwrite_pdf: true + twist_proportional_sigma: true sigma_x: 1.0 sigma_y: 1.0 sigma_z: 1.0 sigma_roll: 0.5 sigma_pitch: 0.5 sigma_yaw: 0.5 + +viso_odom_calculator: + rate: 10 twist_proportional_sigma: true force_feedback_sigma: 1.0 distribution_feedback_minimum_sigma: 0.1 @@ -14,3 +18,4 @@ viso_odom_calculator: biped_particle_odometry: particle_num: 20 rate: 100 + use_imu: false From e6e055e8c7daaba9ad647c11920beb7adaf273af Mon Sep 17 00:00:00 2001 From: iori Date: Sat, 19 Dec 2015 21:11:57 +0900 Subject: [PATCH 11/15] [jsk_robot_startup] Set default publish_tf as False in unnecessary tfs and do not make broadcast when publish_tf is false --- .../launch/biped_localization.launch | 3 +- .../launch/particle_odometry.launch | 3 ++ .../jsk_robot_startup/launch/viso.launch | 1 + .../scripts/CameraToBaseOffset.py | 36 +++++-------------- .../OdometryFeedbackWrapper.py | 5 +-- .../src/jsk_robot_startup/OdometryOffset.py | 5 +-- .../src/jsk_robot_startup/ParticleOdometry.py | 7 ++-- 7 files changed, 24 insertions(+), 36 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch b/jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch index 0292a9d275..7cea89a4ca 100644 --- a/jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch +++ b/jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch @@ -24,12 +24,11 @@ - + - diff --git a/jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch b/jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch index d22c907cd8..88f4361f36 100644 --- a/jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch +++ b/jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch @@ -15,6 +15,7 @@ base_odom_frame: odom_init odom_frame: biped_odom_offset base_link_frame: BODY + publish_tf: false @@ -27,6 +28,7 @@ base_odom_frame: odom_init odom_frame: viso_odom_offset base_link_frame: BODY + publish_tf: false @@ -42,6 +44,7 @@ odom_frame: viso_odom_integrated base_link_frame: BODY + publish_tf: false diff --git a/jsk_robot_common/jsk_robot_startup/launch/viso.launch b/jsk_robot_common/jsk_robot_startup/launch/viso.launch index 4650f9c3b9..c5d572399c 100644 --- a/jsk_robot_common/jsk_robot_startup/launch/viso.launch +++ b/jsk_robot_common/jsk_robot_startup/launch/viso.launch @@ -37,6 +37,7 @@ + diff --git a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py index 09f03a4bcc..8147b72ae6 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -9,6 +9,7 @@ import time import threading import copy +from jsk_robot_startup import broadcast_transform, make_homogeneous_matrix class CameraToBaseOffset(object): def __init__(self): @@ -17,12 +18,13 @@ def __init__(self): self.rate = float(rospy.get_param("~rate", 100)) # tf parameters self.publish_tf = rospy.get_param("~publish_tf", True) - self.invert_tf = rospy.get_param("~invert_tf", True) + if self.publish_tf: + self.invert_tf = rospy.get_param("~invert_tf", True) + self.broadcast = tf.TransformBroadcaster() self.base_link_frame = rospy.get_param("~base_link_frame", "BODY") self.camera_frame = rospy.get_param("~camera_frame", "left_camera_optical_frame") self.odom_frame = rospy.get_param("~odom_frame", "viso_odom") self.tf_duration = rospy.get_param("~tf_duration", 1) - self.broadcast = tf.TransformBroadcaster() self.listener = tf.TransformListener(True, rospy.Duration(10)) self.r = rospy.Rate(self.rate) self.initial_matrix = None @@ -53,9 +55,9 @@ def source_odom_callback(self, msg): camera_relative_base_transformation = numpy.dot(self.initial_matrix, current_camera_to_base) # base_link transformation in camera coords # calculate offseted odometry - source_odom_matrix = self.make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z], - [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, - msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) + source_odom_matrix = make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z], + [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) new_odom_matrix = camera_relative_base_transformation.dot(source_odom_matrix) # make odometry msg. twist is copied from source_odom @@ -73,27 +75,7 @@ def source_odom_callback(self, msg): # publish self.pub.publish(new_odom) if self.publish_tf: - self.broadcast_transform(new_odom) - - def make_homogeneous_matrix(self, trans, rot): - homogeneous_matrix = tf.transformations.quaternion_matrix(rot) - homogeneous_matrix[:3, 3] = numpy.array(trans).reshape(1, 3) - return homogeneous_matrix - - def broadcast_transform(self, odom): - position = [odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z] - orientation = [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w] - if self.invert_tf: - homogeneous_matrix = self.make_homogeneous_matrix(position, orientation) - homogeneous_matrix_inv = numpy.linalg.inv(homogeneous_matrix) - position = list(homogeneous_matrix_inv[:3, 3]) - orientation = list(tf.transformations.quaternion_from_matrix(homogeneous_matrix_inv)) - parent_frame = odom.child_frame_id - target_frame = odom.header.frame_id - else: - parent_frame = odom.header.frame_id - target_frame = odom.child_frame_id - self.broadcast.sendTransform(position, orientation, odom.header.stamp, target_frame, parent_frame) + broadcast_transform(self.broadcast, new_odom, self.invert_tf) def calculate_camera_to_base_transform(self, stamp): try: @@ -105,7 +87,7 @@ def calculate_camera_to_base_transform(self, stamp): except: rospy.logwarn("[%s] failed to solve camera_to_base tf: %s to %s", rospy.get_name(), self.camera_frame, self.base_link_frame) return None - camera_to_base_link = self.make_homogeneous_matrix(trans, rot) # camera -> base_link + camera_to_base_link = make_homogeneous_matrix(trans, rot) # camera -> base_link return camera_to_base_link if __name__ == '__main__': diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py index 3f476df1ff..ca74f697ec 100755 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py @@ -23,13 +23,14 @@ def __init__(self): rospy.init_node("OdometryFeedbackWrapper", anonymous=True) self.rate = float(rospy.get_param("~rate", 100)) self.publish_tf = rospy.get_param("~publish_tf", True) - self.invert_tf = rospy.get_param("~invert_tf", True) self.odom_frame = rospy.get_param("~odom_frame", "feedback_odom") self.base_link_frame = rospy.get_param("~base_link_frame", "BODY") self.max_feedback_time = rospy.get_param("~max_feedback_time", 60) # if max_feedback_time <= 0, feedback is not occurs by time self.tf_cache_time = rospy.get_param("~tf_cache_time", 60) # determined from frequency of feedback_odom self.twist_proportional_sigma = rospy.get_param("~twist_proportional_sigma", False) - self.broadcast = tf.TransformBroadcaster() + if self.publish_tf: + self.broadcast = tf.TransformBroadcaster() + self.invert_tf = rospy.get_param("~invert_tf", True) self.listener = tf.TransformListener(True, rospy.Duration(self.tf_cache_time + 10)) # 10[sec] is safety mergin for feedback self.odom = None # belief of this wrapper self.feedback_odom = None diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py index 9e0477378d..734e70646a 100644 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py @@ -16,7 +16,6 @@ class OdometryOffset(object): def __init__(self): rospy.init_node("OdometryFeedbackWrapper", anonymous=True) - self.broadcast = tf.TransformBroadcaster() self.listener = tf.TransformListener(True, rospy.Duration(120)) self.offset_matrix = None self.prev_odom = None @@ -26,7 +25,9 @@ def __init__(self): self.r = rospy.Rate(self.rate) # tf parameters self.publish_tf = rospy.get_param("~publish_tf", True) - self.invert_tf = rospy.get_param("~invert_tf", True) + if self.publish_tf: + self.broadcast = tf.TransformBroadcaster() + self.invert_tf = rospy.get_param("~invert_tf", True) self.odom_frame = rospy.get_param("~odom_frame", "offset_odom") self.base_odom_frame = rospy.get_param("~base_odom_frame", "odom_init") self.base_link_frame = rospy.get_param("~base_link_frame", "BODY") diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py index 46fafddcdc..7e1a4de45a 100644 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py @@ -52,9 +52,10 @@ def __init__(self): rospy.get_param("~init_sigma_yaw", 0.05)] # tf self.listener = tf.TransformListener(True, rospy.Duration(10)) - self.broadcast = tf.TransformBroadcaster() - self.publish_tf = rospy.get_param("~publish_tf", True) - self.invert_tf = rospy.get_param("~invert_tf", True) + self.publish_tf = rospy.get_param("~publish_tf", True) + if self.publish_tf: + self.broadcast = tf.TransformBroadcaster() + self.invert_tf = rospy.get_param("~invert_tf", True) # publisher self.pub = rospy.Publisher("~output", Odometry, queue_size = 1) # subscriber From ba7e886386a7e27bf9077d322000278077133914 Mon Sep 17 00:00:00 2001 From: iori Date: Sat, 19 Dec 2015 21:48:05 +0900 Subject: [PATCH 12/15] [jsk_robot_startup] Fix import bug of CameraToBaseOffset --- .../jsk_robot_startup/scripts/CameraToBaseOffset.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py index 8147b72ae6..296a0f542a 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -9,7 +9,7 @@ import time import threading import copy -from jsk_robot_startup import broadcast_transform, make_homogeneous_matrix +from jsk_robot_startup.odometry_utils import broadcast_transform, make_homogeneous_matrix class CameraToBaseOffset(object): def __init__(self): From 0c062eeee094e97c36d18107fd968d2214b100bb Mon Sep 17 00:00:00 2001 From: iori Date: Mon, 21 Dec 2015 12:48:41 +0900 Subject: [PATCH 13/15] [jsk_robot_startup] Trust stopping status when mean offset is accumulated to twist in OdometryOffset --- .../src/jsk_robot_startup/OdometryOffset.py | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py index 734e70646a..41dacf419e 100644 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py @@ -102,22 +102,19 @@ def source_odom_callback(self, msg): 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: - vel = [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] - vel = self.median_filter(vel) - new_odom.twist.twist = Twist(Vector3(*vel[0:3]), Vector3(*vel[3: 6])) + 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: - # shift twist according to error mean - new_odom.twist.twist.linear.x += self.v_err_mean[0] - new_odom.twist.twist.linear.y += self.v_err_mean[1] - new_odom.twist.twist.linear.z += self.v_err_mean[2] - new_odom.twist.twist.angular.x += self.v_err_mean[3] - new_odom.twist.twist.angular.y += self.v_err_mean[4] - new_odom.twist.twist.angular.z += self.v_err_mean[5] + if all([abs(x) < 1e-6 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 new_odom.twist.covariance = update_twist_covariance(new_odom.twist, self.v_err_sigma, self.twist_proportional_sigma) From 87f089a8179a962cc02a0fe62ba806f56053c25c Mon Sep 17 00:00:00 2001 From: iori Date: Mon, 21 Dec 2015 13:16:17 +0900 Subject: [PATCH 14/15] [jsk_robot_startup] Remove twist_proportional_sigma from OdometryFeedbackWrapper --- .../src/jsk_robot_startup/OdometryFeedbackWrapper.py | 1 - 1 file changed, 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py index ca74f697ec..8683462723 100755 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py @@ -27,7 +27,6 @@ def __init__(self): self.base_link_frame = rospy.get_param("~base_link_frame", "BODY") self.max_feedback_time = rospy.get_param("~max_feedback_time", 60) # if max_feedback_time <= 0, feedback is not occurs by time self.tf_cache_time = rospy.get_param("~tf_cache_time", 60) # determined from frequency of feedback_odom - self.twist_proportional_sigma = rospy.get_param("~twist_proportional_sigma", False) if self.publish_tf: self.broadcast = tf.TransformBroadcaster() self.invert_tf = rospy.get_param("~invert_tf", True) From 57eef8e2c3b244b99c50f9d405ca483552a5182c Mon Sep 17 00:00:00 2001 From: iori Date: Mon, 21 Dec 2015 13:22:35 +0900 Subject: [PATCH 15/15] [jsk_robot_startup] Twist proportional sigma option should be processed by individual class, not common utils --- .../src/jsk_robot_startup/OdometryOffset.py | 8 ++++++-- .../src/jsk_robot_startup/odometry_utils.py | 13 +++++-------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py index 41dacf419e..4a866ca756 100644 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py @@ -115,8 +115,12 @@ def source_odom_callback(self, msg): # 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 - new_odom.twist.covariance = update_twist_covariance(new_odom.twist, self.v_err_sigma, self.twist_proportional_sigma) + # 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: + 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) diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py index 091fd41a51..9ff1c29b9b 100644 --- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py +++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py @@ -61,16 +61,13 @@ def update_quaternion(orientation, angular, dt): # angular is assumed to be glob return Quaternion(*numpy.array(new_quat_vec).reshape(-1,).tolist()) # covariance calculation -def update_twist_covariance(twist, v_sigma, twist_proportional_sigma): # twist is assumed to be local +def update_twist_covariance(twist, v_sigma, min_sigma = 1e-3): # twist is assumed to be local twist_list = [twist.twist.linear.x, twist.twist.linear.y, twist.twist.linear.z, twist.twist.angular.x, twist.twist.angular.y, twist.twist.angular.z] - if twist_proportional_sigma == True: - current_sigma = [x * y for x, y in zip(twist_list, v_sigma)] + if all([abs(x) < 1e-6 for x in twist_list]): + current_sigma = [min_sigma] * 6 # trust "completely stopping" state else: - if all([abs(x) < 1e-6 for x in twist_list]): - current_sigma = [1e-6] * 6 # trust "completely stopping" state - else: - current_sigma = v_sigma - return numpy.diag([max(x**2, 0.001*0.001) for x in current_sigma]).reshape(-1,).tolist() # covariance should be singular + current_sigma = v_sigma + return numpy.diag([max(x ** 2, min_sigma ** 2) for x in current_sigma]).reshape(-1,).tolist() # covariance should be singular def update_pose_covariance(pose_cov, global_twist_cov, dt): ret_pose_cov = []