Rtabmap

README.md

@@ -1,41 +1,82 @@
-# ros-gz-crazyflie
-
-> Still work in progress!
-
-> The modules for the multiranger, like the simple mapper and wall following are moved to: https://github.com/knmcguire/crazyflie_ros2_multiranger
-
-Made for ROS2 humble and Gazebo Harmonic on Ubuntu 22.04
-
+# ros_gz_crazyflie
+
+## Overview
+The `ros_gz_crazyflie` repository contains several ROS 2 packages,
+each designed for simulating the crazyflie robot in Gazebo Fortress.
+These packages provide the necessary files and configurations to
+spawn the crazyflie robot in the Gazebo simulation environment and
+interact with it using ROS 2.
+## Features
+- Pre-configured launch files for easy simulation setup.
+- Crazyflie SDF model integration.
+- Compatibility with Gazebo Fortress.
+- ROS 2 interface for controlling and monitoring the robot.
+## Requirements
+- **Operating System:** Ubuntu 22.04
+- **ROS 2 Distribution:** Humble
+- **Gazebo Version:** Fortress
+## Installation
+1. Clone the repository into your ROS 2 workspace:
+```bash
+cd ~/ros2_ws/src
+git clone <repository-url>
+```
+2. Build the workspace:
+```bash
+cd ~/ros2_ws
+colcon build
+```
+3. Source the workspace:
+```bash
+source ~/ros2_ws/install/setup.bash
+```
 ## Usage
+### Launching the Simulation
+#### Indoor Simulation
+To launch the Crazyflie in an indoor environment, use the following
+command:
+```bash
+ros2 launch ros_gz_crazyflie_bringup crazyflie_house.launch.py
+```
+#### Outdoor Simulation
+To launch the Crazyflie in an outdoor environment, use the following
+command:
+```bash
+ros2 launch ros_gz_crazyflie_bringup crazyflie_forest.launch.py
+```
+### Controlling the Robot
+You can control the crazyflie using teleoperation
+node. For example, to control the robot via the `/cmd_vel` topic:
+```bash
+ros2 run teleop_twist_keyboard teleop_twist_keyboard
+```
+Take off with pressing 't' and then control the crazyflie with the keyboard according to the teleop_twist_keyboard instructions.
 
-Make a workspace and clone this gazebo-ros crazyflie simulation repo
-
-    mkdir ~/ros2_ws/src
-    cd ~/ros2_ws/src
-    git clone https://github.com/knmcguire/ros_gz_crazyflie
-
-
-Clone the crazyflie simulation repo and source the crazyflie model
-
-    mkdir ~/simulation_models/
-    cd ~/simulation_models/
-    git clone [email protected]:bitcraze/crazyflie-simulation.git
-    export GZ_SIM_RESOURCE_PATH=~/simulation_models/simulator_files/gazebo/"
-
-Build the ros workspace with
-
-    cd  ~/ros2_ws/
-    colcon build --cmake-args -DBUILD_TESTING=ON
-
-Run with
 
-    ros2 launch ros_gz_crazyflie_bringup crazyflie_simulation.launch.py
+## Crazyflie Mapping
 
+To run RTAB-Map for mapping, use the following command:
+```
+ros2 launch ros_gz_crazyflie_nav rtab_mapping_house.launch.py
+```
+This launch file will open Gazebo Ignition, RViz, and RTAB-Map visualization tools (rtabviz). When the map construction is completed, exit the map construction node by pressing Ctrl+C. The system will automatically save the map.
 
-In a different terminal, open ROS 2 teleop twist keyboard node with
+The default saving path of the map is:
+```
+~/.ros/rtabmap.db
+```
+To view the map database, use the following command:
+```
+rtabmap-databaseViewer path/to/rtabmap.db
+```
+From there, you can save the map in .pcd format for further use. The .pcd file can be seen by downloading the extension `pcd-viewer` on vscode
 
-    ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap cmd_vel:=crazyflie/cmd_vel
+## Troubleshooting
+### Gazebo Fails to Launch
+Ensure that your `IGN_GAZEBO_RESOURCE_PATH` environment variable is correctly set, for example: 
+```bash
+export IGN_GAZEBO_RESOURCE_PATH=~/ros2_ws/src/ros_gz_crazyflie/simulator_files/gazebo
+```
 
-Take off with pressing 't' and then control the crazyflie with the keyboard according to the teleop_twist_keyboard instructions.
 
 

crazyflie-simulation/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2022 Bitcraze
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

crazyflie-simulation/README.md

@@ -0,0 +1,18 @@
+# crazyflie-simulation
+
+Hi! Welkom to the crazyflie simulation repo. This repo is still at the early stages and still in development so just ask if you are looking for anything in particular.
+
+Currently it contains low resolution meshes based on the design of the actual [crazyflie 2.1](https://www.bitcraze.io/products/crazyflie-2-1/) but heavily simplified to contain as little vertices as possible, so it can be used to simulate a lot of them.
+
+
+## Installation
+
+See the  [installation instructions](/docs/installing/) in the Github docs folder.
+
+## Official documentation
+
+Check out the [Bitcraze crazyflie-simulation documentation](https://www.bitcraze.io/documentation/repository/crazyflie-simulation/main/) on our website.
+
+## Contribute
+
+Go to the [contribute page](https://www.bitcraze.io/development/contribute/) on our website to learn more.

crazyflie-simulation/controllers_shared/c_based/pid_controller.c

@@ -0,0 +1,131 @@
+/*
+ * Copyright 2022 Bitcraze AB
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ *  ...........       ____  _ __
+ *  |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ *  | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ *  | / ,..´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *     +.......   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ *
+ * @file pid_controller.c
+ * Description: A simple PID controller for attitude,
+ *          height and velocity  control of an quadcopter
+ * Author:      Kimberly McGuire (Bitcraze AB)
+
+ */
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "pid_controller.h"
+
+float constrain(float value, const float minVal, const float maxVal) {
+  return fminf(maxVal, fmaxf(minVal, value));
+}
+
+double pastAltitudeError, pastPitchError, pastRollError, pastYawRateError;
+double pastVxError, pastVyError;
+double altitudeIntegrator;
+
+void init_pid_attitude_fixed_height_controller() {
+  pastAltitudeError = 0;
+  pastYawRateError = 0;
+  pastPitchError = 0;
+  pastRollError = 0;
+  pastVxError = 0;
+  pastVyError = 0;
+  altitudeIntegrator = 0;
+}
+
+void pid_attitude_fixed_height_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid,
+                                          double dt, motor_power_t *motorCommands) {
+  control_commands_t control_commands = {0};
+  pid_fixed_height_controller(actual_state, desired_state, gains_pid, dt, &control_commands);
+  pid_attitude_controller(actual_state, desired_state, gains_pid, dt, &control_commands);
+  motor_mixing(control_commands, motorCommands);
+}
+
+void pid_velocity_fixed_height_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid,
+                                          double dt, motor_power_t *motorCommands) {
+  control_commands_t control_commands = {0};
+  pid_horizontal_velocity_controller(actual_state, desired_state, gains_pid, dt);
+  pid_fixed_height_controller(actual_state, desired_state, gains_pid, dt, &control_commands);
+  pid_attitude_controller(actual_state, desired_state, gains_pid, dt, &control_commands);
+  motor_mixing(control_commands, motorCommands);
+}
+
+void pid_fixed_height_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid, double dt,
+                                 control_commands_t *control_commands) {
+  double altitudeError = desired_state->altitude - actual_state.altitude;
+  double altitudeDerivativeError = (altitudeError - pastAltitudeError) / dt;
+  control_commands->altitude =
+    gains_pid.kp_z * constrain(altitudeError, -1, 1) + gains_pid.kd_z * altitudeDerivativeError + gains_pid.ki_z;
+
+  altitudeIntegrator += altitudeError * dt;
+  control_commands->altitude = gains_pid.kp_z * constrain(altitudeError, -1, 1) + gains_pid.kd_z * altitudeDerivativeError +
+                               gains_pid.ki_z * altitudeIntegrator + 48;
+  pastAltitudeError = altitudeError;
+}
+
+void motor_mixing(control_commands_t control_commands, motor_power_t *motorCommands) {
+  // Motor mixing
+  motorCommands->m1 = control_commands.altitude - control_commands.roll + control_commands.pitch + control_commands.yaw;
+  motorCommands->m2 = control_commands.altitude - control_commands.roll - control_commands.pitch - control_commands.yaw;
+  motorCommands->m3 = control_commands.altitude + control_commands.roll - control_commands.pitch + control_commands.yaw;
+  motorCommands->m4 = control_commands.altitude + control_commands.roll + control_commands.pitch - control_commands.yaw;
+}
+
+void pid_attitude_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid, double dt,
+                             control_commands_t *control_commands) {
+  // Calculate errors
+  double pitchError = desired_state->pitch - actual_state.pitch;
+  double pitchDerivativeError = (pitchError - pastPitchError) / dt;
+  double rollError = desired_state->roll - actual_state.roll;
+  double rollDerivativeError = (rollError - pastRollError) / dt;
+  double yawRateError = desired_state->yaw_rate - actual_state.yaw_rate;
+
+  // PID control
+  control_commands->roll = gains_pid.kp_att_rp * constrain(rollError, -1, 1) + gains_pid.kd_att_rp * rollDerivativeError;
+  control_commands->pitch = -gains_pid.kp_att_rp * constrain(pitchError, -1, 1) - gains_pid.kd_att_rp * pitchDerivativeError;
+  control_commands->yaw = gains_pid.kp_att_y * constrain(yawRateError, -1, 1);
+
+  // Save error for the next round
+  pastPitchError = pitchError;
+  pastRollError = rollError;
+  pastYawRateError = yawRateError;
+}
+
+void pid_horizontal_velocity_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid,
+                                        double dt) {
+  double vxError = desired_state->vx - actual_state.vx;
+  double vxDerivative = (vxError - pastVxError) / dt;
+  double vyError = desired_state->vy - actual_state.vy;
+  double vyDerivative = (vyError - pastVyError) / dt;
+
+  // PID control
+  double pitchCommand = gains_pid.kp_vel_xy * constrain(vxError, -1, 1) + gains_pid.kd_vel_xy * vxDerivative;
+  double rollCommand = -gains_pid.kp_vel_xy * constrain(vyError, -1, 1) - gains_pid.kd_vel_xy * vyDerivative;
+
+  desired_state->pitch = pitchCommand;
+  desired_state->roll = rollCommand;
+
+  // Save error for the next round
+  pastVxError = vxError;
+  pastVyError = vyError;
+}

crazyflie-simulation/controllers_shared/c_based/pid_controller.h

@@ -0,0 +1,94 @@
+#pragma once
+
+/*
+ * Copyright 2022 Bitcraze AB
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ *  ...........       ____  _ __
+ *  |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ *  | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ *  | / ,..´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *     +.......   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ *
+ * @file pid_controller.h
+ * Description:  PID controller header file
+ * Author:       Kimberly McGuire (Bitcraze AB)
+ */
+
+typedef struct motor_power_s {
+  double m1;
+  double m2;
+  double m3;
+  double m4;
+} motor_power_t;
+
+typedef struct control_commands_s {
+  double roll;
+  double pitch;
+  double yaw;
+  double altitude;
+} control_commands_t;
+
+typedef struct desired_state_s {
+  double roll;
+  double pitch;
+  double yaw_rate;
+  double altitude;
+  double vx;
+  double vy;
+} desired_state_t;
+
+typedef struct actual_state_s {
+  double roll;
+  double pitch;
+  double yaw_rate;
+  double altitude;
+  double vx;
+  double vy;
+} actual_state_t;
+
+typedef struct gains_pid_s {
+  double kp_att_rp;
+  double kd_att_rp;
+  double kp_att_y;
+  double kd_att_y;
+  double kp_vel_xy;
+  double kd_vel_xy;
+  double kp_z;
+  double kd_z;
+  double ki_z;
+} gains_pid_t;
+
+float constrain(float value, const float minVal, const float maxVal);
+void init_pid_attitude_fixed_height_controller();
+
+void pid_attitude_fixed_height_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid,
+                                          double dt, motor_power_t *motorCommands);
+
+void pid_velocity_fixed_height_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid,
+                                          double dt, motor_power_t *motorCommands);
+
+void pid_fixed_height_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid, double dt,
+                                 control_commands_t *control_commands);
+
+void motor_mixing(control_commands_t control_commands, motor_power_t *motorCommands);
+
+void pid_attitude_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid, double dt,
+                             control_commands_t *control_commands);
+
+void pid_horizontal_velocity_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid,
+                                        double dt);