fc.py 文件源码

def process_palm_msg(self, msg):
        if math.isnan(msg.x) or math.isnan(msg.y) or math.isnan(msg.z):
            self.oclog("Warning - NAN position found.  skipping");
            return

        point_vector = Vector3(msg.x, msg.y, msg.z)

        if not msg.relative:
            point = PointStamped()
            point.point = point_vector
            if self.cloud is None:
                point.header.seq = 1
                point.header.stamp = rospy.get_rostime() - rospy.Duration(0.1)
            else:
                point.header = deepcopy(self.cloud.header)
            if msg.use_left_foot:
                point.header.frame_id = self.zarj.walk.lfname

            desired_frame = self.zarj.transform.lookup_transform('world', point.header.frame_id, point.header.stamp)

            result = self.zarj.transform.tf_buffer.transform(point, 'world')

            if math.isnan(msg.yaw) or math.isnan(msg.pitch) or math.isnan(msg.roll):
                current = self.zarj.hands.get_current_hand_center_transform(msg.sidename)
                rotation = current.rotation
            else:
                desired_rotation = quaternion_multiply(
                    msg_q_to_q(desired_frame.transform.rotation),
                    yaw_pitch_roll_to_q([msg.yaw, msg.pitch, msg.roll]) )
                rotation = q_to_msg_q(desired_rotation)
            self.zarj.hands.move_hand_center_abs(msg.sidename, result.point, rotation)
            self.oclog("Palm move to {}, {}, {}; yaw {}, pitch {}, roll {} commanded".format(
                fmt(msg.x), fmt(msg.y), fmt(msg.z), fmt(msg.yaw), fmt(msg.pitch), fmt(msg.roll)))
        else:
            if math.isnan(msg.yaw):
                msg.yaw = 0.0
            if math.isnan(msg.pitch):
                msg.pitch = 0.0
            if math.isnan(msg.roll):
                msg.roll = 0.0
            self.zarj.hands.move_with_yaw_pitch_roll(msg.sidename, point_vector,
                [ msg.yaw, msg.pitch, msg.roll ])
            self.oclog("Relative palm move to {}, {}, {} commanded; yaw {}, pitch {}, roll {}".format(
                    fmt(msg.x), fmt(msg.y), fmt(msg.z), fmt(msg.yaw), fmt(msg.pitch), fmt(msg.roll)))