getting airsim wrapper

airsim_ros_pkgs/CMakeLists.txt

@@ -4,28 +4,75 @@ project(airsim_ros_pkgs)
 set(CMAKE_BUILD_TYPE "Release")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
 
-add_compile_options(-std=c++14)
+# Third party libraries.
+add_compile_options(-std=c++17) # C++17 standard for third party libraries.
+set(AIRSIM_ROOT ${PROJECT_SOURCE_DIR}/../third_party) # Couldn't change the name because it is used in other files.
+add_subdirectory("${AIRSIM_ROOT}/cmake/AirLib" AirsimLib)
+add_subdirectory("${AIRSIM_ROOT}/cmake/MavLinkCom" MavLinkCom)
+add_subdirectory("${AIRSIM_ROOT}/cmake/rpclib_wrapper" RpcLibWrapper)
 
-find_package(catkin REQUIRED COMPONENTS
+find_package(Eigen3 REQUIRED)
+
+set(MSG_DEPENDENCIES
+  geometry_msgs
+  geographic_msgs
+  nav_msgs
+  mavros_msgs
+  sensor_msgs
+  std_msgs)
+set(COMPONENT_DEPENDENCIES
+  message_generation
   roscpp
   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")
+file(GLOB SERVICE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/srv "srv/*.srv")
+add_message_files(FILES ${MESSAGE_FILES})
+add_service_files(FILES ${SERVICE_FILES})
+generate_messages(DEPENDENCIES ${MSG_DEPENDENCIES})
 
 catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS message_runtime ${MSG_DEPENDENCIES} ${COMPONENT_DEPENDENCIES})
 
-  # INCLUDE_DIRS include
-  # LIBRARIES airsim_ros_pkgs
-  # CATKIN_DEPENDS roscpp rospy
-  # DEPENDS system_lib
-)
+message(STATUS "Eigen3 include dir: ${AIRSIM_ROOT}/AirLib/deps/eigen3")
 
 include_directories(
   ${catkin_INCLUDE_DIRS}
-)
+  ${EIGEN3_INCLUDE_DIRS}
+  ${AIRSIM_ROOT}/AirLib/include
+  ${AIRSIM_ROOT}/MavLinkCom/include
+  ${AIRSIM_ROOT}/MavLinkCom/common_utils
+  include)
+
+add_library(airsim_settings_parser src/airsim_settings_parser.cpp)
+target_link_libraries(airsim_settings_parser
+  ${catkin_LIBRARIES}
+  ${EIGEN3_LIBRARIES}
+  AirLib)
+
+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})
+target_link_libraries(airsim_ros
+  ${catkin_LIBRARIES}
+  AirLib
+  airsim_settings_parser)
 
 add_executable(airsim_node src/airsim_node.cpp)
 target_link_libraries(airsim_node
-  ${catkin_LIBRARIES})
+  ${catkin_LIBRARIES}
+  airsim_settings_parser
+  airsim_ros)
 
 install(TARGETS
   airsim_node

airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp

@@ -1,4 +1,132 @@
 #ifndef AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_AIRSIM_ROS_WRAPPER_HPP_
 #define AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_AIRSIM_ROS_WRAPPER_HPP_
 
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+#include "rpc/rpc_error.h"
+
+// Airsim library headers
+#include "common/AirSimSettings.hpp"
+#include "vehicles/car/api/CarRpcLibClient.hpp"
+#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"
+
+// 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;
+
+  explicit 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;
+  AirsimRosWrapper(const AirsimRosWrapper &) = delete;
+  AirsimRosWrapper &operator=(const AirsimRosWrapper &) = delete;
+  AirsimRosWrapper(AirsimRosWrapper &&) = delete;
+  AirsimRosWrapper &operator=(AirsimRosWrapper &&) = delete;
+
+  AirsimRosWrapper(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private,
+                   const std::string &host_ip);
+
+ private:
+  enum class AirsimMode { kDrone, kCar };
+
+  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.
+  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
+}  // namespace airsim_ros
+
 #endif  // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_AIRSIM_ROS_WRAPPER_HPP_

airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp

@@ -0,0 +1,48 @@
+#ifndef AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_AIRSIM_SETTINGS_PARSER_HPP_
+#define AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_AIRSIM_SETTINGS_PARSER_HPP_
+
+#include <iostream>
+#include <string>
+
+// Airsim library headers
+#include "common/AirSimSettings.hpp"
+#include "common/common_utils/FileSystem.hpp"
+#include "common/common_utils/StrictMode.hpp"
+#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"
+
+STRICT_MODE_OFF
+#ifndef RPCLIB_MSGPACK
+#define RPCLIB_MSGPACK clmdep_msgpack
+#endif  // !RPCLIB_MSGPACK
+#include "rpc/rpc_error.h"
+STRICT_MODE_ON
+
+namespace airsim_ros {
+namespace airsim_settings_parser {
+
+// a minimal airsim settings parser, adapted from
+// Unreal/Plugins/AirSim/SimHUD/SimHUD.h
+class AirSimSettingsParser {
+ public:
+  using AirSimSettings = msr::airlib::AirSimSettings;
+  using VehicleSetting = msr::airlib::AirSimSettings::VehicleSetting;
+
+  AirSimSettingsParser();
+
+  bool success() const;
+
+ private:
+  bool initializeSettings();
+  std::string getSimMode() const;
+  bool readSettingsTextFromFile(const std::string& settingsFilepath,
+                                std::string& settingsText);
+  bool getSettingsText(std::string& settingsText);
+
+  bool success_;
+  std::string settingsText_;
+};
+
+}  // namespace airsim_settings_parser
+}  // namespace airsim_ros
+
+#endif  // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_AIRSIM_SETTINGS_PARSER_HPP_

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

@@ -0,0 +1,58 @@
+#ifndef AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_MESSAGES_HPP_
+#define AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_MESSAGES_HPP_
+
+#include <string>
+
+#include "mavros_msgs/AttitudeTarget.h"
+
+// ROS custom msgs.
+#include "airsim_ros_pkgs/Altimeter.h"
+#include "airsim_ros_pkgs/CarControls.h"
+#include "airsim_ros_pkgs/CarState.h"
+#include "airsim_ros_pkgs/Environment.h"
+#include "airsim_ros_pkgs/GPSYaw.h"
+#include "airsim_ros_pkgs/GimbalAngleEulerCmd.h"
+#include "airsim_ros_pkgs/GimbalAngleQuatCmd.h"
+#include "airsim_ros_pkgs/VelCmd.h"
+#include "airsim_ros_pkgs/VelCmdGroup.h"
+
+// ROS custom srvs.
+#include "airsim_ros_pkgs/Land.h"
+#include "airsim_ros_pkgs/LandGroup.h"
+#include "airsim_ros_pkgs/Reset.h"
+#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 {
+
+struct VelCmd {
+  double x;
+  double y;
+  double z;
+  msr::airlib::DrivetrainType drivetrain;
+  msr::airlib::YawMode yaw_mode;
+  std::string vehicle_name;
+};
+
+struct GimbalCmd {
+  std::string vehicle_name;
+  std::string camera_name;
+  msr::airlib::Quaternionr target_quat;
+};
+
+struct RpyThrustCmd {
+  double body_rate_x;
+  double body_rate_y;
+  double body_rate_z;
+  double thrust;
+};
+
+}  // namespace messages
+}  // namespace airsim_ros
+
+#endif  // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_MESSAGES_HPP_