## For instruction on writing tutorials ## http://www.ros.org/wiki/WritingTutorials #################################### ##FILL ME IN #################################### ## for a custom note with links: ## note = ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0= ## descriptive title for the tutorial ## title = Connecting your GNC project to the simulator ## multi-line description to be displayed in search ## description = Shows how to connect a GNC project to the simulator using the [[https://github.com/EPFLRocketTeam/template_gnc| template_gnc]] package. ## the next tutorial description (optional) ## next = ## links to next tutorial (optional) ## next.0.link= ## next.1.link= ## what level user is this tutorial for ## level= BeginnerCategory ## keywords = #################################### <> <> ## AUTOGENERATED DO NOT DELETE ## TutorialCategory ## FILL IN THE STACK TUTORIAL CATEGORY HERE The important feature of this framework is that your GNC project will be exactly the same in the simulation environment as when deployed on the real system. To achieve this, we need to follow a special architecture, already implemented in the [[https://github.com/EPFLRocketTeam/template_gnc| template_gnc]] package. == Creating and testing your first GNC package == === Installation === Follow the instructions of the [[https://github.com/EPFLRocketTeam/template_gnc| template_gnc]] package to install it in your catkin workspace, and update the code with the name of your GNC project. In this tutorial we assume that your project is named ''test_rocket_gnc'' as proposed in the package instructions, but any other name would work. {{{#!wiki blue/solid '''Note:''' Don't forget to link your new package to another git repository if you want to version control your modifications. }}} === First closed-loop simulation === Your new GNC package already contains a basic implementation of the ''Navigation'', ''Control'' and ''Guidance'' algorithms to stabilize the rocket orientation and reach a target apogee. We can thus directly launch a simulation with this GNC: {{{ roslaunch test_rocket_utils test_rocket_SIL.launch }}} This command starts the simulator nodes and your GNC nodes, and open the same graphical interface as in the previous tutorial. You can start the simulation with the '''Publish''' button, and see the rocket reaching close to 2000 meters of apogee. Hit '''CTRL+c''' in the terminal to stop the simulation. {{{#!wiki solid/yellow Try changing the target altitude height in ''test_rocket_utils/config/environment_parameters.yaml'' to test the performance of the GNC. }}} === Analyzing the results === In simulation mode (SiL) the flight data are recorded using ''rosbag'' in the file ''real_time_simulator/log/log.bag''. We can use the post-processing tools of the '''real_time_simulator''' package to analyze it. {{{ roscd real_time_simulator python3 postProcess/postProcess.py }}} This launches a classic Matplotlib windows displaying the simulated and estimated state of the vehicle, as well as the controlled and measured commands. You can also use the bokeh application to display a more interactive version of these plots: {{{ roscd real_time_simulator bokeh serve --show postProcess/fancy_plotter.py }}} {{attachment:bokeh_app.png||width=100%}} == Upgrading the GNC == Let's see what's inside this GNC package. In the launch file ''test_rocket_SIL.launch'', we first include all necessary nodes from the simulator: {{{ }}} As before, we also include the configuration files, this time from the GNC package. Finally we launch the nodes of the GNC: {{{ }}} We launch one node for each of the three algorithms (Guidance, Navigation, and Control), as well as ''test_rocket_fsm'' that runs the state machine to define if we are in ''Idle'', ''Rail'', ''Launch'' or ''Coast'' mode. The ''av_interface'' nodes is the interface between your GNC and the input/output of your vehicle. Here it just redirects the simulation data to the GNC. === Guidance === This algorithm computes a trajectory from the current state to the apogee, to be tracked by the Control algorithm. The trajectory is sent as a custom '''Trajectory''' message, which is simply an array of '''Waypoint''' messages, defined as: {{{ float32 time geometry_msgs/Point position geometry_msgs/Vector3 speed float32 propeller_mass float32 thrust }}} In our template example, we only computes a linear trajectory in 3D position, and leave the other fields empty. You can improve this algorithm by computing a more realistic trajectory, and filling the other fields of the message to better assist the Control algorithm. Additionally, this algorithm predicts the apogee of the rocket to send the shutdown command to the engine at the right time. ( ''' !! Not implemented in new-template-gnc !! -> to do for nicer tuto ''' {{{#!wiki solid/yellow Try to compute the trajectory as a piece-wise parabolic function in position, and piece-wise linear function in velocity. You can also compute the evolution of thrust and mass from this simplified model. }}} === Navigation === This algorithm estimates the state of the vehicle using direct integration of the IMU data. A simple one-dimensional kalman filter then corrects the altitude estimation by comparing it with the barometer measurement. If you've run a few simulations, you probably have noticed that the resulting apogee is significantly higher than the target one. The issue is not from the Guidance algorithm, but actually from Navigation, as this simple algorithm cannot properly estimates the vertical velocity, which is a very important information to predict the apogee. {{attachment:speed_position_estimation.png||width=40%}} This error in velocity estimation comes from the delay of the sensors, as we only start integrating the acceleration into a velocity after detecting the liftoff, which means the rocket will already have a significant speed when we start the Navigation algorithm. We clearly see this in the plot above, the error in velocity is actually a pure delay. The error in position on the other end is quickly corrected by the kalman filter. One solution to this is to integrate a velocity correction into the kalman filter, as it was done in [[https://github.com/EPFLRocketTeam/bellalui_gnc| bellalui_gnc]]. Another simpler way is to increase the sensor sampling frequency (defined in ''real_time_simulator/config/perturbations_parameter.yaml'') as it is the main source of delay. {{{#!wiki solid/yellow Try to implement a better velocity estimation, or an algorithm to compensate delay. }}} === Control === This algorithm should track the reference trajectory defined by guidance, during the time the engine is ignited. This template implementation is only a linear controller proportional to the angular rate, but you can add more terms to track the position and orientation too. The output of this algorithm is a message of type '''GimbalControl''', which send commands to the simulated gimbal actuator: {{{ Header header float64 outer_angle float64 inner_angle float64 thrust }}} {{{#!wiki solid/yellow Try to implement a two-stage PID controller to stabilize both the position and orientation to zero. }}}