Big Commit will fix more later sorry

cart_endpoints/scripts/gps_node.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python
+import socket
+import rospy
+from sensor_msgs.msg import NavSatFix
+
+class GPS_Parser(object):
+
+    def __init__(self):
+        rospy.init_node('gps_parser')
+
+        self.fix_pub = rospy.Publisher('fix', 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.sleep(1)
+
+        while not rospy.is_shutdown():
+            self.get_and_pub_packet()
+
+    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()
+
+        latitude_degrees = float(line[16:18])
+        latitude_minutes = float(line[18:25])
+        latitude = self.decimal_degrees(degs=latitude_degrees, mins=latitude_minutes)
+        if (line[26] == 'S'):
+            latitude = -1 * latitude
+
+        longitude_degrees = float(line[28:31])
+        longitude_minutes = float(line[31:38])
+        longitude = self.decimal_degrees(degs=longitude_degrees, mins=longitude_minutes)
+        if(line[39] == 'W'):
+            longitude = -1 * longitude
+
+        my_fix.latitude = latitude
+        my_fix.longitude = longitude
+        my_fix.altitude = 0.0
+
+        rospy.loginfo("Latitude: " + str(my_fix.latitude) + " Longitude: " + str(my_fix.longitude))
+
+
+
+        self.fix_pub.publish(my_fix)
+
+    def decimal_degrees(self, degs=0, mins=0, secs=0):
+        return degs + (mins/60.0) + (secs/3600.0)
+
+if __name__ == "__main__":
+    GPS_Parser()
+

cart_endpoints/scripts/motor_endpoint.py

@@ -5,6 +5,7 @@
 import bitstruct
 from navigation_msgs.msg import VelAngle
 from navigation_msgs.msg import EmergencyStop
+from std_msgs.msg import Int8, Bool
 cart_port = '/dev/ttyUSB0' #hardcoded depending on computer
 
 class MotorEndpoint(object):
@@ -16,15 +17,18 @@ def __init__(self):
         self.goal_speed = 0.0
         self.goal_angle = 0.0
         self.new_vel = True
+        self.debug = False
+        self.angle_adjust = 0
+        self.delay_print = 0
 
         self.cmd_msg = None
         """ Set up the node. """
         rospy.init_node('motor_endpoint')
         rospy.loginfo("Starting motor node!")
         #Connect to arduino for sending speed
         try:
-            #rospy.loginfo("remove comments")
-            self.speed_ser = serial.Serial(cart_port, 57600, write_timeout=0)
+            rospy.loginfo("remove comments")
+            #self.speed_ser = serial.Serial(cart_port, 57600, write_timeout=0)
         except Exception as e:
             print( "Motor_endpoint: " + str(e))
             rospy.logerr("Motor_endpoint: " + str(e))
@@ -34,12 +38,13 @@ def __init__(self):
         """
         Connect to arduino for steering
         """
-        self.speed_string = ''
         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.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():
@@ -51,6 +56,8 @@ def motion_callback(self, planned_vel_angle):
         self.cmd_msg = planned_vel_angle  
         self.new_vel = True     
 
+    def param_callback(self, msg):
+        self.angle_adjust += (msg.data * 10)
 
     def kill_callback(self, data):
         if self.cmd_msg is not None:
@@ -60,10 +67,13 @@ def kill_callback(self, data):
         if self.killswitch:
             self.cmd_msg.vel = -100
 
+    def debug_callback(self, msg):
+        self.debug = msg.data
+
 
     def send_to_motors(self):
         if self.new_vel:
-            self.new_vel = False
+            #self.new_vel = False #this is set to false after adjusting the new target angle below
             self.cmd_msg.vel *= 50
             self.cmd_msg.vel_curr *= 50
         if self.cmd_msg.vel > 254:
@@ -75,11 +85,19 @@ def send_to_motors(self):
         target_speed = int(self.cmd_msg.vel) #float64
         current_speed = int(self.cmd_msg.vel_curr) #float64
 
-        target_angle = 100 - int(( (self.cmd_msg.angle + 60) / 120 ) * 100)
-
-        # rospy.loginfo(str(target_speed) + " " + str(current_speed) + " " + str(target_angle))
-        rospy.loginfo("Endpoint Angle: " + str(target_angle))
-        rospy.loginfo("Endpoint Speed: " + str(target_speed))
+        target_angle = 100 - int(( (self.cmd_msg.angle + 70) / 140 ) * 100)
+        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))
+        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)
 
@@ -89,7 +107,7 @@ def send_to_motors(self):
         else:
             bitstruct.pack_into('u8u8u8u8u8', data, 0, 42, 21, abs(target_speed), 0, target_angle)
 
-        self.speed_ser.write(data) 
+        #self.speed_ser.write(data) 
 
 
 if __name__ == "__main__":

cart_endpoints/scripts/ros_client.py

@@ -0,0 +1,145 @@
+#!/usr/bin/env python
+
+# import gps
+# import speed
+import socketio
+import time
+import rospy
+import json
+# import camera
+# 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 speech_recognition_location import startRecognize,stopRecognize
+isConnected = False
+
+id = '1bcadd2fea88'
+
+sio = socketio.Client()
+
+
+
+@sio.event(namespace='/cart')
+def connect():
+    print('id: ', sio.sid)
+    print('connection established')
+    send('connect', '23423')
+
+
+
+    isConnected = True
+    # while isConnected:
+    #     send('video', camera.getVideo())
+    #     time.sleep(.1)
+
+
+def send(msg, data):
+    sio.emit(msg, data, namespace='/cart')
+
+@sio.on('transit_await',namespace='/cart')
+def onTransitAwait(data):
+    location_speech_pub.publish(True)
+    #startRecognize(sendAudio)
+
+def sendAudio(msg):
+    json = {
+        "msg": msg,
+        "id":id
+    }
+    send('audio',JSON.dumps(json))
+
+#audio call pullover
+def sendPullOver():
+   send('pull_over',id)
+
+#pose tracking send when unsafe
+def sendUnsafe():
+    send('passenger_unsafe',id)
+
+
+def sendPassengerExit():
+    send('passenger_exit',id)
+
+#pose tracking send when ready, AI start driving
+def sendReady():
+    send('passenger_ready',id)
+
+@sio.on('pull_over',namespace='/cart')
+def onPullOver():
+    print("help this needs changed")
+    #do stuff for the AI
+
+@sio.on('resume_driving',namespace='/cart')
+def onResume():
+    print("help this needs changed")
+    #do stuff for the AI
+
+#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}
+    raw_waypoint = data
+
+    #Prepare goal waypoint message
+    requested_waypoint = GoalWaypoint()
+    requested_waypoint.start = -1
+    requested_waypoint.goal = raw_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)
+
+@sio.event(namespace='/cart')
+def disconnect():
+    #camera.cleanUp()
+    isConnected = False
+    print('disconnected from server')
+
+def sendPositionIndex(data):
+    send("position", data.data)
+
+def arrivedDestination(data):
+	send('arrived','','/cart')
+
+#Handles destination arrival as well as various other vehicle state changes
+def status_update(data):
+    if data.is_navigating == False:
+        if data.reached_destination == True:
+            send("arrived", '/cart')
+
+def pulloverCallback(data):
+    if data:
+        sendPullOver()
+    else:
+        pass
+        #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)
+    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)
+    rate = rospy.Rate(10)  # 10hz
+
+    #rospy.spin()
+    rospy.loginfo("Attempting connection with socketio server")
+    sio.connect('http://35.238.125.238:8020', namespaces=['/cart'])
+    while not rospy.is_shutdown():
+        rate.sleep()
+
+