Skip to content

Fix bug with ENU odom being published on odom_local_ned topic #4631

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Fix bug with ENU odom being published on odom_local_ned topic #4631

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9510083
Fix bug with `odom_local_ned` topic containing enu messages when usin…
nikola-j Jul 20, 2022
dc262e8
Disable enu publisher when in ned
nikola-j Jul 20, 2022
ecdcdb8
Reformat file
nikola-j Aug 1, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -138,6 +138,7 @@ class AirsimROSWrapper

/// All things ROS
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_local_pub_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_local_enu_pub_;
rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr global_gps_pub_;
rclcpp::Publisher<airsim_interfaces::msg::Environment>::SharedPtr env_pub_;
airsim_interfaces::msg::Environment env_msg_;
Expand Down Expand Up @@ -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;
Expand Down
51 changes: 41 additions & 10 deletions ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +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<nav_msgs::msg::Odometry>(topic_prefix + "/" + odom_frame_id_, 10);
if (isENU_)
vehicle_ros->odom_local_enu_pub_ = nh_->create_publisher<nav_msgs::msg::Odometry>(topic_prefix + "/" + ENU_ODOM_FRAME_ID, 10);

vehicle_ros->env_pub_ = nh_->create_publisher<airsim_interfaces::msg::Environment>(topic_prefix + "/environment", 10);

Expand Down Expand Up @@ -603,6 +605,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();
Expand All @@ -618,16 +622,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;
}
Expand Down Expand Up @@ -1026,6 +1054,9 @@ void AirsimROSWrapper::publish_vehicle_state()

// odom and transforms
vehicle_ros->odom_local_pub_->publish(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_);

// ground truth GPS position from sim/HITL
Expand Down