[GAZEBO_7] Add Mellinger's and Silano et. all controllers

.travis.yml

@@ -88,6 +88,7 @@ before_install:
   - sudo apt-get install ros-$ROS_DISTRO-desktop-full ros-$ROS_DISTRO-joy ros-$ROS_DISTRO-octomap-ros ros-$ROS_DISTRO-mavlink
   - sudo apt-get install python-wstool python-catkin-tools python-rosinstall build-essential
   - sudo apt-get install protobuf-compiler libgoogle-glog-dev python-rosinstall-generator
+  - if [[ "$ROS_DISTRO" == "melodic" ]]; then sudo apt-get install python-rosdep; fi
   # Prepare rosdep to install dependencies.
   - sudo rosdep init
   - rosdep update
@@ -107,6 +108,7 @@ install:
   - if [[ "$ROS_DISTRO" == "melodic" ]]; then git checkout dev/ros-melodic; fi
   - cd ~/catkin_ws/src/mav_comm
   - if [[ "$ROS_DISTRO" == "kinetic" ]]; then git checkout crazys; fi
+  - if [[ "$ROS_DISTRO" == "indigo" ]]; then git checkout crazys; fi
   - if [[ "$ROS_DISTRO" == "melodic" ]]; then git checkout med18_gazebo9; fi
   - cd ~/catkin_ws
   # Please, withe ROS Indigo follow what is reported at the link http://rosindex.github.io/p/mavros/#indigo

README.md

@@ -203,6 +203,13 @@ $ catkin build
 $ source ~/.bashrc
 ```
 
+> **Note** In case the `ERROR[rotors_gazebo_plugins]` error is displayed, run the following commands
+>```console
+>$ sudo apt-get install ros-kinetic-gazebo9-plugins
+>$ sudo apt-get install apt ros-kinetic-gazebo9-ros
+>$ sudo apt-get install apt ros-kinetic-gazebo9-dev
+>```
+
 This guide can be used a basis for fixing what has been discussed in [ethz-asl/rotors_simulator#506](https://github.com/ethz-asl/rotors_simulator/pull/506).
 
 Installation Instructions - Ubuntu 14.04 with ROS Indigo
@@ -298,6 +305,26 @@ There are some basic launch files where you can load the different multicopters
 
 The `world_name` argument looks for a .world file with a corresponding name in `~/catkin_ws/src/CrazyS/rotors_gazebo/worlds`. By default, all launch files, with the exception of those that have the world name explicitly included in the file name, use the empty world described in `basic.world`.
 
+Using the `csvFilesStoring` variable is possible to enable (true) or disable (false) the data storage. The log files are saved in the home directory (the path can be easily changed modifying the `position_controller.cpp` file). The recording time can be set via the `csvFilesStoringTime` while the user account can be set via the `user_account` variable. Of course, the log features can be used with and without the complementary filter.
+
+```console
+$ roslaunch rotors_gazebo crazyflie2_hovering_example.launch csvFilesStoring:=true
+```
+
+An alternative controller is available on the repository. For running the simulation by using the Internal Model Control as described in #27, #28 and #29, simply run
+
+```console
+$ roslaunch rotors_gazebo crazyflie2_crazyflie2_internal_model_controller.luanch
+```
+
+> **Note** There is also a draft of the Mellinger's controller implementation in the package. This is a NOT WORKING example. As soon as the problems are resolved, a working version will be made available.
+
+The package also provides a launch file for piloting the Crazyflie using a PC joystick. To run the simulation simple copy and paste the command in the following in a terminal window
+
+```console
+$ roslaunch rotors_gazebo crazyflie2_with_joy.launch
+```
+
 Gazebo Version
 --------------
 

rotors_comm/CHANGELOG.rst

@@ -2,6 +2,15 @@
 Changelog for package rotors_comm
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.0.10 (2020-04-14)
+-------------------
+
+4.0.9 (2020-03-22)
+------------------
+
+4.0.8 (2020-02-09)
+------------------
+
 4.0.7 (2019-12-28)
 ------------------
 

rotors_comm/package.xml

@@ -1,6 +1,6 @@
 <package>
   <name>rotors_comm</name>
-  <version>4.0.7</version>
+  <version>4.0.10</version>
   <description>RotorS specific messages and services.</description>
 
   <maintainer email="giuseppe.silano@unisannio.it">Giuseppe Silano</maintainer>

rotors_control/CHANGELOG.rst

@@ -2,9 +2,21 @@
 Changelog for package rotors_control
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-4.0.8 (2020-01-18)
+4.0.10 (2020-04-14)
+-------------------
+* Improvements in the include folder
+* Add INDI and Mellinger's controllers
+* Contributors: Ria Sonecha, Giuseppe Silano
+
+4.0.9 (2020-03-22)
+------------------
+* Add data saving features
+* Contributors: Giuseppe Silano
+
+4.0.8 (2020-02-09)
 ------------------
 * Fix typo in the position_controller_node with the enable_state_estimator variable #24
+* Add RollPitchYawRateThrust controller library for piloting the Crazyflie using the joystick #30
 * Contributors: Giuseppe Silano
 
 4.0.7 (2019-12-28)

rotors_control/CMakeLists.txt

@@ -18,7 +18,9 @@ find_package(
 
 catkin_package(
   INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS}
-  LIBRARIES lee_position_controller position_controller crazyflie_onboard_controller roll_pitch_yawrate_thrust_controller sensfusion6 crazyflie_complementary_filter
+  LIBRARIES lee_position_controller position_controller crazyflie_onboard_controller
+    roll_pitch_yawrate_thrust_controller sensfusion6 crazyflie_complementary_filter
+    mellinger_controller internal_model_controller
   CATKIN_DEPENDS geometry_msgs mav_msgs nav_msgs roscpp sensor_msgs
   DEPENDS Eigen
 )
@@ -54,12 +56,26 @@ add_library(sensfusion6
   src/library/sensfusion6.cpp
 )
 
+add_library(mellinger_controller
+  src/library/mellinger_controller.cpp
+)
+
+add_library(internal_model_controller
+  src/library/internal_model_controller.cpp
+)
+
 target_link_libraries(lee_position_controller ${catkin_LIBRARIES})
 add_dependencies(lee_position_controller ${catkin_EXPORTED_TARGETS})
 
 target_link_libraries(position_controller ${catkin_LIBRARIES})
 add_dependencies(position_controller ${catkin_EXPORTED_TARGETS})
 
+target_link_libraries(mellinger_controller ${catkin_LIBRARIES})
+add_dependencies(mellinger_controller ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(internal_model_controller ${catkin_LIBRARIES})
+add_dependencies(internal_model_controller ${catkin_EXPORTED_TARGETS})
+
 target_link_libraries(roll_pitch_yawrate_thrust_controller ${catkin_LIBRARIES})
 add_dependencies(roll_pitch_yawrate_thrust_controller ${catkin_EXPORTED_TARGETS})
 
@@ -80,15 +96,17 @@ target_link_libraries(lee_position_controller_node
 add_executable(position_controller_node src/nodes/position_controller_node.cpp)
 add_dependencies(position_controller_node ${catkin_EXPORTED_TARGETS})
 target_link_libraries(position_controller_node
-  position_controller crazyflie_complementary_filter crazyflie_onboard_controller sensfusion6 ${catkin_LIBRARIES})
+  position_controller crazyflie_complementary_filter crazyflie_onboard_controller
+  sensfusion6 mellinger_controller internal_model_controller ${catkin_LIBRARIES})
 
 add_executable(roll_pitch_yawrate_thrust_controller_node
   src/nodes/roll_pitch_yawrate_thrust_controller_node.cpp)
 add_dependencies(roll_pitch_yawrate_thrust_controller_node ${catkin_EXPORTED_TARGETS})
 target_link_libraries(roll_pitch_yawrate_thrust_controller_node
   roll_pitch_yawrate_thrust_controller ${catkin_LIBRARIES})
 
-install(TARGETS lee_position_controller position_controller crazyflie_onboard_controller roll_pitch_yawrate_thrust_controller
+install(TARGETS lee_position_controller position_controller crazyflie_onboard_controller
+  roll_pitch_yawrate_thrust_controller mellinger_controller internal_model_controller
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 )
 

rotors_control/include/rotors_control/controller_parameters_mellinger.h

@@ -0,0 +1,101 @@
+/*
+ * Copyright 2020 Ria Sonecha, Massachusetts Institute of Technology in Cambridge, MA, USA
+ * Copyright 2020 Giuseppe Silano, University of Sannio in Benevento, Italy
+ *
+ * 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
+ *
+ *     http://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.
+ */
+
+#ifndef CONTROLLER_PARAMETERS_MELLINGER_H
+#define CONTROLLER_PARAMETERS_MELLINGER_H
+
+// Default values for the position controller of the Crazyflie2. XYController [x,y], AttitudeController [phi,theta]
+//RateController [p,q,r], YawController[yaw], HoveringController[z]
+static const Eigen::Vector2f kpXYDefaultPositionController = Eigen::Vector2f(0.4, 0.4);
+static const Eigen::Vector2f kdXYDefaultPositionController = Eigen::Vector2f(0.2, 0.2);
+static const Eigen::Vector2f kiXYDefaultPositionController = Eigen::Vector2f(0.05, 0.05);
+
+static const double kpZDefaultPositionController = 1.25;
+static const double kdZDefaultPositionController = 0.4;
+static const double kiZDefaultPositionController = 0.05;
+
+static const Eigen::Vector2f krXYDefaultPositionController = Eigen::Vector2f(70000, 70000);
+static const Eigen::Vector2f kwXYDefaultPositionController = Eigen::Vector2f(20000, 20000);
+static const Eigen::Vector2f ki_mXYDefaultPositionController = Eigen::Vector2f(0.0, 0.0);
+
+static const double krZDefaultPositionController = 60000;
+static const double kwZDefaultPositionController = 12000;
+static const double ki_mZDefaultPositionController = 500;
+
+static const Eigen::Vector2f iRangeMXY = Eigen::Vector2f(1.0, 1.0);
+static const Eigen::Vector2f iRangeXY = Eigen::Vector2f(2.0, 2.0);
+
+static const double iRangeMZ = 1500;
+static const double iRangeZ = 0.4;
+
+static const Eigen::Vector2f kdOmegaRP = Eigen::Vector2f(200, 200);
+
+
+namespace rotors_control {
+
+	class MellingerControllerParameters {
+	 public:
+	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	  MellingerControllerParameters()
+	      : kpXYPositionController_(kpXYDefaultPositionController),
+					kdXYPositionController_(kdXYDefaultPositionController),
+					kiXYPositionController_(kiXYDefaultPositionController),
+					kpZPositionController_(kpZDefaultPositionController),
+					kdZPositionController_(kdZDefaultPositionController),
+					kiZPositionController_(kiZDefaultPositionController),
+					krXYPositionController_(krXYDefaultPositionController),
+					kwXYPositionController_(kwXYDefaultPositionController),
+					ki_mXYPositionController_(ki_mXYDefaultPositionController),
+					krZPositionController_(krZDefaultPositionController),
+					kwZPositionController_(kwZDefaultPositionController),
+					ki_mZPositionController_(ki_mZDefaultPositionController),
+					iRangeMXY_(iRangeMXY),
+					iRangeXY_(iRangeXY),
+					iRangeMZ_(iRangeMZ),
+					iRangeZ_(iRangeZ),
+					kdOmegaRP_(kdOmegaRP){
+	  }
+
+
+		Eigen::Vector2f kpXYPositionController_;
+		Eigen::Vector2f kdXYPositionController_;
+		Eigen::Vector2f kiXYPositionController_;
+
+		double kpZPositionController_;
+		double kdZPositionController_;
+		double kiZPositionController_;
+
+		Eigen::Vector2f krXYPositionController_;
+		Eigen::Vector2f kwXYPositionController_;
+		Eigen::Vector2f ki_mXYPositionController_;
+
+		double krZPositionController_;
+		double kwZPositionController_;
+		double ki_mZPositionController_;
+
+		Eigen::Vector2f iRangeMXY_;
+		Eigen::Vector2f iRangeXY_;
+
+		double iRangeMZ_;
+		double iRangeZ_;
+
+		Eigen::Vector2f kdOmegaRP_;
+	};
+
+}
+
+#endif // CONTROLLER_PARAMETERS_MELLINGER_H