merged

Author: danielzhuang11

.gitignore

@@ -165,3 +165,11 @@ cython_debug/
 #  and can be added to the global gitignore or merged into this file.  For a more nuclear
 #  option (not recommended) you can uncomment the following to ignore the entire idea folder.
 #.idea/
+
+.vscode/
+ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run
+onedrive_config.json
+ros.asc
+scenes/gazebo.yaml
+zed_sdk.run
+

GEMstack/knowledge/routes/gazebo.csv

@@ -1,32 +1,55 @@
+0.0 , 0.0
+-0.010000000000000009 , 0.0
+-0.020000000000000018 , 0.0
 -0.030000000000000027 , 0.0
--0.1499999999999999 , 0.010000000000001563
--0.45999999999999996 , 0.010000000000001563
--0.9300000000000002 , 0.019999999999999574
--1.7200000000000002 , 0.030000000000001137
--3.7699999999999996 , 0.05000000000000071
--4.98 , 0.05999999999999872
--7.029999999999999 , 0.0799999999999983
--8.33 , 0.08999999999999986
--9.84 , 0.10999999999999943
--12.61 , 0.14000000000000057
--14.14 , 0.14999999999999858
--15.510000000000002 , 0.1700000000000017
--18.48 , 0.21000000000000085
--19.85 , 0.23000000000000043
--21.07 , 0.23999999999999844
--22.54 , 0.26000000000000156
--24.29 , 0.23000000000000043
--26.47 , -0.05000000000000071
--28.84 , -0.8399999999999999
--31.049999999999997 , -2.34
--32.37 , -4.870000000000001
--32.12 , -7.75
--30.13 , -9.96
--27.68 , -10.510000000000002
--25.19 , -9.71
--23.15 , -7.629999999999999
--21.85 , -4.699999999999999
--21.25 , -0.9699999999999989
--21.86 , 0.7800000000000011
--21.94 , 1.0700000000000003
--21.91 , 0.8500000000000014
+-0.050000000000000044 , 0.0
+-0.09000000000000008 , 0.0
+-0.15999999999999992 , 0.0
+-0.26 , 0.0
+-0.3999999999999999 , 0.0
+-0.6100000000000001 , 0.0
+-0.9099999999999999 , 0.0
+-1.2899999999999998 , 0.0
+-1.7 , 0.0
+-2.1100000000000003 , 0.0
+-2.49 , 0.0
+-2.8900000000000006 , 0.0
+-3.38 , 0.0
+-3.96 , 0.0
+-4.63 , 0.0
+-5.4 , 0.0
+-6.21 , 0.0
+-6.98 , 0.0
+-7.790000000000001 , 0.0
+-8.67 , 0.0
+-9.61 , 0.0
+-10.63 , 0.0
+-11.69 , 0.0
+-12.700000000000001 , 0.0
+-13.78 , 0.0
+-14.9 , 0.0
+-16.049999999999997 , 0.010000000000001563
+-17.209999999999997 , 0.010000000000001563
+-18.34 , 0.010000000000001563
+-19.459999999999997 , 0.010000000000001563
+-20.56 , 0.010000000000001563
+-21.599999999999998 , 0.010000000000001563
+-22.599999999999998 , 0.019999999999999574
+-23.56 , 0.019999999999999574
+-24.47 , 0.019999999999999574
+-25.36 , 0.019999999999999574
+-26.24 , 0.019999999999999574
+-27.029999999999998 , 0.010000000000001563
+-27.799999999999997 , -0.030000000000001137
+-28.529999999999998 , -0.10999999999999943
+-29.18 , -0.25
+-29.779999999999998 , -0.4800000000000004
+-31.529999999999998 , -0.17999999999999972
+-33.82 , -1.2399999999999984
+-35.03 , -3.0799999999999983
+-35.34 , -5.609999999999999
+-33.6 , -8.440000000000001
+-30.55 , -9.5
+-28.25 , -8.219999999999999
+-26.439999999999998 , -4.300000000000001
+-24.47 , -1.1499999999999986

GEMstack/onboard/interface/gem_gazebo.py

@@ -620,12 +620,12 @@ def quaternion_to_euler(x, y, z, w):
         msg = AckermannDrive()
         msg.acceleration = acceleration
         msg.speed = float('inf') if acceleration >0 else 0  #acceleration * self.dt 
-        msg.steering_angle = -phides
-        msg.steering_angle_velocity = -steering_angle_rate
+        msg.steering_angle = phides
+        msg.steering_angle_velocity = steering_angle_rate
 
 
-        self.ackermann_pub.publish(msg)
-        rospy.loginfo(f"Published AckermannDrive: {msg}")
+        #self.ackermann_pub.publish(msg)
+        #rospy.loginfo(f"Published AckermannDrive: {msg}")
         self.last_command = command
 
 

GEMstack/onboard/planning/pure_pursuit.py

@@ -8,6 +8,9 @@
 from ..interface.gem import GEMVehicleCommand
 from ..component import Component
 import numpy as np
+import rospy
+from ackermann_msgs.msg import AckermannDrive
+
 
 class PurePursuit(object):   
     """Implements a pure pursuit controller on a second-order Dubins vehicle."""
@@ -39,6 +42,7 @@ def __init__(self, lookahead = None, lookahead_scale = None, crosstrack_gain = N
         self.current_traj_parameter = 0.0
         self.t_last = None
 
+
     def set_path(self, path : Path):
         if path == self.path_arg:
             return
@@ -91,8 +95,8 @@ def compute(self, state : VehicleState, component : Component = None):
         #(rather than just advancing the parameter)
         des_parameter = closest_parameter + self.look_ahead + self.look_ahead_scale * speed
         print("Closest parameter: " + str(closest_parameter),"distance to path",closest_dist)
-        if closest_dist > 0.1:
-            print("Closest point",self.path.eval(closest_parameter),"vs",(curr_x,curr_y))
+        #if closest_dist > 0.1:
+        rospy.loginfo(f"Closest point {self.path.eval(closest_parameter)} vs ,{(curr_x,curr_y)}")
         if des_parameter >= self.path.domain()[1]:
             #we're at the end of the path, calculate desired point by extrapolating from the end of the path
             end_pt = self.path.points[-1]
@@ -106,7 +110,7 @@ def compute(self, state : VehicleState, component : Component = None):
             desired_x,desired_y = self.path.eval(des_parameter)
         desired_yaw = np.arctan2(desired_y-curr_y,desired_x-curr_x)
         #print("Desired point",(desired_x,desired_y)," with lookahead distance",self.look_ahead + self.look_ahead_scale * speed)
-        #print("Current yaw",curr_yaw,"desired yaw",desired_yaw)
+        print("Current yaw",curr_yaw,"desired yaw",desired_yaw)
 
         # distance between the desired point and the vehicle
         L = transforms.vector2_dist((desired_x,desired_y),(curr_x,curr_y))
@@ -127,8 +131,8 @@ def compute(self, state : VehicleState, component : Component = None):
         #print("Closest point distance: " + str(L))
         print("Forward velocity: " + str(speed))
         ct_error = np.sin(alpha) * L
-        print("Crosstrack Error: " + str(round(ct_error,3)))
-        print("Front wheel angle: " + str(round(np.degrees(f_delta),2)) + " degrees")
+        #print("Crosstrack Error: " + str(round(ct_error,3)))
+        #print("Front wheel angle: " + str(round(np.degrees(f_delta),2)) + " degrees")
         steering_angle = np.clip(front2steer(f_delta), self.steering_angle_range[0], self.steering_angle_range[1])
         print("Steering wheel angle: " + str(round(np.degrees(steering_angle),2)) + " degrees" )
 
@@ -194,6 +198,7 @@ class PurePursuitTrajectoryTracker(Component):
     def __init__(self,vehicle_interface=None, **args):
         self.pure_pursuit = PurePursuit(**args)
         self.vehicle_interface = vehicle_interface
+        self.ackermann_pub = rospy.Publisher("ackermann_cmd", AckermannDrive, queue_size=1)
 
     def rate(self):
         return 50.0
@@ -207,10 +212,20 @@ def state_outputs(self):
     def update(self, vehicle : VehicleState, trajectory: Trajectory):
         self.pure_pursuit.set_path(trajectory)
         accel,wheel_angle = self.pure_pursuit.compute(vehicle, self)
-        #print("Desired wheel angle",wheel_angle)
+        print("Desired wheel angle",wheel_angle)
         steering_angle = np.clip(front2steer(wheel_angle), self.pure_pursuit.steering_angle_range[0], self.pure_pursuit.steering_angle_range[1])
         #print("Desired steering angle",steering_angle)
-        self.vehicle_interface.send_command(self.vehicle_interface.simple_command(accel,steering_angle, vehicle))
+        #rospy.loginfo(f"accel from pure pursuit {wheel_angle}")
+
+        msg = AckermannDrive()
+        msg.acceleration = accel
+        msg.speed = 3#float('inf') if accel >0 else 0  #acceleration * self.dt 
+        msg.steering_angle = wheel_angle
+        #msg.steering_angle_velocity = steering_angle_rate
+
+        self.ackermann_pub.publish(msg)
+
+        #self.vehicle_interface.send_command(self.vehicle_interface.simple_command(accel,steering_angle, vehicle))
 
     def healthy(self):
         return self.pure_pursuit.path is not None

launch/gazebo_simulation.yaml

@@ -13,7 +13,7 @@ recovery:
 drive: 
     perception:
         state_estimation : GNSSStateEstimator
-        agent_detection : object_detection.ObjectDetection
+        #agent_detection : object_detection.ObjectDetection
     planning:
         # relations_estimation: pedestrian_yield_logic.PedestrianYielder
         # route_planning:
@@ -74,14 +74,14 @@ variants:
 
                 planning:
                     route_planning:
-                        # type: StaticRoutePlanner
-                        # args: [!relative_path '../GEMstack/knowledge/routes/gazebo.csv']
+                        type: StaticRoutePlanner
+                        args: [!relative_path '../GEMstack/knowledge/routes/gazebo.csv']
                     motion_planning:
-                        # type: RouteToTrajectoryPlanner
+                        type: RouteToTrajectoryPlanner
                         # args: [null]  #desired speed in m/s.  If null, this will keep the route untimed for the trajectory tracker
                     trajectory_tracking:
-                        # type: pure_pursuit.PurePursuitTrajectoryTracker
-                        # args: {desired_speed: 2.5}  #approximately 5mph
+                        type: pure_pursuit.PurePursuitTrajectoryTracker
+                        args: {desired_speed: 2.5}  #approximately 5mph
                         # print: False
             # visualization: !include "klampt_visualization.yaml"
             # visualization: !include "mpl_visualization.yaml"