From 97579117d7d4c7c52ed8374d5442a2b7d76097e1 Mon Sep 17 00:00:00 2001 From: harderthan Date: Mon, 9 Oct 2023 16:26:45 +0900 Subject: [PATCH 01/15] add msgs and services --- airsim_ros_pkgs/CMakeLists.txt | 12 +++++++++++ .../airsim_ros_pkgs/airsim_ros_wrapper.hpp | 20 +++++++++++++++++++ airsim_ros_pkgs/msg/Altimeter.msg | 4 ++++ airsim_ros_pkgs/msg/CarControls.msg | 8 ++++++++ airsim_ros_pkgs/msg/CarState.msg | 8 ++++++++ airsim_ros_pkgs/msg/Environment.msg | 8 ++++++++ airsim_ros_pkgs/msg/GPSYaw.msg | 4 ++++ airsim_ros_pkgs/msg/GimbalAngleEulerCmd.msg | 6 ++++++ airsim_ros_pkgs/msg/GimbalAngleQuatCmd.msg | 4 ++++ airsim_ros_pkgs/msg/VelCmd.msg | 2 ++ airsim_ros_pkgs/msg/VelCmdGroup.msg | 2 ++ airsim_ros_pkgs/srv/Land.srv | 3 +++ airsim_ros_pkgs/srv/LandGroup.srv | 4 ++++ airsim_ros_pkgs/srv/Reset.srv | 4 ++++ airsim_ros_pkgs/srv/SetGPSPosition.srv | 8 ++++++++ airsim_ros_pkgs/srv/SetLocalPosition.srv | 11 ++++++++++ airsim_ros_pkgs/srv/Takeoff.srv | 3 +++ airsim_ros_pkgs/srv/TakeoffGroup.srv | 4 ++++ 18 files changed, 115 insertions(+) create mode 100644 airsim_ros_pkgs/msg/Altimeter.msg create mode 100644 airsim_ros_pkgs/msg/CarControls.msg create mode 100644 airsim_ros_pkgs/msg/CarState.msg create mode 100644 airsim_ros_pkgs/msg/Environment.msg create mode 100644 airsim_ros_pkgs/msg/GPSYaw.msg create mode 100644 airsim_ros_pkgs/msg/GimbalAngleEulerCmd.msg create mode 100644 airsim_ros_pkgs/msg/GimbalAngleQuatCmd.msg create mode 100644 airsim_ros_pkgs/msg/VelCmd.msg create mode 100644 airsim_ros_pkgs/msg/VelCmdGroup.msg create mode 100644 airsim_ros_pkgs/srv/Land.srv create mode 100644 airsim_ros_pkgs/srv/LandGroup.srv create mode 100644 airsim_ros_pkgs/srv/Reset.srv create mode 100644 airsim_ros_pkgs/srv/SetGPSPosition.srv create mode 100644 airsim_ros_pkgs/srv/SetLocalPosition.srv create mode 100644 airsim_ros_pkgs/srv/Takeoff.srv create mode 100644 airsim_ros_pkgs/srv/TakeoffGroup.srv diff --git a/airsim_ros_pkgs/CMakeLists.txt b/airsim_ros_pkgs/CMakeLists.txt index 304e9b0..2d9970d 100644 --- a/airsim_ros_pkgs/CMakeLists.txt +++ b/airsim_ros_pkgs/CMakeLists.txt @@ -6,11 +6,23 @@ set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") add_compile_options(-std=c++14) +set(MSG_DEPENDENCIES std_msgs geometry_msgs geographic_msgs) + +file(GLOB MESSAGE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/msg "*.msg") +file(GLOB SERVICE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/srv "*.srv") + find_package(catkin REQUIRED COMPONENTS + ${MSG_DEPENDENCIES} + message_generation roscpp rospy ) +# Custom messages and services. +add_message_files(FILES ${MESSAGE_FILES}) +add_service_files(FILES ${SERVICE_FILES}) +generate_messages(DEPENDENCIES ${MSG_DEPENDENCIES}) + catkin_package( # INCLUDE_DIRS include diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp index c1c6e77..6f9d55e 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp @@ -1,4 +1,24 @@ #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 "ros/ros.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" + #endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_AIRSIM_ROS_WRAPPER_HPP_ diff --git a/airsim_ros_pkgs/msg/Altimeter.msg b/airsim_ros_pkgs/msg/Altimeter.msg new file mode 100644 index 0000000..3a3cd1e --- /dev/null +++ b/airsim_ros_pkgs/msg/Altimeter.msg @@ -0,0 +1,4 @@ +Header header +float32 altitude +float32 pressure +float32 qnh \ No newline at end of file diff --git a/airsim_ros_pkgs/msg/CarControls.msg b/airsim_ros_pkgs/msg/CarControls.msg new file mode 100644 index 0000000..3dffecb --- /dev/null +++ b/airsim_ros_pkgs/msg/CarControls.msg @@ -0,0 +1,8 @@ +std_msgs/Header header +float32 throttle +float32 brake +float32 steering +bool handbrake +bool manual +int8 manual_gear +bool gear_immediate \ No newline at end of file diff --git a/airsim_ros_pkgs/msg/CarState.msg b/airsim_ros_pkgs/msg/CarState.msg new file mode 100644 index 0000000..4c780ad --- /dev/null +++ b/airsim_ros_pkgs/msg/CarState.msg @@ -0,0 +1,8 @@ +std_msgs/Header header +geometry_msgs/PoseWithCovariance pose +geometry_msgs/TwistWithCovariance twist +float32 speed +int8 gear +float32 rpm +float32 maxrpm +bool handbrake \ No newline at end of file diff --git a/airsim_ros_pkgs/msg/Environment.msg b/airsim_ros_pkgs/msg/Environment.msg new file mode 100644 index 0000000..e0c1ea7 --- /dev/null +++ b/airsim_ros_pkgs/msg/Environment.msg @@ -0,0 +1,8 @@ +Header header +geometry_msgs/Vector3 position +geographic_msgs/GeoPoint geo_point +geometry_msgs/Vector3 gravity +float32 air_pressure +float32 temperature +float32 air_density + diff --git a/airsim_ros_pkgs/msg/GPSYaw.msg b/airsim_ros_pkgs/msg/GPSYaw.msg new file mode 100644 index 0000000..2787e39 --- /dev/null +++ b/airsim_ros_pkgs/msg/GPSYaw.msg @@ -0,0 +1,4 @@ +float64 latitude +float64 longitude +float64 altitude +float64 yaw \ No newline at end of file diff --git a/airsim_ros_pkgs/msg/GimbalAngleEulerCmd.msg b/airsim_ros_pkgs/msg/GimbalAngleEulerCmd.msg new file mode 100644 index 0000000..411d1e3 --- /dev/null +++ b/airsim_ros_pkgs/msg/GimbalAngleEulerCmd.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +string camera_name +string vehicle_name +float64 roll +float64 pitch +float64 yaw \ No newline at end of file diff --git a/airsim_ros_pkgs/msg/GimbalAngleQuatCmd.msg b/airsim_ros_pkgs/msg/GimbalAngleQuatCmd.msg new file mode 100644 index 0000000..1b6c101 --- /dev/null +++ b/airsim_ros_pkgs/msg/GimbalAngleQuatCmd.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +string camera_name +string vehicle_name +geometry_msgs/Quaternion orientation \ No newline at end of file diff --git a/airsim_ros_pkgs/msg/VelCmd.msg b/airsim_ros_pkgs/msg/VelCmd.msg new file mode 100644 index 0000000..6a63100 --- /dev/null +++ b/airsim_ros_pkgs/msg/VelCmd.msg @@ -0,0 +1,2 @@ +geometry_msgs/Twist twist +# string vehicle_name \ No newline at end of file diff --git a/airsim_ros_pkgs/msg/VelCmdGroup.msg b/airsim_ros_pkgs/msg/VelCmdGroup.msg new file mode 100644 index 0000000..a2de40b --- /dev/null +++ b/airsim_ros_pkgs/msg/VelCmdGroup.msg @@ -0,0 +1,2 @@ +geometry_msgs/Twist twist +string[] vehicle_names \ No newline at end of file diff --git a/airsim_ros_pkgs/srv/Land.srv b/airsim_ros_pkgs/srv/Land.srv new file mode 100644 index 0000000..07caa63 --- /dev/null +++ b/airsim_ros_pkgs/srv/Land.srv @@ -0,0 +1,3 @@ +bool waitOnLastTask +--- +bool success \ No newline at end of file diff --git a/airsim_ros_pkgs/srv/LandGroup.srv b/airsim_ros_pkgs/srv/LandGroup.srv new file mode 100644 index 0000000..98975cf --- /dev/null +++ b/airsim_ros_pkgs/srv/LandGroup.srv @@ -0,0 +1,4 @@ +string[] vehicle_names +bool waitOnLastTask +--- +bool success \ No newline at end of file diff --git a/airsim_ros_pkgs/srv/Reset.srv b/airsim_ros_pkgs/srv/Reset.srv new file mode 100644 index 0000000..c783bb9 --- /dev/null +++ b/airsim_ros_pkgs/srv/Reset.srv @@ -0,0 +1,4 @@ +# string vehicle_name +bool waitOnLastTask +--- +bool success \ No newline at end of file diff --git a/airsim_ros_pkgs/srv/SetGPSPosition.srv b/airsim_ros_pkgs/srv/SetGPSPosition.srv new file mode 100644 index 0000000..fd86e15 --- /dev/null +++ b/airsim_ros_pkgs/srv/SetGPSPosition.srv @@ -0,0 +1,8 @@ +float64 latitude +float64 longitude +float64 altitude +float64 yaw +string vehicle_name +--- +#Response : return success=true, (if async=false && if setpoint reached before timeout = 30sec) || (if async=true && command sent to autopilot) +bool success \ No newline at end of file diff --git a/airsim_ros_pkgs/srv/SetLocalPosition.srv b/airsim_ros_pkgs/srv/SetLocalPosition.srv new file mode 100644 index 0000000..622ca59 --- /dev/null +++ b/airsim_ros_pkgs/srv/SetLocalPosition.srv @@ -0,0 +1,11 @@ +#Request : expects position setpoint via x, y, z +#Request : expects yaw setpoint via yaw (send yaw_valid=true) +float64 x +float64 y +float64 z +float64 yaw +string vehicle_name +--- +#Response : success=true - (if async=false && if setpoint reached before timeout = 30sec) || (if async=true) +bool success +string message \ No newline at end of file diff --git a/airsim_ros_pkgs/srv/Takeoff.srv b/airsim_ros_pkgs/srv/Takeoff.srv new file mode 100644 index 0000000..07caa63 --- /dev/null +++ b/airsim_ros_pkgs/srv/Takeoff.srv @@ -0,0 +1,3 @@ +bool waitOnLastTask +--- +bool success \ No newline at end of file diff --git a/airsim_ros_pkgs/srv/TakeoffGroup.srv b/airsim_ros_pkgs/srv/TakeoffGroup.srv new file mode 100644 index 0000000..98975cf --- /dev/null +++ b/airsim_ros_pkgs/srv/TakeoffGroup.srv @@ -0,0 +1,4 @@ +string[] vehicle_names +bool waitOnLastTask +--- +bool success \ No newline at end of file From ea7d71da906d83bb03744adcc2b3e1cba1903e79 Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Sat, 14 Oct 2023 17:26:49 +0900 Subject: [PATCH 02/15] add third party libraries included AirsimLib --- airsim_ros_pkgs/CMakeLists.txt | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/airsim_ros_pkgs/CMakeLists.txt b/airsim_ros_pkgs/CMakeLists.txt index 2d9970d..9dc02be 100644 --- a/airsim_ros_pkgs/CMakeLists.txt +++ b/airsim_ros_pkgs/CMakeLists.txt @@ -4,10 +4,16 @@ 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) +include_directories(${AIRSIM_ROOT}/AirLib/include ${AIRSIM_ROOT}/AirLib/deps/eigen3) +add_subdirectory("${AIRSIM_ROOT}/cmake/MavLinkCom" MavLinkCom) +include_directories(${AIRSIM_ROOT}/MavLinkCom/include ${AIRSIM_ROOT}/MavLinkCom/common_utils) +add_subdirectory("${AIRSIM_ROOT}/cmake/rpclib_wrapper" RpcLibWrapper) set(MSG_DEPENDENCIES std_msgs geometry_msgs geographic_msgs) - file(GLOB MESSAGE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/msg "*.msg") file(GLOB SERVICE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/srv "*.srv") From 787ca20ae2a37831accb5d7e4dbf929a10c3b5a9 Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Sat, 14 Oct 2023 18:02:41 +0900 Subject: [PATCH 03/15] add airsim_settings_parser --- airsim_ros_pkgs/CMakeLists.txt | 21 +++++++++++++++---- .../airsim_settings_parser.hpp | 15 +++++++++++++ .../src/airsim_settings_parser.cpp | 1 + 3 files changed, 33 insertions(+), 4 deletions(-) create mode 100644 airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp create mode 100644 airsim_ros_pkgs/src/airsim_settings_parser.cpp diff --git a/airsim_ros_pkgs/CMakeLists.txt b/airsim_ros_pkgs/CMakeLists.txt index 9dc02be..b623201 100644 --- a/airsim_ros_pkgs/CMakeLists.txt +++ b/airsim_ros_pkgs/CMakeLists.txt @@ -8,11 +8,11 @@ set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 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) -include_directories(${AIRSIM_ROOT}/AirLib/include ${AIRSIM_ROOT}/AirLib/deps/eigen3) add_subdirectory("${AIRSIM_ROOT}/cmake/MavLinkCom" MavLinkCom) -include_directories(${AIRSIM_ROOT}/MavLinkCom/include ${AIRSIM_ROOT}/MavLinkCom/common_utils) add_subdirectory("${AIRSIM_ROOT}/cmake/rpclib_wrapper" RpcLibWrapper) +find_package(Eigen3 REQUIRED) + set(MSG_DEPENDENCIES std_msgs geometry_msgs geographic_msgs) file(GLOB MESSAGE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/msg "*.msg") file(GLOB SERVICE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/srv "*.srv") @@ -37,13 +37,26 @@ catkin_package( # 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) add_executable(airsim_node src/airsim_node.cpp) target_link_libraries(airsim_node - ${catkin_LIBRARIES}) + ${catkin_LIBRARIES} + airsim_settings_parser) install(TARGETS airsim_node diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp new file mode 100644 index 0000000..8ede858 --- /dev/null +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp @@ -0,0 +1,15 @@ +#include +#include + +// 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 \ No newline at end of file diff --git a/airsim_ros_pkgs/src/airsim_settings_parser.cpp b/airsim_ros_pkgs/src/airsim_settings_parser.cpp new file mode 100644 index 0000000..e34d967 --- /dev/null +++ b/airsim_ros_pkgs/src/airsim_settings_parser.cpp @@ -0,0 +1 @@ +#include "airsim_ros_pkgs/airsim_settings_parser.hpp" \ No newline at end of file From 84c5909e08dad507b5642db0f3b36037f1b592c5 Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Tue, 17 Oct 2023 23:42:33 +0900 Subject: [PATCH 04/15] implement airsim_setings_parser --- .../airsim_settings_parser.hpp | 34 +++++++++- .../src/airsim_settings_parser.cpp | 62 ++++++++++++++++++- 2 files changed, 93 insertions(+), 3 deletions(-) diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp index 8ede858..fc26c30 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp @@ -1,4 +1,6 @@ -#include +#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 // Airsim library headers @@ -12,4 +14,32 @@ STRICT_MODE_OFF #define RPCLIB_MSGPACK clmdep_msgpack #endif // !RPCLIB_MSGPACK #include "rpc/rpc_error.h" -STRICT_MODE_ON \ No newline at end of file +STRICT_MODE_ON + +namespace airsim_ros { +namespace airsim_settings_parser { + +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_ diff --git a/airsim_ros_pkgs/src/airsim_settings_parser.cpp b/airsim_ros_pkgs/src/airsim_settings_parser.cpp index e34d967..7c87266 100644 --- a/airsim_ros_pkgs/src/airsim_settings_parser.cpp +++ b/airsim_ros_pkgs/src/airsim_settings_parser.cpp @@ -1 +1,61 @@ -#include "airsim_ros_pkgs/airsim_settings_parser.hpp" \ No newline at end of file +#include "airsim_ros_pkgs/airsim_settings_parser.hpp" + +#include + +namespace airsim_ros { +namespace airsim_settings_parser { + +AirSimSettingsParser::AirSimSettingsParser() { + success_ = initializeSettings(); +} + +// mimics void ASimHUD::initializeSettings() +bool AirSimSettingsParser::initializeSettings() { + if (!getSettingsText(settingsText_)) { + std::cerr << "failed to get settings text" << std::endl; + return false; + } + + AirSimSettings::initializeSettings(settingsText_); + + // not sure where settings_json initialized in + // AirSimSettings::initializeSettings() is actually used + Settings& settings_json = Settings::loadJSonString(settingsText_); + std::cout << "simmode_name: " << settings_json.getString("SimMode", "") + << "\n"; + + AirSimSettings::singleton().load( + std::bind(&AirSimSettingsParser::getSimMode, this)); + + return true; +} + +bool AirSimSettingsParser::success() const { return success_; } + +std::string AirSimSettingsParser::getSimMode() const { + return Settings::loadJSonString(settingsText_).getString("SimMode", ""); +} + +bool AirSimSettingsParser::readSettingsTextFromFile( + const std::string& settingsFilepath, std::string& settingsText) { + auto ifs = std::ifstream(settingsFilepath); + if (ifs.good()) { + std::stringstream buffer; + buffer << ifs.rdbuf(); + // todo airsim's simhud.cpp does error checking here + settingsText = buffer.str(); // todo convert to utf8 as done in simhud.cpp? + return true; + } + return false; +} + +bool AirSimSettingsParser::getSettingsText(std::string& settingsText) { + bool success = readSettingsTextFromFile( + msr::airlib::Settings::Settings::getUserDirectoryFullPath( + "settings.json"), + settingsText); + return success; +} + +} // namespace airsim_settings_parser +} // namespace airsim_ros From 96c79267b6e587b4933661d174fafa74fe26fb5a Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Tue, 24 Oct 2023 20:12:32 +0900 Subject: [PATCH 05/15] add comment --- .../include/airsim_ros_pkgs/airsim_settings_parser.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp index fc26c30..9248192 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp @@ -19,6 +19,8 @@ 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; From 04ed102ccb6295a121f638755368cab0f631c557 Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Tue, 24 Oct 2023 20:12:58 +0900 Subject: [PATCH 06/15] add structures --- .../include/airsim_ros_pkgs/messages.hpp | 29 +++++++++++++++++++ .../include/airsim_ros_pkgs/utils.hpp | 20 +++++++++++++ 2 files changed, 49 insertions(+) create mode 100644 airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp create mode 100644 airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp new file mode 100644 index 0000000..4534fd3 --- /dev/null +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp @@ -0,0 +1,29 @@ +#ifndef AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_MESSAGES_HPP_ +#define AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_MESSAGES_HPP_ + +#include + +#include "common/AirSimSettings.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; +}; + +} // namespace messages +} // namespace airsim_ros + +#endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_MESSAGES_HPP_ \ No newline at end of file diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp new file mode 100644 index 0000000..e27937d --- /dev/null +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp @@ -0,0 +1,20 @@ +#ifndef AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_UTILS_HPP_ +#define AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_UTILS_HPP_ + +namespace airsim_ros { +namespace utils { + +// TODO: use Eigen instead of this. +struct SimpleMatrix { + int rows; + int cols; + double *data; + + SimpleMatrix(int rows, int cols, double *data) + : rows(rows), cols(cols), data(data) {} +}; + +} // namespace utils +} // namespace airsim_ros + +#endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_UTILS_HPP_ \ No newline at end of file From 0c1de8b5a011b6ea1e0bff873c1320fbc21eed23 Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Tue, 24 Oct 2023 20:26:29 +0900 Subject: [PATCH 07/15] add robot_ros.hpp --- .../include/airsim_ros_pkgs/robot_ros.hpp | 76 +++++++++++++++++++ 1 file changed, 76 insertions(+) create mode 100644 airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp new file mode 100644 index 0000000..a30c315 --- /dev/null +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp @@ -0,0 +1,76 @@ +#ifndef AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_ROBOT_HPP_ +#define AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_ROBOT_HPP_ + +#include +#include + +#include "common/AirSimSettings.hpp" +#include "messages.hpp" +#include "ros/ros.h" + +namespace airsim_ros { +namespace robot_ros { + +struct SensorPublisher { + SensorBase::SensorType sensor_type; + std::string sensor_name; + ros::Publisher publisher; +}; + +class VehicleROS { + public: + virtual ~VehicleROS() {} + std::string vehicle_name; + + /// All things ROS + ros::Publisher odom_local_pub; + ros::Publisher global_gps_pub; + ros::Publisher env_pub; + airsim_ros_pkgs::Environment env_msg; + std::vector sensor_pubs; + // handle lidar seperately for max performance as data is collected on its + // own thread/callback + std::vector lidar_pubs; + + nav_msgs::Odometry curr_odom; + sensor_msgs::NavSatFix gps_sensor_msg; + + std::vector static_tf_msg_vec; + + ros::Time stamp; + + std::string odom_frame_id; +}; + +class CarROS : public VehicleROS { + public: + msr::airlib::CarApiBase::CarState curr_car_state; + + ros::Subscriber car_cmd_sub; + ros::Publisher car_state_pub; + airsim_ros_pkgs::CarState car_state_msg; + + bool has_car_cmd; + msr::airlib::CarApiBase::CarControls car_cmd; +}; + +class MultiRotorROS : public VehicleROS { + public: + /// State + msr::airlib::MultirotorState curr_drone_state; + msr::airlib::Kinematics::State truth_state; + + ros::Subscriber vel_cmd_body_frame_sub; + ros::Subscriber vel_cmd_world_frame_sub; + + ros::ServiceServer takeoff_srvr; + ros::ServiceServer land_srvr; + + bool has_vel_cmd; + VelCmd vel_cmd; +}; + +} // namespace robot_ros +} // namespace airsim_ros + +#endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_ROBOT_HPP_ From 1a529b290539bb87fc925916542e2f02fd48ce52 Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Tue, 24 Oct 2023 20:30:22 +0900 Subject: [PATCH 08/15] add AirsimRosWrapper --- .../airsim_ros_pkgs/airsim_ros_wrapper.hpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp index 6f9d55e..d09c28c 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp @@ -1,6 +1,8 @@ #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 + #include "ros/ros.h" // ROS custom msgs. @@ -21,4 +23,19 @@ #include "airsim_ros_pkgs/Takeoff.h" #include "airsim_ros_pkgs/TakeoffGroup.h" +namespace airsim_ros { +namespace utils { + +class AirsimRosWrapper { + public: + AirsimRosWrapper(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private, + const std::string &host_ip); + ~AirsimRosWrapper(){}; + + private: +} + +} // namespace utils +} // namespace airsim_ros + #endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_AIRSIM_ROS_WRAPPER_HPP_ From b383e7700e264003904fb8ddc116113f2b800b30 Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Tue, 24 Oct 2023 21:23:15 +0900 Subject: [PATCH 09/15] make package launch --- airsim_ros_pkgs/CMakeLists.txt | 23 ++++++++++++------- .../airsim_ros_pkgs/airsim_ros_wrapper.hpp | 6 ++--- airsim_ros_pkgs/package.xml | 11 ++++----- airsim_ros_pkgs/src/airsim_node.cpp | 10 ++++++++ airsim_ros_pkgs/src/airsim_ros_wrapper.cpp | 14 +++++++++++ 5 files changed, 47 insertions(+), 17 deletions(-) diff --git a/airsim_ros_pkgs/CMakeLists.txt b/airsim_ros_pkgs/CMakeLists.txt index b623201..b52b6d3 100644 --- a/airsim_ros_pkgs/CMakeLists.txt +++ b/airsim_ros_pkgs/CMakeLists.txt @@ -14,8 +14,8 @@ add_subdirectory("${AIRSIM_ROOT}/cmake/rpclib_wrapper" RpcLibWrapper) find_package(Eigen3 REQUIRED) set(MSG_DEPENDENCIES std_msgs geometry_msgs geographic_msgs) -file(GLOB MESSAGE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/msg "*.msg") -file(GLOB SERVICE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/srv "*.srv") +file(GLOB MESSAGE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/msg "msg/*.msg") +file(GLOB SERVICE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/srv "srv/*.srv") find_package(catkin REQUIRED COMPONENTS ${MSG_DEPENDENCIES} @@ -30,11 +30,8 @@ add_service_files(FILES ${SERVICE_FILES}) generate_messages(DEPENDENCIES ${MSG_DEPENDENCIES}) catkin_package( - - # INCLUDE_DIRS include - # LIBRARIES airsim_ros_pkgs - # CATKIN_DEPENDS roscpp rospy - # DEPENDS system_lib + INCLUDE_DIRS include + CATKIN_DEPENDS message_runtime roscpp std_msgs # nodelet ) message(STATUS "Eigen3 include dir: ${AIRSIM_ROOT}/AirLib/deps/eigen3") @@ -53,10 +50,20 @@ target_link_libraries(airsim_settings_parser ${EIGEN3_LIBRARIES} AirLib) +add_library(airsim_ros src/airsim_ros_wrapper.cpp) +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} - airsim_settings_parser) + airsim_settings_parser + airsim_ros) install(TARGETS airsim_node diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp index d09c28c..60a68e9 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp @@ -24,7 +24,7 @@ #include "airsim_ros_pkgs/TakeoffGroup.h" namespace airsim_ros { -namespace utils { +namespace airsim_ros_wrapper { class AirsimRosWrapper { public: @@ -33,9 +33,9 @@ class AirsimRosWrapper { ~AirsimRosWrapper(){}; private: -} +}; -} // namespace utils +} // namespace airsim_ros_wrapper } // namespace airsim_ros #endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_AIRSIM_ROS_WRAPPER_HPP_ diff --git a/airsim_ros_pkgs/package.xml b/airsim_ros_pkgs/package.xml index 07fa835..b4c2153 100644 --- a/airsim_ros_pkgs/package.xml +++ b/airsim_ros_pkgs/package.xml @@ -9,14 +9,13 @@ MIT catkin - roscpp - rospy - roscpp - rospy - roscpp - rospy + roscpp + rospy + std_msgs + message_generation + message_runtime diff --git a/airsim_ros_pkgs/src/airsim_node.cpp b/airsim_ros_pkgs/src/airsim_node.cpp index 4f6c813..bcf5c90 100644 --- a/airsim_ros_pkgs/src/airsim_node.cpp +++ b/airsim_ros_pkgs/src/airsim_node.cpp @@ -1,5 +1,11 @@ #include "ros/ros.h" +#include "airsim_ros_pkgs/airsim_ros_wrapper.hpp" + +namespace { // unnamed namespace +using airsim_ros::airsim_ros_wrapper::AirsimRosWrapper; +} + int main(int argc, char** argv) { ros::init(argc, argv, "airsim_node"); ros::NodeHandle nh; // not be used @@ -7,6 +13,10 @@ int main(int argc, char** argv) { ROS_INFO("Running airsim_node..."); + std::string host_ip = "localhost"; + nh_private.getParam("host_ip", host_ip); + AirsimRosWrapper airsim_ros_wrapper(nh, nh_private, host_ip); + ros::spin(); return 0; diff --git a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index 71aaa04..8f91025 100644 --- a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -1 +1,15 @@ #include "airsim_ros_pkgs/airsim_ros_wrapper.hpp" + +#include + +#include "ros/ros.h" + +namespace airsim_ros { +namespace airsim_ros_wrapper { + +AirsimRosWrapper::AirsimRosWrapper(const ros::NodeHandle &nh, + const ros::NodeHandle &nh_private, + const std::string &host_ip) {} + +} // namespace airsim_ros_wrapper +} // namespace airsim_ros \ No newline at end of file From 73495a31dc297f7bf0a8dd051f92b0805dae2e63 Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Tue, 24 Oct 2023 23:44:15 +0900 Subject: [PATCH 10/15] update message.hpp and implement AirsimRosWrapper constructor --- airsim_ros_pkgs/CMakeLists.txt | 15 +++-- .../airsim_ros_pkgs/airsim_ros_wrapper.hpp | 61 +++++++++++++------ .../include/airsim_ros_pkgs/messages.hpp | 28 +++++++++ .../include/airsim_ros_pkgs/robot_ros.hpp | 11 +++- .../include/airsim_ros_pkgs/utils.hpp | 2 + airsim_ros_pkgs/src/airsim_ros_wrapper.cpp | 60 +++++++++++++++++- 6 files changed, 150 insertions(+), 27 deletions(-) diff --git a/airsim_ros_pkgs/CMakeLists.txt b/airsim_ros_pkgs/CMakeLists.txt index b52b6d3..5e5a388 100644 --- a/airsim_ros_pkgs/CMakeLists.txt +++ b/airsim_ros_pkgs/CMakeLists.txt @@ -13,18 +13,21 @@ add_subdirectory("${AIRSIM_ROOT}/cmake/rpclib_wrapper" RpcLibWrapper) find_package(Eigen3 REQUIRED) -set(MSG_DEPENDENCIES std_msgs geometry_msgs geographic_msgs) -file(GLOB MESSAGE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/msg "msg/*.msg") -file(GLOB SERVICE_FILES RELATIVE ${CMAKE_CURRENT_LIST_DIR}/srv "srv/*.srv") - +set(MSG_DEPENDENCIES + geometry_msgs + geographic_msgs + nav_msgs + sensor_msgs + std_msgs) find_package(catkin REQUIRED COMPONENTS ${MSG_DEPENDENCIES} message_generation roscpp - rospy -) + rospy) # 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}) diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp index 60a68e9..8930ba0 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp @@ -3,36 +3,61 @@ #include +#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" -// 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 "airsim_ros_pkgs/messages.hpp" +#include "airsim_ros_pkgs/robot_ros.hpp" namespace airsim_ros { namespace airsim_ros_wrapper { +struct Config { + std::string host_ip; + ros::NodeHandle nh; + ros::NodeHandle nh_private; +}; + 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); - ~AirsimRosWrapper(){}; private: + enum class AirsimMode { kDrone, kCar }; + + void InitializeAirsim(); + void InitializeRos(); + + Config config_; + + // 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 airsim_client_robot_; + msr::airlib::RpcLibClientBase airsim_client_images_; + msr::airlib::RpcLibClientBase airsim_client_lidar_; + + std::unordered_map> + vehicle_name_ptr_map_; + + ros::Publisher origin_geo_point_pub_; // home geo coord of drones + msr::airlib::GeoPoint origin_geo_point_; // gps coord of unreal origin + airsim_ros_pkgs::GPSYaw origin_geo_point_msg_; // todo duplicate }; } // namespace airsim_ros_wrapper diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp index 4534fd3..485107f 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp @@ -4,6 +4,25 @@ #include #include "common/AirSimSettings.hpp" +#include "common/CommonStructs.hpp" + +// 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" namespace airsim_ros { namespace messages { @@ -23,6 +42,15 @@ 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; +} + } // namespace messages } // namespace airsim_ros diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp index a30c315..49ba217 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp @@ -4,9 +4,16 @@ #include #include +// Airsim library headers #include "common/AirSimSettings.hpp" -#include "messages.hpp" + +// ROS headers +#include "geometry_msgs/TransformStamped.h" +#include "nav_msgs/Odometry.h" #include "ros/ros.h" +#include "sensor_msgs/NavSatFix.h" + +#include "messages.hpp" namespace airsim_ros { namespace robot_ros { @@ -67,7 +74,7 @@ class MultiRotorROS : public VehicleROS { ros::ServiceServer land_srvr; bool has_vel_cmd; - VelCmd vel_cmd; + messages::VelCmd vel_cmd; }; } // namespace robot_ros diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp index e27937d..1729e49 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp @@ -14,6 +14,8 @@ struct SimpleMatrix { : rows(rows), cols(cols), data(data) {} }; +airsim_ros_pkgs::GPSYaw get_gps_msg_from_airsim_geo_point( + const msr::airlib::GeoPoint &geo_point); } // namespace utils } // namespace airsim_ros diff --git a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index 8f91025..3b5f734 100644 --- a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -2,14 +2,72 @@ #include +#include "common/AirSimSettings.hpp" +#include "vehicles/car/api/CarRpcLibClient.hpp" +#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" + #include "ros/ros.h" +#include "airsim_ros_pkgs/messages.hpp" + namespace airsim_ros { namespace airsim_ros_wrapper { AirsimRosWrapper::AirsimRosWrapper(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private, - const std::string &host_ip) {} + const std::string &host_ip) + : config_{host_ip, nh, nh_private} { + if (msr::airlib::AirSimSettings::singleton().simmode_name == "Car") { + airsim_mode_ = AirsimMode::kCar; + ROS_INFO("Setting ROS wrapper to CAR mode"); + } else { + airsim_mode_ = AirsimMode::kDrone; + ROS_INFO("Setting ROS wrapper to DRONE mode"); + } + // TODO: check other modes + + InitializeRos(); + + ROS_INFO("Initialized AirsimRosWrapper"); +} + +void AirsimRosWrapper::InitializeAirsim() { + try { + if (airsim_mode_ == AirsimMode::kDrone) { + airsim_client_robot_ = + std::make_unique( + config_.host_ip); + } else if (airsim_mode_ == AirsimMode::kCar) { + airsim_client_robot_ = + std::make_unique(config_.host_ip); + } + + airsim_client_robot_->confirmConnection(); + airsim_client_images_.confirmConnection(); + airsim_client_lidar_.confirmConnection(); + + for (const auto &vehicle_name_ptr_pair : vehicle_name_ptr_map_) { + airsim_client_robot_->enableApiControl( + true, vehicle_name_ptr_pair.first); // todo expose as rosservice? + airsim_client_robot_->armDisarm( + true, vehicle_name_ptr_pair.first); // todo exposes as rosservice? + } + + origin_geo_point_ = airsim_client_robot_->getHomeGeoPoint(""); + // todo there's only one global origin geopoint for environment. but airsim + // API accept a parameter vehicle_name? inside carsimpawnapi.cpp, there's a + // geopoint being assigned in the constructor. by? + origin_geo_point_msg_ = + messages::get_gps_msg_from_airsim_geo_point(origin_geo_point_); + } catch (rpc::rpc_error &e) { + std::string msg = e.get_error().as(); + ROS_ERROR_STREAM("Exception raised by the API, something went wrong." + << std::endl + << msg << std::endl); + } +} + +void AirsimRosWrapper::InitializeRos() {} } // namespace airsim_ros_wrapper } // namespace airsim_ros \ No newline at end of file From 6d4a33b5ca40cced89017d0a82f31262515c294f Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Tue, 24 Oct 2023 23:48:50 +0900 Subject: [PATCH 11/15] fix cpplint error --- .../include/airsim_ros_pkgs/airsim_ros_wrapper.hpp | 2 ++ airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp | 2 +- airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp | 8 ++++---- airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp | 2 +- airsim_ros_pkgs/src/airsim_ros_wrapper.cpp | 2 ++ airsim_ros_pkgs/src/airsim_settings_parser.cpp | 1 + 6 files changed, 11 insertions(+), 6 deletions(-) diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp index 8930ba0..e06fec6 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp @@ -1,7 +1,9 @@ #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 #include +#include #include "rpc/rpc_error.h" diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp index 485107f..920d46e 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp @@ -54,4 +54,4 @@ airsim_ros_pkgs::GPSYaw get_gps_msg_from_airsim_geo_point( } // namespace messages } // namespace airsim_ros -#endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_MESSAGES_HPP_ \ No newline at end of file +#endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_MESSAGES_HPP_ diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp index 49ba217..bc88f85 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp @@ -1,5 +1,5 @@ -#ifndef AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_ROBOT_HPP_ -#define AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_ROBOT_HPP_ +#ifndef AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_ROBOT_ROS_HPP_ +#define AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_ROBOT_ROS_HPP_ #include #include @@ -13,7 +13,7 @@ #include "ros/ros.h" #include "sensor_msgs/NavSatFix.h" -#include "messages.hpp" +#include "airsim_ros_pkgs/messages.hpp" namespace airsim_ros { namespace robot_ros { @@ -80,4 +80,4 @@ class MultiRotorROS : public VehicleROS { } // namespace robot_ros } // namespace airsim_ros -#endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_ROBOT_HPP_ +#endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_ROBOT_ROS_HPP_ diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp index 1729e49..747e194 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp @@ -19,4 +19,4 @@ airsim_ros_pkgs::GPSYaw get_gps_msg_from_airsim_geo_point( } // namespace utils } // namespace airsim_ros -#endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_UTILS_HPP_ \ No newline at end of file +#endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_UTILS_HPP_ diff --git a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index 3b5f734..02b8a9d 100644 --- a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -1,6 +1,8 @@ #include "airsim_ros_pkgs/airsim_ros_wrapper.hpp" +#include #include +#include #include "common/AirSimSettings.hpp" #include "vehicles/car/api/CarRpcLibClient.hpp" diff --git a/airsim_ros_pkgs/src/airsim_settings_parser.cpp b/airsim_ros_pkgs/src/airsim_settings_parser.cpp index 7c87266..5e235ee 100644 --- a/airsim_ros_pkgs/src/airsim_settings_parser.cpp +++ b/airsim_ros_pkgs/src/airsim_settings_parser.cpp @@ -1,6 +1,7 @@ #include "airsim_ros_pkgs/airsim_settings_parser.hpp" #include +#include namespace airsim_ros { namespace airsim_settings_parser { From 2d6f397e56c975cfc1aa84a769e99d3362d8dc30 Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Tue, 24 Oct 2023 23:52:41 +0900 Subject: [PATCH 12/15] apply lambda --- airsim_ros_pkgs/src/airsim_settings_parser.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/airsim_ros_pkgs/src/airsim_settings_parser.cpp b/airsim_ros_pkgs/src/airsim_settings_parser.cpp index 5e235ee..1272951 100644 --- a/airsim_ros_pkgs/src/airsim_settings_parser.cpp +++ b/airsim_ros_pkgs/src/airsim_settings_parser.cpp @@ -25,18 +25,15 @@ bool AirSimSettingsParser::initializeSettings() { std::cout << "simmode_name: " << settings_json.getString("SimMode", "") << "\n"; - AirSimSettings::singleton().load( - std::bind(&AirSimSettingsParser::getSimMode, this)); + AirSimSettings::singleton().load([this]() -> std::string { + return Settings::loadJSonString(settingsText_).getString("SimMode", ""); + }); return true; } bool AirSimSettingsParser::success() const { return success_; } -std::string AirSimSettingsParser::getSimMode() const { - return Settings::loadJSonString(settingsText_).getString("SimMode", ""); -} - bool AirSimSettingsParser::readSettingsTextFromFile( const std::string& settingsFilepath, std::string& settingsText) { auto ifs = std::ifstream(settingsFilepath); From fabd91865285cb433db60205b1d9433925ff0ad6 Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Tue, 24 Oct 2023 23:54:34 +0900 Subject: [PATCH 13/15] fix cpplint error --- .../include/airsim_ros_pkgs/airsim_settings_parser.hpp | 1 + airsim_ros_pkgs/src/airsim_ros_wrapper.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp index 9248192..df7d68c 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_settings_parser.hpp @@ -2,6 +2,7 @@ #define AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_AIRSIM_SETTINGS_PARSER_HPP_ #include +#include // Airsim library headers #include "common/AirSimSettings.hpp" diff --git a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index 02b8a9d..4438e1b 100644 --- a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -72,4 +72,4 @@ void AirsimRosWrapper::InitializeAirsim() { void AirsimRosWrapper::InitializeRos() {} } // namespace airsim_ros_wrapper -} // namespace airsim_ros \ No newline at end of file +} // namespace airsim_ros From 49f884cc3bff4bd384343ca1e8703f75bbcc187c Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Wed, 25 Oct 2023 11:27:08 +0900 Subject: [PATCH 14/15] remove temporary variable --- .../include/airsim_ros_pkgs/airsim_ros_wrapper.hpp | 1 - airsim_ros_pkgs/src/airsim_ros_wrapper.cpp | 5 +++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp index e06fec6..e6b00bb 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp @@ -58,7 +58,6 @@ class AirsimRosWrapper { vehicle_name_ptr_map_; ros::Publisher origin_geo_point_pub_; // home geo coord of drones - msr::airlib::GeoPoint origin_geo_point_; // gps coord of unreal origin airsim_ros_pkgs::GPSYaw origin_geo_point_msg_; // todo duplicate }; diff --git a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index 4438e1b..478e7ea 100644 --- a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -55,12 +55,13 @@ void AirsimRosWrapper::InitializeAirsim() { true, vehicle_name_ptr_pair.first); // todo exposes as rosservice? } - origin_geo_point_ = airsim_client_robot_->getHomeGeoPoint(""); + // Get home point from airsim. + const auto origin_geo_point = airsim_client_robot_->getHomeGeoPoint(""); // todo there's only one global origin geopoint for environment. but airsim // API accept a parameter vehicle_name? inside carsimpawnapi.cpp, there's a // geopoint being assigned in the constructor. by? origin_geo_point_msg_ = - messages::get_gps_msg_from_airsim_geo_point(origin_geo_point_); + messages::get_gps_msg_from_airsim_geo_point(origin_geo_point); } catch (rpc::rpc_error &e) { std::string msg = e.get_error().as(); ROS_ERROR_STREAM("Exception raised by the API, something went wrong." From 04e55f3e2337adc6bf6049e1ebc68c5aaf5b7427 Mon Sep 17 00:00:00 2001 From: Nicky Kim Date: Thu, 9 Nov 2023 17:58:35 +0900 Subject: [PATCH 15/15] add command_subscriber.cpp/.hpp --- airsim_ros_pkgs/CMakeLists.txt | 18 +++-- .../airsim_ros_pkgs/airsim_ros_wrapper.hpp | 67 +++++++++++++++++- .../airsim_ros_pkgs/command_subscriber.hpp | 46 +++++++++++++ .../include/airsim_ros_pkgs/messages.hpp | 21 +++--- .../include/airsim_ros_pkgs/utils.hpp | 65 +++++++++++++++-- airsim_ros_pkgs/package.xml | 6 ++ airsim_ros_pkgs/src/airsim_ros_wrapper.cpp | 63 +++++++++-------- airsim_ros_pkgs/src/command_subscriber.cpp | 69 +++++++++++++++++++ 8 files changed, 304 insertions(+), 51 deletions(-) create mode 100644 airsim_ros_pkgs/include/airsim_ros_pkgs/command_subscriber.hpp create mode 100644 airsim_ros_pkgs/src/command_subscriber.cpp diff --git a/airsim_ros_pkgs/CMakeLists.txt b/airsim_ros_pkgs/CMakeLists.txt index 5e5a388..b63c113 100644 --- a/airsim_ros_pkgs/CMakeLists.txt +++ b/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}) diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp index e6b00bb..8891313 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/airsim_ros_wrapper.hpp +++ b/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; + + explicit AirsimClient(const std::string &host_ip) { + try { + airsim_client_robot_ = + std::make_unique(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::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 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 airsim_client_robot_; + AirsimClient airsim_client_robot_; msr::airlib::RpcLibClientBase airsim_client_images_; msr::airlib::RpcLibClientBase airsim_client_lidar_; std::unordered_map> 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 diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/command_subscriber.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/command_subscriber.hpp new file mode 100644 index 0000000..a446109 --- /dev/null +++ b/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_ diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp index 920d46e..988af3a 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp @@ -3,8 +3,7 @@ #include -#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 diff --git a/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp b/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp index 747e194..0ff3ba6 100644 --- a/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp @@ -1,6 +1,12 @@ #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 { @@ -8,14 +14,65 @@ namespace utils { 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 +inline T rad2deg(const T radians) { + return (radians / M_PI) * 180.0; +} + +template +inline T deg2rad(const T degrees) { + return (degrees / 180.0) * M_PI; +} + +template +inline T wrap_to_pi(T radians) { + int m = static_cast(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 +inline void wrap_to_pi_inplace(T& a) { + a = wrap_to_pi(a); +} + +template +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 diff --git a/airsim_ros_pkgs/package.xml b/airsim_ros_pkgs/package.xml index b4c2153..9c2312c 100644 --- a/airsim_ros_pkgs/package.xml +++ b/airsim_ros_pkgs/package.xml @@ -12,6 +12,12 @@ roscpp rospy + tf2_ros + geometry_msgs + geographic_msgs + nav_msgs + mavros_msgs + sensor_msgs std_msgs message_generation diff --git a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index 478e7ea..354c435 100644 --- a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -9,6 +9,9 @@ #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" #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/messages.hpp" @@ -18,15 +21,13 @@ namespace airsim_ros_wrapper { AirsimRosWrapper::AirsimRosWrapper(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private, const std::string &host_ip) - : config_{host_ip, nh, nh_private} { - if (msr::airlib::AirSimSettings::singleton().simmode_name == "Car") { - airsim_mode_ = AirsimMode::kCar; - ROS_INFO("Setting ROS wrapper to CAR mode"); - } else { - airsim_mode_ = AirsimMode::kDrone; - ROS_INFO("Setting ROS wrapper to DRONE mode"); - } - // TODO: check other modes + : config_{false, true, host_ip, nh, nh_private}, + tf_{}, + airsim_client_robot_(host_ip), + command_subscriber_(nh, nh_private), + airsim_mode_{AirsimMode::kDrone} { + airsim_mode_ = AirsimMode::kDrone; + ROS_INFO("Setting ROS wrapper to DRONE mode"); InitializeRos(); @@ -35,33 +36,16 @@ AirsimRosWrapper::AirsimRosWrapper(const ros::NodeHandle &nh, void AirsimRosWrapper::InitializeAirsim() { try { - if (airsim_mode_ == AirsimMode::kDrone) { - airsim_client_robot_ = - std::make_unique( - config_.host_ip); - } else if (airsim_mode_ == AirsimMode::kCar) { - airsim_client_robot_ = - std::make_unique(config_.host_ip); - } + airsim_client_robot_.ConfirmConnection(); - airsim_client_robot_->confirmConnection(); airsim_client_images_.confirmConnection(); airsim_client_lidar_.confirmConnection(); for (const auto &vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - airsim_client_robot_->enableApiControl( - true, vehicle_name_ptr_pair.first); // todo expose as rosservice? - airsim_client_robot_->armDisarm( - true, vehicle_name_ptr_pair.first); // todo exposes as rosservice? + airsim_client_robot_.EnableApiControl(vehicle_name_ptr_pair.first); } - // Get home point from airsim. - const auto origin_geo_point = airsim_client_robot_->getHomeGeoPoint(""); - // todo there's only one global origin geopoint for environment. but airsim - // API accept a parameter vehicle_name? inside carsimpawnapi.cpp, there's a - // geopoint being assigned in the constructor. by? - origin_geo_point_msg_ = - messages::get_gps_msg_from_airsim_geo_point(origin_geo_point); + origin_geo_point_msg_ = airsim_client_robot_.GetHomeGeoPoint(); } catch (rpc::rpc_error &e) { std::string msg = e.get_error().as(); ROS_ERROR_STREAM("Exception raised by the API, something went wrong." @@ -70,7 +54,26 @@ void AirsimRosWrapper::InitializeAirsim() { } } -void AirsimRosWrapper::InitializeRos() {} +void AirsimRosWrapper::InitializeRos() { + // ros params + double update_airsim_control_every_n_sec; + config_.nh_private.getParam("is_vulkan", config_.is_vulkan); + config_.nh_private.getParam("update_airsim_control_every_n_sec", + update_airsim_control_every_n_sec); + config_.nh_private.getParam("publish_clock", config_.publish_clock); + config_.nh_private.param("world_frame_id", tf_.world_frame_id, + tf_.world_frame_id); + tf_.odom_frame_id = tf_.world_frame_id == frame_ids::kAirsimFrameId + ? frame_ids::kAirsimOdomFrameId + : frame_ids::kEnuOdomFrameId; + config_.nh_private.param("odom_frame_id", tf_.odom_frame_id, + tf_.odom_frame_id); + tf_.is_enu = !(tf_.odom_frame_id == frame_ids::kAirsimOdomFrameId); + config_.nh_private.param("coordinate_system_enu", tf_.is_enu, tf_.is_enu); +} + +void AirsimRosWrapper::CreateRosPubsFromSettingsJson() { +} } // namespace airsim_ros_wrapper } // namespace airsim_ros diff --git a/airsim_ros_pkgs/src/command_subscriber.cpp b/airsim_ros_pkgs/src/command_subscriber.cpp new file mode 100644 index 0000000..91763ff --- /dev/null +++ b/airsim_ros_pkgs/src/command_subscriber.cpp @@ -0,0 +1,69 @@ +#include "airsim_ros_pkgs/command_subscriber.hpp" + +#include "ros/ros.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/convert.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + +#include "airsim_ros_pkgs/utils.hpp" + +namespace airsim_ros { +namespace command_subscriber { + +CommandSubscriber::CommandSubscriber(const ros::NodeHandle &nh, + const ros::NodeHandle &nh_private) + : nh_(nh), nh_private_(nh_private) { + gimbal_angle_quat_cmd_sub_ = nh_private_.subscribe( + "gimbal_angle_quat_cmd", 50, + &CommandSubscriber::GimbalAngleQuatCommandCallback, this); + gimbal_angle_euler_cmd_sub_ = nh_private_.subscribe( + "gimbal_angle_euler_cmd", 50, + &CommandSubscriber::GimbalAngleEulerCommandCallback, this); + ros::Subscriber rpy_thrust_sub_ = + nh_private_.subscribe("/setpoint_raw/attitude", 50, + &CommandSubscriber::RpyThrustCommandCallback, this, + ros::TransportHints().tcpNoDelay()); +} + +void CommandSubscriber::GimbalAngleQuatCommandCallback( + const airsim_ros_pkgs::GimbalAngleQuatCmdPtr &gimbal_angle_quat_cmd_msg) { + tf2::Quaternion quaternion_command; + try { + tf2::convert(gimbal_angle_quat_cmd_msg->orientation, quaternion_command); + quaternion_command.normalize(); + gimbal_cmd_.target_quat = utils::GetAirlibQuatFromRos(quaternion_command); + gimbal_cmd_.camera_name = gimbal_angle_quat_cmd_msg->camera_name; + gimbal_cmd_.vehicle_name = gimbal_angle_quat_cmd_msg->vehicle_name; + has_gimbal_cmd_ = true; + } catch (tf2::TransformException &ex) { + ROS_WARN("%s", ex.what()); + } +} + +void CommandSubscriber::GimbalAngleEulerCommandCallback( + const airsim_ros_pkgs::GimbalAngleEulerCmdPtr &gimbal_angle_euler_cmd_msg) { + try { + tf2::Quaternion quaternion_command; + quaternion_command.setRPY(utils::deg2rad(gimbal_angle_euler_cmd_msg->roll), + utils::deg2rad(gimbal_angle_euler_cmd_msg->pitch), + utils::deg2rad(gimbal_angle_euler_cmd_msg->yaw)); + quaternion_command.normalize(); + gimbal_cmd_.target_quat = utils::GetAirlibQuatFromRos(quaternion_command); + gimbal_cmd_.camera_name = gimbal_angle_euler_cmd_msg->camera_name; + gimbal_cmd_.vehicle_name = gimbal_angle_euler_cmd_msg->vehicle_name; + has_gimbal_cmd_ = true; + } catch (tf2::TransformException &ex) { + ROS_WARN("%s", ex.what()); + } +} + +void CommandSubscriber::RpyThrustCommandCallback( + const mavros_msgs::AttitudeTargetPtr &rpy_thrust_cmd_msg) { + rpy_thrust_cmd_.body_rate_x = rpy_thrust_cmd_msg->body_rate.x; + rpy_thrust_cmd_.body_rate_y = rpy_thrust_cmd_msg->body_rate.y; + rpy_thrust_cmd_.body_rate_z = rpy_thrust_cmd_msg->body_rate.z; + rpy_thrust_cmd_.thrust = rpy_thrust_cmd_msg->thrust; +} + +} // namespace command_subscriber +} // namespace airsim_ros