#!/usr/bin/env python
'''
Live plotting of pose trajectory (3D space). LEGACY, DO NOT USE.
'''
import rospy
from std_msgs.msg import Float64MultiArray, MultiArrayDimension, Float64, Float32, Float32MultiArray
from geometry_msgs.msg import PoseStamped, Pose, PoseArray
from visualization_msgs.msg import Marker
import matplotlib.pyplot as plt
import numpy as np
from math import pi, sin, pow
import csv


RIGHT_ARM = 0
LEFT_ARM = 1

# --- Classes ---
class RobotData:
    def __init__(self):
        self.arms = [ArmData("right"), ArmData("left")]
        self.period_time = 7

class ArmData:
    def __init__(self, name):
        self.currentPose = PoseStamped() 
        self.targetPose = PoseStamped()
        self.t_array = []
        self.target_array = []
        self.current_array = []
        self.name = name

    def plot(self):
        plt.figure(self.name)
        plt.cla()
        # plt.axis([0, 7, -1.5, 1.5])
        plt.plot(self.t_array, self.current_array, '-o', color="blue")
        plt.plot(self.t_array, self.target_array, '-x', color="red")
# --- end Classes ---

# Global variables
robotData = RobotData()

def rightCurrentPoseCallback(data):
    robotData.arms[RIGHT_ARM].currentPose = data

def leftCurrentPassCallback(data):
    robotData.arms[LEFT_ARM].currentPose = data


def leftTargetCallback(data):
    robotData.arms[LEFT_ARM].targetPose = data

def rightTargetCallback(data):
    robotData.arms[RIGHT_ARM].targetPose = data


if __name__ == '__main__':
    rospy.init_node('pose_tracking_graph')

    rospy.Subscriber("/robot/right/currentState", PoseStamped, rightCurrentPoseCallback, queue_size=1)
    rospy.Subscriber("/robot/left/currentState", PoseStamped, leftCurrentPassCallback, queue_size=1)

    rospy.Subscriber("/vive/controller/left/processed/pose", PoseStamped, leftTargetCallback, queue_size=1)
    rospy.Subscriber("/vive/controller/right/processed/pose", PoseStamped, rightTargetCallback, queue_size=1)

    freq = 10.0
    rate = rospy.Rate(freq)
    t = 0.0
    period_time = 7.0
    robotData.period_time = period_time
    while not rospy.is_shutdown():
        for arm in robotData.arms:
            arm.t_array.append(t)
            arm.current_array.append(arm.currentPose.pose.position.y)
            arm.target_array.append(arm.targetPose.pose.position.y)
            # arm.plot()
        # plt.pause(0.05)
        # plt.show(block=False)
        t += 1.0/freq
        with open('sweepsine_left.csv', 'a') as fd:
            row =   [
                        str(t),
                        str(robotData.arms[LEFT_ARM].currentPose.pose.position.y),
                        str(robotData.arms[LEFT_ARM].targetPose.pose.position.y)
                    ]
            writer = csv.writer(fd)
            writer.writerow(row)

        rate.sleep()


8c9d0f365331e5b6e8fc7500491382acddfcfda3
<mask token>


class RobotData:

    def __init__(self):
        self.arms = [ArmData('right'), ArmData('left')]
        self.period_time = 7


class ArmData:

    def __init__(self, name):
        self.currentPose = PoseStamped()
        self.targetPose = PoseStamped()
        self.t_array = []
        self.target_array = []
        self.current_array = []
        self.name = name

    def plot(self):
        plt.figure(self.name)
        plt.cla()
        plt.plot(self.t_array, self.current_array, '-o', color='blue')
        plt.plot(self.t_array, self.target_array, '-x', color='red')


<mask token>

<mask token>


class RobotData:

    def __init__(self):
        self.arms = [ArmData('right'), ArmData('left')]
        self.period_time = 7


class ArmData:

    def __init__(self, name):
        self.currentPose = PoseStamped()
        self.targetPose = PoseStamped()
        self.t_array = []
        self.target_array = []
        self.current_array = []
        self.name = name

    def plot(self):
        plt.figure(self.name)
        plt.cla()
        plt.plot(self.t_array, self.current_array, '-o', color='blue')
        plt.plot(self.t_array, self.target_array, '-x', color='red')


<mask token>


def rightCurrentPoseCallback(data):
    robotData.arms[RIGHT_ARM].currentPose = data


<mask token>


def leftTargetCallback(data):
    robotData.arms[LEFT_ARM].targetPose = data


<mask token>

<mask token>


class RobotData:

    def __init__(self):
        self.arms = [ArmData('right'), ArmData('left')]
        self.period_time = 7


class ArmData:

    def __init__(self, name):
        self.currentPose = PoseStamped()
        self.targetPose = PoseStamped()
        self.t_array = []
        self.target_array = []
        self.current_array = []
        self.name = name

    def plot(self):
        plt.figure(self.name)
        plt.cla()
        plt.plot(self.t_array, self.current_array, '-o', color='blue')
        plt.plot(self.t_array, self.target_array, '-x', color='red')


<mask token>


def rightCurrentPoseCallback(data):
    robotData.arms[RIGHT_ARM].currentPose = data


def leftCurrentPassCallback(data):
    robotData.arms[LEFT_ARM].currentPose = data


def leftTargetCallback(data):
    robotData.arms[LEFT_ARM].targetPose = data


def rightTargetCallback(data):
    robotData.arms[RIGHT_ARM].targetPose = data


if __name__ == '__main__':
    rospy.init_node('pose_tracking_graph')
    rospy.Subscriber('/robot/right/currentState', PoseStamped,
        rightCurrentPoseCallback, queue_size=1)
    rospy.Subscriber('/robot/left/currentState', PoseStamped,
        leftCurrentPassCallback, queue_size=1)
    rospy.Subscriber('/vive/controller/left/processed/pose', PoseStamped,
        leftTargetCallback, queue_size=1)
    rospy.Subscriber('/vive/controller/right/processed/pose', PoseStamped,
        rightTargetCallback, queue_size=1)
    freq = 10.0
    rate = rospy.Rate(freq)
    t = 0.0
    period_time = 7.0
    robotData.period_time = period_time
    while not rospy.is_shutdown():
        for arm in robotData.arms:
            arm.t_array.append(t)
            arm.current_array.append(arm.currentPose.pose.position.y)
            arm.target_array.append(arm.targetPose.pose.position.y)
        t += 1.0 / freq
        with open('sweepsine_left.csv', 'a') as fd:
            row = [str(t), str(robotData.arms[LEFT_ARM].currentPose.pose.
                position.y), str(robotData.arms[LEFT_ARM].targetPose.pose.
                position.y)]
            writer = csv.writer(fd)
            writer.writerow(row)
        rate.sleep()

<mask token>
import rospy
from std_msgs.msg import Float64MultiArray, MultiArrayDimension, Float64, Float32, Float32MultiArray
from geometry_msgs.msg import PoseStamped, Pose, PoseArray
from visualization_msgs.msg import Marker
import matplotlib.pyplot as plt
import numpy as np
from math import pi, sin, pow
import csv
RIGHT_ARM = 0
LEFT_ARM = 1


class RobotData:

    def __init__(self):
        self.arms = [ArmData('right'), ArmData('left')]
        self.period_time = 7


class ArmData:

    def __init__(self, name):
        self.currentPose = PoseStamped()
        self.targetPose = PoseStamped()
        self.t_array = []
        self.target_array = []
        self.current_array = []
        self.name = name

    def plot(self):
        plt.figure(self.name)
        plt.cla()
        plt.plot(self.t_array, self.current_array, '-o', color='blue')
        plt.plot(self.t_array, self.target_array, '-x', color='red')


robotData = RobotData()


def rightCurrentPoseCallback(data):
    robotData.arms[RIGHT_ARM].currentPose = data


def leftCurrentPassCallback(data):
    robotData.arms[LEFT_ARM].currentPose = data


def leftTargetCallback(data):
    robotData.arms[LEFT_ARM].targetPose = data


def rightTargetCallback(data):
    robotData.arms[RIGHT_ARM].targetPose = data


if __name__ == '__main__':
    rospy.init_node('pose_tracking_graph')
    rospy.Subscriber('/robot/right/currentState', PoseStamped,
        rightCurrentPoseCallback, queue_size=1)
    rospy.Subscriber('/robot/left/currentState', PoseStamped,
        leftCurrentPassCallback, queue_size=1)
    rospy.Subscriber('/vive/controller/left/processed/pose', PoseStamped,
        leftTargetCallback, queue_size=1)
    rospy.Subscriber('/vive/controller/right/processed/pose', PoseStamped,
        rightTargetCallback, queue_size=1)
    freq = 10.0
    rate = rospy.Rate(freq)
    t = 0.0
    period_time = 7.0
    robotData.period_time = period_time
    while not rospy.is_shutdown():
        for arm in robotData.arms:
            arm.t_array.append(t)
            arm.current_array.append(arm.currentPose.pose.position.y)
            arm.target_array.append(arm.targetPose.pose.position.y)
        t += 1.0 / freq
        with open('sweepsine_left.csv', 'a') as fd:
            row = [str(t), str(robotData.arms[LEFT_ARM].currentPose.pose.
                position.y), str(robotData.arms[LEFT_ARM].targetPose.pose.
                position.y)]
            writer = csv.writer(fd)
            writer.writerow(row)
        rate.sleep()

#!/usr/bin/env python
'''
Live plotting of pose trajectory (3D space). LEGACY, DO NOT USE.
'''
import rospy
from std_msgs.msg import Float64MultiArray, MultiArrayDimension, Float64, Float32, Float32MultiArray
from geometry_msgs.msg import PoseStamped, Pose, PoseArray
from visualization_msgs.msg import Marker
import matplotlib.pyplot as plt
import numpy as np
from math import pi, sin, pow
import csv


RIGHT_ARM = 0
LEFT_ARM = 1

# --- Classes ---
class RobotData:
    def __init__(self):
        self.arms = [ArmData("right"), ArmData("left")]
        self.period_time = 7

class ArmData:
    def __init__(self, name):
        self.currentPose = PoseStamped() 
        self.targetPose = PoseStamped()
        self.t_array = []
        self.target_array = []
        self.current_array = []
        self.name = name

    def plot(self):
        plt.figure(self.name)
        plt.cla()
        # plt.axis([0, 7, -1.5, 1.5])
        plt.plot(self.t_array, self.current_array, '-o', color="blue")
        plt.plot(self.t_array, self.target_array, '-x', color="red")
# --- end Classes ---

# Global variables
robotData = RobotData()

def rightCurrentPoseCallback(data):
    robotData.arms[RIGHT_ARM].currentPose = data

def leftCurrentPassCallback(data):
    robotData.arms[LEFT_ARM].currentPose = data


def leftTargetCallback(data):
    robotData.arms[LEFT_ARM].targetPose = data

def rightTargetCallback(data):
    robotData.arms[RIGHT_ARM].targetPose = data


if __name__ == '__main__':
    rospy.init_node('pose_tracking_graph')

    rospy.Subscriber("/robot/right/currentState", PoseStamped, rightCurrentPoseCallback, queue_size=1)
    rospy.Subscriber("/robot/left/currentState", PoseStamped, leftCurrentPassCallback, queue_size=1)

    rospy.Subscriber("/vive/controller/left/processed/pose", PoseStamped, leftTargetCallback, queue_size=1)
    rospy.Subscriber("/vive/controller/right/processed/pose", PoseStamped, rightTargetCallback, queue_size=1)

    freq = 10.0
    rate = rospy.Rate(freq)
    t = 0.0
    period_time = 7.0
    robotData.period_time = period_time
    while not rospy.is_shutdown():
        for arm in robotData.arms:
            arm.t_array.append(t)
            arm.current_array.append(arm.currentPose.pose.position.y)
            arm.target_array.append(arm.targetPose.pose.position.y)
            # arm.plot()
        # plt.pause(0.05)
        # plt.show(block=False)
        t += 1.0/freq
        with open('sweepsine_left.csv', 'a') as fd:
            row =   [
                        str(t),
                        str(robotData.arms[LEFT_ARM].currentPose.pose.position.y),
                        str(robotData.arms[LEFT_ARM].targetPose.pose.position.y)
                    ]
            writer = csv.writer(fd)
            writer.writerow(row)

        rate.sleep()

