migrate lqr to lqg controller implementation, fix ros packaging

lqg_controller/CMakeLists.txt

@@ -1,13 +1,13 @@
 cmake_minimum_required(VERSION 2.8.3)
 #cmake_minimum_required(VERSION 3.1.0)
-project(lqr_control)
+project(lqg_control)
 set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -Wall" )
 
 
 find_package(catkin REQUIRED COMPONENTS
   message_generation
   roscpp
-  common_msgs
+  freyja_msgs
   cmake_modules
 )
 #pkg_search_module( Eigen3 REQUIRED eigen3 )
@@ -28,7 +28,7 @@ endif()
 
 catkin_package(
   INCLUDE_DIRS include
-#  LIBRARIES lqr_control
+#  LIBRARIES lqg_control
   CATKIN_DEPENDS Eigen3 message_generation roscpp
   DEPENDS Eigen
 )
@@ -44,9 +44,9 @@ include_directories(
 )
 
 # lqr with bias estimator 
-add_executable(lqr_control_node src/lqr_control_bias.cpp src/bias_estimator.cpp)
-add_dependencies(lqr_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(lqr_control_node
+add_executable(lqg_control_node src/lqg_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}
  )
 

lqg_controller/include/bias_estimator.h

@@ -16,7 +16,7 @@
 #include <ros/ros.h>
 #include <eigen3/Eigen/Dense>
 
-#include <common_msgs/CurrentStateBiasEst.h>
+#include <freyja_msgs/CurrentStateBiasEst.h>
 
 typedef std::chrono::microseconds uSeconds;
 
@@ -67,7 +67,7 @@ class BiasEstimator
   std::chrono::duration<double> prop_interval_;
 
   /* final ros data published */
-  common_msgs::CurrentStateBiasEst state_msg_;
+  freyja_msgs::CurrentStateBiasEst state_msg_;
   public:
     BiasEstimator();
     ~BiasEstimator();

lqg_controller/include/lqr_control.h

@@ -16,19 +16,19 @@
 
 #include <ros/ros.h>
 #include <geometry_msgs/TransformStamped.h>
-#include <common_msgs/CurrentState.h>
-#include <common_msgs/CtrlCommand.h>
-#include <common_msgs/ControllerDebug.h>
-#include <common_msgs/ReferenceState.h>
+#include <freyja_msgs/CurrentState.h>
+#include <freyja_msgs/CtrlCommand.h>
+#include <freyja_msgs/ControllerDebug.h>
+#include <freyja_msgs/ReferenceState.h>
 #include <eigen3/Eigen/Dense>
 
-typedef common_msgs::ReferenceState TrajRef;
+typedef freyja_msgs::ReferenceState TrajRef;
 
 
 class LQRController
 {
   ros::NodeHandle nh_, priv_nh_;
-  common_msgs::CurrentState state_vector_;
+  freyja_msgs::CurrentState state_vector_;
   Eigen::Matrix<double, 7, 1> reduced_state_;
 
   /* Reference state vector */
@@ -60,7 +60,7 @@ class LQRController
     void initLqrSystem();
 
     ros::Subscriber state_sub_;
-    void stateCallback( const common_msgs::CurrentState::ConstPtr & );
+    void stateCallback( const freyja_msgs::CurrentState::ConstPtr & );
 
     ros::Publisher atti_cmd_pub_;
     ros::Publisher controller_debug_pub_;

lqg_controller/include/lqr_control_bias.h

@@ -18,23 +18,23 @@
 #include <geometry_msgs/TransformStamped.h>
 #include <std_srvs/SetBool.h>
 
-#include <common_msgs/CurrentState.h>
-#include <common_msgs/CtrlCommand.h>
-#include <common_msgs/ControllerDebug.h>
-#include <common_msgs/ReferenceState.h>
+#include <freyja_msgs/CurrentState.h>
+#include <freyja_msgs/CtrlCommand.h>
+#include <freyja_msgs/ControllerDebug.h>
+#include <freyja_msgs/ReferenceState.h>
 
 #include <eigen3/Eigen/Dense>
 
 #include "bias_estimator.h"
 
-typedef common_msgs::ReferenceState TrajRef;
+typedef freyja_msgs::ReferenceState TrajRef;
 typedef std_srvs::SetBool::Request BoolServReq;
 typedef std_srvs::SetBool::Response BoolServRsp;
 
 class LQRController
 {
   ros::NodeHandle nh_, priv_nh_;
-  common_msgs::CurrentState state_vector_;
+  freyja_msgs::CurrentState state_vector_;
   Eigen::Matrix<double, 7, 1> reduced_state_;
 
   /* Reference state vector */
@@ -74,7 +74,7 @@ class LQRController
     void initLqrSystem();
 
     ros::Subscriber state_sub_;
-    void stateCallback( const common_msgs::CurrentState::ConstPtr & );
+    void stateCallback( const freyja_msgs::CurrentState::ConstPtr & );
 
     ros::ServiceServer bias_enable_serv_;
     bool biasEnableServer( BoolServReq&, BoolServRsp& );

lqg_controller/include/lqr_vel_control.h

@@ -16,19 +16,19 @@
 
 #include <ros/ros.h>
 #include <geometry_msgs/TransformStamped.h>
-#include <common_msgs/CurrentState.h>
-#include <common_msgs/CtrlCommand.h>
-#include <common_msgs/ControllerDebug.h>
-#include <common_msgs/ReferenceState.h>
+#include <freyja_msgs/CurrentState.h>
+#include <freyja_msgs/CtrlCommand.h>
+#include <freyja_msgs/ControllerDebug.h>
+#include <freyja_msgs/ReferenceState.h>
 #include <eigen3/Eigen/Dense>
 
-typedef common_msgs::ReferenceState TrajRef;
+typedef freyja_msgs::ReferenceState TrajRef;
 
 
 class LQRController
 {
   ros::NodeHandle nh_, priv_nh_;
-  common_msgs::CurrentState state_vector_;
+  freyja_msgs::CurrentState state_vector_;
   Eigen::Matrix<double, 4, 1> reduced_state_;
 
   /* Reference state vector */
@@ -60,7 +60,7 @@ class LQRController
     void initLqrSystem();
 
     ros::Subscriber state_sub_;
-    void stateCallback( const common_msgs::CurrentState::ConstPtr & );
+    void stateCallback( const freyja_msgs::CurrentState::ConstPtr & );
 
     ros::Publisher atti_cmd_pub_;
     ros::Publisher controller_debug_pub_;

lqg_controller/package.xml

@@ -1,16 +1,16 @@
 <?xml version="1.0"?>
 <package>
-  <name>lqr_control</name>
+  <name>lqg_control</name>
   <version>0.1.0</version>
-  <description>The lqr_control package</description>
+  <description>The lqg_control package</description>
 
   <maintainer email="[email protected]">aj</maintainer>
-  <license>Risky</license>
+  <license>GPLv3</license>
 
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>roscpp</build_depend>
-  <build_depend>common_msgs</build_depend>
+  <build_depend>freyja_msgs</build_depend>
   <build_depend>cmake_modules</build_depend>
   <build_depend>Eigen3</build_depend>
 

lqg_controller/src/bias_estimator.cpp

@@ -21,7 +21,7 @@ BiasEstimator::BiasEstimator() : nh_(), priv_nh_("~")
   std::string output_topic;
   priv_nh_.param( "estimator_debug_topic", output_topic,
                   std::string("/bias_estimates") );
-  bias_pub_ = nh_.advertise <common_msgs::CurrentStateBiasEst>
+  bias_pub_ = nh_.advertise <freyja_msgs::CurrentStateBiasEst>
                               ( output_topic, 1, true );
 
   /* Parameters for thread */
@@ -36,7 +36,7 @@ BiasEstimator::BiasEstimator() : nh_(), priv_nh_("~")
   /* ROS timers are pretty much unreliable for "high" rates. Don't use:
     estimator_timer_ = nh_.createTimer( ros::Duration(estimator_period),
                                         &BiasEstimator::state_propagation, this );
-                                        
+  
     Use thread libraries instead:
   */
 
@@ -181,8 +181,6 @@ void BiasEstimator::setMeasurement( const Eigen::Matrix<double, 6, 1> &m )
 {
   measurement_z_ = m;
   state_updation();
-//  st_upd_thread_ = std::thread( &BiasEstimator::state_updation, this );
-//  st_upd_thread_.detach();
 }
 
 void BiasEstimator::setControlInput( const Eigen::Matrix<double, 4, 1> &c )
@@ -191,11 +189,8 @@ void BiasEstimator::setControlInput( const Eigen::Matrix<double, 4, 1> &c )
     ctrl_input_u_[0] = c[0];
     ctrl_input_u_[1] = c[1];
     ctrl_input_u_[2] = c[2] + 9.81;
-    /* smooth input control artificially for 1 time step */
-    //ctrl_input_u_ = (ctrl_input_u_ + prev_ctrl_input_)/2.0;
     n_stprops_since_update_ = 0;
   state_prop_mutex_.unlock();
-  //prev_ctrl_input_ = ctrl_input_u_;
 }
 void BiasEstimator::getEstimatedBiases( Eigen::Matrix<double, 3, 1> &eb )
 {

lqg_controller/src/lqr_control.cpp

@@ -32,9 +32,9 @@ LQRController::LQRController() : nh_(), priv_nh_("~")
                               &LQRController::trajectoryReferenceCallback, this );
 
   /* Announce publisher for controller output */
-  atti_cmd_pub_ = nh_.advertise <common_msgs::CtrlCommand>
+  atti_cmd_pub_ = nh_.advertise <freyja_msgs::CtrlCommand>
                     ( "/rpyt_command", 1, true );
-  controller_debug_pub_ = nh_.advertise <common_msgs::ControllerDebug>
+  controller_debug_pub_ = nh_.advertise <freyja_msgs::ControllerDebug>
                     ( "controller_debug", 1, true );
 
   /* Timer to run the LQR controller perdiodically */
@@ -70,7 +70,7 @@ double LQRController::calcYawError( const double &a, const double &b )
   return yd-pi; 
 }
 
-void LQRController::stateCallback( const common_msgs::CurrentState::ConstPtr &msg )
+void LQRController::stateCallback( const freyja_msgs::CurrentState::ConstPtr &msg )
 {
   /* Parse message to obtain state and reduced state information */
   //std::vector<double> sv(13);
@@ -156,7 +156,7 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
   }
 
   /* Debug information */
-  common_msgs::ControllerDebug debug_msg;
+  freyja_msgs::ControllerDebug debug_msg;
   debug_msg.header.stamp = ros::Time::now();
   debug_msg.lqr_u[0] = control_input(0);
   debug_msg.lqr_u[1] = control_input(1);
@@ -170,7 +170,7 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
   controller_debug_pub_.publish( debug_msg );
 
   /* Actual commanded input */
-  common_msgs::CtrlCommand ctrl_cmd;
+  freyja_msgs::CtrlCommand ctrl_cmd;
   ctrl_cmd.roll = roll;
   ctrl_cmd.pitch = pitch;
   ctrl_cmd.yaw = yaw;

lqg_controller/src/lqr_control_bias.cpp

@@ -8,10 +8,9 @@
    -- aj / 17th Nov, 2017.
 */
 
-#include "lqr_control_bias.h"
-//#include "bias_estimator.h"
+#include "lqg_control_bias.h"
 
-#define ROS_NODE_NAME "lqr_control"
+#define ROS_NODE_NAME "lqg_control"
 #define pi 3.1416
 
 LQRController::LQRController(BiasEstimator &b) : nh_(),
@@ -41,11 +40,11 @@ LQRController::LQRController(BiasEstimator &b) : nh_(),
   bias_enable_serv_ = nh_.advertiseService( "/set_bias_compensation",
                               &LQRController::biasEnableServer, this );
   /* Announce publisher for controller output */
-  atti_cmd_pub_ = nh_.advertise <common_msgs::CtrlCommand>
+  atti_cmd_pub_ = nh_.advertise <freyja_msgs::CtrlCommand>
                     ( "/rpyt_command", 1, true );
-  controller_debug_pub_ = nh_.advertise <common_msgs::ControllerDebug>
+  controller_debug_pub_ = nh_.advertise <freyja_msgs::ControllerDebug>
                     ( "/controller_debug", 1, true );
-                    
+
   /* Timer to run the LQR controller perdiodically */
   float controller_period = 1.0/controller_rate_;
   controller_timer_ = nh_.createTimer( ros::Duration(controller_period),
@@ -109,7 +108,7 @@ double LQRController::calcYawError( const double &a, const double &b )
   return yd-pi; 
 }
 
-void LQRController::stateCallback( const common_msgs::CurrentState::ConstPtr &msg )
+void LQRController::stateCallback( const freyja_msgs::CurrentState::ConstPtr &msg )
 {
   /* Parse message to obtain state and reduced state information */
   const double *msgptr = msg -> state_vector.data();
@@ -204,7 +203,7 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
   }
 
   /* Debug information */
-  common_msgs::ControllerDebug debug_msg;
+  freyja_msgs::ControllerDebug debug_msg;
   debug_msg.header.stamp = ros::Time::now();
   debug_msg.lqr_u[0] = control_input(0);
   debug_msg.lqr_u[1] = control_input(1);
@@ -218,7 +217,7 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
   controller_debug_pub_.publish( debug_msg );
 
   /* Actual commanded input */
-  common_msgs::CtrlCommand ctrl_cmd;
+  freyja_msgs::CtrlCommand ctrl_cmd;
   ctrl_cmd.roll = roll;
   ctrl_cmd.pitch = pitch;
   ctrl_cmd.yaw = yaw;

lqg_controller/src/lqr_vel_control.cpp

@@ -33,9 +33,9 @@ LQRController::LQRController() : nh_(), priv_nh_("~")
                               &LQRController::trajectoryReferenceCallback, this );
 
   /* Announce publisher for controller output */
-  atti_cmd_pub_ = nh_.advertise <common_msgs::CtrlCommand>
+  atti_cmd_pub_ = nh_.advertise <freyja_msgs::CtrlCommand>
                     ( "/rpyt_command", 1, true );
-  controller_debug_pub_ = nh_.advertise <common_msgs::ControllerDebug>
+  controller_debug_pub_ = nh_.advertise <freyja_msgs::ControllerDebug>
                     ( "controller_debug", 1, true );
 
   /* Timer to run the LQR controller perdiodically */
@@ -76,7 +76,7 @@ double LQRController::calcYawError( const double &a, const double &b )
   return yd-pi; 
 }
 
-void LQRController::stateCallback( const common_msgs::CurrentState::ConstPtr &msg )
+void LQRController::stateCallback( const freyja_msgs::CurrentState::ConstPtr &msg )
 {
   /* Parse message to obtain state and reduced state information */
   //std::vector<double> sv(13);
@@ -160,7 +160,7 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
   }
 
   /* Debug information */
-  common_msgs::ControllerDebug debug_msg;
+  freyja_msgs::ControllerDebug debug_msg;
   debug_msg.header.stamp = ros::Time::now();
   debug_msg.lqr_u[0] = control_input(0);
   debug_msg.lqr_u[1] = control_input(1);
@@ -174,7 +174,7 @@ void LQRController::computeFeedback( const ros::TimerEvent &event )
   controller_debug_pub_.publish( debug_msg );
 
   /* Actual commanded input */
-  common_msgs::CtrlCommand ctrl_cmd;
+  freyja_msgs::CtrlCommand ctrl_cmd;
   ctrl_cmd.roll = roll;
   ctrl_cmd.pitch = pitch;
   ctrl_cmd.yaw = yaw;