Skip to content
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

Jump to bottom

Add test for geopose heading #28091

Merged
merged 1 commit into from
Sep 17, 2024
Merged

Add test for geopose heading #28091

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
38 changes: 34 additions & 4 deletions Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_geopose_msg_received.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,13 @@
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.

"""Bring up ArduPilot SITL and check the GeoPose message is being published."""
"""
Bring up ArduPilot SITL and check the GeoPoseStamped message is being published.

colcon test --packages-select ardupilot_dds_tests \
--event-handlers=console_cohesion+ --pytest-args -k test_geopose_msg_received

"""

import launch_pytest
import math
Expand Down Expand Up @@ -41,7 +47,7 @@
CMAC_HEADING = 353


def ros_quat_to_heading(quat):
def ros_quat_to_heading_deg(quat):
# By default, scipy follows scalar-last order – (x, y, z, w)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

scipy is a large library to pull in just for quaternions and rotations. Have you thought about transforms3d (https://matthew-brett.github.io/transforms3d/)?

Copy link
Collaborator Author

@Ryanf55 Ryanf55 Sep 13, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yea, I just saw scipy was already used a bunch of places, including in the setup script, and thought the team would prefer a library they are familiar with.

ardupilot/Tools/environment_install/install-prereqs-ubuntu.sh

Line 195 in 72a0139

PYTHON_PKGS+=" matplotlib scipy opencv-python pyyaml"

Your link looks like a much more focused library, but the apt version is out of date:
matthew-brett/transforms3d#65

rot = R.from_quat([quat.x, quat.y, quat.z, quat.w])
r, p, y = rot.as_euler(seq="xyz", degrees=True)
Expand All @@ -57,10 +63,28 @@ def validate_position_cmac(position):
and math.isclose(position.altitude, CMAC_ABS_ALT, abs_tol=1.0)
)

def wrap_360(angle):
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could use the same calc as AP_Math here?

float wrap_360(const float angle)
{
    float res = fmodf(angle, 360.0f);
    if (res < 0) {
        res += 360.0f;
    }
    return res;
}

The calc does not wrap 360 deg to 0, not sure if that's an issue (means the function is not invertible).

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I pulled this from pymavlink but added recursion so you can handle multi-wrap. If the initial heading was directly north, the comparison would get tricky, but this util is only used in this hard-coded cmac test.

if angle > 360:
angle -= 360.0
return wrap_360(angle)
if angle < 0:
angle += 360.0
return wrap_360(angle)
return angle

def validate_heading_cmac(orientation):
"""Return true if the vehicle is facing the right way for CMAC."""
return math.isclose(ros_quat_to_heading(orientation), CMAC_HEADING)
"""
Return true if the vehicle is facing the right way for CMAC.

Per ROS REP-103, the quaternion should report 0 when the vehicle faces east,
and 90 degrees when facing north.
Because CMAC is NNW, we expect ~97 degees.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

./Tools/autotest/locations.txt has this for CMAC. Heading is 353 => rotation = 97 as described.

CMAC=-35.363261,149.165230,584,353

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yea, I just had to add a tolerance because the data is through the EAHRS with noise added. Peter mentioned to set the EKF type to match the SIM so it's exact, but we don't depend on mavproxy or support params yet, so I left that out of scope and used a tolerance instead.

See this PR for more info:
* https://github.com/ros-infrastructure/rep/pull/407
"""
# The following converts from compass bearing (locations.txt) to REP-103 heading.
CMAC_YAW_ENU_REP103 = wrap_360(-1 * CMAC_HEADING) + 90
return math.isclose(ros_quat_to_heading_deg(orientation), CMAC_YAW_ENU_REP103, abs_tol=5)


class GeoPoseListener(rclpy.node.Node):
Expand All @@ -71,6 +95,7 @@ def __init__(self):
super().__init__("geopose_listener")
self.msg_event_object = threading.Event()
self.position_correct_event_object = threading.Event()
self.orientation_event_object = threading.Event()

# Declare and acquire `topic` parameter
self.declare_parameter("topic", TOPIC)
Expand Down Expand Up @@ -101,6 +126,9 @@ def subscriber_callback(self, msg):
if validate_position_cmac(msg.pose.position):
self.position_correct_event_object.set()

if validate_heading_cmac(msg.pose.orientation):
self.orientation_event_object.set()


@launch_pytest.fixture
def launch_sitl_copter_dds_serial(sitl_copter_dds_serial):
Expand Down Expand Up @@ -155,6 +183,8 @@ def test_dds_serial_geopose_msg_recv(launch_context, launch_sitl_copter_dds_seri
assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs."
pose_correct_flag = node.position_correct_event_object.wait(timeout=10.0)
assert pose_correct_flag, f"Did not receive correct position."
orientation_correct_flag = node.orientation_event_object.wait(timeout=10.0)
assert orientation_correct_flag, f"Did not receive correct orientation."
finally:
rclpy.shutdown()
yield
Expand Down
Loading