action_database.py 文件源码

def create_pose_velocity_msg(self,cart_velo,transform=False):
        if transform:
            if self.listen.frameExists("/root") and self.listen.frameExists("/j2n6a300_end_effector"):
                self.listen.waitForTransform('/root',"/j2n6a300_end_effector",rospy.Time(),rospy.Duration(100.0))
                t = self.listen.getLatestCommonTime("/root", "/j2n6a300_end_effector")
                translation, quaternion = self.listen.lookupTransform("/root", "/j2n6a300_end_effector", t)
            transform = self.transformer.fromTranslationRotation(translation, quaternion)
            transformed_vel = np.dot(transform,(cart_velo[0:3]+[1.0]))
            print(transformed_vel)
            cart_velo[0:3] = transformed_vel[0:3]
        msg = PoseVelocity(
        twist_linear_x=cart_velo[0],
        twist_linear_y=cart_velo[1],
        twist_linear_z=cart_velo[2],
        twist_angular_x=cart_velo[3],
        twist_angular_y=cart_velo[4],
        twist_angular_z=cart_velo[5])
        return msg