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)