Fix bug with ENU odom being published on odom_local_ned topic

[nikola-j]

Fixes: #4629

About

When using 'coordinate_system_enu': True, the messages published to odom_local_ned topic will be in ENU frame, this confuses the pd_position_controller_simple node which listens to odom_local_ned for controlling the drone.
This PR fixes that by publishing ENU messages on a new seprate topic odom_local_enu when coordinate system is set to ENU.

How Has This Been Tested?

Set coordinate system to ENU, issue /airsim_node/local_position_goal service.

Screenshots (if appropriate):

[dzywater]

@nikola-j Based on your changes, I think that the original version can meet your request unless you want both enu_odom and ned_odom at the same time. The enu and ned should not exist at the same time at least by original intention.

[nikola-j]

It doesn't make sense to have a topic called odom_local_ned and publish a message in ENU frame on it. Without this change, the simple PD position controller won't work. Here is a video demonstrating its behavior on the main branch when launching with 'coordinate_system_enu': True: https://drive.google.com/file/d/1kt70bK1VST9IA7JgefTSaTFa4Z-HNQ2l/view?usp=sharing

When using 'coordinate_system_enu': True there needs to be a new topic called odom_local_enu, but because the PD controller expects NED, we need odom_local_ned turned on also.

[dzywater]

It doesn't make sense to have a topic called odom_local_ned and publish a message in ENU frame on it. Without this change, the simple PD position controller won't work. Here is a video demonstrating its behavior on the main branch when launching with 'coordinate_system_enu': True: https://drive.google.com/file/d/1kt70bK1VST9IA7JgefTSaTFa4Z-HNQ2l/view?usp=sharing

When using 'coordinate_system_enu': True there needs to be a new topic called odom_local_enu, but because the PD controller expects NED, we need odom_local_ned turned on also.

If you want NED, set rosparam world_frame_id as "world_ned" (default).
If you want ENU, set rosparam world_frame_id as others like "world_enu".
See the function: AirsimROSWrapper::initialize_ros
See from

AirSim/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h

Line 331 in 6688d27

const std::string AIRSIM_FRAME_ID = "world_ned";

and

AirSim/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp

Line 92 in 6688d27

void AirsimROSWrapper::initialize_ros()

Optionally, you could decide ENU or NED by setting coordinate_system_enu directly. Names of topic/service depend on parameters you set. But the names are not important but what the topic data is.

[nikola-j]

Okay, thank you, that makes sense, setting the coordinate_system_enu parameter will change only the data, and not the topic name, if that is intended, that is fine.
But when setting world_frame_id to world_enu simple PD controller still listens to /airsim_node/drone_1/odom_local_ned, and it waits for odometry forever. Do you think there should be another publisher for ned odom for the PD controller?

AirSim/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp

Line 79 in 6688d27

airsim_odom_sub_ = nh_->create_subscription<nav_msgs::msg::Odometry>("/airsim_node/" + vehicle_name + "/odom_local_ned", 50, std::bind(&PIDPositionController::airsim_odom_cb, this, _1));

[dzywater]

1. pd_position_controller_simple should be enhanced, because it uses "odom_local_ned" directly, not considering ENU system.
2. Simply, You can set world_frame_id=world_ned(default, no need to do actually) and coordinate_system_enu=true, then the data is based on ENU, but name is odom_local_ned. So you can use ENU control and controller can get data.

Actually, as AirsimROSWrapper::initialize_ros() shows, odom_frame_id is independent of coordinate_system_enu. You can set odom_frame_id as any name you expect.

[nikola-j]

I think the only way to solve this is to either change the PD controller to support enu based navigation, or to add a new publisher for ned data. The easier fix is to add a ned publisher when there is no ned publisher.
2. won't work, since that is the original issue that this PR is supposed to fix (the issue from the video happens in that case since PD controller works with only ned data)

[dzywater]

1.It is not reasonable to modify airsim_ros_wrapper, because this goes against the airsim_ros framework. You only see odom, not many other topics/services/TFs.
2.You should check why it does not work. I have no problems by this way in my all applications.

If some changes must be done, it should be pd_position_controller_simple.

[nikola-j]

2. I have no idea why it wouldn't work for me, do you give it a goal in ENU or NED coordinates for it to work? And the only parameter that you set is coordinate_system_enu=true?

[dzywater]

2. I have no idea why it wouldn't work for me, do you give it a goal in ENU or NED coordinates for it to work? And the only parameter that you set is coordinate_system_enu=true?

Could you share me your project or tell me how to create similar one ? I will try it.

[nikola-j]

Thanks, sure:

1. Download AirSim from master, build it
2. Set the parameter in ros2/src/airsim_ros_pkgs/launch/airsim_node.launch.py, under airsim_node with coordinate_system_enu': True
3. Build ros2 with colcon build --cmake-args -DCMAKE_C_COMPILER=gcc-8 --cmake-args -DCMAKE_CXX_COMPILER=g++-8 --cmake-args -DCMAKE_BUILD_TYPE=Release
4. Download an environment, for example Blocks, https://github.com/microsoft/AirSim/releases/download/v1.8.1/Blocks.zip
5. Launch it ./Blocks.sh -windowed
6. Start airsim ros: . install/setub.bash && ros2 launch airsim_ros_pkgs airsim_with_simple_PD_position_controller.launch.py
7. Give a local_position_goal, eg: ros2 service call /airsim_node/local_position_goal/override airsim_interfaces/srv/SetLocalPosition "x: 1.0 y: 0.0 z: -1.0 yaw: 0.0 vehicle_name: ''"
8. It then kinda flies off, never completing the goal

[dzywater]

7. local_position_goal

Checked pd_position_controller_simple.cpp, I guess that PIDPositionController only supports NED mode.