wip from laptop

Author: Tomas Baca

CMakeLists.txt

@@ -46,6 +46,7 @@ set(DEPENDENCIES
 
 set(LIBRARIES
   MrsUavControllers_MpcController
+  MrsUavControllers_Se3Controller
   MrsUavControllers_FailsafeController
   )
 
@@ -75,6 +76,20 @@ ament_target_dependencies(MrsUavControllers_MpcController
 
 target_compile_definitions(MrsUavControllers_MpcController PRIVATE USE_ROS_TIMER=${USE_ROS_TIMER})
 
+# SE3 Controller
+
+add_library(MrsUavControllers_Se3Controller SHARED
+  src/se3_controller.cpp
+  src/common.cpp
+  )
+
+ament_target_dependencies(MrsUavControllers_Se3Controller
+  mrs_mpc_solvers
+  ${DEPENDENCIES}
+  )
+
+target_compile_definitions(MrsUavControllers_Se3Controller PRIVATE USE_ROS_TIMER=${USE_ROS_TIMER})
+
 # Failsafe Controller
 
 add_library(MrsUavControllers_FailsafeController SHARED

plugins.xml

@@ -4,6 +4,12 @@
   </class>
 </library>
 
+<library path="MrsUavControllers_Se3Controller">
+  <class type="mrs_uav_controllers::se3_controller::Se3Controller" base_class_type="mrs_uav_managers::Controller">
+    <description>Se3Controller</description>
+  </class>
+</library>
+
 <library path="MrsUavControllers_FailsafeController">
   <class type="mrs_uav_controllers::failsafe_controller::FailsafeController" base_class_type="mrs_uav_managers::Controller">
     <description>FailsafeController</description>

src/se3_controller.cpp

@@ -1,34 +1,46 @@
 /* includes //{ */
 
-#include <ros/ros.h>
-#include <ros/package.h>
+#include <rclcpp/rclcpp.hpp>
 
 #include <common.h>
 #include <pid.hpp>
 
 #include <mrs_uav_managers/controller.h>
 
-#include <dynamic_reconfigure/server.h>
-#include <mrs_uav_controllers/se3_controllerConfig.h>
-
 #include <mrs_lib/profiler.h>
 #include <mrs_lib/utils.h>
 #include <mrs_lib/mutex.h>
 #include <mrs_lib/attitude_converter.h>
 #include <mrs_lib/geometry/cyclic.h>
-#include <mrs_lib/subscribe_handler.h>
+#include <mrs_lib/subscriber_handler.h>
+
+#include <sensor_msgs/msg/imu.hpp>
 
-#include <sensor_msgs/Imu.h>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
 
-#include <geometry_msgs/Vector3Stamped.h>
+#include <ament_index_cpp/get_package_share_directory.hpp>
 
 //}
 
+/* typedefs //{ */
+
+#if USE_ROS_TIMER == 1
+typedef mrs_lib::ROSTimer TimerType;
+#else
+typedef mrs_lib::ThreadTimer TimerType;
+#endif
+
+//}
+
+/* defines //{ */
+
 #define OUTPUT_ACTUATORS 0
 #define OUTPUT_CONTROL_GROUP 1
 #define OUTPUT_ATTITUDE_RATE 2
 #define OUTPUT_ATTITUDE 3
 
+//}
+
 namespace mrs_uav_controllers
 {