From 3606f58a9583c38bbe7f5648c228ed95b3013bb1 Mon Sep 17 00:00:00 2001 From: Giacomo Franchini Date: Wed, 3 Apr 2024 12:35:02 +0200 Subject: [PATCH 1/2] spatial gps + imu setup --- launch/hal_an_spatial.launch.py | 74 +++++++++++++++++++++--------- params/params.yaml | 3 +- src/advancednavigation_spatial.cpp | 28 +++++------ 3 files changed, 70 insertions(+), 35 deletions(-) diff --git a/launch/hal_an_spatial.launch.py b/launch/hal_an_spatial.launch.py index b864e9b..404e8b5 100644 --- a/launch/hal_an_spatial.launch.py +++ b/launch/hal_an_spatial.launch.py @@ -1,27 +1,59 @@ from launch import LaunchDescription +from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory -import launch_ros.actions import os -import yaml -from launch.substitutions import EnvironmentVariable -import pathlib -import launch.actions -from launch.actions import DeclareLaunchArgument -from launch_ros.actions import Node +import numpy as np -from launch.actions.execute_process import ExecuteProcess +def get_quaternion_from_euler(roll, pitch, yaw): + """ + Convert an Euler angle to a quaternion. + Input + :param roll: The roll (rotation around x-axis) angle in radians. + :param pitch: The pitch (rotation around y-axis) angle in radians. + :param yaw: The yaw (rotation around z-axis) angle in radians. + Output + :return qx, qy, qz, qw: The orientation in quaternion [x,y,z,w] format + """ + qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) + qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + return [qx, qy, qz, qw] + +rot_vect_imu = np.array([np.deg2rad(180.0), np.deg2rad(0.0), np.deg2rad(180.0)]) +rot_vect_gps = np.array([np.deg2rad(0.0), np.deg2rad(0.0), np.deg2rad(0.0)]) +q_imu = get_quaternion_from_euler(rot_vect_imu[0], rot_vect_imu[1], rot_vect_imu[2]) +q_gps = get_quaternion_from_euler(rot_vect_gps[0], rot_vect_gps[1], rot_vect_gps[2]) def generate_launch_description(): - return LaunchDescription([ - - Node( - package='hal_an_spatial', - executable='HALSpatial', - name='hal_an_spatial', - output={ - "stdout": "screen", - "stderr": "screen", - }, - parameters=[os.path.join(get_package_share_directory("hal_an_spatial"), 'params', 'params.yaml')], - ) -]) + + imu_tf = Node( + package='tf2_ros', + executable='static_transform_publisher', + namespace='', + name='spatial_imu_transform', + output='screen', + arguments=["0.469", "0", "0.462", str(q_imu[0]), str(q_imu[1]), str(q_imu[2]), str(q_imu[3]), "base_link", "imu2"] + ) + + gps_tf = Node( + package='tf2_ros', + executable='static_transform_publisher', + namespace='', + name='spatial_gps_transform', + output='screen', + arguments=["0.8131", "0", "-0.01935", str(q_gps[0]), str(q_gps[1]), str(q_gps[2]), str(q_gps[3]), "imu2", "gps"] + ) + + spatial_node = Node( + package='hal_an_spatial', + executable='HALSpatial', + name='hal_an_spatial', + output={ + "stdout": "screen", + "stderr": "screen", + }, + parameters=[os.path.join(get_package_share_directory("hal_an_spatial"), 'params', 'params.yaml')], + ) + + return LaunchDescription([imu_tf, gps_tf, spatial_node]) diff --git a/params/params.yaml b/params/params.yaml index 96d09ad..e9e078c 100644 --- a/params/params.yaml +++ b/params/params.yaml @@ -4,7 +4,8 @@ hal_an_spatial: baud: 115200 gnss_rate: 10 imu_rate: 80 - frame_id: odom + imu_frame_id: imu2 + gps_frame_id: gps publishers: imu_topic: /imu2/data mag_topic: /imu2/magnetic diff --git a/src/advancednavigation_spatial.cpp b/src/advancednavigation_spatial.cpp index 7d8898e..ec5268a 100644 --- a/src/advancednavigation_spatial.cpp +++ b/src/advancednavigation_spatial.cpp @@ -26,15 +26,16 @@ class HALSpatialNode : public rclcpp::Node { public: HALSpatialNode() : Node("hal_an_spatial") { - port = declare_parameter("port", "/dev/ttyUSB0"); - baud = (uint32_t) declare_parameter("baud", 115200); + port = declare_parameter("port", "/dev/ttyUSB0"); + baud = (uint32_t) declare_parameter("baud", 115200); gnss_rate = (uint32_t) declare_parameter("gnss_rate", 1); imu_rate = (uint32_t) declare_parameter("imu_rate", 50); imu_topic = declare_parameter("publishers.imu_topic", "/imu"); mag_topic = declare_parameter("publishers.mag_topic", "/mag"); nav_topic = declare_parameter("publishers.nav_topic", "/gnss"); rtcm_topic = declare_parameter("publishers.rtcm_topic", "/rtcm"); - reference_frame = declare_parameter("frame_id", "base_link"); + imu_reference_frame = declare_parameter("imu_frame_id", "imu"); + gps_reference_frame = declare_parameter("gps_frame_id", "gps"); try { ser = std::make_shared(); ser->setPort(port); @@ -79,9 +80,9 @@ class HALSpatialNode : public rclcpp::Node { an_packet_free(&out_packet); } - imu_publisher_ = create_publisher(imu_topic, 1); - mag_publisher_ = create_publisher(mag_topic, 1); - nav_publisher_ = create_publisher(nav_topic, 1); + imu_publisher_ = create_publisher(imu_topic, 1); + mag_publisher_ = create_publisher(mag_topic, 1); + nav_publisher_ = create_publisher(nav_topic, 1); rtcm_subscription_ = create_subscription(rtcm_topic, 10, std::bind(&HALSpatialNode::rtcm_callback, this, _1)); worker_thread = std::thread([this]{this->feedback_loop();}); @@ -96,16 +97,17 @@ class HALSpatialNode : public rclcpp::Node { private: std::thread worker_thread; std::string port; - std::string reference_frame; - uint32_t baud, imu_rate, gnss_rate; + std::string imu_reference_frame; + std::string gps_reference_frame; + uint32_t baud, imu_rate, gnss_rate; std::shared_ptr ser; std::string imu_topic, mag_topic, nav_topic, rtcm_topic; an_decoder_t an_decoder; int bytes_received; bool stop = false; - rclcpp::Publisher::SharedPtr imu_publisher_; - rclcpp::Publisher::SharedPtr mag_publisher_; + rclcpp::Publisher::SharedPtr imu_publisher_; + rclcpp::Publisher::SharedPtr mag_publisher_; rclcpp::Publisher::SharedPtr nav_publisher_; rclcpp::Subscription::SharedPtr rtcm_subscription_; @@ -155,7 +157,7 @@ class HALSpatialNode : public rclcpp::Node { } if (type == packet_id_raw_sensors){ - imu_msg.header.frame_id = reference_frame; + imu_msg.header.frame_id = imu_reference_frame; imu_msg.angular_velocity.x = sensor_packet.gyroscopes[0]; imu_msg.angular_velocity.y = sensor_packet.gyroscopes[1]; imu_msg.angular_velocity.z = sensor_packet.gyroscopes[2]; @@ -163,7 +165,7 @@ class HALSpatialNode : public rclcpp::Node { imu_msg.linear_acceleration.y = sensor_packet.accelerometers[1]; imu_msg.linear_acceleration.z = sensor_packet.accelerometers[2]; imu_publisher_->publish(imu_msg); - mag_msg.header.frame_id = reference_frame; + mag_msg.header.frame_id = imu_reference_frame; mag_msg.magnetic_field.x = sensor_packet.magnetometers[0]; mag_msg.magnetic_field.y = sensor_packet.magnetometers[1]; mag_msg.magnetic_field.z = sensor_packet.magnetometers[2]; @@ -188,7 +190,7 @@ class HALSpatialNode : public rclcpp::Node { } //RCLCPP_INFO(get_logger(), "Fix type: %d", int(system_packet.filter_status.b.gnss_fix_type)); nav_msg.status.service = nav_msg.status.SERVICE_GPS | nav_msg.status.SERVICE_GLONASS; - nav_msg.header.frame_id = reference_frame; + nav_msg.header.frame_id = gps_reference_frame; nav_msg.longitude = system_packet.longitude/ M_PI * 180.0; nav_msg.latitude = system_packet.latitude/ M_PI * 180.0; nav_msg.altitude = system_packet.height; From 6531bd934a8179ea5ff5752a1fbe0f835e24282d Mon Sep 17 00:00:00 2001 From: Giacomo Franchini Date: Wed, 3 Apr 2024 14:30:13 +0200 Subject: [PATCH 2/2] revert changes on hal_an_spatial.launch.py --- launch/hal_an_spatial.launch.py | 74 ++++++++++----------------------- params/params.yaml | 4 +- 2 files changed, 23 insertions(+), 55 deletions(-) diff --git a/launch/hal_an_spatial.launch.py b/launch/hal_an_spatial.launch.py index 404e8b5..b864e9b 100644 --- a/launch/hal_an_spatial.launch.py +++ b/launch/hal_an_spatial.launch.py @@ -1,59 +1,27 @@ from launch import LaunchDescription -from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory +import launch_ros.actions import os -import numpy as np - -def get_quaternion_from_euler(roll, pitch, yaw): - """ - Convert an Euler angle to a quaternion. - Input - :param roll: The roll (rotation around x-axis) angle in radians. - :param pitch: The pitch (rotation around y-axis) angle in radians. - :param yaw: The yaw (rotation around z-axis) angle in radians. - Output - :return qx, qy, qz, qw: The orientation in quaternion [x,y,z,w] format - """ - qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) - qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) - qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) - return [qx, qy, qz, qw] +import yaml +from launch.substitutions import EnvironmentVariable +import pathlib +import launch.actions +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node -rot_vect_imu = np.array([np.deg2rad(180.0), np.deg2rad(0.0), np.deg2rad(180.0)]) -rot_vect_gps = np.array([np.deg2rad(0.0), np.deg2rad(0.0), np.deg2rad(0.0)]) -q_imu = get_quaternion_from_euler(rot_vect_imu[0], rot_vect_imu[1], rot_vect_imu[2]) -q_gps = get_quaternion_from_euler(rot_vect_gps[0], rot_vect_gps[1], rot_vect_gps[2]) +from launch.actions.execute_process import ExecuteProcess def generate_launch_description(): - - imu_tf = Node( - package='tf2_ros', - executable='static_transform_publisher', - namespace='', - name='spatial_imu_transform', - output='screen', - arguments=["0.469", "0", "0.462", str(q_imu[0]), str(q_imu[1]), str(q_imu[2]), str(q_imu[3]), "base_link", "imu2"] - ) - - gps_tf = Node( - package='tf2_ros', - executable='static_transform_publisher', - namespace='', - name='spatial_gps_transform', - output='screen', - arguments=["0.8131", "0", "-0.01935", str(q_gps[0]), str(q_gps[1]), str(q_gps[2]), str(q_gps[3]), "imu2", "gps"] - ) - - spatial_node = Node( - package='hal_an_spatial', - executable='HALSpatial', - name='hal_an_spatial', - output={ - "stdout": "screen", - "stderr": "screen", - }, - parameters=[os.path.join(get_package_share_directory("hal_an_spatial"), 'params', 'params.yaml')], - ) - - return LaunchDescription([imu_tf, gps_tf, spatial_node]) + return LaunchDescription([ + + Node( + package='hal_an_spatial', + executable='HALSpatial', + name='hal_an_spatial', + output={ + "stdout": "screen", + "stderr": "screen", + }, + parameters=[os.path.join(get_package_share_directory("hal_an_spatial"), 'params', 'params.yaml')], + ) +]) diff --git a/params/params.yaml b/params/params.yaml index e9e078c..91ae837 100644 --- a/params/params.yaml +++ b/params/params.yaml @@ -4,8 +4,8 @@ hal_an_spatial: baud: 115200 gnss_rate: 10 imu_rate: 80 - imu_frame_id: imu2 - gps_frame_id: gps + imu_frame_id: imu2 # imu module frame id + gps_frame_id: gps # gps antenna frame id publishers: imu_topic: /imu2/data mag_topic: /imu2/magnetic