Merge pull request #127 from krishauser/A4_planning

A4 planning merge request into teamA branch

Author: udymd

GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml

@@ -2,4 +2,5 @@ ros_topics:
   top_lidar: /ouster/points
   front_camera: /oak/rgb/image_raw
   front_depth: /oak/stereo/image_raw
-  gnss: /novatel/inspva
+  gnss: /septentrio_gnss/insnavgeod
+

GEMstack/onboard/interface/gem_hardware.py

@@ -1,7 +1,7 @@
 from .gem import *
 from ...utils import settings
 import math
-
+import time
 # ROS Headers
 import rospy
 from std_msgs.msg import String, Bool, Float32, Float64
@@ -178,13 +178,15 @@ def callback_with_gnss_reading(inspva_msg: Inspva):
                 else:
                     def callback_with_gnss_reading(msg: INSNavGeod):
                         pose = ObjectPose(ObjectFrameEnum.GLOBAL,
+                                    t = time.time(),
                                     x=msg.longitude,
                                     y=msg.latitude,
                                     z=msg.height,
                                     yaw=math.radians(msg.heading),  #heading from north in degrees (TODO: maybe?? check this)
                                     roll=math.radians(msg.roll),
                                     pitch=math.radians(msg.pitch),
                                     )
+                        # print("@@@@@, POSE", pose.x, pose.y)
                         speed = np.sqrt(msg.ve**2 + msg.vn**2)
                         callback(GNSSReading(pose,speed,('error' if msg.error else 'ok')))
                     self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading)

GEMstack/onboard/perception/state_estimation.py

@@ -37,8 +37,8 @@ def state_outputs(self) -> List[str]:
         return ['vehicle']
 
     def healthy(self):
-        return True
-        # return self.gnss_pose is not None TODO: fix
+        # return True
+        return self.gnss_pose is not None
 
     def update(self) -> VehicleState:
         if self.gnss_pose is None:

GEMstack/onboard/planning/longitudinal_planning.py

@@ -19,7 +19,11 @@ drive:
         route_planning:
             type: StaticRoutePlanner
             args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv','start']
-        motion_planning: longitudinal_planning.YieldTrajectoryPlanner
+        motion_planning:
+            type: longitudinal_planning.YieldTrajectoryPlanner
+            args:
+                mode: 'real'
+                planner: 'dt' # 'milestone', 'dt', or 'dx'
         trajectory_tracking:
             type: pure_pursuit.PurePursuitTrajectoryTracker
             print: False
@@ -97,3 +101,8 @@ variants:
                     # agent_detection : pedestrian_detection.FakePedestrianDetector2D
                     agent_detection : OmniscientAgentDetector  #this option reads agents from the simulator
                     state_estimation : OmniscientStateEstimator
+                planning:
+                    motion_planning:
+                        type: longitudinal_planning.YieldTrajectoryPlanner
+                        args:
+                            mode: 'simulation'

launch/pedestrian_detection.yaml

@@ -3,7 +3,9 @@ agents:
   ped1:
     type: pedestrian
     position: [15.0, 2.0]
+    # position: [10.0, 2.0]
     nominal_velocity: 0.5
     target: [15.0,10.0]
+    # target: [10.0,10.0]
     behavior: loop
 

scenes/xyhead_demo.yaml

@@ -0,0 +1,30 @@
+#needed to import GEMstack from top level directory
+import sys
+import os
+sys.path.append(os.getcwd())
+
+from GEMstack.onboard.planning.longitudinal_planning import CollisionDetector
+import time
+
+if __name__ == "__main__":
+    # Vehicle parameters. x, y, theta (angle in radians)
+    x1, y1, t1 = 4.0, 5.0, 0
+    # Pedestrian parameters. x, y, theta (angle in radians)
+    x2, y2, t2 = 15.0, 2.1, 0
+    # Velocity vectors: [vx, vy]
+    v1 = [0.1, 0]     # Vehicle speed vector
+    v2 = [0, 0.5]     # Pedestrian speed vector
+    # Total simulation time
+    total_time = 10.0
+
+    desired_speed = 0.5
+    acceleration  = -0.5
+
+    # Create and run the simulation.
+    start_time = time.time()
+    # Simulate with the above parameters: Whether to hit without decelerating
+    sim = CollisionDetector(x1, y1, t1, x2, y2, t2, v1, v2, total_time, desired_speed, acceleration)
+
+    collision_distance = sim.run(is_displayed=True)
+    print(f"Collision distance: {collision_distance}")
+    # print(f"Simulation took {time.time() - start_time:.3f} seconds.")

test_longitudinal_plan.py

@@ -6,7 +6,11 @@
 from GEMstack.state import Path, ObjectFrameEnum
 from GEMstack.onboard.planning.longitudinal_planning import longitudinal_plan,longitudinal_brake
 import matplotlib.pyplot as plt
-    
+
+
+mode = "milestone" # milestone, dt, dx
+
+
 def test_longitudinal_planning():
     test_path = Path(ObjectFrameEnum.START,[(0,0),(1,0),(2,0),(3,0),(4,0),(5,0),(6,0)])
     test_path2 = Path(ObjectFrameEnum.START,[(5,0),(6,1),(7,2),(9,4)])
@@ -39,7 +43,7 @@ def test_longitudinal_planning():
     plt.ylabel('velocity')
     plt.show()
 
-    test_traj = longitudinal_plan(test_path, 1.1, 2.0, 3.0, 0.0)
+    test_traj = longitudinal_plan(test_path, 1.1, 2.0, 3.0, 0.0, mode)
     assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) )
     plt.plot(test_traj.times,[p[0] for p in test_traj.points])
     plt.title("Accelerating from 0 m/s")
@@ -54,7 +58,7 @@ def test_longitudinal_planning():
     plt.show()
 
 
-    test_traj = longitudinal_plan(test_path, 1.0, 2.0, 3.0, 2.0)
+    test_traj = longitudinal_plan(test_path, 1.0, 2.0, 3.0, 2.0, mode)
     assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) )
     plt.plot(test_traj.times,[p[0] for p in test_traj.points])
     plt.title("Accelerating from 2 m/s")
@@ -69,7 +73,7 @@ def test_longitudinal_planning():
     plt.show()
 
 
-    test_traj = longitudinal_plan(test_path, 0.0, 2.0, 3.0, 3.1)
+    test_traj = longitudinal_plan(test_path, 0.0, 2.0, 3.0, 3.1, mode)
     assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) )
     plt.plot(test_traj.times,[p[0] for p in test_traj.points])
     plt.title("Keeping constant velocity at 3.1 m/s")
@@ -83,7 +87,7 @@ def test_longitudinal_planning():
     plt.ylabel('velocity')
     plt.show()
 
-    test_traj = longitudinal_plan(test_path, 2.0, 2.0, 20.0, 10.0)
+    test_traj = longitudinal_plan(test_path, 2.0, 2.0, 20.0, 10.0, mode)
     assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) )
     plt.plot(test_traj.times,[p[0] for p in test_traj.points])
     plt.title("Too little time to stop, starting at 10 m/s")
@@ -110,7 +114,7 @@ def test_longitudinal_planning():
     plt.ylabel('velocity')
     plt.show()
 
-    test_traj = longitudinal_plan(test_path2, 1.0, 2.0, 3.0, 0.0)
+    test_traj = longitudinal_plan(test_path2, 1.0, 2.0, 3.0, 0.0, mode)
     plt.plot(test_traj.times,[p[0] for p in test_traj.points])
     plt.title("Nonuniform planning")
     plt.xlabel('time')