Skip to content

Commit

Permalink
Merge pull request #2 from gsilano/fix/controller
Browse files Browse the repository at this point in the history
CrazyS improvements and bug fixing into the control architecture
  • Loading branch information
gsilano authored Jun 4, 2018
2 parents 836a1b3 + 9519641 commit 12a4b49
Show file tree
Hide file tree
Showing 49 changed files with 1,662 additions and 1,738 deletions.
25 changes: 13 additions & 12 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,15 @@ CrazyS

CrazyS is an extension of the ROS package [RotorS](https://github.com/ethz-asl/rotors_simulator), aimed to modeling, developing and integrating the [Crazyflie 2.0](https://www.bitcraze.io/crazyflie-2/) nano-quadcopter in the physics based simulation environment Gazebo.

Such simulation platform allows to understand quickly the behavior of the flight control system by comparing and evaluating different indoor and outdoor scenarios, with details level quite close to reality. The proposed extension expands RotorS capabilities by considering the Crazyflie 2.0 physical model and its flight control system, as well.
Such simulation platform allows to understand quickly the behavior of the flight control system by comparing and evaluating different indoor and outdoor scenarios, with a details level quite close to reality. The proposed extension expands RotorS capabilities by considering the Crazyflie 2.0 physical model and its flight control system, as well.

A simple case study is considered (`crazyflie2_hovering_example.launch`) in order to show how the package works and the validity of the employed dynamical model together the control architecture of the quadcopter.

The code is released under Apache license, thus making it available for scientific and educational activities.

Below we provide the instructions necessary for getting started. See CrazyS' wiki for more instructions and examples (https://github.com/gsilano/CrazyS/wiki).
The platform has been developed by using Ubuntu 16.04 and the Kinetic Kame version of ROS, but it is fully compatible with Indigo Igloo and Ubuntu 14.04 (it is not recommended, the support will be untile the April, 2019).

Below we provide the instructions necessary for getting started. See [CrazyS' wiki](https://github.com/gsilano/CrazyS/wiki) for more instructions and examples .

If you are using this simulator within the research for your publication, please cite:
```bibtex
Expand Down Expand Up @@ -46,9 +48,8 @@ Installation Instructions - Ubuntu 16.04 with ROS Kinetic
$ cd ~/catkin_ws/src
$ catkin_init_workspace # initialize your catkin workspace
$ wstool init
$ wget https://raw.githubusercontent.com/gsilano/rotors_simulator/master/rotors_hil.rosinstall
$ wstool merge rotors_hil.rosinstall
$ wstool update
$ git clone https://github.com/gsilano/CrazyS.git
$ git clone https://github.com/gsilano/mav_comm.git
```

> **Note** On OS X you need to install yaml-cpp using Homebrew `brew install yaml-cpp`.
Expand Down Expand Up @@ -88,7 +89,7 @@ Installation Instructions - Ubuntu 14.04 with ROS Indigo
$ catkin_init_workspace # initialize your catkin workspace
$ wstool init
```
> **Note** for setups with multiple workspaces please refer to the official documentation at http://docs.ros.org/independent/api/rosinstall/html/ by replacing `rosws` by `wstool`.
> **Note** for setups with multiple workspaces please refer to the [official documentation](http://docs.ros.org/independent/api/rosinstall/html/) by replacing `rosws` by `wstool`.
3. Get the simulator and additional dependencies

```
Expand Down Expand Up @@ -129,18 +130,18 @@ Installation Instructions - Ubuntu 14.04 with ROS Indigo
Basic Usage
-----------

Launch the simulator with the Crazyflie 2.0 model in a basic world.
Launch the simulator with the Crazyflie 2.0 model in a basic world. By changing the value of the `enable_state_estimator` variable (`true` or `false`), you can enable or disable the state estimator. In other words, it is possible to simulate the behavior of the drone when the real or ideal sensors are in the loop, respectively.

```
$ roslaunch rotors_gazebo crazyflie2_hovering_example.launch
$ roslaunch rotors_gazebo crazyflie2_hovering_example.launch enable_state_estimator:=true
```

> **Note** The first run of gazebo might take considerably long, as it will download some models from an online database.
> **Note** The first run of gazebo might take considerably long, as it will download some models from an online database. You can decided to enable or not the state estimator, thus to use the virtual (with noise and bias) or the real sensors. The default value of `enable_state_estimator` is `false`.
The whole process is the following: the desired trajectory coordinates (![equation](http://latex.codecogs.com/gif.latex?%24x_r%24%2C%20%24y_r%24%2C%20%24z_r%24%2C%20%24%5Cpsi_r%24)) are published by the hovering example node on the topic `command/trajectory`, to whom the position controller
node (i.e., the Crazyflie controller) is subscribed. The drone state (`odometry_sensor1/odometry` topic) and the references are used to run the control strategy designed for the path tracking. The outputs of the control algorithm consists into the actuation commands (![equation](http://latex.codecogs.com/gif.latex?%24%5Comega_1%24%2C%20%24%5Comega_2%24%2C%20%24%5Comega_3%24%2C%20%24%5Comega_4%24)) sent to Gazebo (`command/motor_speed`) for the virtual rendering, so to update the aircraft position and orientation.
The whole process is the following: the desired trajectory coordinates (x_r, y_r, z_r and \psi_r) are published by the hovering example node on the topic `command/trajectory`, to whom the position controller
node (i.e., the Crazyflie controller) is subscribed. The drone state (`odometry_sensor1/odometry` topic) and the references are used to run the control strategy designed for the path tracking. The outputs of the control algorithm consists into the actuation commands (\omega_1, \omega_2, \omega_3 and \omega_4) sent to Gazebo (`command/motor_speed`) for the virtual rendering, so to update the aircraft position and orientation.

There are some basic launch files where you can load the different multicopters with additional sensors. They can all be found in `~/catkin_ws/src/CrazyS/rotors_gazebo/launch`.
There are some basic launch files where you can load the different multicopters with additional sensors. They can all be found in `~/catkin_ws/src/CrazyS/rotors_gazebo/launch`. Suche scenarios are better explained in the [RotorS](https://github.com/ethz-asl/rotors_simulator) repository.

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`.

Expand Down
37 changes: 4 additions & 33 deletions rotors_comm/CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -2,42 +2,13 @@
Changelog for package rotors_comm
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

2.1.1 (2017-04-27)
-----------
* update maintainers
* Contributors: fmina

2.1.0 (2017-04-08)
-----------
* Return the origin of the gazebo coordinates in lat/long/alt as part of the octomap service response.
* Add option to publish octomap in the ServiceCallback of the gazebo_octomap_plugin.
* Adding ability to start and stop rosbag recording on command
* Contributors: Fadri Furrer, Pavel, acfloria

2.0.1 (2015-08-10)
------------------

2.0.0 (2015-08-09)
------------------

1.1.6 (2015-06-11)
------------------

1.1.5 (2015-06-09)
------------------

1.1.4 (2015-05-28)
------------------

1.1.3 (2015-05-28)
------------------

1.1.2 (2015-05-27)
4.0.3 (2018-06-04)
------------------

1.1.1 (2015-04-24)
4.0.2 (2018-02-23)
------------------

1.1.0 (2015-04-24)
4.0.1 (2018-02-02)
------------------
* initial Ubuntu package release
* Contributors: Giuseppe Silano, Luigi Iannelli
11 changes: 5 additions & 6 deletions rotors_comm/package.xml
Original file line number Diff line number Diff line change
@@ -1,16 +1,15 @@
<package>
<name>rotors_comm</name>
<version>2.1.1</version>
<version>4.0.3</version>
<description>RotorS specific messages and services.</description>

<maintainer email="[email protected]">Giuseppe Silano</maintainer>
<maintainer email="[email protected]">Luigi Iannelli</maintainer>

<author>Fadri Furrer</author>
<author>Michael Burri</author>
<author>Mina Kamel</author>
<author>Janosch Nikolic</author>
<author>Markus Achtelik</author>
<author>Giuseppe Silano</author>
<author>Emanuele Aucone</author>
<author>Benjamin Rodriguez</author>
<author>Luigi Iannelli</author>

<license>ASL 2.0</license>

Expand Down
67 changes: 13 additions & 54 deletions rotors_control/CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -2,64 +2,23 @@
Changelog for package rotors_control
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

2.2.0.1 (2018-02-23)
-----------
4.0.3 (2018-06-04)
------------------
* bug fixing in the controller position node and in the controller position, complementary filter and sensfusion6 algorithms
* the controller position node algorithm has been split into two parts: with and without state estimator
* the position controller algorithm now is able to handle both ideal and real simulations (with and without real sensors)
* the control algorithm has been split into two parts: high and low level. In this way, the high level part can be restructured without changes in the low one. Indeed, the Crazyflie on board control architectured does not change unless the firmware is changed
* Contributors: Giuseppe Silano, Luigi Iannelli

4.0.2 (2018-02-23)
------------------
* added the Crazyflie 2.0 default state estimator: complementary filter, in according to the last firmware release 2018.01.01
* modified the position controller in order to take into account the state estimator
* Contributors: Giuseppe Silano, Luigi Iannelli

2.2.0 (2018-02-02)
-----------
4.0.1 (2018-02-02)
------------------
* added Crazyflie 2.0 position controller. The lower level controller is the same of the Crazyflie 2.0 firmware (released 2018.01.01)
* started from the 2.1.1 (2017-04-27) release version of RotorS
* Contributors: Giuseppe Silano, Emanuele Aucone, Benjamin Rodriguez, Luigi Iannelli

2.1.1 (2017-04-27)
-----------
* update maintainers
* Contributors: fmina

2.1.0 (2017-04-08)
-----------
* Removing unfinished parts of px4 dummy controller and gazebo mavlink interface. Refactoring gazebo mavlink interface.
* set all queue lengths to 1
* make rotors_control CMakeLists consistent with upstream
* bring rotors_control CMakeLists closer to upstream
* update launch and xacro files to new names, update use of odometry plugin
* px4 dummy controller: fix include paths after merge
* fix include dir
* move px4 files to new directories
* Contributors: Fadri Furrer, Thomas Gubler, pvechersky

2.0.1 (2015-08-10)
------------------

2.0.0 (2015-08-09)
------------------
* Converted to new mav_comm messages, including new MultiDOFJointTrajectory and PoseStamped as waypoints.
* Added subscriber for MultiDOFJointTrajectory messages and extended waypoint_publisher_file to test it. resolves `#243 <https://github.com/ethz-asl/rotors_simulator/issues/243>`_
* Contributors: Helen Oleynikova, Markus Achtelik

1.1.6 (2015-06-11)
------------------

1.1.5 (2015-06-09)
------------------
* added install targets

1.1.4 (2015-05-28)
------------------

1.1.3 (2015-05-28)
------------------
* added installation of controller libraries

1.1.2 (2015-05-27)
------------------
* added nav_msgs dependency and fixed rotors_evaluation's setup.py

1.1.1 (2015-04-24)
------------------

1.1.0 (2015-04-24)
------------------
* initial Ubuntu package release
26 changes: 19 additions & 7 deletions rotors_control/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ find_package(

catkin_package(
INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS}
LIBRARIES lee_position_controller position_controller roll_pitch_yawrate_thrust_controller sensfusion6 complementary_filter_crazyflie2
LIBRARIES lee_position_controller position_controller crazyflie_onboard_controller roll_pitch_yawrate_thrust_controller sensfusion6 complementary_filter_crazyflie2
CATKIN_DEPENDS geometry_msgs mav_msgs nav_msgs roscpp sensor_msgs
DEPENDS Eigen
)
Expand All @@ -38,6 +38,10 @@ add_library(position_controller
src/library/position_controller.cpp
)

add_library(crazyflie_onboard_controller
src/library/crazyflie_onboard_controller.cpp
)

add_library(roll_pitch_yawrate_thrust_controller
src/library/roll_pitch_yawrate_thrust_controller.cpp
)
Expand All @@ -59,6 +63,9 @@ add_dependencies(position_controller ${catkin_EXPORTED_TARGETS})
target_link_libraries(roll_pitch_yawrate_thrust_controller ${catkin_LIBRARIES})
add_dependencies(roll_pitch_yawrate_thrust_controller ${catkin_EXPORTED_TARGETS})

target_link_libraries(crazyflie_onboard_controller ${catkin_LIBRARIES})
add_dependencies(crazyflie_onboard_controller ${catkin_EXPORTED_TARGETS})

target_link_libraries(complementary_filter_crazyflie2 ${catkin_LIBRARIES})
add_dependencies(complementary_filter_crazyflie2 ${catkin_EXPORTED_TARGETS})

Expand All @@ -70,22 +77,27 @@ add_dependencies(lee_position_controller_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(lee_position_controller_node
lee_position_controller ${catkin_LIBRARIES})

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 complementary_filter_crazyflie2 sensfusion6 ${catkin_LIBRARIES})
add_executable(position_controller_node_with_stateEstimator src/nodes/position_controller_node_with_stateEstimator.cpp)
add_dependencies(position_controller_node_with_stateEstimator ${catkin_EXPORTED_TARGETS})
target_link_libraries(position_controller_node_with_stateEstimator
position_controller complementary_filter_crazyflie2 crazyflie_onboard_controller sensfusion6 ${catkin_LIBRARIES})

add_executable(position_controller_node_without_stateEstimator src/nodes/position_controller_node_without_stateEstimator.cpp)
add_dependencies(position_controller_node_without_stateEstimator ${catkin_EXPORTED_TARGETS})
target_link_libraries(position_controller_node_without_stateEstimator
position_controller complementary_filter_crazyflie2 crazyflie_onboard_controller sensfusion6 ${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 roll_pitch_yawrate_thrust_controller
install(TARGETS lee_position_controller position_controller crazyflie_onboard_controller roll_pitch_yawrate_thrust_controller
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)

install(TARGETS lee_position_controller_node position_controller roll_pitch_yawrate_thrust_controller_node
install(TARGETS lee_position_controller_node position_controller_node_with_stateEstimator position_controller_node_without_stateEstimator roll_pitch_yawrate_thrust_controller_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,20 +49,21 @@

namespace rotors_control {

class ComplementaryFilterCrazyflie2 {
public:
ComplementaryFilterCrazyflie2();
~ComplementaryFilterCrazyflie2();
class ComplementaryFilterCrazyflie2 {
public:
ComplementaryFilterCrazyflie2();
~ComplementaryFilterCrazyflie2();

void EstimatorComplementary(state_t* state, sensorData_t* sensorData);
void EstimateRate(state_t* state, sensorData_t* sensorData);
void EstimateAttitude(state_t* state, sensorData_t* sensorData);

private:
private:

SensFusion sensors_fusion_;

void PositionUpdateVelocity(double* angularVelocityY, double* angularVelocityX, double* angularAccY, double* angularAccX);

};
SensFusion sensors_fusion_;
void PositionUpdateAngularVelocity(double* angularVelocityZ, double* angularVelocityY,
double* angularVelocityX, double* angularAccZ, double* angularAccY, double* angularAccX);
};

}

Expand Down
79 changes: 79 additions & 0 deletions rotors_control/include/rotors_control/controller_parameters.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
/*
* Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy
* Copyright 2018 Emanuele Aucone, University of Sannio in Benevento, Italy
* Copyright 2018 Benjamin Rodriguez, MIT, USA
* Copyright 2018 Luigi Iannelli, 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_H
#define CONTROLLER_PARAMETERS_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 kPDefaultXYController = Eigen::Vector2f(15, -15);
static const Eigen::Vector2f kIDefaultXYController = Eigen::Vector2f(1, -1);

static const Eigen::Vector2f kPDefaultAttitudeController = Eigen::Vector2f(3.5, 3.5);
static const Eigen::Vector2f kIDefaultAttitudeController = Eigen::Vector2f(2, 2);

static const Eigen::Vector3f kPDefaultRateController = Eigen::Vector3f(110, 110, 110);
static const Eigen::Vector3f kIDefaultRateController = Eigen::Vector3f(0, 0, 16.7);

static const double kPDefaultYawController = 3;
static const double kIDefaultYawController = 1;

static const double kPDefaultHoveringController = 14000;
static const double kIDefaultHoveringController = 15000;
static const double kDDefaultHoveringController = 20000;

namespace rotors_control {

class PositionControllerParameters {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
PositionControllerParameters()
: xy_gain_kp_(kPDefaultXYController),
xy_gain_ki_(kIDefaultXYController),
attitude_gain_kp_(kPDefaultAttitudeController),
attitude_gain_ki_(kIDefaultAttitudeController),
rate_gain_kp_(kPDefaultRateController),
rate_gain_ki_(kIDefaultRateController),
yaw_gain_kp_(kPDefaultYawController),
yaw_gain_ki_(kIDefaultYawController),
hovering_gain_kp_(kPDefaultHoveringController),
hovering_gain_ki_(kIDefaultHoveringController),
hovering_gain_kd_(kDDefaultHoveringController) {
}

Eigen::Vector2f xy_gain_kp_;
Eigen::Vector2f xy_gain_ki_;

Eigen::Vector2f attitude_gain_kp_;
Eigen::Vector2f attitude_gain_ki_;

Eigen::Vector3f rate_gain_kp_;
Eigen::Vector3f rate_gain_ki_;

double yaw_gain_kp_;
double yaw_gain_ki_;

double hovering_gain_kp_;
double hovering_gain_ki_;
double hovering_gain_kd_;
};

}

#endif // CONTROLLER_PARAMETERS_H
Loading

0 comments on commit 12a4b49

Please sign in to comment.