Tweeks to the driver and documentation

CMakeLists.txt

@@ -53,14 +53,7 @@ include_directories(include
 # --------------------------------
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  "msg/MotorState.msg"
   "msg/BmsState.msg"
-  "msg/BmsCmd.msg"
-  "msg/Cartesian.msg"
-  "msg/IMU.msg"
-  "msg/LED.msg"
-  "msg/HighCmd.msg"
-  "msg/HighState.msg"
   DEPENDENCIES builtin_interfaces std_msgs geometry_msgs sensor_msgs nav_msgs
 )
 

README.md

@@ -1,27 +1,26 @@
 # Unitree Go1 ROS2 Driver
 
-## Description
+## 🤖 Description
 
-This is a ROS2 package which can be used to control the legged robot **Unitree Go1** 
+This is a ROS2 package which can be used to control the legged robot **Unitree Go1**
 using ROS topics.
 With this driver, you can send commands to the robot via ROS topics such as `/cmd_vel` and receive robot sensors states such as `odometry` and `IMU` information.
-In addition, this driver features some other cool functions such as standing up the robot. 
+In addition, this driver features some other cool functions such as standing up the robot.
 More of the features can be found [here](#features).
 
-
 For more information about the different topics this ROS package subscribes or publishes, please refer to [ROS Topics](#ros-topics) section.
 
-
-## Table of Contents
+## 📖 Table of Contents
 
 <!--toc:start-->
+
 - [Installation](#installation)
   - [Dependencies](#dependencies)
 - [Usage](#usage)
 - [Features](#features)
   - [ROS Topics](#ros-topics)
-    - [Subscribers](#subscribers)
-    - [Publishers](#publishers)
+    - [Subscribed Topics](#subscribed-topics)
+    - [Published Topics](#published-topis)
   - [Robot LED statuses](#robot-led-statuses)
   - [Low Battery Protection](#low-battery-protection)
   - [Obstacle Avoidance](#obstacle-avoidance)
@@ -30,17 +29,17 @@ For more information about the different topics this ROS package subscribes or p
 - [Credits](#credits)
   - [Maintainers](#maintainers)
   - [Third-party Assets](#third-party-assets)
-<!--toc:end-->
+  <!--toc:end-->
 
-
-## Installation
+## 🚀 Installation
 
 > _Before using this ROS2 package, you will need to make sure you have `ROS2 Foxy` installed on your machine.
-In case you want to use a ROS distro above the Foxy distribution, there is no 
-guarantee that this package can run there.
-Nevertheless, plans to upgrade to a newer distro might be coming shortly._
+> In case you want to use a ROS distro above the Foxy distribution, there is no
+> guarantee that this package can run there.
+> Nevertheless, plans to upgrade to a newer distro might be coming shortly._
 
 To install and use this ROS2 package, you will need to clone it first, into a desired workspace.
+
 ```sh
 mkdir -p ~/unitree_ws/src
 cd ~/unitree_ws/src
@@ -64,20 +63,22 @@ After having build the workspace, you should now be able to use the driver to co
 
 - [unitree_ros_to_real](https://github.com/unitreerobotics/unitree_ros_to_real): This is a ROS1 package provided by the Unitree robotics. The ROS messages have been adapted for ROS2 support.
 
-- **`faceLightSDK_nano`**: This is the SDK that can be found on the internal computers of the Unitree Go1. It has been ported to this ROS package with the goal of being able to control the face LEDs, which are used to 
-give some robot statuses.
-
+- **`faceLightSDK_nano`**: This is the SDK that can be found on the internal computers of the Unitree Go1. It has been ported to this ROS package with the goal of being able to control the face LEDs, which are used to
+  give some robot statuses.
 
-## Usage
+## 🏗️ Usage
 
 Before using the driver, you will need to make a decision whether you want to control the robot
 using a Wi-Fi connection or a wired connection. In case you go for a wired connection, you won't need
 to do anything. In case you want to use the Wi-Fi connection, then you will need to change the IP address
 of the robot. To do so, go to the config folder and open the `params.yaml` file. There, change the following line:
+
 ```yaml
 robot_ip: '192.168.123.161' # Change to 192.168.12.1 for WIFI
 ```
+
 to
+
 ```yaml
 robot_ip: '192.168.12.1' # Change to 192.168.123.161 for wired
 ```
@@ -86,43 +87,59 @@ In addition, you might want to change some of the other parameters available, su
 
 After having set the correct robot IP address, you are now able to run the driver. To do so, a launch file
 is available, which makes it easier. Run the following commands to launch the driver.
+
 ```sh
 source ~/unitree_ws/install/setup.bash # or zsh if using the zsh shell!
 ros2 launch unitree_ros unitree_driver_launch.py
 ```
 
 In case you do not want to use the launch file, just proceed with the following commands:
+
 ```sh
 source ~/unitree_ws/install/setup.bash # or zsh if using the zsh shell!
 ros2 run unitree_ros unitree_driver
 ```
 
-## Features
+## 📌 Features
 
 ### ROS Topics
 
-#### Subscribers
-
-- `/cmd_vel` [[geometry_mgs/msg/Twist.msg](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Twist.html)]: 
-This is used by the driver to receive velocity commands and send the appropriate commands to the robot.
-
-- `/stand_up` [[std_msgs/msg/Empty.msg](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/Empty.html)]: 
-Triggers the robot to stand up
-
-- `/stand_down` [[std_msgs/msg/Empty.msg](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/Empty.html)]: 
-Triggers the robot to stand down
-
-#### Publishers
-
-- `/odom` [[nav_msgs/msg/Odometry.msg](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html)]: The odometry state received from the robot is being published to this topic.
-
-- `/imu` [[sensor_msgs/msg/IMU.msg](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Imu.html)]: The IMU state received from the robot is being published to this topic.
-
-- `/bms` [[unitree_ros/msg/bms.msg](https://github.com/snt-arg/unitree_ros/blob/main/msg/BmsState.msg)]: The battery state received from the robot is being published to this topic.
-
-### Robot LED statuses
-
-The robot has a few predetermined LED statuses, which are useful to give some information to 
+#### 🔽 Subscribed Topics
+
+| Topic name    | Message Type                                                                                      | Description                                                                                             |
+| ------------- | ------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------- |
+| `cmd_vel`     | [geometry_mgs/msg/Twist.msg](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Twist.html) | This is used by the driver to receive velocity commands and send the appropriate commands to the robot. |
+| `/stand_up`   | [std_msgs/msg/Empty.msg](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/Empty.html)         | Triggers the robot to stand up                                                                          |
+| `/stand_down` | [std_msgs/msg/Empty.msg](http://docs.ros.org/en/melodic/api/std_msgs/html/msg/Empty.html)         | Triggers the robot to stand down                                                                        |
+
+#### 🔼 Published Topics
+
+| Topic name | Message Type                                                                                   | Description                                                                  |
+| ---------- | ---------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------- |
+| `/odom`    | [nav_msgs/msg/Odometry.msg](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) | The odometry state received from the robot is being published to this topic. |
+| `/imu`     | [sensor_msgs/msg/IMU.msg](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Imu.html)     | The IMU state received from the robot is being published to this topic.      |
+| `/bms`     | [unitree_ros/msg/bms.msg](https://github.com/snt-arg/unitree_ros/blob/main/msg/BmsState.msg)   | The battery state received from the robot is being published to this topic.  |
+
+## ⚙️ ROS Parameters
+
+| Parameter Name                | Default value   | Description                                                                                              |
+| ----------------------------- | --------------- | -------------------------------------------------------------------------------------------------------- |
+| `ns`                          | -               | Name space that should be given to robot driver                                                          |
+| `robot_ip`                    | 192.168.123.161 | Robot IP that should be used to establish the UDP connection. For a Wi-Fi conntection use `192.168.12.1` |
+| `robot_target_port`           | 8082            | The port that should be used to communicate with the robot.                                              |
+| `cmd_vel_topic_name`          | /cmd_vel        | Topic name that should be used for subscribing to velocity commands                                      |
+| `odom_topic_name`             | /odom           | Topic name that should be used for publishing the odometry state                                         |
+| `imu_topic_name`              | /imu            | Topic name that should be used for publishing the IMU state                                              |
+| `bms_state_topic_name`        | /bms_state      | Topic name that should be used for publishing the battery management state, such as battery level.       |
+| `imu_frame_id`                | imu             | Frame id that should be used for the IMU frame                                                           |
+| `odom_frame_id`               | odom            | Frame id that should be used for the odometry frame                                                      |
+| `odom_child_frame_id`         | base_footprint  | Frame id of the body of the robot                                                                        |
+| `use_obstacle_avoidance`      | false           | Enables (true) or disables (false) the robot obstacle avoidance.                                         |
+| `low_batt_shutdown_threshold` | 20              | Battery threshold for when to stop the robot from moving, in case the battery is below                   |
+
+### 💡 Robot LED statuses
+
+The robot has a few predetermined LED statuses, which are useful to give some information to
 anyone using the robot.
 The following statuses are available:
 
@@ -132,33 +149,32 @@ The following statuses are available:
 - **Yellow Light**: Low battery _(< 30 %)_
 - **Red Light**: Any internal error (Not yet implemented)
 
-### Low Battery Protection
+### 🔋 Low Battery Protection
 
 By specifying a low battery threshold using the parameters file (`low_batt_shutdown_threshold`), the driver will stop the robot
 from moving and will stand it down. _By default, the low battery threshold value is set to 20%._
 
-### Obstacle Avoidance
+### 🚧 Obstacle Avoidance
 
-The robot has an obstacle avoidance mode. However, this mode is not enabled by default. Therefore, 
+The robot has an obstacle avoidance mode. However, this mode is not enabled by default. Therefore,
 this driver allows you to enable it using the parameters file (`use_obstacle_avoidance`). _By default, this is
 set to false_
 
-
-## License
+## 🔑 License
 
 This project is licensed under the BSD-3 License - see the LICENSE file for details.
 
 ## How to contribute
-Contributions are welcome! If you have any suggestions, bug reports, or feature requests, 
+
+Contributions are welcome! If you have any suggestions, bug reports, or feature requests,
 please create a new issue or pull request.
 
-## Credits
+## 🏆 Credits
 
 #### Maintainers
 
-* [Pedro Soares](https://www.github.com/PedroS235)
+- [Pedro Soares](https://www.github.com/PedroS235)
 
 #### Third-party Assets
 
-* [Unitree Robotics](https://github.com/unitreerobotics)
-
+- [Unitree Robotics](https://github.com/unitreerobotics)

include/unitree_ros/serializers.hpp

@@ -6,8 +6,8 @@
 #include <nav_msgs/msg/detail/odometry__struct.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/imu.hpp>
+#include <unitree_ros/common_defines.hpp>
 #include <unitree_ros/msg/bms_state.hpp>
-#include <unitree_ros/unitree_data.hpp>
 
 #include "unitree_legged_sdk/comm.h"
 

include/unitree_ros/unitree_driver.hpp

@@ -4,10 +4,14 @@
 #include <FaceLightClient.h>
 #include <unitree_legged_sdk/unitree_legged_sdk.h>
 
-#include <unitree_ros/unitree_data.hpp>
+#include <unitree_ros/common_defines.hpp>
 
 #include "unitree_legged_sdk/comm.h"
 
+/*
+ * @brief Class Unitree driver which is use for the main interaction with
+ * the Unitree Go1 robot.
+ */
 class UnitreeDriver {
     // Unitree SDK related
    private:
@@ -43,6 +47,10 @@ class UnitreeDriver {
      * @param target_port_: Port to be used to communicate with robot (default: 8082)
      */
     UnitreeDriver(std::string ip_addr = "192.168.123.161", int target_port = 8082);
+    /*
+     * @brief Deconstructor for the class UnitreeDriver. When called, it makes
+     * the robot stand down and deactivate the motors.
+     */
     ~UnitreeDriver();
 
     /**
@@ -68,14 +76,15 @@ class UnitreeDriver {
     /**
      * @brief Move robot with with velocity commands
      *
-     * @param x: forward/backards direction
-     * @param y: left/right direction
-     * @param yaw: rotation along the z axis
+     * @param x: forward(+)/backards(-) direction
+     * @param y: left(+)/right(-) direction
+     * @param yaw: rotation along the z axis (+ccw)
      */
     void walk_w_vel(float x, float y, float yaw);
 
     /**
-     * @brief Move robot with position commands
+     * @brief Move robot with position commands relative where the robot was booted
+     * (Never tested)
      *
      * @param position: {x, y z}
      * @param orientation: {z, y, z, w}
@@ -158,9 +167,26 @@ class UnitreeDriver {
      */
     void stop();
 
+    /**
+     * @brief Sets the robot face LEDs color
+     *
+     * @param r: percentage of the red color (0-254)
+     * @param g: percentage of the green color (0-254)
+     * @param b: percentage of the blue color (0-254)
+     */
     void set_head_led(uint8_t r, uint8_t g, uint8_t b);
+
+    /**
+     * @brief Sets the robot face LEDs color
+     *
+     * @param led: struct containing the 3 primary colors
+     */
     void set_head_led(UNITREE_LEGGED_SDK::LED led);
-    void show_robot_status();
+
+    /**
+     * @brief Sends the colors to the face LEDs based on the robot status.
+     */
+    void update_robot_status();
 
    private:
     /**
@@ -196,7 +222,19 @@ class UnitreeDriver {
      */
     velocity_t get_velocity_();
 
+    /**
+     * Makes the robot's face LEDs blink.
+     *
+     * @param r: percentage of the red color (0-254)
+     * @param g: percentage of the green color (0-254)
+     * @param b: percentage of the blue color (0-254)
+     */
     void blink_face_led(uint8_t r, uint8_t g, uint8_t b);
+    /**
+     * Makes the robot's face LEDs blink.
+     *
+     * @param led: struct containing the 3 primary colors
+     */
     void blink_face_led(UNITREE_LEGGED_SDK::LED led);
 };
 

include/unitree_ros/unitree_ros.hpp

@@ -13,7 +13,7 @@
 #include <std_msgs/msg/u_int8_multi_array.hpp>
 #include <unitree_ros/msg/bms_state.hpp>
 
-#include "unitree_ros/unitree_data.hpp"
+#include "unitree_ros/common_defines.hpp"
 #include "unitree_ros/unitree_driver.hpp"
 
 class UnitreeRosNode : public rclcpp::Node {
@@ -47,7 +47,8 @@ class UnitreeRosNode : public rclcpp::Node {
 
     // Timers
     rclcpp::TimerBase::SharedPtr robot_state_timer_;
-    rclcpp::TimerBase::SharedPtr robot_status_led_timer_;
+    // This timer is used to reset the velocity command to 0, which is a safety
+    // measure.
     rclcpp::TimerBase::SharedPtr cmd_vel_reset_timer_;
     rclcpp::TimerBase::SharedPtr check_robot_battery_timer_;
     rclcpp::Time prev_cmd_vel_sent_;
@@ -61,10 +62,20 @@ class UnitreeRosNode : public rclcpp::Node {
     bool use_obstacle_avoidance_ = false;
 
    public:
+    /**
+     * @brief Constructor of the class UnitreeRosNode
+     */
     UnitreeRosNode();
+    /**
+     * @brief deconstructor of the class UnitreeRosNode
+     */
     ~UnitreeRosNode();
 
    private:
+    /**
+     * @brief Reads all the availabe ROS parameters and writes their values to
+     * the appropriate attribues
+     */
     void read_parameters_();
 
     void init_subscriptions_();
@@ -73,7 +84,6 @@ class UnitreeRosNode : public rclcpp::Node {
 
     void cmd_vel_callback_(const geometry_msgs::msg::Twist::UniquePtr msg);
     void robot_state_callback_();
-    void robot_status_led_callback_();
     void check_robot_battery_callback_();
     void cmd_vel_reset_callback_();
     void stand_up_callback_(const std_msgs::msg::Empty::UniquePtr msg);
@@ -83,7 +93,6 @@ class UnitreeRosNode : public rclcpp::Node {
     void publish_imu_(rclcpp::Time time);
     void publish_bms_();
     void publish_odom_tf_(rclcpp::Time time, odom_t odom);
-    void publish_remote_();
 
     void apply_namespace_to_topic_names_();
 };