The purpose of this project was to dive into ROS2 and Gazebo Ignition. I also wanted to learn how to design a robot in CAD and program it using C++. Because I'm broke, I decided making it work in simulation would be a great start to making it a reality. To keep things simple, I used a scikit-learn model model over more complex approaches like reinforcement learning to get a better understanding of the simulation tools. This was the part I was most excited about was connecting a machine learning model to the robot and watching it perform. Although autonomous navigation could technically be achieved with basic sensors, my goal was to deepen my learning by integrating a machine learning model. Below is a CAD render of CHAR 01 in Fusion360. I like to keep it minimal. Hopefully I'll use my school 3D printers to print this bad boy out, praying that I can get free filament.

Finding current resources for ROS2 and Gazebo was quite a task, given how often they're updated. But I hit the jackpot with this cool course on [Udemy by Raymond Andrade](https://www.udemy.com/course/ros2-cpp-robotics-developer-course/). It really hit the mark in terms of staying current, which seems like a tall order in this field. Raymond's course was a big help, especially with its clear, visual explanations. It's got everything to get you started - like ROS2 subscribers and packages. Definitely a solid pick for anyone diving into these techs for the first time. If you don't understand the basics, I definitely suggest getting a handle on the basics first before diving into creating your own stuff or even trying to follow my code. It'll give you a way better understanding of how everything fits together. I'm going to skip the beginner stuff and jump right into the cool parts - like how I built the model, crafted the worlds, and set up the topics and subscribers. It's more fun that way! For a similar project using an Arduino, check out [Niko Dembartnik](https://youtu.be/PdSDhdciSpE?si=lA5s7GLW5RZzkDLo). I used a lot of his code and modified it to my simulation, check out his repo! If you want to dive more into my code, check out the repo [here!](https://github.com/willdphan/char-01.git) Other than that, let's get started! Here are the files I'll focus on in the repo: - `char_01_model.sdf` - `char_01_world.sdf` - `lidar_node` - `visualize.py` - `train_model.py` - `predict.py` - `control.cpp` ## Model Design As shown in my CAD drawing, I attempted to mimic its shape in Gazebo Ignition. I couldn't convert it to URDF for some reason and figured it was quicker to create it in an SDF file since it consisted of simple shapes, you can the code for this here! Here's the CHAR 01 look-alike next to a sick Hummer. It's a little big, but it'll do. Code is [here](https://github.com/willdphan/char-01/blob/4210aafeb5c262d53692909b3e651718de7305d2/ros2_pkg/models/char_01/char_01_model.sdf). CHAR-01

Keep in mind, we need to connect model to the `/cmd_vel topic` into the `char_01_model.sdf` file. `/cmd_vel` topic usually carries messages of type `geometry_msgs/Twist`, which contain fields for linear `(x, y, z)` and angular `(x, y, z)` velocities. In a typical scenario, a ROS node controlling the robot's movement will subscribe to this topic and translate the received velocity commands into motor commands to drive the robot's wheels or tracks. Here, we implemented the diff drive system plugin in the `char-01-model.sdf` file. ```py left_wheel_joint right_wheel_joint 1.2 0.4 1 cmd_vel ``` ## World Design As for the model world, I created an SDF file in the worlds folder and connected four walls in neon blue to contain the CHAR 01 robot. We also added some lighting. Code is [here](https://github.com/willdphan/char-01/blob/4210aafeb5c262d53692909b3e651718de7305d2/ros2_pkg/worlds/char_01_world.sdf).

## LiDAR Node I used this tutorial in ROS2 Documentation: Humble in order to get a feel of what visualizing lidar sensor is like. The tutorial makes it easy to run a pre-made world with a lidar model so check it out! Afterwards, I developed this ROS2 node in C++ for a LiDAR sensor called lidar_node.cpp shown below.

Overall, the LidarNode class inherits from rclcpp::Node, which is a base class for ROS2 nodes. In the constructor, the node is named lidar_node. It creates a subscription that listens to sensor_msgs::msg::LaserScanmessages on the lidar topic. The callback function for this subscription is LidarNode::callback. ```cpp class LidarNode : public rclcpp::Node { // Class definition goes here }; public: LidarNode() : Node("lidar_node") { sub_ = this->create_subscription( "lidar", 10, std::bind(&LidarNode::callback, this, std::placeholders::_1)); } ``` Within the class, this function is called whenever a new LaserScan message is received. It writes the range readings to a file. ```cpp private: void callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { std::ofstream file; std::string package_path = ament_index_cpp::get_package_share_directory("ros2_pkg"); std::string file_path = ".../char-01/ros2_pkg/data"; file.open(file_path, std::ios_base::app); for (const auto& range : msg->ranges) { file << range << " "; } file << "\n"; file.close(); } ``` This function initializes the ROS2 system, spins the LidarNode (i.e., starts the node and keeps it running to process callbacks), and then shuts down the ROS2 system when the node stops. ```cpp int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } ``` In this version of lidar_node.cc, the LidarNode only subscribes to LaserScan messages and writes the range readings to a file. Make sure u add the new lidar_node to your CMakeLists.txt file when building your package! After sourcing your bash file, you can go into terminal and type rviz2 for another window to pop up. In the window, you can configure the "Fixed Frame". I entered vehicle_blue/chassis/gpu_lidar but this may vary depending how you set up your model. And then click in the button "Add" to include a display and visualize the lidar.

## Collect Data You could manually control the robot with the lidar sensor in order to gather data from the simulation but... that would be time-consuming, and why do that when Niko already did this for us? In his repo, I snatched the all.txt data, visualize.py, train_model.py, and predict.py files and altered them to fit my project. In his all.txt data, the end of each row generated by the sensor consists of one of these letters: - `F` = Forward - `I` = Forward Right - `R` = Right - `G` = Forward Left - `L` = Left These letters are used to label the sensor data with the corresponding movement or direction, which is crucial for training a model to understand and predict movements based on sensor inputs. ## Visualize Data At a high level, the visualize.py script visualizes LiDAR sensor data. It reads data from the all.txt file, processes it, and for each set of data that matches the LiDAR resolution, it generates a polar plot. CHAR-01 Plot

Each data point is represented as a black dot, and an additional orange dot is plotted at the center of the plot the orange dot plotted at the coordinates (400, 400) representing the location of the LiDAR sensor or the robot model itself in the visualization. The plot is then displayed to the user. ```py for line in Lines: distances = GetData(line) print(len(distances)) if (len(distances) == LIDAR_RESOLUTION): # Create a new figure for each line plt.figure() for x in range(VISUALIZATION_RESOLUTION): if distances[x] == 0: distances[x] = 20 a = int(float(distances[x]))/2000 plt.plot(line_positions[x][0]*a+400, line_positions[x][1]*a+400, 'ko') plt.plot(400, 400, 'o', color=(252/255, 132/255, 3/255)) plt.show() ``` ## Train Model This `train_model.py` script performs a machine learning workflow, where it first preprocesses and encodes a dataset, then selects key features for training a [Random Forest Classifier](https://scikit-learn.org/stable/modules/generated/sklearn.ensemble.RandomForestClassifier.html). After training, the model's accuracy is evaluated and the model, along with its components, is saved for future use. This end-to-end process encompasses data preparation, model training, evaluation, and serialization, making it a comprehensive solution for a classification task. The script reads the `all.txt` file and loads it into a pandas DataFrame. ```py data = pd.read_csv('.../data/[path_to_all.txt]', header=None) ``` Here, the script renames the last column to "Label" and filters out rows with specific labels `(L, R, H, J)`. It then splits the data into features `(X)` and labels `(y)`. ```py data.rename(columns={data.columns[-1]: 'Label'}, inplace=True) data = data[(data['Label'] != 'L') & (data['Label'] != 'R') & (data['Label'] != 'H') & (data['Label'] != 'J')] data.reset_index(drop=True, inplace=True) X = data.iloc[:, :-1] y = data.iloc[:, -1] ``` The `LabelEncoder` is used to convert categorical labels into a numeric format. ```pylabel_encoder = LabelEncoder() y = label_encoder.fit_transform(y) ``` The dataset is divided into training and testing sets, with 20% of the data reserved for testing. ```py X_train, X_test, y_train, y_test = train_test_split(X, y, test_size=0.2, random_state=42) ``` The script selects the top 80 features using ANOVA F-value as the score function. The indices of these features are printed. ```py k = 80 k_best = SelectKBest(score_func=f_classif, k=k) k_best.fit(X_train, y_train) selected_feature_indices = k_best.get_support(indices=True) print("selected_feature_indices: ", selected_feature_indices) ``` A Random Forest Classifier is trained on the selected features of the training set. ```py clf = RandomForestClassifier(max_depth=3, random_state=42) clf.fit(X_train.iloc[:, selected_feature_indices], y_train) ``` The model makes predictions on the test set, and the script evaluates its performance by calculating the accuracy and generating a classification report. ```py y_pred = clf.predict(X_test.iloc[:, selected_feature_indices]) accuracy = accuracy_score(y_test, y_pred) print(f'Accuracy: {accuracy}') class_names = label_encoder.classes_ report = classification_report(y_test, y_pred, target_names=class_names, zero_division=0) print('Classification Report:\n', report) ``` Finally, the trained Random Forest model, the `SelectKBest` object, and the `LabelEncoder` are saved to files for future use. ```py joblib.dump(clf, '...data/rf_model/path_to_random_forest_model.pkl') joblib.dump(k_best, '...data/k_best/path_to_k_best.pkl') joblib.dump(label_encoder, '.../data/label_encoder/path_to_label_encoder.pkl') ``` This is the overall accuracy of the model, which is 66.69%. It tells you how often the model makes the correct prediction across all classes. From these results, you can infer that the model is better at correctly identifying forward movements `F` compared to left `G` or right `I` ones. The model is relatively precise when it predicts a movement as forward right `I`, but it doesn't recall them well (many forward right movements are missed), indicating an imbalance between precision and recall for this class. Either way, I'll just use this for now.

## Predictions This `predict.py` defines a ROS2 node in Python for making predictions based on LiDAR data using a trained machine learning model, specifically a Random Forest Classifier. Here's a breakdown of its functionality: - `Predictor` class is a custom ROS2 node. It subscribes to the `lidar` topic, loads a trained model, a feature selector, and a label encoder, and publishes predictions to the `prediction` topic. - `joblib.load` function is used to load the trained Random Forest model from a file. The loaded model is stored in `self.clf`. - `joblib.load` function is also used to load the feature selector from a file. The loaded feature selector is stored in `self.k_best`. - Similarly, the `joblib.load` function is used to load the label encoder from a file. The loaded label encoder is stored in `self.label_encoder`. The `listener_callback` method is called whenever a new message is published on the `lidar` topic. It preprocesses the data, makes a prediction using the trained model, and publishes the prediction. ```py # the received data is preprocessed and # then passed to the model to make a prediction. def listener_callback(self, msg): print("listener_callback called") # Convert the LaserScan data to a numpy array data = np.array(msg.ranges) # Select a subset of the range readings data = data[:240] # Replace infinities with a large finite number data[data == np.inf] = 1e10 # Preprocess the data data = self.preprocess_data(data) # Make a prediction prediction = self.clf.predict(data) print("Made prediction:", prediction) # Convert the prediction to a string prediction_str = ' '.join(map(str, prediction)) # Create a String message and set its data field prediction_msg = String() prediction_msg.data = prediction_str # Publish the prediction self.publisher_.publish(prediction_msg) ``` The `preprocess_data` method is used to preprocess the LiDAR data before it's passed to the machine learning model for prediction. ```py # Preprocess data here def preprocess_data(self, data): # This will depend on how you preprocessed your data when training the model # For example, you might need to select the best features like this: data = self.k_best.transform(data.reshape(1, -1)) return data ``` While the script is running, it will keep making predictions every time a new message is published on the `lidar` topic, and it will only shut down when the ROS2 system is shut down. ```py def main(args=None): rclpy.init(args=args) predictor = Predictor() rclpy.spin(predictor) # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) predictor.destroy_node() rclpy.shutdown() if __name__ == '__main__': main() ``` ## Control Node This C++ file, `control.cpp`, defines a node that listens for predictions and publishes corresponding Twist messages to control a robot. Its constructor for `ControlNode` initializes the node with the name `control_node`, creates a publisher that publishes `geometry_msgs::msg::Twist` messages on the `cmd_vel` topic, and creates a subscription that listens for `std_msgs::msg::String` messages on the `prediction` topic. ```cpp public: ControlNode() : Node("control_node") { // publishes to /cmd_vel publisher_ = this->create_publisher("cmd_vel", 10); // listens to prediction subscription_ = this->create_subscription( "prediction", 10, std::bind(&ControlNode::callback, this, std::placeholders::_1)); } ``` The callback function is called whenever a new message is received on the `prediction` topic. It converts the prediction from a string to an integer, creates a `geometry_msgs::msg::Twist` message based on the prediction, and publishes this message on the `cmd_vel` topic. ```cpp private: void callback(const std_msgs::msg::String::SharedPtr msg) { geometry_msgs::msg::Twist twist; // Convert the prediction from string to int int prediction = std::stoi(msg->data); if (prediction == 1) { twist.linear.x = 1.0; // Move forward at 1.0 m/s twist.angular.z = 0.0; // No rotation } else if (prediction == 2) { twist.linear.x = 0.0; // No forward movement twist.angular.z = 1.0; // Rotate counter-clockwise at 1.0 rad/s } else if (prediction == 3) { twist.linear.x = 0.0; // No forward movement twist.angular.z = -1.0; // Rotate clockwise at 1.0 rad/s } publisher_->publish(twist); } ``` The main function initializes the ROS2 system, creates a `ControlNode`, spins the node to process callbacks, and then shuts down the ROS2 system when the node is done. ```cpp int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } ``` ## Launch Finally, a launch file is created to start a simulation environment and several ROS2 nodes for a robotic system. At a high level, it does the following: 1. Starts the Ignition Gazebo simulator with a specific world file. 2. Launches a node (`predict_node`) that uses a machine learning model to make predictions based on LiDAR data. 3. Launches a node (`control_node`) for controlling the robot (the specifics of what this node does aren't provided in the launch file). 4. Launches a node (`lidar_node`) for handling LiDAR data (the specifics of what this node does aren't provided in the launch file). 5. Sets up bridges between ROS2 and Ignition Gazebo for the `/lidar` and `/cmd_vel` topics, allowing ROS2 nodes to interact with the simulator. ```cpp ExecuteProcess( cmd=['ign', 'gazebo', '.../worlds/char_01_world.sdf'], output='screen' ) Node( package="ros2_pkg", executable="predict.py", name="predict_node", output='screen' ) Node( package="ros2_pkg", executable="control", name="control_node", output='screen' ) Node( package="ros2_pkg", executable="lidar_node", name="lidar_node", output='screen' ) ExecuteProcess( cmd=['ros2', 'run', 'ros_gz_bridge', 'parameter_bridge', '/lidar@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan'], output='screen' ) ExecuteProcess( cmd=['ros2', 'run', 'ros_gz_bridge', 'parameter_bridge', '/cmd_vel@geometry_msgs/msg/Twist@ignition.msgs.Twist'], output='screen' ) ``` The video is sped up 1.5x times... but look at CHAR 01 go! It's moving!

In essence, this launch file sets up a complete system for simulating, controlling, and making predictions for a robot using ROS2 and Ignition Gazebo. ## What's Next? Now you need to build the package, source the `setup.bash`, then run the launch file to get the simulation started! The model sometimes pauses to try and predict its next move. If you want to just clone this project and get started, check out the [README.md](https://github.com/willdphan/char-01.git). As you can see, the model I've created isn't perfect and there's definitely room for improvement. For instance, experimenting with different models might yield more efficient results. I'm also thinking that incorporating reinforcement learning could be a game-changer, especially in terms of enabling CHAR 01 to better explore its environment. But hey, this project is just one step in my ongoing learning journey, and I'm genuinely thrilled about what lies ahead. Overall, I'm really proud of the progress I've made with ROS2, Gazebo, and C++, and I'm excited to continue exploring this field brimming with possibilities. Hope to update you all!