ExperimentPlatform/map_test.py at master · ndhuu/ExperimentPlatform · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127

import os
import csv
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import animation
from matplotlib.animation import FuncAnimation
import sys
from copy import copy
from math import cos, sin, atan, asin, pi
import math
import time
import os
import abc

import rospy
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2, LaserScan
import laser_geometry.laser_geometry as lg
from geometry_msgs.msg import PoseArray
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import Twist
from std_msgs.msg import Int16, String
from sensor_msgs.msg import Joy
from nav_msgs.msg import OccupancyGrid
from tf.transformations import euler_from_quaternion, quaternion_from_euler

from map import PostProcessPose


class PostProcessPoseWrapper(PostProcessPose):
    def __init__(self):
        super(PostProcessPoseWrapper, self ).__init__(0.1, 0.2, 5)
        self.currentPosition = {"x": 0, "y": 0, "angle": 0, 'sampleTime': 0}

    def callback(self, message):
        super(PostProcessPoseWrapper, self).callback(message)
        (roll, pitch, angle) = euler_from_quaternion([message.pose.pose.orientation.x,
                                                      message.pose.pose.orientation.y,
                                                      message.pose.pose.orientation.z,
                                                      message.pose.pose.orientation.w])
        self.currentPosition["angle"] = angle
        self.currentPosition["x"]     = message.pose.pose.position.x
        self.currentPosition["y"]     = message.pose.pose.position.y
        # print(self.currentPosition)
        self.currentPosition["sampleTime"] = rospy.get_time()

    def get_current_position(self):
        return self.currentPosition


class BaseTestStrategy(object):
    def __init__(self, postProscessPoseObject):
        self.testResultPublisher = rospy.Publisher("test_result", String, queue_size=5)
        self.postProscessPoseObject = postProscessPoseObject

    @abc.abstractmethod
    def loop_looking_for_signal(self):
        pass

    @abc.abstractmethod
    def change_to_next_target(self, currentTargetPosition):
        pass

    def publish_result(self, currentPosition, targetPosition):
        message = "%f, %f, %f, %f, %f, %f, %f, %f" % (rospy.get_time(), currentPosition["sampleTime"],
                                                      currentPosition["x"], currentPosition["y"], currentPosition["angle"],
                                                      targetPosition["x"], targetPosition["y"], targetPosition["angle"])
        self.testResultPublisher.publish(message)


class JoyTestStrategy(BaseTestStrategy):
    def __init__(self, postProscessPoseObject):
        super(JoyTestStrategy, self).__init__(postProscessPoseObject)
        self.is_sampled = False
        self.targetPosition  = 0
        self.targetPositions =  [] #{"x": 0, "y": 0, "angle": 0}

    def loop_looking_for_signal(self):
        self.targetPositions = [{"x": x[1], "y": x[2], "angle": x[0]} for x in np.loadtxt("target.txt")]
        print(self.targetPositions)
        rospy.Subscriber("joy", Joy, self.joy_callback, queue_size=1)

    def joy_callback(self, message):
        if (int(message.buttons[0]) == 1): #capture a sample
            self.publish_result(self.postProscessPoseObject.currentPosition, self.targetPositions[self.targetPosition])
        if (int(message.buttons[1] == 1)):
            print("change target")
            self.change_to_next_target(self.targetPosition)

    def change_to_next_target(self, currentTargetPosition):
        self.targetPosition = int(input("Enter target index:"))
        # if (self.isTargetIndexIncrease):
        #     self.targetPosition = currentTargetPosition + 1
        # else:
        #     self.targetPosition = currentTargetPosition - 1
        # if (self.targetPosition == 0 or self.targetPosition == self.numTargets-1):
        #     self.isTargetIndexIncrease = not self.isTargetIndexIncrease

class KeyTestStrategy(BaseTestStrategy):
    def __init__(self, postProscessPoseObject):
        super(KeyTestStrategy, self).__init__(postProscessPoseObject)

    def loop_looking_for_signal(self):
        pass

    def change_to_next_target(self, currentTargetPosition):
        pass

class SamplingEvent(object):
    def __init__(self, testStrategy, num_points):
        rospy.Subscriber("joy", Joy, self.callback)
        self.publisher = rospy.Publisher("test_events", Int16, queue_size=1)
        self.num_points = num_points
        self.curr_point = 0

    def callback(self, message):
        """
        docstring
        """
        pass

if __name__ == "__main__":
    rospy.init_node('Post_Process', anonymous=True)
    test = JoyTestStrategy(PostProcessPoseWrapper())
    test.loop_looking_for_signal()
    rospy.spin()