diff --git a/build_depends.repos b/build_depends.repos index b9134cf0..6bab4a00 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -6,4 +6,4 @@ repositories: autoware_internal_msgs: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git - version: main \ No newline at end of file + version: main diff --git a/sensing/autoware_gnss_poser/README.md b/sensing/autoware_gnss_poser/README.md index dc339eef..464498c2 100644 --- a/sensing/autoware_gnss_poser/README.md +++ b/sensing/autoware_gnss_poser/README.md @@ -17,29 +17,29 @@ If the transformation from `base_link` to the antenna cannot be obtained, it out | Name | Type | Description | | ------------------------------ | ------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------ | -| `/map/map_projector_info` | `autoware_map_msgs::msg::MapProjectorInfo` | map projection info | +| `/map/map_projector_info` | `autoware_map_msgs::msg::MapProjectorInfo` | map projection info | | `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message | | `~/input/autoware_orientation` | `autoware_sensing_msgs::msg::GnssInsOrientationStamped` | orientation [click here for more details](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_sensing_msgs) | ### Output -| Name | Type | Description | -| ------------------------ | ----------------------------------------------- | -------------------------------------------------------------- | -| `~/output/pose` | `geometry_msgs::msg::PoseStamped` | vehicle pose calculated from gnss sensing data | -| `~/output/gnss_pose_cov` | `geometry_msgs::msg::PoseWithCovarianceStamped` | vehicle pose with covariance calculated from gnss sensing data | -| `~/output/gnss_fixed` | `autoware_internal_debug_msgs::msg::BoolStamped` | gnss fix status | +| Name | Type | Description | +| ------------------------ | ------------------------------------------------ | -------------------------------------------------------------- | +| `~/output/pose` | `geometry_msgs::msg::PoseStamped` | vehicle pose calculated from gnss sensing data | +| `~/output/gnss_pose_cov` | `geometry_msgs::msg::PoseWithCovarianceStamped` | vehicle pose with covariance calculated from gnss sensing data | +| `~/output/gnss_fixed` | `autoware_internal_debug_msgs::msg::BoolStamped` | gnss fix status | ## Parameters Parameters in below table -| Name | Type | Default | Description | -| ------------------------ | -----------------------| ------------------------ | -------------------------------------------------------------- | -| `base_frame` | `string` | `base_link` | frame id for base_frame | -| `gnss_base_frame` | `string` | `gnss_base_link` | frame id for gnss_base_frame | -| `map_frame` | `string` | `map` | frame id for map_frame | -| `use_gnss_ins_orientation` | `boolean` | `true` | use Gnss-Ins orientation | -| `gnss_pose_pub_method` | `integer` | `0` | 0: Instant Value 1: Average Value 2: Median Value. If `buffer_epoch` is set to 0, `gnss_pose_pub_method` loses affect. Range: 0~2. | -| `buff_epoch` | `integer` | `1` | Buffer epoch. Range: 0~inf. | +| Name | Type | Default | Description | +| -------------------------- | --------- | ---------------- | ---------------------------------------------------------------------------------------------------------------------------------- | +| `base_frame` | `string` | `base_link` | frame id for base_frame | +| `gnss_base_frame` | `string` | `gnss_base_link` | frame id for gnss_base_frame | +| `map_frame` | `string` | `map` | frame id for map_frame | +| `use_gnss_ins_orientation` | `boolean` | `true` | use Gnss-Ins orientation | +| `gnss_pose_pub_method` | `integer` | `0` | 0: Instant Value 1: Average Value 2: Median Value. If `buffer_epoch` is set to 0, `gnss_pose_pub_method` loses affect. Range: 0~2. | +| `buff_epoch` | `integer` | `1` | Buffer epoch. Range: 0~inf. | All above parameters can be changed in config file [gnss_poser.param.yaml](./config/gnss_poser.param.yaml "Click here to open config file") . diff --git a/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp b/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp index deed6748..a9ada9e9 100644 --- a/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp +++ b/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp @@ -14,14 +14,14 @@ #ifndef AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_ #define AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_ -#include #include +#include +#include #include #include #include #include -#include #include @@ -46,7 +46,8 @@ class GNSSPoser : public rclcpp::Node explicit GNSSPoser(const rclcpp::NodeOptions & node_options); private: - void callback_map_projector_info(const autoware_map_msgs::msg::MapProjectorInfo::ConstSharedPtr msg); + void callback_map_projector_info( + const autoware_map_msgs::msg::MapProjectorInfo::ConstSharedPtr msg); void callback_nav_sat_fix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); void callback_gnss_ins_orientation_stamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg); diff --git a/sensing/autoware_gnss_poser/package.xml b/sensing/autoware_gnss_poser/package.xml index 05703e0a..6746f625 100644 --- a/sensing/autoware_gnss_poser/package.xml +++ b/sensing/autoware_gnss_poser/package.xml @@ -20,8 +20,9 @@ libboost-dev - autoware_map_msgs autoware_geography_utils + autoware_internal_debug_msgs + autoware_map_msgs autoware_sensing_msgs geographic_msgs geographiclib @@ -32,7 +33,6 @@ tf2 tf2_geometry_msgs tf2_ros - autoware_internal_debug_msgs ament_lint_auto autoware_lint_common diff --git a/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp b/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp index 27878731..131e250f 100644 --- a/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp +++ b/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp @@ -59,7 +59,8 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) pose_pub_ = create_publisher("gnss_pose", rclcpp::QoS{1}); pose_cov_pub_ = create_publisher( "gnss_pose_cov", rclcpp::QoS{1}); - fixed_pub_ = create_publisher("gnss_fixed", rclcpp::QoS{1}); + fixed_pub_ = + create_publisher("gnss_fixed", rclcpp::QoS{1}); // Set msg_gnss_ins_orientation_stamped_ with temporary values (not to publish zero value // covariances) @@ -68,7 +69,8 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_z = 1.0; } -void GNSSPoser::callback_map_projector_info(const autoware_map_msgs::msg::MapProjectorInfo::ConstSharedPtr msg) +void GNSSPoser::callback_map_projector_info( + const autoware_map_msgs::msg::MapProjectorInfo::ConstSharedPtr msg) { projector_info_ = *msg; received_map_projector_info_ = true; @@ -117,8 +119,8 @@ void GNSSPoser::callback_nav_sat_fix( geometry_msgs::msg::Point position = autoware::geography_utils::project_forward(gps_point, projector_info_); position.z = autoware::geography_utils::convert_height( - position.z, gps_point.latitude, gps_point.longitude, autoware_map_msgs::msg::MapProjectorInfo::WGS84, - projector_info_.vertical_datum); + position.z, gps_point.latitude, gps_point.longitude, + autoware_map_msgs::msg::MapProjectorInfo::WGS84, projector_info_.vertical_datum); geometry_msgs::msg::Pose gnss_antenna_pose{}; diff --git a/sensing/autoware_gnss_poser/test/test_gnss_poser_node.cpp b/sensing/autoware_gnss_poser/test/test_gnss_poser_node.cpp index 6cf20e04..e9da51e1 100644 --- a/sensing/autoware_gnss_poser/test/test_gnss_poser_node.cpp +++ b/sensing/autoware_gnss_poser/test/test_gnss_poser_node.cpp @@ -12,17 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "autoware/gnss_poser/gnss_poser_node.hpp" + #include -#include -#include -#include -#include "autoware/gnss_poser/gnss_poser_node.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "autoware_internal_debug_msgs/msg/bool_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" -#include "autoware_internal_debug_msgs/msg/bool_stamped.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" + +#include + +#include +#include +#include using namespace autoware::gnss_poser; @@ -44,8 +47,7 @@ class GNSSPoserTest : public ::testing::Test // Create subscribers to capture published messages pose_sub_ = gnss_poser_->create_subscription( - "gnss_pose", rclcpp::QoS{1}, - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + "gnss_pose", rclcpp::QoS{1}, [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { received_pose_ = *msg; pose_received_ = true; }); @@ -65,21 +67,15 @@ class GNSSPoserTest : public ::testing::Test }); // Create publisher to simulate incoming messages - nav_sat_fix_pub_ = gnss_poser_->create_publisher( - "fix", rclcpp::QoS{1}); + nav_sat_fix_pub_ = + gnss_poser_->create_publisher("fix", rclcpp::QoS{1}); executor_.add_node(gnss_poser_); } - void TearDown() override - { - executor_.remove_node(gnss_poser_); - } + void TearDown() override { executor_.remove_node(gnss_poser_); } - void spin_some() - { - executor_.spin_some(std::chrono::milliseconds(100)); - } + void spin_some() { executor_.spin_some(std::chrono::milliseconds(100)); } rclcpp::executors::SingleThreadedExecutor executor_; std::shared_ptr gnss_poser_; @@ -114,7 +110,8 @@ TEST_F(GNSSPoserTest, CallbackNavSatFixTest) nav_sat_fix_msg.latitude = 35.6895; nav_sat_fix_msg.longitude = 139.6917; nav_sat_fix_msg.altitude = 35.0; - nav_sat_fix_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + nav_sat_fix_msg.position_covariance_type = + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; // Publish the message nav_sat_fix_pub_->publish(nav_sat_fix_msg); @@ -139,11 +136,11 @@ TEST_F(GNSSPoserTest, CallbackNavSatFixTest) EXPECT_EQ(received_pose_cov_.header.stamp, nav_sat_fix_msg.header.stamp); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { rclcpp::init(argc, argv); ::testing::InitGoogleTest(&argc, argv); int result = RUN_ALL_TESTS(); rclcpp::shutdown(); return result; -} \ No newline at end of file +}