diff --git a/airsim_ros_pkgs/CMakeLists.txt b/airsim_ros_pkgs/CMakeLists.txt index 304e9b0..b63c113 100644 --- a/airsim_ros_pkgs/CMakeLists.txt +++ b/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 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..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 @@ -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 +#include +#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" +#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; + 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> + 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_ 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..df7d68c --- /dev/null +++ b/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 +#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 + +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_ 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 new file mode 100644 index 0000000..988af3a --- /dev/null +++ b/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 + +#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_ 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..bc88f85 --- /dev/null +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/robot_ros.hpp @@ -0,0 +1,83 @@ +#ifndef AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_ROBOT_ROS_HPP_ +#define AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_ROBOT_ROS_HPP_ + +#include +#include + +// Airsim library headers +#include "common/AirSimSettings.hpp" + +// ROS headers +#include "geometry_msgs/TransformStamped.h" +#include "nav_msgs/Odometry.h" +#include "ros/ros.h" +#include "sensor_msgs/NavSatFix.h" + +#include "airsim_ros_pkgs/messages.hpp" + +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; + messages::VelCmd vel_cmd; +}; + +} // namespace robot_ros +} // namespace airsim_ros + +#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 new file mode 100644 index 0000000..0ff3ba6 --- /dev/null +++ b/airsim_ros_pkgs/include/airsim_ros_pkgs/utils.hpp @@ -0,0 +1,79 @@ +#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; + + SimpleMatrix(int rows, int cols, double* data) + : rows(rows), cols(cols), data(data) {} +}; + +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 + +#endif // AIRSIM_ROS_PKGS_INCLUDE_AIRSIM_ROS_PKGS_UTILS_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/package.xml b/airsim_ros_pkgs/package.xml index 07fa835..9c2312c 100644 --- a/airsim_ros_pkgs/package.xml +++ b/airsim_ros_pkgs/package.xml @@ -9,14 +9,19 @@ MIT catkin - roscpp - rospy - roscpp - rospy - roscpp - rospy + roscpp + rospy + tf2_ros + geometry_msgs + geographic_msgs + nav_msgs + mavros_msgs + sensor_msgs + 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..354c435 100644 --- a/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -1 +1,79 @@ #include "airsim_ros_pkgs/airsim_ros_wrapper.hpp" + +#include +#include +#include + +#include "common/AirSimSettings.hpp" +#include "vehicles/car/api/CarRpcLibClient.hpp" +#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" + +namespace airsim_ros { +namespace airsim_ros_wrapper { + +AirsimRosWrapper::AirsimRosWrapper(const ros::NodeHandle &nh, + const ros::NodeHandle &nh_private, + const std::string &host_ip) + : 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(); + + ROS_INFO("Initialized AirsimRosWrapper"); +} + +void AirsimRosWrapper::InitializeAirsim() { + try { + 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(vehicle_name_ptr_pair.first); + } + + 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." + << std::endl + << msg << std::endl); + } +} + +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/airsim_settings_parser.cpp b/airsim_ros_pkgs/src/airsim_settings_parser.cpp new file mode 100644 index 0000000..1272951 --- /dev/null +++ b/airsim_ros_pkgs/src/airsim_settings_parser.cpp @@ -0,0 +1,59 @@ +#include "airsim_ros_pkgs/airsim_settings_parser.hpp" + +#include +#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([this]() -> std::string { + return Settings::loadJSonString(settingsText_).getString("SimMode", ""); + }); + + return true; +} + +bool AirSimSettingsParser::success() const { return success_; } + +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 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 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