Vehicle Control Tutorial

### 1. Make and run PolySync parrot projects Begin by opening two terminals. In the first terminal, navigate to the "parrot_controller" project in the location shown below: ```bash /usr/local/polysync/examples/c/parrot_controller ├── include │ ├── common.h │ ├── gl_headers.h │ ├── grid.h │ ├── gui.h │ ├── ps_interface.h │ ├── render.h │ ├── vehicle_control.h │ └── waypoint.h ├── Makefile └── src ├── grid.c ├── gui.c ├── parrot_controller.c ├── ps_interface.c ├── render.c ├── vehicle_control.c └── waypoint.c ``` In the second terminal, open the "parrot_visualizer" project: ```bash /usr/local/polysync/examples/c/parrot_visualizer ├── include │   ├── common.h │   ├── driver_vehicle.h │   ├── gl_headers.h │   ├── gui.h │   ├── ps_interface.h │   ├── render.h │   └── sliding_filter.h ├── Makefile ├── res │   └── parrot.png └── src ├── driver_vehicle.c ├── gui.c ├── parrot_visualizer.c ├── ps_interface.c ├── render.c └── sliding_filter.c ``` Using the existing makefiles, make both projects and then run them. ![Parrot Windows](https://help.polysync.io/releases/2.0.7//wp-content/uploads/2016/09/parrotwindows-1.png) ### 2. Run the waypoint controller Begin manually specifying waypoints by pointing and clicking on a few different locations on the "parrot_controller" example. Waypoints become visible where clicked, and the vehicle in the "parrot_visualizer" example will start to move towards the closest one. You are now driving your first autonomous vehicle in PolySync. ![Parrot Driving](https://help.polysync.io/releases/2.0.7//wp-content/uploads/2016/09/parrotdriving-1.png) ### 3. Using Trace to view messages on the bus Begin by opening PolySync Studio: ```bash $ polysync-studio ``` Then, click on Trace in the "Launch Plugin" right-hand sidebar. Trace should open up as shown below. ![Studio Trace](https://help.polysync.io/releases/2.0.7//wp-content/uploads/2016/09/trace1.png) The drop down list in the left-hand sidebar contains all of the message types that have been seen on the bus . It will update dynamically. Select a name for the Trace window in the text box above the drop down list. Once a message type and name for the Trace is selected hit "Start Trace." Now that Trace is running, you can create some waypoints by clicking in the parrot_controller example. ![Trace Control](https://help.polysync.io/releases/2.0.7//wp-content/uploads/2016/09/trace_steering1.png) For a deeper understanding of what is going on here take a look below. The source file "vehicle_control.c" is where the pure pursuit algorithm lives that does the vehicle control. ```bash /usr/local/polysync/examples/c/parrot_controller └── src └── vehicle_control.c ``` The function below is called by a loop in main. It receives the list of current waypoints which have been selected in the GUI. It also receives the user_data structure which contains the current position of the vehicle (parrot) on the screen. This vehicle position is continuously updated by the parrot-visualizer example which estimates the vehicles position based on steering and throttle messages. The parrot-visualizer example then loads this estimated position into a ps_platform_motion_msg and sends it out on the PolySync bus. The below operation, using the position information that has been received and processed, calls the "findNextWaypoint" function which determines what is the current waypoint goal based on the queue of waypoints (and also eliminates the current goal waypoint if it has been reached in this iteration). Once the waypoint goal is known, this is passed to the pure pursuit algorithm. Take a look at the next code snippet for an explanation of how this works. ```c void send_psync_messages_for_vehicle_control( node_data_s * user_data, waypoint_s * waypoints ) { // local vars node_data_s *node_data = NULL; // cast node_data = ( node_data_s* ) user_data; // ignore if not valid if( ( node_data == NULL ) || ( waypoints == NULL ) ) { return; } waypoint_s nextWaypoint; if( findNextWaypoint( waypoints, &nextWaypoint, node_data->current_vehicle_position ) ) { return; } double steeringAngle = 0; if( calculate_steering_angle_based_on_goal_waypoint( &nextWaypoint, node_data->current_vehicle_position, &steeringAngle ) ) { return; } publish_steering_command( node_data->node, steeringAngle ); publish_throttle_command( node_data->node, DEFAULT_THROTTLE ); } ``` This function calculates the angle between the current heading of the vehicle and the angle from the vehicle's current position to the goal waypoint. It then determines the error between these two angles, and uses this to calculate the lateral error of the vehicle from the goal path. Multiplied by a steering control factor this is applied to steering the vehicle. ```c int calculate_steering_angle_based_on_goal_waypoint( waypoint_s * goalWaypoint, waypoint_s currentPosition, double * steeringAngle ) { if( ! goalWaypoint->valid ) { return 1; } double vehicleToWaypointAngle = calculate_angle_between_coordinates( currentPosition.x, currentPosition.y, goalWaypoint->x, goalWaypoint->y ); double errorInHeading = calculate_smallest_interior_angle( currentPosition.heading*RAD2DEG, vehicleToWaypointAngle*RAD2DEG ); double lateralError = sin( errorInHeading*DEG2RAD ); ( *steeringAngle ) = -STEERING_CONTROL_FACTOR*lateralError; return 0; } ``` Once you have the desired steering angle, and desired throttle value, these are packaged up and published to the PolySync bus in the below. ```c publish_steering_command( node_data->node, steeringAngle ); publish_throttle_command( node_data->node, DEFAULT_THROTTLE ); ``` These messages are received and processed by the parrot_control example, then used to estimate the position of the "vehicle" closing the loop. ### Conclusion Congratulations! You have now successfully controlled your first PolySync autonomous vehicle.