def from_dict(self, in_dict):
"""
Sets values parsed from a given dictionary.
:param in_dict: input dictionary.
:type in_dict: dict[string, string|dict[string,float]]
:rtype: None
"""
self.eye_on_hand = in_dict['eye_on_hand']
self.transformation = TransformStamped(
child_frame_id=in_dict['tracking_base_frame'],
transform=Transform(
Vector3(in_dict['transformation']['x'],
in_dict['transformation']['y'],
in_dict['transformation']['z']),
Quaternion(in_dict['transformation']['qx'],
in_dict['transformation']['qy'],
in_dict['transformation']['qz'],
in_dict['transformation']['qw'])
)
)
if self.eye_on_hand:
self.transformation.header.frame_id = in_dict['robot_effector_frame']
else:
self.transformation.header.frame_id = in_dict['robot_base_frame']
python类TransformStamped()的实例源码
def get_navigation_tf(self, navigation_pose):
navigation_tf = TransformStamped()
navigation_tf.header.frame_id = "/map"
navigation_tf.header.stamp = rospy.Time.now()
navigation_tf.child_frame_id = "/odom"
navigation_tf.transform.translation .x = navigation_pose.x
navigation_tf.transform.translation .y = navigation_pose.y
navigation_tf.transform.rotation = self.get_ros_quaternion(
almath.Quaternion_fromAngleAndAxisRotation(navigation_pose.theta, 0, 0, 1))
return navigation_tf
def _send_transform(self, trans, rotq, i):
t = TransformStamped()
t.header.stamp = rospy.Time().now()
t.header.frame_id = "velodyne"
t.child_frame_id = 'obs%d' % i
t.transform.translation.x = trans[0]
t.transform.translation.y = trans[1]
t.transform.translation.z = trans[2]
t.transform.rotation.x = rotq[0]
t.transform.rotation.y = rotq[1]
t.transform.rotation.z = rotq[2]
t.transform.rotation.w = rotq[3]
self.broadcaster.sendTransform(t)
def __init__(self,
eye_on_hand=False,
robot_base_frame=None,
robot_effector_frame=None,
tracking_base_frame=None,
transformation=None):
"""
Creates a HandeyeCalibration object.
:param eye_on_hand: if false, it is a eye-on-base calibration
:type eye_on_hand: bool
:param robot_base_frame: needed only for eye-on-base calibrations: robot base link tf name
:type robot_base_frame: string
:param robot_effector_frame: needed only for eye-on-hand calibrations: robot tool tf name
:type robot_effector_frame: string
:param tracking_base_frame: tracking system tf name
:type tracking_base_frame: string
:param transformation: transformation between optical origin and base/tool robot frame as tf tuple
:type transformation: ((float, float, float), (float, float, float, float))
:return: a HandeyeCalibration object
:rtype: easy_handeye.handeye_calibration.HandeyeCalibration
"""
if transformation is None:
transformation = ((0, 0, 0), (0, 0, 0, 1))
self.eye_on_hand = eye_on_hand
"""
if false, it is a eye-on-base calibration
:type: bool
"""
self.transformation = TransformStamped(transform=Transform(
Vector3(*transformation[0]), Quaternion(*transformation[1])))
"""
transformation between optical origin and base/tool robot frame
:type: geometry_msgs.msg.TransformedStamped
"""
# tf names
if self.eye_on_hand:
self.transformation.header.frame_id = robot_effector_frame
else:
self.transformation.header.frame_id = robot_base_frame
self.transformation.child_frame_id = tracking_base_frame
self.filename = HandeyeCalibration.DIRECTORY + '/' + rospy.get_namespace().rstrip('/').split('/')[-1] + '.yaml'
def __init__(self):
self.rate = rospy.get_param("~rate", 20.0)
self.period = 1.0 / self.rate
# angular mode maps angular z directly to steering angle
# (adjusted appropriately)
# non-angular mode is somewhat suspect, but it turns
# a linear y into a command to turn just so that the
# achieved linear x and y match the desired, though
# the vehicle has to turn to do so.
# Non-angular mode is not yet supported.
self.angular_mode = rospy.get_param("~angular_mode", True)
# Not used yet
# self.tf_buffer = tf2_ros.Buffer()
# self.tf = tf2_ros.TransformListener(self.tf_buffer)
# broadcast odometry
self.br = tf2_ros.TransformBroadcaster()
self.ts = TransformStamped()
self.ts.header.frame_id = "map"
self.ts.child_frame_id = "base_link"
self.ts.transform.rotation.w = 1.0
self.angle = 0
# The cmd_vel is assumed to be in the base_link frame centered
# on the middle of the two driven wheels
# This is half the distance between the two drive wheels
self.base_radius = rospy.get_param("~base_radius", 0.06)
self.wheel_radius = rospy.get_param("~wheel_radius", 0.04)
self.left_wheel_joint = rospy.get_param("~left_wheel_joint", "wheel_front_left_joint")
self.right_wheel_joint = rospy.get_param("~right_wheel_joint", "wheel_front_right_joint")
self.point_pub = rospy.Publisher("cmd_vel_spin_center", PointStamped, queue_size=1)
self.joint_pub = {}
self.joint_pub['left'] = rospy.Publisher("front_left/joint_state",
JointState, queue_size=1)
self.joint_pub['right'] = rospy.Publisher("front_right/joint_state",
JointState, queue_size=1)
# TODO(lucasw) is there a way to get TwistStamped out of standard
# move_base publishers?
# TODO(lucasw) make this more generic, read in a list of any number of wheels
# the requirement is that that all have to be aligned, and also need
# a set spin center.
self.ind = {}
self.joint_state = {}
self.joint_state['left'] = JointState()
self.joint_state['left'].name.append(self.left_wheel_joint)
self.joint_state['left'].position.append(0.0)
self.joint_state['left'].velocity.append(0.0)
self.joint_state['right'] = JointState()
self.joint_state['right'].name.append(self.right_wheel_joint)
self.joint_state['right'].position.append(0.0)
self.joint_state['right'].velocity.append(0.0)
self.cmd_vel = Twist()
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=2)
self.timer = rospy.Timer(rospy.Duration(self.period), self.update)
