From eafacffeb88172b6bcf6b0a652c89d76e7831589 Mon Sep 17 00:00:00 2001 From: Yuki Omori Date: Thu, 3 Dec 2020 21:32:46 +0900 Subject: [PATCH 01/21] modify transform in case when the odom_frame is not same as initial sensor frame --- .../scripts/CameraToBaseOffset.py | 31 ++++++++++--------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py index 590a7ac1ed..3d32ae87ec 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -23,11 +23,11 @@ def __init__(self): 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.odom_frame = rospy.get_param("~odom_frame", "viso_odom") self.tf_duration = rospy.get_param("~tf_duration", 1) self.listener = tf.TransformListener(True, rospy.Duration(10)) self.r = rospy.Rate(self.rate) - self.initial_matrix = None + self.initial_base_to_odom_transformation = None self.lock = threading.Lock() 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) @@ -40,8 +40,8 @@ def execute(self): def init_signal_callback(self, msg): time.sleep(1) # wait to update odom_init frame with self.lock: - self.initial_matrix = None - + self.initial_base_to_odom_transformation = None + def source_odom_callback(self, msg): with self.lock: # calculate camera transform @@ -49,16 +49,18 @@ def source_odom_callback(self, msg): if current_camera_to_base is None: return - if self.initial_matrix is None: - self.initial_matrix = numpy.linalg.inv(current_camera_to_base) - - 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 = 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) + + if self.initial_base_to_odom_transformation is None: + self.initial_base_to_odom_transformation = numpy.linalg.inv( + numpy.dot(source_odom_matrix, current_camera_to_base)) + + # calculate offseted odometry + current_odom_to_base = numpy.dot(source_odom_matrix, current_camera_to_base) + odom_relative_base_transformation = numpy.dot(self.initial_base_to_odom_transformation, current_odom_to_base) + new_odom_matrix = odom_relative_base_transformation # make odometry msg. twist is copied from source_odom new_odom = copy.deepcopy(msg) @@ -67,11 +69,11 @@ def source_odom_callback(self, msg): 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] + rotation_matrix = odom_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: @@ -89,10 +91,9 @@ def calculate_camera_to_base_transform(self, stamp): return None camera_to_base_link = make_homogeneous_matrix(trans, rot) # camera -> base_link return camera_to_base_link - + if __name__ == '__main__': try: node = CameraToBaseOffset() node.execute() except rospy.ROSInterruptException: pass - From 1d19ac0c68d0a1d91fc4ec653cbc05c60ff1dbc2 Mon Sep 17 00:00:00 2001 From: JAXONRED Date: Wed, 31 Mar 2021 17:30:34 +0900 Subject: [PATCH 02/21] erase warnings --- .../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 3d32ae87ec..1899363aa7 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -84,7 +84,7 @@ def calculate_camera_to_base_transform(self, stamp): (trans,rot) = self.listener.lookupTransform(self.camera_frame, self.base_link_frame, stamp) except: try: - rospy.logwarn("[%s] failed to solve camera_to_base tf in %f. Use rospy.Time(0): %s to %s", rospy.get_name(), stamp.to_sec(), self.camera_frame, self.base_link_frame) + # rospy.logwarn("[%s] failed to solve camera_to_base tf in %f. Use rospy.Time(0): %s to %s", rospy.get_name(), stamp.to_sec(), self.camera_frame, self.base_link_frame) (trans,rot) = self.listener.lookupTransform(self.camera_frame, self.base_link_frame, rospy.Time(0)) except: rospy.logwarn("[%s] failed to solve camera_to_base tf: %s to %s", rospy.get_name(), self.camera_frame, self.base_link_frame) From df07f738313109c3aad66fbf8f070df057780fc0 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 14:22:03 +0900 Subject: [PATCH 03/21] [jsk_robot_startup] fix format of CameraBaseToOffset.py --- .../scripts/CameraToBaseOffset.py | 77 +++++++++++++------ 1 file changed, 53 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 1899363aa7..61c9a6da07 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -3,13 +3,20 @@ import rospy import numpy from nav_msgs.msg import Odometry -from geometry_msgs.msg import Point, Quaternion, Twist, Vector3, TwistWithCovariance -from std_msgs.msg import Float64, Empty +from geometry_msgs.msg import Point +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import Twist +from geometry_msgs.msg import Vector3 +from geometry_msgs.msg import TwistWithCovariance +from std_msgs.msg import Float64 +from std_msgs.msg import Empty import tf import time import threading import copy -from jsk_robot_startup.odometry_utils import broadcast_transform, make_homogeneous_matrix +from jsk_robot_startup.odometry_utils import broadcast_transform +from jsk_robot_startup.odometry_utils import make_homogeneous_matrix + class CameraToBaseOffset(object): def __init__(self): @@ -22,44 +29,55 @@ def __init__(self): 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.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.listener = tf.TransformListener(True, rospy.Duration(10)) self.r = rospy.Rate(self.rate) self.initial_base_to_odom_transformation = None self.lock = threading.Lock() - 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) + 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 execute(self): while not rospy.is_shutdown(): self.r.sleep() def init_signal_callback(self, msg): - time.sleep(1) # wait to update odom_init frame + time.sleep(1) # wait to update odom_init frame with self.lock: self.initial_base_to_odom_transformation = None 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) + current_camera_to_base = self.calculate_camera_to_base_transform( + msg.header.stamp) if current_camera_to_base is None: return - 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]) + 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]) if self.initial_base_to_odom_transformation is None: self.initial_base_to_odom_transformation = numpy.linalg.inv( numpy.dot(source_odom_matrix, current_camera_to_base)) # calculate offseted odometry - current_odom_to_base = numpy.dot(source_odom_matrix, current_camera_to_base) - odom_relative_base_transformation = numpy.dot(self.initial_base_to_odom_transformation, current_odom_to_base) + current_odom_to_base = numpy.dot( + source_odom_matrix, current_camera_to_base) + odom_relative_base_transformation = numpy.dot( + self.initial_base_to_odom_transformation, current_odom_to_base) new_odom_matrix = odom_relative_base_transformation # make odometry msg. twist is copied from source_odom @@ -67,12 +85,17 @@ def source_odom_callback(self, 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) + 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 = odom_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() + 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) @@ -81,19 +104,25 @@ def source_odom_callback(self, msg): def calculate_camera_to_base_transform(self, stamp): try: - (trans,rot) = self.listener.lookupTransform(self.camera_frame, self.base_link_frame, stamp) + (trans, rot) = self.listener.lookupTransform( + self.camera_frame, self.base_link_frame, stamp) except: try: # rospy.logwarn("[%s] failed to solve camera_to_base tf in %f. Use rospy.Time(0): %s to %s", rospy.get_name(), stamp.to_sec(), self.camera_frame, self.base_link_frame) - (trans,rot) = self.listener.lookupTransform(self.camera_frame, self.base_link_frame, rospy.Time(0)) + (trans, rot) = self.listener.lookupTransform( + self.camera_frame, self.base_link_frame, rospy.Time(0)) except: - rospy.logwarn("[%s] failed to solve camera_to_base tf: %s to %s", rospy.get_name(), self.camera_frame, self.base_link_frame) + 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 = 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__': try: node = CameraToBaseOffset() node.execute() - except rospy.ROSInterruptException: pass + except rospy.ROSInterruptException: + pass From bf09f4fbdb1f4dd013d328b5d39586154cce0ea9 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 14:23:42 +0900 Subject: [PATCH 04/21] [jsk_robot_startup] remove ~rate parameter of CameraBaseToOffset.py --- .../jsk_robot_startup/scripts/CameraToBaseOffset.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py index 61c9a6da07..e2f0fc4f73 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -21,8 +21,6 @@ class CameraToBaseOffset(object): def __init__(self): rospy.init_node("CameraToBaseOffset", anonymous=True) - # execute rate - self.rate = float(rospy.get_param("~rate", 100)) # tf parameters self.publish_tf = rospy.get_param("~publish_tf", True) if self.publish_tf: @@ -34,7 +32,6 @@ def __init__(self): self.odom_frame = rospy.get_param("~odom_frame", "viso_odom") self.tf_duration = rospy.get_param("~tf_duration", 1) self.listener = tf.TransformListener(True, rospy.Duration(10)) - self.r = rospy.Rate(self.rate) self.initial_base_to_odom_transformation = None self.lock = threading.Lock() self.source_odom_sub = rospy.Subscriber( @@ -44,8 +41,7 @@ def __init__(self): self.pub = rospy.Publisher("~output", Odometry, queue_size=1) def execute(self): - while not rospy.is_shutdown(): - self.r.sleep() + rospy.spin() def init_signal_callback(self, msg): time.sleep(1) # wait to update odom_init frame From befb191381cbbb361cd6ea2a4f84cc8bf277275a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 14:49:42 +0900 Subject: [PATCH 05/21] [jsk_robot_startup] refactor variable names of CameraBaseToOffset.py --- .../scripts/CameraToBaseOffset.py | 118 ++++++++++-------- 1 file changed, 69 insertions(+), 49 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py index e2f0fc4f73..b47d875388 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -21,19 +21,24 @@ class CameraToBaseOffset(object): def __init__(self): rospy.init_node("CameraToBaseOffset", anonymous=True) + + # frame_id + self.base_frame_id = rospy.get_param("~base_frame_id", "BODY") + self.camera_frame_id = rospy.get_param("~camera_frame_id", "left_camera_optical_frame") + self.odom_frame_id = rospy.get_param("~odom_frame_id", "viso_odom") + # tf parameters self.publish_tf = rospy.get_param("~publish_tf", True) + self.invert_tf = rospy.get_param("~invert_tf", True) + self.tf_duration = rospy.get_param("~tf_duration", 1) + + # other members 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.listener = tf.TransformListener(True, rospy.Duration(10)) - self.initial_base_to_odom_transformation = None + self.htm_odom_base_to_odom_camera = None self.lock = threading.Lock() + self.source_odom_sub = rospy.Subscriber( "~source_odom", Odometry, self.source_odom_callback) self.init_signal_sub = rospy.Subscriber( @@ -46,74 +51,89 @@ def execute(self): def init_signal_callback(self, msg): time.sleep(1) # wait to update odom_init frame with self.lock: - self.initial_base_to_odom_transformation = None + self.htm_odom_base_to_odom_camera = None 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 is None: + htm_camera_to_base = self.get_htm_camera_to_base(msg.header.stamp) + if htm_camera_to_base is None: return - source_odom_matrix = make_homogeneous_matrix( + # htm (Homogeneous Transformation Matrix) + # from odom frame of camera to current camera frame + htm_odom_camera_to_camera = 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]) + msg.pose.pose.orientation.w] + ) - if self.initial_base_to_odom_transformation is None: - self.initial_base_to_odom_transformation = numpy.linalg.inv( - numpy.dot(source_odom_matrix, current_camera_to_base)) + if self.htm_odom_base_to_odom_camera is None: + self.htm_odom_base_to_odom_camera = numpy.linalg.inv( + numpy.dot(htm_odom_camera_to_camera, htm_camera_to_base)) # calculate offseted odometry - current_odom_to_base = numpy.dot( - source_odom_matrix, current_camera_to_base) - odom_relative_base_transformation = numpy.dot( - self.initial_base_to_odom_transformation, current_odom_to_base) - new_odom_matrix = odom_relative_base_transformation + htm_odom_camera_to_base = numpy.dot( + htm_odom_camera_to_camera, htm_camera_to_base) + htm_odom_base_to_base = numpy.dot( + self.htm_odom_base_to_odom_camera, htm_odom_camera_to_base) + + # calculate transformed pose covariance + transformed_pose_cov_matrix = numpy.matrix(msg.pose.covariance).reshape(6, 6) + rotation_matrix = htm_odom_base_to_base[:3, :3] + transformed_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot( + transformed_pose_cov_matrix[:3, :3].dot(rotation_matrix)) + transformed_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot( + transformed_pose_cov_matrix[3:6, 3:6].dot(rotation_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 = odom_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() + transformed_odom_msg = copy.deepcopy(msg) + transformed_odom_msg.header.frame_id = self.odom_frame_id + transformed_odom_msg.child_frame_id = self.base_frame_id + transformed_odom_msg.pose.pose.position = Point(*list(htm_odom_base_to_base[:3, 3])) + transformed_odom_msg.pose.pose.orientation = Quaternion( + *list(tf.transformations.quaternion_from_matrix(htm_odom_base_to_base))) + transformed_odom_msg.pose.covariance = numpy.array(transformed_pose_cov_matrix).reshape(-1,).tolist() # publish - self.pub.publish(new_odom) + self.pub.publish(transformed_odom_msg) if self.publish_tf: - broadcast_transform(self.broadcast, new_odom, self.invert_tf) + broadcast_transform(self.broadcast, transformed_odom_msg, self.invert_tf) - def calculate_camera_to_base_transform(self, stamp): + def get_htm_camera_to_base(self, stamp): try: (trans, rot) = self.listener.lookupTransform( - self.camera_frame, self.base_link_frame, stamp) - except: + self.camera_frame_id, + self.base_frame_id, + stamp) + except (tf.LookupException, + tf.ConnectivityException, + tf.ExtrapolationException): try: - # rospy.logwarn("[%s] failed to solve camera_to_base tf in %f. Use rospy.Time(0): %s to %s", rospy.get_name(), stamp.to_sec(), self.camera_frame, self.base_link_frame) + rospy.logwarn( + "[%s] failed to solve camera_to_base tf in %f. Use rospy.Time(0): %s to %s", + rospy.get_name(), + stamp.to_sec(), + self.camera_frame_id, + self.base_frame_id) (trans, rot) = self.listener.lookupTransform( - self.camera_frame, self.base_link_frame, rospy.Time(0)) - except: - rospy.logwarn("[%s] failed to solve camera_to_base tf: %s to %s", rospy.get_name( - ), self.camera_frame, self.base_link_frame) + self.camera_frame_id, + self.base_frame_id, + rospy.Time(0)) + except (tf.LookupException, + tf.ConnectivityException, + tf.ExtrapolationException): + rospy.logwarn( + "[%s] failed to solve camera_to_base tf: %s to %s", + rospy.get_name(), + self.camera_frame_id, + self.base_frame_id) return None - camera_to_base_link = make_homogeneous_matrix( - trans, rot) # camera -> base_link - return camera_to_base_link + return make_homogeneous_matrix(trans, rot) if __name__ == '__main__': From 8766a6e4710e836400bee43b28d3115ba26aa133 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 16:23:15 +0900 Subject: [PATCH 06/21] [jsk_robot_startup] support twist calculation --- .../scripts/CameraToBaseOffset.py | 111 ++++++++++++------ 1 file changed, 77 insertions(+), 34 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py index b47d875388..65b2a0aa6e 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -5,10 +5,7 @@ from nav_msgs.msg import Odometry from geometry_msgs.msg import Point from geometry_msgs.msg import Quaternion -from geometry_msgs.msg import Twist from geometry_msgs.msg import Vector3 -from geometry_msgs.msg import TwistWithCovariance -from std_msgs.msg import Float64 from std_msgs.msg import Empty import tf import time @@ -24,7 +21,8 @@ def __init__(self): # frame_id self.base_frame_id = rospy.get_param("~base_frame_id", "BODY") - self.camera_frame_id = rospy.get_param("~camera_frame_id", "left_camera_optical_frame") + self.camera_frame_id = rospy.get_param( + "~camera_frame_id", "left_camera_optical_frame") self.odom_frame_id = rospy.get_param("~odom_frame_id", "viso_odom") # tf parameters @@ -63,63 +61,108 @@ def source_odom_callback(self, msg): # htm (Homogeneous Transformation Matrix) # from odom frame of camera to current camera frame htm_odom_camera_to_camera = 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] - ) + [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] + ) if self.htm_odom_base_to_odom_camera is None: self.htm_odom_base_to_odom_camera = numpy.linalg.inv( numpy.dot(htm_odom_camera_to_camera, htm_camera_to_base)) - # calculate offseted odometry + # calculate transformed pose htm_odom_camera_to_base = numpy.dot( htm_odom_camera_to_camera, htm_camera_to_base) htm_odom_base_to_base = numpy.dot( self.htm_odom_base_to_odom_camera, htm_odom_camera_to_base) # calculate transformed pose covariance - transformed_pose_cov_matrix = numpy.matrix(msg.pose.covariance).reshape(6, 6) + raw_pose_cov_matrix = numpy.matrix( + msg.pose.covariance).reshape(6, 6) rotation_matrix = htm_odom_base_to_base[:3, :3] + transformed_pose_cov_matrix = copy.deepcopy(raw_pose_cov_matrix) transformed_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot( - transformed_pose_cov_matrix[:3, :3].dot(rotation_matrix)) + raw_pose_cov_matrix[:3, :3].dot(rotation_matrix)) transformed_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot( - transformed_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix)) - - # make odometry msg. twist is copied from source_odom + raw_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix)) + + # calculate transformed twist + # + # NOTICE: Effect from 1st derivative of position and rotation + # from camera to base is not considered. + # + twist_on_camera = numpy.array([[msg.twist.twist.linear.x], + [msg.twist.twist.linear.y], + [msg.twist.twist.linear.y], + [msg.twist.twist.angular.x], + [msg.twist.twist.angular.y], + [msg.twist.twist.angular.y]]) + rotation_matrix_camera_to_base = htm_camera_to_base[:3, :3] + pos_c2b = htm_camera_to_base[4, :3] + twist_transform_matrix = numpy.zeros((4, 4)) + twist_transform_matrix[0:3, 0:3] = rotation_matrix_camera_to_base + twist_transform_matrix[3:6, 3:6] = rotation_matrix_camera_to_base + twist_transform_matrix[0:3, 3:6] = numpy.dot( + numpy.array([[0, -pos_c2b[2], pos_c2b[1]], + [pos_c2b[2], 0, -pos_c2b[0]], + [-pos_c2b[1], pos_c2b[0], 0]]), + rotation_matrix_camera_to_base + ) + twist_on_base = numpy.dot( + twist_transform_matrix, + twist_on_camera) + + # calculate transformed twist covariance + raw_twist_cov_matrix = numpy.matrix( + msg.twist.covariance).reshape(6, 6) + transformed_twist_cov_matrix = (twist_transform_matrix.T).dot( + raw_twist_cov_matrix.dot( + twist_transform_matrix)) + + # make odometry msg. transformed_odom_msg = copy.deepcopy(msg) transformed_odom_msg.header.frame_id = self.odom_frame_id transformed_odom_msg.child_frame_id = self.base_frame_id - transformed_odom_msg.pose.pose.position = Point(*list(htm_odom_base_to_base[:3, 3])) + transformed_odom_msg.pose.pose.position = Point( + *list(htm_odom_base_to_base[:3, 3])) transformed_odom_msg.pose.pose.orientation = Quaternion( - *list(tf.transformations.quaternion_from_matrix(htm_odom_base_to_base))) - transformed_odom_msg.pose.covariance = numpy.array(transformed_pose_cov_matrix).reshape(-1,).tolist() + *list(tf.transformations.quaternion_from_matrix( + htm_odom_base_to_base))) + transformed_odom_msg.pose.covariance = numpy.array( + transformed_pose_cov_matrix).reshape(-1,).tolist() + transformed_odom_msg.twist.twist.linear = Vector3( + *list(twist_on_base[:3])) + transformed_odom_msg.twist.twist.angular = Vector3( + *list(twist_on_base[3:6])) + transformed_odom_msg.twist.covariance = numpy.array( + transformed_twist_cov_matrix).reshape(-1,).tolist() # publish self.pub.publish(transformed_odom_msg) if self.publish_tf: - broadcast_transform(self.broadcast, transformed_odom_msg, self.invert_tf) + broadcast_transform( + self.broadcast, transformed_odom_msg, self.invert_tf) def get_htm_camera_to_base(self, stamp): try: (trans, rot) = self.listener.lookupTransform( - self.camera_frame_id, - self.base_frame_id, - stamp) + self.camera_frame_id, + self.base_frame_id, + stamp) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): try: rospy.logwarn( - "[%s] failed to solve camera_to_base tf in %f. Use rospy.Time(0): %s to %s", - rospy.get_name(), - stamp.to_sec(), - self.camera_frame_id, - self.base_frame_id) + "[%s] failed to solve camera_to_base tf in %f. Use rospy.Time(0): %s to %s", + rospy.get_name(), + stamp.to_sec(), + self.camera_frame_id, + self.base_frame_id) (trans, rot) = self.listener.lookupTransform( self.camera_frame_id, self.base_frame_id, @@ -128,10 +171,10 @@ def get_htm_camera_to_base(self, stamp): tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn( - "[%s] failed to solve camera_to_base tf: %s to %s", - rospy.get_name(), - self.camera_frame_id, - self.base_frame_id) + "[%s] failed to solve camera_to_base tf: %s to %s", + rospy.get_name(), + self.camera_frame_id, + self.base_frame_id) return None return make_homogeneous_matrix(trans, rot) From d1fc914990d6768c2c5b210c07b3f607bd37ac11 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 16:28:15 +0900 Subject: [PATCH 07/21] [jsk_robot_startup] add description of CameraBaseToOffset.py --- jsk_robot_common/jsk_robot_startup/README.md | 44 ++++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md index ab240ab2d1..4bbe12ae97 100644 --- a/jsk_robot_common/jsk_robot_startup/README.md +++ b/jsk_robot_common/jsk_robot_startup/README.md @@ -5,6 +5,50 @@ jsk_robot_startup see [lifelog/README.md](lifelog/README.md) +## CameraBaseToOffset.py + +This node publishes transformed odometry topics and TF with raw odometry topic.(e.g. Realsense T265). + + + +### Subscribers + +- `~source_odom` (type: `nav_msgs/Odometry`) + +Raw odometry topic + +### Publishers + +- `~output` (type: `nav_msgs/Odometry`) + +Transformed odometry topic. + +- `/tf` (type: `tf2_msgs/TFMessage`) + +Transformed odometry frames. + +### Parameters + +- `~base_frame_id` (type: `string`, default: `BODY`) + +Frame ID of robot base_link. + +- `~camera_frame_id` (type: `string`, default: `left_camera_optical_frame`) + +Frame ID of base frame of odometry source. + +- `~odom_frame_id` (type: `string`, default: `viso_odom`) + +Frame ID name to be broadcasted as odometry frame + +- `~publish_tf` (type: `bool`, default: `True`) + +If set to true, TF from odometry frame to base_link is broadcasted. + +- `~invert_tf` (type: `bool`, default: `True`) + +If set to true, published tf transformation is not from odom to base, but from base to odom. + ## scripts/email_topic.py This node sends email based on received rostopic (jsk_robot_startup/Email). From 506b342a8fb9fd807a39d5a3fca3015e7c77f35d Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 16:52:35 +0900 Subject: [PATCH 08/21] [jsk_robot_startup] remove tf_duration param of CameraBaseToOffset.py --- jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py | 1 - 1 file changed, 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 65b2a0aa6e..24f66e3c72 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -28,7 +28,6 @@ def __init__(self): # tf parameters self.publish_tf = rospy.get_param("~publish_tf", True) self.invert_tf = rospy.get_param("~invert_tf", True) - self.tf_duration = rospy.get_param("~tf_duration", 1) # other members if self.publish_tf: From e08c8412e1a805e8a8ad33350f16bec7e4174aac Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 17:01:23 +0900 Subject: [PATCH 09/21] [jsk_robot_startup] Update README.md --- jsk_robot_common/jsk_robot_startup/README.md | 26 +++++++++++++++++--- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md index 4bbe12ae97..2d69a5e12a 100644 --- a/jsk_robot_common/jsk_robot_startup/README.md +++ b/jsk_robot_common/jsk_robot_startup/README.md @@ -9,15 +9,29 @@ see [lifelog/README.md](lifelog/README.md) This node publishes transformed odometry topics and TF with raw odometry topic.(e.g. Realsense T265). - +![CameraBaseToOffset_concept](https://user-images.githubusercontent.com/9410362/173223304-70a26fd8-204e-42a5-af11-de95fd098379.png) -### Subscribers +### Concept + +TODO + +### Demo + +Connect Realsense T265 to your machine and run + +```bash +roslaunch jsk_robot_startup some_great_example.launch +``` + +### ROS Interfaces of the node + +#### Subscribers - `~source_odom` (type: `nav_msgs/Odometry`) Raw odometry topic -### Publishers +#### Publishers - `~output` (type: `nav_msgs/Odometry`) @@ -27,7 +41,7 @@ Transformed odometry topic. Transformed odometry frames. -### Parameters +#### Parameters - `~base_frame_id` (type: `string`, default: `BODY`) @@ -49,6 +63,10 @@ If set to true, TF from odometry frame to base_link is broadcasted. If set to true, published tf transformation is not from odom to base, but from base to odom. +### How to use this with your robot + +TODO + ## scripts/email_topic.py This node sends email based on received rostopic (jsk_robot_startup/Email). From 615ce0bda01f65596ce70c432aab2e4f227f84c7 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 18:45:25 +0900 Subject: [PATCH 10/21] [jsk_robot_startup] Update README.md of CameraBaseToOffset --- jsk_robot_common/jsk_robot_startup/README.md | 59 ++++++++++++++++++++ 1 file changed, 59 insertions(+) diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md index 2d69a5e12a..40c5a30d65 100644 --- a/jsk_robot_common/jsk_robot_startup/README.md +++ b/jsk_robot_common/jsk_robot_startup/README.md @@ -13,7 +13,66 @@ This node publishes transformed odometry topics and TF with raw odometry topic.( ### Concept +Coordinates for visual odometry is often different from coordinates robot base frame. To use it as robot odometry, Odometry topic have to be transformed. +`nav_msgs/Odometry` has information below + +- pose +- covariance for pose +- twist +- covariance for twist + +In this section, transformation for each values are described. + +#### Pose transformation + +See figure above and notations below. + +- ${}^\mathrm{Ovis}H_\mathrm{vis}$: pose of $\Sigma_\mathrm{vis}$ in $\Sigma_\mathrm{Ovis}$ (raw odometry, `htm_odom_camera_to_camera` in source code.) +- ${}^\mathrm{Ovis}H_\mathrm{Obase}$: pose of $\Sigma_\mathrm{Obase}$ in $\Sigma_\mathrm{Ovis}$ (`htm_odom_base_to_odom_camera` in source code.) +- ${}^\mathrm{vis}H_\mathrm{base}$: pose of $\Sigma_\mathrm{base}$ in $\Sigma_\mathrm{vis}$ (`htm_camera_to_base` in source code.) + +From these, transformed pose (`htm_odom_base_to_base` in source code) is calculated like below. + +$$ +{}^\mathrm{Obase}H_\mathrm{base} = {}^\mathrm{Ovis}H_\mathrm{Obase}^{-1} {}^\mathrm{Ovis}H_\mathrm{vis} {}^\mathrm{vis}H_\mathrm{base} +$$ + +#### Pose covariance transformation + +TODO + +#### Twist transformation + + + +See the figure above. + +Velocity of base frame can be calculated as + +$$ +\begin{eqnarray} +{}^\mathrm{vis}v_\mathrm{vis} &=& {}^\mathrm{base}R_\mathrm{O} {}^\mathrm{O}v_\mathrm{base}\\ +&=& {}^\mathrm{base}R_\mathrm{O} \frac{d}{dt}({}^\mathrm{O}p_\mathrm{base})\\ +&=& {}^\mathrm{base}R_\mathrm{O} \frac{d}{dt}({}^\mathrm{O}p_\mathrm{vis} + {}^\mathrm{O}R_\mathrm{vis} {}^\mathrm{vis}p_\mathrm{base})\\ +&=& {}^\mathrm{base}R_\mathrm{O} \frac{d}{dt}({}^\mathrm{O}p_\mathrm{vis}) + {}^\mathrm{base}R_\mathrm{O} \frac{d}{dt}({}^\mathrm{O}R_\mathrm{vis}) {}^\mathrm{vis}p_\mathrm{base} + {}^\mathrm{base}R_\mathrm{O} {}^\mathrm{O}R_\mathrm{vis} \frac{d}{dt}({}^\mathrm{vis}p_\mathrm{base}) \\ +&=& {}^\mathrm{base}R_\mathrm{O} {}^\mathrm{O}v_\mathrm{vis} + {}^\mathrm{base}R_\mathrm{O} [{}^\mathrm{O}\omega_\mathrm{vis}\times] {}^\mathrm{O}R_\mathrm{vis} {}^\mathrm{vis}p_\mathrm{base} + {}^\mathrm{base}R_\mathrm{O} {}^\mathrm{O}R_\mathrm{vis} \frac{d}{dt}({}^\mathrm{vis}p_\mathrm{base}) \\ +&=& {}^\mathrm{base}R_\mathrm{vis} {}^\mathrm{vis}v_\mathrm{vis} + {}^\mathrm{base}R_\mathrm{vis} [{}^\mathrm{vis}\omega_\mathrm{vis}\times] {}^\mathrm{vis}p_\mathrm{base} + {}^\mathrm{base}R_\mathrm{vis} \frac{d}{dt}({}^\mathrm{vis}p_\mathrm{base}) +\end{eqnarray} +$$ + +And it is assumed that $\frac{d}{dt}({}^\mathrm{vis}p_\mathrm{base}) \simeq 0$. so + +$$ +{}^\mathrm{vis}v_\mathrm{vis} = {}^\mathrm{base}R_\mathrm{vis} {}^\mathrm{vis}v_\mathrm{vis} + {}^\mathrm{base}R_\mathrm{vis} [{}^\mathrm{vis}\omega_\mathrm{vis}\times] {}^\mathrm{vis}p_\mathrm{base} +$$ + +And angular velocity of base frame can be calculated as + +$$ +\begin{eqnarray} TODO +\end{eqnarray} +$$ ### Demo From 1fe30a2f8f378ad092fbee8ef4e30f02107a3245 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 20:19:47 +0900 Subject: [PATCH 11/21] [jsk_robot_startup] add comment to CameraToBaseOffset.py --- .../jsk_robot_startup/scripts/CameraToBaseOffset.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py index 24f66e3c72..63abafba4d 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -80,6 +80,10 @@ def source_odom_callback(self, msg): self.htm_odom_base_to_odom_camera, htm_odom_camera_to_base) # calculate transformed pose covariance + # + # NOTICE: transformation of covariance for rotation + # seems wrong + # raw_pose_cov_matrix = numpy.matrix( msg.pose.covariance).reshape(6, 6) rotation_matrix = htm_odom_base_to_base[:3, :3] From e0dda1ef4f9984573314c9d039934fbddc65d323 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 21:32:50 +0900 Subject: [PATCH 12/21] [jsk_robot_startup] update README.md for CameraBaseToOffset --- jsk_robot_common/jsk_robot_startup/README.md | 53 ++++++++++++++++++++ 1 file changed, 53 insertions(+) diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md index 40c5a30d65..3062bda8e0 100644 --- a/jsk_robot_common/jsk_robot_startup/README.md +++ b/jsk_robot_common/jsk_robot_startup/README.md @@ -39,7 +39,60 @@ $$ #### Pose covariance transformation +When $y = f(x)$, it is approximated near $x = x_0$ as $y = (\left.\frac{\partial}{\partial x}f\right|_{x=x_0}) x$. +So relationship of covariance matricies $C(x)$ for $x$ and $C(y)$ for $y$ when $x=x_0$ is like + +$$ +C(y) = F C(x) F^\mathrm{T} +$$ + +where $F=\left.\frac{\partial}{\partial x}f\right|_{x=x_0}$. + +Here, we have already got transformation of position and rotation. + +$$ +\begin{align*} + {}^\mathrm{Obase}R_\mathrm{base} = {}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}R_\mathrm{vis} {}^\mathrm{vis}R_\mathrm{base} \\ + {}^\mathrm{Obase}p_\mathrm{base} = {}^\mathrm{Obase}p_\mathrm{Ovis} + {}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}p_\mathrm{vis} + {}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}R_\mathrm{vis} {}^\mathrm{vis}p_\mathrm{base} +\end{align*} +$$ + +Since rotation covariance is calculated in euler angles in `geometry_msgs/PoseWithCovariance`, rotation transformation is + +$$ +\begin{align*} + {}^\mathrm{Obase}rot_\mathrm{base} = f({}^\mathrm{Ovis}p_\mathrm{vis}, {}^\mathrm{Ovis}rot_\mathrm{vis}) \\ + = convert^{-1}({}^\mathrm{Obase}R_\mathrm{base}) \\ + = convert^{-1}({}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}R_\mathrm{vis} {}^\mathrm{vis}R_\mathrm{base}) \\ + = convert^{-1}({}^\mathrm{Obase}R_\mathrm{Ovis} convert({}^\mathrm{Ovis}rot_\mathrm{vis}) {}^\mathrm{vis}R_\mathrm{base}) +\end{align*} +$$ + +where $convert(rot)$ is a function to convert rotation vector in euler angle to rotation matrix representation. + +and transform function for position is + +$$ +\begin{align*} +{}^\mathrm{Obase}p_\mathrm{base} = g({}^\mathrm{Ovis}p_\mathrm{vis}, {}^\mathrm{Ovis}rot_\mathrm{vis}) = {}^\mathrm{Obase}p_\mathrm{Ovis} + {}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}p_\mathrm{vis} + {}^\mathrm{Obase}R_\mathrm{Ovis} convert({}^\mathrm{Ovis}rot_\mathrm{vis}) {}^\mathrm{vis}p_\mathrm{base} +\end{align*} +$$ + +So + +$$ +\begin{eqnarray} TODO +\end{eqnarray} +$$ + +And then we can transform covariance matrix + +$$ +\begin{eqnarray} +TODO +\end{eqnarray} +$$ #### Twist transformation From bb2bc906fee41331eec96ad46107be8885ca23b0 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 21:39:42 +0900 Subject: [PATCH 13/21] [jsk_robot_startup] update README.md for CameraBaseToOffset --- jsk_robot_common/jsk_robot_startup/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md index 3062bda8e0..0b704e3056 100644 --- a/jsk_robot_common/jsk_robot_startup/README.md +++ b/jsk_robot_common/jsk_robot_startup/README.md @@ -78,11 +78,11 @@ $$ \end{align*} $$ -So +So when $pose = (p, rot)^T$, $$ \begin{eqnarray} -TODO +{}^\mathrm{Obase}pose_\mathrm{base} = T({}^\mathrm{Ovis}pose_\mathrm{vis}} = (g({}^\mathrm{Ovis}pose_\mathrm{vis}), f({}^\mathrm{Ovis}pose_\mathrm{vis})^T \end{eqnarray} $$ @@ -90,7 +90,7 @@ And then we can transform covariance matrix $$ \begin{eqnarray} -TODO +C({}^\mathrm{Obase}pose_\mathrm{base}) = (\left.\frac{\partial}{\partial pose}T\right|_{pose=pose_0}) C({}^\mathrm{Ovis}pose_\mathrm{vis}) (\left.\frac{\partial}{\partial pose}T\right|_{pose=pose_0})^T \end{eqnarray} $$ From 9010cab6dd626370d306625b7716e55ddbf77a61 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 21:42:16 +0900 Subject: [PATCH 14/21] [jsk_robot_startup] fix CameraBaseToOffset docs --- jsk_robot_common/jsk_robot_startup/README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md index 0b704e3056..4449bcbcf4 100644 --- a/jsk_robot_common/jsk_robot_startup/README.md +++ b/jsk_robot_common/jsk_robot_startup/README.md @@ -82,7 +82,7 @@ So when $pose = (p, rot)^T$, $$ \begin{eqnarray} -{}^\mathrm{Obase}pose_\mathrm{base} = T({}^\mathrm{Ovis}pose_\mathrm{vis}} = (g({}^\mathrm{Ovis}pose_\mathrm{vis}), f({}^\mathrm{Ovis}pose_\mathrm{vis})^T +{}^\mathrm{Obase}pose_\mathrm{base} = T({}^\mathrm{Ovis}pose_\mathrm{vis}) = (g({}^\mathrm{Ovis}pose_\mathrm{vis}), f({}^\mathrm{Ovis}pose_\mathrm{vis})^T \end{eqnarray} $$ @@ -90,10 +90,12 @@ And then we can transform covariance matrix $$ \begin{eqnarray} -C({}^\mathrm{Obase}pose_\mathrm{base}) = (\left.\frac{\partial}{\partial pose}T\right|_{pose=pose_0}) C({}^\mathrm{Ovis}pose_\mathrm{vis}) (\left.\frac{\partial}{\partial pose}T\right|_{pose=pose_0})^T +C({}^\mathrm{Obase}pose_\mathrm{base}) = T_0 C({}^\mathrm{Ovis}pose_\mathrm{vis}) T_0^T \end{eqnarray} $$ +Where $T_0=\left.\frac{\partial}{\partial pose}T\right|_{pose=pose_0}$ + #### Twist transformation From 36863c06053b3c8bc26c23c45ba720c45c566cbf Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 12 Jun 2022 22:36:21 +0900 Subject: [PATCH 15/21] [jsk_robot_startup] update CameraBaseToOffset docs --- jsk_robot_common/jsk_robot_startup/README.md | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md index 4449bcbcf4..443023fdda 100644 --- a/jsk_robot_common/jsk_robot_startup/README.md +++ b/jsk_robot_common/jsk_robot_startup/README.md @@ -125,10 +125,19 @@ And angular velocity of base frame can be calculated as $$ \begin{eqnarray} -TODO +{}^\mathrm{base}\omega_\mathrm{base} = {}^\mathrm{base}R_\mathrm{vis} {}^\mathrm{vis}\omega_\mathrm{vis} \end{eqnarray} $$ + + +#### Twist covariance transformation + +TODO + ### Demo Connect Realsense T265 to your machine and run From b1765f5a7beb5d455adb73808add48ec4e2bb105 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 13 Jun 2022 07:39:48 +0900 Subject: [PATCH 16/21] [jsk_robot_startup] fix docs for CameraBaseToOffset.py --- jsk_robot_common/jsk_robot_startup/README.md | 76 +++++++++++++------- 1 file changed, 51 insertions(+), 25 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md index 443023fdda..8da04e80a8 100644 --- a/jsk_robot_common/jsk_robot_startup/README.md +++ b/jsk_robot_common/jsk_robot_startup/README.md @@ -34,7 +34,9 @@ See figure above and notations below. From these, transformed pose (`htm_odom_base_to_base` in source code) is calculated like below. $$ -{}^\mathrm{Obase}H_\mathrm{base} = {}^\mathrm{Ovis}H_\mathrm{Obase}^{-1} {}^\mathrm{Ovis}H_\mathrm{vis} {}^\mathrm{vis}H_\mathrm{base} +\begin{eqnarray} + {}^\mathrm{Obase}H_\mathrm{base} = {}^\mathrm{Ovis}H_\mathrm{Obase}^{-1} {}^\mathrm{Ovis}H_\mathrm{vis} {}^\mathrm{vis}H_\mathrm{base} +\end{eqnarray} $$ #### Pose covariance transformation @@ -43,7 +45,9 @@ When $y = f(x)$, it is approximated near $x = x_0$ as $y = (\left.\frac{\partial So relationship of covariance matricies $C(x)$ for $x$ and $C(y)$ for $y$ when $x=x_0$ is like $$ -C(y) = F C(x) F^\mathrm{T} +\begin{eqnarray} + C(y) = F C(x) F^\mathrm{T} +\end{eqnarray} $$ where $F=\left.\frac{\partial}{\partial x}f\right|_{x=x_0}$. @@ -51,21 +55,23 @@ where $F=\left.\frac{\partial}{\partial x}f\right|_{x=x_0}$. Here, we have already got transformation of position and rotation. $$ -\begin{align*} - {}^\mathrm{Obase}R_\mathrm{base} = {}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}R_\mathrm{vis} {}^\mathrm{vis}R_\mathrm{base} \\ - {}^\mathrm{Obase}p_\mathrm{base} = {}^\mathrm{Obase}p_\mathrm{Ovis} + {}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}p_\mathrm{vis} + {}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}R_\mathrm{vis} {}^\mathrm{vis}p_\mathrm{base} -\end{align*} +\begin{eqnarray} + {}^\mathrm{Obase}R_\mathrm{base} &=& {}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}R_\mathrm{vis} {}^\mathrm{vis}R_\mathrm{base} \\ + {}^\mathrm{Obase}p_\mathrm{base} &=& {}^\mathrm{Obase}p_\mathrm{Ovis} + + {}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}p_\mathrm{vis} + + {}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}R_\mathrm{vis} {}^\mathrm{vis}p_\mathrm{base} +\end{eqnarray} $$ Since rotation covariance is calculated in euler angles in `geometry_msgs/PoseWithCovariance`, rotation transformation is $$ -\begin{align*} - {}^\mathrm{Obase}rot_\mathrm{base} = f({}^\mathrm{Ovis}p_\mathrm{vis}, {}^\mathrm{Ovis}rot_\mathrm{vis}) \\ - = convert^{-1}({}^\mathrm{Obase}R_\mathrm{base}) \\ - = convert^{-1}({}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}R_\mathrm{vis} {}^\mathrm{vis}R_\mathrm{base}) \\ - = convert^{-1}({}^\mathrm{Obase}R_\mathrm{Ovis} convert({}^\mathrm{Ovis}rot_\mathrm{vis}) {}^\mathrm{vis}R_\mathrm{base}) -\end{align*} +\begin{eqnarray} + {}^\mathrm{Obase}rot_\mathrm{base} &=& f({}^\mathrm{Ovis}p_\mathrm{vis}, {}^\mathrm{Ovis}rot_\mathrm{vis}) \\ + &=& convert^{-1}({}^\mathrm{Obase}R_\mathrm{base}) \\ + &=& convert^{-1}({}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}R_\mathrm{vis} {}^\mathrm{vis}R_\mathrm{base}) \\ + &=& convert^{-1}({}^\mathrm{Obase}R_\mathrm{Ovis} convert({}^\mathrm{Ovis}rot_\mathrm{vis}) {}^\mathrm{vis}R_\mathrm{base}) +\end{eqnarray} $$ where $convert(rot)$ is a function to convert rotation vector in euler angle to rotation matrix representation. @@ -73,16 +79,24 @@ where $convert(rot)$ is a function to convert rotation vector in euler angle to and transform function for position is $$ -\begin{align*} -{}^\mathrm{Obase}p_\mathrm{base} = g({}^\mathrm{Ovis}p_\mathrm{vis}, {}^\mathrm{Ovis}rot_\mathrm{vis}) = {}^\mathrm{Obase}p_\mathrm{Ovis} + {}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}p_\mathrm{vis} + {}^\mathrm{Obase}R_\mathrm{Ovis} convert({}^\mathrm{Ovis}rot_\mathrm{vis}) {}^\mathrm{vis}p_\mathrm{base} -\end{align*} +\begin{eqnarray} + {}^\mathrm{Obase}p_\mathrm{base} &=& g({}^\mathrm{Ovis}p_\mathrm{vis}, {}^\mathrm{Ovis}rot_\mathrm{vis}) \\ + &=& {}^\mathrm{Obase}p_\mathrm{Ovis} + + {}^\mathrm{Obase}R_\mathrm{Ovis} {}^\mathrm{Ovis}p_\mathrm{vis} + + {}^\mathrm{Obase}R_\mathrm{Ovis} convert({}^\mathrm{Ovis}rot_\mathrm{vis}) {}^\mathrm{vis}p_\mathrm{base} +\end{eqnarray} $$ So when $pose = (p, rot)^T$, $$ \begin{eqnarray} -{}^\mathrm{Obase}pose_\mathrm{base} = T({}^\mathrm{Ovis}pose_\mathrm{vis}) = (g({}^\mathrm{Ovis}pose_\mathrm{vis}), f({}^\mathrm{Ovis}pose_\mathrm{vis})^T + {}^\mathrm{Obase}pose_\mathrm{base} = T({}^\mathrm{Ovis}pose_\mathrm{vis}) = \left( + \begin{array}{c} + g({}^\mathrm{Ovis}pose_\mathrm{vis}) \\ + f({}^\mathrm{Ovis}pose_\mathrm{vis}) + \end{array} + \right) \end{eqnarray} $$ @@ -90,7 +104,7 @@ And then we can transform covariance matrix $$ \begin{eqnarray} -C({}^\mathrm{Obase}pose_\mathrm{base}) = T_0 C({}^\mathrm{Ovis}pose_\mathrm{vis}) T_0^T + C({}^\mathrm{Obase}pose_\mathrm{base}) = T_0 C({}^\mathrm{Ovis}pose_\mathrm{vis}) T_0^\mathrm{T} \end{eqnarray} $$ @@ -106,31 +120,43 @@ Velocity of base frame can be calculated as $$ \begin{eqnarray} -{}^\mathrm{vis}v_\mathrm{vis} &=& {}^\mathrm{base}R_\mathrm{O} {}^\mathrm{O}v_\mathrm{base}\\ -&=& {}^\mathrm{base}R_\mathrm{O} \frac{d}{dt}({}^\mathrm{O}p_\mathrm{base})\\ -&=& {}^\mathrm{base}R_\mathrm{O} \frac{d}{dt}({}^\mathrm{O}p_\mathrm{vis} + {}^\mathrm{O}R_\mathrm{vis} {}^\mathrm{vis}p_\mathrm{base})\\ -&=& {}^\mathrm{base}R_\mathrm{O} \frac{d}{dt}({}^\mathrm{O}p_\mathrm{vis}) + {}^\mathrm{base}R_\mathrm{O} \frac{d}{dt}({}^\mathrm{O}R_\mathrm{vis}) {}^\mathrm{vis}p_\mathrm{base} + {}^\mathrm{base}R_\mathrm{O} {}^\mathrm{O}R_\mathrm{vis} \frac{d}{dt}({}^\mathrm{vis}p_\mathrm{base}) \\ -&=& {}^\mathrm{base}R_\mathrm{O} {}^\mathrm{O}v_\mathrm{vis} + {}^\mathrm{base}R_\mathrm{O} [{}^\mathrm{O}\omega_\mathrm{vis}\times] {}^\mathrm{O}R_\mathrm{vis} {}^\mathrm{vis}p_\mathrm{base} + {}^\mathrm{base}R_\mathrm{O} {}^\mathrm{O}R_\mathrm{vis} \frac{d}{dt}({}^\mathrm{vis}p_\mathrm{base}) \\ -&=& {}^\mathrm{base}R_\mathrm{vis} {}^\mathrm{vis}v_\mathrm{vis} + {}^\mathrm{base}R_\mathrm{vis} [{}^\mathrm{vis}\omega_\mathrm{vis}\times] {}^\mathrm{vis}p_\mathrm{base} + {}^\mathrm{base}R_\mathrm{vis} \frac{d}{dt}({}^\mathrm{vis}p_\mathrm{base}) + {}^\mathrm{base}v_\mathrm{base} &=& {}^\mathrm{base}R_\mathrm{O} {}^\mathrm{O}v_\mathrm{base}\\ + &=& {}^\mathrm{base}R_\mathrm{O} \frac{d}{dt}({}^\mathrm{O}p_\mathrm{base})\\ + &=& {}^\mathrm{base}R_\mathrm{O} \frac{d}{dt}({}^\mathrm{O}p_\mathrm{vis} + + {}^\mathrm{O}R_\mathrm{vis} {}^\mathrm{vis}p_\mathrm{base})\\ + &=& {}^\mathrm{base}R_\mathrm{O} \frac{d}{dt}({}^\mathrm{O}p_\mathrm{vis}) + + {}^\mathrm{base}R_\mathrm{O} \frac{d}{dt}({}^\mathrm{O}R_\mathrm{vis}) {}^\mathrm{vis}p_\mathrm{base} + + {}^\mathrm{base}R_\mathrm{O} {}^\mathrm{O}R_\mathrm{vis} \frac{d}{dt}({}^\mathrm{vis}p_\mathrm{base}) \\ + &=& {}^\mathrm{base}R_\mathrm{O} {}^\mathrm{O}v_\mathrm{vis} + + {}^\mathrm{base}R_\mathrm{O} [{}^\mathrm{O}\omega_\mathrm{vis}\times] {}^\mathrm{O}R_\mathrm{vis} {}^\mathrm{vis}p_\mathrm{base} + + {}^\mathrm{base}R_\mathrm{O} {}^\mathrm{O}R_\mathrm{vis} \frac{d}{dt}({}^\mathrm{vis}p_\mathrm{base}) \\ + &=& {}^\mathrm{base}R_\mathrm{vis} {}^\mathrm{vis}v_\mathrm{vis} + + {}^\mathrm{base}R_\mathrm{vis} [{}^\mathrm{vis}\omega_\mathrm{vis}\times] {}^\mathrm{vis}p_\mathrm{base} + + {}^\mathrm{base}R_\mathrm{vis} \frac{d}{dt}({}^\mathrm{vis}p_\mathrm{base}) \end{eqnarray} $$ And it is assumed that $\frac{d}{dt}({}^\mathrm{vis}p_\mathrm{base}) \simeq 0$. so $$ -{}^\mathrm{vis}v_\mathrm{vis} = {}^\mathrm{base}R_\mathrm{vis} {}^\mathrm{vis}v_\mathrm{vis} + {}^\mathrm{base}R_\mathrm{vis} [{}^\mathrm{vis}\omega_\mathrm{vis}\times] {}^\mathrm{vis}p_\mathrm{base} +\begin{eqnarray} + {}^\mathrm{base}v_\mathrm{base} = {}^\mathrm{base}R_\mathrm{vis} {}^\mathrm{vis}v_\mathrm{vis} + + {}^\mathrm{base}R_\mathrm{vis} [{}^\mathrm{vis}\omega_\mathrm{vis}\times] {}^\mathrm{vis}p_\mathrm{base} +\end{eqnarray} $$ And angular velocity of base frame can be calculated as $$ \begin{eqnarray} -{}^\mathrm{base}\omega_\mathrm{base} = {}^\mathrm{base}R_\mathrm{vis} {}^\mathrm{vis}\omega_\mathrm{vis} + {}^\mathrm{base}\omega_\mathrm{base} = {}^\mathrm{base}R_\mathrm{vis} {}^\mathrm{vis}\omega_\mathrm{vis} \end{eqnarray} $$ From 7289f567e36e9675988150c4b9ef4323bf226f77 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 13 Jun 2022 07:47:51 +0900 Subject: [PATCH 17/21] [jsk_robot_startup] update docs for CameraBaseToOffset.py --- jsk_robot_common/jsk_robot_startup/README.md | 22 +++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md index 8da04e80a8..f9e22fc729 100644 --- a/jsk_robot_common/jsk_robot_startup/README.md +++ b/jsk_robot_common/jsk_robot_startup/README.md @@ -162,7 +162,27 @@ $$ #### Twist covariance transformation -TODO +When $\nu = \left(v,\ \omega\right)^\mathrm{T}$, transformation of $\nu$ is like + +$$ +\begin{eqnarray} + {}^\mathrm{base}\nu_\mathrm{base} = A {}^\mathrm{vis}\nu_\mathrm{vis} \\ + where\ A = \left( + \begin{array}{c} + a & b \\ + c & d + \end{array} + \right) +\end{eqnarray} +$$ + +So, covariance transformation is + +$$ +\begin{eqnarray} + C({}^\mathrm{base}\nu_\mathrm{base}) = A C({}^\mathrm{vis}\nu_\mathrm{vis}) A^\mathrm{T} +\end{eqnarray} +$$ ### Demo From f97f694d6ae148f66ef845b85116a978dfb63230 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 13 Jun 2022 07:56:57 +0900 Subject: [PATCH 18/21] [jsk_robot_startup] add enable_covariance param to CameraBaseToOffset.py --- .../scripts/CameraToBaseOffset.py | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py index 63abafba4d..9df1a83cc9 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -29,6 +29,9 @@ def __init__(self): self.publish_tf = rospy.get_param("~publish_tf", True) self.invert_tf = rospy.get_param("~invert_tf", True) + # other parameters + self.enable_covariance = rospy.get_param("~enable_covariance", False) + # other members if self.publish_tf: self.broadcast = tf.TransformBroadcaster() @@ -127,22 +130,28 @@ def source_odom_callback(self, msg): twist_transform_matrix)) # make odometry msg. - transformed_odom_msg = copy.deepcopy(msg) + transformed_odom_msg = Odometry() + transformed_odom_msg.header.stamp = msg.header.stamp transformed_odom_msg.header.frame_id = self.odom_frame_id transformed_odom_msg.child_frame_id = self.base_frame_id + # Pose transformed_odom_msg.pose.pose.position = Point( *list(htm_odom_base_to_base[:3, 3])) transformed_odom_msg.pose.pose.orientation = Quaternion( *list(tf.transformations.quaternion_from_matrix( htm_odom_base_to_base))) - transformed_odom_msg.pose.covariance = numpy.array( - transformed_pose_cov_matrix).reshape(-1,).tolist() + # Twist transformed_odom_msg.twist.twist.linear = Vector3( *list(twist_on_base[:3])) transformed_odom_msg.twist.twist.angular = Vector3( *list(twist_on_base[3:6])) - transformed_odom_msg.twist.covariance = numpy.array( - transformed_twist_cov_matrix).reshape(-1,).tolist() + if self.enable_covariance: + # Pose covariance + transformed_odom_msg.pose.covariance = numpy.array( + transformed_pose_cov_matrix).reshape(-1,).tolist() + # Twist covariance + transformed_odom_msg.twist.covariance = numpy.array( + transformed_twist_cov_matrix).reshape(-1,).tolist() # publish self.pub.publish(transformed_odom_msg) From 951fa1bced906ead3576d5045a6b5a618d7f2036 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 13 Jun 2022 11:27:14 +0900 Subject: [PATCH 19/21] [jsk_robot_startup] Update docs of CameraToBaseOffset --- jsk_robot_common/jsk_robot_startup/README.md | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md index f9e22fc729..91075e526a 100644 --- a/jsk_robot_common/jsk_robot_startup/README.md +++ b/jsk_robot_common/jsk_robot_startup/README.md @@ -9,7 +9,7 @@ see [lifelog/README.md](lifelog/README.md) This node publishes transformed odometry topics and TF with raw odometry topic.(e.g. Realsense T265). -![CameraBaseToOffset_concept](https://user-images.githubusercontent.com/9410362/173223304-70a26fd8-204e-42a5-af11-de95fd098379.png) +![CameraToBaseOffsetConceptPose](https://user-images.githubusercontent.com/9410362/173268226-3707a424-0099-42fb-a595-f61161b73a7b.svg) ### Concept @@ -112,7 +112,7 @@ Where $T_0=\left.\frac{\partial}{\partial pose}T\right|_{pose=pose_0}$ #### Twist transformation - +![CameraToBaseOffsetConceptTwist](https://user-images.githubusercontent.com/9410362/173268459-e8fe9b14-25f6-47ee-9be1-b55e6181f432.svg) See the figure above. @@ -189,7 +189,7 @@ $$ Connect Realsense T265 to your machine and run ```bash -roslaunch jsk_robot_startup some_great_example.launch +roslaunch jsk_robot_startup sample_camera_to_base_offset.launch ``` ### ROS Interfaces of the node @@ -232,6 +232,10 @@ If set to true, TF from odometry frame to base_link is broadcasted. If set to true, published tf transformation is not from odom to base, but from base to odom. +- `~enable_covariance` (type: `bool`, default: `True`) + +If set to true, pose covariance and twist covariance are transformed and published. otherwise, covariances of output odometry is set to zero. + ### How to use this with your robot TODO From 75d2bc7a10eb66ada5f4560f853ea21929f4f496 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 13 Jun 2022 08:49:29 +0900 Subject: [PATCH 20/21] [jsk_robot_startup] add 2d_mode to CameraToBaseOffset --- .../scripts/CameraToBaseOffset.py | 27 +++++++++++++++---- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py index 9df1a83cc9..63d9fa4027 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py @@ -30,6 +30,7 @@ def __init__(self): self.invert_tf = rospy.get_param("~invert_tf", True) # other parameters + self.twod_mode = rospy.get_param("~2d_mode", False) self.enable_covariance = rospy.get_param("~enable_covariance", False) # other members @@ -126,8 +127,8 @@ def source_odom_callback(self, msg): raw_twist_cov_matrix = numpy.matrix( msg.twist.covariance).reshape(6, 6) transformed_twist_cov_matrix = (twist_transform_matrix.T).dot( - raw_twist_cov_matrix.dot( - twist_transform_matrix)) + raw_twist_cov_matrix.dot( + twist_transform_matrix)) # make odometry msg. transformed_odom_msg = Odometry() @@ -139,7 +140,7 @@ def source_odom_callback(self, msg): *list(htm_odom_base_to_base[:3, 3])) transformed_odom_msg.pose.pose.orientation = Quaternion( *list(tf.transformations.quaternion_from_matrix( - htm_odom_base_to_base))) + htm_odom_base_to_base))) # Twist transformed_odom_msg.twist.twist.linear = Vector3( *list(twist_on_base[:3])) @@ -148,10 +149,26 @@ def source_odom_callback(self, msg): if self.enable_covariance: # Pose covariance transformed_odom_msg.pose.covariance = numpy.array( - transformed_pose_cov_matrix).reshape(-1,).tolist() + transformed_pose_cov_matrix).reshape(-1,).tolist() # Twist covariance transformed_odom_msg.twist.covariance = numpy.array( - transformed_twist_cov_matrix).reshape(-1,).tolist() + transformed_twist_cov_matrix).reshape(-1,).tolist() + if self.twod_mode: + transformed_odom_msg.pose.pose.position.x = 0 + transformed_odom_msg.pose.pose.position.y = 0 + q_2d = numpy.array([ + 0, + 0, + transformed_odom_msg.pose.pose.orientation.z, + transformed_odom_msg.pose.pose.orientation.w]) + q_2d /= numpy.linalg.norm(q_2d) + transformed_odom_msg.pose.pose.orientation.x = q_2d[0] + transformed_odom_msg.pose.pose.orientation.y = q_2d[1] + transformed_odom_msg.pose.pose.orientation.z = q_2d[2] + transformed_odom_msg.pose.pose.orientation.w = q_2d[3] + transformed_odom_msg.twist.twist.linear.z = 0 + transformed_odom_msg.twist.twist.angular.x = 0 + transformed_odom_msg.twist.twist.angular.y = 0 # publish self.pub.publish(transformed_odom_msg) From 3bcfd36ed278b31c84e85b30399d671309b4c37b Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 13 Jun 2022 13:13:15 +0900 Subject: [PATCH 21/21] [jsk_robot_startup] add sample_camera_base_to_offset.launch --- .../sample_camera_base_to_offset.launch | 85 +++++++++++++++++++ 1 file changed, 85 insertions(+) create mode 100644 jsk_robot_common/jsk_robot_startup/launch/sample_camera_base_to_offset.launch diff --git a/jsk_robot_common/jsk_robot_startup/launch/sample_camera_base_to_offset.launch b/jsk_robot_common/jsk_robot_startup/launch/sample_camera_base_to_offset.launch new file mode 100644 index 0000000000..21d223306c --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/launch/sample_camera_base_to_offset.launch @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + base_frame_id: $(arg base_frame_id) + camera_frame_id: $(arg camera_frame_id) + odom_frame_id: $(arg odom_frame_id) + publish_tf: $(arg publish_tf) + invert_tf: $(arg invert_tf) + enable_covariance: $(arg enable_covariance) + + + + + + /tracking_module/enable_mapping: false + /tracking_module/enable_relocalization: false + /tracking_module/enable_pose_jumping: false + /tracking_module/enable_dynamic_calibration: true + /tracking_module/enable_map_preservation: false + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +