Additional testing has been done a new dictionary for stopping messag…

…es has been createdx

cart_endpoints/scripts/cart_health_monitor.py

@@ -2,7 +2,7 @@
 
 import socket
 import rospy
-from navigation_msgs.msg import VehicleState
+from navigation_msgs.msg import VehicleState, EmergencyStop
 from geometry_msgs.msg import TwistStamped
 from autoware_msgs.msg import NDTStat
 from std_msgs.msg import Bool
@@ -13,8 +13,8 @@ def __init__(self):
         rospy.init_node('cart_health_monitor')
 
         self.is_navigating = False
-
-        self.emergency_stop_pub = rospy.Publisher('/emergency_stop', Bool, queue_size=10)
+        self.stopping = False
+        self.emergency_stop_pub = rospy.Publisher('/emergency_stop', EmergencyStop, queue_size=10)
 
         self.vehicle_state_sub = rospy.Subscriber('/vehicle_state', VehicleState, self.status_update)
         self.ndt_stat_sub = rospy.Subscriber('/ndt_stat', NDTStat, self.ndt_stat_cb)
@@ -37,16 +37,24 @@ def ndt_stat_cb(self, msg):
             rospy.logfatal("Localizer has extremely bad health")
             if self.is_navigating:
                 rospy.logfatal("Sending Emergency Stop due to bad localization!")
-                self.send_stop()
+                self.send_stop(True, 4)
                 self.is_navigating = False
 
     def speed_check(self, msg):
         if msg.twist.linear.x >= 4:
             rospy.logfatal("Overspeeding! Sending Emergency Stop message!")
-            self.send_stop()
+            self.send_stop(True, 4)
+            self.stopping = True
+        elif self.stopping == True and msg.twist.linear.x <= 2:
+            self.stopping = False
+            rospy.loginfo("Speed has been reduced resuming ride!")
+            self.send_stop(False, 4)
 
-    def send_stop(self):
-        self.emergency_stop_pub.publish(True)
+    def send_stop(self, stop, sender_id):
+        stop_msg = EmergencyStop()
+        stop_msg.emergency_stop = stop
+        stop_msg.sender_id = sender_id
+        self.emergency_stop_pub.publish(stop_msg)
 
 if __name__ == "__main__":
     try:

cart_endpoints/scripts/logs/2019-12-03_17:17:25.249565/log.txt

@@ -0,0 +1 @@
+System booted: 2019-12-03 17:17:25.249565

cart_endpoints/scripts/logs/2019-12-03_17:17:37.754736/log.txt

@@ -0,0 +1 @@
+System booted: 2019-12-03 17:17:37.754736

cart_endpoints/scripts/logs/2019-12-03_17:23:56.603106/log.txt

@@ -0,0 +1 @@
+System booted: 2019-12-03 17:23:56.603106

cart_endpoints/scripts/logs/2019-12-03_17:24:15.119210/log.txt

@@ -0,0 +1 @@
+System booted: 2019-12-03 17:24:15.119210

cart_endpoints/scripts/logs/2019-12-03_17:24:47.221770/log.txt

@@ -0,0 +1 @@
+System booted: 2019-12-03 17:24:47.221770

cart_endpoints/scripts/logs/2019-12-03_17:55:27.581875/log.txt

@@ -0,0 +1 @@
+System booted: 2019-12-03 17:55:27.581875

cart_endpoints/scripts/logs/2019-12-03_17:57:01.529793/log.txt

@@ -0,0 +1 @@
+System booted: 2019-12-03 17:57:01.529793

cart_endpoints/scripts/logs/2019-12-03_18:01:35.908608/log.txt

@@ -0,0 +1 @@
+System booted: 2019-12-03 18:01:35.908608

cart_endpoints/scripts/logs/2019-12-03_18:14:33.710350/log.txt

@@ -0,0 +1 @@
+System booted: 2019-12-03 18:14:33.710350

cart_endpoints/scripts/logs/2019-12-03_18:14:37.193522/log.txt

@@ -0,0 +1 @@
+System booted: 2019-12-03 18:14:37.193522

cart_endpoints/scripts/logs/2019-12-03_18:26:50.826079/log.txt

@@ -0,0 +1 @@
+System booted: 2019-12-03 18:26:50.826079

cart_endpoints/scripts/motor_endpoint.py

@@ -19,7 +19,7 @@ def __init__(self):
         self.new_vel = True
         self.debug = False
         self.angle_adjust = 0
-        self.stopping = False
+        self.stopping_dictionary = {0: False, 1: False, 2: False, 3:False, 4:False}
         self.delay_print = 0
 
         self.cmd_msg = None
@@ -41,7 +41,7 @@ def __init__(self):
         """
         self.motion_subscriber = rospy.Subscriber('/nav_cmd', VelAngle, self.motion_callback,
                                                   queue_size=10)
-        self.stop_subscriber = rospy.Subscriber('/emergency_stop', Bool,
+        self.stop_subscriber = rospy.Subscriber('/emergency_stop', EmergencyStop,
                                                       self.stop_callback, queue_size=10)
         self.param_subscriber = rospy.Subscriber('/realtime_a_param_change', Int8, self.param_callback, queue_size=10)
 
@@ -62,44 +62,49 @@ def param_callback(self, msg):
         self.angle_adjust += (msg.data * 10)
 
     def stop_callback(self, msg):
-        print("Motor endpoint: " + str(msg.data))
-        self.stopping = msg.data
+        self.stopping_dictionary[msg.sender_id] = msg.emergency_stop
 
     def debug_callback(self, msg):
         self.debug = msg.data
 
 
     def send_to_motors(self):
+        #The first time we get a new target speed and angle we must convert it
         if self.new_vel:
-            #self.new_vel = False #this is set to false after adjusting the new target angle below
+            self.new_vel = False
             self.cmd_msg.vel *= 50
             self.cmd_msg.vel_curr *= 50
-        if self.cmd_msg.vel > 254:
-            self.cmd_msg.vel = 254
-        if self.cmd_msg.vel < -254:
-            self.cmd_msg.vel = -254
-        if self.cmd_msg.vel_curr > 254:
-            self.cmd_msg.vel_curr = 254
+            if self.cmd_msg.vel > 254:
+                self.cmd_msg.vel = 254
+            if self.cmd_msg.vel < -254:
+                self.cmd_msg.vel = -254
+            if self.cmd_msg.vel_curr > 254:
+                self.cmd_msg.vel_curr = 254
         target_speed = int(self.cmd_msg.vel) #float64
         current_speed = int(self.cmd_msg.vel_curr) #float64
-
+        #adjust the target_angle range from (-70 <-> 70) to (0 <-> 100)
         target_angle = 100 - int(( (self.cmd_msg.angle + 70) / 140 ) * 100)
+        #adjust the target angle additionally using a realtime adjustable testing value
         if self.new_vel:
-            self.new_vel = False
             if target_angle < self.angle_adjust:
                 target_angle -= (10 + int(self.angle_adjust/2))
             if target_angle > 100 - self.angle_adjust:
                 target_angle += (10 + int(self.angle_adjust/2))
+        data = (target_speed,current_speed,target_angle)
+        data = bytearray(b'\x00' * 5)
+
+        #if debug printing is requested print speed and angle info
         if self.debug:
             self.delay_print -= 1
             if self.delay_print <= 0:
                 self.delay_print = 5
                 rospy.loginfo("Endpoint Angle: " + str(target_angle))
                 rospy.loginfo("Endpoint Speed: " + str(target_speed))
-        data = (target_speed,current_speed,target_angle)
-        data = bytearray(b'\x00' * 5)
 
-        if self.stopping:
+        for y in self.stopping_dictionary:
+            print(y, self.stopping_dictionary[y])
+        #checks if any of the stopping values are True, meaning a service is requesting to stop
+        if any(x == True for x in self.stopping_dictionary.values()):
             print("STOPPING")
             bitstruct.pack_into('u8u8u8u8u8', data, 0, 42, 21, 0, 200, 50) #currently a flat 200 braking number
         else:

cart_endpoints/scripts/ros_client.py

@@ -10,7 +10,7 @@
 # import destination
 from std_msgs.msg import Int8, String, Bool
 from geometry_msgs.msg import PoseStamped
-from navigation_msgs.msg import GoalWaypoint, VehicleState
+from navigation_msgs.msg import GoalWaypoint, VehicleState, EmergencyStop
 from speech_recognition_location import startRecognize,stopRecognize
 from sensor_msgs.msg import NavSatFix
 isConnected = False
@@ -43,7 +43,7 @@ def onTransitAwait(data):
     location_speech_pub.publish(True)
     #startRecognize(sendAudio)
 
-def sendAudio(msg):
+def send_audio(msg):
     json = {
         "msg": msg,
         "id":id
@@ -58,7 +58,6 @@ def sendPullOver():
 def sendUnsafe():
     send('passenger_unsafe',id)
 
-
 def sendPassengerExit():
     send('passenger_exit',id)
 
@@ -71,11 +70,11 @@ def sendLocation():
 
 @sio.on('pull_over',namespace='/cart')
 def onPullOver():
-    stop_pub.publish(True)
+    send_stop(True, 1)
 
 @sio.on('resume_driving',namespace='/cart')
 def onResume():
-    stop_pub.publish(False)
+    send_stop(False, 1)
 
 #send index + lat/lng + string
 @sio.on('destination', namespace='/cart')
@@ -100,7 +99,7 @@ def onDestination(data):
 
 @sio.on('stop',namespace='/cart')
 def onStop(data):
-    stop_pub.publish(True)
+    send_stop(True, 1)
 
 @sio.event(namespace='/cart')
 def disconnect():
@@ -120,28 +119,51 @@ def status_update(data):
         if data.reached_destination == True:
             send("arrived", '/cart')
 
-def pulloverCallback(msg):
-    print("Pull over callback: " + str(msg.data))
+def pullover_callback(msg):
     if msg.data == True:
-        print("TRUE")
-        stop_pub.publish(True)
+        send_stop(True, 2)
         sendPullOver()
     else:
-        print("FALSE")
-        stop_pub.publish(False)
-        sendReady() #is this how it should work?
+        send_stop(False, 2)
+        sendReady() #is this how it should work?'
+
+def passenger_safe_callback(msg):
+    if msg.data == True:
+        send_stop(False, 3)
+        sendReady()
+    else:
+        send_stop(True, 3)
+        sendUnsafe()
+
+def passenger_exit_callback(msg):
+    sendPassengerExit()
+
+# sender_id is important to ensure all parties 
+# are ready to resume before releasing the stop command
+# ie both voice and pose tell us we need to stop and then
+# pose gives us the all clear but we should still be 
+# waiting for voice to also give the all clear
+# sender_id = 1 is the server, 2 is voice, 3 is pose, 4 is health monitor, 
+# 0 is for internal usage but is currently unused
+def send_stop(stop, sender_id):
+    stop_msg = EmergencyStop()
+    stop_msg.emergency_stop = stop
+    stop_msg.sender_id = sender_id
+    stop_pub.publish(stop_msg)
 
 if __name__ == "__main__":
     rospy.init_node('network_node')
-    stop_pub = rospy.Publisher('/emergency_stop', Bool, queue_size=10)
+    stop_pub = rospy.Publisher('/emergency_stop', EmergencyStop, queue_size=10)
     req_pub = rospy.Publisher('/path_request', GoalWaypoint, queue_size=10)
     location_speech_pub = rospy.Publisher('/location_speech', Bool, queue_size=10)
     #pub = rospy.Publisher('network_node_pub', String, queue_size=10)
     rospy.Subscriber('/current_position', Int8, sendPositionIndex)
     rospy.Subscriber('/vehicle_state', VehicleState, status_update)
-    rospy.Subscriber('/pullover', Bool, pulloverCallback)
-    rospy.Subscriber('/speech_text', String, sendAudio)
+    rospy.Subscriber('/pullover', Bool, pullover_callback)
+    rospy.Subscriber('/speech_text', String, send_audio)
     rospy.Subscriber('/gps_coordinates', NavSatFix, sendLocation)
+    rospy.Subscriber('/passenger_safe', Bool, passenger_safe_callback)
+    rospy.Subscriber('/passenger_exit', Bool, passenger_exit_callback)
     rate = rospy.Rate(10)  # 10hz
 
     #rospy.spin()

cart_endpoints/scripts/speech_recognition_core.py

@@ -85,7 +85,7 @@ def loop(self):
                                     self.active = 2
                                     self.pullover_pub.publish(True)
                                     break
-                                if text_array[y] == "cancel":
+                                if text_array[y] == "cancel" or text_array[y] == "resume":
                                     print("Emergency Canceled")
                                     self.active = 1
                                     self.pullover_pub.publish(False)

cart_endpoints/scripts/test_live_analysis_v2.py

@@ -15,20 +15,27 @@
 import time
 import datetime
 import sys
+import rospy
 import numpy as np
-import pickle
+import cPickle as pickle
 from sklearn.ensemble import RandomForestClassifier
 import paho.mqtt.client as mqtt
+from std_msgs.msg import Int8, String, Bool
+from navigation_msgs import EmergencyStop
 import json
 
 # Add openpose to system PATH and import
-sys.path.append('/home/jeffercize/catkin_ws/src/openpose/build/python');
+sys.path.append('/home/jeffercize/catkin_ws/src/openpose/build/python')
 from openpose import pyopenpose as op
 
+rospy.init_node('pose_tracker')
+rospy.loginfo("Started pose tracking node!")
+passenger_safe_pub = rospy.Publisher('/passenger_safe', Bool, queue_size=10)
+passenger_exit_pub = rospy.Publisher('/passenger_exit', Bool, queue_size=10)
 #######################
 # Set up global variables for use in all methods.
 ##############
-cam = cv2.VideoCapture(1)
+cam = cv2.VideoCapture(0)
 start_time = time.time()
 start_time_stamp = datetime.datetime.now()
 # Setup logging file
@@ -40,7 +47,7 @@
 full_path = path + str(start_time_stamp.date()) + "_" + str(start_time_stamp.time())
 os.makedirs(full_path)
 
-f = open(full_path + "/log.txt", "x")
+f = open(full_path + "/log.txt", "wa")
 f.write("System booted: {}\n".format(start_time_stamp))
 
 CONFIDENCE_THRESHOLD = 15
@@ -58,56 +65,21 @@
 opWrapper.start()
 
 ######################
-# Network Client
+# ROS Topic Stuff
 ###########
 
-def onConnect():
-    print('mqtt connected')
-
-
-def onMessage(data):
-    parsedData = json.loads(data)
-    if(parsedData['command'] == 'passenger_ready'):
-        print('passenger is ready')
-        trip_live = True
-        initial_safety()
-        safety_analysis()
-
-    elif(parsedData['command'] == 'passenger_stop'):
-        print('passenger stopped the cart')
-        trip_live = False
-        passenger_exit()
-
-client = mqtt.Client()
-client.on_connect = onConnect
-client.on_message = onMessage
-client.connect("localhost", 1883, 60)
-
-
 def sendPassengerUnsafe():
-    data = {
-        "command": "passenger_unsafe",
-        "data": "passenger_unsafe"
-    }
-    client.publish("/pose", json.dumps(data))
+    passenger_safe_pub.publish(True)
 
 
 def sendPassengerSafe():
-    data = {
-        "command": "passenger_safe",
-        "data": "passenger_safe"
-    }
-    client.publish("/pose", json.dumps(data))
+    passenger_safe_pub.publish(False)
 
 
 def sendPassengerExit():
-    data = {
-        "command": "passenger_exit",
-        "data": "passenger_exit"
-    }
-    client.publish("/pose", json.dumps(data))
+    passenger_exit_pub.publish(True)
 
-#######################################
+# #######################################
 
 
 ######################
@@ -294,6 +266,10 @@ def edit_video(img):
 def unsafe():
     sendPassengerUnsafe()
     print("PASSENGER UNSAFE")
+
+
+
+
 
 ###################
 # Safety Monitoring