add command_subscriber.cpp/.hpp

airsim_ros_pkgs/CMakeLists.txt

@@ -17,13 +17,17 @@ set(MSG_DEPENDENCIES
   geometry_msgs
   geographic_msgs
   nav_msgs
+  mavros_msgs
   sensor_msgs
   std_msgs)
-find_package(catkin REQUIRED COMPONENTS
-  ${MSG_DEPENDENCIES}
+set(COMPONENT_DEPENDENCIES
   message_generation
   roscpp
-  rospy)
+  rospy
+  tf2_ros)
+find_package(catkin REQUIRED COMPONENTS
+  ${MSG_DEPENDENCIES}
+  ${COMPONENT_DEPENDENCIES})
 
 # Custom messages and services.
 file(GLOB MESSAGE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/msg "msg/*.msg")
@@ -34,8 +38,7 @@ generate_messages(DEPENDENCIES ${MSG_DEPENDENCIES})
 
 catkin_package(
   INCLUDE_DIRS include
-  CATKIN_DEPENDS message_runtime roscpp std_msgs # nodelet
-)
+  CATKIN_DEPENDS message_runtime ${MSG_DEPENDENCIES} ${COMPONENT_DEPENDENCIES})
 
 message(STATUS "Eigen3 include dir: ${AIRSIM_ROOT}/AirLib/deps/eigen3")
 
@@ -53,7 +56,10 @@ target_link_libraries(airsim_settings_parser
   ${EIGEN3_LIBRARIES}
   AirLib)
 
-add_library(airsim_ros src/airsim_ros_wrapper.cpp)
+set(ARISIM_SOURCES
+  src/airsim_ros_wrapper.cpp
+  src/command_subscriber.cpp)
+add_library(airsim_ros ${ARISIM_SOURCES})
 add_dependencies(airsim_ros
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
   ${catkin_EXPORTED_TARGETS})

airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp

@@ -14,19 +14,77 @@
 
 // ROS headers
 #include "ros/ros.h"
+#include "tf2_ros/static_transform_broadcaster.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
 
+#include "airsim_ros_pkgs/command_subscriber.hpp"
 #include "airsim_ros_pkgs/messages.hpp"
 #include "airsim_ros_pkgs/robot_ros.hpp"
+#include "airsim_ros_pkgs/utils.hpp"
 
 namespace airsim_ros {
 namespace airsim_ros_wrapper {
 
+namespace frame_ids {
+constexpr char kAirsimFrameId[] = "world_ned";
+constexpr char kAirsimOdomFrameId[] = "odom_local_ned";
+constexpr char kEnuOdomFrameId[] = "odom_local_enu";
+}  // namespace frame_ids
+
 struct Config {
+  bool is_vulkan;
+  bool publish_clock;
   std::string host_ip;
   ros::NodeHandle nh;
   ros::NodeHandle nh_private;
 };
 
+struct TF {
+  bool is_enu = false;
+  std::string world_frame_id = frame_ids::kAirsimFrameId;
+  std::string odom_frame_id = frame_ids::kAirsimOdomFrameId;
+  tf2_ros::TransformBroadcaster tf_broadcaster_;
+  tf2_ros::StaticTransformBroadcaster static_tf_pub_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_ = tf_buffer_;
+};
+
+class AirsimClient {
+ public:
+  AirsimClient() = delete;
+  AirsimClient(const AirsimClient &) = delete;
+  AirsimClient &operator=(const AirsimClient &) = delete;
+  AirsimClient(AirsimClient &&) = delete;
+  AirsimClient &operator=(AirsimClient &&) = delete;
+
+  AirsimClient(const std::string &host_ip) {
+    try {
+      airsim_client_robot_ =
+          std::make_unique<msr::airlib::MultirotorRpcLibClient>(host_ip);
+    } catch (rpc::rpc_error &e) {
+      ROS_ERROR_STREAM("Exception raised by the API, something went wrong."
+                       << std::endl
+                       << e.get_error().as<std::string>() << std::endl);
+    }
+  }
+
+  void ConfirmConnection() const { airsim_client_robot_->confirmConnection(); }
+
+  void EnableApiControl(const std::string &vehicle_name) const {
+    airsim_client_robot_->enableApiControl(true, vehicle_name);
+    airsim_client_robot_->armDisarm(true, vehicle_name);
+  }
+
+  airsim_ros_pkgs::GPSYaw GetHomeGeoPoint() const {
+    const auto origin_geo_point = airsim_client_robot_->getHomeGeoPoint("");
+    return utils::GetGpsMsgFromAirsimGeoPoint(origin_geo_point);
+  }
+
+ private:
+  std::unique_ptr<msr::airlib::RpcLibClientBase> airsim_client_robot_;
+};
+
 class AirsimRosWrapper {
  public:
   AirsimRosWrapper() = delete;
@@ -44,21 +102,28 @@ class AirsimRosWrapper {
   void InitializeAirsim();
   void InitializeRos();
 
+  void CreateRosPubsFromSettingsJson();
+
   Config config_;
+  TF tf_;
 
   // A flag to indicate whether the node is running in drone mode or car mode.
   AirsimMode airsim_mode_;
 
   // RPC clients to communicate with AirSim server.
-  std::unique_ptr<msr::airlib::RpcLibClientBase> airsim_client_robot_;
+  AirsimClient airsim_client_robot_;
   msr::airlib::RpcLibClientBase airsim_client_images_;
   msr::airlib::RpcLibClientBase airsim_client_lidar_;
 
   std::unordered_map<std::string, std::unique_ptr<robot_ros::VehicleROS>>
       vehicle_name_ptr_map_;
 
+  ros::Timer airsim_control_update_timer_;
+
   ros::Publisher origin_geo_point_pub_;           // home geo coord of drones
   airsim_ros_pkgs::GPSYaw origin_geo_point_msg_;  // todo duplicate
+
+  command_subscriber::CommandSubscriber command_subscriber_;
 };
 
 }  // namespace airsim_ros_wrapper

airsim_ros_pkgs/include/airsim_ros_pkgs/command_subscriber.hpp

@@ -0,0 +1,46 @@
+#ifndef AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_COMMAND_SUBSCRIBER_HPP_
+#define AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_COMMAND_SUBSCRIBER_HPP_
+
+#include "ros/ros.h"
+
+#include "airsim_ros_pkgs/messages.hpp"
+#include "airsim_ros_pkgs/utils.hpp"
+
+namespace airsim_ros {
+namespace command_subscriber {
+
+class CommandSubscriber {
+ public:
+  CommandSubscriber() = delete;
+  CommandSubscriber(const CommandSubscriber &) = delete;
+  CommandSubscriber &operator=(const CommandSubscriber &) = delete;
+  CommandSubscriber(CommandSubscriber &&) = delete;
+  CommandSubscriber &operator=(CommandSubscriber &&) = delete;
+
+  CommandSubscriber(const ros::NodeHandle &nh,
+                    const ros::NodeHandle &nh_private);
+
+ private:
+  void GimbalAngleQuatCommandCallback(
+      const airsim_ros_pkgs::GimbalAngleQuatCmdPtr &gimbal_angle_quat_cmd_msg);
+  void GimbalAngleEulerCommandCallback(
+      const airsim_ros_pkgs::GimbalAngleEulerCmdPtr
+          &gimbal_angle_euler_cmd_msg);
+  void RpyThrustCommandCallback(
+      const mavros_msgs::AttitudeTargetPtr &rpy_thrust_cmd_msg);
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private_;
+  ros::Subscriber gimbal_angle_quat_cmd_sub_;
+  ros::Subscriber gimbal_angle_euler_cmd_sub_;
+  ros::Subscriber rpy_thrust_sub_;
+
+  bool has_gimbal_cmd_;
+  messages::GimbalCmd gimbal_cmd_;
+  messages::RpyThrustCmd rpy_thrust_cmd_;
+};
+
+}  // namespace command_subscriber
+}  // namespace airsim_ros
+
+#endif  // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_COMMAND_SUBSCRIBER_HPP_

airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp

@@ -3,8 +3,7 @@
 
 #include <string>
 
-#include "common/AirSimSettings.hpp"
-#include "common/CommonStructs.hpp"
+#include "mavros_msgs/AttitudeTarget.h"
 
 // ROS custom msgs.
 #include "airsim_ros_pkgs/Altimeter.h"
@@ -24,6 +23,10 @@
 #include "airsim_ros_pkgs/Takeoff.h"
 #include "airsim_ros_pkgs/TakeoffGroup.h"
 
+#include "common/AirSimSettings.hpp"
+#include "common/CommonStructs.hpp"
+#include "vehicles/multirotor/api/MultirotorCommon.hpp"
+
 namespace airsim_ros {
 namespace messages {
 
@@ -42,14 +45,12 @@ struct GimbalCmd {
   msr::airlib::Quaternionr target_quat;
 };
 
-airsim_ros_pkgs::GPSYaw get_gps_msg_from_airsim_geo_point(
-    const msr::airlib::GeoPoint& geo_point) {
-  airsim_ros_pkgs::GPSYaw gps_msg;
-  gps_msg.latitude = geo_point.latitude;
-  gps_msg.longitude = geo_point.longitude;
-  gps_msg.altitude = geo_point.altitude;
-  return gps_msg;
-}
+struct RpyThrustCmd {
+  double body_rate_x;
+  double body_rate_y;
+  double body_rate_z;
+  double thrust;
+};
 
 }  // namespace messages
 }  // namespace airsim_ros

airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp

@@ -1,21 +1,78 @@
 #ifndef AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_UTILS_HPP_
 #define AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_UTILS_HPP_
 
+#include "tf2/LinearMath/Quaternion.h"
+
+#include "common/CommonStructs.hpp"
+
+#include "airsim_ros_pkgs/messages.hpp"
+
 namespace airsim_ros {
 namespace utils {
 
 // TODO: use Eigen instead of this.
 struct SimpleMatrix {
   int rows;
   int cols;
-  double *data;
+  double* data;
 
-  SimpleMatrix(int rows, int cols, double *data)
+  SimpleMatrix(int rows, int cols, double* data)
       : rows(rows), cols(cols), data(data) {}
 };
 
-airsim_ros_pkgs::GPSYaw get_gps_msg_from_airsim_geo_point(
-    const msr::airlib::GeoPoint &geo_point);
+template <typename T>
+inline T rad2deg(const T radians) {
+  return (radians / M_PI) * 180.0;
+}
+
+template <typename T>
+inline T deg2rad(const T degrees) {
+  return (degrees / 180.0) * M_PI;
+}
+
+template <typename T>
+inline T wrap_to_pi(T radians) {
+  int m = (int)(radians / (2 * M_PI));
+  radians = radians - m * 2 * M_PI;
+  if (radians > M_PI)
+    radians -= 2.0 * M_PI;
+  else if (radians < -M_PI)
+    radians += 2.0 * M_PI;
+  return radians;
+}
+
+template <typename T>
+inline void wrap_to_pi_inplace(T& a) {
+  a = wrap_to_pi(a);
+}
+
+template <class T>
+inline T angular_dist(T from, T to) {
+  wrap_to_pi_inplace(from);
+  wrap_to_pi_inplace(to);
+  T d = to - from;
+  if (d > M_PI)
+    d -= 2. * M_PI;
+  else if (d < -M_PI)
+    d += 2. * M_PI;
+  return d;
+}
+
+inline airsim_ros_pkgs::GPSYaw GetGpsMsgFromAirsimGeoPoint(
+    const msr::airlib::GeoPoint& geo_point) {
+  airsim_ros_pkgs::GPSYaw gps_msg;
+  gps_msg.latitude = geo_point.latitude;
+  gps_msg.longitude = geo_point.longitude;
+  gps_msg.altitude = geo_point.altitude;
+  return gps_msg;
+}
+
+inline msr::airlib::Quaternionr GetAirlibQuatFromRos(
+    const tf2::Quaternion& tf2_quat) {
+  return msr::airlib::Quaternionr(tf2_quat.w(), tf2_quat.x(), tf2_quat.y(),
+                                  tf2_quat.z());
+}
+
 }  // namespace utils
 }  // namespace airsim_ros
 

airsim_ros_pkgs/package.xml

@@ -12,6 +12,12 @@
 
   <depend>roscpp</depend>
   <depend>rospy</depend>
+  <depend>tf2_ros</depend>
+  <depend>geometry_msgs</depend>
+  <depend>geographic_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>mavros_msgs</depend>
+  <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
 
   <depend>message_generation</depend>