diff --git a/cart_control/launch/navigation.launch b/cart_control/launch/navigation.launch index aa657cad..aaefe683 100644 --- a/cart_control/launch/navigation.launch +++ b/cart_control/launch/navigation.launch @@ -1,10 +1,11 @@ - + - + + @@ -17,6 +18,7 @@ + diff --git a/cart_endpoints/launch/hardware_interface.launch b/cart_endpoints/launch/hardware_interface.launch index 84722359..6880a5fa 100644 --- a/cart_endpoints/launch/hardware_interface.launch +++ b/cart_endpoints/launch/hardware_interface.launch @@ -2,13 +2,19 @@ + + + - + + + + diff --git a/cart_endpoints/scripts/cart_health_monitor.py b/cart_endpoints/scripts/cart_health_monitor.py index 0067bc05..8238993b 100755 --- a/cart_endpoints/scripts/cart_health_monitor.py +++ b/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: diff --git a/cart_endpoints/scripts/gps_node.py b/cart_endpoints/scripts/gps_node.py old mode 100644 new mode 100755 index 904fef2d..74394908 --- a/cart_endpoints/scripts/gps_node.py +++ b/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) diff --git a/cart_endpoints/scripts/motor_endpoint.py b/cart_endpoints/scripts/motor_endpoint.py index 4aebdf7c..8cd024df 100755 --- a/cart_endpoints/scripts/motor_endpoint.py +++ b/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) diff --git a/cart_endpoints/scripts/ros_client.py b/cart_endpoints/scripts/ros_client.py index 15060e46..cf91e6e8 100755 --- a/cart_endpoints/scripts/ros_client.py +++ b/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,29 +66,33 @@ 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) @@ -95,9 +100,7 @@ def onDestination(data): @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,16 +120,20 @@ 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) @@ -134,6 +141,7 @@ def pulloverCallback(data): 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() diff --git a/cart_endpoints/scripts/speech_recognition_core.py b/cart_endpoints/scripts/speech_recognition_core.py index c7c97e79..e2c0ce08 100755 --- a/cart_endpoints/scripts/speech_recognition_core.py +++ b/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: