From 95100831730b6e6703cb5238d86951fc20e0e12e Mon Sep 17 00:00:00 2001 From: Nikola Jovicic Date: Wed, 20 Jul 2022 08:40:03 +0200 Subject: [PATCH 1/3] Fix bug with `odom_local_ned` topic containing enu messages when using ``'coordinate_system_enu': True`, publish enu messages on a new seprate topic --- .../include/airsim_ros_wrapper.h | 2 + .../src/airsim_ros_wrapper.cpp | 51 +++++++++++++++---- 2 files changed, 43 insertions(+), 10 deletions(-) diff --git a/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h index a5126034b7..a92c9b3a03 100755 --- a/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ b/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h @@ -138,6 +138,7 @@ class AirsimROSWrapper /// All things ROS rclcpp::Publisher::SharedPtr odom_local_pub_; + rclcpp::Publisher::SharedPtr odom_local_enu_pub_; rclcpp::Publisher::SharedPtr global_gps_pub_; rclcpp::Publisher::SharedPtr env_pub_; airsim_interfaces::msg::Environment env_msg_; @@ -255,6 +256,7 @@ class AirsimROSWrapper 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 convert_odom_to_enu(const nav_msgs::msg::Odometry original_odom_msg) 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; diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index ce3f3390df..8a6b3864bc 100755 --- a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -153,6 +153,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() const std::string topic_prefix = "~/" + curr_vehicle_name; vehicle_ros->odom_local_pub_ = nh_->create_publisher(topic_prefix + "/" + odom_frame_id_, 10); + vehicle_ros->odom_local_enu_pub_ = nh_->create_publisher(topic_prefix + "/" + ENU_ODOM_FRAME_ID, 10); vehicle_ros->env_pub_ = nh_->create_publisher(topic_prefix + "/environment", 10); @@ -603,6 +604,8 @@ nav_msgs::msg::Odometry AirsimROSWrapper::get_odom_msg_from_kinematic_state(cons { nav_msgs::msg::Odometry odom_msg; + odom_msg.header.frame_id = AIRSIM_FRAME_ID; + 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(); @@ -618,16 +621,40 @@ nav_msgs::msg::Odometry AirsimROSWrapper::get_odom_msg_from_kinematic_state(cons 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::convert_odom_to_enu(const nav_msgs::msg::Odometry original_odom_msg) const +{ + nav_msgs::msg::Odometry odom_msg; + + odom_msg.header.stamp = original_odom_msg.header.stamp; + odom_msg.header.frame_id = "world_enu"; + + odom_msg.pose.pose.position.x = original_odom_msg.pose.pose.position.x; + odom_msg.pose.pose.position.y = original_odom_msg.pose.pose.position.y; + odom_msg.pose.pose.position.z = original_odom_msg.pose.pose.position.z; + + odom_msg.pose.pose.orientation.x = original_odom_msg.pose.pose.orientation.x; + odom_msg.pose.pose.orientation.y = original_odom_msg.pose.pose.orientation.y; + odom_msg.pose.pose.orientation.z = original_odom_msg.pose.pose.orientation.z; + odom_msg.pose.pose.orientation.w = original_odom_msg.pose.pose.orientation.w; + + odom_msg.twist.twist.linear.x = original_odom_msg.twist.twist.linear.x; + odom_msg.twist.twist.linear.y = original_odom_msg.twist.twist.linear.y; + odom_msg.twist.twist.linear.z = original_odom_msg.twist.twist.linear.z; + odom_msg.twist.twist.angular.x = original_odom_msg.twist.twist.angular.x; + odom_msg.twist.twist.angular.y = original_odom_msg.twist.twist.angular.y; + odom_msg.twist.twist.angular.z = original_odom_msg.twist.twist.angular.z; + + 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; } @@ -1010,6 +1037,8 @@ rclcpp::Time AirsimROSWrapper::update_state() return curr_ros_time; } + + void AirsimROSWrapper::publish_vehicle_state() { for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { @@ -1026,6 +1055,8 @@ void AirsimROSWrapper::publish_vehicle_state() // odom and transforms vehicle_ros->odom_local_pub_->publish(vehicle_ros->curr_odom_); + vehicle_ros->odom_local_enu_pub_->publish(convert_odom_to_enu(vehicle_ros->curr_odom_)); + publish_odom_tf(vehicle_ros->curr_odom_); // ground truth GPS position from sim/HITL From dc262e8a996f67688b9a3e633644148e376a642f Mon Sep 17 00:00:00 2001 From: Nikola Jovicic Date: Wed, 20 Jul 2022 08:53:03 +0200 Subject: [PATCH 2/3] Disable enu publisher when in ned --- ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index 8a6b3864bc..ae0a9e2fa4 100755 --- a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -153,7 +153,8 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() const std::string topic_prefix = "~/" + curr_vehicle_name; vehicle_ros->odom_local_pub_ = nh_->create_publisher(topic_prefix + "/" + odom_frame_id_, 10); - vehicle_ros->odom_local_enu_pub_ = nh_->create_publisher(topic_prefix + "/" + ENU_ODOM_FRAME_ID, 10); + if (isENU_) + vehicle_ros->odom_local_enu_pub_ = nh_->create_publisher(topic_prefix + "/" + ENU_ODOM_FRAME_ID, 10); vehicle_ros->env_pub_ = nh_->create_publisher(topic_prefix + "/environment", 10); @@ -1055,7 +1056,8 @@ void AirsimROSWrapper::publish_vehicle_state() // odom and transforms vehicle_ros->odom_local_pub_->publish(vehicle_ros->curr_odom_); - vehicle_ros->odom_local_enu_pub_->publish(convert_odom_to_enu(vehicle_ros->curr_odom_)); + if (isENU_) + vehicle_ros->odom_local_enu_pub_->publish(convert_odom_to_enu(vehicle_ros->curr_odom_)); publish_odom_tf(vehicle_ros->curr_odom_); From ecdcdb811f25f2ce0f20bc1aeb284773c48e7848 Mon Sep 17 00:00:00 2001 From: Nikola Jovicic Date: Mon, 1 Aug 2022 15:36:30 +0200 Subject: [PATCH 3/3] Reformat file --- ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index ae0a9e2fa4..ecb59110f3 100755 --- a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -1038,8 +1038,6 @@ rclcpp::Time AirsimROSWrapper::update_state() return curr_ros_time; } - - void AirsimROSWrapper::publish_vehicle_state() { for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) {