Add a swarm example with the Crazyflie #44 (Gazebo 7)

gsilano · Apr 23, 2020 · 5834328 · 5834328
1 parent cf4d581
commit 5834328
Show file tree

Hide file tree

Showing 36 changed files with 579 additions and 157 deletions.
diff --git a/README.md b/README.md
@@ -317,6 +317,12 @@ An alternative controller is available on the repository. For running the simula
 $ roslaunch rotors_gazebo crazyflie2_crazyflie2_internal_model_controller.luanch
 ```
 
+while a simple swarm example is available at
+
+```console
+$ roslaunch rotors_gazebo crazyflie2_swarm_hovering_example.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

diff --git a/rotors_comm/CHANGELOG.rst b/rotors_comm/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package rotors_comm
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.0.11 (2020-04-23)
+-------------------
+
 4.0.10 (2020-04-14)
 -------------------
 

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

diff --git a/rotors_control/CHANGELOG.rst b/rotors_control/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package rotors_control
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.0.11 (2020-04-23)
+-------------------
+* Contributors: fix missing input error when running the control algorithm
+* Contributors: Giuseppe Silano
+
 4.0.10 (2020-04-14)
 -------------------
 * Improvements in the include folder

diff --git a/rotors_control/package.xml b/rotors_control/package.xml
@@ -1,6 +1,6 @@
 <package>
   <name>rotors_control</name>
-  <version>4.0.10</version>
+  <version>4.0.11</version>
   <description>RotorS control package</description>
 
   <maintainer email="giuseppe.silano@unisannio.it">Giuseppe Silano</maintainer>

diff --git a/rotors_control/src/nodes/position_controller_node.cpp b/rotors_control/src/nodes/position_controller_node.cpp
@@ -170,30 +170,30 @@ void PositionControllerNode::InitializeParams() {
 
     // Parameters reading from rosparam.
     GetRosParameter(pnh, "xy_gain_kp/x",
-                    position_controller_.controller_parameters_.xy_gain_kp_.x(),
-                    &position_controller_.controller_parameters_.xy_gain_kp_.x());
+                   position_controller_.controller_parameters_.xy_gain_kp_.x(),
+                   &position_controller_.controller_parameters_.xy_gain_kp_.x());
     GetRosParameter(pnh, "xy_gain_kp/y",
-                    position_controller_.controller_parameters_.xy_gain_kp_.y(),
-                    &position_controller_.controller_parameters_.xy_gain_kp_.y());
+                   position_controller_.controller_parameters_.xy_gain_kp_.y(),
+                   &position_controller_.controller_parameters_.xy_gain_kp_.y());
     GetRosParameter(pnh, "xy_gain_ki/x",
-                    position_controller_.controller_parameters_.xy_gain_ki_.x(),
-                    &position_controller_.controller_parameters_.xy_gain_ki_.x());
+                   position_controller_.controller_parameters_.xy_gain_ki_.x(),
+                   &position_controller_.controller_parameters_.xy_gain_ki_.x());
     GetRosParameter(pnh, "xy_gain_ki/y",
-                    position_controller_.controller_parameters_.xy_gain_ki_.y(),
-                    &position_controller_.controller_parameters_.xy_gain_ki_.y());
+                   position_controller_.controller_parameters_.xy_gain_ki_.y(),
+                   &position_controller_.controller_parameters_.xy_gain_ki_.y());
 
     GetRosParameter(pnh, "attitude_gain_kp/phi",
-                    position_controller_.controller_parameters_.attitude_gain_kp_.x(),
-                    &position_controller_.controller_parameters_.attitude_gain_kp_.x());
+                   position_controller_.controller_parameters_.attitude_gain_kp_.x(),
+                   &position_controller_.controller_parameters_.attitude_gain_kp_.x());
     GetRosParameter(pnh, "attitude_gain_kp/phi",
-                    position_controller_.controller_parameters_.attitude_gain_kp_.y(),
-                    &position_controller_.controller_parameters_.attitude_gain_kp_.y());
+                   position_controller_.controller_parameters_.attitude_gain_kp_.y(),
+                   &position_controller_.controller_parameters_.attitude_gain_kp_.y());
     GetRosParameter(pnh, "attitude_gain_ki/theta",
-                    position_controller_.controller_parameters_.attitude_gain_ki_.x(),
-                    &position_controller_.controller_parameters_.attitude_gain_ki_.x());
+                   position_controller_.controller_parameters_.attitude_gain_ki_.x(),
+                   &position_controller_.controller_parameters_.attitude_gain_ki_.x());
     GetRosParameter(pnh, "attitude_gain_ki/theta",
-                    position_controller_.controller_parameters_.attitude_gain_ki_.y(),
-                    &position_controller_.controller_parameters_.attitude_gain_ki_.y());
+                   position_controller_.controller_parameters_.attitude_gain_ki_.y(),
+                   &position_controller_.controller_parameters_.attitude_gain_ki_.y());
 
     GetRosParameter(pnh, "rate_gain_kp/p",
                     position_controller_.controller_parameters_.rate_gain_kp_.x(),
@@ -233,7 +233,36 @@ void PositionControllerNode::InitializeParams() {
 
     position_controller_.SetControllerGains();
 
-     ROS_INFO_ONCE("[Position Controller] Set controller gains and vehicle parameters");
+    ROS_INFO_ONCE("[Position Controller] Set controller gains and vehicle parameters");
+
+    //Reading the parameters come from the launch file
+    bool dataStoringActive;
+    double dataStoringTime;
+    std::string user;
+
+    if (pnh.getParam("user_account", user)){
+    ROS_INFO("Got param 'user_account': %s", user.c_str());
+    position_controller_.user_ = user;
+    }
+    else
+       ROS_ERROR("Failed to get param 'user'");
+
+    if (pnh.getParam("csvFilesStoring", dataStoringActive)){
+    ROS_INFO("Got param 'csvFilesStoring': %d", dataStoringActive);
+    position_controller_.dataStoring_active_ = dataStoringActive;
+    }
+    else
+       ROS_ERROR("Failed to get param 'csvFilesStoring'");
+
+    if (pnh.getParam("csvFilesStoringTime", dataStoringTime)){
+    ROS_INFO("Got param 'csvFilesStoringTime': %f", dataStoringTime);
+    position_controller_.dataStoringTime_ = dataStoringTime;
+    }
+    else
+       ROS_ERROR("Failed to get param 'csvFilesStoringTime'");
+
+    position_controller_.SetLaunchFileParameters();
+
    }
 
  if(enable_mellinger_controller_){
@@ -403,34 +432,6 @@ void PositionControllerNode::InitializeParams() {
   if (enable_state_estimator_)
     position_controller_.crazyflie_onboard_controller_.SetControllerGains(position_controller_.controller_parameters_);
 
-  //Reading the parameters come from the launch file
-  bool dataStoringActive;
-  double dataStoringTime;
-  std::string user;
-
-  if (pnh.getParam("user_account", user)){
-	  ROS_INFO("Got param 'user_account': %s", user.c_str());
-	  position_controller_.user_ = user;
-  }
-  else
-      ROS_ERROR("Failed to get param 'user'");
-
-  if (pnh.getParam("csvFilesStoring", dataStoringActive)){
-	  ROS_INFO("Got param 'csvFilesStoring': %d", dataStoringActive);
-	  position_controller_.dataStoring_active_ = dataStoringActive;
-  }
-  else
-      ROS_ERROR("Failed to get param 'csvFilesStoring'");
-
-  if (pnh.getParam("csvFilesStoringTime", dataStoringTime)){
-	  ROS_INFO("Got param 'csvFilesStoringTime': %f", dataStoringTime);
-	  position_controller_.dataStoringTime_ = dataStoringTime;
-  }
-  else
-      ROS_ERROR("Failed to get param 'csvFilesStoringTime'");
-
-  position_controller_.SetLaunchFileParameters();
-
 }
 
 void PositionControllerNode::Publish(){

diff --git a/rotors_description/CHANGELOG.rst b/rotors_description/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package rotors_description
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.0.11 (2020-04-23)
+------------------
+* Fix issue according to https://github.com/gsilano/CrazyS/issues/44
+* Changed the "measurement_divisor" (from 10 to 1) at line 895 in the component_snippets.xacro file
+* Contributors: Giuseppe Silano
+
 4.0.10 (2020-04-14)
 ------------------
 * Add ideal imu sensor for the Crazyflie

diff --git a/rotors_description/package.xml b/rotors_description/package.xml
@@ -1,6 +1,6 @@
 <package>
   <name>rotors_description</name>
-  <version>4.0.10</version>
+  <version>4.0.11</version>
   <description>The rotors_description package provides URDF models of the AscTec multicopters.</description>
 
   <maintainer email="giuseppe.silano@unisannio.it">Giuseppe Silano</maintainer>

diff --git a/rotors_description/urdf/component_snippets.xacro b/rotors_description/urdf/component_snippets.xacro
@@ -892,7 +892,7 @@
         parent_frame_id="world"
         child_frame_id="${namespace}/base_link"
         mass_odometry_sensor="0.00001"
-        measurement_divisor="10"
+        measurement_divisor="1"
         measurement_delay="0"
         unknown_delay="0.0"
         noise_normal_position="0 0 0"

diff --git a/rotors_description/urdf/crazyflie2.xacro b/rotors_description/urdf/crazyflie2.xacro
@@ -21,7 +21,7 @@
 
 <robot name="crazyflie2" xmlns:xacro="http://ros.org/wiki/xacro">
   <!-- Properties (Taken from Novel Dynamic Inversion Architecture Design for Quadrocopter Control by Jian Wang et al.) -->
-  <xacro:property name="namespace" value="$(arg mav_name)" />
+  <xacro:property name="namespace" value="$(arg namespace)" />
   <xacro:property name="rotor_velocity_slowdown_sim" value="50" />
   <xacro:property name="use_mesh_file" value="true" />
   <xacro:property name="mesh_file" value="package://rotors_description/meshes/crazyflie2.dae" />

diff --git a/rotors_description/urdf/crazyflie2_base.xacro b/rotors_description/urdf/crazyflie2_base.xacro
@@ -24,7 +24,7 @@
   <!-- Instantiate crazyflie2 "mechanics" -->
   <xacro:include filename="$(find rotors_description)/urdf/crazyflie2.xacro" />
 
-  <!-- Instantiate a controller. -->
+  <!-- Instantiate a controller.  -->
   <xacro:controller_plugin_macro namespace="${namespace}" imu_sub_topic="imu" />
 
   <!-- Enable the MAVLink interface. It is useful for the HIL (Hardware-in-the-loop) tests -->
@@ -44,7 +44,7 @@
   </xacro:if>
 
   <!-- Mount a generic odometry sensor. -->
-  <xacro:crazyflie2_odometry namespace="${namespace}" parent_link="${namespace}/base_link" />
+  <xacro:crazyflie2_odometry namespace="${namespace}" parent_link="${namespace}/base_link"  />
 
   <!-- Mount a generic odometry sensor employed as ground truth -->
   <xacro:if value="$(arg enable_ground_truth)">
@@ -56,7 +56,8 @@
     <xacro:bag_plugin_macro
       namespace="${namespace}"
       bag_file="$(arg log_file)"
-      rotor_velocity_slowdown_sim="${rotor_velocity_slowdown_sim}" />
+      rotor_velocity_slowdown_sim="${rotor_velocity_slowdown_sim}"
+      wait_to_record_bag="$(arg wait_to_record_bag)" />
   </xacro:if>
 
 </robot>
diff --git a/rotors_evaluation/CHANGELOG.rst b/rotors_evaluation/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package rotors_evaluation
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.0.11 (2020-04-23)
+------------------
+
 4.0.10 (2020-04-14)
 ------------------
 

diff --git a/rotors_evaluation/package.xml b/rotors_evaluation/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rotors_evaluation</name>
-  <version>4.0.10</version>
+  <version>4.0.11</version>
   <description>The dataset evaluation package for the RotorS simulator.</description>
 
   <maintainer email="giuseppe.silano@unisannio.it">Giuseppe Silano</maintainer>

diff --git a/rotors_gazebo/CHANGELOG.rst b/rotors_gazebo/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package rotors_gazebo
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.0.11 (2020-04-23)
+------------------
+* Add a swarm example with the Crazyflie 2.0
+* The spline trajectory generator now is a shared library
+* Add a trajectory generator when using splines for the swarm example
+* Contributors: Giuseppe Silano
+
 4.0.10 (2020-04-14)
 -------------------
 * Add INDI and Mellinger's (it does not work yet) controllers to the spaw_mav_crazyflie.launch file

diff --git a/...e/rotors_gazebo/hovering_example_spline.h → ...tors_gazebo/spline_trajectory_generator.h b/...e/rotors_gazebo/hovering_example_spline.h → ...tors_gazebo/spline_trajectory_generator.h
@@ -42,11 +42,11 @@
 
  namespace rotors_gazebo {
 
-     class HoveringExampleSpline{
+     class SplineTrajectoryGenerator{
          public:
 
-           HoveringExampleSpline();
-           ~HoveringExampleSpline();
+           SplineTrajectoryGenerator();
+           ~SplineTrajectoryGenerator();
 
            void TrajectoryCallback(mav_msgs::EigenDroneState* odometry, double* time_final, double* time_init);
            void InitializeParams();

diff --git a/rotors_gazebo/launch/crazyflie2_hovering_example.launch b/rotors_gazebo/launch/crazyflie2_hovering_example.launch
@@ -1,3 +1,4 @@
+<?xml version="1.0"?>
 <launch>
   <arg name="mav_name" default="crazyflie2"/>
   <arg name="world_name" default="basic"/>

diff --git a/rotors_gazebo/launch/crazyflie2_internal_model_controller.launch b/rotors_gazebo/launch/crazyflie2_internal_model_controller.launch
@@ -1,3 +1,4 @@
+<?xml version="1.0"?>
 <launch>
   <arg name="mav_name" default="crazyflie2"/>
   <!-- Pay attention that the world sampling time is line with the one used in the

diff --git a/rotors_gazebo/launch/crazyflie2_mellinger_controller.launch b/rotors_gazebo/launch/crazyflie2_mellinger_controller.launch
@@ -1,3 +1,4 @@
+<?xml version="1.0"?>
 <launch>
   <arg name="mav_name" default="crazyflie2"/>
   <!-- Pay attention that the world sampling time is line with the one used in the