v1.0 (#68)

* Revised readme.md, created low level control dir * fixes #16 * Updated README.md, shell script requires root * Initial commit of control * start future improvements lists * fix typo * Commit for work transfer to linux machine * #21 start dev guide * #9 configure usb_cam and add cv_camera, just in case * minor change commit * remove old version directory cv_camera * remove old version usb_cam directory * updated files from usb_cam * fix cv_camera, cleanup lanes-v1 * Implemented pathHeading Calc * cleanup and add debug code * launch views video1, does not display image (intentinoally) * Completed lat_controller core algo * Imported fix to GPS error * fixed error message. calculation likely still incorrect * reformat and add __future__ imports * Add debugger node to verify camera functionality * Fix typo in docstring * Reformat and add main() methods * Changed exception handler style * Add lane detection v2 source. Need to convert to python 2 * Built control core library * Update module README with credits * Added RAZOR 9DOF * Attempt to fix the repo link for razor * rm razor * Final IMU * Initial commit of the control node * Advanced Lanes work in python 2 * Added Interface and Common pkg * committing all changes * Added three ROS nodes. Lane offset data of a video feed can now be published * Removed swp, swn, swo files * Converting Vec3 to vector of cluster indices * Fixed some bugs * Fixed bugs * mm * Display label instead of intesnity * Maybe sped up gpf * missed semicolon * Edited params * this code is so bad :( * whoops * this is getting annoying * mm * Hopefully fixed * Implemented scanline wrapping * fix bug * mm * Maybe fixed scanline wrapping * moar bugs * mm * mm * mmm * ugh * sadasdfas * last time * Update README.md * Complete lane v2 node structure * Cleaned up lane v2 nodes * Add condition for no detected lane * Calibrate ELP camera * - Finished making Segmentation as stable as possible (for now). - Improved readability -Removed old files * Fixed gitignore * Delete .vscode files * revert to old gpf code * newest_steering_test.ino * Fixed over-segmentation of ground-plane * Add files via upload Initial low-level MCU node for low-level control. * Update redcar_control.ino Initial low-level control code for driving and steering control. * Add files via upload Initial commit of Control_Adapter_Arduino_node. * Delete low_lvl_mcu_node.py * Change no lane reading to "nan" Float32 * temp gpf fix * Implemented Point Filtering - NOT TESTED * Completed Rev1 Control node w/ custom message * Remove default test videos * Cleanup and refresh cached files * Added an IMU callback for control node to retrieve orientation * Updates after testing * #25 Add placeholder for face detector * #25 Add initial face detector * #25 Add haarcascade xml files * Add updated cache files * New point count based obstacle detection algorithm * #25 Altered CMakeLists to integrate cpp files * #25 Update face detector * Update control_node.cpp * Updated Object detection field * Change swiftnav to output vel_down * Fixed size_t error * fixed range to type float * Start launch file * Added control launch file * initial path interpolation file * Added launchfile, now publishing boolean * edited launch file * Fixed launch file ..again * Finished and tested lidar code * final parameter tweak * Fixed timer function bug, Added obstacle detection and driving mode mechanism * Add new arduino test code * Update path interpolation * Update new steering test * Fixed path heading check issue, placed CATKIN_IGNORE in localization pkg to prevent build error * Removed catkin_ignore from localization, planned to merge into ISS-29 * Added geodetic to ECEF conversion * Path interpolation publishes a path message * Set y position message to correct ouput * Path interpolation works with global reference frame * Add relative X and Y calculation * Improve interpolation function * Updated IMU Accel, Gyro Calibation * Fixed compilation error * Read locations from file * Fixed missing last point corner case in #42 * Hotfix commented out x_n * Fixed file parser #42 * Quick cleanup documentation #42 * Remove unnecessary globals * Modularize path interpolation * Add adaptive point density calculation * Add steering angle conversion * #42 debug in process for finePointsY equation * Fixed interpolation formula #42 * Cleanup #42 * Update launch file for path interpolation * Create geodesy package * Started real time position output node * Add mgrs library for UTM conversion * Remove mgrs library * Add geodetic to UTM calculation from utm pypy library * Update launch file * Add adaptive point density calc for UTM * Reorganize data #42 * Quick cleanup #42 * Cleanup Arduino and Python code * Cleanup Arduino Code * fix imu config * imu usb port change * Changed UTM to OOP implementation * Add OOP implementation for ECEF interpolation * Refactor ECEF path interpolation * Refactor UTM path interpolation * Update .gitignore * Start ENU converter * Stopped tracking cache files * Update manifest * Add live GPS position converter * Add velocity calculation * Hotfix debug message * Update geodesy launch * Including instructions at the topic for running code * Including instructions at the topic for running code * Added heading estimation into path cb * Fixed small struct bug * Fix for reseting prev_pos, added control debug layer * Added control files * Small update * Tuned PID parameters for 12 Volts and added timeout safety features * fixed rc, fixed control launch to include python node * Added rosserial launch * Added 2 new Paths * Fixed some low lvl python logic * removed old python low lvl node * launch files for path * updated joy installation * pip install * control bug fix * updated path * Obstacle detection fix * changed mitchell's code to reduce buffer size * Control Bug Fix * New student union path * Most updated Arduino code, tested some filtering at idle to lower oscillation * Control update * Arduino Fix * Preliminary error confirmation * Add point converter #53 * Fixed ENU data to ECEF data bug #53 * Update formatting for debugging #53 * Updated ardu_adapter.py and newest_steering_test.ino to remove magic numbers * Forgot an indent * working low level control code * Controller logic fix, fit was having error in having duplicated points * Start tf master node * Small fix * Fixed the wrapping angle issue where i forgot to remap the output to the right variable * Least squares gpf method w/ Eigen * Kalman Filter Core function implementationn * Fixed eigen compile errors * Update IMU message published * Move validation data to appropriate location * Note future TF msg improvements * Implemented SVD approach to groun plane fitting (NOT TESTED YET) * Fixed Eigen compilation bug * Verified that new CalculatePlaneNormal works as desired * fixed compilation bugs * Added include guards to all header files, tweaked gpf parameters * fixed z_min error * Gpf working commit * Update initial framework messages for tf * Update executable target * IMU Calibration, Control node update * Removed libuvc from util
SJSU-AD · Sep 16, 2018 · 78b9821 · 78b9821
1 parent 4c1bce0
commit 78b9821
Show file tree

Hide file tree

Showing 268 changed files with 367,768 additions and 1,417 deletions.
diff --git a/.DS_Store b/.DS_Store
diff --git a/.gitignore b/.gitignore
@@ -1,3 +1,36 @@
 build/
 devel/
 
+.vscode*
+######################
+####### Python #######
+######################
+
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+.hypothesis/
+.pytest_cache/
+
+# pyenv
+.python-version
+
+#####################
+
+# Mac OSX Cache
+.DS_Store
diff --git a/README.md b/README.md
@@ -5,7 +5,7 @@ FusionAD is an autonomous driving software stack developed by engineering studen
 #### **Run the ```ext_package_build.sh``` to ensure all the prerequisites of the external ROS packages used in this stack are met and installed**
 
 ```
-sh ext_package_build.sh
+sudo sh ext_package_build.sh
 ```
 ### Development:
-Please pull from the branch "develop", not master.
+Please develop from the branch "develop", not master.
diff --git a/arduino/newest_steering_test/newest_steering_test.ino b/arduino/newest_steering_test/newest_steering_test.ino
@@ -0,0 +1,330 @@
+/* HOW TO USE 
+ *  
+ * The Arduino is subscribed to ardu_adapter and receives steering + driving values from ardu_adapter
+ * These values have already been translated to analog inputs that are applicable to steering and driving
+ * To run this code, use the following command
+ * $rosrun rosserial_python serial_node.py /dev/ttyACM0
+ * Check the USB port and correct for these        ^^^^ if needed
+ *
+ */
+
+
+#include <ros.h>
+#include <std_msgs/Float64.h>
+ros::NodeHandle nh;
+
+std_msgs::Float64 feedback;
+std_msgs::Float64 driving_feedback;
+std_msgs::Float64 steering_error_feedback;
+ros::Publisher steering_response("/control/steering_response", &feedback); // publish error instead
+ros::Publisher driving_response("/control/driving_response", &driving_feedback);
+ros::Publisher steering_error_response("/control/steering_error", &steering_error_feedback);
+
+#include <PID_v1.h> // PID LIBRARY 
+
+#define R_EN 12 // ENABLE PIN steering
+#define L_EN 13
+
+#define RPWM 6 // inward motion steering
+#define LPWM 5 // outward motion steering
+
+#define Motor_R_EN 3 // ENABLE PIN DRIVE
+#define Motor_L_EN 4 
+
+#define Motor_RPWM 9 //
+#define Motor_LPWM 10
+
+double left_setpoint = 0; // declare ALL the variables
+double right_setpoint = 0;
+double setpoint = 319;
+double input = 0;
+double right_output = 0;
+double left_output = 0;
+double wheel_angle = 0;
+double error = 0; 
+double upper_steering_limit = 462;
+double lower_steering_limit = 182;
+double driving_limit = 100;
+double steering_tolerance = 13;
+
+/* POTENTIOMETER VALUES!!!
+   BOUNDS ARE 462 FULL LOCK RIGHT
+              319 STRAIGHT
+              182  FULL LOCK LEFT
+*/
+
+unsigned int Kp = 120; // proportional gain
+unsigned int Ki = 1;    // integral gain
+unsigned int Kd = 1;  // derivative gain
+
+PID left(&input, &left_output, &left_setpoint, Kp, Ki, Kd, REVERSE); // Turning left is more negative
+PID right(&input, &right_output, &right_setpoint, Kp, Ki, Kd, DIRECT); // Turning right is more positive (referencing the pot)
+
+float steering_value = 319;
+float driving_value = 0;
+
+unsigned long steering_timestamp = 0;
+unsigned long loop_timestamp = 0;
+unsigned long time_out = 200;
+
+
+void drivingcallback(const std_msgs::Float64& driving_msg)
+{
+  driving_operation(driving_value);
+  driving_value = driving_msg.data;
+}
+
+void steeringcallback(const std_msgs::Float64& steering_msg)
+{
+  steering_timestamp = millis();
+
+    if (steering_msg.data > upper_steering_limit)
+    {
+      steering_value = upper_steering_limit;
+    }
+    else if (steering_msg.data < lower_steering_limit)
+    {
+      steering_value = lower_steering_limit;
+    }
+    else
+    {
+      steering_value = steering_msg.data;
+      if((abs(steering_value-(analogRead(0)) < steering_tolerance))) // need to rethink requirements of low lvl control on test platform
+      {
+        no_operation();
+      }
+      else
+      {
+        operation(steering_value);
+      }
+    }
+
+    feedback.data = analogRead(0); // feedback.data is equal to the input of the linear actuator
+    steering_error_feedback.data = steering_value-feedback.data;
+    driving_feedback.data = driving_value; // driving feedback is equal to the driving value (until we have motor feedback)
+    driving_response.publish(&driving_feedback); 
+    steering_response.publish(&feedback);
+    steering_error_response.publish(&steering_error_feedback);    
+}
+
+ros::Subscriber<std_msgs::Float64> steering_sub("/control/steering_channel", &steeringcallback); //subscriber initialization
+ros::Subscriber<std_msgs::Float64> driving_sub("/control/driving_channel", &drivingcallback); 
+
+void setup() {
+  // put your setup code here, to run once:
+  nh.initNode(); // initialize ROS node
+  nh.subscribe(steering_sub); // subscriber to ardu_adapter for steering
+  nh.subscribe(driving_sub); // subscriber to ardu_adapter for driving
+  nh.advertise(steering_response); // publisher to ardu_adapter for steering
+  nh.advertise(driving_response); // publisher to ardu_adapter for driving
+  nh.advertise(steering_error_response); // publisher for error for PID tuning
+
+  pinMode(R_EN, OUTPUT); // initializing enable pins on linear actuator
+  pinMode(L_EN, OUTPUT);
+
+  pinMode(RPWM, OUTPUT); // initializing PWM pins on linear actuator
+  pinMode(LPWM, OUTPUT);
+
+  pinMode(Motor_R_EN, OUTPUT); // initializing enable pins on motor
+  pinMode(Motor_L_EN, OUTPUT);
+
+  pinMode(Motor_RPWM, OUTPUT); // initializing PWM pins on motor
+  pinMode(Motor_LPWM, OUTPUT); 
+
+  left.SetMode(AUTOMATIC); // turning on the PID control
+  right.SetMode(AUTOMATIC);
+
+  digitalWrite(R_EN, HIGH); // leave these as high to allow steering control operation
+  digitalWrite(L_EN, HIGH);
+
+  digitalWrite(Motor_R_EN, HIGH); // leave these as high to allow the motor control operation
+  digitalWrite(Motor_L_EN, HIGH);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  nh.spinOnce();
+
+  loop_timestamp = millis();
+
+  if (abs(loop_timestamp-steering_timestamp)>time_out)
+  {
+    no_operation();
+  }
+
+  delay(20);
+}
+
+void operation(double incoming_input)
+{
+  input = analogRead(0);
+  setpoint = incoming_input;
+  error = setpoint - input;
+
+  if (error > 0)
+  {
+    right_operation(setpoint); // turn right
+    steering_limits(input);
+  }
+  else if (error < 0)
+  {
+    left_operation(setpoint); // turn left
+    steering_limits(input);
+  }
+  else if (error == 0)
+  {
+    no_operation(); // go straight
+  }
+}
+
+void right_operation(double right_side_setpoint)
+{
+  right_setpoint = right_side_setpoint;
+  input = analogRead(0);
+  steering_limits(input);
+
+  if (input >= right_side_setpoint)
+  {
+    input = analogRead(0);
+    right_setpoint = right_side_setpoint;
+    left.SetMode(MANUAL);
+    right.SetMode(MANUAL);
+    analogWrite(LPWM, 0);
+    analogWrite(RPWM, 0);
+  }
+  else if (input < right_side_setpoint)
+  {
+    input = analogRead(0);
+    right_setpoint = right_side_setpoint;
+    left.SetMode(MANUAL);
+    right.SetMode(AUTOMATIC);
+    right.Compute();
+    analogWrite(LPWM, 0);
+    analogWrite(RPWM, right_output);
+
+    if (input >= right_side_setpoint)
+    {
+      input = analogRead(0);
+      right_setpoint = right_side_setpoint;
+      left.SetMode(MANUAL);
+      right.SetMode(MANUAL);
+      analogWrite(LPWM, 0);
+      analogWrite(RPWM, 0);
+    }
+  }
+}
+
+void left_operation(double left_side_setpoint)
+{
+  steering_limits(input);
+
+  if (input <= left_side_setpoint)
+  {
+    input = analogRead(0);
+    left_setpoint = left_side_setpoint;
+    right.SetMode(MANUAL);
+    left.SetMode(MANUAL);
+    analogWrite(RPWM, 0);
+    analogWrite(LPWM, 0);
+  }
+  else if (input > left_side_setpoint)
+  {
+    input = analogRead(0);
+    left_setpoint = left_side_setpoint;
+    right.SetMode(MANUAL);
+    left.SetMode(AUTOMATIC);
+    left.Compute();
+    analogWrite(RPWM, 0);
+    analogWrite(LPWM, left_output);
+
+    if (input <= left_side_setpoint)
+    {
+      input = analogRead(0);
+      left_setpoint = left_side_setpoint;
+      right.SetMode(MANUAL);
+      left.SetMode(MANUAL);
+      analogWrite(RPWM, 0);
+      analogWrite(LPWM, 0);
+    }
+  }
+}
+
+void no_operation()
+{
+  input = analogRead(0);
+  right.SetMode(MANUAL);
+  left.SetMode(MANUAL);
+  analogWrite(RPWM, 0);
+  analogWrite(LPWM, 0);
+}
+
+void driving_operation(double incoming_driving_input)
+{
+  if (incoming_driving_input > 0)
+  {
+    forward_drive(incoming_driving_input);
+  }
+  else if (incoming_driving_input < 0)
+  {
+    reverse_drive(incoming_driving_input);
+  }
+  else if (incoming_driving_input == 0)
+  {
+    braking();
+  }
+}
+
+void forward_drive(double driving_pwm)
+{
+  if (driving_pwm <= driving_limit)
+  {
+    digitalWrite(Motor_R_EN, HIGH);
+    digitalWrite(Motor_L_EN, HIGH);
+    analogWrite(Motor_LPWM, 0);
+    analogWrite(Motor_RPWM, driving_pwm);
+  }
+  else if (driving_pwm>driving_limit)
+  {
+    digitalWrite(Motor_R_EN, HIGH);
+    digitalWrite(Motor_L_EN, HIGH);
+    analogWrite(Motor_LPWM, 0);
+    analogWrite(Motor_RPWM, 100);
+  }
+}
+
+void reverse_drive(double driving_pwm)
+{
+  if (abs(driving_pwm <= driving_limit))
+  {
+    digitalWrite(Motor_R_EN, HIGH);
+    digitalWrite(Motor_L_EN, HIGH);
+    analogWrite(Motor_LPWM, abs(driving_pwm));
+    analogWrite(Motor_RPWM, 0);
+  }
+  else if (abs(driving_pwm>driving_limit))
+  {
+    digitalWrite(Motor_R_EN, HIGH);
+    digitalWrite(Motor_L_EN, HIGH);
+    analogWrite(Motor_LPWM, abs(driving_pwm));
+    analogWrite(Motor_RPWM, 0);
+  }
+}
+void braking()
+{
+  digitalWrite(Motor_R_EN, LOW);
+  digitalWrite(Motor_L_EN, LOW);
+  analogWrite(Motor_RPWM, 0);
+  analogWrite(Motor_LPWM, 0);
+}
+
+void steering_limits(double steering_input_limit)
+{
+  if (steering_input_limit >= upper_steering_limit)
+  {
+    no_operation();
+  }
+  else if (steering_input_limit <= lower_steering_limit)
+  {
+    no_operation();
+  }
+}
diff --git a/docs/standards/git/git_dev_guide.txt b/docs/standards/git/git_dev_guide.txt
@@ -0,0 +1 @@
+###==== FusionAD Git Dev Guide ====###
diff --git a/docs/standards/ros/ros_style_guide.txt b/docs/standards/ros/ros_style_guide.txt
@@ -0,0 +1,2 @@
+###==== FusionAD ROS Dev Guide ====###
+