diff --git a/.gitignore b/.gitignore index 85669e3f14..2dadbe6731 100644 --- a/.gitignore +++ b/.gitignore @@ -381,6 +381,11 @@ ros/logs/ ros/.catkin_workspace ros/src/CMakeLists.txt +# ROS2 +ros2/install/ +ros2/log/ +ros2/src/log + # docs docs/README.md build_docs/ diff --git a/ros2/src/airsim_interfaces/CMakeLists.txt b/ros2/src/airsim_interfaces/CMakeLists.txt new file mode 100644 index 0000000000..7a5edbb03b --- /dev/null +++ b/ros2/src/airsim_interfaces/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.5) + +project(airsim_interfaces) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(geographic_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/GimbalAngleEulerCmd.msg" + "msg/GimbalAngleQuatCmd.msg" + "msg/GPSYaw.msg" + "msg/VelCmd.msg" + "msg/VelCmdGroup.msg" + "msg/CarControls.msg" + "msg/CarState.msg" + "msg/Altimeter.msg" + "msg/Environment.msg" + "srv/SetGPSPosition.srv" + "srv/Takeoff.srv" + "srv/TakeoffGroup.srv" + "srv/Land.srv" + "srv/LandGroup.srv" + "srv/Reset.srv" + "srv/SetLocalPosition.srv" DEPENDENCIES std_msgs geometry_msgs geographic_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/ros2/src/airsim_interfaces/msg/Altimeter.msg b/ros2/src/airsim_interfaces/msg/Altimeter.msg new file mode 100755 index 0000000000..34e3dc1d70 --- /dev/null +++ b/ros2/src/airsim_interfaces/msg/Altimeter.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +float32 altitude +float32 pressure +float32 qnh \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/msg/CarControls.msg b/ros2/src/airsim_interfaces/msg/CarControls.msg new file mode 100755 index 0000000000..a6a2fd23c6 --- /dev/null +++ b/ros2/src/airsim_interfaces/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/ros2/src/airsim_interfaces/msg/CarState.msg b/ros2/src/airsim_interfaces/msg/CarState.msg new file mode 100755 index 0000000000..e5bde5a59d --- /dev/null +++ b/ros2/src/airsim_interfaces/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/ros2/src/airsim_interfaces/msg/Environment.msg b/ros2/src/airsim_interfaces/msg/Environment.msg new file mode 100755 index 0000000000..fdeed971cc --- /dev/null +++ b/ros2/src/airsim_interfaces/msg/Environment.msg @@ -0,0 +1,8 @@ +std_msgs/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/ros2/src/airsim_interfaces/msg/GPSYaw.msg b/ros2/src/airsim_interfaces/msg/GPSYaw.msg new file mode 100755 index 0000000000..1e03ea33a4 --- /dev/null +++ b/ros2/src/airsim_interfaces/msg/GPSYaw.msg @@ -0,0 +1,4 @@ +float64 latitude +float64 longitude +float64 altitude +float64 yaw \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/msg/GimbalAngleEulerCmd.msg b/ros2/src/airsim_interfaces/msg/GimbalAngleEulerCmd.msg new file mode 100755 index 0000000000..cf3cdf4bc8 --- /dev/null +++ b/ros2/src/airsim_interfaces/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/ros2/src/airsim_interfaces/msg/GimbalAngleQuatCmd.msg b/ros2/src/airsim_interfaces/msg/GimbalAngleQuatCmd.msg new file mode 100755 index 0000000000..819dd53c52 --- /dev/null +++ b/ros2/src/airsim_interfaces/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/ros2/src/airsim_interfaces/msg/VelCmd.msg b/ros2/src/airsim_interfaces/msg/VelCmd.msg new file mode 100755 index 0000000000..060f1e2873 --- /dev/null +++ b/ros2/src/airsim_interfaces/msg/VelCmd.msg @@ -0,0 +1 @@ +geometry_msgs/Twist twist \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/msg/VelCmdGroup.msg b/ros2/src/airsim_interfaces/msg/VelCmdGroup.msg new file mode 100755 index 0000000000..1be795d0a7 --- /dev/null +++ b/ros2/src/airsim_interfaces/msg/VelCmdGroup.msg @@ -0,0 +1,2 @@ +airsim_interfaces/VelCmd vel_cmd +string[] vehicle_names \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/package.xml b/ros2/src/airsim_interfaces/package.xml new file mode 100644 index 0000000000..9b19439500 --- /dev/null +++ b/ros2/src/airsim_interfaces/package.xml @@ -0,0 +1,26 @@ + + + airsim_interfaces + 0.1.0 + Contains message and service definitions used by the examples. + AAA + Apache License 2.0 + + AAA + + ament_cmake + + rosidl_default_generators + + std_msgs + geometry_msgs + geographic_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/ros2/src/airsim_interfaces/srv/Land.srv b/ros2/src/airsim_interfaces/srv/Land.srv new file mode 100755 index 0000000000..2100b641c0 --- /dev/null +++ b/ros2/src/airsim_interfaces/srv/Land.srv @@ -0,0 +1,3 @@ +bool wait_on_last_task +--- +bool success \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/srv/LandGroup.srv b/ros2/src/airsim_interfaces/srv/LandGroup.srv new file mode 100755 index 0000000000..1c900c14b8 --- /dev/null +++ b/ros2/src/airsim_interfaces/srv/LandGroup.srv @@ -0,0 +1,4 @@ +string[] vehicle_names +bool wait_on_last_task +--- +bool success \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/srv/Reset.srv b/ros2/src/airsim_interfaces/srv/Reset.srv new file mode 100755 index 0000000000..3e754478e9 --- /dev/null +++ b/ros2/src/airsim_interfaces/srv/Reset.srv @@ -0,0 +1,4 @@ +# string vehicle_name +bool wait_on_last_task +--- +bool success \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/srv/SetGPSPosition.srv b/ros2/src/airsim_interfaces/srv/SetGPSPosition.srv new file mode 100755 index 0000000000..76cc2c081b --- /dev/null +++ b/ros2/src/airsim_interfaces/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/ros2/src/airsim_interfaces/srv/SetLocalPosition.srv b/ros2/src/airsim_interfaces/srv/SetLocalPosition.srv new file mode 100755 index 0000000000..f6293fc6e6 --- /dev/null +++ b/ros2/src/airsim_interfaces/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/ros2/src/airsim_interfaces/srv/Takeoff.srv b/ros2/src/airsim_interfaces/srv/Takeoff.srv new file mode 100755 index 0000000000..55472170ea --- /dev/null +++ b/ros2/src/airsim_interfaces/srv/Takeoff.srv @@ -0,0 +1,3 @@ +bool wait_on_last_task +--- +bool success diff --git a/ros2/src/airsim_interfaces/srv/TakeoffGroup.srv b/ros2/src/airsim_interfaces/srv/TakeoffGroup.srv new file mode 100755 index 0000000000..1c900c14b8 --- /dev/null +++ b/ros2/src/airsim_interfaces/srv/TakeoffGroup.srv @@ -0,0 +1,4 @@ +string[] vehicle_names +bool wait_on_last_task +--- +bool success \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/CMakeLists.txt b/ros2/src/airsim_ros_pkgs/CMakeLists.txt new file mode 100644 index 0000000000..6d961a430f --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/CMakeLists.txt @@ -0,0 +1,136 @@ +cmake_minimum_required(VERSION 3.5) +project(airsim_ros_pkgs) + +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geographic_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_sensor_msgs REQUIRED) +find_package(rclpy REQUIRED) +find_package(tf2 REQUIRED) +find_package(image_transport REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_typesupport_cpp REQUIRED) +find_package(rosidl_default_runtime REQUIRED) +find_package(airsim_interfaces REQUIRED) +find_package(OpenCV REQUIRED) + +set(AIRSIM_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/../../../) + +add_subdirectory("${AIRSIM_ROOT}/cmake/rpclib_wrapper" rpclib_wrapper) +add_subdirectory("${AIRSIM_ROOT}/cmake/AirLib" AirLib) +add_subdirectory("${AIRSIM_ROOT}/cmake/MavLinkCom" MavLinkCom) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_FLAGS "-O3 -Wall -Wextra -Wnoexcept -Wstrict-null-sentinel") +set(CXX_EXP_LIB "-nostdinc++ -I/usr/include/c++/8 -I/usr/include/x86_64-linux-gnu/c++/8 -nodefaultlibs +-l/usr/lib/x86_64-linux-gnu/libc++.so -l/usr/lib/x86_64-linux-gnu/libc++abi.so +-lm -lc -lgcc_s -lgcc +-lstdc++fs -fmax-errors=10") +set(RPC_LIB_INCLUDES " ${AIRSIM_ROOT}/external/rpclib/rpclib-2.3.0/include") +set(RPC_LIB rpc) +message(STATUS "found RPC_LIB_INCLUDES=${RPC_LIB_INCLUDES}") + +set(INCLUDE_DIRS include + ${AIRSIM_ROOT}/AirLib/deps/eigen3 + ${AIRSIM_ROOT}/AirLib/include + ${RPC_LIB_INCLUDES} + ${AIRSIM_ROOT}/MavLinkCom/include + ${AIRSIM_ROOT}/MavLinkCom/common_utils + ${OpenCV_INCLUDE_DIRS} + ) +include_directories(${INCLUDE_DIRS}) + +add_library(airsim_settings_parser src/airsim_settings_parser.cpp) +target_link_libraries(airsim_settings_parser AirLib) + +add_library(pd_position_controller_simple src/pd_position_controller_simple.cpp) +target_link_libraries(pd_position_controller_simple AirLib) +ament_target_dependencies(pd_position_controller_simple + rclcpp + nav_msgs + geometry_msgs + airsim_interfaces +) + +add_library(airsim_ros src/airsim_ros_wrapper.cpp) +target_link_libraries(airsim_ros ${OpenCV_LIBS} yaml-cpp AirLib airsim_settings_parser) +ament_target_dependencies(airsim_ros + rclcpp + sensor_msgs + geometry_msgs + nav_msgs + image_transport + tf2_ros + cv_bridge + airsim_interfaces +) + +add_executable(airsim_node src/airsim_node.cpp) +target_link_libraries(airsim_node airsim_ros AirLib) +ament_target_dependencies(airsim_node + rclcpp +) + +add_executable(pd_position_controller_simple_node + src/pd_position_controller_simple_node.cpp) +target_link_libraries(pd_position_controller_simple_node + pd_position_controller_simple airsim_ros AirLib) +ament_target_dependencies(pd_position_controller_simple_node + rclcpp +) + +# rosidl_target_interfaces(airsim_node +# ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# rosidl_target_interfaces(pd_position_controller_simple_node +# ${PROJECT_NAME} "rosidl_typesupport_cpp") + +install(TARGETS airsim_node pd_position_controller_simple_node + DESTINATION lib/${PROJECT_NAME}) + +install(TARGETS airsim_ros pd_position_controller_simple + ARCHIVE + DESTINATION lib + LIBRARY + DESTINATION lib) + +install(FILES README.md DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}) + + + +ament_export_dependencies(rclcpp) +ament_export_dependencies(nav_msgs) +ament_export_dependencies(geographic_msgs) +ament_export_dependencies(std_srvs) +ament_export_dependencies(tf2_ros) +ament_export_dependencies(tf2_sensor_msgs) +ament_export_dependencies(rclpy) +ament_export_dependencies(tf2) +ament_export_dependencies(image_transport) +ament_export_dependencies(geometry_msgs) +ament_export_dependencies(cv_bridge) +ament_export_dependencies(tf2_geometry_msgs) +ament_export_dependencies(sensor_msgs) +ament_export_dependencies(mavros_msgs) +ament_export_dependencies(rosidl_default_generators) +ament_export_dependencies(ament_cmake) +ament_export_dependencies(std_msgs) +ament_export_dependencies(rosidl_default_runtime) +ament_export_dependencies(airsim_interfaces) + +ament_export_include_directories(${INCLUDE_DIRS}) +ament_export_libraries(airsim_settings_parser pd_position_controller_simple + airsim_ros) + +ament_package() diff --git a/ros2/src/airsim_ros_pkgs/README.md b/ros2/src/airsim_ros_pkgs/README.md new file mode 100755 index 0000000000..3f32237f5a --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/README.md @@ -0,0 +1,3 @@ +# airsim_ros_pkgs + +This page has moved [here](https://github.com/microsoft/AirSim/blob/master/docs/airsim_ros_pkgs.md). \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h new file mode 100644 index 0000000000..cf94c080d9 --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h @@ -0,0 +1,376 @@ +#include "common/common_utils/StrictMode.hpp" +STRICT_MODE_OFF //todo what does this do? +#ifndef RPCLIB_MSGPACK +#define RPCLIB_MSGPACK clmdep_msgpack +#endif // !RPCLIB_MSGPACK +#include "rpc/rpc_error.h" + STRICT_MODE_ON + +#include "airsim_settings_parser.h" +#include "common/AirSimSettings.hpp" +#include "common/common_utils/FileSystem.hpp" +#include "sensors/lidar/LidarSimpleParams.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensors/imu/ImuBase.hpp" +#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" +#include "vehicles/car/api/CarRpcLibClient.hpp" +#include "yaml-cpp/yaml.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include //hector_uav_msgs defunct? +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + // todo move airlib typedefs to separate header file? + typedef msr::airlib::ImageCaptureBase::ImageRequest ImageRequest; +typedef msr::airlib::ImageCaptureBase::ImageResponse ImageResponse; +typedef msr::airlib::ImageCaptureBase::ImageType ImageType; +typedef msr::airlib::AirSimSettings::CaptureSetting CaptureSetting; +typedef msr::airlib::AirSimSettings::VehicleSetting VehicleSetting; +typedef msr::airlib::AirSimSettings::CameraSetting CameraSetting; +typedef msr::airlib::AirSimSettings::LidarSetting LidarSetting; + +struct SimpleMatrix +{ + int rows; + int cols; + double* data; + + SimpleMatrix(int rows, int cols, double* data) + : rows(rows), cols(cols), data(data) + { + } +}; + +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; +}; + +template +struct SensorPublisher +{ + SensorBase::SensorType sensor_type; + std::string sensor_name; + typename rclcpp::Publisher::SharedPtr publisher; +}; + +class AirsimROSWrapper +{ +public: + enum class AIRSIM_MODE : unsigned + { + DRONE, + CAR + }; + + AirsimROSWrapper(const std::shared_ptr nh, const std::shared_ptr nh_img, const std::shared_ptr nh_lidar, const std::string& host_ip); + ~AirsimROSWrapper(){}; + + void initialize_airsim(); + void initialize_ros(); + + bool is_used_lidar_timer_cb_queue_; + bool is_used_img_timer_cb_queue_; + +private: + // utility struct for a SINGLE robot + class VehicleROS + { + public: + virtual ~VehicleROS() {} + std::string vehicle_name_; + + /// All things ROS + rclcpp::Publisher::SharedPtr odom_local_pub_; + rclcpp::Publisher::SharedPtr global_gps_pub_; + rclcpp::Publisher::SharedPtr env_pub_; + airsim_interfaces::msg::Environment env_msg_; + + std::vector> barometer_pubs_; + std::vector> imu_pubs_; + std::vector> gps_pubs_; + std::vector> magnetometer_pubs_; + std::vector> distance_pubs_; + std::vector> lidar_pubs_; + + // handle lidar seperately for max performance as data is collected on its own thread/callback + + nav_msgs::msg::Odometry curr_odom_; + sensor_msgs::msg::NavSatFix gps_sensor_msg_; + + std::vector static_tf_msg_vec_; + + rclcpp::Time stamp_; + + std::string odom_frame_id_; + }; + + class CarROS : public VehicleROS + { + public: + msr::airlib::CarApiBase::CarState curr_car_state_; + + rclcpp::Subscription::SharedPtr car_cmd_sub_; + rclcpp::Publisher::SharedPtr car_state_pub_; + airsim_interfaces::msg::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_; + + rclcpp::Subscription::SharedPtr vel_cmd_body_frame_sub_; + rclcpp::Subscription::SharedPtr vel_cmd_world_frame_sub_; + + rclcpp::Service::SharedPtr takeoff_srvr_; + rclcpp::Service::SharedPtr land_srvr_; + + bool has_vel_cmd_; + VelCmd vel_cmd_; + }; + + /// ROS timer callbacks + void img_response_timer_cb(); // update images from airsim_client_ every nth sec + void drone_state_timer_cb(); // update drone state from airsim_client_ every nth sec + void lidar_timer_cb(); + + /// ROS subscriber callbacks + void vel_cmd_world_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg, const std::string& vehicle_name); + void vel_cmd_body_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg, const std::string& vehicle_name); + + void vel_cmd_group_body_frame_cb(const airsim_interfaces::msg::VelCmdGroup::SharedPtr msg); + void vel_cmd_group_world_frame_cb(const airsim_interfaces::msg::VelCmdGroup::SharedPtr msg); + + void vel_cmd_all_world_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg); + void vel_cmd_all_body_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg); + + // void vel_cmd_body_frame_cb(const airsim_interfaces::msg::VelCmd& msg, const std::string& vehicle_name); + void gimbal_angle_quat_cmd_cb(const airsim_interfaces::msg::GimbalAngleQuatCmd::SharedPtr gimbal_angle_quat_cmd_msg); + void gimbal_angle_euler_cmd_cb(const airsim_interfaces::msg::GimbalAngleEulerCmd::SharedPtr gimbal_angle_euler_cmd_msg); + + // commands + void car_cmd_cb(const airsim_interfaces::msg::CarControls::SharedPtr msg, const std::string& vehicle_name); + void update_commands(); + + // state, returns the simulation timestamp best guess based on drone state timestamp, airsim needs to return timestap for environment + rclcpp::Time update_state(); + void update_and_publish_static_transforms(VehicleROS* vehicle_ros); + void publish_vehicle_state(); + + /// ROS service callbacks + bool takeoff_srv_cb(const std::shared_ptr request, const std::shared_ptr response, const std::string& vehicle_name); + bool takeoff_group_srv_cb(const std::shared_ptr request, const std::shared_ptr response); + bool takeoff_all_srv_cb(const std::shared_ptr request, const std::shared_ptr response); + bool land_srv_cb(const std::shared_ptr request, const std::shared_ptr response, const std::string& vehicle_name); + bool land_group_srv_cb(const std::shared_ptr request, const std::shared_ptr response); + bool land_all_srv_cb(const std::shared_ptr request, const std::shared_ptr response); + bool reset_srv_cb(const std::shared_ptr request, const std::shared_ptr response); + + /// ROS tf broadcasters + void publish_camera_tf(const ImageResponse& img_response, const rclcpp::Time& ros_time, const std::string& frame_id, const std::string& child_frame_id); + void publish_odom_tf(const nav_msgs::msg::Odometry& odom_msg); + + /// camera helper methods + sensor_msgs::msg::CameraInfo generate_cam_info(const std::string& camera_name, const CameraSetting& camera_setting, const CaptureSetting& capture_setting) const; + cv::Mat manual_decode_depth(const ImageResponse& img_response) const; + + std::shared_ptr get_img_msg_from_response(const ImageResponse& img_response, const rclcpp::Time curr_ros_time, const std::string frame_id); + std::shared_ptr get_depth_img_msg_from_response(const ImageResponse& img_response, const rclcpp::Time curr_ros_time, const std::string frame_id); + + void process_and_publish_img_response(const std::vector& img_response_vec, const int img_response_idx, const std::string& vehicle_name); + + // methods which parse setting json ang generate ros pubsubsrv + void create_ros_pubs_from_settings_json(); + void convert_tf_msg_to_enu(geometry_msgs::msg::TransformStamped& tf_msg); + void append_static_camera_tf(VehicleROS* vehicle_ros, const std::string& camera_name, const CameraSetting& camera_setting); + void append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const msr::airlib::LidarSimpleParams& lidar_setting); + void append_static_vehicle_tf(VehicleROS* vehicle_ros, const VehicleSetting& vehicle_setting); + void set_nans_to_zeros_in_pose(VehicleSetting& vehicle_setting) const; + void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, CameraSetting& camera_setting) const; + void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, LidarSetting& lidar_setting) const; + + /// utils. todo parse into an Airlib<->ROS conversion class + tf2::Quaternion get_tf2_quat(const msr::airlib::Quaternionr& airlib_quat) const; + msr::airlib::Quaternionr get_airlib_quat(const geometry_msgs::msg::Quaternion& geometry_msgs_quat) const; + msr::airlib::Quaternionr get_airlib_quat(const tf2::Quaternion& tf2_quat) const; + nav_msgs::msg::Odometry get_odom_msg_from_kinematic_state(const msr::airlib::Kinematics::State& kinematics_estimated) const; + nav_msgs::msg::Odometry get_odom_msg_from_multirotor_state(const msr::airlib::MultirotorState& drone_state) const; + nav_msgs::msg::Odometry get_odom_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const; + airsim_interfaces::msg::CarState get_roscarstate_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const; + msr::airlib::Pose get_airlib_pose(const float& x, const float& y, const float& z, const msr::airlib::Quaternionr& airlib_quat) const; + airsim_interfaces::msg::GPSYaw get_gps_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const; + sensor_msgs::msg::NavSatFix get_gps_sensor_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const; + sensor_msgs::msg::Imu get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data) const; + airsim_interfaces::msg::Altimeter get_altimeter_msg_from_airsim(const msr::airlib::BarometerBase::Output& alt_data) const; + sensor_msgs::msg::Range get_range_from_airsim(const msr::airlib::DistanceSensorData& dist_data) const; + sensor_msgs::msg::PointCloud2 get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data, const std::string& vehicle_name) const; + sensor_msgs::msg::NavSatFix get_gps_msg_from_airsim(const msr::airlib::GpsBase::Output& gps_data) const; + sensor_msgs::msg::MagneticField get_mag_msg_from_airsim(const msr::airlib::MagnetometerBase::Output& mag_data) const; + airsim_interfaces::msg::Environment get_environment_msg_from_airsim(const msr::airlib::Environment::State& env_data) const; + msr::airlib::GeoPoint get_origin_geo_point() const; + VelCmd get_airlib_world_vel_cmd(const airsim_interfaces::msg::VelCmd& msg) const; + VelCmd get_airlib_body_vel_cmd(const airsim_interfaces::msg::VelCmd& msg, const msr::airlib::Quaternionr& airlib_quat) const; + geometry_msgs::msg::Transform get_transform_msg_from_airsim(const msr::airlib::Vector3r& position, const msr::airlib::AirSimSettings::Rotation& rotation); + geometry_msgs::msg::Transform get_transform_msg_from_airsim(const msr::airlib::Vector3r& position, const msr::airlib::Quaternionr& quaternion); + + // not used anymore, but can be useful in future with an unreal camera calibration environment + void read_params_from_yaml_and_fill_cam_info_msg(const std::string& file_name, sensor_msgs::msg::CameraInfo& cam_info) const; + void convert_yaml_to_simple_mat(const YAML::Node& node, SimpleMatrix& m) const; // todo ugly + + // simulation time utility + rclcpp::Time airsim_timestamp_to_ros(const msr::airlib::TTimePoint& stamp) const; + rclcpp::Time chrono_timestamp_to_ros(const std::chrono::system_clock::time_point& stamp) const; + + template + const SensorPublisher create_sensor_publisher(const string& sensor_type_name, const string& sensor_name, SensorBase::SensorType sensor_type, const string& topic_name, int QoS); + +private: + // subscriber / services for ALL robots + rclcpp::Subscription::SharedPtr vel_cmd_all_body_frame_sub_; + rclcpp::Subscription::SharedPtr vel_cmd_all_world_frame_sub_; + rclcpp::Service::SharedPtr takeoff_all_srvr_; + rclcpp::Service::SharedPtr land_all_srvr_; + + // todo - subscriber / services for a GROUP of robots, which is defined by a list of `vehicle_name`s passed in the ros msg / srv request + rclcpp::Subscription::SharedPtr vel_cmd_group_body_frame_sub_; + rclcpp::Subscription::SharedPtr vel_cmd_group_world_frame_sub_; + rclcpp::Service::SharedPtr takeoff_group_srvr_; + rclcpp::Service::SharedPtr land_group_srvr_; + + AIRSIM_MODE airsim_mode_ = AIRSIM_MODE::DRONE; + + rclcpp::Service::SharedPtr reset_srvr_; + rclcpp::Publisher::SharedPtr origin_geo_point_pub_; // home geo coord of drones + msr::airlib::GeoPoint origin_geo_point_; // gps coord of unreal origin + airsim_interfaces::msg::GPSYaw origin_geo_point_msg_; // todo duplicate + + AirSimSettingsParser airsim_settings_parser_; + std::unordered_map> vehicle_name_ptr_map_; + static const std::unordered_map image_type_int_to_string_map_; + + bool is_vulkan_; // rosparam obtained from launch file. If vulkan is being used, we BGR encoding instead of RGB + + std::string host_ip_; + std::unique_ptr airsim_client_; + // seperate busy connections to airsim, update in their own thread + msr::airlib::RpcLibClientBase airsim_client_images_; + msr::airlib::RpcLibClientBase airsim_client_lidar_; + + std::shared_ptr nh_; + std::shared_ptr nh_img_; + std::shared_ptr nh_lidar_; + + // todo not sure if async spinners shuold be inside this class, or should be instantiated in airsim_node.cpp, and cb queues should be public + // todo for multiple drones with multiple sensors, this won't scale. make it a part of VehicleROS? + + std::mutex control_mutex_; + + // gimbal control + bool has_gimbal_cmd_; + GimbalCmd gimbal_cmd_; + + /// ROS tf + const std::string AIRSIM_FRAME_ID = "world_ned"; + std::string world_frame_id_ = AIRSIM_FRAME_ID; + const std::string AIRSIM_ODOM_FRAME_ID = "odom_local_ned"; + const std::string ENU_ODOM_FRAME_ID = "odom_local_enu"; + std::string odom_frame_id_ = AIRSIM_ODOM_FRAME_ID; + std::shared_ptr tf_broadcaster_; + std::shared_ptr static_tf_pub_; + + bool isENU_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + /// ROS params + double vel_cmd_duration_; + + /// ROS Timers. + rclcpp::TimerBase::SharedPtr airsim_img_response_timer_; + rclcpp::TimerBase::SharedPtr airsim_control_update_timer_; + rclcpp::TimerBase::SharedPtr airsim_lidar_update_timer_; + + typedef std::pair, std::string> airsim_img_request_vehicle_name_pair; + std::vector airsim_img_request_vehicle_name_pair_vec_; + std::vector image_pub_vec_; + std::vector::SharedPtr> cam_info_pub_vec_; + + std::vector camera_info_msg_vec_; + + /// ROS other publishers + rclcpp::Publisher::SharedPtr clock_pub_; + rosgraph_msgs::msg::Clock ros_clock_; + bool publish_clock_; + + rclcpp::Subscription::SharedPtr gimbal_angle_quat_cmd_sub_; + rclcpp::Subscription::SharedPtr gimbal_angle_euler_cmd_sub_; + + static constexpr char CAM_YML_NAME[] = "camera_name"; + static constexpr char WIDTH_YML_NAME[] = "image_width"; + static constexpr char HEIGHT_YML_NAME[] = "image_height"; + static constexpr char K_YML_NAME[] = "camera_matrix"; + static constexpr char D_YML_NAME[] = "distortion_coefficients"; + static constexpr char R_YML_NAME[] = "rectification_matrix"; + static constexpr char P_YML_NAME[] = "projection_matrix"; + static constexpr char DMODEL_YML_NAME[] = "distortion_model"; +}; \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/include/airsim_settings_parser.h b/ros2/src/airsim_ros_pkgs/include/airsim_settings_parser.h new file mode 100755 index 0000000000..2124ddff5e --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/include/airsim_settings_parser.h @@ -0,0 +1,35 @@ +#include "common/common_utils/StrictMode.hpp" +STRICT_MODE_OFF +#ifndef RPCLIB_MSGPACK +#define RPCLIB_MSGPACK clmdep_msgpack +#endif // !RPCLIB_MSGPACK +#include "rpc/rpc_error.h" +STRICT_MODE_ON + +#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" +#include "common/common_utils/FileSystem.hpp" +#include +#include +#include "common/AirSimSettings.hpp" + +// a minimal airsim settings parser, adapted from Unreal/Plugins/AirSim/SimHUD/SimHUD.h +class AirSimSettingsParser +{ +public: + typedef msr::airlib::AirSimSettings AirSimSettings; + +public: + AirSimSettingsParser(const std::string& host_ip); + ~AirSimSettingsParser() = default; + + bool success(); + +private: + std::string getSimMode(); + bool getSettingsText(std::string& settings_text) const; + bool initializeSettings(); + + bool success_; + std::string settings_text_; + std::string host_ip_; +}; \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/include/geodetic_conv.hpp b/ros2/src/airsim_ros_pkgs/include/geodetic_conv.hpp new file mode 100755 index 0000000000..33e5342fd8 --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/include/geodetic_conv.hpp @@ -0,0 +1,227 @@ +// full credits to https://github.com/ethz-asl/geodetic_utils +// todo add as external lib +#ifndef GEODETIC_CONVERTER_H_ +#define GEODETIC_CONVERTER_H_ + +#include "math.h" +#include + +namespace geodetic_converter +{ +// Geodetic system parameters +static double kSemimajorAxis = 6378137; +static double kSemiminorAxis = 6356752.3142; +static double kFirstEccentricitySquared = 6.69437999014 * 0.001; +static double kSecondEccentricitySquared = 6.73949674228 * 0.001; +static double kFlattening = 1 / 298.257223563; + +class GeodeticConverter +{ +public: + GeodeticConverter() + { + haveReference_ = false; + } + + ~GeodeticConverter() + { + } + + // Default copy constructor and assignment operator are OK. + + bool isInitialised() + { + return haveReference_; + } + + void getReference(double* latitude, double* longitude, double* altitude) + { + *latitude = initial_latitude_; + *longitude = initial_longitude_; + *altitude = initial_altitude_; + } + + void initialiseReference(const double latitude, const double longitude, const double altitude) + { + // Save NED origin + initial_latitude_ = deg2Rad(latitude); + initial_longitude_ = deg2Rad(longitude); + initial_altitude_ = altitude; + + // Compute ECEF of NED origin + geodetic2Ecef(latitude, longitude, altitude, &initial_ecef_x_, &initial_ecef_y_, &initial_ecef_z_); + + // Compute ECEF to NED and NED to ECEF matrices + double phiP = atan2(initial_ecef_z_, sqrt(pow(initial_ecef_x_, 2) + pow(initial_ecef_y_, 2))); + + ecef_to_ned_matrix_ = nRe(phiP, initial_longitude_); + ned_to_ecef_matrix_ = nRe(initial_latitude_, initial_longitude_).transpose(); + + haveReference_ = true; + } + + void geodetic2Ecef(const double latitude, const double longitude, const double altitude, double* x, + double* y, double* z) + { + // Convert geodetic coordinates to ECEF. + // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22 + double lat_rad = deg2Rad(latitude); + double lon_rad = deg2Rad(longitude); + double xi = sqrt(1 - kFirstEccentricitySquared * sin(lat_rad) * sin(lat_rad)); + *x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad); + *y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad); + *z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) * sin(lat_rad); + } + + void ecef2Geodetic(const double x, const double y, const double z, double* latitude, + double* longitude, double* altitude) + { + // Convert ECEF coordinates to geodetic coordinates. + // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates + // to geodetic coordinates," IEEE Transactions on Aerospace and + // Electronic Systems, vol. 30, pp. 957-961, 1994. + + double r = sqrt(x * x + y * y); + double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis; + double F = 54 * kSemiminorAxis * kSemiminorAxis * z * z; + double G = r * r + (1 - kFirstEccentricitySquared) * z * z - kFirstEccentricitySquared * Esq; + double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3); + double S = cbrt(1 + C + sqrt(C * C + 2 * C)); + double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G); + double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P); + double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q) + sqrt( + 0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q) - P * (1 - kFirstEccentricitySquared) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r); + double U = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + z * z); + double V = sqrt( + pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * z * z); + double Z_0 = kSemiminorAxis * kSemiminorAxis * z / (kSemimajorAxis * V); + *altitude = U * (1 - kSemiminorAxis * kSemiminorAxis / (kSemimajorAxis * V)); + *latitude = rad2Deg(atan((z + kSecondEccentricitySquared * Z_0) / r)); + *longitude = rad2Deg(atan2(y, x)); + } + + void ecef2Ned(const double x, const double y, const double z, double* north, double* east, + double* down) + { + // Converts ECEF coordinate position into local-tangent-plane NED. + // Coordinates relative to given ECEF coordinate frame. + + Eigen::Vector3d vect, ret; + vect(0) = x - initial_ecef_x_; + vect(1) = y - initial_ecef_y_; + vect(2) = z - initial_ecef_z_; + ret = ecef_to_ned_matrix_ * vect; + *north = ret(0); + *east = ret(1); + *down = -ret(2); + } + + void ned2Ecef(const double north, const double east, const double down, double* x, double* y, + double* z) + { + // NED (north/east/down) to ECEF coordinates + Eigen::Vector3d ned, ret; + ned(0) = north; + ned(1) = east; + ned(2) = -down; + ret = ned_to_ecef_matrix_ * ned; + *x = ret(0) + initial_ecef_x_; + *y = ret(1) + initial_ecef_y_; + *z = ret(2) + initial_ecef_z_; + } + + void geodetic2Ned(const double latitude, const double longitude, const double altitude, + double* north, double* east, double* down) + { + // Geodetic position to local NED frame + double x, y, z; + geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z); + ecef2Ned(x, y, z, north, east, down); + } + + void ned2Geodetic(const double north, const double east, const double down, double* latitude, + double* longitude, double* altitude) + { + // Local NED position to geodetic coordinates + double x, y, z; + ned2Ecef(north, east, down, &x, &y, &z); + ecef2Geodetic(x, y, z, latitude, longitude, altitude); + } + + void geodetic2Enu(const double latitude, const double longitude, const double altitude, + double* east, double* north, double* up) + { + // Geodetic position to local ENU frame + double x, y, z; + geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z); + + double aux_north, aux_east, aux_down; + ecef2Ned(x, y, z, &aux_north, &aux_east, &aux_down); + + *east = aux_east; + *north = aux_north; + *up = -aux_down; + } + + void enu2Geodetic(const double east, const double north, const double up, double* latitude, + double* longitude, double* altitude) + { + // Local ENU position to geodetic coordinates + + const double aux_north = north; + const double aux_east = east; + const double aux_down = -up; + double x, y, z; + ned2Ecef(aux_north, aux_east, aux_down, &x, &y, &z); + ecef2Geodetic(x, y, z, latitude, longitude, altitude); + } + +private: + inline Eigen::Matrix3d nRe(const double lat_radians, const double lon_radians) + { + const double sLat = sin(lat_radians); + const double sLon = sin(lon_radians); + const double cLat = cos(lat_radians); + const double cLon = cos(lon_radians); + + Eigen::Matrix3d ret; + ret(0, 0) = -sLat * cLon; + ret(0, 1) = -sLat * sLon; + ret(0, 2) = cLat; + ret(1, 0) = -sLon; + ret(1, 1) = cLon; + ret(1, 2) = 0.0; + ret(2, 0) = cLat * cLon; + ret(2, 1) = cLat * sLon; + ret(2, 2) = sLat; + + return ret; + } + + inline double rad2Deg(const double radians) + { + return (radians / M_PI) * 180.0; + } + + inline double deg2Rad(const double degrees) + { + return (degrees / 180.0) * M_PI; + } + + double initial_latitude_; + double initial_longitude_; + double initial_altitude_; + + double initial_ecef_x_; + double initial_ecef_y_; + double initial_ecef_z_; + + Eigen::Matrix3d ecef_to_ned_matrix_; + Eigen::Matrix3d ned_to_ecef_matrix_; + + bool haveReference_; + +}; // class GeodeticConverter +}; // namespace geodetic_conv + +#endif // GEODETIC_CONVERTER_H_ diff --git a/ros2/src/airsim_ros_pkgs/include/math_common.h b/ros2/src/airsim_ros_pkgs/include/math_common.h new file mode 100755 index 0000000000..375a035aa7 --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/include/math_common.h @@ -0,0 +1,45 @@ +namespace math_common +{ +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 = (int)(radians / (2 * M_PI)); + radians = radians - m * 2 * M_PI; + if (radians > M_PI) + radians -= 2.0 * M_PI; + else if (radians < -M_PI) + radians += 2.0 * M_PI; + return radians; +} + +template +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; +} +} \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/include/pd_position_controller_simple.h b/ros2/src/airsim_ros_pkgs/include/pd_position_controller_simple.h new file mode 100644 index 0000000000..b2424b27f3 --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/include/pd_position_controller_simple.h @@ -0,0 +1,139 @@ +#ifndef _PID_POSITION_CONTROLLER_SIMPLE_H_ +#define _PID_POSITION_CONTROLLER_SIMPLE_H_ + +#include "common/common_utils/StrictMode.hpp" +STRICT_MODE_OFF //todo what does this do? +#ifndef RPCLIB_MSGPACK +#define RPCLIB_MSGPACK clmdep_msgpack +#endif // !RPCLIB_MSGPACK +#include "rpc/rpc_error.h" + STRICT_MODE_ON + +#include "common/common_utils/FileSystem.hpp" +#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + // todo nicer api + class PIDParams +{ +public: + double kp_x; + double kp_y; + double kp_z; + double kp_yaw; + double kd_x; + double kd_y; + double kd_z; + double kd_yaw; + + double reached_thresh_xyz; + double reached_yaw_degrees; + + PIDParams() + : kp_x(0.5), kp_y(0.5), kp_z(0.5), kp_yaw(0.5), kd_x(0.1), kd_y(0.1), kd_z(0.1), kd_yaw(0.1), reached_thresh_xyz(0.5), reached_yaw_degrees(5.0) + { + } + + bool load_from_rosparams(const std::shared_ptr nh); +}; + +// todo should be a common representation +struct XYZYaw +{ + double x; + double y; + double z; + double yaw; +}; + +// todo should be a common representation +class DynamicConstraints +{ +public: + double max_vel_horz_abs; // meters/sec + double max_vel_vert_abs; + double max_yaw_rate_degree; + + DynamicConstraints() + : max_vel_horz_abs(1.0), max_vel_vert_abs(0.5), max_yaw_rate_degree(10.0) + { + } + + bool load_from_rosparams(const std::shared_ptr nh); +}; + +class PIDPositionController +{ +public: + PIDPositionController(const std::shared_ptr nh); + + // ROS service callbacks + bool local_position_goal_srv_cb(const std::shared_ptr request, std::shared_ptr response); + bool local_position_goal_srv_override_cb(const std::shared_ptr request, std::shared_ptr response); + bool gps_goal_srv_cb(const std::shared_ptr request, std::shared_ptr response); + bool gps_goal_srv_override_cb(const std::shared_ptr request, std::shared_ptr response); + + // ROS subscriber callbacks + void airsim_odom_cb(const nav_msgs::msg::Odometry::SharedPtr odom_msg); + void home_geopoint_cb(const airsim_interfaces::msg::GPSYaw::SharedPtr gps_msg); + + void update_control_cmd_timer_cb(); + + void reset_errors(); + + void initialize_ros(); + void compute_control_cmd(); + void enforce_dynamic_constraints(); + void publish_control_cmd(); + void check_reached_goal(); + +private: + geodetic_converter::GeodeticConverter geodetic_converter_; + const bool use_eth_lib_for_geodetic_conv_; + + const std::shared_ptr nh_; + + DynamicConstraints constraints_; + PIDParams params_; + XYZYaw target_position_; + XYZYaw curr_position_; + XYZYaw prev_error_; + XYZYaw curr_error_; + + bool has_home_geo_; + airsim_interfaces::msg::GPSYaw gps_home_msg_; + + nav_msgs::msg::Odometry curr_odom_; + airsim_interfaces::msg::VelCmd vel_cmd_; + bool reached_goal_; + bool has_goal_; + bool has_odom_; + bool got_goal_once_; + // todo check for odom msg being older than n sec + + rclcpp::Publisher::SharedPtr airsim_vel_cmd_world_frame_pub_; + rclcpp::Subscription::SharedPtr airsim_odom_sub_; + rclcpp::Subscription::SharedPtr home_geopoint_sub_; + + rclcpp::Service::SharedPtr local_position_goal_srvr_; + rclcpp::Service::SharedPtr local_position_goal_override_srvr_; + rclcpp::Service::SharedPtr gps_goal_srvr_; + rclcpp::Service::SharedPtr gps_goal_override_srvr_; + + rclcpp::TimerBase::SharedPtr update_control_cmd_timer_; +}; + +#endif /* _PID_POSITION_CONTROLLER_SIMPLE_ */ \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/include/utils.h b/ros2/src/airsim_ros_pkgs/include/utils.h new file mode 100755 index 0000000000..a62f691a63 --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/include/utils.h @@ -0,0 +1,14 @@ +#include +namespace utils +{ +inline double get_yaw_from_quat_msg(const geometry_msgs::msg::Quaternion& quat_msg) +{ + tf2::Quaternion quat_tf; + double roll, pitch, yaw; + tf2::fromMsg(quat_msg, quat_tf); + tf2::Matrix3x3(quat_tf).getRPY(roll, pitch, yaw); + return yaw; +} + +// OdometryEuler get_eigen_odom_from_rosmsg(const nav::msgs &odom_msg); +} \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/launch/airsim_node.launch.py b/ros2/src/airsim_ros_pkgs/launch/airsim_node.launch.py new file mode 100644 index 0000000000..8343f596ee --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/launch/airsim_node.launch.py @@ -0,0 +1,62 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + output = DeclareLaunchArgument( + "output", + default_value='log') + + publish_clock = DeclareLaunchArgument( + "publish_clock", + default_value='False') + + is_vulkan = DeclareLaunchArgument( + "is_vulkan", + default_value='True') + + host = DeclareLaunchArgument( + "host", + default_value='localhost') + + airsim_node = Node( + package='airsim_ros_pkgs', + executable='airsim_node', + name='airsim_node', + output='screen', + parameters=[{ + 'is_vulkan': False, + 'update_airsim_img_response_every_n_sec': 0.05, + 'update_airsim_control_every_n_sec': 0.01, + 'update_lidar_every_n_sec': 0.01, + 'publish_clock': LaunchConfiguration('publish_clock'), + 'host_ip': LaunchConfiguration('host') + }]) + + static_transforms = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('airsim_ros_pkgs'), 'launch/static_transforms.launch.py') + ) + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(output) + ld.add_action(publish_clock) + ld.add_action(is_vulkan) + ld.add_action(host) + + ld.add_action(static_transforms) + ld.add_action(airsim_node) + + return ld diff --git a/ros2/src/airsim_ros_pkgs/launch/airsim_with_simple_PD_position_controller.launch.py b/ros2/src/airsim_ros_pkgs/launch/airsim_with_simple_PD_position_controller.launch.py new file mode 100644 index 0000000000..55ce609470 --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/launch/airsim_with_simple_PD_position_controller.launch.py @@ -0,0 +1,30 @@ +import os + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'airsim_ros_pkgs'), 'launch/airsim_node.launch.py') + ) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'airsim_ros_pkgs'), 'launch/dynamic_constraints.launch.py') + ) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'airsim_ros_pkgs'), 'launch/position_controller_simple.launch.py') + ) + ) + ]) + return ld diff --git a/ros2/src/airsim_ros_pkgs/launch/dynamic_constraints.launch.py b/ros2/src/airsim_ros_pkgs/launch/dynamic_constraints.launch.py new file mode 100644 index 0000000000..d8449582df --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/launch/dynamic_constraints.launch.py @@ -0,0 +1,46 @@ + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument + +def generate_launch_description(): + ld = LaunchDescription([ + + DeclareLaunchArgument( + "max_vel_vert_abs", + default_value='10.0'), + + DeclareLaunchArgument( + "max_vel_horz_abs", + default_value='0.5'), + + DeclareLaunchArgument( + "max_yaw_rate_degree", + default_value='1.0'), + + DeclareLaunchArgument( + "gimbal_front_center_max_pitch", + default_value='40.0'), + + DeclareLaunchArgument( + "gimbal_front_center_min_pitch", + default_value='-130.0'), + + DeclareLaunchArgument( + "gimbal_front_center_max_yaw", + default_value='320.0'), + + DeclareLaunchArgument( + "gimbal_front_center_min_yaw", + default_value='-320.0'), + + DeclareLaunchArgument( + "gimbal_front_center_min_yaw", + default_value='20.0'), + + DeclareLaunchArgument( + "gimbal_front_center_min_yaw", + default_value='-20.0') + + ]) + + return ld diff --git a/ros2/src/airsim_ros_pkgs/launch/position_controller_simple.launch.py b/ros2/src/airsim_ros_pkgs/launch/position_controller_simple.launch.py new file mode 100644 index 0000000000..036d87c061 --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/launch/position_controller_simple.launch.py @@ -0,0 +1,32 @@ +import launch +from launch_ros.actions import Node + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + Node( + package='airsim_ros_pkgs', + executable='pd_position_controller_simple_node', + name='pid_position_node', + output='screen', + parameters=[{ + 'update_control_every_n_sec': 0.01, + + 'kp_x': 0.30, + 'kp_y': 0.30, + 'kp_z': 0.30, + 'kp_yaw': 0.30, + + 'kd_x': 0.05, + 'kd_y': 0.05, + 'kd_z': 0.05, + 'kd_yaw': 0.05, + + 'reached_thresh_xyz': 0.1, + 'reached_yaw_degrees': 5.0 + } + ] + ) + ]) + + return ld diff --git a/ros2/src/airsim_ros_pkgs/launch/rviz.launch.py b/ros2/src/airsim_ros_pkgs/launch/rviz.launch.py new file mode 100644 index 0000000000..19a83d48fe --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/launch/rviz.launch.py @@ -0,0 +1,21 @@ +import os + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + pkg_share = get_package_share_directory('airsim_ros_pkgs') + default_rviz_path = os.path.join(pkg_share, 'rviz/default.rviz') + + ld = launch.LaunchDescription([ + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', default_rviz_path] + ) + ]) + return ld \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/launch/static_transforms.launch.py b/ros2/src/airsim_ros_pkgs/launch/static_transforms.launch.py new file mode 100644 index 0000000000..420137977d --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/launch/static_transforms.launch.py @@ -0,0 +1,15 @@ + +import launch +import launch_ros.actions + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + name='ned_to_enu_pub', + arguments=['0', '0', '0', '1.57', '0', '3.14', 'world_ned', 'world_enu']#, '100'] + ) + ]) + return ld diff --git a/ros2/src/airsim_ros_pkgs/package.xml b/ros2/src/airsim_ros_pkgs/package.xml new file mode 100644 index 0000000000..74d1f44f20 --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/package.xml @@ -0,0 +1,53 @@ + + + airsim_ros_pkgs + 0.0.1 + ROS Wrapper over AirSim's C++ client library + + Ratnesh Madaan + + MIT + + ament_cmake + + rosidl_interface_packages + + geometry_msgs + image_transport + message_runtime + mavros_msgs + nav_msgs + rclcpp + rclpy + sensor_msgs + std_msgs + geographic_msgs + geometry_msgs + tf2_sensor_msgs + + airsim_interfaces + + builtin_interfaces + builtin_interfaces + rosidl_default_generators + + geometry_msgs + image_transport + message_generation + message_runtime + nav_msgs + rclcpp + rclpy + sensor_msgs + std_msgs + joy + + rosidl_default_generators + rosidl_default_runtime + + + ament_cmake + + + ament_cmake + diff --git a/ros2/src/airsim_ros_pkgs/rviz/default.rviz b/ros2/src/airsim_ros_pkgs/rviz/default.rviz new file mode 100755 index 0000000000..7f68da5d14 --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/rviz/default.rviz @@ -0,0 +1,276 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1 + - /TF1/Frames1 + - /Odometry1 + - /Odometry1/Shape1 + - /PointCloud21 + Splitter Ratio: 0.5051546096801758 + Tree Height: 635 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Time + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + SimpleFlight: + Value: true + SimpleFlight/LidarCustom: + Value: true + SimpleFlight/odom_local_ned: + Value: true + front_camera_body: + Value: true + front_camera_body/static: + Value: true + front_camera_optical: + Value: true + front_camera_optical/static: + Value: true + world_enu: + Value: false + world_ned: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world_ned: + SimpleFlight: + SimpleFlight/odom_local_ned: + SimpleFlight/LidarCustom: + {} + front_camera_body/static: + {} + front_camera_optical/static: + {} + front_camera_body: + {} + front_camera_optical: + {} + world_enu: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: front_left_depth_planar_registered_point_cloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /airsim_node/front_left_custom/DepthPlanar/registered/points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 25 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.20000000298023224 + Head Radius: 0.05000000074505806 + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.019999999552965164 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /airsim_node/MyQuad/odom_local_ned + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /airsim_node/drone_1/lidar/LidarCustom + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world_enu + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 864 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Width: 1846 + X: 72 + Y: 27 diff --git a/ros2/src/airsim_ros_pkgs/scripts/car_joy b/ros2/src/airsim_ros_pkgs/scripts/car_joy new file mode 100755 index 0000000000..e7b3f42c79 --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/scripts/car_joy @@ -0,0 +1,150 @@ +#!/usr/bin/env python + +#capture joystick events using ROS and convert to AirSim Car API commands +#to enable: +# rosrun joy joy_node + +import rospy +import threading +import sensor_msgs +import sensor_msgs.msg +import airsim_ros_pkgs as air +import airsim_ros_pkgs.msg + +class CarCommandTranslator(object): + def __init__(self): + self.lock = threading.Lock() + + self.last_forward_btn = 0 + self.last_reverse_btn = 0 + self.last_nuetral_btn = 0 + self.last_park_btn = 0 + self.last_shift_down_btn = 0 + self.last_shift_up_btn = 0 + self.parked = True + self.last_gear = 0 + self.shift_mode_manual = True + + update_rate_hz = rospy.get_param('~update_rate_hz', 20.0) + self.max_curvature = rospy.get_param('~max_curvature', 0.75) + self.steer_sign = rospy.get_param('~steer_sign', -1) + self.throttle_brake_sign = rospy.get_param('~throttle_brake_sign', 1) + self.auto_gear_max = rospy.get_param('~auto_gear_max', 5) + self.manual_transmission = rospy.get_param('~manual_transmission', True) + self.forward_btn_index = rospy.get_param('~forward_button_index', 0) + self.reverse_btn_index = rospy.get_param('~reverse_button_index', 1) + self.nuetral_btn_index = rospy.get_param('~nuetral_button_index', 2) + self.park_btn_index = rospy.get_param('~park_button_index', 3) + self.shift_down_btn_index = rospy.get_param('~shift_down_index', 4) + self.shift_up_btn_index = rospy.get_param('~shift_up_index', 5) + car_control_topic = rospy.get_param('~car_control_topic', '/airsim_node/drone_1/car_cmd') + + self.joy_msg = None + + self.joy_sub = rospy.Subscriber( + 'joy', + sensor_msgs.msg.Joy, + self.handle_joy) + + self.command_pub = rospy.Publisher( + car_control_topic, + air.msg.CarControls, + queue_size=0 + ) + + self.update_time = rospy.Timer( + rospy.Duration(1.0/update_rate_hz), + self.handle_update_timer + ) + + def handle_joy(self, msg): + with self.lock: + self.joy_msg = msg + + def handle_update_timer(self, ignored): + joy = None + with self.lock: + joy = self.joy_msg + + if joy is None: + return + + controls = airsim_ros_pkgs.msg.CarControls() + + controls.steering = self.steer_sign * self.max_curvature * joy.axes[2] + u = joy.axes[1] * self.throttle_brake_sign + if u > 0.0: + controls.throttle = abs(u) + controls.brake = 0.0 + else: + controls.throttle = 0.0 + controls.brake = abs(u) + + forward_btn = joy.buttons[self.forward_btn_index] + reverse_btn = joy.buttons[self.reverse_btn_index] + nuetral_btn = joy.buttons[self.nuetral_btn_index] + park_btn = joy.buttons[self.park_btn_index] + shift_up_btn = joy.buttons[self.shift_up_btn_index] + shift_down_btn = joy.buttons[self.shift_down_btn_index] + + + # gearing: -1 reverse, 0 N, >= 1 drive + controls.manual = True #set to False for automatic transmission along with manual_gear > 1 + if not self.last_nuetral_btn and nuetral_btn: + self.last_gear = 0 + self.parked = False + controls.manual = True + elif not self.last_forward_btn and forward_btn: + if self.manual_transmission: + self.last_gear = 1 + self.shift_mode_manual = True + else: + self.shift_mode_manual = False + self.last_gear = self.auto_gear_max + + self.parked = False + elif not self.last_reverse_btn and reverse_btn: + self.last_gear = -1 + self.parked = False + self.shift_mode_manual = True + elif not self.last_park_btn and park_btn: + self.parked = True + elif not self.last_shift_down_btn and shift_down_btn and self.last_gear > 1 and self.manual_transmission: + self.last_gear-=1 + self.parked = False + self.shift_mode_manual = True + elif not self.last_shift_up_btn and shift_up_btn and self.last_gear >= 1 and self.manual_transmission: + self.last_gear+=1 + self.parked = False + self.shift_mode_manual = True + + if self.parked: + self.last_gear = 0 + self.shift_mode_manual = True + controls.handbrake = True + else: + controls.handbrake = False + + controls.manual_gear = self.last_gear + controls.manual = self.shift_mode_manual + + now = rospy.Time.now() + controls.header.stamp = now + controls.gear_immediate = True + + self.last_nuetral_btn = nuetral_btn + self.last_forward_btn = forward_btn + self.last_reverse_btn = reverse_btn + self.last_park_btn = park_btn + self.last_shift_down_btn = shift_down_btn + self.last_shift_up_btn = shift_up_btn + + self.command_pub.publish(controls) + + def run(self): + rospy.spin() + +if __name__ == '__main__': + rospy.init_node('car_joy') + node = CarCommandTranslator() + node.run() \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp new file mode 100644 index 0000000000..1003f8556a --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp @@ -0,0 +1,35 @@ +#include +#include "airsim_ros_wrapper.h" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + std::shared_ptr nh = rclcpp::Node::make_shared("airsim_node", node_options); + std::shared_ptr nh_img = nh->create_sub_node("img"); + std::shared_ptr nh_lidar = nh->create_sub_node("lidar"); + std::string host_ip; + nh->get_parameter("host_ip", host_ip); + AirsimROSWrapper airsim_ros_wrapper(nh, nh_img, nh_lidar, host_ip); + + if (airsim_ros_wrapper.is_used_img_timer_cb_queue_) { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(nh_img); + while (rclcpp::ok()) { + executor.spin(); + } + } + + if (airsim_ros_wrapper.is_used_lidar_timer_cb_queue_) { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(nh_lidar); + while (rclcpp::ok()) { + executor.spin(); + } + } + + rclcpp::spin(nh); + + return 0; +} \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp new file mode 100755 index 0000000000..0d8aeb0328 --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -0,0 +1,1441 @@ +#include +#include "common/AirSimSettings.hpp" +#include + +using namespace std::placeholders; + +constexpr char AirsimROSWrapper::CAM_YML_NAME[]; +constexpr char AirsimROSWrapper::WIDTH_YML_NAME[]; +constexpr char AirsimROSWrapper::HEIGHT_YML_NAME[]; +constexpr char AirsimROSWrapper::K_YML_NAME[]; +constexpr char AirsimROSWrapper::D_YML_NAME[]; +constexpr char AirsimROSWrapper::R_YML_NAME[]; +constexpr char AirsimROSWrapper::P_YML_NAME[]; +constexpr char AirsimROSWrapper::DMODEL_YML_NAME[]; + +const std::unordered_map AirsimROSWrapper::image_type_int_to_string_map_ = { + { 0, "Scene" }, + { 1, "DepthPlanar" }, + { 2, "DepthPerspective" }, + { 3, "DepthVis" }, + { 4, "DisparityNormalized" }, + { 5, "Segmentation" }, + { 6, "SurfaceNormals" }, + { 7, "Infrared" } +}; + +AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr nh, const std::shared_ptr nh_img, const std::shared_ptr nh_lidar, const std::string& host_ip) + : is_used_lidar_timer_cb_queue_(false) + , is_used_img_timer_cb_queue_(false) + , airsim_settings_parser_(host_ip) + , host_ip_(host_ip) + , airsim_client_(nullptr) + , airsim_client_images_(host_ip) + , airsim_client_lidar_(host_ip) + , nh_(nh) + , nh_img_(nh_img) + , nh_lidar_(nh_lidar) + , isENU_(false) + , publish_clock_(false) +{ + ros_clock_.clock = rclcpp::Time(0); + + if (AirSimSettings::singleton().simmode_name != AirSimSettings::kSimModeTypeCar) { + airsim_mode_ = AIRSIM_MODE::DRONE; + RCLCPP_INFO(nh_->get_logger(), "Setting ROS wrapper to DRONE mode"); + } + else { + airsim_mode_ = AIRSIM_MODE::CAR; + RCLCPP_INFO(nh_->get_logger(), "Setting ROS wrapper to CAR mode"); + } + tf_buffer_ = std::make_shared(nh_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + tf_broadcaster_ = std::make_shared(nh_); + static_tf_pub_ = std::make_shared(nh_); + + initialize_ros(); + + RCLCPP_INFO(nh_->get_logger(), "AirsimROSWrapper Initialized!"); +} + +void AirsimROSWrapper::initialize_airsim() +{ + // todo do not reset if already in air? + try { + + if (airsim_mode_ == AIRSIM_MODE::DRONE) { + airsim_client_ = std::unique_ptr(new msr::airlib::MultirotorRpcLibClient(host_ip_)); + } + else { + airsim_client_ = std::unique_ptr(new msr::airlib::CarRpcLibClient(host_ip_)); + } + airsim_client_->confirmConnection(); + airsim_client_images_.confirmConnection(); + airsim_client_lidar_.confirmConnection(); + + for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { + airsim_client_->enableApiControl(true, vehicle_name_ptr_pair.first); // todo expose as rosservice? + airsim_client_->armDisarm(true, vehicle_name_ptr_pair.first); // todo exposes as rosservice? + } + + origin_geo_point_ = get_origin_geo_point(); + // 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_ = get_gps_msg_from_airsim_geo_point(origin_geo_point_); + } + catch (rpc::rpc_error& e) { + std::string msg = e.get_error().as(); + RCLCPP_ERROR(nh_->get_logger(), "Exception raised by the API, something went wrong.\n%s", msg); + rclcpp::shutdown(); + } +} + +void AirsimROSWrapper::initialize_ros() +{ + + // ros params + double update_airsim_control_every_n_sec; + nh_->get_parameter("is_vulkan", is_vulkan_); + nh_->get_parameter("update_airsim_control_every_n_sec", update_airsim_control_every_n_sec); + nh_->get_parameter("publish_clock", publish_clock_); + nh_->get_parameter_or("world_frame_id", world_frame_id_, world_frame_id_); + odom_frame_id_ = world_frame_id_ == AIRSIM_FRAME_ID ? AIRSIM_ODOM_FRAME_ID : ENU_ODOM_FRAME_ID; + nh_->get_parameter_or("odom_frame_id", odom_frame_id_, odom_frame_id_); + isENU_ = (odom_frame_id_ == ENU_ODOM_FRAME_ID); + nh_->get_parameter_or("coordinate_system_enu", isENU_, isENU_); + vel_cmd_duration_ = 0.05; // todo rosparam + // todo enforce dynamics constraints in this node as well? + // nh_->get_parameter("max_vert_vel_", max_vert_vel_); + // nh_->get_parameter("max_horz_vel", max_horz_vel_) + + nh_->declare_parameter("vehicle_name", rclcpp::ParameterValue("")); + create_ros_pubs_from_settings_json(); + airsim_control_update_timer_ = nh_->create_wall_timer(std::chrono::duration(update_airsim_control_every_n_sec), std::bind(&AirsimROSWrapper::drone_state_timer_cb, this)); +} + +void AirsimROSWrapper::create_ros_pubs_from_settings_json() +{ + // subscribe to control commands on global nodehandle + gimbal_angle_quat_cmd_sub_ = nh_->create_subscription("~/gimbal_angle_quat_cmd", 50, std::bind(&AirsimROSWrapper::gimbal_angle_quat_cmd_cb, this, _1)); + gimbal_angle_euler_cmd_sub_ = nh_->create_subscription("~/gimbal_angle_euler_cmd", 50, std::bind(&AirsimROSWrapper::gimbal_angle_euler_cmd_cb, this, _1)); + origin_geo_point_pub_ = nh_->create_publisher("~/origin_geo_point", 10); + + airsim_img_request_vehicle_name_pair_vec_.clear(); + image_pub_vec_.clear(); + cam_info_pub_vec_.clear(); + camera_info_msg_vec_.clear(); + vehicle_name_ptr_map_.clear(); + size_t lidar_cnt = 0; + + image_transport::ImageTransport image_transporter(nh_); + + // iterate over std::map> vehicles; + for (const auto& curr_vehicle_elem : AirSimSettings::singleton().vehicles) { + auto& vehicle_setting = curr_vehicle_elem.second; + auto curr_vehicle_name = curr_vehicle_elem.first; + + nh_->set_parameter(rclcpp::Parameter("vehicle_name", curr_vehicle_name)); + + set_nans_to_zeros_in_pose(*vehicle_setting); + + std::unique_ptr vehicle_ros = nullptr; + + if (airsim_mode_ == AIRSIM_MODE::DRONE) { + vehicle_ros = std::unique_ptr(new MultiRotorROS()); + } + else { + vehicle_ros = std::unique_ptr(new CarROS()); + } + + vehicle_ros->odom_frame_id_ = curr_vehicle_name + "/" + odom_frame_id_; + vehicle_ros->vehicle_name_ = curr_vehicle_name; + + append_static_vehicle_tf(vehicle_ros.get(), *vehicle_setting); + + const std::string topic_prefix = "~/" + curr_vehicle_name; + vehicle_ros->odom_local_pub_ = nh_->create_publisher(topic_prefix + "/" + odom_frame_id_, 10); + + vehicle_ros->env_pub_ = nh_->create_publisher(topic_prefix + "/environment", 10); + + vehicle_ros->global_gps_pub_ = nh_->create_publisher(topic_prefix + "/global_gps", 10); + + if (airsim_mode_ == AIRSIM_MODE::DRONE) { + auto drone = static_cast(vehicle_ros.get()); + + // bind to a single callback. todo optimal subs queue length + // bind multiple topics to a single callback, but keep track of which vehicle name it was by passing curr_vehicle_name as the 2nd argument + + std::function fcn_vel_cmd_body_frame_sub = std::bind(&AirsimROSWrapper::vel_cmd_body_frame_cb, this, _1, vehicle_ros->vehicle_name_); + drone->vel_cmd_body_frame_sub_ = nh_->create_subscription(topic_prefix + "/vel_cmd_body_frame", 1, fcn_vel_cmd_body_frame_sub); // todo ros::TransportHints().tcpNoDelay(); + + std::function fcn_vel_cmd_world_frame_sub = std::bind(&AirsimROSWrapper::vel_cmd_world_frame_cb, this, _1, vehicle_ros->vehicle_name_); + drone->vel_cmd_world_frame_sub_ = nh_->create_subscription(topic_prefix + "/vel_cmd_world_frame", 1, fcn_vel_cmd_world_frame_sub); + + std::function, std::shared_ptr)> fcn_takeoff_srvr = std::bind(&AirsimROSWrapper::takeoff_srv_cb, this, _1, _2, vehicle_ros->vehicle_name_); + drone->takeoff_srvr_ = nh_->create_service(topic_prefix + "/takeoff", fcn_takeoff_srvr); + + std::function, std::shared_ptr)> fcn_land_srvr = std::bind(&AirsimROSWrapper::land_srv_cb, this, _1, _2, vehicle_ros->vehicle_name_); + drone->land_srvr_ = nh_->create_service(topic_prefix + "/land", fcn_land_srvr); + + // vehicle_ros.reset_srvr = nh_->create_service(curr_vehicle_name + "/reset",&AirsimROSWrapper::reset_srv_cb, this); + } + else { + auto car = static_cast(vehicle_ros.get()); + std::function fcn_car_cmd_sub = std::bind(&AirsimROSWrapper::car_cmd_cb, this, _1, vehicle_ros->vehicle_name_); + car->car_cmd_sub_ = nh_->create_subscription(topic_prefix + "/car_cmd", 1, fcn_car_cmd_sub); + car->car_state_pub_ = nh_->create_publisher(topic_prefix + "/car_state", 10); + } + + // iterate over camera map std::map .cameras; + for (auto& curr_camera_elem : vehicle_setting->cameras) { + auto& camera_setting = curr_camera_elem.second; + auto& curr_camera_name = curr_camera_elem.first; + + set_nans_to_zeros_in_pose(*vehicle_setting, camera_setting); + append_static_camera_tf(vehicle_ros.get(), curr_camera_name, camera_setting); + // camera_setting.gimbal + std::vector current_image_request_vec; + current_image_request_vec.clear(); + + // iterate over capture_setting std::map capture_settings + for (const auto& curr_capture_elem : camera_setting.capture_settings) { + auto& capture_setting = curr_capture_elem.second; + + // todo why does AirSimSettings::loadCaptureSettings calls AirSimSettings::initializeCaptureSettings() + // which initializes default capture settings for _all_ NINE msr::airlib::ImageCaptureBase::ImageType + if (!(std::isnan(capture_setting.fov_degrees))) { + ImageType curr_image_type = msr::airlib::Utils::toEnum(capture_setting.image_type); + // if scene / segmentation / surface normals / infrared, get uncompressed image with pixels_as_floats = false + if (curr_image_type == ImageType::Scene || curr_image_type == ImageType::Segmentation || curr_image_type == ImageType::SurfaceNormals || curr_image_type == ImageType::Infrared) { + current_image_request_vec.push_back(ImageRequest(curr_camera_name, curr_image_type, false, false)); + } + // if {DepthPlanar, DepthPerspective,DepthVis, DisparityNormalized}, get float image + else { + current_image_request_vec.push_back(ImageRequest(curr_camera_name, curr_image_type, true)); + } + + const std::string camera_topic = topic_prefix + "/" + curr_camera_name + "/" + image_type_int_to_string_map_.at(capture_setting.image_type); + image_pub_vec_.push_back(image_transporter.advertise(camera_topic, 1)); + cam_info_pub_vec_.push_back(nh_->create_publisher(camera_topic + "/camera_info", 10)); + camera_info_msg_vec_.push_back(generate_cam_info(curr_camera_name, camera_setting, capture_setting)); + } + } + // push back pair (vector of image captures, current vehicle name) + airsim_img_request_vehicle_name_pair_vec_.push_back(std::make_pair(current_image_request_vec, curr_vehicle_name)); + } + + // iterate over sensors + for (auto& curr_sensor_map : vehicle_setting->sensors) { + auto& sensor_name = curr_sensor_map.first; + auto& sensor_setting = curr_sensor_map.second; + + if (sensor_setting->enabled) { + + switch (sensor_setting->sensor_type) { + case SensorBase::SensorType::Barometer: { + SensorPublisher sensor_publisher = + create_sensor_publisher("Barometer", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/altimeter/" + sensor_name, 10); + vehicle_ros->barometer_pubs_.emplace_back(sensor_publisher); + break; + } + case SensorBase::SensorType::Imu: { + SensorPublisher sensor_publisher = + create_sensor_publisher("Imu", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/imu/" + sensor_name, 10); + vehicle_ros->imu_pubs_.emplace_back(sensor_publisher); + break; + } + case SensorBase::SensorType::Gps: { + SensorPublisher sensor_publisher = + create_sensor_publisher("Gps", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/gps/" + sensor_name, 10); + vehicle_ros->gps_pubs_.emplace_back(sensor_publisher); + break; + } + case SensorBase::SensorType::Magnetometer: { + SensorPublisher sensor_publisher = + create_sensor_publisher("Magnetometer", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/magnetometer/" + sensor_name, 10); + vehicle_ros->magnetometer_pubs_.emplace_back(sensor_publisher); + break; + } + case SensorBase::SensorType::Distance: { + SensorPublisher sensor_publisher = + create_sensor_publisher("Distance", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/distance/" + sensor_name, 10); + vehicle_ros->distance_pubs_.emplace_back(sensor_publisher); + break; + } + case SensorBase::SensorType::Lidar: { + auto lidar_setting = *static_cast(sensor_setting.get()); + msr::airlib::LidarSimpleParams params; + params.initializeFromSettings(lidar_setting); + append_static_lidar_tf(vehicle_ros.get(), sensor_name, params); + + SensorPublisher sensor_publisher = + create_sensor_publisher("Lidar", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/lidar/" + sensor_name, 10); + vehicle_ros->lidar_pubs_.emplace_back(sensor_publisher); + lidar_cnt += 1; + break; + } + default: { + throw std::invalid_argument("Unexpected sensor type"); + } + } + } + } + + vehicle_name_ptr_map_.emplace(curr_vehicle_name, std::move(vehicle_ros)); // allows fast lookup in command callbacks in case of a lot of drones + } + + // add takeoff and land all services if more than 2 drones + if (vehicle_name_ptr_map_.size() > 1 && airsim_mode_ == AIRSIM_MODE::DRONE) { + takeoff_all_srvr_ = nh_->create_service("~/all_robots/takeoff", std::bind(&AirsimROSWrapper::takeoff_all_srv_cb, this, _1, _2)); + land_all_srvr_ = nh_->create_service("~/all_robots/land", std::bind(&AirsimROSWrapper::land_all_srv_cb, this, _1, _2)); + + vel_cmd_all_body_frame_sub_ = nh_->create_subscription("~/all_robots/vel_cmd_body_frame", 1, std::bind(&AirsimROSWrapper::vel_cmd_all_body_frame_cb, this, _1)); + vel_cmd_all_world_frame_sub_ = nh_->create_subscription("~/all_robots/vel_cmd_world_frame", 1, std::bind(&AirsimROSWrapper::vel_cmd_all_world_frame_cb, this, _1)); + + vel_cmd_group_body_frame_sub_ = nh_->create_subscription("~/group_of_robots/vel_cmd_body_frame", 1, std::bind(&AirsimROSWrapper::vel_cmd_group_body_frame_cb, this, _1)); + vel_cmd_group_world_frame_sub_ = nh_->create_subscription("~/group_of_robots/vel_cmd_world_frame", 1, std::bind(&AirsimROSWrapper::vel_cmd_group_world_frame_cb, this, _1)); + + takeoff_group_srvr_ = nh_->create_service("~/group_of_robots/takeoff", std::bind(&AirsimROSWrapper::takeoff_group_srv_cb, this, _1, _2)); + land_group_srvr_ = nh_->create_service("~/group_of_robots/land", std::bind(&AirsimROSWrapper::land_group_srv_cb, this, _1, _2)); + } + + // todo add per vehicle reset in AirLib API + reset_srvr_ = nh_->create_service("~/reset", std::bind(&AirsimROSWrapper::reset_srv_cb, this, _1, _2)); + + if (publish_clock_) { + clock_pub_ = nh_->create_publisher("~/clock", 1); + } + + // if >0 cameras, add one more thread for img_request_timer_cb + if (!airsim_img_request_vehicle_name_pair_vec_.empty()) { + double update_airsim_img_response_every_n_sec; + nh_->get_parameter("update_airsim_img_response_every_n_sec", update_airsim_img_response_every_n_sec); + + airsim_img_response_timer_ = nh_img_->create_wall_timer(std::chrono::duration(update_airsim_img_response_every_n_sec), std::bind(&AirsimROSWrapper::img_response_timer_cb, this)); + is_used_img_timer_cb_queue_ = true; + } + + // lidars update on their own callback/thread at a given rate + if (lidar_cnt > 0) { + double update_lidar_every_n_sec; + nh_->get_parameter("update_lidar_every_n_sec", update_lidar_every_n_sec); + airsim_lidar_update_timer_ = nh_lidar_->create_wall_timer(std::chrono::duration(update_lidar_every_n_sec), std::bind(&AirsimROSWrapper::lidar_timer_cb, this)); + is_used_lidar_timer_cb_queue_ = true; + } + + initialize_airsim(); +} + +// QoS - The depth of the publisher message queue. +// more details here - https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html +template +const SensorPublisher AirsimROSWrapper::create_sensor_publisher(const string& sensor_type_name, const string& sensor_name, SensorBase::SensorType sensor_type, const string& topic_name, int QoS) +{ + RCLCPP_INFO_STREAM(nh_->get_logger(), sensor_type_name); + SensorPublisher sensor_publisher; + sensor_publisher.sensor_name = sensor_name; + sensor_publisher.sensor_type = sensor_type; + sensor_publisher.publisher = nh_->create_publisher("~/" + topic_name, QoS); + return sensor_publisher; +} + +// todo: error check. if state is not landed, return error. +bool AirsimROSWrapper::takeoff_srv_cb(std::shared_ptr request, std::shared_ptr response, const std::string& vehicle_name) +{ + unused(response); + std::lock_guard guard(control_mutex_); + + if (request->wait_on_last_task) + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? + // response->success = + else + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name); + // response->success = + + return true; +} + +bool AirsimROSWrapper::takeoff_group_srv_cb(std::shared_ptr request, std::shared_ptr response) +{ + unused(response); + std::lock_guard guard(control_mutex_); + + if (request->wait_on_last_task) + for (const auto& vehicle_name : request->vehicle_names) + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? + // response->success = + else + for (const auto& vehicle_name : request->vehicle_names) + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name); + // response->success = + + return true; +} + +bool AirsimROSWrapper::takeoff_all_srv_cb(std::shared_ptr request, std::shared_ptr response) +{ + unused(response); + std::lock_guard guard(control_mutex_); + + if (request->wait_on_last_task) + for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name_ptr_pair.first)->waitOnLastTask(); // todo value for timeout_sec? + // response->success = + else + for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name_ptr_pair.first); + // response->success = + + return true; +} + +bool AirsimROSWrapper::land_srv_cb(std::shared_ptr request, std::shared_ptr response, const std::string& vehicle_name) +{ + unused(response); + std::lock_guard guard(control_mutex_); + + if (request->wait_on_last_task) + static_cast(airsim_client_.get())->landAsync(60, vehicle_name)->waitOnLastTask(); + else + static_cast(airsim_client_.get())->landAsync(60, vehicle_name); + + return true; //todo +} + +bool AirsimROSWrapper::land_group_srv_cb(std::shared_ptr request, std::shared_ptr response) +{ + unused(response); + std::lock_guard guard(control_mutex_); + + if (request->wait_on_last_task) + for (const auto& vehicle_name : request->vehicle_names) + static_cast(airsim_client_.get())->landAsync(60, vehicle_name)->waitOnLastTask(); + else + for (const auto& vehicle_name : request->vehicle_names) + static_cast(airsim_client_.get())->landAsync(60, vehicle_name); + + return true; //todo +} + +bool AirsimROSWrapper::land_all_srv_cb(std::shared_ptr request, std::shared_ptr response) +{ + unused(response); + std::lock_guard guard(control_mutex_); + + if (request->wait_on_last_task) + for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) + static_cast(airsim_client_.get())->landAsync(60, vehicle_name_ptr_pair.first)->waitOnLastTask(); + else + for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) + static_cast(airsim_client_.get())->landAsync(60, vehicle_name_ptr_pair.first); + + return true; //todo +} + +// todo add reset by vehicle_name API to airlib +// todo not async remove wait_on_last_task +bool AirsimROSWrapper::reset_srv_cb(std::shared_ptr request, std::shared_ptr response) +{ + unused(request); + unused(response); + std::lock_guard guard(control_mutex_); + + airsim_client_.reset(); + return true; //todo +} + +tf2::Quaternion AirsimROSWrapper::get_tf2_quat(const msr::airlib::Quaternionr& airlib_quat) const +{ + return tf2::Quaternion(airlib_quat.x(), airlib_quat.y(), airlib_quat.z(), airlib_quat.w()); +} + +msr::airlib::Quaternionr AirsimROSWrapper::get_airlib_quat(const geometry_msgs::msg::Quaternion& geometry_msgs_quat) const +{ + return msr::airlib::Quaternionr(geometry_msgs_quat.w, geometry_msgs_quat.x, geometry_msgs_quat.y, geometry_msgs_quat.z); +} + +msr::airlib::Quaternionr AirsimROSWrapper::get_airlib_quat(const tf2::Quaternion& tf2_quat) const +{ + return msr::airlib::Quaternionr(tf2_quat.w(), tf2_quat.x(), tf2_quat.y(), tf2_quat.z()); +} + +void AirsimROSWrapper::car_cmd_cb(const airsim_interfaces::msg::CarControls::SharedPtr msg, const std::string& vehicle_name) +{ + std::lock_guard guard(control_mutex_); + + auto car = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); + car->car_cmd_.throttle = msg->throttle; + car->car_cmd_.steering = msg->steering; + car->car_cmd_.brake = msg->brake; + car->car_cmd_.handbrake = msg->handbrake; + car->car_cmd_.is_manual_gear = msg->manual; + car->car_cmd_.manual_gear = msg->manual_gear; + car->car_cmd_.gear_immediate = msg->gear_immediate; + + car->has_car_cmd_ = true; +} + +msr::airlib::Pose AirsimROSWrapper::get_airlib_pose(const float& x, const float& y, const float& z, const msr::airlib::Quaternionr& airlib_quat) const +{ + return msr::airlib::Pose(msr::airlib::Vector3r(x, y, z), airlib_quat); +} + +void AirsimROSWrapper::vel_cmd_body_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg, const std::string& vehicle_name) +{ + std::lock_guard guard(control_mutex_); + + auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); + drone->vel_cmd_ = get_airlib_body_vel_cmd(*msg, drone->curr_drone_state_.kinematics_estimated.pose.orientation); + drone->has_vel_cmd_ = true; +} + +void AirsimROSWrapper::vel_cmd_group_body_frame_cb(const airsim_interfaces::msg::VelCmdGroup::SharedPtr msg) +{ + std::lock_guard guard(control_mutex_); + + for (const auto& vehicle_name : msg->vehicle_names) { + auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); + drone->vel_cmd_ = get_airlib_body_vel_cmd(msg->vel_cmd, drone->curr_drone_state_.kinematics_estimated.pose.orientation); + drone->has_vel_cmd_ = true; + } +} + +void AirsimROSWrapper::vel_cmd_all_body_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg) +{ + std::lock_guard guard(control_mutex_); + + // todo expose wait_on_last_task or nah? + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { + auto drone = static_cast(vehicle_name_ptr_pair.second.get()); + drone->vel_cmd_ = get_airlib_body_vel_cmd(*msg, drone->curr_drone_state_.kinematics_estimated.pose.orientation); + drone->has_vel_cmd_ = true; + } +} + +void AirsimROSWrapper::vel_cmd_world_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg, const std::string& vehicle_name) +{ + std::lock_guard guard(control_mutex_); + + auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); + drone->vel_cmd_ = get_airlib_world_vel_cmd(*msg); + drone->has_vel_cmd_ = true; +} + +// this is kinda unnecessary but maybe it makes life easier for the end user. +void AirsimROSWrapper::vel_cmd_group_world_frame_cb(const airsim_interfaces::msg::VelCmdGroup::SharedPtr msg) +{ + std::lock_guard guard(control_mutex_); + + for (const auto& vehicle_name : msg->vehicle_names) { + auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); + drone->vel_cmd_ = get_airlib_world_vel_cmd(msg->vel_cmd); + drone->has_vel_cmd_ = true; + } +} + +void AirsimROSWrapper::vel_cmd_all_world_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg) +{ + std::lock_guard guard(control_mutex_); + + // todo expose wait_on_last_task or nah? + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { + auto drone = static_cast(vehicle_name_ptr_pair.second.get()); + drone->vel_cmd_ = get_airlib_world_vel_cmd(*msg); + drone->has_vel_cmd_ = true; + } +} + +// todo support multiple gimbal commands +void AirsimROSWrapper::gimbal_angle_quat_cmd_cb(const airsim_interfaces::msg::GimbalAngleQuatCmd::SharedPtr gimbal_angle_quat_cmd_msg) +{ + tf2::Quaternion quat_control_cmd; + try { + tf2::convert(gimbal_angle_quat_cmd_msg->orientation, quat_control_cmd); + quat_control_cmd.normalize(); + gimbal_cmd_.target_quat = get_airlib_quat(quat_control_cmd); // airsim uses wxyz + 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) { + RCLCPP_WARN(nh_->get_logger(), "%s", ex.what()); + } +} + +// todo support multiple gimbal commands +// 1. find quaternion of default gimbal pose +// 2. forward multiply with quaternion equivalent to desired euler commands (in degrees) +// 3. call airsim client's setCameraPose which sets camera pose wrt world (or takeoff?) ned frame. todo +void AirsimROSWrapper::gimbal_angle_euler_cmd_cb(const airsim_interfaces::msg::GimbalAngleEulerCmd::SharedPtr gimbal_angle_euler_cmd_msg) +{ + try { + tf2::Quaternion quat_control_cmd; + quat_control_cmd.setRPY(math_common::deg2rad(gimbal_angle_euler_cmd_msg->roll), math_common::deg2rad(gimbal_angle_euler_cmd_msg->pitch), math_common::deg2rad(gimbal_angle_euler_cmd_msg->yaw)); + quat_control_cmd.normalize(); + gimbal_cmd_.target_quat = get_airlib_quat(quat_control_cmd); + 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) { + RCLCPP_WARN(nh_->get_logger(), "%s", ex.what()); + } +} + +airsim_interfaces::msg::CarState AirsimROSWrapper::get_roscarstate_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const +{ + airsim_interfaces::msg::CarState state_msg; + const auto odo = get_odom_msg_from_car_state(car_state); + + state_msg.pose = odo.pose; + state_msg.twist = odo.twist; + state_msg.speed = car_state.speed; + state_msg.gear = car_state.gear; + state_msg.rpm = car_state.rpm; + state_msg.maxrpm = car_state.maxrpm; + state_msg.handbrake = car_state.handbrake; + state_msg.header.stamp = airsim_timestamp_to_ros(car_state.timestamp); + + return state_msg; +} + +nav_msgs::msg::Odometry AirsimROSWrapper::get_odom_msg_from_kinematic_state(const msr::airlib::Kinematics::State& kinematics_estimated) const +{ + nav_msgs::msg::Odometry odom_msg; + + odom_msg.pose.pose.position.x = kinematics_estimated.pose.position.x(); + odom_msg.pose.pose.position.y = kinematics_estimated.pose.position.y(); + odom_msg.pose.pose.position.z = kinematics_estimated.pose.position.z(); + odom_msg.pose.pose.orientation.x = kinematics_estimated.pose.orientation.x(); + odom_msg.pose.pose.orientation.y = kinematics_estimated.pose.orientation.y(); + odom_msg.pose.pose.orientation.z = kinematics_estimated.pose.orientation.z(); + odom_msg.pose.pose.orientation.w = kinematics_estimated.pose.orientation.w(); + + odom_msg.twist.twist.linear.x = kinematics_estimated.twist.linear.x(); + odom_msg.twist.twist.linear.y = kinematics_estimated.twist.linear.y(); + odom_msg.twist.twist.linear.z = kinematics_estimated.twist.linear.z(); + odom_msg.twist.twist.angular.x = kinematics_estimated.twist.angular.x(); + odom_msg.twist.twist.angular.y = kinematics_estimated.twist.angular.y(); + odom_msg.twist.twist.angular.z = kinematics_estimated.twist.angular.z(); + + if (isENU_) { + std::swap(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y); + odom_msg.pose.pose.position.z = -odom_msg.pose.pose.position.z; + std::swap(odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y); + odom_msg.pose.pose.orientation.z = -odom_msg.pose.pose.orientation.z; + std::swap(odom_msg.twist.twist.linear.x, odom_msg.twist.twist.linear.y); + odom_msg.twist.twist.linear.z = -odom_msg.twist.twist.linear.z; + std::swap(odom_msg.twist.twist.angular.x, odom_msg.twist.twist.angular.y); + odom_msg.twist.twist.angular.z = -odom_msg.twist.twist.angular.z; + } + + return odom_msg; +} + +nav_msgs::msg::Odometry AirsimROSWrapper::get_odom_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const +{ + return get_odom_msg_from_kinematic_state(car_state.kinematics_estimated); +} + +nav_msgs::msg::Odometry AirsimROSWrapper::get_odom_msg_from_multirotor_state(const msr::airlib::MultirotorState& drone_state) const +{ + return get_odom_msg_from_kinematic_state(drone_state.kinematics_estimated); +} + +// https://docs.ros.org/jade/api/sensor_msgs/html/point__cloud__conversion_8h_source.html#l00066 +// look at UnrealLidarSensor.cpp UnrealLidarSensor::getPointCloud() for math +// read this carefully https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/PointCloud2.html +sensor_msgs::msg::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data, const std::string& vehicle_name) const +{ + sensor_msgs::msg::PointCloud2 lidar_msg; + lidar_msg.header.stamp = nh_->now(); + lidar_msg.header.frame_id = vehicle_name; + + if (lidar_data.point_cloud.size() > 3) { + lidar_msg.height = 1; + lidar_msg.width = lidar_data.point_cloud.size() / 3; + + lidar_msg.fields.resize(3); + lidar_msg.fields[0].name = "x"; + lidar_msg.fields[1].name = "y"; + lidar_msg.fields[2].name = "z"; + + int offset = 0; + + for (size_t d = 0; d < lidar_msg.fields.size(); ++d, offset += 4) { + lidar_msg.fields[d].offset = offset; + lidar_msg.fields[d].datatype = sensor_msgs::msg::PointField::FLOAT32; + lidar_msg.fields[d].count = 1; + } + + lidar_msg.is_bigendian = false; + lidar_msg.point_step = offset; // 4 * num fields + lidar_msg.row_step = lidar_msg.point_step * lidar_msg.width; + + lidar_msg.is_dense = true; // todo + std::vector data_std = lidar_data.point_cloud; + + const unsigned char* bytes = reinterpret_cast(data_std.data()); + vector lidar_msg_data(bytes, bytes + sizeof(float) * data_std.size()); + lidar_msg.data = std::move(lidar_msg_data); + } + else { + // msg = [] + } + + if (isENU_) { + try { + sensor_msgs::msg::PointCloud2 lidar_msg_enu; + auto transformStampedENU = tf_buffer_->lookupTransform(AIRSIM_FRAME_ID, vehicle_name, rclcpp::Time(0), rclcpp::Duration(1)); + tf2::doTransform(lidar_msg, lidar_msg_enu, transformStampedENU); + + lidar_msg_enu.header.stamp = lidar_msg.header.stamp; + lidar_msg_enu.header.frame_id = lidar_msg.header.frame_id; + + lidar_msg = std::move(lidar_msg_enu); + } + catch (tf2::TransformException& ex) { + RCLCPP_WARN(nh_->get_logger(), "%s", ex.what()); + rclcpp::Rate(1.0).sleep(); + } + } + + return lidar_msg; +} + +airsim_interfaces::msg::Environment AirsimROSWrapper::get_environment_msg_from_airsim(const msr::airlib::Environment::State& env_data) const +{ + airsim_interfaces::msg::Environment env_msg; + env_msg.position.x = env_data.position.x(); + env_msg.position.y = env_data.position.y(); + env_msg.position.z = env_data.position.z(); + env_msg.geo_point.latitude = env_data.geo_point.latitude; + env_msg.geo_point.longitude = env_data.geo_point.longitude; + env_msg.geo_point.altitude = env_data.geo_point.altitude; + env_msg.gravity.x = env_data.gravity.x(); + env_msg.gravity.y = env_data.gravity.y(); + env_msg.gravity.z = env_data.gravity.z(); + env_msg.air_pressure = env_data.air_pressure; + env_msg.temperature = env_data.temperature; + env_msg.air_density = env_data.temperature; + + return env_msg; +} + +sensor_msgs::msg::MagneticField AirsimROSWrapper::get_mag_msg_from_airsim(const msr::airlib::MagnetometerBase::Output& mag_data) const +{ + sensor_msgs::msg::MagneticField mag_msg; + mag_msg.magnetic_field.x = mag_data.magnetic_field_body.x(); + mag_msg.magnetic_field.y = mag_data.magnetic_field_body.y(); + mag_msg.magnetic_field.z = mag_data.magnetic_field_body.z(); + std::copy(std::begin(mag_data.magnetic_field_covariance), + std::end(mag_data.magnetic_field_covariance), + std::begin(mag_msg.magnetic_field_covariance)); + mag_msg.header.stamp = airsim_timestamp_to_ros(mag_data.time_stamp); + + return mag_msg; +} + +// todo covariances +sensor_msgs::msg::NavSatFix AirsimROSWrapper::get_gps_msg_from_airsim(const msr::airlib::GpsBase::Output& gps_data) const +{ + sensor_msgs::msg::NavSatFix gps_msg; + gps_msg.header.stamp = airsim_timestamp_to_ros(gps_data.time_stamp); + gps_msg.latitude = gps_data.gnss.geo_point.latitude; + gps_msg.longitude = gps_data.gnss.geo_point.longitude; + gps_msg.altitude = gps_data.gnss.geo_point.altitude; + gps_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GLONASS; + gps_msg.status.status = gps_data.gnss.fix_type; + // gps_msg.position_covariance_type = + // gps_msg.position_covariance = + + return gps_msg; +} + +sensor_msgs::msg::Range AirsimROSWrapper::get_range_from_airsim(const msr::airlib::DistanceSensorData& dist_data) const +{ + sensor_msgs::msg::Range dist_msg; + dist_msg.header.stamp = airsim_timestamp_to_ros(dist_data.time_stamp); + dist_msg.range = dist_data.distance; + dist_msg.min_range = dist_data.min_distance; + dist_msg.max_range = dist_data.min_distance; + + return dist_msg; +} + +airsim_interfaces::msg::Altimeter AirsimROSWrapper::get_altimeter_msg_from_airsim(const msr::airlib::BarometerBase::Output& alt_data) const +{ + airsim_interfaces::msg::Altimeter alt_msg; + alt_msg.header.stamp = airsim_timestamp_to_ros(alt_data.time_stamp); + alt_msg.altitude = alt_data.altitude; + alt_msg.pressure = alt_data.pressure; + alt_msg.qnh = alt_data.qnh; + + return alt_msg; +} + +// todo covariances +sensor_msgs::msg::Imu AirsimROSWrapper::get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data) const +{ + sensor_msgs::msg::Imu imu_msg; + // imu_msg.header.frame_id = "/airsim/odom_local_ned";// todo multiple drones + imu_msg.header.stamp = airsim_timestamp_to_ros(imu_data.time_stamp); + imu_msg.orientation.x = imu_data.orientation.x(); + imu_msg.orientation.y = imu_data.orientation.y(); + imu_msg.orientation.z = imu_data.orientation.z(); + imu_msg.orientation.w = imu_data.orientation.w(); + + // todo radians per second + imu_msg.angular_velocity.x = imu_data.angular_velocity.x(); + imu_msg.angular_velocity.y = imu_data.angular_velocity.y(); + imu_msg.angular_velocity.z = imu_data.angular_velocity.z(); + + // meters/s2^m + imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x(); + imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y(); + imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z(); + + // imu_msg.orientation_covariance = ; + // imu_msg.angular_velocity_covariance = ; + // imu_msg.linear_acceleration_covariance = ; + + return imu_msg; +} + +void AirsimROSWrapper::publish_odom_tf(const nav_msgs::msg::Odometry& odom_msg) +{ + geometry_msgs::msg::TransformStamped odom_tf; + odom_tf.header = odom_msg.header; + odom_tf.child_frame_id = odom_msg.child_frame_id; + odom_tf.transform.translation.x = odom_msg.pose.pose.position.x; + odom_tf.transform.translation.y = odom_msg.pose.pose.position.y; + odom_tf.transform.translation.z = odom_msg.pose.pose.position.z; + odom_tf.transform.rotation = odom_msg.pose.pose.orientation; + tf_broadcaster_->sendTransform(odom_tf); +} + +airsim_interfaces::msg::GPSYaw AirsimROSWrapper::get_gps_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const +{ + airsim_interfaces::msg::GPSYaw gps_msg; + gps_msg.latitude = geo_point.latitude; + gps_msg.longitude = geo_point.longitude; + gps_msg.altitude = geo_point.altitude; + return gps_msg; +} + +sensor_msgs::msg::NavSatFix AirsimROSWrapper::get_gps_sensor_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const +{ + sensor_msgs::msg::NavSatFix gps_msg; + gps_msg.latitude = geo_point.latitude; + gps_msg.longitude = geo_point.longitude; + gps_msg.altitude = geo_point.altitude; + return gps_msg; +} + +msr::airlib::GeoPoint AirsimROSWrapper::get_origin_geo_point() const +{ + HomeGeoPoint geo_point = AirSimSettings::singleton().origin_geopoint; + return geo_point.home_geo_point; +} + +VelCmd AirsimROSWrapper::get_airlib_world_vel_cmd(const airsim_interfaces::msg::VelCmd& msg) const +{ + VelCmd vel_cmd; + vel_cmd.x = msg.twist.linear.x; + vel_cmd.y = msg.twist.linear.y; + vel_cmd.z = msg.twist.linear.z; + vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + vel_cmd.yaw_mode.is_rate = true; + vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); + return vel_cmd; +} + +VelCmd AirsimROSWrapper::get_airlib_body_vel_cmd(const airsim_interfaces::msg::VelCmd& msg, const msr::airlib::Quaternionr& airlib_quat) const +{ + VelCmd vel_cmd; + double roll, pitch, yaw; + tf2::Matrix3x3(get_tf2_quat(airlib_quat)).getRPY(roll, pitch, yaw); // ros uses xyzw + + // todo do actual body frame? + vel_cmd.x = (msg.twist.linear.x * cos(yaw)) - (msg.twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll + vel_cmd.y = (msg.twist.linear.x * sin(yaw)) + (msg.twist.linear.y * cos(yaw)); //body frame + vel_cmd.z = msg.twist.linear.z; + vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + vel_cmd.yaw_mode.is_rate = true; + // airsim uses degrees + vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); + + return vel_cmd; +} + +geometry_msgs::msg::Transform AirsimROSWrapper::get_transform_msg_from_airsim(const msr::airlib::Vector3r& position, const msr::airlib::AirSimSettings::Rotation& rotation) +{ + geometry_msgs::msg::Transform transform; + transform.translation.x = position.x(); + transform.translation.y = position.y(); + transform.translation.z = position.z(); + tf2::Quaternion quat; + quat.setRPY(rotation.roll, rotation.pitch, rotation.yaw); + transform.rotation.x = quat.x(); + transform.rotation.y = quat.y(); + transform.rotation.z = quat.z(); + transform.rotation.w = quat.w(); + + return transform; +} + +geometry_msgs::msg::Transform AirsimROSWrapper::get_transform_msg_from_airsim(const msr::airlib::Vector3r& position, const msr::airlib::Quaternionr& quaternion) +{ + geometry_msgs::msg::Transform transform; + transform.translation.x = position.x(); + transform.translation.y = position.y(); + transform.translation.z = position.z(); + transform.rotation.x = quaternion.x(); + transform.rotation.y = quaternion.y(); + transform.rotation.z = quaternion.z(); + transform.rotation.w = quaternion.w(); + + return transform; +} + +rclcpp::Time AirsimROSWrapper::chrono_timestamp_to_ros(const std::chrono::system_clock::time_point& stamp) const +{ + + auto dur = std::chrono::duration_cast(stamp.time_since_epoch()); + rclcpp::Time cur_time(dur.count()); + return cur_time; +} + +rclcpp::Time AirsimROSWrapper::airsim_timestamp_to_ros(const msr::airlib::TTimePoint& stamp) const +{ + // airsim appears to use chrono::system_clock with nanosecond precision + std::chrono::nanoseconds dur(stamp); + std::chrono::time_point tp(dur); + rclcpp::Time cur_time = chrono_timestamp_to_ros(tp); + return cur_time; +} + +void AirsimROSWrapper::drone_state_timer_cb() +{ + try { + // todo this is global origin + origin_geo_point_pub_->publish(origin_geo_point_msg_); + + // get the basic vehicle pose and environmental state + const auto now = update_state(); + + // on init, will publish 0 to /clock as expected for use_sim_time compatibility + if (!airsim_client_->simIsPaused()) { + // airsim_client needs to provide the simulation time in a future version of the API + ros_clock_.clock = now; + } + // publish the simulation clock + if (publish_clock_) { + clock_pub_->publish(ros_clock_); + } + + // publish vehicle state, odom, and all basic sensor types + publish_vehicle_state(); + + // send any commands out to the vehicles + update_commands(); + } + catch (rpc::rpc_error& e) { + std::string msg = e.get_error().as(); + RCLCPP_ERROR(nh_->get_logger(), "Exception raised by the API:\n%s", msg); + } +} + +void AirsimROSWrapper::update_and_publish_static_transforms(VehicleROS* vehicle_ros) +{ + if (vehicle_ros && !vehicle_ros->static_tf_msg_vec_.empty()) { + for (auto& static_tf_msg : vehicle_ros->static_tf_msg_vec_) { + static_tf_msg.header.stamp = vehicle_ros->stamp_; + static_tf_pub_->sendTransform(static_tf_msg); + } + } +} + +rclcpp::Time AirsimROSWrapper::update_state() +{ + bool got_sim_time = false; + rclcpp::Time curr_ros_time = nh_->now(); + + //should be easier way to get the sim time through API, something like: + //msr::airlib::Environment::State env = airsim_client_->simGetGroundTruthEnvironment(""); + //curr_ros_time = airsim_timestamp_to_ros(env.clock().nowNanos()); + + // iterate over drones + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { + rclcpp::Time vehicle_time; + // get drone state from airsim + auto& vehicle_ros = vehicle_name_ptr_pair.second; + + // vehicle environment, we can get ambient temperature here and other truths + auto env_data = airsim_client_->simGetGroundTruthEnvironment(vehicle_ros->vehicle_name_); + + if (airsim_mode_ == AIRSIM_MODE::DRONE) { + auto drone = static_cast(vehicle_ros.get()); + auto rpc = static_cast(airsim_client_.get()); + drone->curr_drone_state_ = rpc->getMultirotorState(vehicle_ros->vehicle_name_); + + vehicle_time = airsim_timestamp_to_ros(drone->curr_drone_state_.timestamp); + if (!got_sim_time) { + curr_ros_time = vehicle_time; + got_sim_time = true; + } + + vehicle_ros->gps_sensor_msg_ = get_gps_sensor_msg_from_airsim_geo_point(drone->curr_drone_state_.gps_location); + vehicle_ros->gps_sensor_msg_.header.stamp = vehicle_time; + + vehicle_ros->curr_odom_ = get_odom_msg_from_multirotor_state(drone->curr_drone_state_); + } + else { + auto car = static_cast(vehicle_ros.get()); + auto rpc = static_cast(airsim_client_.get()); + car->curr_car_state_ = rpc->getCarState(vehicle_ros->vehicle_name_); + + vehicle_time = airsim_timestamp_to_ros(car->curr_car_state_.timestamp); + if (!got_sim_time) { + curr_ros_time = vehicle_time; + got_sim_time = true; + } + + vehicle_ros->gps_sensor_msg_ = get_gps_sensor_msg_from_airsim_geo_point(env_data.geo_point); + vehicle_ros->gps_sensor_msg_.header.stamp = vehicle_time; + + vehicle_ros->curr_odom_ = get_odom_msg_from_car_state(car->curr_car_state_); + + airsim_interfaces::msg::CarState state_msg = get_roscarstate_msg_from_car_state(car->curr_car_state_); + state_msg.header.frame_id = vehicle_ros->vehicle_name_; + car->car_state_msg_ = state_msg; + } + + vehicle_ros->stamp_ = vehicle_time; + + airsim_interfaces::msg::Environment env_msg = get_environment_msg_from_airsim(env_data); + env_msg.header.frame_id = vehicle_ros->vehicle_name_; + env_msg.header.stamp = vehicle_time; + vehicle_ros->env_msg_ = env_msg; + + // convert airsim drone state to ROS msgs + vehicle_ros->curr_odom_.header.frame_id = vehicle_ros->vehicle_name_; + vehicle_ros->curr_odom_.child_frame_id = vehicle_ros->odom_frame_id_; + vehicle_ros->curr_odom_.header.stamp = vehicle_time; + } + + return curr_ros_time; +} + +void AirsimROSWrapper::publish_vehicle_state() +{ + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { + auto& vehicle_ros = vehicle_name_ptr_pair.second; + + // simulation environment truth + vehicle_ros->env_pub_->publish(vehicle_ros->env_msg_); + + if (airsim_mode_ == AIRSIM_MODE::CAR) { + // dashboard reading from car, RPM, gear, etc + auto car = static_cast(vehicle_ros.get()); + car->car_state_pub_->publish(car->car_state_msg_); + } + + // odom and transforms + vehicle_ros->odom_local_pub_->publish(vehicle_ros->curr_odom_); + publish_odom_tf(vehicle_ros->curr_odom_); + + // ground truth GPS position from sim/HITL + vehicle_ros->global_gps_pub_->publish(vehicle_ros->gps_sensor_msg_); + + for (auto& sensor_publisher : vehicle_ros->barometer_pubs_) { + auto baro_data = airsim_client_->getBarometerData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name_); + airsim_interfaces::msg::Altimeter alt_msg = get_altimeter_msg_from_airsim(baro_data); + alt_msg.header.frame_id = vehicle_ros->vehicle_name_; + sensor_publisher.publisher->publish(alt_msg); + } + + for (auto& sensor_publisher : vehicle_ros->imu_pubs_) { + auto imu_data = airsim_client_->getImuData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name_); + sensor_msgs::msg::Imu imu_msg = get_imu_msg_from_airsim(imu_data); + imu_msg.header.frame_id = vehicle_ros->vehicle_name_; + sensor_publisher.publisher->publish(imu_msg); + } + for (auto& sensor_publisher : vehicle_ros->distance_pubs_) { + auto distance_data = airsim_client_->getDistanceSensorData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name_); + sensor_msgs::msg::Range dist_msg = get_range_from_airsim(distance_data); + dist_msg.header.frame_id = vehicle_ros->vehicle_name_; + sensor_publisher.publisher->publish(dist_msg); + } + for (auto& sensor_publisher : vehicle_ros->gps_pubs_) { + auto gps_data = airsim_client_->getGpsData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name_); + sensor_msgs::msg::NavSatFix gps_msg = get_gps_msg_from_airsim(gps_data); + gps_msg.header.frame_id = vehicle_ros->vehicle_name_; + sensor_publisher.publisher->publish(gps_msg); + } + for (auto& sensor_publisher : vehicle_ros->magnetometer_pubs_) { + auto mag_data = airsim_client_->getMagnetometerData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name_); + sensor_msgs::msg::MagneticField mag_msg = get_mag_msg_from_airsim(mag_data); + mag_msg.header.frame_id = vehicle_ros->vehicle_name_; + sensor_publisher.publisher->publish(mag_msg); + } + + update_and_publish_static_transforms(vehicle_ros.get()); + } +} + +void AirsimROSWrapper::update_commands() +{ + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { + auto& vehicle_ros = vehicle_name_ptr_pair.second; + + if (airsim_mode_ == AIRSIM_MODE::DRONE) { + auto drone = static_cast(vehicle_ros.get()); + + // send control commands from the last callback to airsim + if (drone->has_vel_cmd_) { + std::lock_guard guard(control_mutex_); + static_cast(airsim_client_.get())->moveByVelocityAsync(drone->vel_cmd_.x, drone->vel_cmd_.y, drone->vel_cmd_.z, vel_cmd_duration_, msr::airlib::DrivetrainType::MaxDegreeOfFreedom, drone->vel_cmd_.yaw_mode, drone->vehicle_name_); + } + drone->has_vel_cmd_ = false; + } + else { + // send control commands from the last callback to airsim + auto car = static_cast(vehicle_ros.get()); + if (car->has_car_cmd_) { + std::lock_guard guard(control_mutex_); + static_cast(airsim_client_.get())->setCarControls(car->car_cmd_, vehicle_ros->vehicle_name_); + } + car->has_car_cmd_ = false; + } + } + + // Only camera rotation, no translation movement of camera + if (has_gimbal_cmd_) { + std::lock_guard guard(control_mutex_); + airsim_client_->simSetCameraPose(gimbal_cmd_.camera_name, get_airlib_pose(0, 0, 0, gimbal_cmd_.target_quat), gimbal_cmd_.vehicle_name); + } + + has_gimbal_cmd_ = false; +} + +// airsim uses nans for zeros in settings.json. we set them to zeros here for handling tfs in ROS +void AirsimROSWrapper::set_nans_to_zeros_in_pose(VehicleSetting& vehicle_setting) const +{ + if (std::isnan(vehicle_setting.position.x())) + vehicle_setting.position.x() = 0.0; + + if (std::isnan(vehicle_setting.position.y())) + vehicle_setting.position.y() = 0.0; + + if (std::isnan(vehicle_setting.position.z())) + vehicle_setting.position.z() = 0.0; + + if (std::isnan(vehicle_setting.rotation.yaw)) + vehicle_setting.rotation.yaw = 0.0; + + if (std::isnan(vehicle_setting.rotation.pitch)) + vehicle_setting.rotation.pitch = 0.0; + + if (std::isnan(vehicle_setting.rotation.roll)) + vehicle_setting.rotation.roll = 0.0; +} + +// if any nan's in camera pose, set them to match vehicle pose (which has already converted any potential nans to zeros) +void AirsimROSWrapper::set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, CameraSetting& camera_setting) const +{ + if (std::isnan(camera_setting.position.x())) + camera_setting.position.x() = vehicle_setting.position.x(); + + if (std::isnan(camera_setting.position.y())) + camera_setting.position.y() = vehicle_setting.position.y(); + + if (std::isnan(camera_setting.position.z())) + camera_setting.position.z() = vehicle_setting.position.z(); + + if (std::isnan(camera_setting.rotation.yaw)) + camera_setting.rotation.yaw = vehicle_setting.rotation.yaw; + + if (std::isnan(camera_setting.rotation.pitch)) + camera_setting.rotation.pitch = vehicle_setting.rotation.pitch; + + if (std::isnan(camera_setting.rotation.roll)) + camera_setting.rotation.roll = vehicle_setting.rotation.roll; +} + +void AirsimROSWrapper::convert_tf_msg_to_enu(geometry_msgs::msg::TransformStamped& tf_msg) +{ + std::swap(tf_msg.transform.translation.x, tf_msg.transform.translation.y); + std::swap(tf_msg.transform.rotation.x, tf_msg.transform.rotation.y); + tf_msg.transform.translation.z = -tf_msg.transform.translation.z; + tf_msg.transform.rotation.z = -tf_msg.transform.rotation.z; +} + +void AirsimROSWrapper::append_static_vehicle_tf(VehicleROS* vehicle_ros, const VehicleSetting& vehicle_setting) +{ + geometry_msgs::msg::TransformStamped vehicle_tf_msg; + vehicle_tf_msg.header.frame_id = world_frame_id_; + vehicle_tf_msg.header.stamp = nh_->now(); + vehicle_tf_msg.child_frame_id = vehicle_ros->vehicle_name_; + vehicle_tf_msg.transform = get_transform_msg_from_airsim(vehicle_setting.position, vehicle_setting.rotation); + + if (isENU_) { + convert_tf_msg_to_enu(vehicle_tf_msg); + } + + vehicle_ros->static_tf_msg_vec_.emplace_back(vehicle_tf_msg); +} + +void AirsimROSWrapper::append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const msr::airlib::LidarSimpleParams& lidar_setting) +{ + geometry_msgs::msg::TransformStamped lidar_tf_msg; + lidar_tf_msg.header.frame_id = vehicle_ros->vehicle_name_ + "/" + odom_frame_id_; + lidar_tf_msg.child_frame_id = vehicle_ros->vehicle_name_ + "/" + lidar_name; + lidar_tf_msg.transform = get_transform_msg_from_airsim(lidar_setting.relative_pose.position, lidar_setting.relative_pose.orientation); + + if (isENU_) { + convert_tf_msg_to_enu(lidar_tf_msg); + } + + vehicle_ros->static_tf_msg_vec_.emplace_back(lidar_tf_msg); +} + +void AirsimROSWrapper::append_static_camera_tf(VehicleROS* vehicle_ros, const std::string& camera_name, const CameraSetting& camera_setting) +{ + geometry_msgs::msg::TransformStamped static_cam_tf_body_msg; + static_cam_tf_body_msg.header.frame_id = vehicle_ros->vehicle_name_ + "/" + odom_frame_id_; + static_cam_tf_body_msg.child_frame_id = camera_name + "_body/static"; + static_cam_tf_body_msg.transform = get_transform_msg_from_airsim(camera_setting.position, camera_setting.rotation); + + if (isENU_) { + convert_tf_msg_to_enu(static_cam_tf_body_msg); + } + + geometry_msgs::msg::TransformStamped static_cam_tf_optical_msg = static_cam_tf_body_msg; + static_cam_tf_optical_msg.child_frame_id = camera_name + "_optical/static"; + + tf2::Quaternion quat_cam_body; + tf2::Quaternion quat_cam_optical; + tf2::convert(static_cam_tf_body_msg.transform.rotation, quat_cam_body); + tf2::Matrix3x3 mat_cam_body(quat_cam_body); + tf2::Matrix3x3 mat_cam_optical; + mat_cam_optical.setValue(mat_cam_body.getColumn(1).getX(), mat_cam_body.getColumn(2).getX(), mat_cam_body.getColumn(0).getX(), mat_cam_body.getColumn(1).getY(), mat_cam_body.getColumn(2).getY(), mat_cam_body.getColumn(0).getY(), mat_cam_body.getColumn(1).getZ(), mat_cam_body.getColumn(2).getZ(), mat_cam_body.getColumn(0).getZ()); + mat_cam_optical.getRotation(quat_cam_optical); + quat_cam_optical.normalize(); + tf2::convert(quat_cam_optical, static_cam_tf_optical_msg.transform.rotation); + + vehicle_ros->static_tf_msg_vec_.emplace_back(static_cam_tf_body_msg); + vehicle_ros->static_tf_msg_vec_.emplace_back(static_cam_tf_optical_msg); +} + +void AirsimROSWrapper::img_response_timer_cb() +{ + try { + int image_response_idx = 0; + for (const auto& airsim_img_request_vehicle_name_pair : airsim_img_request_vehicle_name_pair_vec_) { + const std::vector& img_response = airsim_client_images_.simGetImages(airsim_img_request_vehicle_name_pair.first, airsim_img_request_vehicle_name_pair.second); + + if (img_response.size() == airsim_img_request_vehicle_name_pair.first.size()) { + process_and_publish_img_response(img_response, image_response_idx, airsim_img_request_vehicle_name_pair.second); + image_response_idx += img_response.size(); + } + } + } + + catch (rpc::rpc_error& e) { + std::string msg = e.get_error().as(); + RCLCPP_ERROR(nh_->get_logger(), "Exception raised by the API, didn't get image response.\n%s", msg); + } +} + +void AirsimROSWrapper::lidar_timer_cb() +{ + try { + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { + if (!vehicle_name_ptr_pair.second->lidar_pubs_.empty()) { + for (auto& lidar_publisher : vehicle_name_ptr_pair.second->lidar_pubs_) { + auto lidar_data = airsim_client_lidar_.getLidarData(lidar_publisher.sensor_name, vehicle_name_ptr_pair.first); + sensor_msgs::msg::PointCloud2 lidar_msg = get_lidar_msg_from_airsim(lidar_data, vehicle_name_ptr_pair.first); + lidar_publisher.publisher->publish(lidar_msg); + } + } + } + } + catch (rpc::rpc_error& e) { + std::string msg = e.get_error().as(); + RCLCPP_ERROR(nh_->get_logger(), "Exception raised by the API, didn't get image response.\n%s", msg); + } +} + +cv::Mat AirsimROSWrapper::manual_decode_depth(const ImageResponse& img_response) const +{ + cv::Mat mat(img_response.height, img_response.width, CV_32FC1, cv::Scalar(0)); + int img_width = img_response.width; + + for (int row = 0; row < img_response.height; row++) + for (int col = 0; col < img_width; col++) + mat.at(row, col) = img_response.image_data_float[row * img_width + col]; + return mat; +} + +std::shared_ptr AirsimROSWrapper::get_img_msg_from_response(const ImageResponse& img_response, + const rclcpp::Time curr_ros_time, + const std::string frame_id) +{ + unused(curr_ros_time); + std::shared_ptr img_msg_ptr = std::make_shared(); + img_msg_ptr->data = img_response.image_data_uint8; + img_msg_ptr->step = img_response.image_data_uint8.size() / img_response.height; + img_msg_ptr->header.stamp = airsim_timestamp_to_ros(img_response.time_stamp); + img_msg_ptr->header.frame_id = frame_id; + img_msg_ptr->height = img_response.height; + img_msg_ptr->width = img_response.width; + img_msg_ptr->encoding = "bgr8"; + if (is_vulkan_) + img_msg_ptr->encoding = "rgb8"; + img_msg_ptr->is_bigendian = 0; + return img_msg_ptr; +} + +std::shared_ptr AirsimROSWrapper::get_depth_img_msg_from_response(const ImageResponse& img_response, + const rclcpp::Time curr_ros_time, + const std::string frame_id) +{ + unused(curr_ros_time); + // todo using img_response.image_data_float direclty as done get_img_msg_from_response() throws an error, + // hence the dependency on opencv and cv_bridge. however, this is an extremely fast op, so no big deal. + cv::Mat depth_img = manual_decode_depth(img_response); + std::shared_ptr depth_img_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "32FC1", depth_img).toImageMsg(); + depth_img_msg->header.stamp = airsim_timestamp_to_ros(img_response.time_stamp); + depth_img_msg->header.frame_id = frame_id; + return depth_img_msg; +} + +// todo have a special stereo pair mode and get projection matrix by calculating offset wrt drone body frame? +sensor_msgs::msg::CameraInfo AirsimROSWrapper::generate_cam_info(const std::string& camera_name, + const CameraSetting& camera_setting, + const CaptureSetting& capture_setting) const +{ + unused(camera_setting); + sensor_msgs::msg::CameraInfo cam_info_msg; + cam_info_msg.header.frame_id = camera_name + "_optical"; + cam_info_msg.height = capture_setting.height; + cam_info_msg.width = capture_setting.width; + float f_x = (capture_setting.width / 2.0) / tan(math_common::deg2rad(capture_setting.fov_degrees / 2.0)); + // todo focal length in Y direction should be same as X it seems. this can change in future a scene capture component which exactly correponds to a cine camera + // float f_y = (capture_setting.height / 2.0) / tan(math_common::deg2rad(fov_degrees / 2.0)); + cam_info_msg.k = { f_x, 0.0, capture_setting.width / 2.0, 0.0, f_x, capture_setting.height / 2.0, 0.0, 0.0, 1.0 }; + cam_info_msg.p = { f_x, 0.0, capture_setting.width / 2.0, 0.0, 0.0, f_x, capture_setting.height / 2.0, 0.0, 0.0, 0.0, 1.0, 0.0 }; + return cam_info_msg; +} + +void AirsimROSWrapper::process_and_publish_img_response(const std::vector& img_response_vec, const int img_response_idx, const std::string& vehicle_name) +{ + // todo add option to use airsim time (image_response.TTimePoint) like Gazebo /use_sim_time param + rclcpp::Time curr_ros_time = nh_->now(); + int img_response_idx_internal = img_response_idx; + + for (const auto& curr_img_response : img_response_vec) { + // todo publishing a tf for each capture type seems stupid. but it foolproofs us against render thread's async stuff, I hope. + // Ideally, we should loop over cameras and then captures, and publish only one tf. + publish_camera_tf(curr_img_response, curr_ros_time, vehicle_name, curr_img_response.camera_name); + + // todo simGetCameraInfo is wrong + also it's only for image type -1. + // msr::airlib::CameraInfo camera_info = airsim_client_.simGetCameraInfo(curr_img_response.camera_name); + + // update timestamp of saved cam info msgs + + camera_info_msg_vec_[img_response_idx_internal].header.stamp = airsim_timestamp_to_ros(curr_img_response.time_stamp); + cam_info_pub_vec_[img_response_idx_internal]->publish(camera_info_msg_vec_[img_response_idx_internal]); + + // DepthPlanar / DepthPerspective / DepthVis / DisparityNormalized + if (curr_img_response.pixels_as_float) { + image_pub_vec_[img_response_idx_internal].publish(get_depth_img_msg_from_response(curr_img_response, + curr_ros_time, + curr_img_response.camera_name + "_optical")); + } + // Scene / Segmentation / SurfaceNormals / Infrared + else { + image_pub_vec_[img_response_idx_internal].publish(get_img_msg_from_response(curr_img_response, + curr_ros_time, + curr_img_response.camera_name + "_optical")); + } + img_response_idx_internal++; + } +} + +// publish camera transforms +// camera poses are obtained from airsim's client API which are in (local) NED frame. +// We first do a change of basis to camera optical frame (Z forward, X right, Y down) +void AirsimROSWrapper::publish_camera_tf(const ImageResponse& img_response, const rclcpp::Time& ros_time, const std::string& frame_id, const std::string& child_frame_id) +{ + unused(ros_time); + geometry_msgs::msg::TransformStamped cam_tf_body_msg; + cam_tf_body_msg.header.stamp = airsim_timestamp_to_ros(img_response.time_stamp); + cam_tf_body_msg.header.frame_id = frame_id; + cam_tf_body_msg.child_frame_id = child_frame_id + "_body"; + cam_tf_body_msg.transform = get_transform_msg_from_airsim(img_response.camera_position, img_response.camera_orientation); + + if (isENU_) { + convert_tf_msg_to_enu(cam_tf_body_msg); + } + + geometry_msgs::msg::TransformStamped cam_tf_optical_msg; + cam_tf_optical_msg.header.stamp = airsim_timestamp_to_ros(img_response.time_stamp); + cam_tf_optical_msg.header.frame_id = frame_id; + cam_tf_optical_msg.child_frame_id = child_frame_id + "_optical"; + cam_tf_optical_msg.transform.translation = cam_tf_body_msg.transform.translation; + + tf2::Quaternion quat_cam_body; + tf2::Quaternion quat_cam_optical; + tf2::convert(cam_tf_body_msg.transform.rotation, quat_cam_body); + tf2::Matrix3x3 mat_cam_body(quat_cam_body); + // tf2::Matrix3x3 mat_cam_optical = matrix_cam_body_to_optical_ * mat_cam_body * matrix_cam_body_to_optical_inverse_; + // tf2::Matrix3x3 mat_cam_optical = matrix_cam_body_to_optical_ * mat_cam_body; + tf2::Matrix3x3 mat_cam_optical; + mat_cam_optical.setValue(mat_cam_body.getColumn(1).getX(), mat_cam_body.getColumn(2).getX(), mat_cam_body.getColumn(0).getX(), mat_cam_body.getColumn(1).getY(), mat_cam_body.getColumn(2).getY(), mat_cam_body.getColumn(0).getY(), mat_cam_body.getColumn(1).getZ(), mat_cam_body.getColumn(2).getZ(), mat_cam_body.getColumn(0).getZ()); + mat_cam_optical.getRotation(quat_cam_optical); + quat_cam_optical.normalize(); + tf2::convert(quat_cam_optical, cam_tf_optical_msg.transform.rotation); + + tf_broadcaster_->sendTransform(cam_tf_body_msg); + tf_broadcaster_->sendTransform(cam_tf_optical_msg); +} + +void AirsimROSWrapper::convert_yaml_to_simple_mat(const YAML::Node& node, SimpleMatrix& m) const +{ + int rows, cols; + rows = node["rows"].as(); + cols = node["cols"].as(); + const YAML::Node& data = node["data"]; + for (int i = 0; i < rows * cols; ++i) { + m.data[i] = data[i].as(); + } +} + +void AirsimROSWrapper::read_params_from_yaml_and_fill_cam_info_msg(const std::string& file_name, sensor_msgs::msg::CameraInfo& cam_info) const +{ + std::ifstream fin(file_name.c_str()); + YAML::Node doc = YAML::Load(fin); + + cam_info.width = doc[WIDTH_YML_NAME].as(); + cam_info.height = doc[HEIGHT_YML_NAME].as(); + + SimpleMatrix K_(3, 3, &cam_info.k[0]); + convert_yaml_to_simple_mat(doc[K_YML_NAME], K_); + SimpleMatrix R_(3, 3, &cam_info.r[0]); + convert_yaml_to_simple_mat(doc[R_YML_NAME], R_); + SimpleMatrix P_(3, 4, &cam_info.p[0]); + convert_yaml_to_simple_mat(doc[P_YML_NAME], P_); + + cam_info.distortion_model = doc[DMODEL_YML_NAME].as(); + + const YAML::Node& D_node = doc[D_YML_NAME]; + int D_rows, D_cols; + D_rows = D_node["rows"].as(); + D_cols = D_node["cols"].as(); + const YAML::Node& D_data = D_node["data"]; + cam_info.d.resize(D_rows * D_cols); + for (int i = 0; i < D_rows * D_cols; ++i) { + cam_info.d[i] = D_data[i].as(); + } +} \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_settings_parser.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_settings_parser.cpp new file mode 100755 index 0000000000..f907e0c9e1 --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/src/airsim_settings_parser.cpp @@ -0,0 +1,43 @@ +#include "airsim_settings_parser.h" + +AirSimSettingsParser::AirSimSettingsParser(const std::string& host_ip) + : host_ip_(host_ip) +{ + success_ = initializeSettings(); +} + +bool AirSimSettingsParser::success() +{ + return success_; +} + +bool AirSimSettingsParser::getSettingsText(std::string& settings_text) const +{ + msr::airlib::RpcLibClientBase airsim_client(host_ip_); + airsim_client.confirmConnection(); + + settings_text = airsim_client.getSettingsString(); + + return !settings_text.empty(); +} + +std::string AirSimSettingsParser::getSimMode() +{ + Settings& settings_json = Settings::loadJSonString(settings_text_); + return settings_json.getString("SimMode", ""); +} + +// mimics void ASimHUD::initializeSettings() +bool AirSimSettingsParser::initializeSettings() +{ + if (getSettingsText(settings_text_)) { + AirSimSettings::initializeSettings(settings_text_); + + AirSimSettings::singleton().load(std::bind(&AirSimSettingsParser::getSimMode, this)); + std::cout << "SimMode: " << AirSimSettings::singleton().simmode_name << std::endl; + + return true; + } + + return false; +} \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp b/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp new file mode 100644 index 0000000000..51fa4d565c --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp @@ -0,0 +1,352 @@ +#include "pd_position_controller_simple.h" +using namespace std::placeholders; + +bool PIDParams::load_from_rosparams(const shared_ptr nh) +{ + bool found = true; + + found = found && nh->get_parameter("kp_x", kp_x); + found = found && nh->get_parameter("kp_y", kp_y); + found = found && nh->get_parameter("kp_z", kp_z); + found = found && nh->get_parameter("kp_yaw", kp_yaw); + + found = found && nh->get_parameter("kd_x", kd_x); + found = found && nh->get_parameter("kd_y", kd_y); + found = found && nh->get_parameter("kd_z", kd_z); + found = found && nh->get_parameter("kd_yaw", kd_yaw); + + found = found && nh->get_parameter("reached_thresh_xyz", reached_thresh_xyz); + found = found && nh->get_parameter("reached_yaw_degrees", reached_yaw_degrees); + + return found; +} + +bool DynamicConstraints::load_from_rosparams(const std::shared_ptr nh) +{ + bool found = true; + + found = found && nh->get_parameter("max_vel_horz_abs", max_vel_horz_abs); + found = found && nh->get_parameter("max_vel_vert_abs", max_vel_vert_abs); + found = found && nh->get_parameter("max_yaw_rate_degree", max_yaw_rate_degree); + + return found; +} + +PIDPositionController::PIDPositionController(const std::shared_ptr nh) + : use_eth_lib_for_geodetic_conv_(true), nh_(nh), has_home_geo_(false), reached_goal_(false), has_goal_(false), has_odom_(false), got_goal_once_(false) +{ + params_.load_from_rosparams(nh_); + constraints_.load_from_rosparams(nh_); + initialize_ros(); + reset_errors(); +} + +void PIDPositionController::reset_errors() +{ + prev_error_.x = 0.0; + prev_error_.y = 0.0; + prev_error_.z = 0.0; + prev_error_.yaw = 0.0; +} + +void PIDPositionController::initialize_ros() +{ + vel_cmd_ = airsim_interfaces::msg::VelCmd(); + // ROS params + double update_control_every_n_sec; + nh_->get_parameter("update_control_every_n_sec", update_control_every_n_sec); + + auto parameters_client = std::make_shared(nh_, "/airsim_node"); + while (!parameters_client->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(nh_->get_logger(), "Interrupted while waiting for the service. Exiting."); + rclcpp::shutdown(); + } + RCLCPP_INFO(nh_->get_logger(), "service not available, waiting again..."); + } + + std::string vehicle_name; + + while (vehicle_name == "") { + vehicle_name = parameters_client->get_parameter("vehicle_name", vehicle_name); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Waiting vehicle name"); + } + + // ROS publishers + airsim_vel_cmd_world_frame_pub_ = nh_->create_publisher("/airsim_node/" + vehicle_name + "/vel_cmd_world_frame", 1); + + // ROS subscribers + airsim_odom_sub_ = nh_->create_subscription("/airsim_node/" + vehicle_name + "/odom_local_ned", 50, std::bind(&PIDPositionController::airsim_odom_cb, this, _1)); + home_geopoint_sub_ = nh_->create_subscription("/airsim_node/home_geo_point", 50, std::bind(&PIDPositionController::home_geopoint_cb, this, _1)); + // todo publish this under global nodehandle / "airsim node" and hide it from user + local_position_goal_srvr_ = nh_->create_service("/airsim_node/local_position_goal", std::bind(&PIDPositionController::local_position_goal_srv_cb, this, _1, _2)); + local_position_goal_override_srvr_ = nh_->create_service("/airsim_node/local_position_goal/override", std::bind(&PIDPositionController::local_position_goal_srv_override_cb, this, _1, _2)); + gps_goal_srvr_ = nh_->create_service("/airsim_node/gps_goal", std::bind(&PIDPositionController::gps_goal_srv_cb, this, _1, _2)); + gps_goal_override_srvr_ = nh_->create_service("/airsim_node/gps_goal/override", std::bind(&PIDPositionController::gps_goal_srv_override_cb, this, _1, _2)); + + // ROS timers + update_control_cmd_timer_ = nh_->create_wall_timer(std::chrono::duration(update_control_every_n_sec), std::bind(&PIDPositionController::update_control_cmd_timer_cb, this)); +} + +void PIDPositionController::airsim_odom_cb(const nav_msgs::msg::Odometry::SharedPtr odom_msg) +{ + has_odom_ = true; + curr_odom_ = *odom_msg; + curr_position_.x = odom_msg->pose.pose.position.x; + curr_position_.y = odom_msg->pose.pose.position.y; + curr_position_.z = odom_msg->pose.pose.position.z; + curr_position_.yaw = utils::get_yaw_from_quat_msg(odom_msg->pose.pose.orientation); +} + +// todo maintain internal representation as eigen vec? +// todo check if low velocity if within thresh? +// todo maintain separate errors for XY and Z +void PIDPositionController::check_reached_goal() +{ + double diff_xyz = sqrt((target_position_.x - curr_position_.x) * (target_position_.x - curr_position_.x) + (target_position_.y - curr_position_.y) * (target_position_.y - curr_position_.y) + (target_position_.z - curr_position_.z) * (target_position_.z - curr_position_.z)); + + double diff_yaw = math_common::angular_dist(target_position_.yaw, curr_position_.yaw); + + // todo save this in degrees somewhere to avoid repeated conversion + if (diff_xyz < params_.reached_thresh_xyz && diff_yaw < math_common::deg2rad(params_.reached_yaw_degrees)) + reached_goal_ = true; +} + +bool PIDPositionController::local_position_goal_srv_cb(const std::shared_ptr request, std::shared_ptr response) +{ + unused(response); + // this tells the update timer callback to not do active hovering + if (!got_goal_once_) + got_goal_once_ = true; + + if (has_goal_ && !reached_goal_) { + // todo maintain array of position goals + RCLCPP_ERROR_STREAM(nh_->get_logger(), "denying position goal request-> I am still following the previous goal"); + return false; + } + + if (!has_goal_) { + target_position_.x = request->x; + target_position_.y = request->y; + target_position_.z = request->z; + target_position_.yaw = request->yaw; + RCLCPP_INFO_STREAM(nh_->get_logger(), "got goal: x=" << target_position_.x << " y=" << target_position_.y << " z=" << target_position_.z << " yaw=" << target_position_.yaw); + + // todo error checks + // todo fill response + has_goal_ = true; + reached_goal_ = false; + reset_errors(); // todo + return true; + } + + // Already have goal, and have reached it + RCLCPP_INFO_STREAM(nh_->get_logger(), "Already have goal and have reached it"); + return false; +} + +bool PIDPositionController::local_position_goal_srv_override_cb(const std::shared_ptr request, std::shared_ptr response) +{ + unused(response); + // this tells the update timer callback to not do active hovering + if (!got_goal_once_) + got_goal_once_ = true; + + target_position_.x = request->x; + target_position_.y = request->y; + target_position_.z = request->z; + target_position_.yaw = request->yaw; + RCLCPP_INFO_STREAM(nh_->get_logger(), "got goal: x=" << target_position_.x << " y=" << target_position_.y << " z=" << target_position_.z << " yaw=" << target_position_.yaw); + + // todo error checks + // todo fill response + has_goal_ = true; + reached_goal_ = false; + reset_errors(); // todo + return true; +} + +void PIDPositionController::home_geopoint_cb(const airsim_interfaces::msg::GPSYaw::SharedPtr gps_msg) +{ + if (has_home_geo_) + return; + gps_home_msg_ = *gps_msg; + has_home_geo_ = true; + RCLCPP_INFO_STREAM(nh_->get_logger(), "GPS reference initializing " << gps_msg->latitude << ", " << gps_msg->longitude << ", " << gps_msg->altitude); + geodetic_converter_.initialiseReference(gps_msg->latitude, gps_msg->longitude, gps_msg->altitude); +} + +// todo do relative altitude, or add an option for the same? +bool PIDPositionController::gps_goal_srv_cb(const std::shared_ptr request, std::shared_ptr response) +{ + if (!has_home_geo_) { + RCLCPP_ERROR_STREAM(nh_->get_logger(), "I don't have home GPS coord. Can't go to GPS goal!"); + response->success = false; + } + + // convert GPS goal to NED goal + + if (!has_goal_) { + msr::airlib::GeoPoint goal_gps_point(request->latitude, request->longitude, request->altitude); + msr::airlib::GeoPoint gps_home(gps_home_msg_.latitude, gps_home_msg_.longitude, gps_home_msg_.altitude); + if (use_eth_lib_for_geodetic_conv_) { + double initial_latitude, initial_longitude, initial_altitude; + geodetic_converter_.getReference(&initial_latitude, &initial_longitude, &initial_altitude); + double n, e, d; + geodetic_converter_.geodetic2Ned(request->latitude, request->longitude, request->altitude, &n, &e, &d); + // RCLCPP_INFO_STREAM("geodetic_converter_ GPS reference initialized correctly (lat long in radians) " << initial_latitude << ", "<< initial_longitude << ", " << initial_altitude); + target_position_.x = n; + target_position_.y = e; + target_position_.z = d; + } + else // use airlib::GeodeticToNedFast + { + RCLCPP_INFO_STREAM(nh_->get_logger(), "home geopoint: lat=" << gps_home.latitude << " long=" << gps_home.longitude << " alt=" << gps_home.altitude << " yaw=" + << "todo"); + msr::airlib::Vector3r ned_goal = msr::airlib::EarthUtils::GeodeticToNedFast(goal_gps_point, gps_home); + target_position_.x = ned_goal[0]; + target_position_.y = ned_goal[1]; + target_position_.z = ned_goal[2]; + } + + target_position_.yaw = request->yaw; // todo + RCLCPP_INFO_STREAM(nh_->get_logger(), "got GPS goal: lat=" << goal_gps_point.latitude << " long=" << goal_gps_point.longitude << " alt=" << goal_gps_point.altitude << " yaw=" << target_position_.yaw); + RCLCPP_INFO_STREAM(nh_->get_logger(), "converted NED goal is: x=" << target_position_.x << " y=" << target_position_.y << " z=" << target_position_.z << " yaw=" << target_position_.yaw); + + // todo error checks + // todo fill response + has_goal_ = true; + reached_goal_ = false; + reset_errors(); // todo + return true; + } + + // Already have goal, this shouldn't happen + RCLCPP_INFO_STREAM(nh_->get_logger(), "Goal already received, ignoring!"); + return false; +} + +// todo do relative altitude, or add an option for the same? +bool PIDPositionController::gps_goal_srv_override_cb(const std::shared_ptr request, std::shared_ptr response) +{ + if (!has_home_geo_) { + RCLCPP_ERROR_STREAM(nh_->get_logger(), "I don't have home GPS coord. Can't go to GPS goal!"); + response->success = false; + } + + // convert GPS goal to NED goal + + msr::airlib::GeoPoint goal_gps_point(request->latitude, request->longitude, request->altitude); + msr::airlib::GeoPoint gps_home(gps_home_msg_.latitude, gps_home_msg_.longitude, gps_home_msg_.altitude); + if (use_eth_lib_for_geodetic_conv_) { + double initial_latitude, initial_longitude, initial_altitude; + geodetic_converter_.getReference(&initial_latitude, &initial_longitude, &initial_altitude); + double n, e, d; + geodetic_converter_.geodetic2Ned(request->latitude, request->longitude, request->altitude, &n, &e, &d); + // RCLCPP_INFO_STREAM("geodetic_converter_ GPS reference initialized correctly (lat long in radians) " << initial_latitude << ", "<< initial_longitude << ", " << initial_altitude); + target_position_.x = n; + target_position_.y = e; + target_position_.z = d; + } + else // use airlib::GeodeticToNedFast + { + RCLCPP_INFO_STREAM(nh_->get_logger(), "home geopoint: lat=" << gps_home.latitude << " long=" << gps_home.longitude << " alt=" << gps_home.altitude << " yaw=" + << "todo"); + msr::airlib::Vector3r ned_goal = msr::airlib::EarthUtils::GeodeticToNedFast(goal_gps_point, gps_home); + target_position_.x = ned_goal[0]; + target_position_.y = ned_goal[1]; + target_position_.z = ned_goal[2]; + } + + target_position_.yaw = request->yaw; // todo + RCLCPP_INFO_STREAM(nh_->get_logger(), "got GPS goal: lat=" << goal_gps_point.latitude << " long=" << goal_gps_point.longitude << " alt=" << goal_gps_point.altitude << " yaw=" << target_position_.yaw); + RCLCPP_INFO_STREAM(nh_->get_logger(), "converted NED goal is: x=" << target_position_.x << " y=" << target_position_.y << " z=" << target_position_.z << " yaw=" << target_position_.yaw); + + // todo error checks + // todo fill response + has_goal_ = true; + reached_goal_ = false; + reset_errors(); // todo + return true; +} + +void PIDPositionController::update_control_cmd_timer_cb() +{ + // todo check if odometry is too old!! + // if no odom, don't do anything. + if (!has_odom_) { + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Waiting for odometry!"); + return; + } + + if (has_goal_) { + check_reached_goal(); + if (reached_goal_) { + RCLCPP_INFO_STREAM(nh_->get_logger(), "Reached goal! Hovering at position."); + has_goal_ = false; + // dear future self, this function doesn't return coz we need to keep on actively hovering at last goal pose. don't act smart + } + else { + RCLCPP_INFO_STREAM(nh_->get_logger(), "Moving to goal."); + } + } + + // only compute and send control commands for hovering / moving to pose, if we received a goal at least once in the past + if (got_goal_once_) { + compute_control_cmd(); + enforce_dynamic_constraints(); + publish_control_cmd(); + } +} + +void PIDPositionController::compute_control_cmd() +{ + curr_error_.x = target_position_.x - curr_position_.x; + curr_error_.y = target_position_.y - curr_position_.y; + curr_error_.z = target_position_.z - curr_position_.z; + curr_error_.yaw = math_common::angular_dist(curr_position_.yaw, target_position_.yaw); + + double p_term_x = params_.kp_x * curr_error_.x; + double p_term_y = params_.kp_y * curr_error_.y; + double p_term_z = params_.kp_z * curr_error_.z; + double p_term_yaw = params_.kp_yaw * curr_error_.yaw; + + double d_term_x = params_.kd_x * prev_error_.x; + double d_term_y = params_.kd_y * prev_error_.y; + double d_term_z = params_.kd_z * prev_error_.z; + double d_term_yaw = params_.kp_yaw * prev_error_.yaw; + + prev_error_ = curr_error_; + + vel_cmd_.twist.linear.x = p_term_x + d_term_x; + vel_cmd_.twist.linear.y = p_term_y + d_term_y; + vel_cmd_.twist.linear.z = p_term_z + d_term_z; + vel_cmd_.twist.angular.z = p_term_yaw + d_term_yaw; // todo +} + +void PIDPositionController::enforce_dynamic_constraints() +{ + double vel_norm_horz = sqrt((vel_cmd_.twist.linear.x * vel_cmd_.twist.linear.x) + (vel_cmd_.twist.linear.y * vel_cmd_.twist.linear.y)); + + if (vel_norm_horz > constraints_.max_vel_horz_abs) { + vel_cmd_.twist.linear.x = (vel_cmd_.twist.linear.x / vel_norm_horz) * constraints_.max_vel_horz_abs; + vel_cmd_.twist.linear.y = (vel_cmd_.twist.linear.y / vel_norm_horz) * constraints_.max_vel_horz_abs; + } + + if (std::fabs(vel_cmd_.twist.linear.z) > constraints_.max_vel_vert_abs) { + // todo just add a sgn funciton in common utils? return double to be safe. + // template double sgn(T val) { return (T(0) < val) - (val < T(0)); } + vel_cmd_.twist.linear.z = (vel_cmd_.twist.linear.z / std::fabs(vel_cmd_.twist.linear.z)) * constraints_.max_vel_vert_abs; + } + // todo yaw limits + if (std::fabs(vel_cmd_.twist.linear.z) > constraints_.max_yaw_rate_degree) { + // todo just add a sgn funciton in common utils? return double to be safe. + // template double sgn(T val) { return (T(0) < val) - (val < T(0)); } + vel_cmd_.twist.linear.z = (vel_cmd_.twist.linear.z / std::fabs(vel_cmd_.twist.linear.z)) * constraints_.max_yaw_rate_degree; + } +} + +void PIDPositionController::publish_control_cmd() +{ + airsim_vel_cmd_world_frame_pub_->publish(vel_cmd_); +} \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple_node.cpp b/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple_node.cpp new file mode 100755 index 0000000000..42cc25cf2a --- /dev/null +++ b/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple_node.cpp @@ -0,0 +1,15 @@ +#include +#include "pd_position_controller_simple.h" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + std::shared_ptr nh = rclcpp::Node::make_shared("pid_position_controller_simple_node", node_options); + + PIDPositionController controller(nh); + + rclcpp::spin(nh); + return 0; +} \ No newline at end of file diff --git a/tools/Dockerfile-ROS2 b/tools/Dockerfile-ROS2 new file mode 100644 index 0000000000..73f801043e --- /dev/null +++ b/tools/Dockerfile-ROS2 @@ -0,0 +1,12 @@ +FROM ros:foxy-ros-base + +RUN apt-get update &&\ + apt-get install -y\ + apt-utils \ + gcc-8 g++-8 \ + ros-$ROS_DISTRO-tf2-sensor-msgs ros-$ROS_DISTRO-tf2-geometry-msgs ros-$ROS_DISTRO-mavros* \ + ros-$ROS_DISTRO-vision-opencv ros-$ROS_DISTRO-image-transport \ + libyaml-cpp-dev &&\ + echo 'source /opt/ros/$ROS_DISTRO/setup.bash' >> ~/.bashrc &&\ + rm -rf /var/lib/apt/lists/* &&\ + apt-get clean \ No newline at end of file