[WIP] Nav observer

CMakeLists.txt

@@ -3,11 +3,25 @@ project(rosgraph_monitor)
 
 find_package(catkin REQUIRED COMPONENTS
     rospy
+    std_msgs
+    message_generation
 )
 
 catkin_python_setup()
 
-catkin_package()
+add_service_files(
+   FILES
+   PredictAction.srv
+)
+
+generate_messages(
+    DEPENDENCIES
+    std_msgs
+)
+
+catkin_package(
+   CATKIN_DEPENDS message_runtime
+)
 
 install(DIRECTORY launch
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}

README.md

@@ -2,27 +2,23 @@
 
 ## Installation
 ```
-$ cd <path/to/workspace/src> git clone -b observers https://github.com/ipa-hsd/rosgraph_monitor/
-$ cd <path/to/workspace/src> git clone -b SoSymPaper https://github.com/ipa-nhg/ros_graph_parser
+$ cd <path/to/workspace/src> git clone -b nav_observer https://github.com/ipa-hsd/rosgraph_monitor/
 $ cd <path/to/workspace>
 $ source /opt/ros/melodic/setup.bash
 $ rosdep install --from-paths src --ignore-src -r -y
 $ catkin build 
 $ source <path/to/workspace/devel/>setup.bash
 ```
+For NavModel.
+```
+pip3 install roslibpy
+https://pypi.python.org/pypi/service_identity
+```
 
 ## Running the system  
-source the workspace in all the terminals
-
 ```
-# Terminal 1
-$ roscore
-
-# Terminal 2
-$ rosrun rosgraph_monitor monitor
-
-# Publish the topics listed in the `QualityObserver`
-
-# In a new terminal 
-$ rosservice call /load_observer "name: 'QualityObserver'"
+roslaunch rosbridge_server rosbridge_websocket.launch
+roslaunch rosgraph_monitor demo.launch
+python3 src/rosgraph_monitor/nav_model.py
+rosservice call /load_observer "name: 'NavObserver'"
 ```

launch/demo.launch

@@ -0,0 +1,6 @@
+<launch>
+    <node name="turtlesim_node" pkg="turtlesim" type="turtlesim_node" output="screen"/>
+    <node name="teleop" pkg="turtlesim" type="turtle_teleop_key" output="screen"/>
+    <node name="monitor" pkg="rosgraph_monitor" type="monitor" output="screen"/>
+    <node name="radar_driver" pkg="rosgraph_monitor" type="radar_driver" output="screen"/>
+</launch>

package.xml

@@ -13,7 +13,7 @@
   <exec_depend>rospy</exec_depend>
   <exec_depend>controller_manager_msgs</exec_depend>
   <exec_depend>diagnostic_msgs</exec_depend>
-  <exec_depend>ros_graph_parser</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 
   <export>
   </export>

scripts/radar_driver

@@ -0,0 +1,83 @@
+#!/usr/bin/env python
+
+import math
+from bisect import bisect_left
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+from threading import Thread, Lock
+
+import rospy
+from turtlesim.msg import Pose
+from std_msgs.msg import Float32MultiArray
+
+quadrants = [90, 180, 270, 360]
+idx = [[0, 1], [1, 2], [2, 3], [3, 0]]
+
+
+def degrees(rad):
+    deg = math.degrees(rad)
+    return deg if rad >= 0 else (deg + 360)
+
+
+class RadarDriver():
+    def __init__(self):
+        self._num_sens = 24
+        self._ranges = []
+        self._thetas = []
+        # self._fig = plt.figure()
+        # self._ax = self._fig.add_subplot(111, projection='polar')
+        # self._mutex = Lock()
+
+        rospy.Subscriber("/turtle1/pose", Pose, self.callback)
+        self._pub = rospy.Publisher('/radar', Float32MultiArray, queue_size=10)
+
+    def callback(self, msg):
+        theta_ = msg.theta
+        data = Float32MultiArray()
+
+        for i in range(self._num_sens):
+            theta = theta_ + (((math.pi * 2) / self._num_sens) * i)
+
+            # print("{} {} {}".format(i, degrees(theta), (math.pi * 2) / self._num_sens))
+            quad = bisect_left(quadrants, degrees(theta))
+            if quad == 4:
+                quad = 0
+            r1 = (11.0 - msg.x) / math.cos(theta)  # size of square = 11?
+            r2 = (11.0 - msg.y) / math.cos((math.pi / 2.0) - theta)
+            t_ = math.radians(degrees(math.pi - theta))
+            r3 = msg.x / math.cos(t_)
+            t_ = math.radians(degrees((3 * math.pi / 2.0) - theta))
+            r4 = msg.y / math.cos(t_)
+
+            r = [abs(r1), abs(r2), abs(r3), abs(r4)]
+
+            # the ray will collide with one of the two boundaries
+            # it collides with the shortest distance
+            dist = min(r[idx[quad][0]], r[idx[quad][1]])
+
+            # self._mutex.acquire()
+            self._ranges.append(dist)
+            self._thetas.append(theta)
+            # self._mutex.release()
+
+        # print(ranges)
+        data.data = self._ranges
+        self._pub.publish(data)
+
+    # def update(self, i):
+    #     self._ax.clear()
+    #     self._mutex.acquire()
+    #     self._ax.plot(self._thetas, self._ranges)
+    #     self._mutex.release()
+
+    # def animate(self):
+        # ani = animation.FuncAnimation(self._fig, self.update, interval=100)
+        # plt.show()
+
+
+if __name__ == '__main__':
+    rospy.init_node('distance_publisher', anonymous=True)
+    driver = RadarDriver()
+    # driver.animate()
+
+rospy.spin()

src/rosgraph_monitor/nav_model.py

@@ -0,0 +1,114 @@
+from rosgraph_monitor.srv import PredictAction, PredictActionResponse
+from std_msgs.msg import String
+import roslibpy
+from sklearn.metrics import mean_absolute_error
+from sklearn.tree import DecisionTreeRegressor
+from sklearn.model_selection import train_test_split
+from sklearn import metrics
+import seaborn as sns
+import pandas as pd
+pd.plotting.register_matplotlib_converters
+
+
+class NavModel:
+    def __init__(self, file_name, num_sens, model_name=""):
+        self._data_set = pd.read_csv(file_name, header=None)
+        self._num_sens = num_sens
+        self._nav_model = DecisionTreeRegressor()
+
+        # the code needs python3 and melodic is on python2
+        # so interface the module with ROS
+        self.ros_client = roslibpy.Ros('localhost', 9090)
+        self.ros_client.on_ready(lambda: print(
+            'Is ROS connected?', self.ros_client.is_connected))
+        service = roslibpy.Service(
+            self.ros_client, '/turtle_action', 'rosgraph_monitor/PredictAction')
+        service.advertise(self._handle_sensor_data)
+
+    def _handle_sensor_data(self, req, resp):
+        if len(req['sensor_data']) == self._num_sens:
+            action = self.get_action(req['sensor_data'])
+            resp['action'] = action
+            return True
+        return False
+
+    def prepare_data(self):
+        # adding header row to the raw dataframe
+        print("reading and processing data-sets")
+
+        self._sensor_labels = list()
+        self._action_labels = list()
+        prefix = 'sensor_'
+        for i in range(self._num_sens):
+            name = prefix + str(i+1)
+            self._sensor_labels.append(name)
+        self._action_labels.append("action")
+        self._data_set.columns = self._sensor_labels + self._action_labels
+
+        # Converting actions from string to int
+        classes = ("Move-Forward", "Slight-Right-Turn",
+                   "Sharp-Right-Turn", "Slight-Left-Turn")
+        for i, item in enumerate(classes):
+            self._data_set = self._data_set.replace(to_replace=item, value=i)
+        # return self._data_set
+
+    def train(self):
+        # Trainng the model using the training data
+        print("training model started...")
+        x = self._data_set[self._sensor_labels]
+        y = self._data_set[self._action_labels]
+        train_x, test_X, train_y, test_y = train_test_split(
+            x, y, random_state=0)
+
+        self._nav_model.fit(train_x, train_y)
+        action = self._predict_action(test_X, test_y)
+        self._eval_metrics(test_y, action)
+
+    def _predict_action(self, test_data, output_data):
+        # The output prediction using the model generated happens here
+        predicted_action = self._nav_model.predict(test_data)
+        print("\nPredicted action = ", predicted_action)
+        return predicted_action
+
+    def _decode_action(self, action):
+        # Decodes the predicted output to readable format
+        return {
+            0: "Move-Forward",
+            1: "Slight-Right-Turn",
+            2: "Sharp-Right-Turn",
+            3: "Slight-Left-Turn"
+        }[action]
+
+    def _eval_metrics(self, output_data, predicted_action):
+        print("\t\t\tEvaluation Metrics")
+        mae = mean_absolute_error(output_data, predicted_action)
+        acc = metrics.accuracy_score(output_data, predicted_action)
+        print("Mean Absolute Error:\t", mae)
+        print("Accuracy of model:\t", acc*100, "%")
+
+    def get_action(self, sensor_raw):
+        # This is where you need to feed the data from turtle bot
+        sensor_input = pd.DataFrame(sensor_raw).T
+        prediction = self._nav_model.predict(sensor_input)
+        action = self._decode_action(prediction[0])
+        return action
+
+    def start_service(self):
+        self.ros_client.run_forever()
+
+
+def main():
+    model = NavModel(
+        'resources/sensor_readings_24.csv', 24)
+    model.prepare_data()
+    model.train()  # should take the sensor array
+    model.start_service()
+
+    sensor_raw = [0.382, 0.612, 0.584, 3.665, 2.953, 2.940, 2.740, 2.629, 1.709, 2.311, 1.860,
+                  1.640, 1.635, 1.654, 1.755, 0.263, 0.545, 0.475, 0.475, 0.185, 0.464, 0.259, 0.468, 0.278]
+    action = model.get_action(sensor_raw)
+    print("\nPrediction by the model:\t", action)
+
+
+if __name__ == "__main__":
+    main()

src/rosgraph_monitor/observers/nav_observer.py

@@ -0,0 +1,36 @@
+from rosgraph_monitor.observer import TopicObserver
+import rospy
+from std_msgs.msg import Float32MultiArray
+from rosgraph_monitor.srv import PredictAction, PredictActionRequest
+from diagnostic_msgs.msg import DiagnosticStatus, KeyValue
+
+
+class NavObserver(TopicObserver):
+    def __init__(self, name):
+        topics = [("/radar", Float32MultiArray)]
+
+        super(NavObserver, self).__init__(
+            name, 10, topics)
+
+        rospy.wait_for_service('/turtle_action', timeout=1.0)
+        self.client = rospy.ServiceProxy('/turtle_action', PredictAction)
+
+    def calculate_attr(self, msgs):
+        status_msg = DiagnosticStatus()
+
+        req = PredictActionRequest()
+        req.sensor_data = msgs[0].data
+        resp = self.client(req)
+        attr = resp.action
+        # print("Predicted action: {0}".format(attr))
+
+        status_msg = DiagnosticStatus()
+        status_msg.level = DiagnosticStatus.OK
+        status_msg.name = self._id
+        status_msg.values.append(
+            KeyValue("action", str(attr)))
+        status_msg.message = "Action prediction"
+
+        print(status_msg)
+
+        return status_msg

srv/PredictAction.srv

@@ -0,0 +1,3 @@
+float32[] sensor_data
+---
+string action