Additional integration, minor testing has been doen

cart_control/launch/navigation.launch

@@ -1,10 +1,11 @@
 <?xml version="1.0" ?>
 <launch>
   <arg name="testing" default="true"/>
-  <arg name="lidar" default="true"/>
+  <arg name="lidar" default="false"/>
   <arg name="networking" default="true"/>
   <arg name="health" default="true"/>
-  <!-- <arg name="rviz" default="true"/> -->
+  <arg name="voice" default="true"/>
+
   <!-- launch testing interface-->
   <group>
      <include file="$(find cart_control)/launch/autoware_localization.launch">
@@ -17,6 +18,7 @@
       <include file="$(find cart_endpoints)/launch/hardware_interface.launch">
         <arg name="networking" value="$(arg networking)"/>
         <arg name="health" value="$(arg health)"/>
+        <arg name="voice" value="$(arg voice)"/>
       </include>
   </group>
   <!--always launch planning nodes-->

cart_endpoints/launch/hardware_interface.launch

@@ -2,13 +2,19 @@
 <launch>
     <arg name="networking" default="true"/>
     <arg name="health" default="true"/>
+    <arg name="voice" default="true"/>
+
     <group>
         <node name="motor_endpoint" pkg="cart_endpoints" type="motor_endpoint.py" output="screen"/>
+        <node name="gps_node" pkg="cart_endpoints" type="gps_node.py" output="screen"/>
     </group>
     <group if="$(arg health)">
         <node name="cart_health_monitor" pkg="cart_endpoints" type="cart_health_monitor.py" output="screen"/>
     </group>
     <group if="$(arg networking)">
-        <node name="network_client" pkg="cart_endpoints" type="client.py" output="screen"/>
+        <node name="network_client" pkg="cart_endpoints" type="ros_client.py" output="screen"/>
+    </group>
+    <group if="$(arg voice)">
+        <node name="speech_recognition" pkg="cart_endpoints" type="speech_recognition_core.py" output="screen"/>
     </group>
 </launch>

cart_endpoints/scripts/cart_health_monitor.py

@@ -2,9 +2,10 @@
 
 import socket
 import rospy
-from navigation_msgs.msg import EmergencyStop, VehicleState
+from navigation_msgs.msg import VehicleState
 from geometry_msgs.msg import TwistStamped
 from autoware_msgs.msg import NDTStat
+from std_msgs.msg import Bool
 
 class CartHealth(object):
 
@@ -13,7 +14,7 @@ def __init__(self):
 
         self.is_navigating = False
 
-        self.emergency_stop_pub = rospy.Publisher('/emergency_stop', EmergencyStop, queue_size=10)
+        self.emergency_stop_pub = rospy.Publisher('/emergency_stop', Bool, 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)
@@ -40,14 +41,12 @@ def ndt_stat_cb(self, msg):
                 self.is_navigating = False
 
     def speed_check(self, msg):
-        if msg.twist.linear.x >= 3:
+        if msg.twist.linear.x >= 4:
             rospy.logfatal("Overspeeding! Sending Emergency Stop message!")
             self.send_stop()
 
     def send_stop(self):
-        stop_msg = EmergencyStop()
-        stop_msg.emergency_stop = True
-        self.emergency_stop_pub.publish(stop_msg)
+        self.emergency_stop_pub.publish(True)
 
 if __name__ == "__main__":
     try:

cart_endpoints/scripts/gps_node.py

@@ -8,15 +8,14 @@ class GPS_Parser(object):
     def __init__(self):
         rospy.init_node('gps_parser')
 
-        self.fix_pub = rospy.Publisher('fix', NavSatFix, queue_size = 10)
+        self.gps_pub = rospy.Publisher('/gps_coordinates', NavSatFix, queue_size = 10)
 
         self.UDP_IP = "192.168.1.201"#"192.168.3.100"
         self.UDP_PORT = 8308#8308
 
         self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
         self.sock.bind(('', self.UDP_PORT))
-        #self.sock.setblocking(0)
-        rospy.loginfo("Connected")
+        rospy.loginfo("GPS Connected")
         rospy.sleep(1)
 
         while not rospy.is_shutdown():
@@ -26,8 +25,8 @@ def get_and_pub_packet(self):
         data, addr = self.sock.recvfrom(512)
         line = data[206:278]
 
-        my_fix = NavSatFix()
-        my_fix.header.stamp = rospy.Time.now()
+        gps_coords = NavSatFix()
+        gps_coords.header.stamp = rospy.Time.now()
 
         latitude_degrees = float(line[16:18])
         latitude_minutes = float(line[18:25])
@@ -41,15 +40,15 @@ def get_and_pub_packet(self):
         if(line[39] == 'W'):
             longitude = -1 * longitude
 
-        my_fix.latitude = latitude
-        my_fix.longitude = longitude
-        my_fix.altitude = 0.0
+        gps_coords.latitude = latitude
+        gps_coords.longitude = longitude
+        gps_coords.altitude = 0.0
 
-        rospy.loginfo("Latitude: " + str(my_fix.latitude) + " Longitude: " + str(my_fix.longitude))
+        #rospy.loginfo("Latitude: " + str(gps_coords.latitude) + " Longitude: " + str(gps_coords.longitude))
 
 
 
-        self.fix_pub.publish(my_fix)
+        self.gps_pub.publish(gps_coords)
 
     def decimal_degrees(self, degs=0, mins=0, secs=0):
         return degs + (mins/60.0) + (secs/3600.0)

cart_endpoints/scripts/motor_endpoint.py

@@ -19,6 +19,7 @@ def __init__(self):
         self.new_vel = True
         self.debug = False
         self.angle_adjust = 0
+        self.stopping = False
         self.delay_print = 0
 
         self.cmd_msg = None
@@ -40,11 +41,12 @@ def __init__(self):
         """
         self.motion_subscriber = rospy.Subscriber('/nav_cmd', VelAngle, self.motion_callback,
                                                   queue_size=10)
-        self.killswitch_subscriber = rospy.Subscriber('/emergency_stop', EmergencyStop,
-                                                      self.kill_callback, queue_size=10)
+        self.stop_subscriber = rospy.Subscriber('/emergency_stop', Bool,
+                                                      self.stop_callback, queue_size=10)
         self.param_subscriber = rospy.Subscriber('/realtime_a_param_change', Int8, self.param_callback, queue_size=10)
 
         self.debug_subscriber = rospy.Subscriber('/realtime_debug_change', Bool, self.debug_callback, queue_size=10)
+
         rate = rospy.Rate(5)
 
         while not rospy.is_shutdown():
@@ -59,13 +61,9 @@ def motion_callback(self, planned_vel_angle):
     def param_callback(self, msg):
         self.angle_adjust += (msg.data * 10)
 
-    def kill_callback(self, data):
-        if self.cmd_msg is not None:
-            self.cmd_msg = VelAngle()
-
-        self.killswitch = data.emergency_stop
-        if self.killswitch:
-            self.cmd_msg.vel = -100
+    def stop_callback(self, msg):
+        print("Motor endpoint: " + str(msg.data))
+        self.stopping = msg.data
 
     def debug_callback(self, msg):
         self.debug = msg.data
@@ -101,11 +99,14 @@ def send_to_motors(self):
         data = (target_speed,current_speed,target_angle)
         data = bytearray(b'\x00' * 5)
 
-        # ('u8u8u8u8u8', data, 0, 42, 21, throttle, brake, steering)
-        if target_speed < 0:
-            bitstruct.pack_into('u8u8u8u8u8', data, 0, 42, 21, 0, abs(target_speed), target_angle)
+        if self.stopping:
+            print("STOPPING")
+            bitstruct.pack_into('u8u8u8u8u8', data, 0, 42, 21, 0, 200, 50) #currently a flat 200 braking number
         else:
-            bitstruct.pack_into('u8u8u8u8u8', data, 0, 42, 21, abs(target_speed), 0, target_angle)
+            if target_speed < 0:
+                bitstruct.pack_into('u8u8u8u8u8', data, 0, 42, 21, 0, abs(target_speed), target_angle)
+            else:
+                bitstruct.pack_into('u8u8u8u8u8', data, 0, 42, 21, abs(target_speed), 0, target_angle)
 
         #self.speed_ser.write(data) 
 

cart_endpoints/scripts/ros_client.py

@@ -10,8 +10,9 @@
 # import destination
 from std_msgs.msg import Int8, String, Bool
 from geometry_msgs.msg import PoseStamped
-from navigation_msgs.msg import GoalWaypoint, EmergencyStop, VehicleState
+from navigation_msgs.msg import GoalWaypoint, VehicleState
 from speech_recognition_location import startRecognize,stopRecognize
+from sensor_msgs.msg import NavSatFix
 isConnected = False
 
 id = '1bcadd2fea88'
@@ -51,7 +52,7 @@ def sendAudio(msg):
 
 #audio call pullover
 def sendPullOver():
-   send('pull_over',id)
+    send('pull_over',id)
 
 #pose tracking send when unsafe
 def sendUnsafe():
@@ -65,39 +66,41 @@ def sendPassengerExit():
 def sendReady():
     send('passenger_ready',id)
 
+def sendLocation():
+    send('current_location',id)
+
 @sio.on('pull_over',namespace='/cart')
 def onPullOver():
-    print("help this needs changed")
-    #do stuff for the AI
+    stop_pub.publish(True)
 
 @sio.on('resume_driving',namespace='/cart')
 def onResume():
-    print("help this needs changed")
-    #do stuff for the AI
+    stop_pub.publish(False)
 
 #send index + lat/lng + string
 @sio.on('destination', namespace='/cart')
 def onDestination(data):
     #Get JSON data
     location_speech_pub.publish(False)
-    #stopRecognize()
-   ## {latitude:123, longtidue:435}
+
+    #{latitude:123, longtidue:435}
     raw_waypoint = data
-
+
+    #Process the lat long into a waypoint
+    calculated_waypoint = 0
+
     #Prepare goal waypoint message
     requested_waypoint = GoalWaypoint()
     requested_waypoint.start = -1
-    requested_waypoint.goal = raw_waypoint
+    requested_waypoint.goal = calculated_waypoint
 
     #Send requested waypoint to planner
     req_pub.publish(requested_waypoint)
 
 
 @sio.on('stop',namespace='/cart')
 def onStop(data):
-    stop_cart = EmergencyStop()
-    stop_cart.emergency_stop = True
-    stop_pub.publish(stop_cart)
+    stop_pub.publish(True)
 
 @sio.event(namespace='/cart')
 def disconnect():
@@ -117,23 +120,28 @@ def status_update(data):
         if data.reached_destination == True:
             send("arrived", '/cart')
 
-def pulloverCallback(data):
-    if data:
+def pulloverCallback(msg):
+    print("Pull over callback: " + str(msg.data))
+    if msg.data == True:
+        print("TRUE")
+        stop_pub.publish(True)
         sendPullOver()
     else:
-        pass
-        #sendReady() #is this how it should work?
+        print("FALSE")
+        stop_pub.publish(False)
+        sendReady() #is this how it should work?
 
 if __name__ == "__main__":
     rospy.init_node('network_node')
-    stop_pub = rospy.Publisher('/emergency_stop', EmergencyStop, queue_size=10)
+    stop_pub = rospy.Publisher('/emergency_stop', Bool, 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('/gps_coordinates', NavSatFix, sendLocation)
     rate = rospy.Rate(10)  # 10hz
 
     #rospy.spin()

cart_endpoints/scripts/speech_recognition_core.py

@@ -10,7 +10,7 @@
 from playsound import playsound
 import vlc
 from std_msgs.msg import Bool, String
-
+from sensor_msgs.msg import NavSatFix
 
 
 class speech_recognition_core(object):
@@ -59,11 +59,12 @@ def loop(self):
                 print(text)
                 #Alucard, autocorrect, autocart possible other words that need added
                 for x in range(len(text_array)):
-                    print(text_array)
                     #checks for single words like autocart and skips looking for individual words if it is found
                     if self.active <= 0:
-                        if text_array[x] == "alucard" or text_array[x] == "autocorrect" or text_array[x] == "autocart":
-                            self.active = 3
+                        if text_array[x] == "alucard" or text_array[x] == "autocorrect" or text_array[x] == "autocart" or text_array[x] == "autocar" or text_array[x] == "omega" or text_array[x] == "mega":
+                            self.ping_in_sound.stop()
+                            self.ping_in_sound.play()
+                            self.active = 2
                     #checks for two words that together form autocart
                     if self.active > 0 or text_array[x] == "auto":
                         if (self.active > 0 or len(text_array) > x+1 and (text_array[x+1] == "cart" or text_array[x+1] == "part" 
@@ -79,12 +80,14 @@ def loop(self):
                                 if text_array[y] == "help" or text_array[y] == "stop" or text_array[y] == "emergency":
                                     self.emergency_sound.stop()
                                     self.emergency_sound.play()
+                                    time.sleep(2)
                                     print("Emergency Issued")
-                                    self.active = 3
+                                    self.active = 2
                                     self.pullover_pub.publish(True)
                                     break
                                 if text_array[y] == "cancel":
                                     print("Emergency Canceled")
+                                    self.active = 1
                                     self.pullover_pub.publish(False)
                                     break
 
@@ -97,7 +100,7 @@ def loop(self):
                                 #stop is done first as the previous play must be ended to play again
                                 self.ping_in_sound.stop()
                                 self.ping_in_sound.play()
-                                self.active = 2
+                                self.active = 1
                             break
 
             except Exception as e: