3DOF vel/accel ctrl for holonomic 4w-mecanum drive robots

- linearised 2nd order system with non-linear wheel-speeds
  - symmetric parallel wheels with 45deg free rollers
- support for selecting 1st order control
- msg format for wheel speeds
- basic handler for robomaster interfaces

Author: ajshank

autopilot_handler/robomaster_handler/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.8)
+project(robomaster_handler)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++14)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(freyja_msgs REQUIRED)
+find_package(robomaster_interfaces REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+ 
+
+# handler node
+add_executable( s1_interface src/s1_handler.cpp )
+
+ament_target_dependencies( s1_interface 
+  rclcpp
+  std_msgs
+  freyja_msgs
+  robomaster_interfaces
+)
+
+install(TARGETS
+  s1_interface
+  DESTINATION lib/${PROJECT_NAME})
+
+
+ament_package()
+

autopilot_handler/robomaster_handler/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>robomaster_handler</name>
+  <version>2.0.0</version>
+  <description>Freyja's robomaster translation package</description>
+  <maintainer email="ashankar@cse.unl.edu">aj</maintainer>
+  <license>GPLv3</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>freyja_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>robomaster_interfaces</depend>
+
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

autopilot_handler/robomaster_handler/src/s1_handler.cpp

@@ -0,0 +1,55 @@
+#include "rclcpp/rclcpp.hpp"
+
+#include "robomaster_interfaces/msg/wheel_speed.hpp"
+#include <freyja_msgs/msg/wheel_command.hpp>
+
+typedef freyja_msgs::msg::WheelCommand WheelCommand;          // rad/s from controller
+typedef robomaster_interfaces::msg::WheelSpeed WheelSpeed;    // rpm or normalised to vehicle
+
+using std::placeholders::_1;
+
+class S1Handler : public rclcpp::Node
+{
+  WheelSpeed target_wheel_rpm_;
+
+  public:
+    S1Handler();
+
+    rclcpp::Subscription<WheelCommand>::SharedPtr speed_cmd_sub_;
+    void wheelSpeedCallback( const WheelCommand::ConstSharedPtr );
+
+    rclcpp::Publisher<WheelSpeed>::SharedPtr speed_rpm_pub_;
+
+};
+
+S1Handler::S1Handler() : rclcpp::Node( "s1_handler" )
+{
+  // associate callback for controller's commands
+  speed_cmd_sub_ = create_subscription<WheelCommand> ( "controller_command", 1,
+                                std::bind(&S1Handler::wheelSpeedCallback, this, _1) );
+
+  // declare publisher for sending out RPM commands
+  speed_rpm_pub_ = create_publisher<WheelSpeed> ( "target_rpm", 1 );
+}
+
+void S1Handler::wheelSpeedCallback( const WheelCommand::ConstSharedPtr msg )
+{
+  /* The controller outputs target wheel speeds in rad/s. We must convert them into
+    RPMs in the range [-1000, 1000] (int16_t) for DJI
+  */
+  target_wheel_rpm_.fl = std::round( msg -> w1 );
+  target_wheel_rpm_.fr = std::round( msg -> w2 );
+  target_wheel_rpm_.rr = std::round( msg -> w3 );
+  target_wheel_rpm_.rl = std::round( msg -> w4 );
+
+  speed_rpm_pub_ -> publish( target_wheel_rpm_ );
+}
+
+
+int main( int argc, char** argv )
+{
+  rclcpp::init( argc, argv );
+  rclcpp::spin( std::make_shared<S1Handler>() );
+  rclcpp::shutdown();
+  return 0;
+}

freyja_msgs/CMakeLists.txt

@@ -6,6 +6,9 @@ add_compile_options(-std=c++14)
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
 
 find_package(rosidl_default_generators REQUIRED)
 
@@ -19,9 +22,18 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/trajectory_provider/WaypointTarget.msg"
   "msg/lqr_ctrl/CtrlCommand.msg"
   "msg/lqr_ctrl/ControllerDebug.msg"
+  "msg/lqr_ctrl/WheelCommand.msg"
   "msg/state_manager/CurrentState.msg"
   "msg/state_manager/CurrentStateBiasEst.msg"
+  DEPENDENCIES std_msgs geometry_msgs builtin_interfaces
  )
 
+#  install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION include/${PROJECT_NAME}
+# )
+
+ament_export_dependencies(rosidl_default_runtime)
+
+
 ament_package()
 

freyja_msgs/msg/lqr_ctrl/CtrlCommand copy.msg

@@ -0,0 +1,9 @@
+# Control commands for the wheel speed controller.
+
+# ctrl_mode is:
+#    unused
+float64 w1
+float64 w2
+float64 w3
+float64 w4
+uint8 ctrl_mode

freyja_msgs/msg/lqr_ctrl/WheelCommand.msg

@@ -0,0 +1,10 @@
+# Control commands (rad/s) for the wheel speed controller.
+## Wheel numbering is clockwise from front-left.
+
+# ctrl_mode is:
+#    unused
+float64 w1
+float64 w2
+float64 w3
+float64 w4
+uint8 ctrl_mode

freyja_msgs/package.xml

@@ -7,6 +7,10 @@
   <license>GPLv3</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>builtin_interfaces</depend>
 
   <build_depend>rosidl_default_generators</build_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>

lqg_controller/include/lqr_control_bias.h

@@ -22,7 +22,7 @@
 #include "std_msgs/msg/float32.hpp"
 
 #include "freyja_msgs/msg/current_state.hpp"
-#include "freyja_msgs/msg/ctrl_command.hpp"
+#include "freyja_msgs/msg/wheel_command.hpp"
 #include <freyja_msgs/msg/controller_debug.hpp>
 #include <freyja_msgs/msg/reference_state.hpp>
 
@@ -33,7 +33,7 @@
 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::WheelCommand Wheel_Command;
 typedef freyja_msgs::msg::ControllerDebug CTRL_Debug;
 
 typedef Eigen::Matrix<double, 6, 1> PosVelNED;
@@ -45,11 +45,11 @@ using std::placeholders::_2;
 class LQRController : public rclcpp::Node
 {
   CurrentState state_vector_;
-  Eigen::Matrix<double, 7, 1> reduced_state_;
+  Eigen::Matrix<double, 6, 1> reduced_state_;
 
   /* Reference state vector */
-  Eigen::Matrix<double, 7, 1> reference_state_;
-  Eigen::Matrix<double, 4, 1> reference_ff_;
+  Eigen::Matrix<double, 6, 1> reference_state_;
+  Eigen::Matrix<double, 3, 1> reference_ff_;
   bool enable_flatness_ff_;
 
   /* Rate of execution for LQR's feedback */
@@ -58,8 +58,11 @@ class LQRController : public rclcpp::Node
   /* System matrices */
   Eigen::MatrixXf sys_A_, sys_B_;
   Eigen::MatrixXf lqr_Q_, lqr_R_;
-  Eigen::Matrix<double, 4, 7> lqr_K_;
+  Eigen::Matrix<double, 3, 6> lqr_K_;
+  Eigen::Matrix<double, 4, 3> wheel_geometry_matrix_;
+  double wheel_rad_;
   bool use_stricter_gains_;
+  std::string controller_type_;
 
   /* Vehicle properties */
   float total_mass_;
@@ -97,7 +100,7 @@ class LQRController : public rclcpp::Node
     void biasEnableServer( const BoolServ::Request::SharedPtr,
                            const BoolServ::Response::SharedPtr );
 
-    rclcpp::Publisher<RPYT_Command>::SharedPtr atti_cmd_pub_;
+    rclcpp::Publisher<Wheel_Command>::SharedPtr wheel_cmd_pub_;
     rclcpp::Publisher<CTRL_Debug>::SharedPtr controller_debug_pub_;
 
     rclcpp::TimerBase::SharedPtr controller_timer_;
@@ -110,6 +113,6 @@ class LQRController : public rclcpp::Node
     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> &, rclcpp::Time & );
+    void estimateMass( const double &, rclcpp::Time & );
     rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr est_mass_pub_;
 };

lqg_controller/src/bias_estimator.cpp

@@ -23,7 +23,7 @@ BiasEstimator::BiasEstimator() : Node( ROS_NODE_NAME )
 
   /* Used for debug and such */
   bias_pub_ = create_publisher <EstimatedState>
-                              ( "/bias_estimates", 1 );
+                              ( "bias_estimates", 1 );
 
   /* Parameters for thread */
   get_parameter( "estimator_rate", estimator_rate_ );