def _bs_input_callback(self, bs_input):
dt = self.timer.dt_sec
vdt = self._input.twist.linear.x * dt
wdt = self._input.twist.angular.z * dt
theta = euler_from_rosquaternion(self._mobile_base_tf_for_calc_from_localized_origin.transform.rotation)[0]
translation = add_vectors([self._mobile_base_tf_for_calc_from_localized_origin.transform.translation,
Vector3(x=vdt * np.cos(theta), y=vdt * np.sin(theta))])
rotation = multiply_ros_quaternion([self._mobile_base_tf_for_calc_from_localized_origin.transform.rotation,
Quaternion(z=np.sin(wdt / 2.0), w=np.cos(wdt / 2.0))])
self._mobile_base_tf_for_calc_from_localized_origin.header.stamp = rospy.Time.now()
self._mobile_base_tf_for_calc_from_localized_origin.transform.translation = translation
self._mobile_base_tf_for_calc_from_localized_origin.transform.rotation = rotation
self._localized_point_tf_for_calc_from_mobile_base = update_tf(self._tf_buffer,
self._localized_point_tf_for_calc_from_mobile_base)
self._input = bs_input
self.timer.update()
def update_input(self):
self.arm_base_tf = update_tf(self._tf_buffer, self.arm_base_tf)
# TODO: diff_poses may be wrong
ee_pose_from_arm_base = diff_poses(self.ref_pose_from_origin_frame, self.arm_base_tf)
return PoseStamped(header=Header(stamp=rospy.Time.now()),
pose=ee_pose_from_arm_base)
