reads in sim value appropriately

cpp_pid/plot.py

@@ -2,15 +2,14 @@
 import matplotlib.pyplot as plt
 import pandas as pd
 
-filename = '/home/auvsl/ros2_ws/2024-10-09_19-26-50'
-cols = ['x','y', 'theta', 'input1', 'input2', 'input3', 'input4', 'linear', 'angular']
+filename = '/home/auvsl/ros2_ws/2024-11-01_14-04-31'
+cols = ['x','y', 'theta', 'input', 'output']
 data = pd.read_csv(filename,names=cols,header=None)
 
 plt.figure()
-plt.plot(np.array([-9.60897, -0.608969, 8.39103, 15.391]),np.array([-2.95891, -2.95891, -2.95891, -2.95891]), 'o')
 plt.plot(data['x'].values,data['y'].values)
-plt.xlim(-50,50)
-plt.ylim(-50,50)
+plt.xlim(0,200)
+plt.ylim(-100,0)
 plt.xlabel('X Position (m)')
 plt.ylabel('Y Position (m)')
 plt.title('XY Position of Rgator When Manually Driving')
@@ -21,40 +20,16 @@
 plt.xlabel('Iterations')
 plt.ylabel('VectorNav Yaw (rad)')
 
-# plt.figure()
-# plt.plot(data['input1'].values)
-# plt.title('Distance Line over Iterations')
-# plt.xlabel('Iterations')
-# plt.ylabel('Distance Line (m)')
-
-# plt.figure()
-# plt.plot(data['input2'].values)
-# plt.title('Theta Near over Iterations')
-# plt.xlabel('Iterations')
-# plt.ylabel('Theta Near (rad)')
-
-# plt.figure()
-# plt.plot(data['input3'].values)
-# plt.title('Theta Far over Iterations')
-# plt.xlabel('Iterations')
-# plt.ylabel('Theta Far (rad)')
-
-# plt.figure()
-# plt.plot(data['input4'].values)
-# plt.title('Distance Target over Iterations')
-# plt.xlabel('Iterations')
-# plt.ylabel('Distance Target (m)')
-
-# plt.figure()
-# plt.plot(data['linear'].values)
-# plt.title('Linear Speed over Iterations')
-# plt.xlabel('Iterations')
-# plt.ylabel('Linear Speed (m/s)')
+plt.figure()
+plt.plot(data['input'].values)
+plt.title('Input over Iterations')
+plt.xlabel('Iterations')
+plt.ylabel('Distance Line (m)')
 
-# plt.figure()
-# plt.plot(data['angular'].values)
-# plt.title('Agnular Velocity over Iterations')
-# plt.xlabel('Iterations')
-# plt.ylabel('Angular Velocity (rad/s)')
+plt.figure()
+plt.plot(data['output'].values)
+plt.title('Angular Velocity over Iterations')
+plt.xlabel('Iterations')
+plt.ylabel('Angular Velocity (rad/s)')
 
 plt.show()

cpp_pid/src/environment.cpp

@@ -140,19 +140,14 @@ namespace auvsl
     // generate inputs to Environment based on the vehicle's postion relative to relavent waypoints 
     double Environment::controllerInput(Eigen::VectorXd odomPos, Eigen::VectorXd waypoints){
         // load in the current position, the last waypoint passed, and the current waypoint being targeted, and the next waypoint to be targeted
-        Eigen::Vector2d currentPos{      odomPos(1),   odomPos(2)};
+        double currentAng = Environment::angSat(odomPos(0));
         Eigen::Vector2d past{   waypoints(0), waypoints(1)};
         Eigen::Vector2d present{waypoints(2), waypoints(3)};
-        Eigen::Vector2d future{ waypoints(4), waypoints(5)};
 
-        // the 'a' and 'b' vectors are used to compute the scale vector rejection, perpendicular distance, from the current path segement
-        Eigen::Vector2d a = currentPos - past;
-        Eigen::Vector2d b =    present - past;
+        double th1 = std::atan2(  present(1) -    past(1), present(0) -    past(0));
 
-        Eigen::Vector2d pre_norm(-b(1), b(0));
-        Eigen::Vector2d unit_vector = pre_norm / pre_norm.norm();
-
-        double error = -a.dot(unit_vector);           // distanceLine (the negative is due to how the membership functions interprests what is left/ right for this input)
+        // determine the vehicle's orientation difference relative the the current path segement and next path segment
+        double error = Environment::angSat(currentAng-th1);
 
         return error;
     }

cpp_pid/src/main.cpp

@@ -33,7 +33,7 @@ std::string getCurrentDateTime() {
 template<class T>
 void pushBack(T &mtrx, const double &x_position, const double &y_position, const double &theta, const double &input, const double &error) {
     auto n = mtrx.rows();
-    mtrx.conservativeResize(n + 1, 9);
+    mtrx.conservativeResize(n + 1, 5);
     mtrx(n, 0) = x_position;
     mtrx(n, 1) = y_position;
     mtrx(n, 2) = theta;
@@ -49,7 +49,7 @@ void writeToCSVfile(std::string name, Eigen::MatrixXd matrix)
 
 int main(int argc, char **argv)
 {   
-    double T = 50, dt = 1/T, max = 70, min = -70, Kp = 10, Ki = 2, Kd = 5;
+    double T = 50, dt = 1/T, max = 70, min = -70, Kp = 100, Ki = 0, Kd = 0;
 
     //make a server object that callsback odomentry information, an object to analysis the relevant waypoints, if the vehcile should stop, path errors fed into the controller, and a controller object
     auvsl::Server*     srvrObj = new auvsl::Server;
@@ -66,9 +66,9 @@ int main(int argc, char **argv)
     auto odomSub = node->create_subscription<vectornav_msgs::msg::CommonGroup>("/vectornav/raw/common", 1, std::bind(&auvsl::Server::odomCallback, srvrObj, std::placeholders::_1));
 
     // set the desired path 
-    Eigen::MatrixXd path {{0,0}, {1, 0}, {2, 0}, {3, 0}};
+    // Eigen::MatrixXd path {{0,0}, {10, 0}, {20, 0}, {30, 0}};
     // Eigen::MatrixXd path {{0,0}, {1,  1}, {2,  2}, {3, 3}};
-    //Eigen::MatrixXd path {{0,0}, {1, -1}, {2, -2}, {3, -3}};
+    Eigen::MatrixXd path {{0,0}, {1, -1}, {2, -2}, {3, -3}};
 
     Eigen::MatrixXd dataXYWypts; 
 
@@ -93,7 +93,7 @@ int main(int argc, char **argv)
 
                 msg2.parkbrake = 0;               // parking break false
                 msg2.gear      = 1;               // forward gear
-                msg2.throttle  = 50;              // full throttle is 100
+                msg2.throttle  = 10;              // full throttle is 100
                 msg2.steering  = output; // full steering is 100
 
                 pushBack(dataXYWypts, srvrObj->odomPos(1), srvrObj->odomPos(2), srvrObj->odomPos(0), input, output);

cpp_pid/src/server.cpp

@@ -21,7 +21,7 @@ namespace auvsl
     // store the global yaw angle, x position, and y position off the vehicle
     void Server::odomCallback(const vectornav_msgs::msg::CommonGroup::SharedPtr msg1)
     {
-        odomPos(0) = -(msg1->yawpitchroll.x) * M_PI/180.0; // convert degrees to radians
+        odomPos(0) = M_PI/2 - (msg1->yawpitchroll.x) * M_PI/180.0; // convert degrees to radians
         double lat  = (msg1->position.x) * M_PI/180.0;
         double lon  = (msg1->position.y) * M_PI/180.0;
         double alt  = msg1->position.z;
@@ -34,8 +34,5 @@ namespace auvsl
 
         // compute the north east down position in meters from the latitude, longitude, and altitude in degrees
         cppmap3d::geodetic2enu(lat, lon, alt, lat0, lon0, alt0, odomPos(1), odomPos(2), throwAway);
-
-        odomPos(0) -= 90;
-
     }	
 }

dbw/dbw/dbw_vel_sim.py

@@ -15,7 +15,7 @@ def __init__(self):
         self.dbw_sub = self.create_subscription(Dbw, '/control_cmd', self.dbw_callback, 10)
 
         # init publisher
-        self.vcu_pub = self.create_publisher(UInt8MultiArray, '/can_bus_propulsion', 10)
+        self.vcu_pub = self.create_publisher(UInt8MultiArray, '/can_bus/propulsion', 10)
 
         # init timer
         self.timer = self.create_timer(0.1, self.timer_callback)