*„Gościom radujemy się dwa razy”: czyli jak mój brat Tomek zamienił weekend w sprawdzian cierpliwości*
— Sławek, pamiętasz, iż w ten weekend przyjeżdża tw# ROS + Gazebo Package for Cozmo
This package was developed to allow simulated Cozmo in Gazebo to be controlled via ROS. The development of the package was heavily inspired from the [TurtleBot3](https://emanual.robotis.com/docs/en/platform/turtlebot3/simulation/) which uses similar tools and components as found in this repository.
More details on the project given in the [Final Report](https://docs.google.com/document/d/1msTgqOFYlKi9OOYIKiPkRmEVWxFP_6eNXnNvKX1pets/edit?usp=sharing)
## **Requirements**
### **Cozmo Package**
* Ubuntu 20.04
* ROS Noetic
* Gazebo ROS (included with desktop-full install)
* Cozmo SDK (optional)
Only a full desktop installation of [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) is required to setup the package. This includes Gazebo and all the components required to simulate and control Cozmo. The Cozmo SDK was primarily used when obtaining model parameters (such as wheel diameter, spacing between wheels, etc.) and testing out the control scheme.
## **Installation**
After cloning the repository into your `.src` workspace folder, run the following to ensure the dependencies are installed:
“`bash
sudo apt-get install ros-noetic-dynamixel-sdk
sudo apt-get install ros-noetic-turtlebot3-msgs
sudo apt-get install ros-noetic-turtlebot3
“`
To compile the package, run `catkin_make` in the workspace directory.
## **Launching the Package**
To spawn Cozmo into the simulation, run the following command in the workspace directory:
“`bash
roslaunch cozmo_description cozmo_gazebo.launch
“`
This will open an empty Gazebo world with Cozmo placed in the middle. To teleop Cozmo via keyboard, run the following command in a separate terminal:
“`bash
roslaunch cozmo_teleop cozmo_teleop_key.launch
“`
More teleoperation options (such as joystick control) can be implemented by modifying the parameters of the `turtlebot3_teleop_key` node (or any other teleop node).
The empty world can be substituted for other maps by specifying its name in the `cozmo_gazebo.launch` file. The simulation supports all worlds that TurtleBot3 does (such as `turtlebot3_house`) however due to Cozmo’s size the robot might have trouble navigating.
## **ROS Topics**
The teleop node primarily published to `/cmd_vel` with a `twist` message. The `robot_state_publisher` node then converts those commands into motor inputs via the Python script `main_controller.py`. This script is responsible for publishing the motor velocities via `/cozmo/cmd_vel` and contains the PID controller.
Other topics include standard ROS navigation stack topics such as `odom`, `tf`, and `/scan` which can be utilized when implementing SLAM capability.
## **Components**
### **URDF Model**
The Cozmo URDF model was entirely built from scratch (rather than using the official model provided with the Cozmo SDK). The wheels, body, head, and lift were all modeled in SolidWorks and exported as `.STL` files. Various Cozmo parameters (such as wheel diameter) were either obtained from Anki’s documentation or from measuring a Cozmo.
In addition the URDF was configured to use Gazebo’s built-in differential drive plugin (as TurtleBot3 uses). This plugin would run if not for the custom controller using `main_controller.py`.
### **Controller**
The controller is implemented as a PID controller (with a feedforward component) inside the `main_controller.py` script. It subscribes to `/cmd_vel` to obtain linear and angular velocities then calculates and publishes the required motor speeds via `/cozmo/cmd_vel`. This controller was tested with the [Cozmo SDK](https://developer.anki.com/cozmo/docs/initial.html) to verify the robot’s movement matched the desired output.
### **SLAM (Simultaneous Localization and Mapping)**
Although currently not implemented, this package does support SLAM standard topics such as `/scan`, `/odom`, and `/tf` which can easily be extended with SLAM packages such as `gmapping` or `cartographer`.
## **Future Updates**
1. **Cozmo Lift + Head** – Currently only Cozmo’s drivetrain is functional. In the future, control of the head and lift will be implemented via topics.
2. **SLAM** – Once the physical model is completed, SLAM packages can be integrated into the ROS package.
3. **Adding Object Recognition + IR Sensors** – Currently Cozmo’s simulated IR sensors only function as cliff sensors (see the `cozmo.gazebo.xacro` file for more details) however these should also act as proximity sensors.
## **Known Issues + Workarounds**
1. **Flipping** – Currently Cozmo is prone to flipping if moving too fast in reverse or turning too sharply. Reducing the maximum velocity parameters can help mitigate this however increasing the mass and tuning the friction parameters can also be helpful.
2. **Motor Slippage** – When moving at a constant velocity, the robot does not maintain its velocity and oscillates between moving and stopping. Adjusting the PID gains can help mitigate this but increasing the mass also helps ensure the wheels have better traction.
3. **Stuck Wheels** – Occasionally when running the simulation Cozmo’s wheels will be stuck at a certain point and cannot be moved via the `cmd_vel` topic. Restarting the simulation (without closing Gazebo) fixes the issue. This might be due to the custom controller interfering with the Gazebo controller.
