.. image:: media/gifs/b2_gazebo.webp
    :alt: B2 Gazebo Simulation
    :width: 100%
    :class: mbsrounded

|

The B2W simulation package uses ROS2 Humble with Gazebo Fortress, featuring the complete B2W
platform with wheeled quadruped locomotion.

Prerequisites
-------------

- ROS 2 Humble
- Gazebo Fortress
- gz_ros2_control

B2 Simulation (Legged)
----------------------

.. code-block:: bash

   ros2 launch b2_gazebo b2_fortress_simulation.launch.py

B2W Simulation (Wheeled)
------------------------

.. code-block:: bash

   ros2 launch b2_gazebo b2w_fortress_simulation.launch.py

To kill Gazebo zombie processes:

.. code-block:: bash

   ros2 run b2_gazebo kill_gz.sh

Environment Variables
---------------------

Configure the simulation using environment variables before launch:

.. list-table::
   :header-rows: 1
   :widths: 20 20 40

   * - Variable
     - Default
     - Description
   * - B2_DESC
     - 1
     - Enable B2 robot description
   * - B2_Z1
     - 0
     - Include Z1 robotic arm

RViz2 Visualization
-------------------

To visualize the robot in RViz2:

.. code-block:: bash

   ros2 launch b2_viz view_robot.launch.py

Teleoperation (Keyboard)
------------------------

To teleoperate the B2W using your keyboard:

.. code-block:: bash

   ros2 run teleop_twist_keyboard teleop_twist_keyboard \
     cmd_vel:=/b2_unit_001/hardware/cmd_vel

.. note::
   Reduce speed by pressing **z** until **0.1** for safe indoor operation.

Joint Trajectory Control (B2 Legged)
------------------------------------

.. code-block:: bash

   ros2 action send_goal \
     /joint_effort_controller/follow_joint_trajectory \
     control_msgs/action/FollowJointTrajectory -f "{
       trajectory: {
         joint_names: [
           'FL_hip_joint', 'FL_thigh_joint', 'FL_calf_joint',
           'FR_hip_joint', 'FR_thigh_joint', 'FR_calf_joint',
           'RL_hip_joint', 'RL_thigh_joint', 'RL_calf_joint',
           'RR_hip_joint', 'RR_thigh_joint', 'RR_calf_joint'
         ],
         points: [{
           positions: [0.0, 0.9, -1.5, 0.0, 0.9, -1.5,
             0.0, 0.9, -1.5, 0.0, 0.9, -1.5],
           time_from_start: {sec: 2, nanosec: 0}
         }]
       }
     }"

B2W Wheel Velocity Control
--------------------------

.. code-block:: bash

   # Forward
   ros2 topic pub /velocity_controller/commands \
     std_msgs/msg/Float64MultiArray \
     "{data: [-1.0, -1.0, -1.0, -1.0]}"

   # Backward
   ros2 topic pub /velocity_controller/commands \
     std_msgs/msg/Float64MultiArray \
     "{data: [1.0, 1.0, 1.0, 1.0]}"

   # Stop
   ros2 topic pub /velocity_controller/commands \
     std_msgs/msg/Float64MultiArray \
     "{data: [0.0, 0.0, 0.0, 0.0]}"

Simulation Troubleshooting
--------------------------

If controllers are not starting, manually activate them:

.. code-block:: bash

   ros2 control set_controller_state \
     -c /b2_unit_001/controller_manager \
     joint_state_broadcaster configure

   ros2 control set_controller_state \
     -c /b2_unit_001/controller_manager \
     joint_state_broadcaster activate

Check hardware interfaces:

.. code-block:: bash

   ros2 control list_hardware_interfaces \
     -c /b2_unit_001/controller_manager