.. _helloworld_sim_reference-label:

Hello World in Simulation
*********************

The Hello World demo (``demo/demo_helloworld``) is a good exaple to understand the logistic of running with our code. Here, we use a Unitree A1 quadruped robot to explain the helloworld demo.

Initializzing the ROS node
=======================

Since our code is developped in ROS, we need initializing the ROS node first. Using ROS components, our program can communicate to other programs.

.. code-block:: c++

    ros::init(argc, argv, "demo_helloworld");
    ros::NodeHandle nh;

Getting the YMAL path
============

Every demo has its own configuration and parameters. These parameters are stored in a ``.yaml`` file. We need tell where the ``.yaml`` is and later we can load parameters from the given ``.yaml`` file. In this example, a Unitree A1 robot is used and its configurations are store in ``demo_helloworld/sim_config`` for simulation. We set a robot name as A1, then we will get the directory ``demo/demo_helloworld``.

.. code-block:: c++

    std::string pathToPackage = ros::package::getPath("demo");
    std::string pathToNode =  pathToPackage + ros::this_node::getName();
    std::string robotName = "a1";

The first two lines are unnecessary in the Hello World demo. However, they may be necessary in other demos.


Resetting the Gazebo controller and robot model
===========================================

We reset the Gazebo simulator's controller and the robot model. This allows the Gazebo to reload the controller plugins and locate the robot at the given position and orientation. Note that, this can avoid relaunching the Gazebo every time when we re-run the helloword demo. 

.. code-block:: c++
    
    ResetRobotBySystem(nh);


Creating a robot
================

After launching the Gazebo, we create a robot (Unitree A1) using the specified parameters stored in the  ``demo/demo_helloworld/sim_config/main.yaml`` file.

.. code-block:: c++

    qrRobot *quadruped = new qrRobotSim(nh, robotName, LocomotionMode::VELOCITY_LOCOMOTION);


We also need initialize a few robot properties by receiving observation from Gazebo.

.. code-block:: c++

    quadruped->ReceiveObservation();


Executing actions
===============

Now the initiliazation is finished. We are ready to execute standing actions. First, we let the robot perform the first action, standing up. It takes 3 seconds to stand up and keep 5 seconds before any other action. The parameter 0.001 is the specified time step (control frequency is 1000Hz). You may try different arguments to understand the action. 

.. code-block:: c++

    Action::StandUp(quadruped, 3.f, 5.f, 0.001f);

After standing up we let the quadruped robot keep standing for 20 seconds, and the control frequency is also 1000Hz.

.. code-block:: c++

    Action::StandUp(quadruped, 3.f, 5.f, 0.001f);

Finally the quadruped robot will sit down in 3 seconds with 1000Hz control frequency.

.. code-block:: c++

    Action::StandUp(quadruped, 3.f, 5.f, 0.001f);


Finishing and shutting down the ROS node
======================

After the demo is finished, we shut down the ROS nodes.

.. code-block:: c++

    ros::shutdown();


Launching the demo
=============

To run the demo, we first launch Gazebo, a high fidelity robot simulator widely used in ROS community. First, in one terminal, source the setup.bash to set up the environment

.. code-block:: c++

    source ${your_workspace}/devel/setup.bash

Second, run the Gazebo simulator and load a robot.

.. code-block:: c++

    roslaunch qr_gazebo normal.launch

Third, in a new terminal, launch a demo and run the quadruped controller node. Here, a demo helloworld lets the quadruped robot stand up.

.. code-block:: c++

    rosrun demo demo_helloworld sim

or omit ``sim`` by default

.. code-block:: c++

    rosrun demo demo_helloworld