Add a swarm example with the Crazyflie #44

gsilano · Apr 23, 2020 · 91499e2 · 91499e2
1 parent 9251f8c
commit 91499e2
Show file tree

Hide file tree

Showing 38 changed files with 595 additions and 166 deletions.
diff --git a/rotors_comm/CHANGELOG.rst b/rotors_comm/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package rotors_comm
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.5 (2020-04-23)
+------------------
+
 6.0.4 (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>6.0.4</version>
+  <version>6.0.5</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
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.4 (2020-04-23)
+------------------
+* Contributors: fix missing input error when running the control algorithm
+* Contributors: Giuseppe Silano
+
 6.0.4 (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>6.0.4</version>
+  <version>6.0.5</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
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.5 (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
+
 6.0.4 (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>6.0.4</version>
+  <version>6.0.5</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
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.5 (2020-04-23)
+------------------
+
 6.0.4 (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>6.0.4</version>
+  <version>6.0.5</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
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.5 (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
+
 6.0.4 (2020-04-14)
 ------------------
 * Add INDI and Mellinger's (it does not work yet) controllers to the spaw_mav_crazyflie.launch file

diff --git a/rotors_gazebo/CMakeLists.txt b/rotors_gazebo/CMakeLists.txt
@@ -60,6 +60,7 @@ find_package(
 
 catkin_package(
   INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS}
+  LIBRARIES spline_trajectory_generator
   CATKIN_DEPENDS geometry_msgs mav_msgs nav_msgs roscpp sensor_msgs gazebo_msgs
   DEPENDS Eigen
 )
@@ -70,23 +71,31 @@ include_directories(
   ${Eigen_INCLUDE_DIRS}
 )
 
-add_executable(waypoint_publisher src/waypoint_publisher.cpp)
+add_library(spline_trajectory_generator
+  src/library/spline_trajectory_generator.cpp
+)
+
+add_executable(waypoint_publisher src/nodes/waypoint_publisher.cpp)
 target_link_libraries(waypoint_publisher ${catkin_LIBRARIES})
 add_dependencies(waypoint_publisher ${catkin_EXPORTED_TARGETS})
 
-add_executable(waypoint_publisher_file src/waypoint_publisher_file.cpp)
+add_executable(waypoint_publisher_file src/nodes/waypoint_publisher_file.cpp)
 target_link_libraries(waypoint_publisher_file ${catkin_LIBRARIES})
 add_dependencies(waypoint_publisher_file ${catkin_EXPORTED_TARGETS})
 
-add_executable(hovering_example src/hovering_example.cpp)
+add_executable(hovering_example src/nodes/hovering_example.cpp)
 target_link_libraries(hovering_example ${catkin_LIBRARIES})
 add_dependencies(hovering_example ${catkin_EXPORTED_TARGETS})
 
-add_executable(hovering_example_spline src/hovering_example_spline.cpp)
-target_link_libraries(hovering_example_spline ${catkin_LIBRARIES})
+add_executable(hovering_example_spline src/nodes/hovering_example_spline.cpp)
+target_link_libraries(hovering_example_spline spline_trajectory_generator ${catkin_LIBRARIES})
 add_dependencies(hovering_example_spline ${catkin_EXPORTED_TARGETS})
 
-add_executable(quaternion_to_rpy src/quaternion_to_rpy.cpp)
+add_executable(hovering_example_spline_swarm src/nodes/hovering_example_spline_swarm.cpp)
+target_link_libraries(hovering_example_spline_swarm spline_trajectory_generator ${catkin_LIBRARIES})
+add_dependencies(hovering_example_spline_swarm ${catkin_EXPORTED_TARGETS})
+
+add_executable(quaternion_to_rpy src/nodes/quaternion_to_rpy.cpp)
 target_link_libraries(quaternion_to_rpy ${catkin_LIBRARIES})
 add_dependencies(quaternion_to_rpy ${catkin_EXPORTED_TARGETS})
 
@@ -95,14 +104,15 @@ foreach(dir launch models resource worlds)
       DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
 endforeach(dir)
 
-install(TARGETS waypoint_publisher waypoint_publisher_file hovering_example quaternion_to_rpy hovering_example_spline
+install(TARGETS waypoint_publisher waypoint_publisher_file hovering_example
+  quaternion_to_rpy hovering_example_spline hovering_example_spline_swarm
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
 install(
-  DIRECTORY include/${PROJECT_NAME}/ 
+  DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   FILES_MATCHING PATTERN "*.h"
 )
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