def publish(self):
stamp = self._mobile_base_tf_for_calc_from_localized_origin.header.stamp
translation = add_vectors([self._mobile_base_tf_for_calc_from_localized_origin.transform.translation,
self._localized_point_tf_for_calc_from_mobile_base.transform.translation])
rotation = multiply_ros_quaternion([self._mobile_base_tf_for_calc_from_localized_origin.transform.rotation,
self._localized_point_tf_for_calc_from_mobile_base.transform.rotation])
self._localized_point_tf_from_localized_origin.header.stamp = rospy.Time.now()
self._localized_point_tf_from_localized_origin.transform.translation = translation
self._localized_point_tf_from_localized_origin.transform.rotation = rotation
self._tf_broadcaster.sendTransform([self._localized_point_tf_from_localized_origin,
self._mobile_base_tf_for_calc_from_localized_origin])
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()