Port controller and state_manager to ROS2, build passing

Author: ajshank

lqg_controller/CMakeLists.txt

@@ -1,58 +1,52 @@
-cmake_minimum_required(VERSION 2.8.3)
-#cmake_minimum_required(VERSION 3.1.0)
+cmake_minimum_required(VERSION 3.8)
 project(lqg_control)
-set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3 -ffast-math -Wall" )
+add_compile_options( -std=c++14 -O3 -ffast-math -Wall )
 
 
-find_package(catkin REQUIRED COMPONENTS
-  message_generation
-  roscpp
-  freyja_msgs
-  cmake_modules
-)
+find_package(ament_cmake REQUIRED)
 #pkg_search_module( Eigen3 REQUIRED eigen3 )
-find_package( Eigen REQUIRED )
+#find_package( Eigen REQUIRED )
+
+
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(freyja_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
 include_directories(${EIGEN3_INCLUDE_DIR})
 add_definitions( ${EIGEN3_DEFINITIONS} )
 
-if( NOT EIGEN3_FOUND )
-  # resort to cmake_modules
-  find_package( cmake_modules REQUIRED )
-  find_package(Eigen REQUIRED)
-  set( EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS} )
-  set( EIGEN3_LIBRARIES ${EIGEN_LIBRARIES} )
-else()
-  set( EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR} )
-endif()
-
-
-catkin_package(
-  INCLUDE_DIRS include
-#  LIBRARIES lqg_control
-  CATKIN_DEPENDS Eigen3 message_generation roscpp
-  DEPENDS Eigen
-)
-
 ###########
 ## Build ##
 ###########
 
 include_directories(include)
 include_directories(
-  ${catkin_INCLUDE_DIRS}
+#  ${catkin_INCLUDE_DIRS}
   ${Eigen_INCLUDE_DIRS}
 )
 
 # lqr with bias estimator 
 add_executable(lqg_control_node src/lqr_control_bias.cpp src/bias_estimator.cpp)
-add_dependencies(lqg_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(lqg_control_node
-   ${catkin_LIBRARIES}
- )
+target_include_directories( lqg_control_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies( lqg_control_node 
+  rclcpp
+  std_msgs
+  freyja_msgs
+  geometry_msgs
+  std_srvs
+)
+
+install(TARGETS
+  lqg_control_node
+  DESTINATION lib/${PROJECT_NAME})
 
 # lqr with only velocity control 
-add_executable(lqr_vel_ctrl_node src/lqr_vel_control.cpp)
-add_dependencies(lqr_vel_ctrl_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(lqr_vel_ctrl_node
-   ${catkin_LIBRARIES}
- )
+#add_executable(lqr_vel_ctrl_node src/lqr_vel_control.cpp)
+#add_dependencies(lqr_vel_ctrl_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+ament_package()

lqg_controller/include/bias_estimator.h

@@ -12,25 +12,28 @@
 #include <mutex>
 #include <thread>
 #include <chrono>
+#include <memory>
 
-#include <ros/ros.h>
+#include "rclcpp/rclcpp.hpp"
 #include <eigen3/Eigen/Dense>
 
-#include <freyja_msgs/CurrentStateBiasEst.h>
+#include "freyja_msgs/msg/current_state_bias_est.hpp"
 
 typedef std::chrono::microseconds uSeconds;
 typedef std::chrono::high_resolution_clock ClockTime;
 typedef std::chrono::time_point<ClockTime> ClockTimePoint;
 
+typedef freyja_msgs::msg::CurrentStateBiasEst EstimatedState;
+
 const int nStates = 9;
 const int nCtrl = 3;
 const int nMeas = 6;
 
-class BiasEstimator
+class BiasEstimator : public rclcpp::Node
 {
   friend class LQRController;
 
-  ros::NodeHandle nh_, priv_nh_;
+  //ros::NodeHandle nh_, priv_nh_;
   Eigen::Matrix<double, nStates, nStates> sys_A_, sys_A_t_;
   Eigen::Matrix<double, nStates, nCtrl> sys_B_;
 
@@ -75,12 +78,12 @@ class BiasEstimator
   std::chrono::duration<double> tc_interval_;
 
   /* final ros data published */
-  freyja_msgs::CurrentStateBiasEst state_msg_;
+  EstimatedState state_msg_;
   public:
     BiasEstimator();
     ~BiasEstimator();
 
-    ros::Publisher bias_pub_;
+    rclcpp::Publisher<EstimatedState>::SharedPtr bias_pub_;
 
     /* State propagation - runs at a fixed rate */
     __attribute__((optimize("unroll-loops")))

lqg_controller/include/lqr_control_bias.h

@@ -13,30 +13,38 @@
    -- aj / 17th Nov, 2017.
 */
 #include <mutex>
+#include <memory>
+#include <chrono>
 
-#include <ros/ros.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <std_srvs/SetBool.h>
-#include <std_msgs/Float32.h>
+#include "rclcpp/rclcpp.hpp"
+//#include "geometry_msgs/msg/TransformStamped.hpp"
+#include "std_srvs/srv/set_bool.hpp"
+#include "std_msgs/msg/float32.hpp"
 
-#include <freyja_msgs/CurrentState.h>
-#include <freyja_msgs/CtrlCommand.h>
-#include <freyja_msgs/ControllerDebug.h>
-#include <freyja_msgs/ReferenceState.h>
+#include "freyja_msgs/msg/current_state.hpp"
+#include "freyja_msgs/msg/ctrl_command.hpp"
+#include <freyja_msgs/msg/controller_debug.hpp>
+#include <freyja_msgs/msg/reference_state.hpp>
 
 #include <eigen3/Eigen/Dense>
 
 #include "bias_estimator.h"
 
-typedef freyja_msgs::ReferenceState TrajRef;
-typedef std_srvs::SetBool::Request BoolServReq;
-typedef std_srvs::SetBool::Response BoolServRsp;
+typedef freyja_msgs::msg::ReferenceState TrajRef;
+typedef freyja_msgs::msg::CurrentState CurrentState;
+typedef std_srvs::srv::SetBool BoolServ;
+typedef freyja_msgs::msg::CtrlCommand RPYT_Command;
+typedef freyja_msgs::msg::ControllerDebug CTRL_Debug;
+
 typedef Eigen::Matrix<double, 6, 1> PosVelNED;
+typedef Eigen::Matrix<double, 4, 1> Vector4d;
+
+using std::placeholders::_1;
+using std::placeholders::_2;
 
-class LQRController
+class LQRController : public rclcpp::Node
 {
-  ros::NodeHandle nh_, priv_nh_;
-  freyja_msgs::CurrentState state_vector_;
+  CurrentState state_vector_;
   Eigen::Matrix<double, 7, 1> reduced_state_;
 
   /* Reference state vector */
@@ -61,7 +69,7 @@ class LQRController
   Eigen::Matrix<double, 3, 3> rot_yaw_;
 
   float STATEFB_MISSING_INTRV_;
-  ros::Time last_state_update_t_;
+  rclcpp::Time last_state_update_t_;
   bool have_state_update_;
   bool have_reference_update_;
 
@@ -82,25 +90,26 @@ class LQRController
     LQRController( BiasEstimator & );
     void initLqrSystem();
 
-    ros::Subscriber state_sub_;
-    void stateCallback( const freyja_msgs::CurrentState::ConstPtr & ) __attribute__((hot));
+    rclcpp::Subscription<CurrentState>::SharedPtr state_sub_;
+    void stateCallback( const CurrentState::ConstSharedPtr ) __attribute__((hot));
 
-    ros::ServiceServer bias_enable_serv_;
-    bool biasEnableServer( BoolServReq&, BoolServRsp& );
+    rclcpp::Service<BoolServ>::SharedPtr bias_enable_serv_;
+    void biasEnableServer( const BoolServ::Request::SharedPtr,
+                           const BoolServ::Response::SharedPtr );
 
-    ros::Publisher atti_cmd_pub_;
-    ros::Publisher controller_debug_pub_;
+    rclcpp::Publisher<RPYT_Command>::SharedPtr atti_cmd_pub_;
+    rclcpp::Publisher<CTRL_Debug>::SharedPtr controller_debug_pub_;
 
-    ros::Timer controller_timer_;
-    void computeFeedback( const ros::TimerEvent & ) __attribute__((optimize("unroll-loops")));
+    rclcpp::TimerBase::SharedPtr controller_timer_;
+    void computeFeedback( ) __attribute__((optimize("unroll-loops")));
 
-    ros::Subscriber reference_sub_;
-    void trajectoryReferenceCallback( const TrajRef::ConstPtr & );
+    rclcpp::Subscription<TrajRef>::SharedPtr reference_sub_;
+    void trajectoryReferenceCallback( const TrajRef::ConstSharedPtr );
 
     /* helper function to calculate yaw error */
     static constexpr inline double calcYawError( const double&, const double& ) __attribute__((always_inline));
 
     /* estimate actual mass in flight */
-    void estimateMass( const Eigen::Matrix<double, 4, 1> &, ros::Time & );
-    ros::Publisher est_mass_pub_;
+    void estimateMass( const Eigen::Matrix<double, 4, 1> &, rclcpp::Time & );
+    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr est_mass_pub_;
 };

lqg_controller/package.xml

@@ -1,22 +1,24 @@
 <?xml version="1.0"?>
-<package>
+<package format="3">
   <name>lqg_control</name>
-  <version>0.1.0</version>
+  <version>2.0.0</version>
   <description>The lqg_control package</description>
 
   <maintainer email="ashankar@cse.unl.edu">aj</maintainer>
   <license>GPLv3</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>message_generation</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>freyja_msgs</build_depend>
-  <build_depend>cmake_modules</build_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>freyja_msgs</depend>
+  
   <build_depend>Eigen3</build_depend>
 
-  <run_depend>roscpp</run_depend>
-  <run_depend>message_runtime</run_depend>
-  <run_depend>message_generation</run_depend>
-  <run_depend>Eigen3</run_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 
 </package>

lqg_controller/src/bias_estimator.cpp

@@ -15,18 +15,18 @@
 #define ZERO3x3 Eigen::MatrixXd::Zero(3,3)
 #define IDEN3x3 Eigen::MatrixXd::Identity(3,3)
 
-BiasEstimator::BiasEstimator() : nh_(), priv_nh_("~")
+BiasEstimator::BiasEstimator() : Node( ROS_NODE_NAME )
 {
+  
+  declare_parameter( "estimator_rate", int(10) );
+  
+  
   /* Used for debug and such */
-  std::string output_topic;
-  priv_nh_.param( "estimator_debug_topic", output_topic,
-                  std::string("/bias_estimates") );
-  bias_pub_ = nh_.advertise <freyja_msgs::CurrentStateBiasEst>
-                              ( output_topic, 1, true );
+  bias_pub_ = create_publisher <EstimatedState>
+                              ( "/bias_estimates", 1 );
 
   /* Parameters for thread */
-  int estimator_rate_default = 10;
-  priv_nh_.param( "estimator_rate", estimator_rate_, estimator_rate_default );
+  get_parameter( "estimator_rate", estimator_rate_ );
   estimator_period_us_ = std::round( (1.0/estimator_rate_)*1000*1000 );
 
   /* Time constant  factor */
@@ -110,7 +110,7 @@ void BiasEstimator::state_propagation( )
   input_shaping << 0.95, 0.95, 0.95;
   Eigen::Matrix<double, 3, 1> BIAS_LIM_ABS;   // clip limits (acceleration)
   BIAS_LIM_ABS << 2.0, 2.0, 3.0;
-  while( ros::ok() )
+  while( rclcpp::ok() )
   {
     /* Mark start(ts) and end(te) times for precise timing */
     ts = std::chrono::high_resolution_clock::now();
@@ -154,7 +154,7 @@ void BiasEstimator::state_propagation( )
       spmtx.unlock();
 
       // publish
-      state_msg_.header.stamp = ros::Time::now();
+      state_msg_.header.stamp = now();
 
       last_prop_t_ = te = std::chrono::high_resolution_clock::now();
       int dt = std::chrono::duration_cast<uSeconds>(te-ts).count();
@@ -164,7 +164,7 @@ void BiasEstimator::state_propagation( )
       state_msg_.state_vector[10] = delta_t;
       //state_msg_.state_vector[10] = state_cov_P_.diagonal().norm();
       state_msg_.state_vector[11] = ctrl_input_u_.norm();
-      bias_pub_.publish( state_msg_ );
+      bias_pub_ -> publish( state_msg_ );
 
       std::this_thread::sleep_for( uSeconds(estimator_period_us_ - dt) );
     }