Added base template for sbus interface

interfaces/kr_sbus_interface/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.10)
+project(kr_sbus_interface)
+
+# set default build type
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE RelWithDebInfo)
+endif()
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+add_compile_options(-Wall)
+
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake/")
+
+find_package(
+  catkin REQUIRED
+  COMPONENTS roscpp
+             kr_mav_msgs
+             nav_msgs
+             nodelet
+             tf_conversions)
+find_package(Eigen3 REQUIRED)
+
+include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
+
+catkin_package(
+LIBRARIES
+${PROJECT_NAME}
+CATKIN_DEPENDS
+roscpp
+kr_mav_msgs
+nav_msgs
+nodelet
+tf_conversions
+DEPENDS
+EIGEN3)
+
+add_library(
+  ${PROJECT_NAME}
+  src/so3cmd_to_sbus_nodelet.cpp
+  src/sbus_bridge.cpp
+  src/sbus_msg.cpp
+  src/sbus_serial_port.cpp
+  src/thrust_mapping.cpp)
+add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
+install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config)
+install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

interfaces/kr_sbus_interface/cmake/FindSnav.cmake

@@ -0,0 +1,21 @@
+FIND_PATH(Snav_INCLUDE_DIR
+  NAMES
+    snav/snapdragon_navigator.h
+  PATHS
+    /usr/include
+  NO_DEFAULT_PATH
+)
+
+FIND_LIBRARY(Snav_LIBRARY
+  NAMES
+    snav_arm
+  PATHS
+    /usr/lib
+  NO_DEFAULT_PATH
+)
+
+IF(Snav_INCLUDE_DIR AND Snav_LIBRARY)
+  SET(Snav_FOUND TRUE)
+ELSE(Snav_INCLUDE_DIR AND Snav_LIBRARY)
+  SET(Snav_FOUND FALSE)
+ENDIF(Snav_INCLUDE_DIR AND Snav_LIBRARY)

interfaces/kr_sbus_interface/config/gains.yaml

@@ -0,0 +1,15 @@
+gains:
+  pos: {x: 3.4, y: 3.4, z: 5.4}
+  vel: {x: 2.8, y: 2.8, z: 3.0}
+  ki:  {x: 0.00, y: 0.00, z: 0.00, yaw: 0.01}
+  kib: {x: 0.00, y: 0.00, z: 0.00}
+  rot: {x: 1.5, y: 1.5, z: 1.0}
+  ang: {x: 0.13, y: 0.13, z: 0.1}
+
+corrections:
+  kf: 0.0e-08
+  r: 0.0
+  p: 0.0
+
+max_pos_int:   0.5
+max_pos_int_b: 0.5

interfaces/kr_sbus_interface/config/mav_manager_params.yaml

@@ -0,0 +1,6 @@
+server_wait_timeout: 0.5
+need_imu: false
+need_output_data: true
+use_attitude_safety_catch: true
+max_attitude_angle: 0.43
+takeoff_height: 0.2

interfaces/kr_sbus_interface/config/tracker_params.yaml

@@ -0,0 +1,25 @@
+# This should contain tracker parameters
+
+line_tracker_distance:
+  default_v_des: 1.0
+  default_a_des: 0.5
+  epsilon: 0.1
+
+line_tracker_min_jerk:
+  default_v_des: 1.0
+  default_a_des: 0.5
+  default_yaw_v_des: 0.8
+  default_yaw_a_des: 0.2
+
+trajectory_tracker:
+  max_vel_des: 0.5
+  max_acc_des: 0.3
+
+velocity_tracker:
+  timeout: 0.5
+
+lissajous_tracker:
+  frame_id: odom
+
+lissajous_adder:
+  frame_id: odom

interfaces/kr_sbus_interface/config/trackers.yaml

@@ -0,0 +1,11 @@
+trackers:
+  - kr_trackers/LineTrackerMinJerk
+  - kr_trackers/LineTrackerDistance
+#  - kr_trackers/LineTrackerTrapezoid
+#  - kr_trackers/LineTrackerYaw
+  - kr_trackers/VelocityTrackerAction
+  - kr_trackers/NullTracker
+  - kr_trackers/CircleTracker
+  - kr_trackers/TrajectoryTracker
+  - kr_trackers/SmoothVelTracker
+  - kr_trackers/LissajousTracker

interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h

@@ -0,0 +1,17 @@
+#pragma once
+
+namespace sbus_bridge {
+
+namespace channel_mapping {
+
+static constexpr uint8_t kThrottle = 0;
+static constexpr uint8_t kRoll = 1;
+static constexpr uint8_t kPitch = 2;
+static constexpr uint8_t kYaw = 3;
+static constexpr uint8_t kArming = 4;
+static constexpr uint8_t kControlMode = 5;
+static constexpr uint8_t kGamepadMode = 6;
+
+}  // namespace channel_mapping
+
+}  // namespace sbus_bridge

interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h

@@ -0,0 +1,136 @@
+#pragma once
+
+#include <atomic>
+#include <mutex>
+#include <thread>
+
+#include <ros/ros.h>
+#include <kr_sbus_interface/sbus_serial_port.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Float32.h>
+#include <kr_mav_msgs/SO3Command.h>
+
+#include <kr_sbus_interface/sbus_msg.h>
+#include <kr_sbus_interface/thrust_mapping.h>
+
+#include <Eigen/Geometry>
+
+namespace sbus_bridge {
+
+enum class BridgeState { OFF, ARMING, AUTONOMOUS_FLIGHT, RC_FLIGHT };
+
+class SBusBridge : public SBusSerialPort {
+ public:
+  SBusBridge(const ros::NodeHandle& nh, const ros::NodeHandle& pnh);
+
+  SBusBridge() : SBusBridge(ros::NodeHandle(), ros::NodeHandle("~")) {}
+
+  void controlCommandCallback(const kr_mav_msgs::SO3Command::ConstPtr& msg, const Eigen::Quaterniond& odom_q);
+
+  virtual ~SBusBridge();
+
+ private:
+  void watchdogThread();
+
+  void handleReceivedSbusMessage(const SBusMsg& received_sbus_msg) override;
+
+  SBusMsg generateSBusMessageFromSO3Command(const kr_mav_msgs::SO3Command::ConstPtr& control_command, const Eigen::Quaterniond& odom_q) const;
+
+  void sendSBusMessageToSerialPort(const SBusMsg& sbus_msg);
+
+  void setBridgeState(const BridgeState& desired_bridge_state);
+
+  void armBridgeCallback(const std_msgs::Bool::ConstPtr& msg);
+  void batteryVoltageCallback(const std_msgs::Float32::ConstPtr& msg);
+  void publishLowLevelFeedback(const ros::TimerEvent& time) const;
+
+  bool loadParameters();
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_;
+
+  // Mutex for:
+  // - bridge_state_
+  // - control_mode_
+  // - bridge_armed_
+  // - time_last_active_control_command_received_
+  // - time_last_rc_msg_received_
+  // - arming_counter_
+  // - time_last_sbus_msg_sent_
+  // Also "setBridgeState" and "sendSBusMessageToSerialPort" should only be
+  // called when "main_mutex_" is locked
+  mutable std::mutex main_mutex_;
+  // Mutex for:
+  // - battery_voltage_
+  // - time_last_battery_voltage_received_
+  // Also "generateSBusMessageFromControlCommand" should only be called when
+  // "battery_voltage_mutex_" is locked
+  mutable std::mutex battery_voltage_mutex_;
+
+  // Publishers
+  ros::Publisher low_level_feedback_pub_;
+  ros::Publisher received_sbus_msg_pub_;
+
+  // Subscribers
+  ros::Subscriber control_command_sub_;
+  ros::Subscriber arm_bridge_sub_;
+  ros::Subscriber battery_voltage_sub_;
+
+  // Timer
+  ros::Timer low_level_feedback_pub_timer_;
+
+  // Watchdog
+  std::thread watchdog_thread_;
+  std::atomic_bool stop_watchdog_thread_;
+  ros::Time time_last_rc_msg_received_;
+  ros::Time time_last_sbus_msg_sent_;
+  ros::Time time_last_battery_voltage_received_;
+  ros::Time time_last_active_control_command_received_;
+
+  BridgeState bridge_state_;
+  ControlMode control_mode_;
+  int arming_counter_;
+  double battery_voltage_;
+
+  // Safety flags
+  bool bridge_armed_;
+  bool rc_was_disarmed_once_;
+
+  std::atomic_bool destructor_invoked_;
+
+  thrust_mapping::CollectiveThrustMapping thrust_mapping_;
+
+  // Parameters
+  std::string port_name_;
+  bool enable_receiving_sbus_messages_;
+
+  double control_command_timeout_;
+  double rc_timeout_;
+
+  double mass_;
+
+  bool disable_thrust_mapping_;
+
+  double max_roll_rate_;
+  double max_pitch_rate_;
+  double max_yaw_rate_;
+
+  double max_roll_angle_;
+  double max_pitch_angle_;
+
+  double alpha_vbat_filter_;
+  bool perform_thrust_voltage_compensation_;
+  int n_lipo_cells_;
+
+  // Constants
+  static constexpr double kLowLevelFeedbackPublishFrequency_ = 50.0;
+
+  static constexpr int kSmoothingFailRepetitions_ = 5;
+
+  static constexpr double kBatteryLowVoltagePerCell_ = 3.6;
+  static constexpr double kBatteryCriticalVoltagePerCell_ = 3.4;
+  static constexpr double kBatteryInvalidVoltagePerCell_ = 3.0;
+  static constexpr double kBatteryVoltageTimeout_ = 1.0;
+};
+
+}  // namespace sbus_bridge

interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h

@@ -0,0 +1,62 @@
+#pragma once
+
+#include <stdint.h>
+
+// #include "kr_sbus_interface/SbusRosMessage.h"
+# include <ros/ros.h>
+
+namespace sbus_bridge {
+
+enum class ControlMode { NONE, ATTITUDE, BODY_RATES };
+
+enum class ArmState { DISARMED, ARMED };
+
+#pragma pack(push)
+#pragma pack(1)
+struct SBusMsg {
+  // Constants
+  static constexpr int kNChannels = 16;
+  static constexpr uint16_t kMinCmd = 192;   // corresponds to 1000 on FC
+  static constexpr uint16_t kMeanCmd = 992;  // corresponds to 1500 on FC
+  static constexpr uint16_t kMaxCmd = 1792;  // corresponds to 2000 on FC
+
+  ros::Time timestamp;
+
+  // Normal 11 bit channels
+  uint16_t channels[kNChannels];
+
+  // Digital channels (ch17 and ch18)
+  bool digital_channel_1;
+  bool digital_channel_2;
+
+  // Flags
+  bool frame_lost;
+  bool failsafe;
+
+  SBusMsg();
+//   SBusMsg(const sbus_bridge::SbusRosMessage& sbus_ros_msg);
+  virtual ~SBusMsg();
+
+//   sbus_bridge::SbusRosMessage toRosMessage() const;
+
+  void limitAllChannelsFeasible();
+  void limitSbusChannelFeasible(const int channel_idx);
+
+  // Setting sbus command helpers
+  void setThrottleCommand(const uint16_t throttle_cmd);
+  void setRollCommand(const uint16_t roll_cmd);
+  void setPitchCommand(const uint16_t pitch_cmd);
+  void setYawCommand(const uint16_t yaw_cmd);
+  void setControlMode(const ControlMode& control_mode);
+  void setControlModeAttitude();
+  void setControlModeBodyRates();
+  void setArmState(const ArmState& arm_state);
+  void setArmStateArmed();
+  void setArmStateDisarmed();
+
+  // Sbus message check helpers
+  bool isArmed() const;
+  ControlMode getControlMode() const;
+};
+#pragma pack(pop)
+}  // namespace sbus_bridge