Merge Cleanup to Master

cart_control/launch/autoware_localization.launch

@@ -21,13 +21,6 @@
 
   <!-- Location of Point Cloud Map file -->
   <arg name="map_arg" default="$(env HOME)/AVData/speedBoiMap.pcd"/>
-
-    <!-- Start lidar code first to avoid issues-->
-    <group if="$(arg lidar)">
-        <include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch"/>
-        <node name="gps_node" pkg="cart_endpoints" type="gps_node.py" output="screen"/>
-   </group> 
-         <!-- <remap from="/velodyne_points" to="/points_raw"/> -->
 
   <group>
       <!-- Start Gazebo with an empty world -->

cart_control/launch/navigation.launch

@@ -7,11 +7,12 @@
 
 
   <!-- Fine tune customization -->
+  <arg name="lidar" default="true"/>
   <arg name="endpoints" default="true"/>
   <arg name="networking" default="true"/>
   <arg name="health" default="false"/>
-  <arg name="voice" default="true"/>
-  <arg name="pose" default="true"/>
+  <arg name="voice" default="false"/>
+  <arg name="pose" default="false"/>
   <arg name="obstacle_detection" default="false"/>
   <arg name="visualize_pose" default="false"/>
 
@@ -47,16 +48,14 @@
         <arg name="pose" value="$(arg pose)"/>
         <arg name="visualize_pose" value="$(arg visualize_pose)"/>
         <arg name="obstacle_detection" value="$(arg obstacle_detection)"/>
+        <arg name="lidar" value="$(arg lidar)"/>
       </include>
   </group>
 
-  <!--always launch planning nodes-->
+  <!--always launch planning nodes, system constants-->
   <group>
       <include file="$(find cart_planning)/launch/planning.launch"/>
-  </group>
-
-  <!--launch the constants on the param server -->
-  <group>
       <include file="$(find cart_planning)/launch/constants.launch"/>
   </group>
+
 </launch>

cart_control/package.xml

@@ -2,12 +2,16 @@
 <package format="2">
   <name>cart_control</name>
   <version>0.0.0</version>
-  <description>Core planning and navigation package for running and testing planning nodes</description>
+  <description>Core package for the primary launch files used to launch the cart system. 
+  Typically navigation.launch calls autoware_localization.launch.
+  For more details on launch cases refer to the wiki.</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
   <maintainer email="[email protected]">weilannw</maintainer>
+  <maintainer email="[email protected]">Tyree Mitchell</maintainer>
+  <maintainer email="[email protected]">Maddie Brower</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->

cart_endpoints/launch/hardware_interface.launch

@@ -2,10 +2,17 @@
 <launch>
     <arg name="networking" default="true"/>
     <arg name="health" default="true"/>
-    <arg name="voice" default="true"/>
+    <arg name="voice" default="false"/>
     <arg name="pose" default="true"/>
     <arg name="visualize_pose" default="false"/>
     <arg name="obstacle_detection" default="true"/>
+    <arg name="lidar" default="true"/>
+
+       <!-- Start lidar code -->
+    <group if="$(arg lidar)">
+        <include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch"/>
+        <node name="gps_node" pkg="cart_endpoints" type="gps_node.py" output="screen"/>
+   </group> 
 
     <group>
         <node name="motor_endpoint" pkg="cart_endpoints" type="motor_endpoint.py" output="screen"/>
@@ -14,15 +21,17 @@
         <node name="cart_health_monitor" pkg="cart_endpoints" type="cart_health_monitor.py" output="screen"/>
     </group>
     <group if="$(arg networking)">
-        <node name="network_node" pkg="cart_endpoints" type="network_node.py" output="screen"/>
+        <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch">
+            <arg name="port" value="9090"/>
+        </include>
     </group>
     <group if="$(arg pose)">
         <node name="pose_tracking" pkg="cart_endpoints" type="pose_tracking.py"/>
     </group>
     <!--<include file="$(find rplidar_ros)/launch/rplidar_a3.launch"/>-->
     <group if="$(arg obstacle_detection)">
         <node name="obstacle_detector" pkg="cart_endpoints" type="obstacle_detector.py" output="screen"/>
-        <node name="local_planner" pkg="cart_endpoints" type="local_planner.py" output="screen"/>
+        <node name="collision_detector" pkg="cart_planning" type="collision_detector.py" output="screen"/>
     </group>
     <include file="$(find video_stream_opencv)/launch/camera.launch">
             <!--This is referring to the zed camera facing the passenger-->
@@ -32,7 +41,6 @@
             <arg name="visualize" value="$(arg visualize_pose)" />
         </include>
     <include file="$(find video_stream_opencv)/launch/camera.launch">
-            This is referring to the front facing camera
             <arg name="camera_name" value="front_facing"/>
             <arg name="video_stream_provider" value="/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._H264_USB_Camera_SN0001-video-index0" />
         </include>

cart_endpoints/package.xml

@@ -2,11 +2,14 @@
 <package format="2">
   <name>cart_endpoints</name>
   <version>0.0.0</version>
-  <description>Package for golfcart's hardware endpoints that interface with planning</description>
+  <description>Package for golfcart's hardware endpoints that interface with planning. 
+  These endpoints essentially deal with data incoming to the system (obtaining LiDAR data) and 
+  data going outside (motor endpoint).</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">Tyree Mitchell</maintainer>
   <maintainer email="[email protected]">Maddie Brower</maintainer>
 
 

cart_endpoints/scripts/cart_health_monitor.py

@@ -5,52 +5,71 @@
 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
+from std_msgs.msg import Bool, Float32
 
 class CartHealth(object):
 
     def __init__(self):
         rospy.init_node('cart_health_monitor')
 
-        self.is_navigating = False
+        self.vehicle_state = VehicleState()
         self.stopping = False
+        # Safety maximums (Speed, and localizer health)
+        self.max_speed = rospy.get_param('max_speed')
+        self.max_ndt_health = rospy.get_param('max_ndt_health')
+
         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)
 
-        self.vehicle_speed_sub = rospy.Subscriber('/estimate_twist', TwistStamped, self.speed_check)
+        self.vehicle_speed_sub = rospy.Subscriber('/estimated_vel_mps', Float32, self.speed_check)
         rospy.spin()
 
     # Keep track if cart is navigating
     def status_update(self, msg):
-        self.is_navigating = False
-        if msg.is_navigating:
-            self.is_navigating = True
+        self.vehicle_state = msg
 
     # Check the localizer's current health
     def ndt_stat_cb(self, msg):
-        cur_fitness = msg.score
-        if cur_fitness > 1.0 and cur_fitness < 1.5:
-            rospy.logwarn("Localizer has bad health, current health: " + str(cur_fitness))
-        elif cur_fitness >= 1.5:
-            rospy.logfatal("Localizer has extremely bad health")
-            if self.is_navigating:
+        """ Makes sure there is a decent localization at all times during cart navigation.
+        Once stopped for this reason there is no allowed resuming, a system restart is required in this case.
+
+        Args:
+            msg(NDTStat ROS MSG): Message coming from the NDT Localizer containing health
+        """
+        if self.vehicle_state.is_navigating:
+            cur_fitness = msg.score
+            # If we have a decent amount of bad health but not as bad as maximum
+            if cur_fitness > 2.0 and cur_fitness < (self.max_ndt_health-1):
+                rospy.logwarn("Localizer has bad health, current health: " + str(cur_fitness))
+            elif cur_fitness >= self.max_ndt_health:
+                rospy.logfatal("Localizer has extremely bad health")
                 rospy.logfatal("Sending Emergency Stop due to bad localization!")
-                self.send_stop(True, 4)
-                self.is_navigating = False
+                self.send_stop(True, "Localizer")
 
     def speed_check(self, msg):
-        if msg.twist.linear.x >= 4:
+        """ Makes sure the cart doesn't overspeed, if for some reason the cart overspeeds stop it
+
+        Args:
+            msg(Float32 ROS MSG): Contains estimated speed of cart in Meters per Second
+        """
+        if msg.data >= self.max_speed:
             rospy.logfatal("Overspeeding! Sending Emergency Stop message!")
-            self.send_stop(True, 4)
+            self.send_stop(True, "Overspeed")
             self.stopping = True
-        elif self.stopping == True and msg.twist.linear.x <= 2:
+        elif self.stopping == True and msg.data <= 1:
             self.stopping = False
-            rospy.loginfo("Speed has been reduced resuming ride!")
-            self.send_stop(False, 4)
+            rospy.loginfo("Speed has been reduced, resuming ride!")
+            self.send_stop(False, "Overspeed")
 
     def send_stop(self, stop, sender_id):
+        """ Function for quickly defining and sending an emergency stop message
+
+        Args:
+            stop(Boolean): Make a request to stop(True) or continue(False)
+            sender_id(String): Unique identifier of function or node making the request 
+        """
         stop_msg = EmergencyStop()
         stop_msg.emergency_stop = stop
         stop_msg.sender_id = sender_id

cart_endpoints/scripts/gps_node.py

@@ -2,7 +2,7 @@
 import socket
 import rospy
 import math
-from sensor_msgs.msg import NavSatFix
+from navigation_msgs.msg import LatLongPoint
 from geometry_msgs.msg import PoseStamped
 
 class GPS_Parser(object):
@@ -12,7 +12,7 @@ def __init__(self):
 
         self.static_position = None
 
-        self.gps_pub = rospy.Publisher('/gps_coordinates', NavSatFix, queue_size = 10)
+        self.gps_pub = rospy.Publisher('/gps_send', LatLongPoint, queue_size = 10)
         self.loc_sub = rospy.Subscriber('/ndt_pose', PoseStamped, self.location_callback)
 
         self.UDP_IP = "192.168.1.201"#"192.168.3.100"
@@ -24,6 +24,7 @@ def __init__(self):
         # Publish the starting position
         self.get_and_pub_packet()
         r = rospy.Rate(10)
+        self.get_and_pub_packet()
         r.sleep()
         while not rospy.is_shutdown():
             if self.static_position is not None:
@@ -46,7 +47,7 @@ def get_and_pub_packet(self):
             data, addr = self.sock.recvfrom(512)
             line = data[206:278]
 
-            gps_coords = NavSatFix()
+            gps_coords = LatLongPoint()
             gps_coords.header.stamp = rospy.Time.now()
 
             latitude_degrees = float(line[16:18])
@@ -67,7 +68,7 @@ def get_and_pub_packet(self):
 
         gps_coords.latitude = latitude_total / poll_size
         gps_coords.longitude = longitude_total / poll_size
-        gps_coords.altitude = 0.0
+        gps_coords.elevation = 0.0
 
         self.gps_pub.publish(gps_coords)
 

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_dictionary = {0: False, 1: False, 2: False, 3:False, 4:False}
+        self.stop = False
         self.delay_print = 0
         self.brake = int(255)
         self.drove_since_braking = True
@@ -40,7 +40,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', EmergencyStop,
+        self.stop_subscriber = rospy.Subscriber('/stop_vehicle', Bool,
                                                       self.stop_callback, queue_size=10)
         self.param_subscriber = rospy.Subscriber('/realtime_a_param_change', Int8, self.param_callback, queue_size=10)
 
@@ -61,7 +61,7 @@ def param_callback(self, msg):
         self.angle_adjust += (msg.data * 10)
 
     def stop_callback(self, msg):
-        self.stopping_dictionary[msg.sender_id] = msg.emergency_stop
+        self.stop = msg.data
 
     def debug_callback(self, msg):
         self.debug = msg.data
@@ -91,12 +91,6 @@ def send_to_motors(self):
         target_angle = 100 - int(( (self.cmd_msg.angle + 45) / 90 ) * 100)
         # rospy.loginfo("Angle after range adjustment: " + str(target_angle))
         #adjust the target angle additionally using a realtime adjustable testing value
-        if self.new_vel:
-            # rospy.loginfo("Doing a realtime adjustment: " + str(target_angle))
-            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)
         # rospy.loginfo("Before readied data" + str(data))
         data = bytearray(b'\x00' * 5)
@@ -109,17 +103,8 @@ def send_to_motors(self):
                 rospy.loginfo("Endpoint Angle: " + str(target_angle))
                 rospy.loginfo("Endpoint Speed: " + str(target_speed))
 
-        # 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
-        for x in self.stopping_dictionary.values():
-           #rospy.loginfo(x)
-           pass
-        if any(x == True for x in self.stopping_dictionary.values()):
+        # Check for request to stop vehicle
+        if self.stop:
             target_speed = (int(-self.brake))
             if(self.drove_since_braking == True):
                 self.braking_duration = 3

cart_endpoints/scripts/network_node.py

@@ -11,7 +11,7 @@
 import base64
 import cv2
 from cv_bridge import CvBridge
-from std_msgs.msg import Int8, String, Bool
+from std_msgs.msg import Int8, UInt64, String, Bool
 from geometry_msgs.msg import PoseStamped
 from navigation_msgs.msg import GoalWaypoint, VehicleState, EmergencyStop, LatLongPoint
 from sensor_msgs.msg import NavSatFix, Image
@@ -222,6 +222,9 @@ def front_image_callback(img_msg):
         send('cart-video', base64.b64encode(buffer))
         last_front_pub = cur_time
 
+def eta_callback(msg):
+    send('arrived-time', msg.data)
+
 if __name__ == "__main__":
     rospy.init_node('network_node')
     server_lock = threading.Lock()
@@ -249,6 +252,7 @@ def front_image_callback(img_msg):
     rospy.Subscriber('/gps_coordinates', NavSatFix, send_location)
     rospy.Subscriber('/passenger_safe', Bool, passenger_safe_callback)
     rospy.Subscriber('/passenger_exit', Bool, passenger_exit_callback)
+    rospy.Subscriber('/eta', UInt64, eta_callback)
 
     exit_sound = vlc.MediaPlayer(os.path.join(os.path.expanduser("~"), "catkin_ws/src/ai-navigation/cart_endpoints/sounds/", "exit.mp3"))
     enter_sound = vlc.MediaPlayer(os.path.join(os.path.expanduser("~"), "catkin_ws/src/ai-navigation/cart_endpoints/sounds/", "enter.mp3"))

cart_endpoints/scripts/speech_recognition_core.py

@@ -57,7 +57,7 @@ def __init__(self):
     def listener(self, recognizer, audio):
         try:
             #audio = self.r.listen(source, phrase_time_limit=5)
-            text = recognizer.recognize_google(audio)
+            text = recognizer.recognize_sphinx(audio)
             text = text.lower()
             text_array = text.split()
             if self.text_passing: