diff --git a/.circleci/config.yml b/.circleci/config.yml index 1ee68680ede..b582398f27d 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -28,7 +28,7 @@ jobs: # Pull docker image from docker hub # XTERM used for better catkin_make output docker: - - image: usdotfhwastol/carma-base:carma-system-4.4.0 + - image: usdotfhwastol/carma-base:carma-system-4.5.0 user: carma environment: TERM: xterm diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 8bcf250837f..0e34d0352dc 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -1,13 +1,17 @@ -name: Docker +name: Docker build on: push: branches-ignore: - "carma-develop" + - "master" + - "release/*" + pull_request: + types: [opened, synchronize, reopened] jobs: docker: - runs-on: ubuntu-latest + runs-on: ubuntu-latest-8-cores steps: - name: Checkout uses: actions/checkout@v3 @@ -16,4 +20,4 @@ jobs: - name: Build uses: docker/build-push-action@v3 with: - context: . + context: . \ No newline at end of file diff --git a/.github/workflows/dockerhub.yml b/.github/workflows/dockerhub.yml index 8767c90c969..c024dbf20cb 100644 --- a/.github/workflows/dockerhub.yml +++ b/.github/workflows/dockerhub.yml @@ -1,26 +1,16 @@ -name: DockerHub +name: Docker Hub build on: push: branches: - "carma-develop" - + - "master" + - "release/*" + tags: + - "carma-system-*" jobs: - docker: - runs-on: ubuntu-latest - steps: - - name: Checkout - uses: actions/checkout@v3 - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 - - name: Login to DockerHub - uses: docker/login-action@v2 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Build - uses: docker/build-push-action@v3 - with: - context: . - push: true - tags: usdotfhwastoldev/${{ github.event.repository.name }}:develop + dockerhub: + uses: usdot-fhwa-stol/actions/.github/workflows/dockerhub.yml@main + secrets: + DOCKERHUB_USERNAME: ${{ secrets.DOCKERHUB_USERNAME }} + DOCKERHUB_TOKEN: ${{ secrets.DOCKERHUB_TOKEN }} diff --git a/Dockerfile b/Dockerfile index 9ea6ed8fb55..90adbe65625 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,4 @@ -FROM usdotfhwastol/carma-base:carma-system-4.4.0 AS base_image +FROM usdotfhwastol/carma-base:carma-system-4.5.0 AS base_image FROM base_image AS build diff --git a/common/autoware_build_flags/CMakeLists.txt b/common/autoware_build_flags/CMakeLists.txt index eab8f612fe5..87524e36c69 100644 --- a/common/autoware_build_flags/CMakeLists.txt +++ b/common/autoware_build_flags/CMakeLists.txt @@ -14,6 +14,9 @@ else() #ROS2 find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() + list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS + cmake/autoware_build_flags-extras.cmake + ) ament_auto_package() endif() diff --git a/common/autoware_build_flags/cmake/autoware_build_flags-extras.cmake b/common/autoware_build_flags/cmake/autoware_build_flags-extras.cmake index 7f6c7531058..cb597900bd4 100644 --- a/common/autoware_build_flags/cmake/autoware_build_flags-extras.cmake +++ b/common/autoware_build_flags/cmake/autoware_build_flags-extras.cmake @@ -4,11 +4,18 @@ elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,-undefined,error") endif() +find_package(ros_environment REQUIRED) +set(ROS_VERSION $ENV{ROS_VERSION}) + +if (${ROS_VERSION} EQUAL 1) # Enable support for C++14 -if(${CMAKE_VERSION} VERSION_LESS "3.1.0") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") -else() - set(CMAKE_CXX_STANDARD 14) + if(${CMAKE_VERSION} VERSION_LESS "3.1.0") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") + else() + set(CMAKE_CXX_STANDARD 14) + endif() +else() # ROS2 + set(CMAKE_CXX_STANDARD 17) endif() message(STATUS "CUDA compilation status: $ENV{AUTOWARE_COMPILE_WITH_CUDA}.") diff --git a/common/autoware_build_flags/package.xml b/common/autoware_build_flags/package.xml index 90fd9743b02..908e8c349f1 100644 --- a/common/autoware_build_flags/package.xml +++ b/common/autoware_build_flags/package.xml @@ -9,6 +9,7 @@ Apache 2 + ros_environment catkin ament_cmake_auto diff --git a/common/hardcoded_params/include/hardcoded_params/control_limits/control_limits.h b/common/hardcoded_params/include/hardcoded_params/control_limits/control_limits.h index 96f89c5432d..ae081e0bc3e 100644 --- a/common/hardcoded_params/include/hardcoded_params/control_limits/control_limits.h +++ b/common/hardcoded_params/include/hardcoded_params/control_limits/control_limits.h @@ -21,9 +21,19 @@ namespace hardcoded_params namespace control_limits { /** - * The maximum allowable longitudinal velocity of the vehicle in m/s + * The maximum allowable longitudinal velocity of the vehicle in m/s */ constexpr double MAX_LONGITUDINAL_VELOCITY_MPS = 35.7632; constexpr double MAX_LONGITUDINAL_ACCEL_MPS2 = 3.5; + +/** +* The maximum allowable simulation parameters +* At the time of writing this, platform uses CARLA 0.9.10 as the vehicle's physics simulator. +* CARLA 0.9.10 uses below hardcoded value for its vehicles (so even if increasing this value, +* actual simulation won't support larger values unless forked and compiled) +* https://github.com/carla-simulator/ros-bridge/blob/0.9.10.1/carla_ackermann_control/src/carla_ackermann_control/carla_control_physics.py#L254 +*/ +constexpr double MAX_SIMULATION_LONGITUDINAL_ACCEL_MPS2 = 8.0; + } } // namespace hardcoded_params diff --git a/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h b/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h index 15741929d17..bad4c6abefc 100644 --- a/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h +++ b/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h @@ -25,9 +25,9 @@ namespace lanelet { /** - * @brief: Enum representing Traffic Light States. + * @brief: Enum representing Traffic Light States. * These states match the SAE J2735 PhaseState definitions used for SPaT messages - * + * * UNAVAILABLE : No data available * DARK : Light is non-functional * STOP_THEN_PROCEED : Flashing Red @@ -53,6 +53,15 @@ enum class CarmaTrafficSignalState { CAUTION_CONFLICTING_TRAFFIC=9 }; +namespace time{ + +//This int is max representable number by 32bit which the posix::time library is using. +//It corresponds to 03:14:07 on Tuesday, 19 January 2038. + +constexpr auto INFINITY_END_TIME_FOR_NOT_ENOUGH_STATES{2147483647}; + +} + struct CarmaTrafficSignalRoleNameString { static constexpr char ControlStart[] = "control_start"; @@ -67,7 +76,7 @@ std::ostream& operator<<(std::ostream& os, CarmaTrafficSignalState s); /** * @brief: Class representing a known timing traffic light. * Normally the traffic light timing information is provided by SAE J2735 SPaT messages although alternative data sources can be supported - * + * * @ingroup RegulatoryElementPrimitives * @ingroup Primitives */ @@ -79,7 +88,7 @@ class CarmaTrafficSignal : public lanelet::RegulatoryElement int revision_ = 0; //indicates when was this last modified boost::posix_time::time_duration fixed_cycle_duration; - std::vector recorded_start_time_stamps; //user must ensure it's 1 to 1 with recorded_time_stamps , + std::vector recorded_start_time_stamps; //user must ensure it's 1 to 1 with recorded_time_stamps , //used in dynamic SPAT processing std::vector> recorded_time_stamps; std::unordered_map signal_durations; @@ -98,11 +107,11 @@ class CarmaTrafficSignal : public lanelet::RegulatoryElement * NOTE: Order of the lanelets does not correlate to the order of the control_end lanelets */ lanelet::ConstLanelets getControlStartLanelets() const; - + /** * @brief getControlEndLanelets function returns lanelets where this element's control ends * NOTE: Order of the lanelets does not correlate to the order of the control_start lanelets - * + * */ lanelet::ConstLanelets getControlEndLanelets() const; @@ -115,7 +124,7 @@ class CarmaTrafficSignal : public lanelet::RegulatoryElement * @throw InvalidInputError if timestamps recorded somehow did not have full cycle */ boost::optional> predictState(boost::posix_time::ptime time_stamp); - + /** * @brief Return the stop_lines related to the entry lanelets in order if exists. */ @@ -124,16 +133,16 @@ class CarmaTrafficSignal : public lanelet::RegulatoryElement /** * @brief Return the stop_lines related to the specified entry lanelet - * @param llt entry_lanelet - * @return optional stop line linestring. - * Empty optional if no stopline, or no entry lanelets, or if specified entry lanelet is not recorded. + * @param llt entry_lanelet + * @return optional stop line linestring. + * Empty optional if no stopline, or no entry lanelets, or if specified entry lanelet is not recorded. */ Optional getConstStopLine(const ConstLanelet& llt); Optional getStopLine(const ConstLanelet& llt); explicit CarmaTrafficSignal(const lanelet::RegulatoryElementDataPtr& data); /** - * @brief: Creating one is not directly usable unless setStates is called + * @brief: Creating one is not directly usable unless setStates is called * Static helper function that creates a stop line data object based on the provided inputs * * @param id The lanelet::Id of this element diff --git a/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp b/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp index 570af43cf78..7436f1955a7 100755 --- a/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp +++ b/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp @@ -60,7 +60,7 @@ LineStrings3d CarmaTrafficSignal::stopLine() return getParameters(RoleName::RefLine); } -Optional CarmaTrafficSignal::getStopLine(const ConstLanelet& llt) +Optional CarmaTrafficSignal::getStopLine(const ConstLanelet& llt) { auto sl = stopLine(); if (sl.empty()) { @@ -78,15 +78,15 @@ Optional CarmaTrafficSignal::getStopLine(const ConstLanelet& llt) return sl.at(size_t(std::distance(llts.begin(), it))); } -Optional CarmaTrafficSignal::getConstStopLine(const ConstLanelet& llt) +Optional CarmaTrafficSignal::getConstStopLine(const ConstLanelet& llt) { Optional mutable_stop_line = getStopLine(llt); - + if (!mutable_stop_line) return boost::none; ConstLineString3d const_stop_line = mutable_stop_line.get(); - + return const_stop_line; } @@ -120,18 +120,13 @@ boost::optional> Ca return boost::none; } - if (recorded_time_stamps.size() == 1) // if only 1 timestamp recorded, this signal doesn't change - { - return std::pair(recorded_time_stamps.front().first, recorded_time_stamps.front().second); - } - if (lanelet::time::toSec(fixed_cycle_duration) < 1.0) // there are recorded states, but no fixed_cycle_duration means it is dynamic { if (recorded_time_stamps.size() != recorded_start_time_stamps.size()) { throw std::invalid_argument("recorded_start_time_stamps size is not equal to recorded_time_stamps size"); } - + // if requested time is BEFORE recorded states' time interval, return STOP_AND_REMAIN if (recorded_start_time_stamps.front() >= time_stamp) { @@ -141,11 +136,11 @@ boost::optional> Ca // if requested time is AFTER recorded states' time interval, return STOP_AND_REMAIN if (recorded_time_stamps.back().first <= time_stamp) { - auto end_infinity_time = timeFromSec(2147483647); //corresponds to 03:14:07 on Tuesday, 19 January 2038. - LOG_WARN_STREAM("CarmaTrafficSignal doesn't have enough state saved, so returning STOP_AND_REMAIN state! End_time: " << end_infinity_time); + auto end_infinity_time = timeFromSec(time::INFINITY_END_TIME_FOR_NOT_ENOUGH_STATES); //corresponds to 03:14:07 on Tuesday, 19 January 2038. + LOG_DEBUG_STREAM("CarmaTrafficSignal doesn't have enough state saved, so returning STOP_AND_REMAIN state! End_time: " << end_infinity_time); return std::pair(end_infinity_time, CarmaTrafficSignalState::STOP_AND_REMAIN); } - + // iterate through states in the dynamic states to get the signal. for (size_t i = 0; i < recorded_time_stamps.size(); i++) { @@ -155,15 +150,16 @@ boost::optional> Ca } } } - + + // This part of the code is used for predicting state if fixed_cycle_duration is set using setStates function // shift starting time to the future or to the past to fit input into a valid cycle boost::posix_time::time_duration accumulated_offset_duration; double offset_duration_dir = recorded_time_stamps.front().first > time_stamp ? -1.0 : 1.0; // -1 if past, +1 if time_stamp is in future int num_of_cycles = std::abs(lanelet::time::toSec(recorded_time_stamps.front().first - time_stamp) / lanelet::time::toSec(fixed_cycle_duration)); accumulated_offset_duration = durationFromSec( num_of_cycles * lanelet::time::toSec(fixed_cycle_duration)); - - if (offset_duration_dir < 0) + + if (offset_duration_dir < 0) { while (recorded_time_stamps.front().first - accumulated_offset_duration > time_stamp) { @@ -175,7 +171,7 @@ boost::optional> Ca { double end_time = lanelet::time::toSec(recorded_time_stamps[i].first) + offset_duration_dir * lanelet::time::toSec(accumulated_offset_duration); if (end_time >= lanelet::time::toSec(time_stamp)) - { + { return std::pair(timeFromSec(end_time), recorded_time_stamps[i].second); } } @@ -186,7 +182,7 @@ boost::optional> Ca lanelet::ConstLanelets CarmaTrafficSignal::getControlStartLanelets() const { return getParameters(CarmaTrafficSignalRoleNameString::ControlStart); -} +} lanelet::ConstLanelets CarmaTrafficSignal::getControlEndLanelets() const { @@ -235,4 +231,4 @@ namespace lanelet::RegisterRegulatoryElement reg; } // namespace -} // namespace lanelet \ No newline at end of file +} // namespace lanelet diff --git a/common/map_file_ros2/CMakeLists.txt b/common/map_file_ros2/CMakeLists.txt new file mode 100644 index 00000000000..ee07ddf1abd --- /dev/null +++ b/common/map_file_ros2/CMakeLists.txt @@ -0,0 +1,123 @@ +# Copyright (C) 2023 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.5) +project(map_file_ros2) + +# Declare carma package and check ROS version +find_package(carma_cmake_common REQUIRED) +carma_check_ros_version(2) +carma_package() + +find_package(PCL COMPONENTS REQUIRED) +find_package(CURL REQUIRED) + +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + + +# Includes +include_directories( + include + ${PCL_INCLUDE_DIRS} + ${CURL_INCLUDE_DIRS} +) + +# Build +ament_auto_add_library(get_file SHARED + lib/map_file/get_file.cpp +) +target_link_libraries(get_file ${CURL_LIBRARIES}) + +ament_auto_add_library(points_map_loader_lib SHARED + nodes/points_map_loader/points_map_loader.cpp +) +ament_auto_add_executable(points_map_loader_exec + nodes/points_map_loader/points_map_loader_node.cpp +) + +ament_auto_add_library(map_param_loader_lib SHARED + nodes/map_param_loader/map_param_loader.cpp +) +ament_auto_add_executable(map_param_loader_exec + nodes/map_param_loader/map_param_loader_node.cpp +) + +ament_auto_add_library(lanelet2_map_loader_lib SHARED + nodes/lanelet2_map_loader/lanelet2_map_loader.cpp +) +ament_auto_add_executable(lanelet2_map_loader_exec + nodes/lanelet2_map_loader/lanelet2_map_loader_node.cpp +) + +ament_auto_add_library(lanelet2_map_visualization_lib SHARED + nodes/lanelet2_map_visualization/lanelet2_map_visualization.cpp +) +ament_auto_add_executable(lanelet2_map_visualization_exec + nodes/lanelet2_map_visualization/lanelet2_map_visualization_node.cpp +) + +# Register component +rclcpp_components_register_nodes(points_map_loader_lib "points_map_loader::PointsMapLoader") + +rclcpp_components_register_nodes(map_param_loader_lib "map_param_loader::MapParamLoader") + +rclcpp_components_register_nodes(lanelet2_map_loader_lib "lanelet2_map_loader::Lanelet2MapLoader") + +rclcpp_components_register_nodes(lanelet2_map_visualization_lib "lanelet2_map_visualization::Lanelet2MapVisualization") + +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(points_map_loader_lib get_file ${PCL_LIBRARIES}) + +target_link_libraries( + points_map_loader_exec + points_map_loader_lib +) + +target_link_libraries( + map_param_loader_exec + map_param_loader_lib +) + +target_link_libraries( + lanelet2_map_loader_exec + lanelet2_map_loader_lib +) + +target_link_libraries( + lanelet2_map_visualization_exec + lanelet2_map_visualization_lib +) + +# Testing +if(BUILD_TESTING) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable + + ament_add_gtest(test_route + test/test_get_transform.cpp + ) + + ament_target_dependencies(test_route ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + + target_link_libraries(test_route map_param_loader_lib) + +endif() + +ament_auto_package( + INSTALL_TO_SHARE launch +) \ No newline at end of file diff --git a/common/map_file_ros2/include/get_file.hpp b/common/map_file_ros2/include/get_file.hpp new file mode 100644 index 00000000000..69db2e89c40 --- /dev/null +++ b/common/map_file_ros2/include/get_file.hpp @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#ifndef _GET_FILE_H_ +#define _GET_FILE_H_ + +#include +#include +#include + + +#define HTTP_HOSTNAME "133.6.148.90" +#define HTTP_PORT (80) +#define HTTP_USER "" +#define HTTP_PASSWORD "" + +class GetFile { +private: + std::string host_name_; + int port_; + std::string user_; + std::string password_; + int sock; + struct sockaddr_in server; + +public: + GetFile(); + explicit GetFile(const std::string& host_name, int port, const std::string& user, const std::string& password); + + int GetHTTPFile(const std::string& value); +}; + + +#endif /* _GET_FILE_H_ */ \ No newline at end of file diff --git a/common/map_file_ros2/include/lanelet2_map_loader.hpp b/common/map_file_ros2/include/lanelet2_map_loader.hpp new file mode 100644 index 00000000000..36204969d8f --- /dev/null +++ b/common/map_file_ros2/include/lanelet2_map_loader.hpp @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace lanelet2_map_loader{ +void printUsage() +{ + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_file_ros2"), "Usage:"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_file_ros2"), "ros2 launch map_file lanelet2_map_loader lanelet2_filename:=[.OSM]:"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_file_ros2"), "ros2 launch map_file lanelet2_map_loader load_type:=download [X] [Y]: WARNING not implemented"); +} + +class Lanelet2MapLoader : public carma_ros2_utils::CarmaLifecycleNode +{ + private: + rclcpp::Publisher::SharedPtr map_bin_pub; + + // Parameters + std::string load_type; + std::string lanelet2_filename; + + rclcpp::TimerBase::SharedPtr timer_; + autoware_lanelet2_msgs::msg::MapBin map_bin_msg_; + + void timer_callback(); + + + public: + Lanelet2MapLoader(const rclcpp::NodeOptions &options); + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + +}; + +} //namespace lanelet2_map_loader + diff --git a/common/map_file_ros2/include/lanelet2_map_visualization.hpp b/common/map_file_ros2/include/lanelet2_map_visualization.hpp new file mode 100644 index 00000000000..bd97b7fba68 --- /dev/null +++ b/common/map_file_ros2/include/lanelet2_map_visualization.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +static bool g_viz_lanelets_centerline = true; + +namespace lanelet2_map_visualization{ + class Lanelet2MapVisualization : public carma_ros2_utils::CarmaLifecycleNode + { + private: + + rclcpp::Publisher::SharedPtr g_map_pub; + rclcpp::Subscription ::SharedPtr bin_map_sub; + + void insertMarkerArray(visualization_msgs::msg::MarkerArray* a1, const visualization_msgs::msg::MarkerArray& a2); + void setColor(std_msgs::msg::ColorRGBA* cl, double r, double g, double b, double a); + void binMapCallback(const autoware_lanelet2_msgs::msg::MapBin::SharedPtr msg); + + public: + Lanelet2MapVisualization(const rclcpp::NodeOptions &options); + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + + }; +} + + + diff --git a/common/map_file_ros2/include/map_param_loader.hpp b/common/map_file_ros2/include/map_param_loader.hpp new file mode 100644 index 00000000000..286bc6bffa0 --- /dev/null +++ b/common/map_file_ros2/include/map_param_loader.hpp @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace map_param_loader +{ + class MapParamLoader : public carma_ros2_utils::CarmaLifecycleNode + { + private: + rclcpp::Publisher::SharedPtr georef_pub_; + std::shared_ptr timer_; + std::string target_frame_; + + void timer_callback(); + + // Broadcast the input transform to tf_static. + void broadcastTransform(const tf2::Transform& transform); + + //parameters + std::string lanelet2_filename; + bool broadcast_earth_frame = false; + + public: + explicit MapParamLoader(const rclcpp::NodeOptions &options); + + // Get transform from map_frame to ecef_frame using respective proj strings. + tf2::Transform getTransform(const std::string& map_frame); + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); + + }; + +} \ No newline at end of file diff --git a/common/map_file_ros2/include/points_map_loader.hpp b/common/map_file_ros2/include/points_map_loader.hpp new file mode 100644 index 00000000000..4baf0299a00 --- /dev/null +++ b/common/map_file_ros2/include/points_map_loader.hpp @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "tf2_ros/buffer.h" + +#include + +namespace points_map_loader +{ + + struct Area { + std::string path; + double x_min; + double y_min; + double z_min; + double x_max; + double y_max; + double z_max; + }; + + constexpr int DEFAULT_UPDATE_RATE = 1000; // ms + constexpr double MARGIN_UNIT = 100; // meter + constexpr int ROUNDING_UNIT = 1000; // meter + const std::string AREALIST_FILENAME = "arealist.txt"; + const std::string TEMPORARY_DIRNAME = "/tmp/"; + + typedef std::vector AreaList; + typedef std::vector> Tbl; + + int fallback_rate; + double margin; + bool can_download; + + rclcpp::Time gnss_time; + rclcpp::Time current_time; + + std_msgs::msg::Bool stat_msg; + AreaList all_areas; + AreaList downloaded_areas; + std::mutex downloaded_areas_mtx; + std::vector cached_arealist_paths; + + class PointsMapLoader : public carma_ros2_utils::CarmaLifecycleNode { + private: + + std::queue queue_; // takes priority over look_ahead_queue_ + std::queue look_ahead_queue_; + std::mutex mtx_; + std::condition_variable cv_; + + // Publishers + rclcpp::Publisher::SharedPtr pcd_pub; + rclcpp::Publisher::SharedPtr stat_pub; + + // Subscribers + rclcpp::Subscription::SharedPtr gnss_sub; + rclcpp::Subscription::SharedPtr current_sub; + rclcpp::Subscription::SharedPtr initial_sub; + rclcpp::Subscription::SharedPtr waypoints_sub; + + GetFile gf; + + // Parameters + int update_rate = DEFAULT_UPDATE_RATE; + std::string area = ""; + std::string load_type = ""; + std::string path_area_list = ""; + std::vector pcd_path = {}; + std::string host_name = HTTP_HOSTNAME; + int port = HTTP_PORT; + std::string user = HTTP_USER; + std::string password = HTTP_PASSWORD; + + + void enqueue(const geometry_msgs::msg::Point& p); + void enqueue_look_ahead(const geometry_msgs::msg::Point& p); + void clear(); + void clear_look_ahead(); + geometry_msgs::msg::Point dequeue(); + + void publish_pcd(sensor_msgs::msg::PointCloud2 pcd, const int* errp); + + Tbl read_csv(const std::string& path); + void write_csv(const std::string& path, const Tbl& tbl); + AreaList read_arealist(const std::string& path); + void write_arealist(const std::string& path, const AreaList& areas); + bool is_downloaded(const std::string& path); + bool is_in_area(double x, double y, const Area& area, double m); + std::string create_location(int x, int y); + void cache_arealist(const Area& area, AreaList& areas); + int download(GetFile gf, const std::string& tmp, const std::string& loc, const std::string& filename); + + void download_map(); + + sensor_msgs::msg::PointCloud2 create_pcd(const geometry_msgs::msg::Point& p); + sensor_msgs::msg::PointCloud2 create_pcd(const std::vector& pcd_paths, int* ret_err); + + void publish_gnss_pcd(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void publish_dragged_pcd(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void publish_current_pcd(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void request_lookahead_download(const autoware_msgs::msg::LaneArray::SharedPtr msg); + + rclcpp::TimerBase::SharedPtr timer_; + void timer_callback(); + + void run(); + + public: + explicit PointsMapLoader(const rclcpp::NodeOptions &); + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &prev_state); + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &prev_state); + + + }; + + void print_usage() + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_file_ros2"), "Usage:"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_file_ros2"), "ros2 launch map_file_ros2 points_map_loader.launch.py --load_type:=noupdate --pcd_path_parameter:=[PCD]..."); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_file_ros2"), "ros2 launch map_file_ros2 points_map_loader.launch.py --path_area_list:={1x1|3x3|5x5|7x7|9x9} --pcd_path_parameter:=[PCD]..."); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_file_ros2"), "ros2 launch map_file_ros2 points_map_loader.launch --path_area_list:{1x1|3x3|5x5|7x7|9x9} --load_type:=download"); + } +} \ No newline at end of file diff --git a/common/map_file_ros2/launch/lanelet2_map_loader.launch.py b/common/map_file_ros2/launch/lanelet2_map_loader.launch.py new file mode 100644 index 00000000000..39d32ed9cba --- /dev/null +++ b/common/map_file_ros2/launch/lanelet2_map_loader.launch.py @@ -0,0 +1,52 @@ +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument + +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + +def generate_launch_description(): + + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument(name = 'log_level', default_value='WARN') + + load_type = LaunchConfiguration('load_type') + declare_load_type = DeclareLaunchArgument(name='load_type', default_value='') + + + lanelet2_filename = LaunchConfiguration('lanelet2_filename') + declare_lanelet2_filename = DeclareLaunchArgument(name='lanelet2_filename', default_value='') + + lanelet2_map_loader_container = ComposableNodeContainer( + package='carma_ros2_utils', + name='lanelet2_map_loader_container', + executable='carma_component_container_mt', + namespace=GetCurrentNamespace(), + composable_node_descriptions=[ + ComposableNode( + package='map_file_ros2', + plugin='lanelet2_map_loader::Lanelet2MapLoader', + name='lanelet2_map_loader', + parameters=[ + {'load_type' : load_type}, + {'lanelet2_filename' : lanelet2_filename} + ] + ), + ComposableNode( + package='map_file_ros2', + plugin='lanelet2_map_visualization::Lanelet2MapVisualization', + name='lanelet2_map_visualization' + ) + + ]) + + return LaunchDescription([ + declare_log_level_arg, + declare_load_type, + declare_lanelet2_filename, + lanelet2_map_loader_container, + ]) \ No newline at end of file diff --git a/common/map_file_ros2/launch/map_loader.launch.py b/common/map_file_ros2/launch/map_loader.launch.py new file mode 100644 index 00000000000..38d2baa5128 --- /dev/null +++ b/common/map_file_ros2/launch/map_loader.launch.py @@ -0,0 +1,92 @@ +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.substitutions import ThisLaunchFileDir +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.conditions import IfCondition +from launch.substitutions import PythonExpression + +from launch.substitutions import PathJoinSubstitution + + +import os + + +def generate_launch_description(): + + + load_type = LaunchConfiguration('load_type') + declare_load_type = DeclareLaunchArgument(name='load_type', default_value='noupdate', description="Enum of the map loading approach to use. Can be 'download', 'noupdate', or 'arealist'") + + single_pcd_path = LaunchConfiguration('single_pcd_path') + declare_single_pcd_path = DeclareLaunchArgument(name='single_pcd_path', default_value='pcd_map.pcd', description='Path to the map pcd file if using the noupdate load type') + + area = LaunchConfiguration('area') + declare_area = DeclareLaunchArgument(name='area', default_value='1x1', description='Dimensions of the square of cells loaded at runtime using the download and arealist load types') + + arealist_path = LaunchConfiguration('arealist_path') + declare_arealist_path = DeclareLaunchArgument(name='arealist_path', default_value='arealist.txt', description='Path to the arealist.txt file which contains the paths and dimensions of each map cell to load with the arealist load_type') + + area_paths=LaunchConfiguration('area_paths') + declare_area_paths = DeclareLaunchArgument(name='area_paths', default_value='', description='List of cell paths to load when using the arealist load type. If this is not filled all the cells will be loaded based on the arealist.txt file') + + + download_launch_group = GroupAction( + condition = IfCondition(PythonExpression(["'",load_type,"' == 'download'"])), + actions = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ThisLaunchFileDir(), '/launch/points_map_loader.launch.py']) + ]), + launch_arguments = { + 'load_type' : load_type, + 'area' : area, + }.items() + ) + ] + ) + + noupdate_launch_group = GroupAction( + condition = IfCondition(PythonExpression(["'",load_type,"' == 'noupdate'"])), + actions = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ThisLaunchFileDir(), 'points_map_loader.launch.py']) + ]), + launch_arguments = { + 'load_type' : load_type, + 'pcd_path_parameter' : single_pcd_path + }.items() + ), + ] + ) + + arealist_launch_group = GroupAction( + condition = IfCondition(PythonExpression(["'",load_type,"' == 'arealist'"])), + actions = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ThisLaunchFileDir(), '/points_map_loader.launch.py']) + ]), + launch_arguments = { + 'load_type' : load_type, + 'area' : area, + 'arealist_path' : arealist_path, + 'area_paths' : area_paths, + }.items(), + ) + ] + ) + + return LaunchDescription([ + declare_load_type, + declare_single_pcd_path, + declare_area, + declare_arealist_path, + declare_area_paths, + download_launch_group, + noupdate_launch_group, + arealist_launch_group + ]) \ No newline at end of file diff --git a/common/map_file_ros2/launch/map_param_loader.launch.py b/common/map_file_ros2/launch/map_param_loader.launch.py new file mode 100644 index 00000000000..cf20a944308 --- /dev/null +++ b/common/map_file_ros2/launch/map_param_loader.launch.py @@ -0,0 +1,48 @@ +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument + +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace +\ + +import os + +def generate_launch_description(): + + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument(name = 'log_level', default_value='WARN') + + broadcast_earth_frame = LaunchConfiguration('broadcast_earth_frame') + declare_broadcast_earth_frame = DeclareLaunchArgument(name='broadcast_earth_frame', default_value='false', + description='If true this node will attempt to compute a homogenious transformation matrix between earth and the map frame origin. NOTE: Not all projections can be linearly described in this way and such a transformation may be inaccurate') + + file_name = LaunchConfiguration('file_name') + declare_file_name = DeclareLaunchArgument(name='file_name', default_value='') + + + map_param_loader_container = ComposableNodeContainer( + package='carma_ros2_utils', + name='map_param_loader_container', + executable='carma_component_container_mt', + namespace=GetCurrentNamespace(), + composable_node_descriptions=[ + ComposableNode( + package='map_file_ros2', + plugin='map_param_loader::MapParamLoader', + name='map_param_loader', + parameters=[ + {'broadcast_earth_frame' : broadcast_earth_frame}, + {'file_name' : file_name} + ] + ) + ]) + + return LaunchDescription([ + declare_log_level_arg, + declare_broadcast_earth_frame, + declare_file_name, + map_param_loader_container + ]) \ No newline at end of file diff --git a/common/map_file_ros2/launch/points_map_loader.launch.py b/common/map_file_ros2/launch/points_map_loader.launch.py new file mode 100644 index 00000000000..66129cb5c57 --- /dev/null +++ b/common/map_file_ros2/launch/points_map_loader.launch.py @@ -0,0 +1,86 @@ +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument + +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + +def generate_launch_description(): + + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument(name = 'log_level', default_value='WARN') + + update_rate = LaunchConfiguration('update_rate') + declare_update_rate = DeclareLaunchArgument(name='update_rate', default_value='1000') + + area = LaunchConfiguration('area') + declare_area = DeclareLaunchArgument(name='area', default_value="") + + load_type = LaunchConfiguration('load_type') + declare_load_type = DeclareLaunchArgument(name='load_type', default_value="") + + path_area_list = LaunchConfiguration('path_area_list') + declare_path_area_list = DeclareLaunchArgument(name='path_area_list', default_value="") + + pcd_path_parameter = LaunchConfiguration('pcd_path_parameter') + declare_pcd_path_parameter = DeclareLaunchArgument(name='pcd_path_parameter', default_value='["",""]') + + host_name = LaunchConfiguration('host_name') + declare_host_name = DeclareLaunchArgument(name='host_name', default_value="133.6.148.90") + + port = LaunchConfiguration('port') + declare_port = DeclareLaunchArgument(name='port', default_value='80') + + user = LaunchConfiguration('user') + declare_user = DeclareLaunchArgument(name='user', default_value="") + + password = LaunchConfiguration('password') + declare_password = DeclareLaunchArgument(name='password', default_value="") + + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='map_file_nodes_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + ComposableNode( + package='map_file_ros2', + plugin='points_map_loader::PointsMapLoader', + name='point_map_loader_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'log_level':log_level} + ], + parameters=[ + {'update_rate': update_rate}, + {'area': area}, + {'load_type': load_type}, + {'path_area_list': path_area_list}, + {'pcd_path_parameter': pcd_path_parameter}, + {'host_name': host_name}, + {'port' : port}, + {'user': user}, + {'password': password} + ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + declare_update_rate, + declare_area, + declare_load_type, + declare_path_area_list, + declare_pcd_path_parameter, + declare_host_name, + declare_port, + declare_user, + declare_password, + container + ]) \ No newline at end of file diff --git a/common/map_file_ros2/lib/map_file/get_file.cpp b/common/map_file_ros2/lib/map_file/get_file.cpp new file mode 100644 index 00000000000..a070aa3bca0 --- /dev/null +++ b/common/map_file_ros2/lib/map_file/get_file.cpp @@ -0,0 +1,105 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + + +GetFile::GetFile() + : GetFile(HTTP_HOSTNAME, HTTP_PORT, HTTP_USER, HTTP_PASSWORD) +{ +} + +GetFile::GetFile(const std::string& host_name, int port, const std::string& user, const std::string& password) + : host_name_(host_name), port_(port), user_(user), password_(password) +{ +} + +typedef std::basic_ofstream::__filebuf_type buffer_t; +typedef __gnu_cxx::stdio_filebuf io_buffer_t; +static FILE* cfile(buffer_t* const fb) { + return (static_cast(fb))->file(); +} + +int GetFile::GetHTTPFile(const std::string& value) +{ + CURL *curl; + + curl = curl_easy_init(); + if (! curl) { + std::cerr << "curl_easy_init failed" << std::endl; + return -1; + } + + std::string filepath("/tmp/" + value); + std::ofstream ofs; + ofs.open(filepath); + if (! ofs.is_open()) { + std::cerr << "cannot open /tmp/" << value << std::endl; + curl_easy_cleanup(curl); + return -2; + } + + std::ostringstream urlss; + urlss << "http://" << host_name_ << ":" << port_ << "/" << value; + curl_easy_setopt(curl, CURLOPT_URL, urlss.str().c_str()); + curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1); + curl_easy_setopt(curl, CURLOPT_MAXREDIRS, 5); + curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, fwrite); + curl_easy_setopt(curl, CURLOPT_WRITEDATA, cfile(ofs.rdbuf())); + if (user_ != "" && password_ != "") { + std::string userpwd = user_ + ":" + password_; + curl_easy_setopt(curl, CURLOPT_HTTPAUTH, CURLAUTH_DIGEST); + curl_easy_setopt(curl, CURLOPT_USERPWD, userpwd.c_str()); + } + CURLcode res = curl_easy_perform(curl); + if (res != CURLE_OK) { + std::cerr << "curl_easy_perform failed: " << + curl_easy_strerror(res) << std::endl; + curl_easy_cleanup(curl); + ofs.close(); + unlink(filepath.c_str()); + return -3; + } + long response_code; + res = curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &response_code); + if (res != CURLE_OK) { + std::cerr << "curl_easy_getinfo failed: " << + curl_easy_strerror(res) << std::endl; + curl_easy_cleanup(curl); + ofs.close(); + unlink(filepath.c_str()); + return -4; + } + curl_easy_cleanup(curl); + ofs.close(); + if (response_code != 200) { + unlink(filepath.c_str()); + return -5; + } + + return 0; +} diff --git a/common/map_file_ros2/nodes/lanelet2_map_loader/lanelet2_map_loader.cpp b/common/map_file_ros2/nodes/lanelet2_map_loader/lanelet2_map_loader.cpp new file mode 100644 index 00000000000..1e02248f48c --- /dev/null +++ b/common/map_file_ros2/nodes/lanelet2_map_loader/lanelet2_map_loader.cpp @@ -0,0 +1,126 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace lanelet2_map_loader{ + + Lanelet2MapLoader::Lanelet2MapLoader(const rclcpp::NodeOptions &options) : carma_ros2_utils::CarmaLifecycleNode(options) + { + // Declare Parameters + load_type = declare_parameter("load_type", load_type); + lanelet2_filename = declare_parameter("lanelet2_filename", lanelet2_filename); + + // Get Parameters + get_parameter("load_type", load_type); + get_parameter("lanelet2_filename", lanelet2_filename); + } + + carma_ros2_utils::CallbackReturn Lanelet2MapLoader::handle_on_configure(const rclcpp_lifecycle::State &) + { + + lanelet::ErrorMessages errors; + lanelet::LaneletMapPtr map; + + int projector_type = 0; + std::string target_frame; + + // Parse geo reference info from the lanelet map (.osm) + lanelet::io_handlers::AutowareOsmParser::parseMapParams(lanelet2_filename, &projector_type, &target_frame); + + if(projector_type == 0) + { + lanelet::projection::MGRSProjector projector; + map = lanelet::load(lanelet2_filename, projector, &errors); + } else if(projector_type == 1) + { + lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str()); + map = lanelet::load(lanelet2_filename, local_projector, &errors); + } + + for(const auto &error: errors) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_file_ros2"), error); + } + if(!errors.empty()) + { + // return EXIT_FAILURE; + return CallbackReturn::FAILURE; + } + + std::string format_version, map_version; + lanelet::io_handlers::AutowareOsmParser::parseVersions(lanelet2_filename, &format_version, &map_version); + + autoware_lanelet2_msgs::msg::MapBin map_bin_msg; + map_bin_msg.header.stamp = this->now(); + map_bin_msg.header.frame_id = "map"; + map_bin_msg.format_version = format_version; + if (!map_version.empty()) { + map_bin_msg.map_version = stoi(map_version); // CARMA Uses monotonically increasing map version numbers in carma_wm + } + lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); + map_bin_msg_ = map_bin_msg; + + // Set Publishers + // NOTE: Currently, intra-process comms must be disabled for the following two publishers that are transient_local: https://github.com/ros2/rclcpp/issues/1753 + rclcpp::PublisherOptions intra_proc_disabled; + intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this PublisherOptions object + // Create a publisher that will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too + auto pub_qos_transient_local = rclcpp::QoS(rclcpp::KeepLast(1)); // A publisher with this QoS will store the "Last" message that it has sent on the topic + pub_qos_transient_local.transient_local(); + + map_bin_pub = create_publisher("lanelet_map_bin", pub_qos_transient_local, intra_proc_disabled); //Make latched + + timer_ = this->create_wall_timer(std::chrono::seconds(1), std::bind(&Lanelet2MapLoader::timer_callback, this)); + + return CallbackReturn::SUCCESS; + } + + void Lanelet2MapLoader::timer_callback(){ + + if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE){ + + if(!map_bin_msg_.header.frame_id.empty()){ + map_bin_msg_.header.stamp = this->now(); + map_bin_pub->publish(map_bin_msg_); + timer_->cancel(); + } + + } + } + + +} //namespace lanelet2_map_loader + + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(lanelet2_map_loader::Lanelet2MapLoader) \ No newline at end of file diff --git a/common/map_file_ros2/nodes/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/common/map_file_ros2/nodes/lanelet2_map_loader/lanelet2_map_loader_node.cpp new file mode 100644 index 00000000000..e9a803f8dce --- /dev/null +++ b/common/map_file_ros2/nodes/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(rclcpp::NodeOptions()); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/common/map_file_ros2/nodes/lanelet2_map_visualization/lanelet2_map_visualization.cpp b/common/map_file_ros2/nodes/lanelet2_map_visualization/lanelet2_map_visualization.cpp new file mode 100644 index 00000000000..d71197000f8 --- /dev/null +++ b/common/map_file_ros2/nodes/lanelet2_map_visualization/lanelet2_map_visualization.cpp @@ -0,0 +1,120 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + + +#include + +namespace lanelet2_map_visualization{ + + Lanelet2MapVisualization::Lanelet2MapVisualization(const rclcpp::NodeOptions &options) : carma_ros2_utils::CarmaLifecycleNode(options) + { + + + } + + carma_ros2_utils::CallbackReturn Lanelet2MapVisualization::handle_on_configure(const rclcpp_lifecycle::State &) + { + + // Set Publisher + // NOTE: Currently, intra-process comms must be disabled for the following two publishers that are transient_local: https://github.com/ros2/rclcpp/issues/1753 + rclcpp::PublisherOptions intra_proc_disabled; + intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this PublisherOptions object + // Create a publisher that will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too + auto pub_qos_transient_local = rclcpp::QoS(rclcpp::KeepLast(1)); // A publisher with this QoS will store the "Last" message that it has sent on the topic + pub_qos_transient_local.transient_local(); + g_map_pub = create_publisher("lanelet2_map_viz", pub_qos_transient_local, intra_proc_disabled); + + //Set Subscriber + rclcpp::SubscriptionOptions sub_options; + sub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto sub_qos_transient_local = rclcpp::QoS(rclcpp::KeepLast(1)); + sub_qos_transient_local.transient_local(); + bin_map_sub = create_subscription("lanelet_map_bin", sub_qos_transient_local, std::bind(&Lanelet2MapVisualization::binMapCallback, this, std::placeholders::_1), sub_options); + + return CallbackReturn::SUCCESS; + } + + + void Lanelet2MapVisualization::insertMarkerArray(visualization_msgs::msg::MarkerArray* a1, const visualization_msgs::msg::MarkerArray& a2) + { + a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); + } + + void Lanelet2MapVisualization::setColor(std_msgs::msg::ColorRGBA* cl, double r, double g, double b, double a) + { + cl->r = r; + cl->g = g; + cl->b = b; + cl->a = a; + } + + void Lanelet2MapVisualization::binMapCallback(const autoware_lanelet2_msgs::msg::MapBin::SharedPtr msg) + { + lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); + + lanelet::utils::conversion::fromBinMsg(*msg, viz_lanelet_map); + RCLCPP_INFO(get_logger(), "Map loaded"); + + // get lanelets etc to visualize + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(viz_lanelet_map); + lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + lanelet::ConstLanelets crosswalk_lanelets = lanelet::utils::query::crosswalkLanelets(all_lanelets); + + std::vector tl_stop_lines = lanelet::utils::query::getTrafficLightStopLines(road_lanelets); + std::vector ss_stop_lines = lanelet::utils::query::getStopSignStopLines(road_lanelets); + std::vector tl_reg_elems = lanelet::utils::query::trafficLights(all_lanelets); + std::vector aw_tl_reg_elems = + lanelet::utils::query::autowareTrafficLights(all_lanelets); + + std_msgs::msg::ColorRGBA cl_road, cl_cross, cl_ll_borders, cl_tl_stoplines, cl_ss_stoplines, cl_trafficlights; + setColor(&cl_road, 0.2, 0.7, 0.7, 0.3); + setColor(&cl_cross, 0.2, 0.7, 0.2, 0.3); + setColor(&cl_ll_borders, 1.0, 1.0, 1.0, 1.0); + setColor(&cl_tl_stoplines, 1.0, 0.5, 0.0, 0.5); + setColor(&cl_ss_stoplines, 1.0, 0.0, 0.0, 0.5); + setColor(&cl_trafficlights, 0.7, 0.7, 0.7, 0.8); + + visualization_msgs::msg::MarkerArray map_marker_array; + + insertMarkerArray(&map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( + road_lanelets, cl_ll_borders, g_viz_lanelets_centerline)); + insertMarkerArray(&map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "road_lanelets", road_lanelets, cl_road)); + insertMarkerArray(&map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); + insertMarkerArray(&map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray( + road_lanelets)); + insertMarkerArray(&map_marker_array, lanelet::visualization::lineStringsAsMarkerArray( + tl_stop_lines, "traffic_light_stop_lines", cl_tl_stoplines, 0.5)); + insertMarkerArray(&map_marker_array, lanelet::visualization::lineStringsAsMarkerArray( + ss_stop_lines, "stop_sign_stop_lines", cl_ss_stoplines, 0.5)); + insertMarkerArray(&map_marker_array, lanelet::visualization::autowareTrafficLightsAsMarkerArray( + aw_tl_reg_elems, cl_trafficlights)); + + RCLCPP_INFO(get_logger(), "Visualizing lanelet2 map with %lu lanelets, %lu stop lines, and %lu traffic lights", + all_lanelets.size(), tl_stop_lines.size() + ss_stop_lines.size(), aw_tl_reg_elems.size()); + + g_map_pub->publish(map_marker_array); + } + + +} //namespace lanelet2_map_visualization + + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(lanelet2_map_visualization::Lanelet2MapVisualization) \ No newline at end of file diff --git a/common/map_file_ros2/nodes/lanelet2_map_visualization/lanelet2_map_visualization_node.cpp b/common/map_file_ros2/nodes/lanelet2_map_visualization/lanelet2_map_visualization_node.cpp new file mode 100644 index 00000000000..cbd21afa6f9 --- /dev/null +++ b/common/map_file_ros2/nodes/lanelet2_map_visualization/lanelet2_map_visualization_node.cpp @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(rclcpp::NodeOptions()); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/common/map_file_ros2/nodes/map_param_loader/map_param_loader.cpp b/common/map_file_ros2/nodes/map_param_loader/map_param_loader.cpp new file mode 100644 index 00000000000..73a20050d08 --- /dev/null +++ b/common/map_file_ros2/nodes/map_param_loader/map_param_loader.cpp @@ -0,0 +1,133 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include "map_param_loader.hpp" + + +namespace map_param_loader +{ + namespace std_ph = std::placeholders; + + MapParamLoader::MapParamLoader(const rclcpp::NodeOptions &options) : carma_ros2_utils::CarmaLifecycleNode(options) + { + + } + + carma_ros2_utils::CallbackReturn MapParamLoader::handle_on_configure(const rclcpp_lifecycle::State &) + { + lanelet2_filename = declare_parameter("file_name", lanelet2_filename); + broadcast_earth_frame = declare_parameter("broadcast_earth_frame", broadcast_earth_frame); + + // NOTE: Currently, intra-process comms must be disabled for the following two publishers that are transient_local: https://github.com/ros2/rclcpp/issues/1753 + rclcpp::PublisherOptions intra_proc_disabled; + intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this PublisherOptions object + + // Create a publisher that will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too + auto pub_qos_transient_local = rclcpp::QoS(rclcpp::KeepAll()); // A publisher with this QoS will store all messages that it has sent on the topic + pub_qos_transient_local.transient_local(); // A publisher with this QoS will re-send all (when KeepAll is used) messages to all late-joining subscribers + // NOTE: The subscriber's QoS must be set to transient_local() as well for earlier messages to be resent to the later-joiner. + + georef_pub_ = this->create_publisher("georeference", pub_qos_transient_local, intra_proc_disabled); + + return CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn MapParamLoader::handle_on_activate(const rclcpp_lifecycle::State &) + { + int projector_type = 1; // default value + + // Parse geo reference info from the lanelet map (.osm) + lanelet::io_handlers::AutowareOsmParser::parseMapParams(lanelet2_filename, &projector_type, &target_frame_); + + if (broadcast_earth_frame) { + // Get the transform to ecef (when parsed target_frame is map_frame) + tf2::Transform tf = getTransform(target_frame_); + + // Broadcast the transform + broadcastTransform(tf); + } + + timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&MapParamLoader::timer_callback, this)); + + return CallbackReturn::SUCCESS; + } + + void MapParamLoader::timer_callback() + { + std_msgs::msg::String georef_msg; + georef_msg.data = target_frame_; + georef_pub_->publish(georef_msg); + } + + + // Get transform from map_frame coord to ecef_frame coord using respective proj strings + tf2::Transform MapParamLoader::getTransform(const std::string& map_frame) + { + + lanelet::projection::LocalFrameProjector local_projector(map_frame.c_str()); + + tf2::Matrix3x3 rot_mat, id = tf2::Matrix3x3::getIdentity(); + lanelet::BasicPoint3d origin_in_map{0,0,0}, origin_in_ecef; + + // Solve map_to_ecef transformation + // Get translation of map with respect to ecef + origin_in_ecef = local_projector.projectECEF(origin_in_map, 1); + + // Solve rotation matrix using (1,0,0), (0,1,0), (0,0,1) vectors in map + for (auto i = 0; i < 3; i ++) + { + lanelet::BasicPoint3d rot_mat_row = local_projector.projectECEF(lanelet::BasicPoint3d{id[i][0],id[i][1],id[i][2]}, 1) - origin_in_ecef; + rot_mat[i][0] = rot_mat_row[0]; + rot_mat[i][1] = rot_mat_row[1]; + rot_mat[i][2] = rot_mat_row[2]; + } + // Transpose due to the way tf2::Matrix3x3 supposed to be stored. + tf2::Vector3 v_origin_in_ecef{origin_in_ecef[0],origin_in_ecef[1],origin_in_ecef[2]}; + tf2::Transform tf(rot_mat.transpose(), v_origin_in_ecef); + + // map_to_ecef tf + return tf; + } + + // broadcast the transform to tf_static topic + void MapParamLoader::broadcastTransform(const tf2::Transform& transform) + { + static tf2_ros::StaticTransformBroadcaster br(this); + geometry_msgs::msg::TransformStamped transformStamped; + + tf2::Vector3 translation = transform.getOrigin(); + tf2::Quaternion rotation = transform.getRotation(); + + transformStamped.header.stamp = rclcpp::Time(0,0); //TODO:: Check if node->now() needed + transformStamped.header.frame_id = "earth"; + transformStamped.child_frame_id = "map"; + transformStamped.transform.translation.x = translation[0]; + transformStamped.transform.translation.y = translation[1]; + transformStamped.transform.translation.z = translation[2]; + transformStamped.transform.rotation.x = rotation[0]; + transformStamped.transform.rotation.y = rotation[1]; + transformStamped.transform.rotation.z = rotation[2]; + transformStamped.transform.rotation.w = rotation[3]; + br.sendTransform(transformStamped); + } + +} // namespace map_param_loader + + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(map_param_loader::MapParamLoader) \ No newline at end of file diff --git a/common/map_file_ros2/nodes/map_param_loader/map_param_loader_node.cpp b/common/map_file_ros2/nodes/map_param_loader/map_param_loader_node.cpp new file mode 100644 index 00000000000..a72083c8f74 --- /dev/null +++ b/common/map_file_ros2/nodes/map_param_loader/map_param_loader_node.cpp @@ -0,0 +1,30 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include "map_param_loader.hpp" + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(rclcpp::NodeOptions()); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/common/map_file_ros2/nodes/points_map_loader/points_map_loader.cpp b/common/map_file_ros2/nodes/points_map_loader/points_map_loader.cpp new file mode 100644 index 00000000000..a81110106a5 --- /dev/null +++ b/common/map_file_ros2/nodes/points_map_loader/points_map_loader.cpp @@ -0,0 +1,540 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +// #include + + +namespace points_map_loader { + + + PointsMapLoader::PointsMapLoader(const rclcpp::NodeOptions &options) : carma_ros2_utils::CarmaLifecycleNode(options) + { + // Initialize parameters + update_rate = declare_parameter("update_rate", update_rate); + + area = declare_parameter("area", area); + load_type = declare_parameter("load_type", load_type); + path_area_list = declare_parameter("path_area_list", path_area_list); + // parameter as lists + declare_parameter("pcd_path_parameter", pcd_path); + + host_name = declare_parameter("host_name", host_name); + port = declare_parameter("port", port); + user = declare_parameter("user", user); + password = declare_parameter("password", password); + + } + + carma_ros2_utils::CallbackReturn PointsMapLoader::handle_on_configure(const rclcpp_lifecycle::State &prev_state) + { + // Get Parameters + get_parameter("update_rate", update_rate); + get_parameter("area", area); + get_parameter("load_type", load_type); + get_parameter("path_area_list", path_area_list); + + // path pcd - list of strings + rclcpp::Parameter pcd_path_parameter = get_parameter("pcd_path_parameter"); + pcd_path = pcd_path_parameter.as_string_array(); + + + // Create publishers + // NOTE: Currently, intra-process comms must be disabled for the following two publishers that are transient_local: https://github.com/ros2/rclcpp/issues/1753 + rclcpp::PublisherOptions intra_proc_disabled; + intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this PublisherOptions object + // Create a publisher that will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too + auto pub_qos_transient_local = rclcpp::QoS(rclcpp::KeepLast(1)); // A publisher with this QoS will store the "Last" message that it has sent on the topic + pub_qos_transient_local.transient_local(); + + pcd_pub = create_publisher("points_map", pub_qos_transient_local, intra_proc_disabled); //Make latched + stat_pub = create_publisher("pmap_stat", pub_qos_transient_local, intra_proc_disabled); //Make latched + + return CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn PointsMapLoader::handle_on_activate(const rclcpp_lifecycle::State &prev_state) + { + timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&PointsMapLoader::timer_callback, this)); + return CallbackReturn::SUCCESS; + } + + void PointsMapLoader::timer_callback() + { + if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE){ + run(); + timer_->cancel(); + } + } + + void PointsMapLoader::run(){ + if (load_type == "noupdate") + margin = -1; + else if (area == "1x1") + margin = 0; + else if (area == "3x3") + margin = MARGIN_UNIT * 1; + else if (area == "5x5") + margin = MARGIN_UNIT * 2; + else if (area == "7x7") + margin = MARGIN_UNIT * 3; + else if (area == "9x9") + margin = MARGIN_UNIT * 4; + else { + print_usage(); + RCLCPP_ERROR_STREAM(get_logger(), "Invalid area argument"); + + } + + std::string arealist_path; + std::vector pcd_paths; + if (margin < 0) { + can_download = false; + // If area = no_update get pcd paths + pcd_paths.insert(pcd_paths.end(), pcd_path.begin(), pcd_path.end()); + } else { + if (load_type == "download") + { + can_download = true; + + get_parameter("host_name", host_name); + get_parameter("port", port); + get_parameter("user", user); + get_parameter("password", password); + + gf = GetFile(host_name, port, user, password); + } + else{ + can_download = false; + arealist_path += path_area_list; + pcd_paths.insert(pcd_paths.end(), pcd_path.begin(), pcd_path.end()); + } + + } + + stat_msg.data = false; + stat_pub->publish(stat_msg); + sensor_msgs::msg::PointCloud2 pcd; + + if (margin < 0) { + int err = 0; + pcd = create_pcd(pcd_paths, &err); + RCLCPP_INFO_STREAM(get_logger(), "Publishing pcd"); + publish_pcd(pcd, &err); + } else{ + fallback_rate = update_rate * 2; // XXX better way? + + // Create subscribers + gnss_sub = create_subscription("gnss_pose", 1000, std::bind(&PointsMapLoader::publish_gnss_pcd, this, std::placeholders::_1)); + current_sub = create_subscription("current_pose", 1000, std::bind(&PointsMapLoader::publish_current_pcd, this, std::placeholders::_1)); + initial_sub = create_subscription("initialpose", 1, std::bind(&PointsMapLoader::publish_dragged_pcd, this, std::placeholders::_1)); + + if (can_download){ + waypoints_sub = create_subscription("traffic_waypoints_array", 1, std::bind(&PointsMapLoader::request_lookahead_download, this, std::placeholders::_1)); + try { + std::thread downloader([this]{this->download_map();}); + downloader.detach(); + } catch (std::exception &ex) { + RCLCPP_ERROR_STREAM(get_logger(), "failed to create thread from " << ex.what()); + } + } else{ + AreaList areas = read_arealist(arealist_path); + for (const Area& area : areas) { + // Check if the user entered pcd paths in addition to the arealist.txt file + if (pcd_paths.size() > 0) { + // Only load cells which the user specified + for (const std::string& path : pcd_paths) { + if (path == area.path) + cache_arealist(area, downloaded_areas); + } + } else { + // The user did not specify any cells to load all the cells contained in the arealist.txt file + cache_arealist(area, downloaded_areas); + } + } + } + + gnss_time = current_time = this->now(); + } + } + + void PointsMapLoader::enqueue(const geometry_msgs::msg::Point& p) + { + std::unique_lock lock(mtx_); + queue_.push(p); + cv_.notify_all(); + } + + void PointsMapLoader::enqueue_look_ahead(const geometry_msgs::msg::Point& p) + { + std::unique_lock lock(mtx_); + look_ahead_queue_.push(p); + cv_.notify_all(); + } + + void PointsMapLoader::clear() + { + std::unique_lock lock(mtx_); + while (!queue_.empty()) + queue_.pop(); + } + + void PointsMapLoader::clear_look_ahead() + { + std::unique_lock lock(mtx_); + while (!look_ahead_queue_.empty()) + look_ahead_queue_.pop(); + } + + geometry_msgs::msg::Point PointsMapLoader::dequeue() + { + std::unique_lock lock(mtx_); + while (queue_.empty() && look_ahead_queue_.empty()) + cv_.wait(lock); + if (!queue_.empty()) { + geometry_msgs::msg::Point p = queue_.front(); + queue_.pop(); + return p; + } else { + geometry_msgs::msg::Point p = look_ahead_queue_.front(); + look_ahead_queue_.pop(); + return p; + } + } + + + Tbl PointsMapLoader::read_csv(const std::string& path) + { + std::ifstream ifs(path.c_str()); + std::string line; + Tbl ret; + while (std::getline(ifs, line)) { + std::istringstream iss(line); + std::string col; + std::vector cols; + while (std::getline(iss, col, ',')) + cols.push_back(col); + ret.push_back(cols); + } + return ret; + } + + void PointsMapLoader::write_csv(const std::string& path, const Tbl& tbl) + { + std::ofstream ofs(path.c_str()); + for (const std::vector& cols : tbl) { + std::string line; + for (size_t i = 0; i < cols.size(); ++i) { + if (i == 0) + line += cols[i]; + else + line += "," + cols[i]; + } + ofs << line << std::endl; + } + } + + AreaList PointsMapLoader::read_arealist(const std::string& path) + { + Tbl tbl = read_csv(path); + AreaList ret; + for (const std::vector& cols : tbl) { + Area area; + area.path = cols[0]; + area.x_min = std::stod(cols[1]); + area.y_min = std::stod(cols[2]); + area.z_min = std::stod(cols[3]); + area.x_max = std::stod(cols[4]); + area.y_max = std::stod(cols[5]); + area.z_max = std::stod(cols[6]); + ret.push_back(area); + } + return ret; + } + + + void PointsMapLoader::write_arealist(const std::string& path, const AreaList& areas) + { + Tbl tbl; + for (const Area& area : areas) { + std::vector cols; + cols.push_back(area.path); + cols.push_back(std::to_string(area.x_min)); + cols.push_back(std::to_string(area.y_min)); + cols.push_back(std::to_string(area.z_min)); + cols.push_back(std::to_string(area.x_max)); + cols.push_back(std::to_string(area.y_max)); + cols.push_back(std::to_string(area.z_max)); + tbl.push_back(cols); + } + write_csv(path, tbl); + } + + bool PointsMapLoader::is_downloaded(const std::string& path) + { + struct stat st; + return (stat(path.c_str(), &st) == 0); + } + + bool PointsMapLoader::is_in_area(double x, double y, const Area& area, double m) + { + return ((area.x_min - m) <= x && x <= (area.x_max + m) && (area.y_min - m) <= y && y <= (area.y_max + m)); + } + + std::string PointsMapLoader::create_location(int x, int y) + { + x -= x % ROUNDING_UNIT; + y -= y % ROUNDING_UNIT; + return ("data/map/" + std::to_string(y) + "/" + std::to_string(x) + "/pointcloud/"); + } + + void PointsMapLoader::cache_arealist(const Area& area, AreaList& areas) + { + for (const Area& a : areas) { + if (a.path == area.path) + return; + } + areas.push_back(area); + } + + int PointsMapLoader::download(GetFile gf, const std::string& tmp, const std::string& loc, const std::string& filename) + { + std::string pathname; + pathname += tmp; + std::istringstream iss(loc); + std::string col; + while (std::getline(iss, col, '/')) { + pathname += col + "/"; + mkdir(pathname.c_str(), 0755); + } + + return gf.GetHTTPFile(loc + filename); + } + + void PointsMapLoader::download_map() + { + while (true) { + geometry_msgs::msg::Point p = dequeue(); + + int x = static_cast(p.x); + int y = static_cast(p.y); + int x_min = static_cast(p.x - margin); + int y_min = static_cast(p.y - margin); + int x_max = static_cast(p.x + margin); + int y_max = static_cast(p.y + margin); + + std::vector locs; + locs.push_back(create_location(x, y)); + locs.push_back(create_location(x_min, y_min)); + locs.push_back(create_location(x_min, y_max)); + locs.push_back(create_location(x_max, y_min)); + locs.push_back(create_location(x_max, y_max)); + for (const std::string& loc : locs) { // XXX better way? + std::string arealist_path = TEMPORARY_DIRNAME + loc + AREALIST_FILENAME; + + bool cached = false; + for (const std::string& path : cached_arealist_paths) { + if (path == arealist_path) { + cached = true; + break; + } + } + if (cached) + continue; + + AreaList areas; + if (is_downloaded(arealist_path)) + areas = read_arealist(arealist_path); + else { + if (download(gf, TEMPORARY_DIRNAME, loc, AREALIST_FILENAME) != 0) + continue; + areas = read_arealist(arealist_path); + for (Area& area : areas) + area.path = TEMPORARY_DIRNAME + loc + basename(area.path.c_str()); + write_arealist(arealist_path, areas); + } + for (const Area& area : areas) + cache_arealist(area, all_areas); + cached_arealist_paths.push_back(arealist_path); + } + + for (const Area& area : all_areas) { + if (is_in_area(p.x, p.y, area, margin)) { + int x_area = static_cast(area.x_max - MARGIN_UNIT); + int y_area = static_cast(area.y_max - MARGIN_UNIT); + std::string loc = create_location(x_area, y_area); + if (is_downloaded(area.path) || + download(gf, TEMPORARY_DIRNAME, loc, basename(area.path.c_str())) == 0) { + std::unique_lock lock(downloaded_areas_mtx); + cache_arealist(area, downloaded_areas); + } + } + } + } + } + + + sensor_msgs::msg::PointCloud2 PointsMapLoader::create_pcd(const geometry_msgs::msg::Point& p) + { + sensor_msgs::msg::PointCloud2 pcd, part; + std::unique_lock lock(downloaded_areas_mtx); + for (const Area& area : downloaded_areas) { + if (is_in_area(p.x, p.y, area, margin)) { + if (pcd.width == 0) + pcl::io::loadPCDFile(area.path.c_str(), pcd); + else { + pcl::io::loadPCDFile(area.path.c_str(), part); + pcd.width += part.width; + pcd.row_step += part.row_step; + pcd.data.insert(pcd.data.end(), part.data.begin(), part.data.end()); + } + } + } + + return pcd; + } + + sensor_msgs::msg::PointCloud2 PointsMapLoader::create_pcd(const std::vector& pcd_paths, int* ret_err = NULL) + { + sensor_msgs::msg::PointCloud2 pcd, part; + for (const std::string& path : pcd_paths) { + // Following outputs are used for progress bar of Runtime Manager. + if (pcd.width == 0) { + if (pcl::io::loadPCDFile(path.c_str(), pcd) == -1) { + std::cerr << "load failed " << path << std::endl; + if (ret_err) *ret_err = 1; + } + } else { + if (pcl::io::loadPCDFile(path.c_str(), part) == -1) { + std::cerr << "load failed " << path << std::endl; + if (ret_err) *ret_err = 1; + } + pcd.width += part.width; + pcd.row_step += part.row_step; + pcd.data.insert(pcd.data.end(), part.data.begin(), part.data.end()); + } + std::cerr << "load " << path << std::endl; + if (!rclcpp::ok()) break; + } + + return pcd; + } + + void PointsMapLoader::publish_pcd(sensor_msgs::msg::PointCloud2 pcd, const int* errp = NULL) + { + + if (pcd.width != 0) { + pcd.header.frame_id = "map"; + pcd_pub->publish(pcd); + + if (errp == NULL || *errp == 0) { + stat_msg.data = true; + stat_pub->publish(stat_msg); + } + } + } + + void PointsMapLoader::publish_gnss_pcd(const geometry_msgs::msg::PoseStamped::SharedPtr msg) + { + rclcpp::Time now = this->now(); + if (((now - current_time).seconds() * 1000) < fallback_rate) + return; + if (((now - gnss_time).seconds() * 1000) < update_rate) + return; + gnss_time = now; + + if (can_download) + enqueue(msg->pose.position); + + publish_pcd(create_pcd(msg->pose.position)); + } + + void PointsMapLoader::publish_current_pcd(const geometry_msgs::msg::PoseStamped::SharedPtr msg) + { + rclcpp::Time now = this->now(); + if (((now - current_time).seconds() * 1000) < update_rate) + return; + current_time = now; + + if (can_download) + enqueue(msg->pose.position); + + publish_pcd(create_pcd(msg->pose.position)); + } + + void PointsMapLoader::publish_dragged_pcd(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) + { + tf2_ros::Buffer buffer(this->get_clock()); + tf2_ros::TransformListener listener(buffer); + geometry_msgs::msg::TransformStamped tf_geom; + tf2::Transform transform; + try { + rclcpp::Time zero = this->now(); + tf_geom = buffer.lookupTransform("map", msg->header.frame_id, zero, rclcpp::Duration(10,0)); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR_STREAM(get_logger(),"failed to create transform from " << ex.what()); + } + + // ros1 implementation called getOrigin from returned transform + // ros2 implementation fills getOrigin in geom msg: https://github.com/ros2/geometry2/blob/d1dc38b8a0e706fbd67a800a241fee950fce39f4/tf2/src/buffer_core.cpp#L636 + geometry_msgs::msg::Point p; + p.x = msg->pose.pose.position.x + tf_geom.transform.translation.x; + p.y = msg->pose.pose.position.y + tf_geom.transform.translation.y; + + if (can_download) + enqueue(p); + + publish_pcd(create_pcd(p)); + } + + void PointsMapLoader::request_lookahead_download(const autoware_msgs::msg::LaneArray::SharedPtr msg) + { + clear_look_ahead(); + + for (const autoware_msgs::msg::Lane& l : msg->lanes) { + size_t end = l.waypoints.size() - 1; + double distance = 0; + double threshold = (MARGIN_UNIT / 2) + margin; // XXX better way? + for (size_t i = 0; i <= end; ++i) { + if (i == 0 || i == end) { + geometry_msgs::msg::Point p; + p.x = l.waypoints[i].pose.pose.position.x; + p.y = l.waypoints[i].pose.pose.position.y; + enqueue_look_ahead(p); + } else { + geometry_msgs::msg::Point p1, p2; + p1.x = l.waypoints[i].pose.pose.position.x; + p1.y = l.waypoints[i].pose.pose.position.y; + p2.x = l.waypoints[i - 1].pose.pose.position.x; + p2.y = l.waypoints[i - 1].pose.pose.position.y; + distance += hypot(p2.x - p1.x, p2.y - p1.y); + if (distance > threshold) { + enqueue_look_ahead(p1); + distance = 0; + } + } + } + } + } + + +} // namespace + + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(points_map_loader::PointsMapLoader) \ No newline at end of file diff --git a/common/map_file_ros2/nodes/points_map_loader/points_map_loader_node.cpp b/common/map_file_ros2/nodes/points_map_loader/points_map_loader_node.cpp new file mode 100644 index 00000000000..10cff2ab6b7 --- /dev/null +++ b/common/map_file_ros2/nodes/points_map_loader/points_map_loader_node.cpp @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + + if (argc < 3) { + points_map_loader::print_usage(); + return EXIT_FAILURE; + } + + auto node = std::make_shared(rclcpp::NodeOptions()); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/common/map_file_ros2/package.xml b/common/map_file_ros2/package.xml new file mode 100644 index 00000000000..a8639acdce6 --- /dev/null +++ b/common/map_file_ros2/package.xml @@ -0,0 +1,55 @@ + + + + + + map_file_ros2 + 4.0.0 + The map_file package + + carma + + Apache 2.0 + + ament_cmake + carma_cmake_common + ament_auto_cmake + + autoware_msgs + rclcpp + rclcpp_components + std_msgs + geometry_msgs + curl + libpcl-all-dev + + tf2_geometry_msgs + tf2_ros + tf2 + visualization_msgs + lanelet2_extension + carma_ros2_utils + autoware_lanelet2_ros_interface + + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros + + + ament_cmake + + diff --git a/common/map_file_ros2/test/test_get_transform.cpp b/common/map_file_ros2/test/test_get_transform.cpp new file mode 100644 index 00000000000..9b74d3241f6 --- /dev/null +++ b/common/map_file_ros2/test/test_get_transform.cpp @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + + +#include +#include + + +TEST(GetTransformTest, testECEFPoints) +{ + //Example frames + std::string map_frame = "+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; + + rclcpp::NodeOptions options; + auto worker_node = std::make_shared(options); + worker_node->configure(); //Call configure state transition + worker_node->activate(); //Call activate state transition to get not read for runtime + + //Points are calculated using cs2cs command line function of proj library. + tf2::Transform tf = worker_node->getTransform(map_frame); + tf2::Vector3 origin = tf(tf2::Vector3{0,0,0}); + ASSERT_FLOAT_EQ (origin[0],1104726.07); + ASSERT_FLOAT_EQ (origin[1],-4842261.44); + ASSERT_FLOAT_EQ (origin[2],3988172.62); + + tf2::Vector3 e1 = tf(tf2::Vector3{1,0,0}); + ASSERT_FLOAT_EQ (e1[0],1104727.04); + ASSERT_FLOAT_EQ (e1[1],-4842261.22); + ASSERT_FLOAT_EQ (e1[2],3988172.62); + + tf2::Vector3 random = tf(tf2::Vector3{4,3,5}); + ASSERT_FLOAT_EQ (random[0],1104730.41); + ASSERT_FLOAT_EQ (random[1],-4842262.50); + ASSERT_FLOAT_EQ (random[2],3988178.10); +} + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + + //Initialize ROS + rclcpp::init(argc, argv); + + bool success = RUN_ALL_TESTS(); + + //shutdown ROS + rclcpp::shutdown(); + + return success; +} + diff --git a/core_perception/dead_reckoner/CMakeLists.txt b/core_perception/dead_reckoner/CMakeLists.txt new file mode 100644 index 00000000000..99de04dc1c8 --- /dev/null +++ b/core_perception/dead_reckoner/CMakeLists.txt @@ -0,0 +1,51 @@ +# Copyright (C) 2023 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.5) +project(dead_reckoner) + +# Declare carma package and check ROS version +find_package(carma_cmake_common REQUIRED) +carma_check_ros_version(2) +carma_package() + +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + + +# Includes +include_directories( + include +) + +# Name build targets +set(node_exec dead_reckoner_exec) +set(node_lib dead_reckoner) + +# Build +ament_auto_add_library(${node_lib} SHARED + src/dead_reckoner.cpp +) + +ament_auto_add_executable(${node_exec} + src/main.cpp +) + +# Register component +rclcpp_components_register_nodes(dead_reckoner "dead_reckoner::DeadReckoner") + +ament_auto_package( + INSTALL_TO_SHARE launch +) \ No newline at end of file diff --git a/core_perception/deadreckoner/README.md b/core_perception/dead_reckoner/README.md similarity index 92% rename from core_perception/deadreckoner/README.md rename to core_perception/dead_reckoner/README.md index 53ac1a01266..24b1cf2cb11 100644 --- a/core_perception/deadreckoner/README.md +++ b/core_perception/dead_reckoner/README.md @@ -1,3 +1,3 @@ -## Deadreckoner Package +## DeadReckoner Package This package is responsible for providing odometry to NDT Matching. Currently only the Twist field of the nav_msgs::Odometry message is populated. This node was originally developed by AutonomouStuff and the git history can be found in their fork of the Autoware repo (https://github.com/astuff/autoware). \ No newline at end of file diff --git a/core_perception/dead_reckoner/include/dead_reckoner.hpp b/core_perception/dead_reckoner/include/dead_reckoner.hpp new file mode 100644 index 00000000000..f66eadf2277 --- /dev/null +++ b/core_perception/dead_reckoner/include/dead_reckoner.hpp @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#ifndef DEADRECKONER_H +#define DEADRECKONER_H + +#include +#include +#include +#include + +namespace dead_reckoner +{ + +class DeadReckoner : public carma_ros2_utils::CarmaLifecycleNode +{ +public: + explicit DeadReckoner(const rclcpp::NodeOptions& options); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); + +private: + carma_ros2_utils::SubPtr twist_sub_; + carma_ros2_utils::PubPtr odom_pub_; + + void twist_cb(geometry_msgs::msg::TwistStamped::UniquePtr msg); +}; + +} // namespace dead_reckoner + +#endif // DEADRECKONER_H diff --git a/core_perception/dead_reckoner/launch/dead_reckoner.launch.py b/core_perception/dead_reckoner/launch/dead_reckoner.launch.py new file mode 100644 index 00000000000..e3481f555a5 --- /dev/null +++ b/core_perception/dead_reckoner/launch/dead_reckoner.launch.py @@ -0,0 +1,63 @@ +# Copyright (C) 2023 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +''' +This file is can be used to launch the CARMA dead_reckoner_node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='dead_reckoner_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='dead_reckoner', + plugin='dead_reckoner::DeadReckoner', + name='dead_reckoner_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + remappings=[ + ("current_twist", "/vehicle/twist" ), + ("current_odom", "/vehicle/odom") + ], + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) + diff --git a/core_perception/dead_reckoner/package.xml b/core_perception/dead_reckoner/package.xml new file mode 100644 index 00000000000..eea1f77ee3b --- /dev/null +++ b/core_perception/dead_reckoner/package.xml @@ -0,0 +1,43 @@ + + + + + dead_reckoner + 4.0.0 + The dead_reckoner package + + carma + + Apache 2.0 + ament_cmake + carma_cmake_common + ament_auto_cmake + + carma_ros2_utils + std_msgs + nav_msgs + geometry_msgs + rclcpp + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros + + + ament_cmake + + + diff --git a/core_perception/dead_reckoner/src/dead_reckoner.cpp b/core_perception/dead_reckoner/src/dead_reckoner.cpp new file mode 100644 index 00000000000..64ef20a9121 --- /dev/null +++ b/core_perception/dead_reckoner/src/dead_reckoner.cpp @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include "dead_reckoner.hpp" + + +namespace dead_reckoner +{ + +namespace std_ph = std::placeholders; + +DeadReckoner::DeadReckoner(const rclcpp::NodeOptions& options) : carma_ros2_utils::CarmaLifecycleNode(options) +{ +} + +carma_ros2_utils::CallbackReturn DeadReckoner::handle_on_configure(const rclcpp_lifecycle::State &) +{ + twist_sub_ = create_subscription("current_twist", 1, std::bind(&DeadReckoner::twist_cb, this, std_ph::_1)); + odom_pub_ = create_publisher("current_odom", 10); + return CallbackReturn::SUCCESS; +} + +carma_ros2_utils::CallbackReturn DeadReckoner::handle_on_activate(const rclcpp_lifecycle::State &) +{ + return CallbackReturn::SUCCESS; +} + +void DeadReckoner::twist_cb(geometry_msgs::msg::TwistStamped::UniquePtr msg) +{ + // TODO: calculate odom.pose.pose by accumulating + nav_msgs::msg::Odometry odom; + odom.header = msg->header; + odom.twist.twist = msg->twist; + odom_pub_->publish(odom); +} + +} // namespace dead_reckoner + + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(dead_reckoner::DeadReckoner) diff --git a/core_perception/dead_reckoner/src/main.cpp b/core_perception/dead_reckoner/src/main.cpp new file mode 100644 index 00000000000..730fa088733 --- /dev/null +++ b/core_perception/dead_reckoner/src/main.cpp @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ +#include +#include "dead_reckoner.hpp" + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(rclcpp::NodeOptions()); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/core_perception/deadreckoner/CMakeLists.txt b/core_perception/deadreckoner/CMakeLists.txt deleted file mode 100644 index 5538a7e0499..00000000000 --- a/core_perception/deadreckoner/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ - - -cmake_minimum_required(VERSION 2.8.3) -project(deadreckoner) - -find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) - -add_compile_options(-std=c++14) - -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - nav_msgs - geometry_msgs -) - -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES deadreckoner -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib -) - -include_directories( - ${catkin_INCLUDE_DIRS} -) - -add_executable(deadreckoner - nodes/deadreckoner_node.cpp - nodes/deadreckoner.cpp -) - -target_link_libraries(deadreckoner - ${catkin_LIBRARIES} -) - -install(TARGETS - deadreckoner - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) diff --git a/core_perception/deadreckoner/launch/deadreckoner.launch b/core_perception/deadreckoner/launch/deadreckoner.launch deleted file mode 100644 index e37fb037688..00000000000 --- a/core_perception/deadreckoner/launch/deadreckoner.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/core_perception/deadreckoner/nodes/deadreckoner.cpp b/core_perception/deadreckoner/nodes/deadreckoner.cpp deleted file mode 100644 index bfd5053e4d7..00000000000 --- a/core_perception/deadreckoner/nodes/deadreckoner.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright (c) 2017, Nagoya University - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of Autoware nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#include "deadreckoner.h" - -DeadRecokner::DeadRecokner() : nh_(), private_nh_("~") -{ - twist_sub_ = nh_.subscribe("current_twist", 10, &DeadRecokner::callbackFromCurrentTwist, this); - odom_pub_ = nh_.advertise("current_odom", 10); -} - -DeadRecokner::~DeadRecokner() -{ -} - -void DeadRecokner::callbackFromCurrentTwist(const geometry_msgs::TwistStampedConstPtr& msg) -{ - // TODO: calurate odom.pose.pose by accumulating - nav_msgs::Odometry odom; - odom.header = msg->header; - odom.twist.twist = msg->twist; - odom_pub_ .publish(odom); -} diff --git a/core_perception/deadreckoner/nodes/deadreckoner.h b/core_perception/deadreckoner/nodes/deadreckoner.h deleted file mode 100644 index 4516b11f28f..00000000000 --- a/core_perception/deadreckoner/nodes/deadreckoner.h +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright (c) 2017, Nagoya University - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of Autoware nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"geometry - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef DEADRECKONER_H -#define DEADRECKONER_H - -#include -#include -#include - -class DeadRecokner -{ -public: - DeadRecokner(); - ~DeadRecokner(); -private: - ros::NodeHandle nh_, private_nh_; - ros::Subscriber twist_sub_; - ros::Publisher odom_pub_; - - void callbackFromCurrentTwist(const geometry_msgs::TwistStampedConstPtr& msg); -}; -#endif // DEADRECKONER_H diff --git a/core_perception/deadreckoner/nodes/deadreckoner_node.cpp b/core_perception/deadreckoner/nodes/deadreckoner_node.cpp deleted file mode 100644 index 1f9765fc2d1..00000000000 --- a/core_perception/deadreckoner/nodes/deadreckoner_node.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Copyright (c) 2017, Nagoya University - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of Autoware nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#include - -#include "deadreckoner.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "deadreckoner"); - DeadRecokner node; - ros::spin(); - return 0; -} diff --git a/core_perception/deadreckoner/package.xml b/core_perception/deadreckoner/package.xml deleted file mode 100644 index e7c8ea8beb1..00000000000 --- a/core_perception/deadreckoner/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - deadreckoner - 0.0.0 - The deadreckoner package - - Akihito Ohsato - - TODO - - Akihito Ohsato - - catkin - carma_cmake_common - - - - - diff --git a/core_perception/ekf_localizer/CMakeLists.txt b/core_perception/ekf_localizer/CMakeLists.txt index 19023a01b15..ef070547de3 100644 --- a/core_perception/ekf_localizer/CMakeLists.txt +++ b/core_perception/ekf_localizer/CMakeLists.txt @@ -1,77 +1,65 @@ -cmake_minimum_required(VERSION 2.8.3) +# Copyright (C) 2023 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.5) project(ekf_localizer) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) - -find_package(catkin REQUIRED - autoware_build_flags - roscpp - std_msgs - tf2 - tf2_ros - geometry_msgs - autoware_msgs - sensor_msgs - rostest - rosunit - amathutils_lib -) +carma_check_ros_version(2) +carma_package() find_package(Eigen3 REQUIRED) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - autoware_build_flags - roscpp - std_msgs - tf2 - tf2_ros - geometry_msgs - autoware_msgs - sensor_msgs - amathutils_lib +# Includes +include_directories( + include + ${EIGEN3_INCLUDE_DIR} ) -########### -## Build ## -########### +ament_auto_add_library(ekf_localizer_lib SHARED + src/ekf_localizer.cpp) -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} +ament_auto_add_executable(ekf_localizer_exec + src/ekf_localizer_node.cpp) + +# Register component +rclcpp_components_register_nodes(ekf_localizer_lib "ekf_localizer::EKFLocalizer") + +target_link_libraries( + ekf_localizer_exec + ekf_localizer_lib ) -add_executable(ekf_localizer src/ekf_localizer_node.cpp src/ekf_localizer.cpp) -add_dependencies(ekf_localizer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(ekf_localizer ${catkin_LIBRARIES}) - -############# -## Install ## -############# - -## Install executables and/or libraries -install(TARGETS ekf_localizer - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -## Install project namespaced headers -install(DIRECTORY include/ekf_localizer/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE) - - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(ekf_localizer-test test/test_ekf_localizer.test - test/src/test_ekf_localizer.cpp - src/ekf_localizer.cpp) - target_link_libraries(ekf_localizer-test ${catkin_LIBRARIES}) +# Testing +if(BUILD_TESTING) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable + + ament_add_gtest(test_ekf_localizer test/test_ekf_localizer.cpp) + + ament_target_dependencies(test_ekf_localizer ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + + target_link_libraries(test_ekf_localizer ekf_localizer_lib) + endif() + +ament_auto_package( + INSTALL_TO_SHARE launch +) diff --git a/core_perception/ekf_localizer/include/ekf_localizer/ekf_localizer.h b/core_perception/ekf_localizer/include/ekf_localizer/ekf_localizer.h deleted file mode 100644 index 812083596ef..00000000000 --- a/core_perception/ekf_localizer/include/ekf_localizer/ekf_localizer.h +++ /dev/null @@ -1,218 +0,0 @@ -/* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "amathutils_lib/kalman_filter.hpp" -#include "amathutils_lib/time_delay_kalman_filter.hpp" - -class EKFLocalizer -{ -public: - EKFLocalizer(); - ~EKFLocalizer(); - -private: - ros::NodeHandle nh_; //!< @brief ros public node handler - ros::NodeHandle pnh_; //!< @brief ros private node handler - ros::Publisher pub_pose_; //!< @brief ekf estimated pose publisher - ros::Publisher pub_pose_cov_; //!< @brief estimated ekf pose with covariance publisher - ros::Publisher pub_twist_; //!< @brief ekf estimated twist publisher - ros::Publisher pub_twist_cov_; //!< @brief ekf estimated twist with covariance publisher - ros::Publisher pub_debug_; //!< @brief debug info publisher - ros::Publisher pub_measured_pose_; //!< @brief debug measurement pose publisher - ros::Publisher pub_yaw_bias_; //!< @brief ekf estimated yaw bias publisher - ros::Subscriber sub_initialpose_; //!< @brief initial pose subscriber - ros::Subscriber sub_pose_; //!< @brief measurement pose subscriber - ros::Subscriber sub_twist_; //!< @brief measurement twist subscriber - ros::Subscriber sub_pose_with_cov_; //!< @brief measurement pose with covariance subscriber - ros::Subscriber sub_twist_with_cov_; //!< @brief measurement twist with covariance subscriber - ros::Timer timer_control_; //!< @brief time for ekf calculation callback - tf2_ros::TransformBroadcaster tf_br_; //!< @brief tf broadcaster - - TimeDelayKalmanFilter ekf_; //!< @brief extended kalman filter instance. - - /* parameters */ - bool show_debug_info_; - double ekf_rate_; //!< @brief EKF predict rate - double ekf_dt_; //!< @brief = 1 / ekf_rate_ - bool enable_yaw_bias_estimation_; //!< @brief for LiDAR mount error. if true, publish /estimate_yaw_bias - std::string pose_frame_id_; //!< @brief Parent frame for pose and tf output - std::string child_frame_id_; //!< @brief Child frame for pose and tf output - - int dim_x_; //!< @brief dimension of EKF state - int extend_state_step_; //!< @brief for time delay compensation - int dim_x_ex_; //!< @brief dimension of extended EKF state (dim_x_ * extended_state_step) - - /* Pose */ - double pose_additional_delay_; //!< @brief compensated pose delay time = (pose.header.stamp - now) + - //!< additional_delay [s] - double pose_measure_uncertainty_time_; //!< @brief added for measurement covariance - double pose_rate_; //!< @brief pose rate [s], used for covariance calculation - double pose_gate_dist_; //!< @brief pose measurement is ignored if the maharanobis distance is larger than this - //!< value. - double pose_stddev_x_; //!< @brief standard deviation for pose position x [m] - double pose_stddev_y_; //!< @brief standard deviation for pose position y [m] - double pose_stddev_yaw_; //!< @brief standard deviation for pose position yaw [rad] - bool use_pose_with_covariance_; //!< @brief use covariance in pose_with_covarianve message - bool use_twist_with_covariance_; //!< @brief use covariance in twist_with_covarianve message - - /* twist */ - double twist_additional_delay_; //!< @brief compensated delay time = (twist.header.stamp - now) + additional_delay - //!< [s] - double twist_rate_; //!< @brief rate [s], used for covariance calculation - double twist_gate_dist_; //!< @brief measurement is ignored if the maharanobis distance is larger than this value. - double twist_stddev_vx_; //!< @brief standard deviation for linear vx - double twist_stddev_wz_; //!< @brief standard deviation for angular wx - - /* process noise variance for discrete model */ - double proc_cov_yaw_d_; //!< @brief discrete yaw process noise - double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias process noise - double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0 - double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0 - - enum IDX - { - X = 0, - Y = 1, - YAW = 2, - YAWB = 3, - VX = 4, - WZ = 5, - }; - - /* for model prediction */ - std::shared_ptr current_twist_ptr_; //!< @brief current measured twist - std::shared_ptr current_pose_ptr_; //!< @brief current measured pose - geometry_msgs::PoseStamped current_ekf_pose_; //!< @brief current estimated pose - geometry_msgs::TwistStamped current_ekf_twist_; //!< @brief current estimated twist - boost::array current_pose_covariance_; - boost::array current_twist_covariance_; - - /** - * @brief computes update & prediction of EKF for each ekf_dt_[s] time - */ - void timerCallback(const ros::TimerEvent& e); - - /** - * @brief publish tf - */ - void broadcastTF(); - - /** - * @brief set pose measurement - */ - void callbackPose(const geometry_msgs::PoseStamped::ConstPtr& msg); - - /** - * @brief set twist measurement - */ - void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr& msg); - - /** - * @brief set poseWithCovariance measurement - */ - void callbackPoseWithCovariance(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg); - - /** - * @brief set twistWithCovariance measurement - */ - void callbackTwistWithCovariance(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg); - - /** - * @brief set initial_pose to current EKF pose - */ - void callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped& msg); - - /** - * @brief initialization of EKF - */ - void initEKF(); - - /** - * @brief compute EKF prediction - */ - void predictKinematicsModel(); - - /** - * @brief compute EKF update with pose measurement - * @param pose measurement value - */ - void measurementUpdatePose(const geometry_msgs::PoseStamped& pose); - - /** - * @brief compute EKF update with pose measurement - * @param twist measurement value - */ - void measurementUpdateTwist(const geometry_msgs::TwistStamped& twist); - - /** - * @brief check whether a measurement value falls within the mahalanobis distance threshold - * @param dist_max mahalanobis distance threshold - * @param estimated current estimated state - * @param measured measured state - * @param estimated_cov current estimation covariance - * @return whether it falls within the mahalanobis distance threshold - */ - bool mahalanobisGate(const double& dist_max, const Eigen::MatrixXd& estimated, const Eigen::MatrixXd& measured, - const Eigen::MatrixXd& estimated_cov); - - /** - * @brief get transform from frame_id - */ - bool getTransformFromTF(std::string parent_frame, std::string child_frame, - geometry_msgs::TransformStamped& transform); - - /** - * @brief normalize yaw angle - * @param yaw yaw angle - * @return normalized yaw - */ - double normalizeYaw(const double& yaw); - - /** - * @brief set current EKF estimation result to current_ekf_pose_ & current_ekf_twist_ - */ - void setCurrentResult(); - - /** - * @brief publish current EKF estimation result - */ - void publishEstimateResult(); - - /** - * @brief for debug - */ - void showCurrentX(); - - friend class EKFLocalizerTestSuite; // for test code -}; diff --git a/core_perception/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/core_perception/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp new file mode 100644 index 00000000000..a09132b7be5 --- /dev/null +++ b/core_perception/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -0,0 +1,238 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "amathutils_lib_ros2/kalman_filter.hpp" +#include "amathutils_lib_ros2/time_delay_kalman_filter.hpp" +#include + +namespace ekf_localizer{ + +class EKFLocalizer: public carma_ros2_utils::CarmaLifecycleNode +{ + private: + + rclcpp::Publisher::SharedPtr pub_pose_; //!< @brief ekf estimated pose publisher + rclcpp::Publisher::SharedPtr pub_pose_cov_; //!< @brief estimated ekf pose with covariance publisher + rclcpp::Publisher::SharedPtr pub_twist_; //!< @brief ekf estimated twist publisher + rclcpp::Publisher::SharedPtr pub_twist_cov_; //!< @brief ekf estimated twist with covariance publisher + rclcpp::Publisher::SharedPtr pub_debug_; //!< @brief debug info publisher + rclcpp::Publisher::SharedPtr pub_measured_pose_; //!< @brief debug measurement pose publisher + rclcpp::Publisher::SharedPtr pub_yaw_bias_; //!< @brief ekf estimated yaw bias publisher + + rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief initial pose subscriber + rclcpp::Subscription::SharedPtr sub_pose_with_cov_; //!< @brief measurement pose with covariance subscriber + rclcpp::Subscription::SharedPtr sub_pose_; //!< @brief measurement pose subscriber + rclcpp::Subscription::SharedPtr sub_twist_with_cov_; //!< @brief measurement twist with covariance subscriber + rclcpp::Subscription::SharedPtr sub_twist_; //!< @brief measurement twist subscriber + + rclcpp::TimerBase::SharedPtr timer_control_; //!< @brief time for ekf calculation callback + + TimeDelayKalmanFilter ekf_; //!< @brief extended kalman filter instance. + + /* parameters */ + double proc_stddev_yaw_c_ = 0.005, proc_stddev_yaw_bias_c_ = 0.001, proc_stddev_vx_c_ = 2.0, proc_stddev_wz_c_ = 0.2; + + bool show_debug_info_ = false; + double ekf_rate_ = 50.0; //!< @brief EKF predict rate + double ekf_dt_; //!< @brief = 1 / ekf_rate_ + bool enable_yaw_bias_estimation_ = true; //!< @brief for LiDAR mount error. if true, publish /estimate_yaw_bias + std::string pose_frame_id_ = "/map"; //!< @brief Parent frame for pose and tf output + std::string child_frame_id_ = "base_link"; //!< @brief Child frame for pose and tf output + + int dim_x_; //!< @brief dimension of EKF state + int extend_state_step_ = 50; //!< @brief for time delay compensation + int dim_x_ex_; //!< @brief dimension of extended EKF state (dim_x_ * extended_state_step) + + /* Pose */ + double pose_additional_delay_ = 0.0; //!< @brief compensated pose delay time = (pose.header.stamp - now) + + //!< additional_delay [s] + double pose_measure_uncertainty_time_ = 0.01; //!< @brief added for measurement covariance + double pose_rate_ = 10.0; //!< @brief pose rate [s], used for covariance calculation + double pose_gate_dist_ = 10000.0; //!< @brief pose measurement is ignored if the maharanobis distance is larger than this + //!< value. + double pose_stddev_x_ = 0.05; //!< @brief standard deviation for pose position x [m] + double pose_stddev_y_ = 0.05; //!< @brief standard deviation for pose position y [m] + double pose_stddev_yaw_ = 0.035; //!< @brief standard deviation for pose position yaw [rad] + bool use_pose_with_covariance_ = false; //!< @brief use covariance in pose_with_covariance message + bool use_twist_with_covariance_ = false; //!< @brief use covariance in twist_with_covariance message + + /* twist */ + double twist_additional_delay_ = 0.0; //!< @brief compensated delay time = (twist.header.stamp - now) + additional_delay + //!< [s] + double twist_rate_ = 10.0; //!< @brief rate [s], used for covariance calculation + double twist_gate_dist_ = 10000.0; //!< @brief measurement is ignored if the maharanobis distance is larger than this value. + double twist_stddev_vx_ = 0.2; //!< @brief standard deviation for linear vx + double twist_stddev_wz_ = 0.03; //!< @brief standard deviation for angular wx + + /* process noise variance for discrete model */ + double proc_cov_yaw_d_; //!< @brief discrete yaw process noise + double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias process noise + double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0 + double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0 + + enum IDX + { + X = 0, + Y = 1, + YAW = 2, + YAWB = 3, + VX = 4, + WZ = 5, + }; + + /* for model prediction */ + std::shared_ptr current_twist_ptr_; //!< @brief current measured twist + std::shared_ptr current_pose_ptr_; //!< @brief current measured pose + geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose + geometry_msgs::msg::TwistStamped current_ekf_twist_; //!< @brief current estimated twist + + //ros1 definition for current_pose_covariance_ and current_twist_covariance_ was as boost::array. + //Since the difference between the two are minimal. It is changed here to std::array inorder to prevent type cast errors + std::array current_pose_covariance_; + std::array current_twist_covariance_; + + rclcpp::Clock clk_; //!< @brief the node clock + + std::shared_ptr tf_br_shared_ptr_ = nullptr; + + /** + * @brief computes update & prediction of EKF for each ekf_dt_[s] time + */ + void timerCallback(); + + /** + * @brief publish tf + */ + void broadcastTF(); + + /** + * @brief set pose measurement + */ + void callbackPose(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + + /** + * @brief set twist measurement + */ + void callbackTwist(const geometry_msgs::msg::TwistStamped::SharedPtr msg); + + /** + * @brief set poseWithCovariance measurement + */ + void callbackPoseWithCovariance(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + + /** + * @brief set twistWithCovariance measurement + */ + void callbackTwistWithCovariance(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + /** + * @brief set initial_pose to current EKF pose + */ + void callbackInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + /** + * @brief initialization of EKF + */ + void initEKF(); + + /** + * @brief compute EKF prediction + */ + void predictKinematicsModel(); + + /** + * @brief compute EKF update with pose measurement + * @param pose measurement value + */ + void measurementUpdatePose(const geometry_msgs::msg::PoseStamped& pose); + + /** + * @brief compute EKF update with pose measurement + * @param twist measurement value + */ + void measurementUpdateTwist(const geometry_msgs::msg::TwistStamped& twist); + + + /** + * @brief check whether a measurement value falls within the mahalanobis distance threshold + * @param dist_max mahalanobis distance threshold + * @param estimated current estimated state + * @param measured measured state + * @param estimated_cov current estimation covariance + * @return whether it falls within the mahalanobis distance threshold + */ + bool mahalanobisGate(const double& dist_max, const Eigen::MatrixXd& estimated, const Eigen::MatrixXd& measured, + const Eigen::MatrixXd& estimated_cov); + + + /** + * @brief get transform from frame_id + */ + bool getTransformFromTF(std::string parent_frame, std::string child_frame, + geometry_msgs::msg::TransformStamped& transform); + + + /** + * @brief normalize yaw angle + * @param yaw yaw angle + * @return normalized yaw + */ + double normalizeYaw(const double& yaw); + + /** + * @brief set current EKF estimation result to current_ekf_pose_ & current_ekf_twist_ + */ + void setCurrentResult(); + + /** + * @brief publish current EKF estimation result + */ + void publishEstimateResult(); + + /** + * @brief for debug + */ + void showCurrentX(); + + + public: + + explicit EKFLocalizer(const rclcpp::NodeOptions &); + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); +}; + +} //namespace ekf_localizer \ No newline at end of file diff --git a/core_perception/ekf_localizer/launch/ekf_localizer.launch b/core_perception/ekf_localizer/launch/ekf_localizer.launch deleted file mode 100644 index 5beec061b2e..00000000000 --- a/core_perception/ekf_localizer/launch/ekf_localizer.launch +++ /dev/null @@ -1,83 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/core_perception/ekf_localizer/launch/ekf_localizer.launch.py b/core_perception/ekf_localizer/launch/ekf_localizer.launch.py new file mode 100644 index 00000000000..b96738955df --- /dev/null +++ b/core_perception/ekf_localizer/launch/ekf_localizer.launch.py @@ -0,0 +1,184 @@ +# Copyright (C) LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA _node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + show_debug_info = LaunchConfiguration('show_debug_info') + declare_show_debug_info = DeclareLaunchArgument(name='show_debug_info', default_value = "False") + + predict_frequency = LaunchConfiguration('predict_frequency') + declare_predict_frequency = DeclareLaunchArgument(name='predict_frequency', default_value = '50.0') + + enable_yaw_bias_estimation = LaunchConfiguration('enable_yaw_bias_estimation') + declare_enable_yaw_bias_estimation = DeclareLaunchArgument(name='enable_yaw_bias_estimation', default_value = "True") + + extend_state_step = LaunchConfiguration('extend_state_step') + declare_extend_state_step = DeclareLaunchArgument(name='extend_state_step', default_value = '50') + + pose_frame_id = LaunchConfiguration('pose_frame_id') + declare_pose_frame_id = DeclareLaunchArgument(name='pose_frame_id', default_value='/map') + + child_frame_id = LaunchConfiguration('child_frame_id') + declare_child_frame_id = DeclareLaunchArgument(name='child_frame_id', default_value ='base_link') + + pose_additional_delay = LaunchConfiguration('pose_additional_delay') + declare_pose_additional_delay = DeclareLaunchArgument(name='pose_additional_delay', default_value = '0.0') + + pose_measure_uncertainty_time = LaunchConfiguration('pose_measure_uncertainty_time') + declare_pose_measure_uncertainty_time = DeclareLaunchArgument(name='pose_measure_uncertainty_time', default_value = '0.01') + + pose_rate = LaunchConfiguration('pose_rate') + declare_pose_rate = DeclareLaunchArgument(name='pose_rate', default_value = '10.0') + + pose_gate_dist = LaunchConfiguration('pose_gate_dist') + declare_pose_gate_dist = DeclareLaunchArgument(name='pose_gate_dist', default_value = '10000.0') + + pose_stddev_x = LaunchConfiguration('pose_stddev_x') + declare_pose_stddev_x = DeclareLaunchArgument(name='pose_stddev_x', default_value= '0.05') + + pose_stddev_y = LaunchConfiguration('pose_stddev_y') + declare_pose_stddev_y = DeclareLaunchArgument(name='pose_stddev_y', default_value= '0.05') + + pose_stddev_yaw = LaunchConfiguration('pose_stddev_yaw') + declare_pose_stddev_yaw = DeclareLaunchArgument(name='pose_stddev_yaw', default_value = '0.035') + + use_pose_with_covariance = LaunchConfiguration('use_pose_with_covariance') + declare_use_pose_with_covariance = DeclareLaunchArgument(name='use_pose_with_covariance', default_value="False") + + twist_additional_delay = LaunchConfiguration('twist_additional_delay') + declare_twist_additional_delay = DeclareLaunchArgument(name='twist_additional_delay', default_value='0.0') + + twist_rate = LaunchConfiguration('twist_rate') + declare_twist_rate = DeclareLaunchArgument(name='twist_rate', default_value='10.0') + + twist_gate_dist = LaunchConfiguration('twist_gate_dist') + declare_twist_gate_dist = DeclareLaunchArgument(name = 'twist_gate_dist', default_value = '10000.0') + + twist_stddev_vx = LaunchConfiguration('twist_stddev_vx') + declare_twist_stddev_vx = DeclareLaunchArgument(name='twist_stddev_vx', default_value = '0.2') + + twist_stddev_wz = LaunchConfiguration('twist_stddev_wz') + declare_twist_stddev_wz = DeclareLaunchArgument(name='twist_stddev_wz', default_value='0.03') + + use_twist_with_covariance = LaunchConfiguration('use_twist_with_covariance') + declare_use_twist_with_covariance = DeclareLaunchArgument(name='use_twist_with_covariance', default_value="False") + + proc_stddev_yaw_c = LaunchConfiguration('proc_stddev_yaw_c') + declare_proc_stddev_yaw_c = DeclareLaunchArgument(name='proc_stddev_yaw_c', default_value='0.005') + + proc_stddev_yaw_bias_c = LaunchConfiguration('proc_stddev_yaw_bias_c') + declare_proc_stddev_yaw_bias_c = DeclareLaunchArgument(name='proc_stddev_yaw_bias_c', default_value = '0.001') + + proc_stddev_vx_c = LaunchConfiguration('proc_stddev_vx_c') + declare_proc_stddev_vx_c = DeclareLaunchArgument(name='proc_stddev_vx_c', default_value = '2.0') + + proc_stddev_wz_c = LaunchConfiguration('proc_stddev_wz_c') + declare_proc_stddev_wz_c = DeclareLaunchArgument(name='proc_stddev_wz_c', default_value='0.2') + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='ekf_localizer_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='ekf_localizer', + plugin='ekf_localizer::EKFLocalizer', + name='ekf_localizer_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ + {'show_debug_info': show_debug_info}, + {'predict_frequency': predict_frequency}, + {'enable_yaw_bias_estimation': enable_yaw_bias_estimation}, + {'extend_state_step': extend_state_step}, + {'pose_frame_id': pose_frame_id}, + {'child_frame_id': child_frame_id}, + {'pose_additional_delay': pose_additional_delay}, + {'pose_measure_uncertainty_time': pose_measure_uncertainty_time}, + {'pose_rate': pose_rate}, + {'pose_gate_dist': pose_gate_dist}, + {'pose_stddev_x': pose_stddev_x}, + {'pose_stddev_y': pose_stddev_y}, + {'pose_stddev_yaw': pose_stddev_yaw}, + {'use_pose_with_covariance': use_pose_with_covariance}, + {'twist_additional_delay': twist_additional_delay}, + {'twist_rate': twist_rate}, + {'twist_gate_dist': twist_gate_dist}, + {'twist_stddev_vx': twist_stddev_vx}, + {'twist_stddev_wz': twist_stddev_wz}, + {'use_twist_with_covariance': use_twist_with_covariance}, + {'proc_stddev_yaw_c': proc_stddev_yaw_c}, + {'proc_stddev_yaw_bias_c': proc_stddev_yaw_bias_c}, + {'proc_stddev_vx_c': proc_stddev_vx_c}, + {'proc_stddev_wz_c': proc_stddev_wz_c} + ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + declare_show_debug_info, + declare_predict_frequency, + declare_enable_yaw_bias_estimation, + declare_extend_state_step, + declare_pose_frame_id, + declare_child_frame_id, + declare_pose_additional_delay, + declare_pose_measure_uncertainty_time, + declare_pose_rate, + declare_pose_gate_dist, + declare_pose_stddev_x, + declare_pose_stddev_y, + declare_pose_stddev_yaw, + declare_use_pose_with_covariance, + declare_twist_additional_delay, + declare_twist_rate, + declare_twist_gate_dist, + declare_twist_stddev_vx, + declare_twist_stddev_wz, + declare_use_twist_with_covariance, + declare_proc_stddev_yaw_c, + declare_proc_stddev_yaw_bias_c, + declare_proc_stddev_vx_c, + declare_proc_stddev_wz_c, + container + ]) \ No newline at end of file diff --git a/core_perception/ekf_localizer/package.xml b/core_perception/ekf_localizer/package.xml index b77fed13258..762c8002ab9 100644 --- a/core_perception/ekf_localizer/package.xml +++ b/core_perception/ekf_localizer/package.xml @@ -1,21 +1,47 @@ - + + + + ekf_localizer 1.12.0 The ekf_localizer package horibe Apache 2.0 - catkin + ament_cmake + carma_cmake_common + ament_auto_cmake amathutils_lib autoware_build_flags autoware_msgs geometry_msgs - roscpp + rclcpp sensor_msgs std_msgs tf2 tf2_ros - carma_cmake_common + carma_ros2_utils + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros + + + ament_cmake + diff --git a/core_perception/ekf_localizer/src/ekf_localizer.cpp b/core_perception/ekf_localizer/src/ekf_localizer.cpp index 990ab33aead..69b7acf78a8 100644 --- a/core_perception/ekf_localizer/src/ekf_localizer.cpp +++ b/core_perception/ekf_localizer/src/ekf_localizer.cpp @@ -1,743 +1,791 @@ /* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * Copyright (C) 2023 LEIDOS. * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at * - * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. */ -#include "ekf_localizer/ekf_localizer.h" +#include "ekf_localizer/ekf_localizer.hpp" // clang-format off #define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl -#define DEBUG_INFO(...) { if (show_debug_info_) { ROS_INFO(__VA_ARGS__); } } +#define DEBUG_INFO(...) { if (show_debug_info_) { RCLCPP_INFO(rclcpp::get_logger("ekf_localizer"), __VA_ARGS__); } } #define DEBUG_PRINT_MAT(X) { if (show_debug_info_) { std::cout << #X << ": " << X << std::endl; } } -// clang-format on -EKFLocalizer::EKFLocalizer() : nh_(""), pnh_("~"), dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */) -{ - pnh_.param("show_debug_info", show_debug_info_, bool(false)); - pnh_.param("predict_frequency", ekf_rate_, double(50.0)); - ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); - pnh_.param("enable_yaw_bias_estimation", enable_yaw_bias_estimation_, bool(true)); - pnh_.param("extend_state_step", extend_state_step_, int(50)); - pnh_.param("pose_frame_id", pose_frame_id_, std::string("/map")); - pnh_.param("child_frame_id", child_frame_id_, std::string("base_link")); - - /* pose measurement */ - pnh_.param("pose_additional_delay", pose_additional_delay_, double(0.0)); - pnh_.param("pose_measure_uncertainty_time", pose_measure_uncertainty_time_, double(0.01)); - pnh_.param("pose_rate", pose_rate_, double(10.0)); // used for covariance calculation - pnh_.param("pose_gate_dist", pose_gate_dist_, double(10000.0)); // Mahalanobis limit - pnh_.param("pose_stddev_x", pose_stddev_x_, double(0.05)); - pnh_.param("pose_stddev_y", pose_stddev_y_, double(0.05)); - pnh_.param("pose_stddev_yaw", pose_stddev_yaw_, double(0.035)); - pnh_.param("use_pose_with_covariance", use_pose_with_covariance_, bool(false)); - - /* twist measurement */ - pnh_.param("twist_additional_delay", twist_additional_delay_, double(0.0)); - pnh_.param("twist_rate", twist_rate_, double(10.0)); // used for covariance calculation - pnh_.param("twist_gate_dist", twist_gate_dist_, double(10000.0)); // Mahalanobis limit - pnh_.param("twist_stddev_vx", twist_stddev_vx_, double(0.2)); - pnh_.param("twist_stddev_wz", twist_stddev_wz_, double(0.03)); - pnh_.param("use_twist_with_covariance", use_twist_with_covariance_, bool(false)); - - /* process noise */ - double proc_stddev_yaw_c, proc_stddev_yaw_bias_c, proc_stddev_vx_c, proc_stddev_wz_c; - pnh_.param("proc_stddev_yaw_c", proc_stddev_yaw_c, double(0.005)); - pnh_.param("proc_stddev_yaw_bias_c", proc_stddev_yaw_bias_c, double(0.001)); - pnh_.param("proc_stddev_vx_c", proc_stddev_vx_c, double(2.0)); - pnh_.param("proc_stddev_wz_c", proc_stddev_wz_c, double(0.2)); - if (!enable_yaw_bias_estimation_) - { - proc_stddev_yaw_bias_c = 0.0; - } - - /* convert to continuous to discrete */ - proc_cov_vx_d_ = std::pow(proc_stddev_vx_c, 2.0) * ekf_dt_; - proc_cov_wz_d_ = std::pow(proc_stddev_wz_c, 2.0) * ekf_dt_; - proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c, 2.0) * ekf_dt_; - proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c, 2.0) * ekf_dt_; - - /* initialize ros system */ - - timer_control_ = nh_.createTimer(ros::Duration(ekf_dt_), &EKFLocalizer::timerCallback, this); - pub_pose_ = nh_.advertise("ekf_pose", 1); - pub_pose_cov_ = nh_.advertise("ekf_pose_with_covariance", 1); - pub_twist_ = nh_.advertise("ekf_twist", 1); - pub_twist_cov_ = nh_.advertise("ekf_twist_with_covariance", 1); - pub_yaw_bias_ = pnh_.advertise("estimated_yaw_bias", 1); - sub_initialpose_ = nh_.subscribe("initialpose", 1, &EKFLocalizer::callbackInitialPose, this); - sub_pose_with_cov_ = nh_.subscribe("in_pose_with_covariance", 1, &EKFLocalizer::callbackPoseWithCovariance, this); - sub_pose_ = nh_.subscribe("in_pose", 1, &EKFLocalizer::callbackPose, this); - sub_twist_with_cov_ = nh_.subscribe("in_twist_with_covariance", 1, &EKFLocalizer::callbackTwistWithCovariance, this); - sub_twist_ = nh_.subscribe("in_twist", 1, &EKFLocalizer::callbackTwist, this); - - dim_x_ex_ = dim_x_ * extend_state_step_; - - initEKF(); - - /* debug */ - pub_debug_ = pnh_.advertise("debug", 1); - pub_measured_pose_ = pnh_.advertise("debug/measured_pose", 1); -}; - -EKFLocalizer::~EKFLocalizer(){}; -/* - * timerCallback - */ -void EKFLocalizer::timerCallback(const ros::TimerEvent& e) -{ - DEBUG_INFO("========================= timer called ========================="); - - /* predict model in EKF */ - auto start = std::chrono::system_clock::now(); - DEBUG_INFO("------------------------- start prediction -------------------------"); - predictKinematicsModel(); - double elapsed = - std::chrono::duration_cast(std::chrono::system_clock::now() - start).count(); - DEBUG_INFO("[EKF] predictKinematicsModel calculation time = %f [ms]", elapsed * 1.0e-6); - DEBUG_INFO("------------------------- end prediction -------------------------\n"); - - /* pose measurement update */ - if (current_pose_ptr_ != nullptr) - { - DEBUG_INFO("------------------------- start Pose -------------------------"); - start = std::chrono::system_clock::now(); - measurementUpdatePose(*current_pose_ptr_); - elapsed = std::chrono::duration_cast(std::chrono::system_clock::now() - start).count(); - DEBUG_INFO("[EKF] measurementUpdatePose calculation time = %f [ms]", elapsed * 1.0e-6); - DEBUG_INFO("------------------------- end Pose -------------------------\n"); - } - - /* twist measurement update */ - if (current_twist_ptr_ != nullptr) - { - DEBUG_INFO("------------------------- start twist -------------------------"); - start = std::chrono::system_clock::now(); - measurementUpdateTwist(*current_twist_ptr_); - elapsed = std::chrono::duration_cast(std::chrono::system_clock::now() - start).count(); - DEBUG_INFO("[EKF] measurementUpdateTwist calculation time = %f [ms]", elapsed * 1.0e-6); - DEBUG_INFO("------------------------- end twist -------------------------\n"); - } - - /* set current pose, twist */ - setCurrentResult(); - - /* publish ekf result */ - publishEstimateResult(); - broadcastTF(); -} - -void EKFLocalizer::showCurrentX() -{ - if (show_debug_info_) - { - Eigen::MatrixXd X(dim_x_, 1); - ekf_.getLatestX(X); - DEBUG_PRINT_MAT(X.transpose()); - } -} +namespace ekf_localizer{ -/* - * setCurrentResult - */ -void EKFLocalizer::setCurrentResult() -{ - current_ekf_pose_.header.frame_id = pose_frame_id_; - current_ekf_pose_.header.stamp = ros::Time::now(); - current_ekf_pose_.pose.position.x = ekf_.getXelement(IDX::X); - current_ekf_pose_.pose.position.y = ekf_.getXelement(IDX::Y); - - tf2::Quaternion q_tf; - double roll, pitch, yaw; - if (current_pose_ptr_ != nullptr) - { - current_ekf_pose_.pose.position.z = current_pose_ptr_->pose.position.z; - tf2::fromMsg(current_pose_ptr_->pose.orientation, q_tf); /* use Pose pitch and roll */ - tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw); - } - else - { - current_ekf_pose_.pose.position.z = 0.0; - roll = 0; - pitch = 0; - } - yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB); - q_tf.setRPY(roll, pitch, yaw); - tf2::convert(q_tf, current_ekf_pose_.pose.orientation); - - current_ekf_twist_.header.frame_id = child_frame_id_; - current_ekf_twist_.header.stamp = current_ekf_pose_.header.stamp; // Twist time stamp should exactly match pose timestamp since they were computed in the same EKF step - current_ekf_twist_.twist.linear.x = ekf_.getXelement(IDX::VX); - current_ekf_twist_.twist.angular.z = ekf_.getXelement(IDX::WZ); -} + EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions &options) : carma_ros2_utils::CarmaLifecycleNode(options), dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */) + { + show_debug_info_ = declare_parameter("show_debug_info", show_debug_info_); + ekf_rate_ = declare_parameter("predict_frequency", ekf_rate_); + enable_yaw_bias_estimation_ = declare_parameter("enable_yaw_bias_estimation", enable_yaw_bias_estimation_); + extend_state_step_ = declare_parameter("extend_state_step",extend_state_step_); + pose_frame_id_ = declare_parameter("pose_frame_id", pose_frame_id_); + child_frame_id_ = declare_parameter("child_frame_id", child_frame_id_); + + /* pose measurement */ + pose_additional_delay_ = declare_parameter("pose_additional_delay", pose_additional_delay_); + pose_measure_uncertainty_time_ = declare_parameter("pose_measure_uncertainty_time", pose_measure_uncertainty_time_); + pose_rate_ = declare_parameter("pose_rate", pose_rate_); + pose_gate_dist_ = declare_parameter("pose_gate_dist", pose_gate_dist_); + pose_stddev_x_ = declare_parameter("pose_stddev_x", pose_stddev_x_); + pose_stddev_y_ = declare_parameter("pose_stddev_y", pose_stddev_y_); + pose_stddev_yaw_ = declare_parameter("pose_stddev_yaw", pose_stddev_yaw_); + use_pose_with_covariance_ = declare_parameter("use_pose_with_covariance", use_pose_with_covariance_); + + /* twist measurement */ + twist_additional_delay_ = declare_parameter("twist_additional_delay", twist_additional_delay_); + twist_rate_ = declare_parameter("twist_rate", twist_rate_); + twist_gate_dist_ = declare_parameter("twist_gate_dist", twist_gate_dist_); + twist_stddev_vx_ = declare_parameter("twist_stddev_vx", twist_stddev_vx_); + twist_stddev_wz_ = declare_parameter("twist_stddev_wz",twist_stddev_wz_); + use_twist_with_covariance_ = declare_parameter("use_twist_with_covariance", use_twist_with_covariance_); + + + /* process noise */ + proc_stddev_yaw_c_ = declare_parameter("proc_stddev_yaw_c", proc_stddev_yaw_c_); + proc_stddev_yaw_bias_c_ = declare_parameter("proc_stddev_yaw_bias_c", proc_stddev_yaw_bias_c_); + proc_stddev_vx_c_ = declare_parameter("proc_stddev_vx_c", proc_stddev_vx_c_); + proc_stddev_wz_c_ = declare_parameter("proc_stddev_wz_c", proc_stddev_wz_c_); + + } -/* - * broadcastTF - */ -void EKFLocalizer::broadcastTF() -{ - if (current_ekf_pose_.header.frame_id == "") - return; - - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = current_ekf_pose_.header.stamp; // Transform stamp should exactly match the same of the data it is set from - transformStamped.header.frame_id = current_ekf_pose_.header.frame_id; - transformStamped.child_frame_id = child_frame_id_; - transformStamped.transform.translation.x = current_ekf_pose_.pose.position.x; - transformStamped.transform.translation.y = current_ekf_pose_.pose.position.y; - transformStamped.transform.translation.z = current_ekf_pose_.pose.position.z; - - transformStamped.transform.rotation.x = current_ekf_pose_.pose.orientation.x; - transformStamped.transform.rotation.y = current_ekf_pose_.pose.orientation.y; - transformStamped.transform.rotation.z = current_ekf_pose_.pose.orientation.z; - transformStamped.transform.rotation.w = current_ekf_pose_.pose.orientation.w; - - tf_br_.sendTransform(transformStamped); -} + carma_ros2_utils::CallbackReturn EKFLocalizer::handle_on_configure(const rclcpp_lifecycle::State &prev_state) + { + get_parameter("show_debug_info", show_debug_info_); + get_parameter("predict_frequency", ekf_rate_); + ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); + get_parameter("enable_yaw_bias_estimation", enable_yaw_bias_estimation_); + get_parameter("extend_state_step",extend_state_step_); + get_parameter("pose_frame_id", pose_frame_id_); + get_parameter("child_frame_id", child_frame_id_); + + /* pose measurement */ + get_parameter("pose_additional_delay", pose_additional_delay_); + get_parameter("pose_measure_uncertainty_time", pose_measure_uncertainty_time_); + get_parameter("pose_rate", pose_rate_); + get_parameter("pose_gate_dist", pose_gate_dist_); + get_parameter("pose_stddev_x", pose_stddev_x_); + get_parameter("pose_stddev_y", pose_stddev_y_); + get_parameter("pose_stddev_yaw", pose_stddev_yaw_); + get_parameter("use_pose_with_covariance", use_pose_with_covariance_); + + /* twist measurement */ + get_parameter("twist_additional_delay", twist_additional_delay_); + get_parameter("twist_rate", twist_rate_); + get_parameter("twist_gate_dist", twist_gate_dist_); + get_parameter("twist_stddev_vx", twist_stddev_vx_); + get_parameter("twist_stddev_wz",twist_stddev_wz_); + get_parameter("use_twist_with_covariance", use_twist_with_covariance_); + + + /* process noise */ + get_parameter("proc_stddev_yaw_c", proc_stddev_yaw_c_); + get_parameter("proc_stddev_yaw_bias_c", proc_stddev_yaw_bias_c_); + get_parameter("proc_stddev_vx_c", proc_stddev_vx_c_); + get_parameter("proc_stddev_wz_c", proc_stddev_wz_c_); + + + if (!enable_yaw_bias_estimation_) + { + proc_stddev_yaw_bias_c_ = 0.0; + } + + /* convert to continuous to discrete */ + proc_cov_vx_d_ = std::pow(proc_stddev_vx_c_, 2.0) * ekf_dt_; + proc_cov_wz_d_ = std::pow(proc_stddev_wz_c_, 2.0) * ekf_dt_; + proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_, 2.0) * ekf_dt_; + proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c_, 2.0) * ekf_dt_; + + tf_br_shared_ptr_ = std::make_shared(shared_from_this()); + + /* initialize ros system */ + timer_control_ = create_wall_timer(std::chrono::milliseconds(int(ekf_dt_*1000)), std::bind(&EKFLocalizer::timerCallback, this)); + pub_pose_ = create_publisher("ekf_pose", 1); + pub_pose_cov_ = create_publisher("ekf_pose_with_covariance", 1); + pub_twist_ = create_publisher("ekf_twist", 1); + pub_twist_cov_ = create_publisher("ekf_twist_with_covariance", 1); + pub_yaw_bias_ = create_publisher("estimated_yaw_bias", 1); + sub_initialpose_ = create_subscription("initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, std::placeholders::_1)); + sub_pose_with_cov_ = create_subscription("in_pose_with_covariance", 1, std::bind(&EKFLocalizer::callbackPoseWithCovariance, this, std::placeholders::_1)); + sub_pose_ = create_subscription("in_pose", 1, std::bind(&EKFLocalizer::callbackPose, this, std::placeholders::_1)); + sub_twist_with_cov_ = create_subscription("in_twist_with_covariance", 1, std::bind(&EKFLocalizer::callbackTwistWithCovariance, this, std::placeholders::_1)); + sub_twist_ = create_subscription("in_twist", 1, std::bind(&EKFLocalizer::callbackTwist, this, std::placeholders::_1)); + + dim_x_ex_ = dim_x_ * extend_state_step_; + initEKF(); + + + pub_debug_ = create_publisher("debug", 1); + pub_measured_pose_ = create_publisher("debug/measured_pose", 1); + + clk_ = *this->get_clock(); + return CallbackReturn::SUCCESS; + } -/* - * getTransformFromTF - */ -bool EKFLocalizer::getTransformFromTF(std::string parent_frame, std::string child_frame, - geometry_msgs::TransformStamped& transform) -{ - tf2_ros::Buffer tf_buffer; - tf2_ros::TransformListener tf_listener(tf_buffer); - ros::Duration(0.1).sleep(); - if (parent_frame.front() == '/') - parent_frame.erase(0, 1); - if (child_frame.front() == '/') - child_frame.erase(0, 1); - - for (int i = 0; i < 50; ++i) - { - try + void EKFLocalizer::timerCallback() { - transform = tf_buffer.lookupTransform(parent_frame, child_frame, ros::Time(0)); - return true; + DEBUG_INFO("========================= timer called ========================="); + /* predict model in EKF */ + auto start = std::chrono::system_clock::now(); + DEBUG_INFO("------------------------- start prediction -------------------------"); + predictKinematicsModel(); + double elapsed = + std::chrono::duration_cast(std::chrono::system_clock::now() - start).count(); + DEBUG_INFO("[EKF] predictKinematicsModel calculation time = %f [ms]", elapsed * 1.0e-6); + DEBUG_INFO("------------------------- end prediction -------------------------\n"); + /* pose measurement update */ + if (current_pose_ptr_ != nullptr) + { + DEBUG_INFO("------------------------- start Pose -------------------------"); + start = std::chrono::system_clock::now(); + measurementUpdatePose(*current_pose_ptr_); + elapsed = std::chrono::duration_cast(std::chrono::system_clock::now() - start).count(); + DEBUG_INFO("[EKF] measurementUpdatePose calculation time = %f [ms]", elapsed * 1.0e-6); + DEBUG_INFO("------------------------- end Pose -------------------------\n"); + } + + /* twist measurement update */ + if (current_twist_ptr_ != nullptr) + { + DEBUG_INFO("------------------------- start twist -------------------------"); + start = std::chrono::system_clock::now(); + measurementUpdateTwist(*current_twist_ptr_); + elapsed = std::chrono::duration_cast(std::chrono::system_clock::now() - start).count(); + DEBUG_INFO("[EKF] measurementUpdateTwist calculation time = %f [ms]", elapsed * 1.0e-6); + DEBUG_INFO("------------------------- end twist -------------------------\n"); + } + + /* set current pose, twist */ + setCurrentResult(); + + /* publish ekf result */ + publishEstimateResult(); + broadcastTF(); } - catch (tf2::TransformException& ex) + + void EKFLocalizer::showCurrentX() { - ROS_WARN("%s", ex.what()); - ros::Duration(0.1).sleep(); + if (show_debug_info_) + { + Eigen::MatrixXd X(dim_x_, 1); + ekf_.getLatestX(X); + DEBUG_PRINT_MAT(X.transpose()); + } } - } - return false; -} -/* - * callbackInitialPose - */ -void EKFLocalizer::callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped& initialpose) -{ - geometry_msgs::TransformStamped transform; - if (!getTransformFromTF(pose_frame_id_, initialpose.header.frame_id, transform)) - { - ROS_ERROR("[EKF] TF transform failed. parent = %s, child = %s", pose_frame_id_.c_str(), - initialpose.header.frame_id.c_str()); - }; - - Eigen::MatrixXd X(dim_x_, 1); - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); - - X(IDX::X) = initialpose.pose.pose.position.x + transform.transform.translation.x; - X(IDX::Y) = initialpose.pose.pose.position.y + transform.transform.translation.y; - X(IDX::YAW) = tf2::getYaw(initialpose.pose.pose.orientation) + tf2::getYaw(transform.transform.rotation); - X(IDX::YAWB) = 0.0; - X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; - - P(IDX::X, IDX::X) = initialpose.pose.covariance[0]; - P(IDX::Y, IDX::Y) = initialpose.pose.covariance[6 + 1]; - P(IDX::YAW, IDX::YAW) = initialpose.pose.covariance[6 * 5 + 5]; - P(IDX::YAWB, IDX::YAWB) = 0.0001; - P(IDX::VX, IDX::VX) = 0.01; - P(IDX::WZ, IDX::WZ) = 0.01; - - ekf_.init(X, P, extend_state_step_); -}; + /* + * setCurrentResult + */ + void EKFLocalizer::setCurrentResult() + { + current_ekf_pose_.header.frame_id = pose_frame_id_; + current_ekf_pose_.header.stamp = this->now(); + current_ekf_pose_.pose.position.x = ekf_.getXelement(IDX::X); + current_ekf_pose_.pose.position.y = ekf_.getXelement(IDX::Y); + + tf2::Quaternion q_tf; + double roll, pitch, yaw; + if (current_pose_ptr_ != nullptr) + { + current_ekf_pose_.pose.position.z = current_pose_ptr_->pose.position.z; + tf2::fromMsg(current_pose_ptr_->pose.orientation, q_tf); /* use Pose pitch and roll */ + tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw); + } + else + { + current_ekf_pose_.pose.position.z = 0.0; + roll = 0; + pitch = 0; + } + yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB); + q_tf.setRPY(roll, pitch, yaw); + tf2::convert(q_tf, current_ekf_pose_.pose.orientation); + + current_ekf_twist_.header.frame_id = child_frame_id_; + current_ekf_twist_.header.stamp = current_ekf_pose_.header.stamp; // Twist time stamp should exactly match pose timestamp since they were computed in the same EKF step + current_ekf_twist_.twist.linear.x = ekf_.getXelement(IDX::VX); + current_ekf_twist_.twist.angular.z = ekf_.getXelement(IDX::WZ); + } -/* - * callbackPose - */ -void EKFLocalizer::callbackPose(const geometry_msgs::PoseStamped::ConstPtr& msg) -{ - if (!use_pose_with_covariance_) - { - current_pose_ptr_ = std::make_shared(*msg); - } -}; -/* - * callbackPoseWithCovariance - */ -void EKFLocalizer::callbackPoseWithCovariance(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) -{ - if (use_pose_with_covariance_) - { - geometry_msgs::PoseStamped pose; - pose.header = msg->header; - pose.pose = msg->pose.pose; - current_pose_ptr_ = std::make_shared(pose); - current_pose_covariance_ = msg->pose.covariance; - } -}; + /* + * broadcastTF + */ + void EKFLocalizer::broadcastTF() + { + + if (current_ekf_pose_.header.frame_id == "") + return; + + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.stamp = current_ekf_pose_.header.stamp; // Transform stamp should exactly match the same of the data it is set from + transformStamped.header.frame_id = current_ekf_pose_.header.frame_id; + transformStamped.child_frame_id = child_frame_id_; + transformStamped.transform.translation.x = current_ekf_pose_.pose.position.x; + transformStamped.transform.translation.y = current_ekf_pose_.pose.position.y; + transformStamped.transform.translation.z = current_ekf_pose_.pose.position.z; + + transformStamped.transform.rotation.x = current_ekf_pose_.pose.orientation.x; + transformStamped.transform.rotation.y = current_ekf_pose_.pose.orientation.y; + transformStamped.transform.rotation.z = current_ekf_pose_.pose.orientation.z; + transformStamped.transform.rotation.w = current_ekf_pose_.pose.orientation.w; + + tf_br_shared_ptr_->sendTransform(transformStamped); + } -/* - * callbackTwist - */ -void EKFLocalizer::callbackTwist(const geometry_msgs::TwistStamped::ConstPtr& msg) -{ - if (!use_twist_with_covariance_) - { - current_twist_ptr_ = std::make_shared(*msg); - } -}; + /* + * getTransformFromTF + */ + bool EKFLocalizer::getTransformFromTF(std::string parent_frame, std::string child_frame, + geometry_msgs::msg::TransformStamped& transform) + { + tf2_ros::Buffer tf_buffer(get_clock()); + tf2_ros::TransformListener tf_listener(tf_buffer); + + rclcpp::sleep_for(std::chrono::milliseconds(100)); + if (parent_frame.front() == '/') + parent_frame.erase(0, 1); + if (child_frame.front() == '/') + child_frame.erase(0, 1); + + for (int i = 0; i < 50; ++i) + { + try + { + transform = tf_buffer.lookupTransform(parent_frame, child_frame, rclcpp::Time(0,0)); + return true; + } + catch (tf2::TransformException& ex) + { + + RCLCPP_WARN(get_logger(), ex.what()); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + } + return false; + } -/* - * callbackTwistWithCovariance - */ -void EKFLocalizer::callbackTwistWithCovariance(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg) -{ - if (use_twist_with_covariance_) - { - geometry_msgs::TwistStamped twist; - twist.header = msg->header; - twist.twist = msg->twist.twist; - current_twist_ptr_ = std::make_shared(twist); - current_twist_covariance_ = msg->twist.covariance; - } -}; + /* + * callbackInitialPose + */ + void EKFLocalizer::callbackInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr initialpose) + { + geometry_msgs::msg::TransformStamped transform; + if (!getTransformFromTF(pose_frame_id_, initialpose->header.frame_id, transform)) + { + RCLCPP_ERROR(get_logger(), "[EKF] TF transform failed. parent = %s, child = %s", pose_frame_id_.c_str(), + initialpose->header.frame_id.c_str()); + }; + + Eigen::MatrixXd X(dim_x_, 1); + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); + + X(IDX::X) = initialpose->pose.pose.position.x + transform.transform.translation.x; + X(IDX::Y) = initialpose->pose.pose.position.y + transform.transform.translation.y; + X(IDX::YAW) = tf2::getYaw(initialpose->pose.pose.orientation) + tf2::getYaw(transform.transform.rotation); + X(IDX::YAWB) = 0.0; + X(IDX::VX) = 0.0; + X(IDX::WZ) = 0.0; + + P(IDX::X, IDX::X) = initialpose->pose.covariance[0]; + P(IDX::Y, IDX::Y) = initialpose->pose.covariance[6 + 1]; + P(IDX::YAW, IDX::YAW) = initialpose->pose.covariance[6 * 5 + 5]; + P(IDX::YAWB, IDX::YAWB) = 0.0001; + P(IDX::VX, IDX::VX) = 0.01; + P(IDX::WZ, IDX::WZ) = 0.01; + + ekf_.init(X, P, extend_state_step_); + } -/* - * initEKF - */ -void EKFLocalizer::initEKF() -{ - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); - Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y - P(IDX::YAW, IDX::YAW) = 50.0; // for yaw - P(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_; // for yaw bias - P(IDX::VX, IDX::VX) = 1000.0; // for vx - P(IDX::WZ, IDX::WZ) = 50.0; // for wz - - ekf_.init(X, P, extend_state_step_); -} + /* + * callbackPose + */ + void EKFLocalizer::callbackPose(const geometry_msgs::msg::PoseStamped::SharedPtr msg) + { + if (!use_pose_with_covariance_) + { + current_pose_ptr_ = msg; + } + } -/* - * predictKinematicsModel - */ -void EKFLocalizer::predictKinematicsModel() -{ - /* == Nonlinear model == - * - * x_{k+1} = x_k + vx_k * cos(yaw_k + b_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + b_k) * dt - * yaw_{k+1} = yaw_k + (wz_k) * dt - * b_{k+1} = b_k - * vx_{k+1} = vz_k - * wz_{k+1} = wz_k - * - * (b_k : yaw_bias_k) - */ - - /* == Linearized model == - * - * A = [ 1, 0, -vx*sin(yaw+b)*dt, -vx*sin(yaw+b)*dt, cos(yaw+b)*dt, 0] - * [ 0, 1, vx*cos(yaw+b)*dt, vx*cos(yaw+b)*dt, sin(yaw+b)*dt, 0] - * [ 0, 0, 1, 0, 0, dt] - * [ 0, 0, 0, 1, 0, 0] - * [ 0, 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 0, 1] - */ - - Eigen::MatrixXd X_curr(dim_x_, 1); // curent state - Eigen::MatrixXd X_next(dim_x_, 1); // predicted state - ekf_.getLatestX(X_curr); - DEBUG_PRINT_MAT(X_curr.transpose()); - - Eigen::MatrixXd P_curr; - ekf_.getLatestP(P_curr); - - const int d_dim_x = dim_x_ex_ - dim_x_; - const double yaw = X_curr(IDX::YAW); - const double yaw_bias = X_curr(IDX::YAWB); - const double vx = X_curr(IDX::VX); - const double wz = X_curr(IDX::WZ); - const double dt = ekf_dt_; - - /* Update for latest state */ - X_next(IDX::X) = X_curr(IDX::X) + vx * cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) - X_next(IDX::Y) = X_curr(IDX::Y) + vx * sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) - X_next(IDX::YAW) = X_curr(IDX::YAW) + (wz)*dt; // dyaw = omega + omega_bias - X_next(IDX::YAWB) = yaw_bias; - X_next(IDX::VX) = vx; - X_next(IDX::WZ) = wz; - - X_next(IDX::YAW) = std::atan2(std::sin(X_next(IDX::YAW)), std::cos(X_next(IDX::YAW))); - - /* Set A matrix for latest state */ - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(dim_x_, dim_x_); - A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; - A(IDX::YAW, IDX::WZ) = dt; - - const double dvx = std::sqrt(P_curr(IDX::VX, IDX::VX)); - const double dyaw = std::sqrt(P_curr(IDX::YAW, IDX::YAW)); - - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x_, dim_x_); - - if (dvx < 10.0 && dyaw < 1.0) - { - // auto covariance calculate for x, y assuming vx & yaw estimation covariance is small - - /* Set covariance matrix Q for process noise. Calc Q by velocity and yaw angle covariance : - dx = Ax + Jp*w -> Q = Jp*w_cov*Jp' */ - Eigen::MatrixXd Jp = Eigen::MatrixXd::Zero(2, 2); // coeff of deviation of vx & yaw - Jp << cos(yaw), -vx * sin(yaw), sin(yaw), vx * cos(yaw); - Eigen::MatrixXd Q_vx_yaw = Eigen::MatrixXd::Zero(2, 2); // cov of vx and yaw - - Q_vx_yaw(0, 0) = P_curr(IDX::VX, IDX::VX) * dt; // covariance of vx - vx - Q_vx_yaw(1, 1) = P_curr(IDX::YAW, IDX::YAW) * dt; // covariance of yaw - yaw - Q_vx_yaw(0, 1) = P_curr(IDX::VX, IDX::YAW) * dt; // covariance of vx - yaw - Q_vx_yaw(1, 0) = P_curr(IDX::YAW, IDX::VX) * dt; // covariance of yaw - vx - Q.block(0, 0, 2, 2) = Jp * Q_vx_yaw * Jp.transpose(); // for pos_x & pos_y - } - else - { - // vx & vy is not converged yet, set constant value. - Q(IDX::X, IDX::X) = 0.05; - Q(IDX::Y, IDX::Y) = 0.05; - } - - Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d_; // for yaw - Q(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_; // for yaw bias - Q(IDX::VX, IDX::VX) = proc_cov_vx_d_; // for vx - Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d_; // for wz - - ekf_.predictWithDelay(X_next, A, Q); - - // debug - Eigen::MatrixXd X_result(dim_x_, 1); - ekf_.getLatestX(X_result); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); -} + /* + * callbackPoseWithCovariance + */ + void EKFLocalizer::callbackPoseWithCovariance(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) + { + if (use_pose_with_covariance_) + { + geometry_msgs::msg::PoseStamped pose; + pose.header = msg->header; + pose.pose = msg->pose.pose; + current_pose_ptr_ = std::make_shared(pose); + current_pose_covariance_ = msg->pose.covariance; + } + + } -/* - * measurementUpdatePose - */ -void EKFLocalizer::measurementUpdatePose(const geometry_msgs::PoseStamped& pose) -{ - if (pose.header.frame_id != pose_frame_id_) - { - ROS_WARN_DELAYED_THROTTLE(2, "pose frame_id is %s, but pose_frame is set as %s. They must be same.", - pose.header.frame_id.c_str(), pose_frame_id_.c_str()); - } - Eigen::MatrixXd X_curr(dim_x_, 1); // curent state - ekf_.getLatestX(X_curr); - DEBUG_PRINT_MAT(X_curr.transpose()); - - constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output - const ros::Time t_curr = ros::Time::now(); - - /* Calculate delay step */ - double delay_time = (t_curr - pose.header.stamp).toSec() + pose_additional_delay_; - if (delay_time < 0.0) - { - delay_time = 0.0; - ROS_WARN_DELAYED_THROTTLE(1.0, "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time); - } - int delay_step = std::roundf(delay_time / ekf_dt_); - if (delay_step > extend_state_step_ - 1) - { - ROS_WARN_DELAYED_THROTTLE(1.0, - "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = " - "extend_state_step * ekf_dt : %f [s]", - delay_time, extend_state_step_ * ekf_dt_); - return; - } - DEBUG_INFO("delay_time: %f [s]", delay_time); - - /* Set yaw */ - const double yaw_curr = ekf_.getXelement((unsigned int)(delay_step * dim_x_ + IDX::YAW)); - double yaw = tf2::getYaw(pose.pose.orientation); - const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW); - const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi - yaw = yaw_error + ekf_yaw; - - /* Set measurement matrix */ - Eigen::MatrixXd y(dim_y, 1); - y << pose.pose.position.x, pose.pose.position.y, yaw; - - if (isnan(y.array()).any() || isinf(y.array()).any()) - { - ROS_WARN("[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); - return; - } - - /* Gate */ - Eigen::MatrixXd y_ekf(dim_y, 1); - y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::X), ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw; - Eigen::MatrixXd P_curr, P_y; - ekf_.getLatestP(P_curr); - P_y = P_curr.block(0, 0, dim_y, dim_y); - if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y)) - { - ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore " - "measurement data."); - return; - } - - DEBUG_PRINT_MAT(y.transpose()); - DEBUG_PRINT_MAT(y_ekf.transpose()); - DEBUG_PRINT_MAT((y - y_ekf).transpose()); - - /* Set measurement matrix */ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_); - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - - /* Set measurement noise covariancs */ - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (use_pose_with_covariance_) - { - R(0, 0) = current_pose_covariance_.at(0); // x - x - R(0, 1) = current_pose_covariance_.at(1); // x - y - R(0, 2) = current_pose_covariance_.at(5); // x - yaw - R(1, 0) = current_pose_covariance_.at(6); // y - x - R(1, 1) = current_pose_covariance_.at(7); // y - y - R(1, 2) = current_pose_covariance_.at(11); // y - yaw - R(2, 0) = current_pose_covariance_.at(30); // yaw - x - R(2, 1) = current_pose_covariance_.at(31); // yaw - y - R(2, 2) = current_pose_covariance_.at(35); // yaw - yaw - } - else - { - const double ekf_yaw = ekf_.getXelement(IDX::YAW); - const double vx = ekf_.getXelement(IDX::VX); - const double wz = ekf_.getXelement(IDX::WZ); - const double cov_pos_x = std::pow(pose_measure_uncertainty_time_ * vx * cos(ekf_yaw), 2.0); - const double cov_pos_y = std::pow(pose_measure_uncertainty_time_ * vx * sin(ekf_yaw), 2.0); - const double cov_yaw = std::pow(pose_measure_uncertainty_time_ * wz, 2.0); - R(0, 0) = std::pow(pose_stddev_x_, 2) + cov_pos_x; // pos_x - R(1, 1) = std::pow(pose_stddev_y_, 2) + cov_pos_y; // pos_y - R(2, 2) = std::pow(pose_stddev_yaw_, 2) + cov_yaw; // yaw - } - - /* In order to avoid a large change at the time of updating, measuremeent update is performed by dividing at every - * step. */ - R *= (ekf_rate_ / pose_rate_); - - ekf_.updateWithDelay(y, C, R, delay_step); - - // debug - Eigen::MatrixXd X_result(dim_x_, 1); - ekf_.getLatestX(X_result); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); -} + /* + * callbackTwist + */ + void EKFLocalizer::callbackTwist(const geometry_msgs::msg::TwistStamped::SharedPtr msg) + { + if (!use_twist_with_covariance_) + { + current_twist_ptr_ = std::make_shared(*msg); + } + } -/* - * measurementUpdateTwist - */ -void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::TwistStamped& twist) -{ - if (twist.header.frame_id != child_frame_id_) - { - ROS_WARN_STREAM_DELAYED_THROTTLE(2.0, "twist frame_id must be " << child_frame_id_); - } - - Eigen::MatrixXd X_curr(dim_x_, 1); // curent state - ekf_.getLatestX(X_curr); - DEBUG_PRINT_MAT(X_curr.transpose()); - - constexpr int dim_y = 2; // vx, wz - const ros::Time t_curr = ros::Time::now(); - - /* Calculate delay step */ - double delay_time = (t_curr - twist.header.stamp).toSec() + twist_additional_delay_; - if (delay_time < 0.0) - { - ROS_WARN_DELAYED_THROTTLE(1.0, "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", - delay_time); - delay_time = 0.0; - } - int delay_step = std::roundf(delay_time / ekf_dt_); - if (delay_step > extend_state_step_ - 1) - { - ROS_WARN_DELAYED_THROTTLE(1.0, - "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = " - "extend_state_step * ekf_dt : %f [s]", - delay_time, extend_state_step_ * ekf_dt_); - return; - } - DEBUG_INFO("delay_time: %f [s]", delay_time); - - /* Set measurement matrix */ - Eigen::MatrixXd y(dim_y, 1); - y << twist.twist.linear.x, twist.twist.angular.z; - - if (isnan(y.array()).any() || isinf(y.array()).any()) - { - ROS_WARN("[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); - return; - } - - /* Gate */ - Eigen::MatrixXd y_ekf(dim_y, 1); - y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::VX), ekf_.getXelement(delay_step * dim_x_ + IDX::WZ); - Eigen::MatrixXd P_curr, P_y; - ekf_.getLatestP(P_curr); - P_y = P_curr.block(4, 4, dim_y, dim_y); - if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y)) - { - ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore " - "measurement data."); - return; - } - - DEBUG_PRINT_MAT(y.transpose()); - DEBUG_PRINT_MAT(y_ekf.transpose()); - DEBUG_PRINT_MAT((y - y_ekf).transpose()); - - /* Set measurement matrix */ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_); - C(0, IDX::VX) = 1.0; // for vx - C(1, IDX::WZ) = 1.0; // for wz - - /* Set measurement noise covariancs */ - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (use_twist_with_covariance_) - { - R(0, 0) = current_twist_covariance_.at(0); // vx - vx - R(0, 1) = current_twist_covariance_.at(5); // vx - wz - R(1, 0) = current_twist_covariance_.at(30); // wz - vx - R(1, 1) = current_twist_covariance_.at(35); // wz - wz - } - else - { - R(0, 0) = twist_stddev_vx_ * twist_stddev_vx_ * ekf_dt_; // for vx - R(1, 1) = twist_stddev_wz_ * twist_stddev_wz_ * ekf_dt_; // for wz - } - - /* In order to avoid a large change by update, measurement update is performed by dividing at every step. */ - R *= (ekf_rate_ / twist_rate_); - - ekf_.updateWithDelay(y, C, R, delay_step); - - // debug - Eigen::MatrixXd X_result(dim_x_, 1); - ekf_.getLatestX(X_result); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); -}; + /* + * callbackTwistWithCovariance + */ + void EKFLocalizer::callbackTwistWithCovariance(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) + { + if (use_twist_with_covariance_) + { + geometry_msgs::msg::TwistStamped twist; + twist.header = msg->header; + twist.twist = msg->twist.twist; + current_twist_ptr_ = std::make_shared(twist); + current_twist_covariance_ = msg->twist.covariance; + } + } -/* - * mahalanobisGate - */ -bool EKFLocalizer::mahalanobisGate(const double& dist_max, const Eigen::MatrixXd& x, const Eigen::MatrixXd& obj_x, - const Eigen::MatrixXd& cov) -{ - Eigen::MatrixXd mahalanobis_squared = (x - obj_x).transpose() * cov.inverse() * (x - obj_x); - DEBUG_INFO("measurement update: mahalanobis = %f, gate limit = %f", std::sqrt(mahalanobis_squared(0)), dist_max); - if (mahalanobis_squared(0) > dist_max * dist_max) - { - return false; - } - - return true; -} -/* - * publishEstimateResult - */ -void EKFLocalizer::publishEstimateResult() -{ - ros::Time current_time = ros::Time::now(); - Eigen::MatrixXd X(dim_x_, 1); - Eigen::MatrixXd P(dim_x_, dim_x_); - ekf_.getLatestX(X); - ekf_.getLatestP(P); - - /* publish latest pose */ - pub_pose_.publish(current_ekf_pose_); - - /* publish latest pose with covariance */ - geometry_msgs::PoseWithCovarianceStamped pose_cov; - pose_cov.header.stamp = current_time; - pose_cov.header.frame_id = current_ekf_pose_.header.frame_id; - pose_cov.pose.pose = current_ekf_pose_.pose; - pose_cov.pose.covariance[0] = P(IDX::X, IDX::X); - pose_cov.pose.covariance[1] = P(IDX::X, IDX::Y); - pose_cov.pose.covariance[5] = P(IDX::X, IDX::YAW); - pose_cov.pose.covariance[6] = P(IDX::Y, IDX::X); - pose_cov.pose.covariance[7] = P(IDX::Y, IDX::Y); - pose_cov.pose.covariance[11] = P(IDX::Y, IDX::YAW); - pose_cov.pose.covariance[30] = P(IDX::YAW, IDX::X); - pose_cov.pose.covariance[31] = P(IDX::YAW, IDX::Y); - pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW); - pub_pose_cov_.publish(pose_cov); - - /* publish latest twist */ - pub_twist_.publish(current_ekf_twist_); - - /* publish latest twist with covariance */ - geometry_msgs::TwistWithCovarianceStamped twist_cov; - twist_cov.header.stamp = current_time; - twist_cov.header.frame_id = current_ekf_twist_.header.frame_id; - twist_cov.twist.twist = current_ekf_twist_.twist; - twist_cov.twist.covariance[0] = P(IDX::VX, IDX::VX); - twist_cov.twist.covariance[5] = P(IDX::VX, IDX::WZ); - twist_cov.twist.covariance[30] = P(IDX::WZ, IDX::VX); - twist_cov.twist.covariance[35] = P(IDX::WZ, IDX::WZ); - pub_twist_cov_.publish(twist_cov); - - - /* publish yaw bias */ - std_msgs::Float64 yawb; - yawb.data = X(IDX::YAWB); - pub_yaw_bias_.publish(yawb); - - /* debug measured pose */ - if (current_pose_ptr_ != nullptr) - { - geometry_msgs::PoseStamped p; - p = *current_pose_ptr_; - p.header.stamp = current_time; - pub_measured_pose_.publish(p); - } - - /* debug publish */ - double RAD2DEG = 180.0 / 3.141592; - double pose_yaw = 0.0; - if (current_pose_ptr_ != nullptr) - pose_yaw = tf2::getYaw(current_pose_ptr_->pose.orientation) * RAD2DEG; - - std_msgs::Float64MultiArray msg; - msg.data.push_back(X(IDX::YAW) * RAD2DEG); // [0] ekf yaw angle - msg.data.push_back(pose_yaw); // [1] measurement yaw angle - msg.data.push_back(X(IDX::YAWB) * RAD2DEG); // [2] yaw bias - pub_debug_.publish(msg); -} - -double EKFLocalizer::normalizeYaw(const double& yaw) -{ - return std::atan2(std::sin(yaw), std::cos(yaw)); -} + /* + * initEKF + */ + void EKFLocalizer::initEKF() + { + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); + Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y + P(IDX::YAW, IDX::YAW) = 50.0; // for yaw + P(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_; // for yaw bias + P(IDX::VX, IDX::VX) = 1000.0; // for vx + P(IDX::WZ, IDX::WZ) = 50.0; // for wz + ekf_.init(X, P, extend_state_step_); + } + + + /* + * predictKinematicsModel + */ + void EKFLocalizer::predictKinematicsModel() + { + /* == Nonlinear model == + * + * x_{k+1} = x_k + vx_k * cos(yaw_k + b_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k + b_k) * dt + * yaw_{k+1} = yaw_k + (wz_k) * dt + * b_{k+1} = b_k + * vx_{k+1} = vz_k + * wz_{k+1} = wz_k + * + * (b_k : yaw_bias_k) + */ + + /* == Linearized model == + * + * A = [ 1, 0, -vx*sin(yaw+b)*dt, -vx*sin(yaw+b)*dt, cos(yaw+b)*dt, 0] + * [ 0, 1, vx*cos(yaw+b)*dt, vx*cos(yaw+b)*dt, sin(yaw+b)*dt, 0] + * [ 0, 0, 1, 0, 0, dt] + * [ 0, 0, 0, 1, 0, 0] + * [ 0, 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 0, 1] + */ + Eigen::MatrixXd X_curr(dim_x_, 1); // curent state + Eigen::MatrixXd X_next(dim_x_, 1); // predicted state + ekf_.getLatestX(X_curr); + + DEBUG_PRINT_MAT(X_curr.transpose()); + + Eigen::MatrixXd P_curr; + ekf_.getLatestP(P_curr); + + const int d_dim_x = dim_x_ex_ - dim_x_; + const double yaw = X_curr(IDX::YAW); + const double yaw_bias = X_curr(IDX::YAWB); + const double vx = X_curr(IDX::VX); + const double wz = X_curr(IDX::WZ); + const double dt = ekf_dt_; + /* Update for latest state */ + X_next(IDX::X) = X_curr(IDX::X) + vx * cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) + X_next(IDX::Y) = X_curr(IDX::Y) + vx * sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) + X_next(IDX::YAW) = X_curr(IDX::YAW) + (wz)*dt; // dyaw = omega + omega_bias + X_next(IDX::YAWB) = yaw_bias; + X_next(IDX::VX) = vx; + X_next(IDX::WZ) = wz; + + X_next(IDX::YAW) = std::atan2(std::sin(X_next(IDX::YAW)), std::cos(X_next(IDX::YAW))); + /* Set A matrix for latest state */ + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(dim_x_, dim_x_); + A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; + A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; + A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; + A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; + A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; + A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; + A(IDX::YAW, IDX::WZ) = dt; + const double dvx = std::sqrt(P_curr(IDX::VX, IDX::VX)); + const double dyaw = std::sqrt(P_curr(IDX::YAW, IDX::YAW)); + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x_, dim_x_); + + if (dvx < 10.0 && dyaw < 1.0) + { + // auto covariance calculate for x, y assuming vx & yaw estimation covariance is small + + /* Set covariance matrix Q for process noise. Calc Q by velocity and yaw angle covariance : + dx = Ax + Jp*w -> Q = Jp*w_cov*Jp' */ + Eigen::MatrixXd Jp = Eigen::MatrixXd::Zero(2, 2); // coeff of deviation of vx & yaw + Jp << cos(yaw), -vx * sin(yaw), sin(yaw), vx * cos(yaw); + Eigen::MatrixXd Q_vx_yaw = Eigen::MatrixXd::Zero(2, 2); // cov of vx and yaw + + Q_vx_yaw(0, 0) = P_curr(IDX::VX, IDX::VX) * dt; // covariance of vx - vx + Q_vx_yaw(1, 1) = P_curr(IDX::YAW, IDX::YAW) * dt; // covariance of yaw - yaw + Q_vx_yaw(0, 1) = P_curr(IDX::VX, IDX::YAW) * dt; // covariance of vx - yaw + Q_vx_yaw(1, 0) = P_curr(IDX::YAW, IDX::VX) * dt; // covariance of yaw - vx + Q.block(0, 0, 2, 2) = Jp * Q_vx_yaw * Jp.transpose(); // for pos_x & pos_y + } + else + { + // vx & vy is not converged yet, set constant value. + Q(IDX::X, IDX::X) = 0.05; + Q(IDX::Y, IDX::Y) = 0.05; + } + + Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d_; // for yaw + Q(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_; // for yaw bias + Q(IDX::VX, IDX::VX) = proc_cov_vx_d_; // for vx + Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d_; // for wz + + ekf_.predictWithDelay(X_next, A, Q); + + // debug + Eigen::MatrixXd X_result(dim_x_, 1); + ekf_.getLatestX(X_result); + DEBUG_PRINT_MAT(X_result.transpose()); + DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + } + + /* + * measurementUpdatePose + */ + void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseStamped& pose) + { + if (pose.header.frame_id != pose_frame_id_) + { + RCLCPP_WARN_THROTTLE(get_logger(), clk_, 2000, "pose frame_id is %s, but pose_frame is set as %s. They must be same.", + pose.header.frame_id.c_str(), pose_frame_id_.c_str()); + } + Eigen::MatrixXd X_curr(dim_x_, 1); // curent state + ekf_.getLatestX(X_curr); + DEBUG_PRINT_MAT(X_curr.transpose()); + + constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output + const rclcpp::Time t_curr = this->now(); + + /* Calculate delay step */ + double delay_time = (t_curr - pose.header.stamp).seconds() + pose_additional_delay_; + if (delay_time < 0.0) + { + delay_time = 0.0; + RCLCPP_WARN_THROTTLE(get_logger(), clk_, 1000, "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time); + } + int delay_step = std::roundf(delay_time / ekf_dt_); + if (delay_step > extend_state_step_ - 1) + { + RCLCPP_WARN_THROTTLE(get_logger(), clk_, 1000, + "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = " + "extend_state_step * ekf_dt : %f [s]", + delay_time, extend_state_step_ * ekf_dt_); + return; + } + DEBUG_INFO("delay_time: %f [s]", delay_time); + + /* Set yaw */ + const double yaw_curr = ekf_.getXelement((unsigned int)(delay_step * dim_x_ + IDX::YAW)); + double yaw = tf2::getYaw(pose.pose.orientation); + const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW); + const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi + yaw = yaw_error + ekf_yaw; + + /* Set measurement matrix */ + Eigen::MatrixXd y(dim_y, 1); + y << pose.pose.position.x, pose.pose.position.y, yaw; + + if (isnan(y.array()).any() || isinf(y.array()).any()) + { + RCLCPP_WARN(get_logger(), "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); + return; + } + + /* Gate */ + Eigen::MatrixXd y_ekf(dim_y, 1); + y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::X), ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw; + Eigen::MatrixXd P_curr, P_y; + ekf_.getLatestP(P_curr); + P_y = P_curr.block(0, 0, dim_y, dim_y); + if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y)) + { + RCLCPP_WARN_THROTTLE(get_logger(), clk_ , 2000,"[EKF] Pose measurement update, mahalanobis distance is over limit. ignore " + "measurement data."); + return; + } + + DEBUG_PRINT_MAT(y.transpose()); + DEBUG_PRINT_MAT(y_ekf.transpose()); + DEBUG_PRINT_MAT((y - y_ekf).transpose()); + + /* Set measurement matrix */ + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_); + C(0, IDX::X) = 1.0; // for pos x + C(1, IDX::Y) = 1.0; // for pos y + C(2, IDX::YAW) = 1.0; // for yaw + + /* Set measurement noise covariancs */ + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); + if (use_pose_with_covariance_) + { + R(0, 0) = current_pose_covariance_.at(0); // x - x + R(0, 1) = current_pose_covariance_.at(1); // x - y + R(0, 2) = current_pose_covariance_.at(5); // x - yaw + R(1, 0) = current_pose_covariance_.at(6); // y - x + R(1, 1) = current_pose_covariance_.at(7); // y - y + R(1, 2) = current_pose_covariance_.at(11); // y - yaw + R(2, 0) = current_pose_covariance_.at(30); // yaw - x + R(2, 1) = current_pose_covariance_.at(31); // yaw - y + R(2, 2) = current_pose_covariance_.at(35); // yaw - yaw + } + else + { + const double ekf_yaw = ekf_.getXelement(IDX::YAW); + const double vx = ekf_.getXelement(IDX::VX); + const double wz = ekf_.getXelement(IDX::WZ); + const double cov_pos_x = std::pow(pose_measure_uncertainty_time_ * vx * cos(ekf_yaw), 2.0); + const double cov_pos_y = std::pow(pose_measure_uncertainty_time_ * vx * sin(ekf_yaw), 2.0); + const double cov_yaw = std::pow(pose_measure_uncertainty_time_ * wz, 2.0); + R(0, 0) = std::pow(pose_stddev_x_, 2) + cov_pos_x; // pos_x + R(1, 1) = std::pow(pose_stddev_y_, 2) + cov_pos_y; // pos_y + R(2, 2) = std::pow(pose_stddev_yaw_, 2) + cov_yaw; // yaw + } + + /* In order to avoid a large change at the time of updating, measuremeent update is performed by dividing at every + * step. */ + R *= (ekf_rate_ / pose_rate_); + + ekf_.updateWithDelay(y, C, R, delay_step); + + // debug + Eigen::MatrixXd X_result(dim_x_, 1); + ekf_.getLatestX(X_result); + DEBUG_PRINT_MAT(X_result.transpose()); + DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + } + + /* + * measurementUpdateTwist + */ + void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::msg::TwistStamped& twist) + { + if (twist.header.frame_id != child_frame_id_) + { + RCLCPP_WARN_THROTTLE(get_logger(), clk_ , 2000,"twist frame_id must be %s", child_frame_id_); + } + + Eigen::MatrixXd X_curr(dim_x_, 1); // curent state + ekf_.getLatestX(X_curr); + DEBUG_PRINT_MAT(X_curr.transpose()); + + constexpr int dim_y = 2; // vx, wz + const rclcpp::Time t_curr = this->now(); + + /* Calculate delay step */ + double delay_time = (t_curr - twist.header.stamp).seconds() + twist_additional_delay_; + if (delay_time < 0.0) + { + RCLCPP_WARN_THROTTLE(get_logger(), clk_ , 1000, "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].", + delay_time); + delay_time = 0.0; + } + int delay_step = std::roundf(delay_time / ekf_dt_); + if (delay_step > extend_state_step_ - 1) + { + RCLCPP_WARN_THROTTLE(get_logger(), clk_ , 1000, + "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = " + "extend_state_step * ekf_dt : %f [s]", + delay_time, extend_state_step_ * ekf_dt_); + return; + } + DEBUG_INFO("delay_time: %f [s]", delay_time); + + /* Set measurement matrix */ + Eigen::MatrixXd y(dim_y, 1); + y << twist.twist.linear.x, twist.twist.angular.z; + + if (isnan(y.array()).any() || isinf(y.array()).any()) + { + RCLCPP_WARN(get_logger(), "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); + return; + } + + /* Gate */ + Eigen::MatrixXd y_ekf(dim_y, 1); + y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::VX), ekf_.getXelement(delay_step * dim_x_ + IDX::WZ); + Eigen::MatrixXd P_curr, P_y; + ekf_.getLatestP(P_curr); + P_y = P_curr.block(4, 4, dim_y, dim_y); + if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y)) + { + RCLCPP_WARN_THROTTLE(get_logger(), clk_ , 2000 , "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore " + "measurement data."); + return; + } + + DEBUG_PRINT_MAT(y.transpose()); + DEBUG_PRINT_MAT(y_ekf.transpose()); + DEBUG_PRINT_MAT((y - y_ekf).transpose()); + + /* Set measurement matrix */ + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_); + C(0, IDX::VX) = 1.0; // for vx + C(1, IDX::WZ) = 1.0; // for wz + + /* Set measurement noise covariancs */ + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); + if (use_twist_with_covariance_) + { + R(0, 0) = current_twist_covariance_.at(0); // vx - vx + R(0, 1) = current_twist_covariance_.at(5); // vx - wz + R(1, 0) = current_twist_covariance_.at(30); // wz - vx + R(1, 1) = current_twist_covariance_.at(35); // wz - wz + } + else + { + R(0, 0) = twist_stddev_vx_ * twist_stddev_vx_ * ekf_dt_; // for vx + R(1, 1) = twist_stddev_wz_ * twist_stddev_wz_ * ekf_dt_; // for wz + } + + /* In order to avoid a large change by update, measurement update is performed by dividing at every step. */ + R *= (ekf_rate_ / twist_rate_); + + ekf_.updateWithDelay(y, C, R, delay_step); + + // debug + Eigen::MatrixXd X_result(dim_x_, 1); + ekf_.getLatestX(X_result); + DEBUG_PRINT_MAT(X_result.transpose()); + DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + } + + /* + * mahalanobisGate + */ + bool EKFLocalizer::mahalanobisGate(const double& dist_max, const Eigen::MatrixXd& x, const Eigen::MatrixXd& obj_x, + const Eigen::MatrixXd& cov) + { + Eigen::MatrixXd mahalanobis_squared = (x - obj_x).transpose() * cov.inverse() * (x - obj_x); + DEBUG_INFO("measurement update: mahalanobis = %f, gate limit = %f", std::sqrt(mahalanobis_squared(0)), dist_max); + if (mahalanobis_squared(0) > dist_max * dist_max) + { + return false; + } + + return true; + } + + + /* + * publishEstimateResult + */ + void EKFLocalizer::publishEstimateResult() + { + rclcpp::Time current_time = this->now(); + Eigen::MatrixXd X(dim_x_, 1); + Eigen::MatrixXd P(dim_x_, dim_x_); + ekf_.getLatestX(X); + ekf_.getLatestP(P); + + /* publish latest pose */ + pub_pose_->publish(current_ekf_pose_); + + /* publish latest pose with covariance */ + geometry_msgs::msg::PoseWithCovarianceStamped pose_cov; + pose_cov.header.stamp = current_time; + pose_cov.header.frame_id = current_ekf_pose_.header.frame_id; + pose_cov.pose.pose = current_ekf_pose_.pose; + pose_cov.pose.covariance[0] = P(IDX::X, IDX::X); + pose_cov.pose.covariance[1] = P(IDX::X, IDX::Y); + pose_cov.pose.covariance[5] = P(IDX::X, IDX::YAW); + pose_cov.pose.covariance[6] = P(IDX::Y, IDX::X); + pose_cov.pose.covariance[7] = P(IDX::Y, IDX::Y); + pose_cov.pose.covariance[11] = P(IDX::Y, IDX::YAW); + pose_cov.pose.covariance[30] = P(IDX::YAW, IDX::X); + pose_cov.pose.covariance[31] = P(IDX::YAW, IDX::Y); + pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW); + pub_pose_cov_->publish(pose_cov); + + /* publish latest twist */ + pub_twist_->publish(current_ekf_twist_); + + /* publish latest twist with covariance */ + geometry_msgs::msg::TwistWithCovarianceStamped twist_cov; + twist_cov.header.stamp = current_time; + twist_cov.header.frame_id = current_ekf_twist_.header.frame_id; + twist_cov.twist.twist = current_ekf_twist_.twist; + twist_cov.twist.covariance[0] = P(IDX::VX, IDX::VX); + twist_cov.twist.covariance[5] = P(IDX::VX, IDX::WZ); + twist_cov.twist.covariance[30] = P(IDX::WZ, IDX::VX); + twist_cov.twist.covariance[35] = P(IDX::WZ, IDX::WZ); + pub_twist_cov_->publish(twist_cov); + + + /* publish yaw bias */ + std_msgs::msg::Float64 yawb; + yawb.data = X(IDX::YAWB); + pub_yaw_bias_->publish(yawb); + + /* debug measured pose */ + if (current_pose_ptr_ != nullptr) + { + geometry_msgs::msg::PoseStamped p; + p = *current_pose_ptr_; + p.header.stamp = current_time; + pub_measured_pose_->publish(p); + } + + /* debug publish */ + double RAD2DEG = 180.0 / 3.141592; + double pose_yaw = 0.0; + if (current_pose_ptr_ != nullptr) + pose_yaw = tf2::getYaw(current_pose_ptr_->pose.orientation) * RAD2DEG; + + std_msgs::msg::Float64MultiArray msg; + msg.data.push_back(X(IDX::YAW) * RAD2DEG); // [0] ekf yaw angle + msg.data.push_back(pose_yaw); // [1] measurement yaw angle + msg.data.push_back(X(IDX::YAWB) * RAD2DEG); // [2] yaw bias + pub_debug_->publish(msg); + } + + double EKFLocalizer::normalizeYaw(const double& yaw) + { + return std::atan2(std::sin(yaw), std::cos(yaw)); + } + +} //namespace ekf_localizer + + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(ekf_localizer::EKFLocalizer) \ No newline at end of file diff --git a/core_perception/ekf_localizer/src/ekf_localizer_node.cpp b/core_perception/ekf_localizer/src/ekf_localizer_node.cpp index 962a232cc62..f1fb2534897 100644 --- a/core_perception/ekf_localizer/src/ekf_localizer_node.cpp +++ b/core_perception/ekf_localizer/src/ekf_localizer_node.cpp @@ -1,27 +1,32 @@ /* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * Copyright (C) 2023 LEIDOS. * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at * - * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. */ -#include "ekf_localizer/ekf_localizer.h" +#include +#include "ekf_localizer/ekf_localizer.hpp" -int main(int argc, char** argv) +int main(int argc, char **argv) { - ros::init(argc, argv, "ekf_localizer"); - EKFLocalizer obj; + rclcpp::init(argc, argv); + + auto node = std::make_shared(rclcpp::NodeOptions()); - ros::spin(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); return 0; -}; +} \ No newline at end of file diff --git a/core_perception/ekf_localizer/test/src/test_ekf_localizer.cpp b/core_perception/ekf_localizer/test/src/test_ekf_localizer.cpp deleted file mode 100644 index 499d54f56ea..00000000000 --- a/core_perception/ekf_localizer/test/src/test_ekf_localizer.cpp +++ /dev/null @@ -1,297 +0,0 @@ -/* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include - -#include -#include -#include -#include -#include - -#include "ekf_localizer/ekf_localizer.h" - -class EKFLocalizerTestSuite : public ::testing::Test -{ -public: - EKFLocalizerTestSuite() : nh_(""), pnh_("~") - { - sub_twist = nh_.subscribe("/ekf_twist", 1, &EKFLocalizerTestSuite::callbackTwist, this); - sub_pose = nh_.subscribe("/ekf_pose", 1, &EKFLocalizerTestSuite::callbackPose, this); - tiemr_ = nh_.createTimer(ros::Duration(0.1), &EKFLocalizerTestSuite::timerCallback, this); - } - ~EKFLocalizerTestSuite() - { - } - - ros::NodeHandle nh_, pnh_; - - std::string frame_id_a_ = "world"; - std::string frame_id_b_ = "base_link"; - - ros::Subscriber sub_twist; - ros::Subscriber sub_pose; - - ros::Timer tiemr_; - - std::shared_ptr current_pose_ptr_; - std::shared_ptr current_twist_ptr_; - - void timerCallback(const ros::TimerEvent& e) - { - /* !!! this should be defined before sendTransform() !!! */ - static tf2_ros::TransformBroadcaster br; - geometry_msgs::TransformStamped sended; - - ros::Time current_time = ros::Time::now(); - - sended.header.stamp = current_time; - sended.header.frame_id = frame_id_a_; - sended.child_frame_id = frame_id_b_; - sended.transform.translation.x = -7.11; - sended.transform.translation.y = 0.0; - sended.transform.translation.z = 0.0; - tf2::Quaternion q; - q.setRPY(0, 0, 0.5); - sended.transform.rotation.x = q.x(); - sended.transform.rotation.y = q.y(); - sended.transform.rotation.z = q.z(); - sended.transform.rotation.w = q.w(); - - br.sendTransform(sended); - }; - - void callbackPose(const geometry_msgs::PoseStamped::ConstPtr& pose) - { - current_pose_ptr_ = std::make_shared(*pose); - } - - void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr& twist) - { - current_twist_ptr_ = std::make_shared(*twist); - } - - void resetCurrentPoseAndTwist() - { - current_pose_ptr_ = nullptr; - current_twist_ptr_ = nullptr; - } -}; - -TEST_F(EKFLocalizerTestSuite, measurementUpdatePose) -{ - EKFLocalizer ekf_; - - ros::Publisher pub_pose = nh_.advertise("/in_pose", 1); - geometry_msgs::PoseStamped in_pose; - in_pose.header.frame_id = "world"; - in_pose.pose.position.x = 1.0; - in_pose.pose.position.y = 2.0; - in_pose.pose.position.z = 3.0; - in_pose.pose.orientation.x = 0.0; - in_pose.pose.orientation.y = 0.0; - in_pose.pose.orientation.z = 0.0; - in_pose.pose.orientation.w = 1.0; - - /* test for valid value */ - const double pos_x = 12.3; - in_pose.pose.position.x = pos_x; // for vaild value - - for (int i = 0; i < 20; ++i) - { - in_pose.header.stamp = ros::Time::now(); - pub_pose.publish(in_pose); - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - - ASSERT_FALSE(current_pose_ptr_ == nullptr); - ASSERT_FALSE(current_twist_ptr_ == nullptr); - - double ekf_x = current_pose_ptr_->pose.position.x; - bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) << "ekf pos x: " << ekf_x << " should be close to " << pos_x; - - /* test for invalid value */ - in_pose.pose.position.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) - { - in_pose.header.stamp = ros::Time::now(); - pub_pose.publish(in_pose); - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdateTwist) -{ - EKFLocalizer ekf_; - - ros::Publisher pub_twist = nh_.advertise("/in_twist", 1); - geometry_msgs::TwistStamped in_twist; - in_twist.header.frame_id = "base_link"; - - /* test for valid value */ - const double vx = 12.3; - in_twist.twist.linear.x = vx; // for vaild value - for (int i = 0; i < 20; ++i) - { - in_twist.header.stamp = ros::Time::now(); - pub_twist.publish(in_twist); - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - - ASSERT_FALSE(current_pose_ptr_ == nullptr); - ASSERT_FALSE(current_twist_ptr_ == nullptr); - - double ekf_vx = current_twist_ptr_->twist.linear.x; - bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE(std::fabs(ekf_vx - vx) < 0.1) << "ekf vel x: " << ekf_vx << ", should be close to " << vx; - - /* test for invalid value */ - in_twist.twist.linear.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) - { - in_twist.header.stamp = ros::Time::now(); - pub_twist.publish(in_twist); - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - - ekf_vx = current_twist_ptr_->twist.linear.x; - is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdatePoseWithCovariance) -{ - - pnh_.setParam("use_pose_with_covariance", true); - ros::Duration(0.2).sleep(); - EKFLocalizer ekf_; - - ros::Publisher pub_pose = nh_.advertise("/in_pose_with_covariance", 1); - geometry_msgs::PoseWithCovarianceStamped in_pose; - in_pose.header.frame_id = "world"; - in_pose.pose.pose.position.x = 1.0; - in_pose.pose.pose.position.y = 2.0; - in_pose.pose.pose.position.z = 3.0; - in_pose.pose.pose.orientation.x = 0.0; - in_pose.pose.pose.orientation.y = 0.0; - in_pose.pose.pose.orientation.z = 0.0; - in_pose.pose.pose.orientation.w = 1.0; - for (int i = 0; i < 36; ++i) - { - in_pose.pose.covariance[i] = 0.1; - } - - /* test for valid value */ - const double pos_x = 99.3; - in_pose.pose.pose.position.x = pos_x; // for vaild value - - for (int i = 0; i < 20; ++i) - { - in_pose.header.stamp = ros::Time::now(); - pub_pose.publish(in_pose); - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - - ASSERT_FALSE(current_pose_ptr_ == nullptr); - ASSERT_FALSE(current_twist_ptr_ == nullptr); - - double ekf_x = current_pose_ptr_->pose.position.x; - bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) << "ekf pos x: " << ekf_x << " should be close to " << pos_x; - - /* test for invalid value */ - in_pose.pose.pose.position.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) - { - in_pose.header.stamp = ros::Time::now(); - pub_pose.publish(in_pose); - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdateTwistWithCovariance) -{ - EKFLocalizer ekf_; - - ros::Publisher pub_twist = nh_.advertise("/in_twist_with_covariance", 1); - geometry_msgs::TwistWithCovarianceStamped in_twist; - in_twist.header.frame_id = "base_link"; - - /* test for valid value */ - const double vx = 12.3; - in_twist.twist.twist.linear.x = vx; // for vaild value - for (int i = 0; i < 36; ++i) - { - in_twist.twist.covariance[i] = 0.1; - } - for (int i = 0; i < 10; ++i) - { - in_twist.header.stamp = ros::Time::now(); - pub_twist.publish(in_twist); - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - - ASSERT_FALSE(current_pose_ptr_ == nullptr); - ASSERT_FALSE(current_twist_ptr_ == nullptr); - - double ekf_vx = current_twist_ptr_->twist.linear.x; - bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE((ekf_vx - vx) < 0.1) << "vel x should be close to " << vx; - - /* test for invalid value */ - in_twist.twist.twist.linear.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) - { - in_twist.header.stamp = ros::Time::now(); - pub_twist.publish(in_twist); - ros::spinOnce(); - ros::Duration(0.1).sleep(); - } - - ekf_vx = current_twist_ptr_->twist.linear.x; - is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "EKFLocalizerTestSuite"); - - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/core_perception/ekf_localizer/test/test_ekf_localizer.cpp b/core_perception/ekf_localizer/test/test_ekf_localizer.cpp new file mode 100644 index 00000000000..fa592b0ec5b --- /dev/null +++ b/core_perception/ekf_localizer/test/test_ekf_localizer.cpp @@ -0,0 +1,373 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include + +#include +#include +#include +#include +#include +#include "ekf_localizer/ekf_localizer.hpp" + + +class EKFLocalizerTestSuite : public rclcpp::Node +{ + private: + rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_pose_; + rclcpp::TimerBase::SharedPtr timer_; + + public: + + EKFLocalizerTestSuite(std::string node_name) : Node (node_name) + { + sub_twist_ = create_subscription("ekf_twist", 1, std::bind(&EKFLocalizerTestSuite::callbackTwist, this, std::placeholders::_1)); + sub_pose_ = create_subscription("ekf_pose", 1, std::bind(&EKFLocalizerTestSuite::callbackPose, this, std::placeholders::_1)); + timer_ = create_wall_timer(std::chrono::milliseconds(100), std::bind(&EKFLocalizerTestSuite::timerCallback, this)); + } + + void timerCallback() + { + /* !!! this should be defined before sendTransform() !!! */ + static tf2_ros::TransformBroadcaster br(shared_from_this()); + geometry_msgs::msg::TransformStamped sended; + + rclcpp::Time current_time = rclcpp::Time(0,0); + + sended.header.stamp = current_time; + sended.header.frame_id = frame_id_a_; + sended.child_frame_id = frame_id_b_; + sended.transform.translation.x = -7.11; + sended.transform.translation.y = 0.0; + sended.transform.translation.z = 0.0; + tf2::Quaternion q; + q.setRPY(0, 0, 0.5); + sended.transform.rotation.x = q.x(); + sended.transform.rotation.y = q.y(); + sended.transform.rotation.z = q.z(); + sended.transform.rotation.w = q.w(); + + br.sendTransform(sended); + } + + void callbackPose(const geometry_msgs::msg::PoseStamped::SharedPtr pose) + { + current_pose_ptr_ = std::make_shared(*pose); + } + + void callbackTwist(const geometry_msgs::msg::TwistStamped::SharedPtr twist) + { + current_twist_ptr_ = std::make_shared(*twist); + } + + void resetCurrentPoseAndTwist() + { + current_pose_ptr_ = nullptr; + current_twist_ptr_ = nullptr; + } + + std::string frame_id_a_ = "world"; + std::string frame_id_b_ = "base_link"; + std::shared_ptr current_pose_ptr_; + std::shared_ptr current_twist_ptr_; + +}; + +/* Class implementation for template message publisher taken from, + https://github.com/cmrobotics/cmr_tests_utils/blob/humble-devel/include/cmr_tests_utils/basic_publisher_node_test.hpp +*/ +template +class TestPublisher: public rclcpp::Node +{ + private: + // Template publisher for testing + typename rclcpp::Publisher::SharedPtr topic_pub_; + mutable std::mutex msg_mutex_; + + public: + + TestPublisher(std::string node_name, std::string topic_name, rclcpp::QoS qos = rclcpp::SystemDefaultsQoS()) : Node(node_name) + { + topic_pub_ = create_publisher(topic_name, qos); + + } + + void publish(const MessageT& msg){ + std::lock_guard lock(msg_mutex_); + topic_pub_->publish(msg); + } +}; + + +TEST(EKFLocalizerTestSuite, measurementUpdatePose) +{ + auto test_node = std::make_shared("pose_test_node"); + auto worker_node = std::make_shared(rclcpp::NodeOptions()); + worker_node->configure(); //Call configure state transition + rclcpp::sleep_for(std::chrono::milliseconds(100)); + worker_node->activate(); //Call activate state transition to get not read for runtime + // Create publisher node + auto pub_pose = std::make_shared>("pose_publisher_node", "in_pose"); + + geometry_msgs::msg::PoseStamped in_pose; + in_pose.header.frame_id = "world"; + in_pose.pose.position.x = 1.0; + in_pose.pose.position.y = 2.0; + in_pose.pose.position.z = 3.0; + in_pose.pose.orientation.x = 0.0; + in_pose.pose.orientation.y = 0.0; + in_pose.pose.orientation.z = 0.0; + in_pose.pose.orientation.w = 1.0; + + /* test for valid value */ + const double pos_x = 12.3; + in_pose.pose.position.x = pos_x; // for valid value + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(test_node->get_node_base_interface()); + executor.add_node(pub_pose->get_node_base_interface()); + executor.add_node(worker_node->get_node_base_interface()); + + + for (int i = 0; i < 20; ++i) + { + in_pose.header.stamp = rclcpp::Time(0,0); + pub_pose->publish(in_pose); + executor.spin_once(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + ASSERT_FALSE(test_node->current_pose_ptr_ == nullptr); + ASSERT_FALSE(test_node->current_twist_ptr_ == nullptr); + + + double ekf_x = test_node->current_pose_ptr_->pose.position.x; + bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) << "ekf pos x: " << ekf_x << " should be close to " << pos_x; + + /* test for invalid value */ + in_pose.pose.position.x = NAN; // check for invalid values + for (int i = 0; i < 10; ++i) + { + in_pose.header.stamp = rclcpp::Time(0,0); + pub_pose->publish(in_pose); + executor.spin_once(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + + test_node->resetCurrentPoseAndTwist(); +} + +TEST(EKFLocalizerTestSuite, measurementUpdateTwist) +{ + auto test_node = std::make_shared("twist_test_node"); + auto worker_node = std::make_shared(rclcpp::NodeOptions()); + worker_node->configure(); //Call configure state transition + rclcpp::sleep_for(std::chrono::milliseconds(100)); + worker_node->activate(); //Call activate state transition to get not read for runtime + // Create publisher node + auto pub_twist = std::make_shared>("twist_publisher_node", "in_twist"); + + geometry_msgs::msg::TwistStamped in_twist; + in_twist.header.frame_id = "base_link"; + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(test_node->get_node_base_interface()); + executor.add_node(pub_twist->get_node_base_interface()); + executor.add_node(worker_node->get_node_base_interface()); + + /* test for valid value */ + const double vx = 12.3; + in_twist.twist.linear.x = vx; // for vaild value + for (int i = 0; i < 20; ++i) + { + in_twist.header.stamp = rclcpp::Time(0,0); + pub_twist->publish(in_twist); + executor.spin_once(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + ASSERT_FALSE(test_node->current_pose_ptr_ == nullptr); + ASSERT_FALSE(test_node->current_twist_ptr_ == nullptr); + + double ekf_vx = test_node->current_twist_ptr_->twist.linear.x; + bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + ASSERT_TRUE(std::fabs(ekf_vx - vx) < 0.1) << "ekf vel x: " << ekf_vx << ", should be close to " << vx; + + /* test for invalid value */ + in_twist.twist.linear.x = NAN; // check for invalid values + + for (int i = 0; i < 10; ++i) + { + in_twist.header.stamp = rclcpp::Time(0,0); + pub_twist->publish(in_twist); + executor.spin_once(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + ekf_vx = test_node->current_twist_ptr_->twist.linear.x; + is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + + test_node->resetCurrentPoseAndTwist(); +} + +TEST(EKFLocalizerTestSuite, measurementUpdatePoseWithCovariance) +{ + std::vector initial_parameters = {rclcpp::Parameter("use_pose_with_covariance", true)}; + rclcpp::NodeOptions node_options; + node_options.parameter_overrides(initial_parameters); + + auto test_node = std::make_shared("pose_with_cov_test_node"); + auto worker_node = std::make_shared(node_options); + worker_node->configure(); //Call configure state transition + rclcpp::sleep_for(std::chrono::milliseconds(100)); + worker_node->activate(); //Call activate state transition to get not read for runtime + // Create publisher node + auto pub_pose_cov = std::make_shared>("pose_with_covariance_pub_node", "in_pose_with_covariance"); + geometry_msgs::msg::PoseWithCovarianceStamped in_pose; + in_pose.header.frame_id = "world"; + in_pose.pose.pose.position.x = 1.0; + in_pose.pose.pose.position.y = 2.0; + in_pose.pose.pose.position.z = 3.0; + in_pose.pose.pose.orientation.x = 0.0; + in_pose.pose.pose.orientation.y = 0.0; + in_pose.pose.pose.orientation.z = 0.0; + in_pose.pose.pose.orientation.w = 1.0; + for (int i = 0; i < 36; ++i) + { + in_pose.pose.covariance[i] = 0.1; + } + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(test_node->get_node_base_interface()); + executor.add_node(pub_pose_cov->get_node_base_interface()); + executor.add_node(worker_node->get_node_base_interface()); + + /* test for valid value */ + const double pos_x = 99.3; + in_pose.pose.pose.position.x = pos_x; // for vaild value + + for (int i = 0; i < 20; ++i) + { + in_pose.header.stamp = rclcpp::Time(0,0); + pub_pose_cov->publish(in_pose); + executor.spin_once(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + ASSERT_FALSE(test_node->current_pose_ptr_ == nullptr); + ASSERT_FALSE(test_node->current_twist_ptr_ == nullptr); + + + double ekf_x = test_node->current_pose_ptr_->pose.position.x; + bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) << "ekf pos x: " << ekf_x << " should be close to " << pos_x; + + + /* test for invalid value */ + in_pose.pose.pose.position.x = NAN; // check for invalid values + for (int i = 0; i < 10; ++i) + { + in_pose.header.stamp = rclcpp::Time(0,0); + pub_pose_cov->publish(in_pose); + executor.spin_once(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + + test_node->resetCurrentPoseAndTwist(); + +} + +TEST(EKFLocalizerTestSuite, measurementUpdateTwistWithCovariance) +{ + auto test_node = std::make_shared("twist_with_cov_node"); + auto worker_node = std::make_shared(rclcpp::NodeOptions()); + worker_node->configure(); //Call configure state transition + rclcpp::sleep_for(std::chrono::milliseconds(100)); + worker_node->activate(); //Call activate state transition to get not read for runtime + // Create publisher node + auto pub_twist_with_cov = std::make_shared>("twist_with_cov_publisher_node", "in_twist_with_covariance"); + + geometry_msgs::msg::TwistWithCovarianceStamped in_twist; + in_twist.header.frame_id = "base_link"; + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(test_node->get_node_base_interface()); + executor.add_node(pub_twist_with_cov->get_node_base_interface()); + executor.add_node(worker_node->get_node_base_interface()); + + /* test for valid value */ + const double vx = 12.3; + in_twist.twist.twist.linear.x = vx; // for vaild value + for (int i = 0; i < 36; ++i) + { + in_twist.twist.covariance[i] = 0.1; + } + + for (int i = 0; i < 10; ++i) + { + in_twist.header.stamp = rclcpp::Time(0,0); + pub_twist_with_cov->publish(in_twist); + executor.spin_once(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + ASSERT_FALSE(test_node->current_pose_ptr_ == nullptr); + ASSERT_FALSE(test_node->current_twist_ptr_ == nullptr); + + double ekf_vx = test_node->current_twist_ptr_->twist.linear.x; + bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; + ASSERT_TRUE((ekf_vx - vx) < 0.1) << "vel x should be close to " << vx; + + /* test for invalid value */ + in_twist.twist.twist.linear.x = NAN; // check for invalid values + for (int i = 0; i < 10; ++i) + { + in_twist.header.stamp = rclcpp::Time(0,0); + pub_twist_with_cov->publish(in_twist); + executor.spin_once(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + ekf_vx = test_node->current_twist_ptr_->twist.linear.x; + is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); + ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + //Initialize ROS + rclcpp::init(argc, argv); + + bool success = RUN_ALL_TESTS(); + + //shutdown ROS + rclcpp::shutdown(); + + return success; +} \ No newline at end of file diff --git a/core_perception/ekf_localizer/test/test_ekf_localizer.test b/core_perception/ekf_localizer/test/test_ekf_localizer.test deleted file mode 100644 index 1878685aff7..00000000000 --- a/core_perception/ekf_localizer/test/test_ekf_localizer.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/core_perception/lidar_localizer_ros2/CMakeLists.txt b/core_perception/lidar_localizer_ros2/CMakeLists.txt new file mode 100644 index 00000000000..a9f0636b982 --- /dev/null +++ b/core_perception/lidar_localizer_ros2/CMakeLists.txt @@ -0,0 +1,75 @@ +# Copyright (C) 2023 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.5) +project(lidar_localizer_ros2) + +# Declare carma package and check ROS version +find_package(carma_cmake_common REQUIRED) +carma_check_ros_version(2) +carma_package() + +find_package(autoware_build_flags REQUIRED) +find_package(PCL COMPONENTS REQUIRED) +find_package(CUDA) +find_package(Eigen3 QUIET) + +AW_CHECK_CUDA() + +if(USE_CUDA) + add_definitions(-DCUDA_FOUND) + list(APPEND PCL_OPENMP_PACKAGES ndt_gpu_ros2) +endif() + +if(NOT EIGEN3_FOUND) + # Fallback to cmake_modules + find_package(cmake_modules REQUIRED) + find_package(Eigen REQUIRED) + set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) + set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only + # Possibly map additional variables to the EIGEN3_ prefix. +else() + set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) +endif() + + +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + + + +# Includes +include_directories( + ${PCL_INCLUDE_DIRS} + ${CUDA_INCLUDE_DIRS} + include + ${EIGEN3_INCLUDE_DIRS} +) + +ament_auto_add_library(ndt_matching_lib SHARED + src/ndt_matching.cpp) + +rclcpp_components_register_nodes(ndt_matching_lib "ndt_matching::NDTMatching") + +target_link_libraries(ndt_matching_lib +${PCL_LIBRARIES} +) + + + + +ament_auto_package( + INSTALL_TO_SHARE launch +) \ No newline at end of file diff --git a/core_perception/lidar_localizer_ros2/include/ndt_matching.hpp b/core_perception/lidar_localizer_ros2/include/ndt_matching.hpp new file mode 100644 index 00000000000..f4fcb4f7771 --- /dev/null +++ b/core_perception/lidar_localizer_ros2/include/ndt_matching.hpp @@ -0,0 +1,280 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "tf2_ros/transform_broadcaster.h" +#include +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#ifdef CUDA_FOUND +#include +#endif +#include +#include + +#include + +#include +#include + +#define PREDICT_POSE_THRESHOLD 0.5 + +#define Wa 0.4 +#define Wb 0.3 +#define Wc 0.3 + +namespace ndt_matching{ +struct pose +{ + double x; + double y; + double z; + double roll; + double pitch; + double yaw; +}; + +enum class MethodType +{ + PCL_GENERIC = 0, + PCL_ANH = 1, + PCL_ANH_GPU = 2, + PCL_OPENMP = 3, +}; + +class NDTMatching : public carma_ros2_utils::CarmaLifecycleNode { + + private: + + // Define publishers + rclcpp::Publisher::SharedPtr predict_pose_pub; + rclcpp::Publisher::SharedPtr predict_pose_imu_pub; + rclcpp::Publisher::SharedPtr predict_pose_odom_pub; + rclcpp::Publisher::SharedPtr predict_pose_imu_odom_pub; + rclcpp::Publisher::SharedPtr ndt_pose_pub; + + // current_pose is published by vel_pose_mux + // static ros::Publisher current_pose_pub; + // static geometry_msgs::PoseStamped current_pose_msg; + + + rclcpp::Publisher::SharedPtr localizer_pose_pub; + rclcpp::Publisher::SharedPtr estimate_twist_pub; + rclcpp::Publisher::SharedPtr estimated_vel_mps_pub, estimated_vel_kmph_pub; + rclcpp::Publisher::SharedPtr estimated_vel_pub; + rclcpp::Publisher::SharedPtr time_ndt_matching_pub; + rclcpp::Publisher::SharedPtr ndt_stat_pub; + rclcpp::Publisher::SharedPtr ndt_reliability_pub; + + // Define subscribers + rclcpp::Subscription::SharedPtr param_sub; + rclcpp::Subscription::SharedPtr gnss_sub; + rclcpp::Subscription::SharedPtr initialpose_sub; + rclcpp::Subscription::SharedPtr points_sub; + rclcpp::Subscription::SharedPtr odom_sub; + rclcpp::Subscription::SharedPtr imu_sub; + rclcpp::Subscription::SharedPtr map_sub; + + int method_type_tmp = 0; + + std::shared_ptr buffer_ptr_ = nullptr; + std::shared_ptr listener_ptr_ = nullptr; + + MethodType _method_type = MethodType::PCL_GENERIC; + + pose initial_pose, predict_pose, predict_pose_imu, predict_pose_odom, predict_pose_imu_odom, previous_pose, + ndt_pose, current_pose, current_pose_imu, current_pose_odom, current_pose_imu_odom, localizer_pose; + + double offset_x, offset_y, offset_z, offset_yaw; // current_pos - previous_pose + double offset_imu_x, offset_imu_y, offset_imu_z, offset_imu_roll, offset_imu_pitch, offset_imu_yaw; + double offset_odom_x, offset_odom_y, offset_odom_z, offset_odom_roll, offset_odom_pitch, offset_odom_yaw; + double offset_imu_odom_x, offset_imu_odom_y, offset_imu_odom_z, offset_imu_odom_roll, offset_imu_odom_pitch, + offset_imu_odom_yaw; + + // Can't load if typed "pcl::PointCloud map, add;" + pcl::PointCloud map, add; + + // If the map is loaded, map_loaded will be 1. + int map_loaded = 0; + int _use_gnss = 1; + int init_pos_set = 0; + + pcl::NormalDistributionsTransform ndt; + cpu::NormalDistributionsTransform anh_ndt; + #ifdef CUDA_FOUND + std::shared_ptr anh_gpu_ndt_ptr = + std::make_shared(); + #endif + #ifdef USE_PCL_OPENMP + pcl_omp::NormalDistributionsTransform omp_ndt; + #endif + + // Default values + int max_iter = 30; // Maximum iterations + float ndt_res = 1.0; // Resolution + double step_size = 0.1; // Step size + double trans_eps = 0.001; // Transformation epsilon. In PCLv1.10 (ros noetic) this value is squared error not base epsilon + // NOTE: A value of 0.0001 can work as well. + // This will increase the required iteration count (and therefore execution time) but might increase accuracy. + + geometry_msgs::msg::PoseStamped predict_pose_msg; + geometry_msgs::msg::PoseStamped predict_pose_imu_msg; + geometry_msgs::msg::PoseStamped predict_pose_odom_msg; + geometry_msgs::msg::PoseStamped predict_pose_imu_odom_msg; + geometry_msgs::msg::PoseStamped ndt_pose_msg; + + geometry_msgs::msg::PoseStamped localizer_pose_msg; + geometry_msgs::msg::TwistStamped estimate_twist_msg; + + + double exe_time = 0.0; + bool has_converged; + int iteration = 0; + double fitness_score = 0.0; + double trans_probability = 0.0; + + // reference for comparing fitness_score, default value set to 500.0 + double _gnss_reinit_fitness = 500.0; + + double diff = 0.0; + double diff_x = 0.0, diff_y = 0.0, diff_z = 0.0, diff_yaw; + + double current_velocity = 0.0, previous_velocity = 0.0, previous_previous_velocity = 0.0; // [m/s] + double current_velocity_x = 0.0, previous_velocity_x = 0.0; + double current_velocity_y = 0.0, previous_velocity_y = 0.0; + double current_velocity_z = 0.0, previous_velocity_z = 0.0; + // double current_velocity_yaw = 0.0, previous_velocity_yaw = 0.0; + double current_velocity_smooth = 0.0; + + double current_velocity_imu_x = 0.0; + double current_velocity_imu_y = 0.0; + double current_velocity_imu_z = 0.0; + + double current_accel = 0.0, previous_accel = 0.0; // [m/s^2] + double current_accel_x = 0.0; + double current_accel_y = 0.0; + double current_accel_z = 0.0; + // double current_accel_yaw = 0.0; + + double angular_velocity = 0.0; + + int use_predict_pose = 0; + + std_msgs::msg::Float32 estimated_vel_mps, estimated_vel_kmph, previous_estimated_vel_kmph; + + std::chrono::time_point matching_start, matching_end; + + std_msgs::msg::Float32 time_ndt_matching; + + int _queue_size = 1; + + autoware_msgs::msg::NDTStat ndt_stat_msg; + + double predict_pose_error = 0.0; + + double _tf_x, _tf_y, _tf_z, _tf_roll, _tf_pitch, _tf_yaw; + Eigen::Matrix4f tf_btol; + + std::string _localizer = "velodyne"; + std::string _offset = "linear"; // linear, zero, quadratic + + std_msgs::msg::Float32 ndt_reliability; + + bool _get_height = false; + bool _use_local_transform = false; + bool _use_imu = false; + bool _use_odom = false; + bool _imu_upside_down = false; + bool _output_log_data = false; + + std::string _imu_topic = "/imu_raw"; + + std::ofstream ofs; + std::string filename; + + sensor_msgs::msg::Imu imu; + nav_msgs::msg::Odometry odom; + + // tf::TransformListener local_transform_listener; + tf2::Stamped local_transform; + + std::string _base_frame = "base_link"; + std::string _map_frame = "map"; + + unsigned int points_map_num = 0; + + pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; + + void param_callback(const autoware_config_msgs::msg::ConfigNDT::SharedPtr input); + void map_callback(const sensor_msgs::msg::PointCloud2::SharedPtr input); + void gnss_callback(const geometry_msgs::msg::PoseStamped::SharedPtr input); + void initialpose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr input); + void points_callback(const sensor_msgs::msg::PointCloud2::SharedPtr input); + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr input); + + void imu_odom_calc(rclcpp::Time current_time); + void odom_calc(rclcpp::Time current_time); + void imu_calc(rclcpp::Time current_time); + + double wrapToPm(double a_num, const double a_max); + double wrapToPmPi(const double a_angle_rad); + double calcDiffForRadian(const double lhs_rad, const double rhs_rad); + + void imuUpsideDown(const sensor_msgs::msg::Imu::SharedPtr input); + void imu_callback(const sensor_msgs::msg::Imu::SharedPtr input); + pose convertPoseIntoRelativeCoordinate(const pose &target_pose, const pose &reference_pose); + + + // Note: the function below and its definitions were taken from pcl_ros package, to support local use while the package is being ported to ros2 + // https://github.com/ros-perception/perception_pcl.git + template + void transformPointCloud(const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, const tf2::Transform & transform); + + public: + explicit NDTMatching(const rclcpp::NodeOptions &); + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &prev_state); + +}; +} + diff --git a/core_perception/lidar_localizer_ros2/launch/ndt_matching.launch.py b/core_perception/lidar_localizer_ros2/launch/ndt_matching.launch.py new file mode 100644 index 00000000000..ed0194879b9 --- /dev/null +++ b/core_perception/lidar_localizer_ros2/launch/ndt_matching.launch.py @@ -0,0 +1,112 @@ +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument + +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + +def generate_launch_description(): + + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument(name = 'log_level', default_value='WARN') + + method_type = LaunchConfiguration('method_type') + declare_method_type = DeclareLaunchArgument(name='method_type', default_value='0') + + use_gnss = LaunchConfiguration('use_gnss') + declare_use_gnss = DeclareLaunchArgument(name='use_gnss', default_value='1') + + use_odom = LaunchConfiguration('use_odom') + declare_use_odom = DeclareLaunchArgument(name='use_odom', default_value="false") + + use_imu = LaunchConfiguration('use_imu') + declare_use_imu = DeclareLaunchArgument(name='use_imu', default_value="false") + + imu_upside_down = LaunchConfiguration('imu_upside_down') + declare_imu_upside_down = DeclareLaunchArgument(name='imu_upside_down', default_value='false') + + imu_topic = LaunchConfiguration('imu_topic') + declare_imu_topic = DeclareLaunchArgument(name='imu_topic', default_value="/imu_raw") + + queue_size = LaunchConfiguration('queue_size') + declare_queue_size = DeclareLaunchArgument(name='queue_size', default_value='1') + + offset = LaunchConfiguration('offset') + declare_offset = DeclareLaunchArgument(name='offset', default_value="linear") + + get_height = LaunchConfiguration('get_height') + declare_get_height = DeclareLaunchArgument(name='get_height', default_value="false") + + use_local_transform = LaunchConfiguration('use_local_transform') + declare_use_local_transform = DeclareLaunchArgument(name='use_local_transform', default_value="false") + + sync = LaunchConfiguration('sync') + declare_sync = DeclareLaunchArgument(name='sync', default_value="false") + + output_log_data = LaunchConfiguration('output_log_data') + declare_output_log_data = DeclareLaunchArgument(name='output_log_data', default_value="false") + + gnss_reinit_fitness = LaunchConfiguration('gnss_reinit_fitness') + declare_gnss_reinit_fitness = DeclareLaunchArgument(name='gnss_reinit_fitness', default_value="500.0") + + base_frame = LaunchConfiguration('base_frame') + declare_base_frame = DeclareLaunchArgument(name='base_frame', default_value="base_link") + + + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='ndt_matching_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + ComposableNode( + package='lidar_localizer_ros2', + plugin='ndt_matching::NDTMatching', + name='ndt_matching', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'log_level':log_level} + ], + parameters=[ + {'method_type': method_type}, + {'use_gnss': use_gnss}, + {'use_odom': use_odom}, + {'use_imu': use_imu}, + {'imu_upside_down': imu_upside_down}, + {'imu_topic': imu_topic}, + {'queue_size' : queue_size}, + {'offset': offset}, + {'get_height': get_height}, + {'use_local_transform': use_local_transform}, + {'sync': sync}, + {'output_log_data': output_log_data}, + {'gnss_reinit_fitness': gnss_reinit_fitness}, + {'base_frame': base_frame} + ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + declare_method_type, + declare_use_gnss, + declare_use_odom, + declare_use_imu, + declare_imu_upside_down, + declare_imu_topic, + declare_queue_size, + declare_offset, + declare_get_height, + declare_use_local_transform, + declare_sync, + declare_output_log_data, + declare_gnss_reinit_fitness, + declare_base_frame, + container + ]) \ No newline at end of file diff --git a/core_perception/lidar_localizer_ros2/package.xml b/core_perception/lidar_localizer_ros2/package.xml new file mode 100644 index 00000000000..d4953b1d49e --- /dev/null +++ b/core_perception/lidar_localizer_ros2/package.xml @@ -0,0 +1,56 @@ + + + + + + lidar_localizer_ros2 + 4.0.0 + The map_file package + + carma + + Apache 2.0 + + ament_cmake + autoware_build_flags + carma_cmake_common + ament_auto_cmake + + carma_ros2_utils + autoware_config_msgs + autoware_msgs + libpcl-all-dev + message_filters + nav_msgs + ndt_cpu + ndt_gpu + pcl_conversions + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_ros + pcl + velodyne_pointcloud + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros + + + ament_cmake + + diff --git a/core_perception/lidar_localizer_ros2/src/ndt_matching.cpp b/core_perception/lidar_localizer_ros2/src/ndt_matching.cpp new file mode 100644 index 00000000000..5312bba2443 --- /dev/null +++ b/core_perception/lidar_localizer_ros2/src/ndt_matching.cpp @@ -0,0 +1,1535 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include + +namespace ndt_matching{ + + +NDTMatching::NDTMatching(const rclcpp::NodeOptions &options) : carma_ros2_utils::CarmaLifecycleNode(options){ + //Initialize parameters + _output_log_data = declare_parameter("output_log_data", _output_log_data); + method_type_tmp = declare_parameter("method_type", method_type_tmp); + + _use_gnss = declare_parameter("use_gnss", _use_gnss); + _queue_size = declare_parameter("queue_size", _queue_size); + _offset = declare_parameter("offset", _offset); + _get_height = declare_parameter("get_height", _get_height); + _use_local_transform = declare_parameter("use_local_transform", _use_local_transform); + _use_imu = declare_parameter("use_imu", _use_imu); + _use_odom = declare_parameter("use_odom", _use_odom); + _imu_upside_down = declare_parameter("imu_upside_down", _imu_upside_down); + _imu_topic = declare_parameter("imu_topic", _imu_topic); + + _gnss_reinit_fitness = declare_parameter("gnss_reinit_fitness", _gnss_reinit_fitness); + _base_frame = declare_parameter("base_frame", _base_frame); + + _localizer = declare_parameter("localizer", _localizer); + _tf_x = declare_parameter("tf_x", _tf_x); + _tf_y = declare_parameter("tf_y", _tf_y); + _tf_z = declare_parameter("tf_z", _tf_z); + _tf_roll = declare_parameter("tf_roll", _tf_roll); + _tf_pitch = declare_parameter("tf_pitch", _tf_pitch); + _tf_yaw = declare_parameter("tf_yaw", _tf_yaw); +} + + +carma_ros2_utils::CallbackReturn NDTMatching::handle_on_configure(const rclcpp_lifecycle::State &prev_state) +{ + // Get parameters + get_parameter("output_log_data", _output_log_data); + get_parameter("method_type", method_type_tmp); + _method_type = static_cast(method_type_tmp); + + get_parameter("use_gnss", _use_gnss); + get_parameter("queue_size", _queue_size); + get_parameter("offset", _offset); + get_parameter("get_height", _get_height); + get_parameter("use_local_transform", _use_local_transform); + get_parameter("use_imu", _use_imu); + get_parameter("use_odom", _use_odom); + get_parameter("imu_upside_down", _imu_upside_down); + get_parameter("imu_topic", _imu_topic); + + if(!get_parameter("localizer", _localizer)){ + std::cout << "localizer is not set." << std::endl; + return CallbackReturn::FAILURE; + } + + if(!get_parameter("tf_x", _tf_x)){ + std::cout << "tf_x is not set." << std::endl; + return CallbackReturn::FAILURE; + } + + if(!get_parameter("tf_y", _tf_y)){ + std::cout << "tf_y is not set." << std::endl; + return CallbackReturn::FAILURE; + } + + if(!get_parameter("tf_z", _tf_z)){ + std::cout << "tf_z is not set." << std::endl; + return CallbackReturn::FAILURE; + } + + if(!get_parameter("tf_roll", _tf_roll)){ + std::cout << "tf_roll is not set." << std::endl; + return CallbackReturn::FAILURE; + } + + if(!get_parameter("tf_pitch", _tf_pitch)){ + std::cout << "tf_pitch is not set." << std::endl; + return CallbackReturn::FAILURE; + } + + if(!get_parameter("tf_yaw", _tf_yaw)){ + std::cout << "tf_yaw is not set." << std::endl; + return CallbackReturn::FAILURE; + } + + std::cout << "-----------------------------------------------------------------" << std::endl; + std::cout << "Log file: " << filename << std::endl; + std::cout << "method_type: " << static_cast(_method_type) << std::endl; + std::cout << "use_gnss: " << _use_gnss << std::endl; + std::cout << "queue_size: " << _queue_size << std::endl; + std::cout << "offset: " << _offset << std::endl; + std::cout << "get_height: " << _get_height << std::endl; + std::cout << "use_local_transform: " << _use_local_transform << std::endl; + std::cout << "use_odom: " << _use_odom << std::endl; + std::cout << "use_imu: " << _use_imu << std::endl; + std::cout << "imu_upside_down: " << _imu_upside_down << std::endl; + std::cout << "imu_topic: " << _imu_topic << std::endl; + std::cout << "localizer: " << _localizer << std::endl; + std::cout << "gnss_reinit_fitness: " << _gnss_reinit_fitness << std::endl; + std::cout << "base_frame: " << _base_frame << std::endl; + std::cout << "(tf_x,tf_y,tf_z,tf_roll,tf_pitch,tf_yaw): (" << _tf_x << ", " << _tf_y << ", " << _tf_z << ", " + << _tf_roll << ", " << _tf_pitch << ", " << _tf_yaw << ")" << std::endl; + std::cout << "-----------------------------------------------------------------" << std::endl; + + #ifndef CUDA_FOUND + if (_method_type == MethodType::PCL_ANH_GPU) + { + std::cerr << "**************************************************************" << std::endl; + std::cerr << "[ERROR]PCL_ANH_GPU is not built. Please use other method type." << std::endl; + std::cerr << "**************************************************************" << std::endl; + exit(1); + } + #endif + #ifndef USE_PCL_OPENMP + if (_method_type == MethodType::PCL_OPENMP) + { + std::cerr << "**************************************************************" << std::endl; + std::cerr << "[ERROR]PCL_OPENMP is not built. Please use other method type." << std::endl; + std::cerr << "**************************************************************" << std::endl; + exit(1); + } + #endif + + Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z); // tl: translation + Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX()); // rot: rotation + Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY()); + Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ()); + tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix(); + + // Updated in initialpose_callback or gnss_callback + initial_pose.x = 0.0; + initial_pose.y = 0.0; + initial_pose.z = 0.0; + initial_pose.roll = 0.0; + initial_pose.pitch = 0.0; + initial_pose.yaw = 0.0; + + //Define tf buffer and listener globally + buffer_ptr_ = std::make_shared(get_clock()); + listener_ptr_ = std::make_shared(*buffer_ptr_); + + // Initialize publishers + predict_pose_pub = create_publisher("predict_pose", 10); + predict_pose_imu_pub = create_publisher("predict_pose_imu", 10); + predict_pose_odom_pub = create_publisher("predict_pose_odom", 10); + predict_pose_imu_odom_pub = create_publisher("predict_pose_imu_odom", 10); + ndt_pose_pub = create_publisher("ndt_pose", 10); + // current_pose_pub = nh.advertise("/current_pose", 10); + localizer_pose_pub = create_publisher("localizer_pose", 10); + estimate_twist_pub = create_publisher("estimate_twist", 10); + estimated_vel_mps_pub = create_publisher("estimated_vel_mps", 10); + estimated_vel_kmph_pub = create_publisher("estimated__vel_kmph", 10); + estimated_vel_pub = create_publisher("estimated_vel", 10); + time_ndt_matching_pub = create_publisher("time_ndt_matching", 10); + ndt_stat_pub = create_publisher("ndt_stat", 10); + ndt_reliability_pub = create_publisher("ndt_reliability", 10); + + // Initialize subscribers + param_sub = create_subscription("config/ndt", 10 , std::bind(&NDTMatching::param_callback, this, std::placeholders::_1)); + gnss_sub = create_subscription("gnss_pose", _queue_size, std::bind(&NDTMatching::gnss_callback, this, std::placeholders::_1)); + // ros::Subscriber map_sub = nh.subscribe("points_map", 1, map_callback); + initialpose_sub = create_subscription("initialpose", _queue_size, std::bind(&NDTMatching::initialpose_callback, this, std::placeholders::_1)); + points_sub = create_subscription("filtered_points", _queue_size, std::bind(&NDTMatching::points_callback, this, std::placeholders::_1)); + odom_sub = create_subscription("vehicle/odom", _queue_size * 10, std::bind(&NDTMatching::odom_callback, this, std::placeholders::_1)); + imu_sub = create_subscription(_imu_topic.c_str(), _queue_size * 10, std::bind(&NDTMatching::imu_callback, this, std::placeholders::_1)); + + // Create callback group to handle points_map callbacks + //points_map topic is published as transient_local, subscriber needs to be set to that + auto points_map_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions points_map_sub_options; + points_map_sub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto sub_qos_transient_local = rclcpp::QoS(rclcpp::KeepLast(10)); + sub_qos_transient_local.transient_local(); + points_map_sub_options.callback_group = points_map_callback_group; + + map_sub = create_subscription("points_map", sub_qos_transient_local, std::bind(&NDTMatching::map_callback, this, std::placeholders::_1), points_map_sub_options); + + return CallbackReturn::SUCCESS; +} + +void NDTMatching::param_callback(const autoware_config_msgs::msg::ConfigNDT::SharedPtr input){ + + if (_use_gnss != input->init_pos_gnss) + { + init_pos_set = 0; + } + else if (_use_gnss == 0 && + (initial_pose.x != input->x || initial_pose.y != input->y || initial_pose.z != input->z || + initial_pose.roll != input->roll || initial_pose.pitch != input->pitch || initial_pose.yaw != input->yaw)) + { + init_pos_set = 0; + } + + _use_gnss = input->init_pos_gnss; + + // Setting parameters + if (input->resolution != ndt_res) + { + ndt_res = input->resolution; + + if (_method_type == MethodType::PCL_GENERIC) + ndt.setResolution(ndt_res); + else if (_method_type == MethodType::PCL_ANH) + anh_ndt.setResolution(ndt_res); + #ifdef CUDA_FOUND + else if (_method_type == MethodType::PCL_ANH_GPU) + anh_gpu_ndt_ptr->setResolution(ndt_res); + #endif + #ifdef USE_PCL_OPENMP + else if (_method_type == MethodType::PCL_OPENMP) + omp_ndt.setResolution(ndt_res); + #endif + } + + if (input->step_size != step_size) + { + step_size = input->step_size; + + if (_method_type == MethodType::PCL_GENERIC) + ndt.setStepSize(step_size); + else if (_method_type == MethodType::PCL_ANH) + anh_ndt.setStepSize(step_size); + #ifdef CUDA_FOUND + else if (_method_type == MethodType::PCL_ANH_GPU) + anh_gpu_ndt_ptr->setStepSize(step_size); + #endif + #ifdef USE_PCL_OPENMP + else if (_method_type == MethodType::PCL_OPENMP) + omp_ndt.setStepSize(ndt_res); + #endif + } + + if (input->trans_epsilon != trans_eps) + { + trans_eps = input->trans_epsilon; + double rot_threshold = 1.0 - trans_eps; + + + if (_method_type == MethodType::PCL_GENERIC) { + RCLCPP_INFO(get_logger(), "Using translation threshold of %f", trans_eps); + RCLCPP_INFO(get_logger(), "Using rotation threshold of %f", rot_threshold); + ndt.setTransformationEpsilon(trans_eps); + ndt.setTransformationRotationEpsilon(rot_threshold); + } + else if (_method_type == MethodType::PCL_ANH) { + anh_ndt.setTransformationEpsilon(trans_eps); + } + #ifdef CUDA_FOUND + else if (_method_type == MethodType::PCL_ANH_GPU) { + anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps); + } + #endif + #ifdef USE_PCL_OPENMP + else if (_method_type == MethodType::PCL_OPENMP) { + omp_ndt.setTransformationEpsilon(ndt_res); + } + #endif + } + + if (input->max_iterations != max_iter) + { + max_iter = input->max_iterations; + + if (_method_type == MethodType::PCL_GENERIC) + ndt.setMaximumIterations(max_iter); + else if (_method_type == MethodType::PCL_ANH) + anh_ndt.setMaximumIterations(max_iter); + #ifdef CUDA_FOUND + else if (_method_type == MethodType::PCL_ANH_GPU) + anh_gpu_ndt_ptr->setMaximumIterations(max_iter); + #endif + #ifdef USE_PCL_OPENMP + else if (_method_type == MethodType::PCL_OPENMP) + omp_ndt.setMaximumIterations(ndt_res); + #endif + } + + if (_use_gnss == 0 && init_pos_set == 0) + { + initial_pose.x = input->x; + initial_pose.y = input->y; + initial_pose.z = input->z; + initial_pose.roll = input->roll; + initial_pose.pitch = input->pitch; + initial_pose.yaw = input->yaw; + + if (_use_local_transform == true) + { + tf2::Vector3 v(input->x, input->y, input->z); + tf2::Quaternion q; + q.setRPY(input->roll, input->pitch, input->yaw); + tf2::Transform transform(q, v); + initial_pose.x = (local_transform.inverse() * transform).getOrigin().getX(); + initial_pose.y = (local_transform.inverse() * transform).getOrigin().getY(); + initial_pose.z = (local_transform.inverse() * transform).getOrigin().getZ(); + + tf2::Matrix3x3 m(q); + m.getRPY(initial_pose.roll, initial_pose.pitch, initial_pose.yaw); + + std::cout << "initial_pose.x: " << initial_pose.x << std::endl; + std::cout << "initial_pose.y: " << initial_pose.y << std::endl; + std::cout << "initial_pose.z: " << initial_pose.z << std::endl; + std::cout << "initial_pose.roll: " << initial_pose.roll << std::endl; + std::cout << "initial_pose.pitch: " << initial_pose.pitch << std::endl; + std::cout << "initial_pose.yaw: " << initial_pose.yaw << std::endl; + } + + // Setting position and posture for the first time. + localizer_pose.x = initial_pose.x; + localizer_pose.y = initial_pose.y; + localizer_pose.z = initial_pose.z; + localizer_pose.roll = initial_pose.roll; + localizer_pose.pitch = initial_pose.pitch; + localizer_pose.yaw = initial_pose.yaw; + + previous_pose.x = initial_pose.x; + previous_pose.y = initial_pose.y; + previous_pose.z = initial_pose.z; + previous_pose.roll = initial_pose.roll; + previous_pose.pitch = initial_pose.pitch; + previous_pose.yaw = initial_pose.yaw; + + current_pose.x = initial_pose.x; + current_pose.y = initial_pose.y; + current_pose.z = initial_pose.z; + current_pose.roll = initial_pose.roll; + current_pose.pitch = initial_pose.pitch; + current_pose.yaw = initial_pose.yaw; + + current_velocity = 0; + current_velocity_x = 0; + current_velocity_y = 0; + current_velocity_z = 0; + angular_velocity = 0; + + current_pose_imu.x = 0; + current_pose_imu.y = 0; + current_pose_imu.z = 0; + current_pose_imu.roll = 0; + current_pose_imu.pitch = 0; + current_pose_imu.yaw = 0; + + current_velocity_imu_x = current_velocity_x; + current_velocity_imu_y = current_velocity_y; + current_velocity_imu_z = current_velocity_z; + init_pos_set = 1; + } + +} + +void NDTMatching::gnss_callback(const geometry_msgs::msg::PoseStamped::SharedPtr input){ + + + tf2::Quaternion gnss_q(input->pose.orientation.x, input->pose.orientation.y, input->pose.orientation.z, + input->pose.orientation.w); + tf2::Matrix3x3 gnss_m(gnss_q); + + pose current_gnss_pose; + current_gnss_pose.x = input->pose.position.x; + current_gnss_pose.y = input->pose.position.y; + current_gnss_pose.z = input->pose.position.z; + gnss_m.getRPY(current_gnss_pose.roll, current_gnss_pose.pitch, current_gnss_pose.yaw); + + static pose previous_gnss_pose = current_gnss_pose; + rclcpp::Time current_gnss_time = input->header.stamp; + static rclcpp::Time previous_gnss_time = current_gnss_time; + + if ((_use_gnss == 1 && init_pos_set == 0) || fitness_score >= _gnss_reinit_fitness) + { + previous_pose.x = previous_gnss_pose.x; + previous_pose.y = previous_gnss_pose.y; + previous_pose.z = previous_gnss_pose.z; + previous_pose.roll = previous_gnss_pose.roll; + previous_pose.pitch = previous_gnss_pose.pitch; + previous_pose.yaw = previous_gnss_pose.yaw; + + current_pose.x = current_gnss_pose.x; + current_pose.y = current_gnss_pose.y; + current_pose.z = current_gnss_pose.z; + current_pose.roll = current_gnss_pose.roll; + current_pose.pitch = current_gnss_pose.pitch; + current_pose.yaw = current_gnss_pose.yaw; + + current_pose_imu = current_pose_odom = current_pose_imu_odom = current_pose; + + diff_x = current_pose.x - previous_pose.x; + diff_y = current_pose.y - previous_pose.y; + diff_z = current_pose.z - previous_pose.z; + diff_yaw = current_pose.yaw - previous_pose.yaw; + diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z); + + const pose trans_current_pose = convertPoseIntoRelativeCoordinate(current_pose, previous_pose); + + const double diff_time = (current_gnss_time - previous_gnss_time).seconds(); + current_velocity = (diff_time > 0) ? (diff / diff_time) : 0; + current_velocity = (trans_current_pose.x >= 0) ? current_velocity : -current_velocity; + current_velocity_x = (diff_time > 0) ? (diff_x / diff_time) : 0; + current_velocity_y = (diff_time > 0) ? (diff_y / diff_time) : 0; + current_velocity_z = (diff_time > 0) ? (diff_z / diff_time) : 0; + angular_velocity = (diff_time > 0) ? (diff_yaw / diff_time) : 0; + + current_accel = 0.0; + current_accel_x = 0.0; + current_accel_y = 0.0; + current_accel_z = 0.0; + + init_pos_set = 1; + } + + previous_gnss_pose.x = current_gnss_pose.x; + previous_gnss_pose.y = current_gnss_pose.y; + previous_gnss_pose.z = current_gnss_pose.z; + previous_gnss_pose.roll = current_gnss_pose.roll; + previous_gnss_pose.pitch = current_gnss_pose.pitch; + previous_gnss_pose.yaw = current_gnss_pose.yaw; + previous_gnss_time = current_gnss_time; + +} + +pose NDTMatching::convertPoseIntoRelativeCoordinate(const pose &target_pose, const pose &reference_pose) +{ + + tf2::Quaternion target_q; + target_q.setRPY(target_pose.roll, target_pose.pitch, target_pose.yaw); + tf2::Vector3 target_v(target_pose.x, target_pose.y, target_pose.z); + tf2::Transform target_tf(target_q, target_v); + + tf2::Quaternion reference_q; + reference_q.setRPY(reference_pose.roll, reference_pose.pitch, reference_pose.yaw); + tf2::Vector3 reference_v(reference_pose.x, reference_pose.y, reference_pose.z); + tf2::Transform reference_tf(reference_q, reference_v); + + tf2::Transform trans_target_tf = reference_tf.inverse() * target_tf; + + pose trans_target_pose; + trans_target_pose.x = trans_target_tf.getOrigin().getX(); + trans_target_pose.y = trans_target_tf.getOrigin().getY(); + trans_target_pose.z = trans_target_tf.getOrigin().getZ(); + tf2::Matrix3x3 tmp_m(trans_target_tf.getRotation()); + tmp_m.getRPY(trans_target_pose.roll, trans_target_pose.pitch, trans_target_pose.yaw); + + return trans_target_pose; +} + +void NDTMatching::initialpose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr input){ + + + geometry_msgs::msg::TransformStamped tf_geom; + + tf2::Stamped transform; + try + { + rclcpp::Time now = this->now(); + tf_geom = buffer_ptr_->lookupTransform(_map_frame, input->header.frame_id, now, rclcpp::Duration(10,0)); + tf2::convert(tf_geom, transform); // note that tf2 is missing child_frame_id + + } + catch (tf2::TransformException& ex) + { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + } + + tf2::Quaternion q(input->pose.pose.orientation.x, input->pose.pose.orientation.y, input->pose.pose.orientation.z, + input->pose.pose.orientation.w); + tf2::Matrix3x3 m(q); + + if (_use_local_transform == true) + { + current_pose.x = input->pose.pose.position.x; + current_pose.y = input->pose.pose.position.y; + current_pose.z = input->pose.pose.position.z; + } + else + { + current_pose.x = input->pose.pose.position.x + transform.getOrigin().x(); + current_pose.y = input->pose.pose.position.y + transform.getOrigin().y(); + current_pose.z = input->pose.pose.position.z + transform.getOrigin().z(); + } + m.getRPY(current_pose.roll, current_pose.pitch, current_pose.yaw); + + if (_get_height == true && map_loaded == 1) + { + double min_distance = DBL_MAX; + double nearest_z = current_pose.z; + for (const auto& p : map) + { + double distance = hypot(current_pose.x - p.x, current_pose.y - p.y); + if (distance < min_distance) + { + min_distance = distance; + nearest_z = p.z; + } + } + current_pose.z = nearest_z; + } + + current_pose_imu = current_pose_odom = current_pose_imu_odom = current_pose; + previous_pose.x = current_pose.x; + previous_pose.y = current_pose.y; + previous_pose.z = current_pose.z; + previous_pose.roll = current_pose.roll; + previous_pose.pitch = current_pose.pitch; + previous_pose.yaw = current_pose.yaw; + + current_velocity = 0.0; + current_velocity_x = 0.0; + current_velocity_y = 0.0; + current_velocity_z = 0.0; + angular_velocity = 0.0; + + current_accel = 0.0; + current_accel_x = 0.0; + current_accel_y = 0.0; + current_accel_z = 0.0; + + offset_x = 0.0; + offset_y = 0.0; + offset_z = 0.0; + offset_yaw = 0.0; + + offset_imu_x = 0.0; + offset_imu_y = 0.0; + offset_imu_z = 0.0; + offset_imu_roll = 0.0; + offset_imu_pitch = 0.0; + offset_imu_yaw = 0.0; + + offset_odom_x = 0.0; + offset_odom_y = 0.0; + offset_odom_z = 0.0; + offset_odom_roll = 0.0; + offset_odom_pitch = 0.0; + offset_odom_yaw = 0.0; + + offset_imu_odom_x = 0.0; + offset_imu_odom_y = 0.0; + offset_imu_odom_z = 0.0; + offset_imu_odom_roll = 0.0; + offset_imu_odom_pitch = 0.0; + offset_imu_odom_yaw = 0.0; + + init_pos_set = 1; + +} + +void NDTMatching::points_callback(const sensor_msgs::msg::PointCloud2::SharedPtr input){ + + if (map_loaded == 1 && init_pos_set == 1) + { + matching_start = std::chrono::system_clock::now(); + + static tf2_ros::TransformBroadcaster br(shared_from_this()); + tf2::Transform transform; + tf2::Quaternion predict_q, ndt_q, current_q, localizer_q; + + pcl::PointXYZ p; + pcl::PointCloud filtered_scan; + + rclcpp::Time current_scan_time = input->header.stamp; + static rclcpp::Time previous_scan_time = current_scan_time; + + pcl::fromROSMsg(*input, filtered_scan); + pcl::PointCloud::Ptr filtered_scan_ptr(new pcl::PointCloud(filtered_scan)); + int scan_points_num = filtered_scan_ptr->size(); + + Eigen::Matrix4f t(Eigen::Matrix4f::Identity()); // base_link + Eigen::Matrix4f t2(Eigen::Matrix4f::Identity()); // localizer + + std::chrono::time_point align_start, align_end, getFitnessScore_start, + getFitnessScore_end; + static double align_time, getFitnessScore_time = 0.0; + + pthread_mutex_lock(&mutex); + + if (_method_type == MethodType::PCL_GENERIC) + ndt.setInputSource(filtered_scan_ptr); + else if (_method_type == MethodType::PCL_ANH) + anh_ndt.setInputSource(filtered_scan_ptr); + #ifdef CUDA_FOUND + else if (_method_type == MethodType::PCL_ANH_GPU) + anh_gpu_ndt_ptr->setInputSource(filtered_scan_ptr); + #endif + #ifdef USE_PCL_OPENMP + else if (_method_type == MethodType::PCL_OPENMP) + omp_ndt.setInputSource(filtered_scan_ptr); + #endif + + // Guess the initial gross estimation of the transformation + double diff_time = (current_scan_time - previous_scan_time).seconds(); + + if (_offset == "linear") + { + offset_x = current_velocity_x * diff_time; + offset_y = current_velocity_y * diff_time; + offset_z = current_velocity_z * diff_time; + offset_yaw = angular_velocity * diff_time; + } + else if (_offset == "quadratic") + { + offset_x = (current_velocity_x + current_accel_x * diff_time) * diff_time; + offset_y = (current_velocity_y + current_accel_y * diff_time) * diff_time; + offset_z = current_velocity_z * diff_time; + offset_yaw = angular_velocity * diff_time; + } + else if (_offset == "zero") + { + offset_x = 0.0; + offset_y = 0.0; + offset_z = 0.0; + offset_yaw = 0.0; + } + + predict_pose.x = previous_pose.x + offset_x; + predict_pose.y = previous_pose.y + offset_y; + predict_pose.z = previous_pose.z + offset_z; + predict_pose.roll = previous_pose.roll; + predict_pose.pitch = previous_pose.pitch; + predict_pose.yaw = previous_pose.yaw + offset_yaw; + + if (_use_imu == true && _use_odom == true) + imu_odom_calc(current_scan_time); + if (_use_imu == true && _use_odom == false) + imu_calc(current_scan_time); + if (_use_imu == false && _use_odom == true) + odom_calc(current_scan_time); + + pose predict_pose_for_ndt; + if (_use_imu == true && _use_odom == true) + predict_pose_for_ndt = predict_pose_imu_odom; + else if (_use_imu == true && _use_odom == false) + predict_pose_for_ndt = predict_pose_imu; + else if (_use_imu == false && _use_odom == true) + predict_pose_for_ndt = predict_pose_odom; + else + predict_pose_for_ndt = predict_pose; + + Eigen::Translation3f init_translation(predict_pose_for_ndt.x, predict_pose_for_ndt.y, predict_pose_for_ndt.z); + Eigen::AngleAxisf init_rotation_x(predict_pose_for_ndt.roll, Eigen::Vector3f::UnitX()); + Eigen::AngleAxisf init_rotation_y(predict_pose_for_ndt.pitch, Eigen::Vector3f::UnitY()); + Eigen::AngleAxisf init_rotation_z(predict_pose_for_ndt.yaw, Eigen::Vector3f::UnitZ()); + Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol; + + pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud); + + if (_method_type == MethodType::PCL_GENERIC) + { + align_start = std::chrono::system_clock::now(); + ndt.align(*output_cloud, init_guess); + align_end = std::chrono::system_clock::now(); + + has_converged = ndt.hasConverged(); + + t = ndt.getFinalTransformation(); + iteration = ndt.getFinalNumIteration(); + + getFitnessScore_start = std::chrono::system_clock::now(); + fitness_score = ndt.getFitnessScore(); + getFitnessScore_end = std::chrono::system_clock::now(); + + trans_probability = ndt.getTransformationProbability(); + } + else if (_method_type == MethodType::PCL_ANH) + { + align_start = std::chrono::system_clock::now(); + anh_ndt.align(init_guess); + align_end = std::chrono::system_clock::now(); + + has_converged = anh_ndt.hasConverged(); + + t = anh_ndt.getFinalTransformation(); + iteration = anh_ndt.getFinalNumIteration(); + + getFitnessScore_start = std::chrono::system_clock::now(); + fitness_score = anh_ndt.getFitnessScore(); + getFitnessScore_end = std::chrono::system_clock::now(); + + trans_probability = anh_ndt.getTransformationProbability(); + } + #ifdef CUDA_FOUND + else if (_method_type == MethodType::PCL_ANH_GPU) + { + align_start = std::chrono::system_clock::now(); + anh_gpu_ndt_ptr->align(init_guess); + align_end = std::chrono::system_clock::now(); + + has_converged = anh_gpu_ndt_ptr->hasConverged(); + + t = anh_gpu_ndt_ptr->getFinalTransformation(); + iteration = anh_gpu_ndt_ptr->getFinalNumIteration(); + + getFitnessScore_start = std::chrono::system_clock::now(); + fitness_score = anh_gpu_ndt_ptr->getFitnessScore(); + getFitnessScore_end = std::chrono::system_clock::now(); + + trans_probability = anh_gpu_ndt_ptr->getTransformationProbability(); + } + #endif + #ifdef USE_PCL_OPENMP + else if (_method_type == MethodType::PCL_OPENMP) + { + align_start = std::chrono::system_clock::now(); + omp_ndt.align(*output_cloud, init_guess); + align_end = std::chrono::system_clock::now(); + + has_converged = omp_ndt.hasConverged(); + + t = omp_ndt.getFinalTransformation(); + iteration = omp_ndt.getFinalNumIteration(); + + getFitnessScore_start = std::chrono::system_clock::now(); + fitness_score = omp_ndt.getFitnessScore(); + getFitnessScore_end = std::chrono::system_clock::now(); + + trans_probability = omp_ndt.getTransformationProbability(); + } + #endif + align_time = std::chrono::duration_cast(align_end - align_start).count() / 1000.0; + + t2 = t * tf_btol.inverse(); + + getFitnessScore_time = + std::chrono::duration_cast(getFitnessScore_end - getFitnessScore_start).count() / + 1000.0; + + pthread_mutex_unlock(&mutex); + + tf2::Matrix3x3 mat_l; // localizer + mat_l.setValue(static_cast(t(0, 0)), static_cast(t(0, 1)), static_cast(t(0, 2)), + static_cast(t(1, 0)), static_cast(t(1, 1)), static_cast(t(1, 2)), + static_cast(t(2, 0)), static_cast(t(2, 1)), static_cast(t(2, 2))); + + // Update localizer_pose + localizer_pose.x = t(0, 3); + localizer_pose.y = t(1, 3); + localizer_pose.z = t(2, 3); + mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1); + + tf2::Matrix3x3 mat_b; // base_link + mat_b.setValue(static_cast(t2(0, 0)), static_cast(t2(0, 1)), static_cast(t2(0, 2)), + static_cast(t2(1, 0)), static_cast(t2(1, 1)), static_cast(t2(1, 2)), + static_cast(t2(2, 0)), static_cast(t2(2, 1)), static_cast(t2(2, 2))); + + // Update ndt_pose + ndt_pose.x = t2(0, 3); + ndt_pose.y = t2(1, 3); + ndt_pose.z = t2(2, 3); + mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1); + + // Calculate the difference between ndt_pose and predict_pose + predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) + + (ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) + + (ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z)); + + if (predict_pose_error <= PREDICT_POSE_THRESHOLD) + { + use_predict_pose = 0; + } + else + { + use_predict_pose = 1; + } + use_predict_pose = 0; + + if (use_predict_pose == 0) + { + current_pose.x = ndt_pose.x; + current_pose.y = ndt_pose.y; + current_pose.z = ndt_pose.z; + current_pose.roll = ndt_pose.roll; + current_pose.pitch = ndt_pose.pitch; + current_pose.yaw = ndt_pose.yaw; + } + else + { + current_pose.x = predict_pose_for_ndt.x; + current_pose.y = predict_pose_for_ndt.y; + current_pose.z = predict_pose_for_ndt.z; + current_pose.roll = predict_pose_for_ndt.roll; + current_pose.pitch = predict_pose_for_ndt.pitch; + current_pose.yaw = predict_pose_for_ndt.yaw; + } + + // Compute the velocity and acceleration + diff_x = current_pose.x - previous_pose.x; + diff_y = current_pose.y - previous_pose.y; + diff_z = current_pose.z - previous_pose.z; + diff_yaw = calcDiffForRadian(current_pose.yaw, previous_pose.yaw); + diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z); + + const pose trans_current_pose = convertPoseIntoRelativeCoordinate(current_pose, previous_pose); + + current_velocity = (diff_time > 0) ? (diff / diff_time) : 0; + current_velocity = (trans_current_pose.x >= 0) ? current_velocity : -current_velocity; + current_velocity_x = (diff_time > 0) ? (diff_x / diff_time) : 0; + current_velocity_y = (diff_time > 0) ? (diff_y / diff_time) : 0; + current_velocity_z = (diff_time > 0) ? (diff_z / diff_time) : 0; + angular_velocity = (diff_time > 0) ? (diff_yaw / diff_time) : 0; + + current_pose_imu.x = current_pose.x; + current_pose_imu.y = current_pose.y; + current_pose_imu.z = current_pose.z; + current_pose_imu.roll = current_pose.roll; + current_pose_imu.pitch = current_pose.pitch; + current_pose_imu.yaw = current_pose.yaw; + + current_velocity_imu_x = current_velocity_x; + current_velocity_imu_y = current_velocity_y; + current_velocity_imu_z = current_velocity_z; + + current_pose_odom.x = current_pose.x; + current_pose_odom.y = current_pose.y; + current_pose_odom.z = current_pose.z; + current_pose_odom.roll = current_pose.roll; + current_pose_odom.pitch = current_pose.pitch; + current_pose_odom.yaw = current_pose.yaw; + + current_pose_imu_odom.x = current_pose.x; + current_pose_imu_odom.y = current_pose.y; + current_pose_imu_odom.z = current_pose.z; + current_pose_imu_odom.roll = current_pose.roll; + current_pose_imu_odom.pitch = current_pose.pitch; + current_pose_imu_odom.yaw = current_pose.yaw; + + current_velocity_smooth = (current_velocity + previous_velocity + previous_previous_velocity) / 3.0; + if (std::fabs(current_velocity_smooth) < 0.2) + { + current_velocity_smooth = 0.0; + } + + current_accel = (diff_time > 0) ? ((current_velocity - previous_velocity) / diff_time) : 0; + current_accel_x = (diff_time > 0) ? ((current_velocity_x - previous_velocity_x) / diff_time) : 0; + current_accel_y = (diff_time > 0) ? ((current_velocity_y - previous_velocity_y) / diff_time) : 0; + current_accel_z = (diff_time > 0) ? ((current_velocity_z - previous_velocity_z) / diff_time) : 0; + + estimated_vel_mps.data = current_velocity; + estimated_vel_kmph.data = current_velocity * 3.6; + + estimated_vel_mps_pub->publish(estimated_vel_mps); + estimated_vel_kmph_pub->publish(estimated_vel_kmph); + + // Set values for publishing pose + predict_q.setRPY(predict_pose.roll, predict_pose.pitch, predict_pose.yaw); + if (_use_local_transform == true) + { + tf2::Vector3 v(predict_pose.x, predict_pose.y, predict_pose.z); + tf2::Transform transform(predict_q, v); + predict_pose_msg.header.frame_id = _map_frame; + predict_pose_msg.header.stamp = current_scan_time; + predict_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX(); + predict_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY(); + predict_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ(); + predict_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x(); + predict_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y(); + predict_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z(); + predict_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w(); + } + else + { + predict_pose_msg.header.frame_id = _map_frame; + predict_pose_msg.header.stamp = current_scan_time; + predict_pose_msg.pose.position.x = predict_pose.x; + predict_pose_msg.pose.position.y = predict_pose.y; + predict_pose_msg.pose.position.z = predict_pose.z; + predict_pose_msg.pose.orientation.x = predict_q.x(); + predict_pose_msg.pose.orientation.y = predict_q.y(); + predict_pose_msg.pose.orientation.z = predict_q.z(); + predict_pose_msg.pose.orientation.w = predict_q.w(); + } + + tf2::Quaternion predict_q_imu; + predict_q_imu.setRPY(predict_pose_imu.roll, predict_pose_imu.pitch, predict_pose_imu.yaw); + predict_pose_imu_msg.header.frame_id = _map_frame; + predict_pose_imu_msg.header.stamp = input->header.stamp; + predict_pose_imu_msg.pose.position.x = predict_pose_imu.x; + predict_pose_imu_msg.pose.position.y = predict_pose_imu.y; + predict_pose_imu_msg.pose.position.z = predict_pose_imu.z; + predict_pose_imu_msg.pose.orientation.x = predict_q_imu.x(); + predict_pose_imu_msg.pose.orientation.y = predict_q_imu.y(); + predict_pose_imu_msg.pose.orientation.z = predict_q_imu.z(); + predict_pose_imu_msg.pose.orientation.w = predict_q_imu.w(); + + predict_pose_imu_pub->publish(predict_pose_imu_msg); + + tf2::Quaternion predict_q_odom; + predict_q_odom.setRPY(predict_pose_odom.roll, predict_pose_odom.pitch, predict_pose_odom.yaw); + predict_pose_odom_msg.header.frame_id = _map_frame; + predict_pose_odom_msg.header.stamp = input->header.stamp; + predict_pose_odom_msg.pose.position.x = predict_pose_odom.x; + predict_pose_odom_msg.pose.position.y = predict_pose_odom.y; + predict_pose_odom_msg.pose.position.z = predict_pose_odom.z; + predict_pose_odom_msg.pose.orientation.x = predict_q_odom.x(); + predict_pose_odom_msg.pose.orientation.y = predict_q_odom.y(); + predict_pose_odom_msg.pose.orientation.z = predict_q_odom.z(); + predict_pose_odom_msg.pose.orientation.w = predict_q_odom.w(); + + predict_pose_odom_pub->publish(predict_pose_odom_msg); + + tf2::Quaternion predict_q_imu_odom; + predict_q_imu_odom.setRPY(predict_pose_imu_odom.roll, predict_pose_imu_odom.pitch, predict_pose_imu_odom.yaw); + predict_pose_imu_odom_msg.header.frame_id = _map_frame; + predict_pose_imu_odom_msg.header.stamp = input->header.stamp; + predict_pose_imu_odom_msg.pose.position.x = predict_pose_imu_odom.x; + predict_pose_imu_odom_msg.pose.position.y = predict_pose_imu_odom.y; + predict_pose_imu_odom_msg.pose.position.z = predict_pose_imu_odom.z; + predict_pose_imu_odom_msg.pose.orientation.x = predict_q_imu_odom.x(); + predict_pose_imu_odom_msg.pose.orientation.y = predict_q_imu_odom.y(); + predict_pose_imu_odom_msg.pose.orientation.z = predict_q_imu_odom.z(); + predict_pose_imu_odom_msg.pose.orientation.w = predict_q_imu_odom.w(); + + predict_pose_imu_odom_pub->publish(predict_pose_imu_odom_msg); + + ndt_q.setRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw); + if (_use_local_transform == true) + { + + tf2::Vector3 v(ndt_pose.x, ndt_pose.y, ndt_pose.z); + tf2::Transform transform(ndt_q, v); + ndt_pose_msg.header.frame_id = _map_frame; + ndt_pose_msg.header.stamp = current_scan_time; + ndt_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX(); + ndt_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY(); + ndt_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ(); + ndt_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x(); + ndt_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y(); + ndt_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z(); + ndt_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w(); + } + else + { + ndt_pose_msg.header.frame_id = _map_frame; + ndt_pose_msg.header.stamp = current_scan_time; + ndt_pose_msg.pose.position.x = ndt_pose.x; + ndt_pose_msg.pose.position.y = ndt_pose.y; + ndt_pose_msg.pose.position.z = ndt_pose.z; + ndt_pose_msg.pose.orientation.x = ndt_q.x(); + ndt_pose_msg.pose.orientation.y = ndt_q.y(); + ndt_pose_msg.pose.orientation.z = ndt_q.z(); + ndt_pose_msg.pose.orientation.w = ndt_q.w(); + } + + current_q.setRPY(current_pose.roll, current_pose.pitch, current_pose.yaw); + // current_pose is published by vel_pose_mux + /* + current_pose_msg.header.frame_id = _map_frame; + current_pose_msg.header.stamp = current_scan_time; + current_pose_msg.pose.position.x = current_pose.x; + current_pose_msg.pose.position.y = current_pose.y; + current_pose_msg.pose.position.z = current_pose.z; + current_pose_msg.pose.orientation.x = current_q.x(); + current_pose_msg.pose.orientation.y = current_q.y(); + current_pose_msg.pose.orientation.z = current_q.z(); + current_pose_msg.pose.orientation.w = current_q.w(); + */ + + localizer_q.setRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw); + if (_use_local_transform == true) + { + tf2::Vector3 v(localizer_pose.x, localizer_pose.y, localizer_pose.z); + tf2::Transform transform(localizer_q, v); + localizer_pose_msg.header.frame_id = _map_frame; + localizer_pose_msg.header.stamp = current_scan_time; + localizer_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX(); + localizer_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY(); + localizer_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ(); + localizer_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x(); + localizer_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y(); + localizer_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z(); + localizer_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w(); + } + else + { + localizer_pose_msg.header.frame_id = _map_frame; + localizer_pose_msg.header.stamp = current_scan_time; + localizer_pose_msg.pose.position.x = localizer_pose.x; + localizer_pose_msg.pose.position.y = localizer_pose.y; + localizer_pose_msg.pose.position.z = localizer_pose.z; + localizer_pose_msg.pose.orientation.x = localizer_q.x(); + localizer_pose_msg.pose.orientation.y = localizer_q.y(); + localizer_pose_msg.pose.orientation.z = localizer_q.z(); + localizer_pose_msg.pose.orientation.w = localizer_q.w(); + } + + + predict_pose_pub->publish(predict_pose_msg); + + ndt_pose_pub->publish(ndt_pose_msg); + // current_pose is published by vel_pose_mux + // current_pose_pub.publish(current_pose_msg); + + localizer_pose_pub->publish(localizer_pose_msg); + + // Send TF _base_frame to _map_frame + transform.setOrigin(tf2::Vector3(current_pose.x, current_pose.y, current_pose.z)); + transform.setRotation(current_q); + // br.sendTransform(tf::StampedTransform(transform, current_scan_time, _map_frame, _base_frame)); + /* Removed to allow CARMA localization node to publish this TF + if (_use_local_transform == true) + { + br.sendTransform(tf::StampedTransform(local_transform * transform, current_scan_time, _map_frame, _base_frame)); + } + else + { + br.sendTransform(tf::StampedTransform(transform, current_scan_time, _map_frame, _base_frame)); + } + */ + + matching_end = std::chrono::system_clock::now(); + exe_time = std::chrono::duration_cast(matching_end - matching_start).count() / 1000.0; + time_ndt_matching.data = exe_time; + + time_ndt_matching_pub->publish(time_ndt_matching); + + // Set values for /estimate_twist + estimate_twist_msg.header.stamp = current_scan_time; + estimate_twist_msg.header.frame_id = _base_frame; + estimate_twist_msg.twist.linear.x = current_velocity; + estimate_twist_msg.twist.linear.y = 0.0; + estimate_twist_msg.twist.linear.z = 0.0; + estimate_twist_msg.twist.angular.x = 0.0; + estimate_twist_msg.twist.angular.y = 0.0; + estimate_twist_msg.twist.angular.z = angular_velocity; + + estimate_twist_pub->publish(estimate_twist_msg); + + geometry_msgs::msg::Vector3Stamped estimate_vel_msg; + estimate_vel_msg.header.stamp = current_scan_time; + estimate_vel_msg.vector.x = current_velocity; + + estimated_vel_pub->publish(estimate_vel_msg); + + // Set values for /ndt_stat + ndt_stat_msg.header.stamp = current_scan_time; + ndt_stat_msg.exe_time = time_ndt_matching.data; + ndt_stat_msg.iteration = iteration; + ndt_stat_msg.score = fitness_score; + ndt_stat_msg.velocity = current_velocity; + ndt_stat_msg.acceleration = current_accel; + ndt_stat_msg.use_predict_pose = 0; + + ndt_stat_pub->publish(ndt_stat_msg); + /* Compute NDT_Reliability */ + ndt_reliability.data = Wa * (exe_time / 100.0) * 100.0 + Wb * (iteration / 10.0) * 100.0 + + Wc * ((2.0 - trans_probability) / 2.0) * 100.0; + + ndt_reliability_pub->publish(ndt_reliability); + + // Write log + if(_output_log_data) + { + if (!ofs) + { + std::cerr << "Could not open " << filename << "." << std::endl; + } + else + { + //ROS2 std_msgs header doesn't contain seq + ofs << scan_points_num << "," << step_size << "," << trans_eps << "," << std::fixed + << std::setprecision(5) << current_pose.x << "," << std::fixed << std::setprecision(5) << current_pose.y << "," + << std::fixed << std::setprecision(5) << current_pose.z << "," << current_pose.roll << "," << current_pose.pitch + << "," << current_pose.yaw << "," << predict_pose.x << "," << predict_pose.y << "," << predict_pose.z << "," + << predict_pose.roll << "," << predict_pose.pitch << "," << predict_pose.yaw << "," + << current_pose.x - predict_pose.x << "," << current_pose.y - predict_pose.y << "," + << current_pose.z - predict_pose.z << "," << current_pose.roll - predict_pose.roll << "," + << current_pose.pitch - predict_pose.pitch << "," << current_pose.yaw - predict_pose.yaw << "," + << predict_pose_error << "," << iteration << "," << fitness_score << "," << trans_probability << "," + << ndt_reliability.data << "," << current_velocity << "," << current_velocity_smooth << "," << current_accel + << "," << angular_velocity << "," << time_ndt_matching.data << "," << align_time << "," << getFitnessScore_time + << std::endl; + } + } + + std::cout << "-----------------------------------------------------------------" << std::endl; + // std::cout << "Sequence: " << input->header.seq << std::endl; + std::cout << "Timestamp: " << rclcpp::Time(input->header.stamp).seconds() << std::endl; + std::cout << "Frame ID: " << input->header.frame_id << std::endl; + // std::cout << "Number of Scan Points: " << scan_ptr->size() << " points." << std::endl; + std::cout << "Number of Filtered Scan Points: " << scan_points_num << " points." << std::endl; + std::cout << "NDT has converged: " << has_converged << std::endl; + std::cout << "Fitness Score: " << fitness_score << std::endl; + std::cout << "Transformation Probability: " << trans_probability << std::endl; + std::cout << "Execution Time: " << exe_time << " ms." << std::endl; + std::cout << "Number of Iterations: " << iteration << std::endl; + std::cout << "NDT Reliability: " << ndt_reliability.data << std::endl; + std::cout << "(x,y,z,roll,pitch,yaw): " << std::endl; + std::cout << "(" << current_pose.x << ", " << current_pose.y << ", " << current_pose.z << ", " << current_pose.roll + << ", " << current_pose.pitch << ", " << current_pose.yaw << ")" << std::endl; + std::cout << "Transformation Matrix: " << std::endl; + std::cout << t << std::endl; + std::cout << "Align time: " << align_time << std::endl; + std::cout << "Get fitness score time: " << getFitnessScore_time << std::endl; + std::cout << "-----------------------------------------------------------------" << std::endl; + + offset_imu_x = 0.0; + offset_imu_y = 0.0; + offset_imu_z = 0.0; + offset_imu_roll = 0.0; + offset_imu_pitch = 0.0; + offset_imu_yaw = 0.0; + + offset_odom_x = 0.0; + offset_odom_y = 0.0; + offset_odom_z = 0.0; + offset_odom_roll = 0.0; + offset_odom_pitch = 0.0; + offset_odom_yaw = 0.0; + + offset_imu_odom_x = 0.0; + offset_imu_odom_y = 0.0; + offset_imu_odom_z = 0.0; + offset_imu_odom_roll = 0.0; + offset_imu_odom_pitch = 0.0; + offset_imu_odom_yaw = 0.0; + + // Update previous_*** + previous_pose.x = current_pose.x; + previous_pose.y = current_pose.y; + previous_pose.z = current_pose.z; + previous_pose.roll = current_pose.roll; + previous_pose.pitch = current_pose.pitch; + previous_pose.yaw = current_pose.yaw; + + previous_scan_time = current_scan_time; + + previous_previous_velocity = previous_velocity; + previous_velocity = current_velocity; + previous_velocity_x = current_velocity_x; + previous_velocity_y = current_velocity_y; + previous_velocity_z = current_velocity_z; + previous_accel = current_accel; + + previous_estimated_vel_kmph.data = estimated_vel_kmph.data; + } + +} + +void NDTMatching::odom_callback(const nav_msgs::msg::Odometry::SharedPtr input){ + odom = *input; + odom_calc(input->header.stamp); +} + +void NDTMatching::odom_calc(rclcpp::Time current_time){ + + static rclcpp::Time previous_time = current_time; + double diff_time = (current_time - previous_time).seconds(); + + double diff_odom_roll = odom.twist.twist.angular.x * diff_time; + double diff_odom_pitch = odom.twist.twist.angular.y * diff_time; + double diff_odom_yaw = odom.twist.twist.angular.z * diff_time; + + current_pose_odom.roll += diff_odom_roll; + current_pose_odom.pitch += diff_odom_pitch; + current_pose_odom.yaw += diff_odom_yaw; + + double diff_distance = odom.twist.twist.linear.x * diff_time; + offset_odom_x += diff_distance * cos(-current_pose_odom.pitch) * cos(current_pose_odom.yaw); + offset_odom_y += diff_distance * cos(-current_pose_odom.pitch) * sin(current_pose_odom.yaw); + offset_odom_z += diff_distance * sin(-current_pose_odom.pitch); + + offset_odom_roll += diff_odom_roll; + offset_odom_pitch += diff_odom_pitch; + offset_odom_yaw += diff_odom_yaw; + + predict_pose_odom.x = previous_pose.x + offset_odom_x; + predict_pose_odom.y = previous_pose.y + offset_odom_y; + predict_pose_odom.z = previous_pose.z + offset_odom_z; + predict_pose_odom.roll = previous_pose.roll + offset_odom_roll; + predict_pose_odom.pitch = previous_pose.pitch + offset_odom_pitch; + predict_pose_odom.yaw = previous_pose.yaw + offset_odom_yaw; + + previous_time = current_time; + + + } + + +void NDTMatching::imu_callback(const sensor_msgs::msg::Imu::SharedPtr input){ + // std::cout << __func__ << std::endl; + + + if (_imu_upside_down) + imuUpsideDown(input); + + const rclcpp::Time current_time = input->header.stamp; + static rclcpp::Time previous_time = current_time; + const double diff_time = (current_time - previous_time).seconds(); + + double imu_roll, imu_pitch, imu_yaw; + tf2::Quaternion imu_orientation; + + tf2::fromMsg(input->orientation, imu_orientation); + tf2::Matrix3x3(imu_orientation).getRPY(imu_roll, imu_pitch, imu_yaw); + + imu_roll = wrapToPmPi(imu_roll); + imu_pitch = wrapToPmPi(imu_pitch); + imu_yaw = wrapToPmPi(imu_yaw); + + static double previous_imu_roll = imu_roll, previous_imu_pitch = imu_pitch, previous_imu_yaw = imu_yaw; + const double diff_imu_roll = calcDiffForRadian(imu_roll, previous_imu_roll); + const double diff_imu_pitch = calcDiffForRadian(imu_pitch, previous_imu_pitch); + const double diff_imu_yaw = calcDiffForRadian(imu_yaw, previous_imu_yaw); + + imu.header = input->header; + imu.linear_acceleration.x = input->linear_acceleration.x; + // imu.linear_acceleration.y = input->linear_acceleration.y; + // imu.linear_acceleration.z = input->linear_acceleration.z; + imu.linear_acceleration.y = 0; + imu.linear_acceleration.z = 0; + + if (diff_time != 0) + { + imu.angular_velocity.x = diff_imu_roll / diff_time; + imu.angular_velocity.y = diff_imu_pitch / diff_time; + imu.angular_velocity.z = diff_imu_yaw / diff_time; + } + else + { + imu.angular_velocity.x = 0; + imu.angular_velocity.y = 0; + imu.angular_velocity.z = 0; + } + + imu_calc(input->header.stamp); + + previous_time = current_time; + previous_imu_roll = imu_roll; + previous_imu_pitch = imu_pitch; + previous_imu_yaw = imu_yaw; + +} + +void NDTMatching::imuUpsideDown(const sensor_msgs::msg::Imu::SharedPtr input) +{ + double input_roll, input_pitch, input_yaw; + + tf2::Quaternion input_orientation; + tf2::fromMsg(input->orientation, input_orientation); + tf2::Matrix3x3(input_orientation).getRPY(input_roll, input_pitch, input_yaw); + + input->angular_velocity.x *= -1; + input->angular_velocity.y *= -1; + input->angular_velocity.z *= -1; + + input->linear_acceleration.x *= -1; + input->linear_acceleration.y *= -1; + input->linear_acceleration.z *= -1; + + input_roll *= -1; + input_pitch *= -1; + input_yaw *= -1; + + tf2::Quaternion quat_tf; + quat_tf.setRPY(input_roll, input_pitch, input_yaw); + tf2::convert(quat_tf, input->orientation); + + +} + +void NDTMatching::imu_calc(rclcpp::Time current_time){ + + static rclcpp::Time previous_time = current_time; + double diff_time = (current_time - previous_time).seconds(); + + double diff_imu_roll = imu.angular_velocity.x * diff_time; + double diff_imu_pitch = imu.angular_velocity.y * diff_time; + double diff_imu_yaw = imu.angular_velocity.z * diff_time; + + current_pose_imu.roll += diff_imu_roll; + current_pose_imu.pitch += diff_imu_pitch; + current_pose_imu.yaw += diff_imu_yaw; + + double accX1 = imu.linear_acceleration.x; + double accY1 = std::cos(current_pose_imu.roll) * imu.linear_acceleration.y - + std::sin(current_pose_imu.roll) * imu.linear_acceleration.z; + double accZ1 = std::sin(current_pose_imu.roll) * imu.linear_acceleration.y + + std::cos(current_pose_imu.roll) * imu.linear_acceleration.z; + + double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1; + double accY2 = accY1; + double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1; + + double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2; + double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2; + double accZ = accZ2; + + offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0; + offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0; + offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0; + + current_velocity_imu_x += accX * diff_time; + current_velocity_imu_y += accY * diff_time; + current_velocity_imu_z += accZ * diff_time; + + offset_imu_roll += diff_imu_roll; + offset_imu_pitch += diff_imu_pitch; + offset_imu_yaw += diff_imu_yaw; + + predict_pose_imu.x = previous_pose.x + offset_imu_x; + predict_pose_imu.y = previous_pose.y + offset_imu_y; + predict_pose_imu.z = previous_pose.z + offset_imu_z; + predict_pose_imu.roll = previous_pose.roll + offset_imu_roll; + predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch; + predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw; + + previous_time = current_time; +} + +double NDTMatching::wrapToPm(double a_num, const double a_max) +{ +if (a_num >= a_max) +{ + a_num -= 2.0 * a_max; +} + return a_num; +} + +double NDTMatching::wrapToPmPi(const double a_angle_rad) +{ + return wrapToPm(a_angle_rad, M_PI); +} + +double NDTMatching::calcDiffForRadian(const double lhs_rad, const double rhs_rad) +{ + double diff_rad = lhs_rad - rhs_rad; + if (diff_rad >= M_PI) + diff_rad = diff_rad - 2 * M_PI; + else if (diff_rad < -M_PI) + diff_rad = diff_rad + 2 * M_PI; + return diff_rad; +} + +void NDTMatching::map_callback(const sensor_msgs::msg::PointCloud2::SharedPtr input) +{ +// if (map_loaded == 0) +if (points_map_num != input->width) +{ + std::cout << "Update points_map." << std::endl; + + _map_frame = input->header.frame_id; // Update map frame to be the frame of the map points topic + + points_map_num = input->width; + + // Convert the data type(from sensor_msgs to pcl). + pcl::fromROSMsg(*input, map); + + if (_use_local_transform == true) + { + // TODO: use_local_transform is currently being run as False. + // In case an update is made to use this as True, the buffer and listener may need to be moved to use global scope + // There are performance issues noticed in ros2 while using locally defined buffer and listener + tf2_ros::Buffer local_buffer(get_clock()); + tf2_ros::TransformListener local_transform_listener(local_buffer); + geometry_msgs::msg::TransformStamped local_tf_geom; + + tf2::Stamped local_transform; + + try + { + rclcpp::Time now = this->now(); + local_tf_geom = local_buffer.lookupTransform(_map_frame, "world", now, rclcpp::Duration(10, 0)); + tf2::convert(local_tf_geom, local_transform); + } + catch (tf2::TransformException& ex) + { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + } + + // Implementation taken from pcl_ros + transformPointCloud(map, map, local_transform.inverse()); + + } + + pcl::PointCloud::Ptr map_ptr(new pcl::PointCloud(map)); + + double rot_threshold = 1.0 - trans_eps; + + // Setting point cloud to be aligned to. + if (_method_type == MethodType::PCL_GENERIC) + { + RCLCPP_INFO(get_logger(), "Using translation threshold of %f", trans_eps); + RCLCPP_INFO(get_logger(), "Using rotation threshold of %f", rot_threshold); + pcl::NormalDistributionsTransform new_ndt; + pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud); + new_ndt.setResolution(ndt_res); + new_ndt.setInputTarget(map_ptr); + new_ndt.setMaximumIterations(max_iter); + new_ndt.setStepSize(step_size); + new_ndt.setTransformationEpsilon(trans_eps); + new_ndt.setTransformationRotationEpsilon(rot_threshold); + + new_ndt.align(*output_cloud, Eigen::Matrix4f::Identity()); + + pthread_mutex_lock(&mutex); + ndt = new_ndt; + pthread_mutex_unlock(&mutex); + + } + else if (_method_type == MethodType::PCL_ANH) + { + cpu::NormalDistributionsTransform new_anh_ndt; + new_anh_ndt.setResolution(ndt_res); + new_anh_ndt.setInputTarget(map_ptr); + new_anh_ndt.setMaximumIterations(max_iter); + new_anh_ndt.setStepSize(step_size); + new_anh_ndt.setTransformationEpsilon(trans_eps); + + pcl::PointCloud::Ptr dummy_scan_ptr(new pcl::PointCloud()); + pcl::PointXYZ dummy_point; + dummy_scan_ptr->push_back(dummy_point); + new_anh_ndt.setInputSource(dummy_scan_ptr); + + new_anh_ndt.align(Eigen::Matrix4f::Identity()); + + pthread_mutex_lock(&mutex); + anh_ndt = new_anh_ndt; + pthread_mutex_unlock(&mutex); + } + #ifdef CUDA_FOUND + else if (_method_type == MethodType::PCL_ANH_GPU) + { + std::shared_ptr new_anh_gpu_ndt_ptr = + std::make_shared(); + new_anh_gpu_ndt_ptr->setResolution(ndt_res); + new_anh_gpu_ndt_ptr->setInputTarget(map_ptr); + new_anh_gpu_ndt_ptr->setMaximumIterations(max_iter); + new_anh_gpu_ndt_ptr->setStepSize(step_size); + new_anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps); + + pcl::PointCloud::Ptr dummy_scan_ptr(new pcl::PointCloud()); + pcl::PointXYZ dummy_point; + dummy_scan_ptr->push_back(dummy_point); + new_anh_gpu_ndt_ptr->setInputSource(dummy_scan_ptr); + + new_anh_gpu_ndt_ptr->align(Eigen::Matrix4f::Identity()); + + pthread_mutex_lock(&mutex); + anh_gpu_ndt_ptr = new_anh_gpu_ndt_ptr; + pthread_mutex_unlock(&mutex); + } + #endif + #ifdef USE_PCL_OPENMP + else if (_method_type == MethodType::PCL_OPENMP) + { + pcl_omp::NormalDistributionsTransform new_omp_ndt; + pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud); + new_omp_ndt.setResolution(ndt_res); + new_omp_ndt.setInputTarget(map_ptr); + new_omp_ndt.setMaximumIterations(max_iter); + new_omp_ndt.setStepSize(step_size); + new_omp_ndt.setTransformationEpsilon(trans_eps); + + new_omp_ndt.align(*output_cloud, Eigen::Matrix4f::Identity()); + + pthread_mutex_lock(&mutex); + omp_ndt = new_omp_ndt; + pthread_mutex_unlock(&mutex); + } + #endif + map_loaded = 1; + } +} + +template +void NDTMatching::transformPointCloud(const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, const tf2::Transform & transform) +{ + // Bullet (used by tf) and Eigen both store quaternions in x,y,z,w order, despite the ordering + // of arguments in Eigen's constructor. We could use an Eigen Map to convert without copy, but + // this only works if Bullet uses floats, that is if BT_USE_DOUBLE_PRECISION is not defined. + // Rather that risking a mistake, we copy the quaternion, which is a small cost compared to + // the conversion of the point cloud anyway. Idem for the origin. + tf2::Quaternion q = transform.getRotation(); + Eigen::Quaternionf rotation(q.w(), q.x(), q.y(), q.z()); // internally stored as (x,y,z,w) + tf2::Vector3 v = transform.getOrigin(); + Eigen::Vector3f origin(v.x(), v.y(), v.z()); + // Eigen::Translation3f translation(v); + // Assemble an Eigen Transform + // Eigen::Transform3f t; + // t = translation * rotation; + pcl::transformPointCloud(cloud_in, cloud_out, origin, rotation); +} + +void NDTMatching::imu_odom_calc(rclcpp::Time current_time){ + static rclcpp::Time previous_time = current_time; + double diff_time = (current_time - previous_time).seconds(); + + double diff_imu_roll = imu.angular_velocity.x * diff_time; + double diff_imu_pitch = imu.angular_velocity.y * diff_time; + double diff_imu_yaw = imu.angular_velocity.z * diff_time; + + current_pose_imu_odom.roll += diff_imu_roll; + current_pose_imu_odom.pitch += diff_imu_pitch; + current_pose_imu_odom.yaw += diff_imu_yaw; + + double diff_distance = odom.twist.twist.linear.x * diff_time; + offset_imu_odom_x += diff_distance * cos(-current_pose_imu_odom.pitch) * cos(current_pose_imu_odom.yaw); + offset_imu_odom_y += diff_distance * cos(-current_pose_imu_odom.pitch) * sin(current_pose_imu_odom.yaw); + offset_imu_odom_z += diff_distance * sin(-current_pose_imu_odom.pitch); + + offset_imu_odom_roll += diff_imu_roll; + offset_imu_odom_pitch += diff_imu_pitch; + offset_imu_odom_yaw += diff_imu_yaw; + + predict_pose_imu_odom.x = previous_pose.x + offset_imu_odom_x; + predict_pose_imu_odom.y = previous_pose.y + offset_imu_odom_y; + predict_pose_imu_odom.z = previous_pose.z + offset_imu_odom_z; + predict_pose_imu_odom.roll = previous_pose.roll + offset_imu_odom_roll; + predict_pose_imu_odom.pitch = previous_pose.pitch + offset_imu_odom_pitch; + predict_pose_imu_odom.yaw = previous_pose.yaw + offset_imu_odom_yaw; + + previous_time = current_time; +} + +} //namespace ndt_matching + + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(ndt_matching::NDTMatching) \ No newline at end of file diff --git a/core_perception/lidar_localizer_ros2/src/ndt_matching_node.cpp b/core_perception/lidar_localizer_ros2/src/ndt_matching_node.cpp new file mode 100644 index 00000000000..05ff7daa330 --- /dev/null +++ b/core_perception/lidar_localizer_ros2/src/ndt_matching_node.cpp @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(rclcpp::NodeOptions()); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/core_perception/ndt_cpu/CMakeLists.txt b/core_perception/ndt_cpu/CMakeLists.txt index d5b87210544..4eea5ec3b82 100644 --- a/core_perception/ndt_cpu/CMakeLists.txt +++ b/core_perception/ndt_cpu/CMakeLists.txt @@ -1,12 +1,12 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ndt_cpu) find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +find_package(ros_environment REQUIRED) +set(ROS_VERSION $ENV{ROS_VERSION}) +find_package(PCL COMPONENTS REQUIRED) find_package(catkin REQUIRED) -find_package(PCL REQUIRED) - find_package(Eigen3 QUIET) if(NOT EIGEN3_FOUND) @@ -20,59 +20,119 @@ else() set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) endif() -catkin_package( - INCLUDE_DIRS include - LIBRARIES ndt_cpu #The exported libraries from the project - DEPENDS PCL -) +if(${ROS_VERSION} EQUAL 1) + + catkin_package( + INCLUDE_DIRS include + LIBRARIES ndt_cpu #The exported libraries from the project + DEPENDS PCL + ) + + include_directories( + "include" + ${PCL_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ) + + set(srcs + src/NormalDistributionsTransform.cpp + src/Registration.cpp + src/VoxelGrid.cpp + src/Octree.cpp + ) + + set(incs + include/ndt_cpu/debug.h + include/ndt_cpu/NormalDistributionsTransform.h + include/ndt_cpu/Registration.h + include/ndt_cpu/SymmetricEigenSolver.h + include/ndt_cpu/VoxelGrid.h + include/ndt_cpu/Octree.h + ) + + if(NOT ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")) + message(WARNING "Not building for release, performance will be slow") + + message(WARNING + "Adding 'EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT' macro to prevent ndt_matching's runtime error in debug mode. + The bug reasons and solutions are written in http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html . + This workaround was discussed on https://gitlab.com/autowarefoundation/autoware.ai/core_perception/merge_requests/57 .") + add_definitions(-DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT) + endif() + + add_library(ndt_cpu ${incs} ${srcs}) + + target_link_libraries(ndt_cpu + ${PCL_LIBRARIES} + ${catkin_LIBRARIES} + ) + + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + ) -include_directories( - "include" + install(TARGETS ndt_cpu + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +else() #ROS2 + ## Find dependencies using ament auto + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + include_directories( ${PCL_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} -) - -set(srcs - src/NormalDistributionsTransform.cpp - src/Registration.cpp - src/VoxelGrid.cpp - src/Octree.cpp -) - -set(incs - include/ndt_cpu/debug.h - include/ndt_cpu/NormalDistributionsTransform.h - include/ndt_cpu/Registration.h - include/ndt_cpu/SymmetricEigenSolver.h - include/ndt_cpu/VoxelGrid.h - include/ndt_cpu/Octree.h -) - -if(NOT ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")) - message(WARNING "Not building for release, performance will be slow") - - message(WARNING - "Adding 'EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT' macro to prevent ndt_matching's runtime error in debug mode. - The bug reasons and solutions are written in http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html . - This workaround was discussed on https://gitlab.com/autowarefoundation/autoware.ai/core_perception/merge_requests/57 .") - add_definitions(-DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT) -endif() + include + ) + + set(srcs + src/NormalDistributionsTransform.cpp + src/Registration.cpp + src/VoxelGrid.cpp + src/Octree.cpp + ) + + set(incs + include/ndt_cpu/debug.h + include/ndt_cpu/NormalDistributionsTransform.h + include/ndt_cpu/Registration.h + include/ndt_cpu/SymmetricEigenSolver.h + include/ndt_cpu/VoxelGrid.h + include/ndt_cpu/Octree.h + ) + + if(NOT ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")) + message(WARNING "Not building for release, performance will be slow") + + message(WARNING + "Adding 'EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT' macro to prevent ndt_matching's runtime error in debug mode. + The bug reasons and solutions are written in http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html . + This workaround was discussed on https://gitlab.com/autowarefoundation/autoware.ai/core_perception/merge_requests/57 .") + add_definitions(-DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT) + endif() + + ament_auto_add_library(ndt_cpu ${srcs} ${incs}) + + target_link_libraries(ndt_cpu + ${PCL_LIBRARIES} + ) -add_library(ndt_cpu ${incs} ${srcs}) + install(DIRECTORY include/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" + ) -target_link_libraries(ndt_cpu - ${PCL_LIBRARIES} - ${catkin_LIBRARIES} -) + install(TARGETS ndt_cpu + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" -) + ament_auto_package() -install(TARGETS ndt_cpu - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +endif() \ No newline at end of file diff --git a/core_perception/ndt_cpu/package.xml b/core_perception/ndt_cpu/package.xml index a5435b3bf1e..d9a977d24f5 100644 --- a/core_perception/ndt_cpu/package.xml +++ b/core_perception/ndt_cpu/package.xml @@ -1,5 +1,5 @@ - + ndt_cpu 1.12.0 The ndt_cpu package @@ -7,11 +7,16 @@ Anh Viet Nguyen Apache 2 - catkin + catkin + ament_cmake_auto libpcl-all-dev libpcl-all carma_cmake_common + + catkin + ament_cmake + diff --git a/core_perception/ndt_gpu/CMakeLists.txt b/core_perception/ndt_gpu/CMakeLists.txt index 7d5cf9deabf..acbaedb8acf 100644 --- a/core_perception/ndt_gpu/CMakeLists.txt +++ b/core_perception/ndt_gpu/CMakeLists.txt @@ -1,113 +1,212 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(ndt_gpu) find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +find_package(ros_environment REQUIRED) +set(ROS_VERSION $ENV{ROS_VERSION}) -find_package(catkin REQUIRED COMPONENTS - autoware_build_flags) -find_package(PCL REQUIRED) +find_package(autoware_build_flags REQUIRED COMPONENTS) +find_package(PCL COMPONENTS REQUIRED) find_package(CUDA) find_package(Eigen3 QUIET) if (NOT EIGEN3_FOUND) - # Fallback to cmake_modules - find_package(cmake_modules REQUIRED) - find_package(Eigen REQUIRED) - set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) - set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only - # Possibly map additional variables to the EIGEN3_ prefix. + # Fallback to cmake_modules + find_package(cmake_modules REQUIRED) + find_package(Eigen REQUIRED) + set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) + set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only + # Possibly map additional variables to the EIGEN3_ prefix. else () - set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) + set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) endif () -AW_CHECK_CUDA() +if(${ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS) + + AW_CHECK_CUDA() + + if (USE_CUDA) + set_directory_properties(PROPERTIES COMPILE_DEFINITIONS "") + + if(CMAKE_CROSSCOMPILING) + if(NOT CUDA_ARCH) + message(FATAL_ERROR "Please define the CUDA_ARCH CMake variable") + endif() + else() + if (NOT DEFINED CUDA_CAPABILITY_VERSION_CHECKER) + set(CUDA_CAPABILITY_VERSION_CHECKER + "${CATKIN_DEVEL_PREFIX}/lib/capability_version_checker") + endif () + + execute_process(COMMAND ${CUDA_CAPABILITY_VERSION_CHECKER} + OUTPUT_VARIABLE CUDA_CAPABILITY_VERSION + OUTPUT_STRIP_TRAILING_WHITESPACE) + + if ("${CUDA_CAPABILITY_VERSION}" MATCHES "^[1-9][0-9]+$") + set(CUDA_ARCH "sm_${CUDA_CAPABILITY_VERSION}") + else () + set(CUDA_ARCH "sm_52") + endif () + endif() -if (USE_CUDA) - set_directory_properties(PROPERTIES COMPILE_DEFINITIONS "") + set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-arch=${CUDA_ARCH};-std=c++14;--ptxas-options=-v) + + set(SUBSYS_DESC "Point cloud ndt gpu library") + + catkin_package( + DEPENDS PCL #Non-catkin CMake projects + INCLUDE_DIRS include #The exported include paths + LIBRARIES ndt_gpu #The exported libraries from the project + ) + + include_directories( + ${PCL_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${CUDA_INCLUDE_DIRS} + include + ${EIGEN3_INCLUDE_DIRS} + ) + + set(srcs + src/MatrixDevice.cu + src/MatrixHost.cu + src/NormalDistributionsTransform.cu + src/Registration.cu + src/VoxelGrid.cu + src/SymmetricEigenSolver.cu + ) + + set(incs + include/ndt_gpu/common.h + include/ndt_gpu/debug.h + include/ndt_gpu/Matrix.h + include/ndt_gpu/MatrixDevice.h + include/ndt_gpu/MatrixHost.h + include/ndt_gpu/NormalDistributionsTransform.h + include/ndt_gpu/Registration.h + include/ndt_gpu/SymmetricEigenSolver.h + include/ndt_gpu/VoxelGrid.h + ) + + cuda_add_library(ndt_gpu ${srcs} ${incs}) + + target_link_libraries(ndt_gpu + ${CUDA_LIBRARIES} + ${CUDA_CUBLAS_LIBRARIES} + ${CUDA_curand_LIBRARY} + ${PCL_LIBRARIES} + ) + + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + ) + + + install(TARGETS ndt_gpu + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) - if(CMAKE_CROSSCOMPILING) - if(NOT CUDA_ARCH) - message(FATAL_ERROR "Please define the CUDA_ARCH CMake variable") - endif() - else() - if (NOT DEFINED CUDA_CAPABILITY_VERSION_CHECKER) - set(CUDA_CAPABILITY_VERSION_CHECKER - "${CATKIN_DEVEL_PREFIX}/lib/capability_version_checker") + else () + message("ndt_gpu will not be built, CUDA was not found.") endif () + +else() #ROS2 + ## Find dependencies using ament auto + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + AW_CHECK_CUDA() + + if (USE_CUDA) + set_directory_properties(PROPERTIES COMPILE_DEFINITIONS "") + + if(CMAKE_CROSSCOMPILING) + if(NOT CUDA_ARCH) + message(FATAL_ERROR "Please define the CUDA_ARCH CMake variable") + endif() + else() + if (NOT DEFINED CUDA_CAPABILITY_VERSION_CHECKER) + set(CUDA_CAPABILITY_VERSION_CHECKER + "${CATKIN_DEVEL_PREFIX}/lib/capability_version_checker") + endif () + + execute_process(COMMAND ${CUDA_CAPABILITY_VERSION_CHECKER} + OUTPUT_VARIABLE CUDA_CAPABILITY_VERSION + OUTPUT_STRIP_TRAILING_WHITESPACE) + + if ("${CUDA_CAPABILITY_VERSION}" MATCHES "^[1-9][0-9]+$") + set(CUDA_ARCH "sm_${CUDA_CAPABILITY_VERSION}") + else () + set(CUDA_ARCH "sm_52") + endif () + endif() - execute_process(COMMAND ${CUDA_CAPABILITY_VERSION_CHECKER} - OUTPUT_VARIABLE CUDA_CAPABILITY_VERSION - OUTPUT_STRIP_TRAILING_WHITESPACE) + set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-arch=${CUDA_ARCH};-std=c++14;--ptxas-options=-v) + + set(SUBSYS_DESC "Point cloud ndt gpu library") + + + include_directories( + ${PCL_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${CUDA_INCLUDE_DIRS} + include + ${EIGEN3_INCLUDE_DIRS} + ) + + set(srcs + src/MatrixDevice.cu + src/MatrixHost.cu + src/NormalDistributionsTransform.cu + src/Registration.cu + src/VoxelGrid.cu + src/SymmetricEigenSolver.cu + ) + + set(incs + include/ndt_gpu/common.h + include/ndt_gpu/debug.h + include/ndt_gpu/Matrix.h + include/ndt_gpu/MatrixDevice.h + include/ndt_gpu/MatrixHost.h + include/ndt_gpu/NormalDistributionsTransform.h + include/ndt_gpu/Registration.h + include/ndt_gpu/SymmetricEigenSolver.h + include/ndt_gpu/VoxelGrid.h + ) + + cuda_add_library(ndt_gpu SHARED ${srcs} ${incs}) + + target_link_libraries(ndt_gpu + ${CUDA_LIBRARIES} + ${CUDA_CUBLAS_LIBRARIES} + ${CUDA_curand_LIBRARY} + ${PCL_LIBRARIES} + ) + + ament_export_libraries(ndt_gpu) + ament_export_include_directories(include) + + ament_auto_package() + + install(DIRECTORY include/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" + ) + + + install(TARGETS ndt_gpu + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) - if ("${CUDA_CAPABILITY_VERSION}" MATCHES "^[1-9][0-9]+$") - set(CUDA_ARCH "sm_${CUDA_CAPABILITY_VERSION}") else () - set(CUDA_ARCH "sm_52") + message("ndt_gpu will not be built, CUDA was not found.") endif () - endif() - - set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-arch=${CUDA_ARCH};-std=c++14;--ptxas-options=-v) - - set(SUBSYS_DESC "Point cloud ndt gpu library") - - catkin_package( - DEPENDS PCL #Non-catkin CMake projects - INCLUDE_DIRS include #The exported include paths - LIBRARIES ndt_gpu #The exported libraries from the project - ) - - include_directories( - ${PCL_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${CUDA_INCLUDE_DIRS} - include - ${EIGEN3_INCLUDE_DIRS} - ) - - set(srcs - src/MatrixDevice.cu - src/MatrixHost.cu - src/NormalDistributionsTransform.cu - src/Registration.cu - src/VoxelGrid.cu - src/SymmetricEigenSolver.cu - ) - - set(incs - include/ndt_gpu/common.h - include/ndt_gpu/debug.h - include/ndt_gpu/Matrix.h - include/ndt_gpu/MatrixDevice.h - include/ndt_gpu/MatrixHost.h - include/ndt_gpu/NormalDistributionsTransform.h - include/ndt_gpu/Registration.h - include/ndt_gpu/SymmetricEigenSolver.h - include/ndt_gpu/VoxelGrid.h - ) - - cuda_add_library(ndt_gpu ${srcs} ${incs}) - - target_link_libraries(ndt_gpu - ${CUDA_LIBRARIES} - ${CUDA_CUBLAS_LIBRARIES} - ${CUDA_curand_LIBRARY} - ${PCL_LIBRARIES} - ) - - install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - ) - - - install(TARGETS ndt_gpu - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) -else () - message("ndt_gpu will not be built, CUDA was not found.") -endif () +endif () \ No newline at end of file diff --git a/core_perception/ndt_gpu/package.xml b/core_perception/ndt_gpu/package.xml index a12868d0c62..bf652746feb 100644 --- a/core_perception/ndt_gpu/package.xml +++ b/core_perception/ndt_gpu/package.xml @@ -1,18 +1,24 @@ - + ndt_gpu - 1.12.0 + 4.0.0 The ndt_gpu package Yuki Kitsukawa Anh Viet Nguyen Apache 2 autoware_build_flags - catkin - + catkin + ament_cmake + + ament_auto_cmake libpcl-all-dev libpcl-all carma_cmake_common + + catkin + ament_cmake + diff --git a/core_perception/points_downsampler/CHANGELOG.rst b/core_perception/points_downsampler/CHANGELOG.rst deleted file mode 100644 index 7b503f6e2ad..00000000000 --- a/core_perception/points_downsampler/CHANGELOG.rst +++ /dev/null @@ -1,284 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package points_downsampler -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.11.0 (2019-03-21) -------------------- -* Feature/update autoware launcher (`#2056 `_) - * Add prototype of plugin file editor - * Change logger level - * Support command line options - * Update gazebo simulator panel - * Add plugin edit tool and update plugins - * Fix a bug of transform edit - * Fix setup.py to install all packages - * Update the code for compatibility with python3 - * Update the code for colcon build - * updated plugin for lane_rule, lane_stop, and voxel_grid_filter - * Fix that the plugin_description is not installed - * Fix launch file error - * Fix test failed - * Skip test of unbuilt packages - * Add run script - * Add profile description - * Fix launch file install path of point_downsampler - * Update README file - * Update README file for plugin file - * Fix README file - * Add notice to run script -* [fix] Install commands for all the packages (`#1861 `_) - * Initial fixes to detection, sensing, semantics and utils - * fixing wrong filename on install command - * Fixes to install commands - * Hokuyo fix name - * Fix obj db - * Obj db include fixes - * End of final cleaning sweep - * Incorrect command order in runtime manager - * Param tempfile not required by runtime_manager - * * Fixes to runtime manager install commands - * Remove devel directory from catkin, if any - * Updated launch files for robosense - * Updated robosense - * Fix/add missing install (`#1977 `_) - * Added launch install to lidar_kf_contour_track - * Added install to op_global_planner - * Added install to way_planner - * Added install to op_local_planner - * Added install to op_simulation_package - * Added install to op_utilities - * Added install to sync - * * Improved installation script for pointgrey packages - * Fixed nodelet error for gmsl cameras - * USe install space in catkin as well - * add install to catkin - * Fix install directives (`#1990 `_) - * Fixed installation path - * Fixed params installation path - * Fixed cfg installation path - * Delete cache on colcon_release -* Fix license notice in corresponding package.xml -* Contributors: Abraham Monrroy Cano, amc-nu, isamu-takagi - -1.10.0 (2019-01-17) -------------------- -* Switch to Apache 2 license (develop branch) (`#1741 `_) - * Switch to Apache 2 - * Replace BSD-3 license header with Apache 2 and reassign copyright to the - Autoware Foundation. - * Update license on Python files - * Update copyright years - * Add #ifndef/define _POINTS_IMAGE_H\_ - * Updated license comment -* Use colcon as the build tool (`#1704 `_) - * Switch to colcon as the build tool instead of catkin - * Added cmake-target - * Added note about the second colcon call - * Added warning about catkin* scripts being deprecated - * Fix COLCON_OPTS - * Added install targets - * Update Docker image tags - * Message packages fixes - * Fix missing dependency -* Contributors: Esteve Fernandez - -1.9.1 (2018-11-06) ------------------- - -1.9.0 (2018-10-31) ------------------- -* Moved configuration messages to autoware_config_msgs -* Contributors: Esteve Fernandez - -1.8.0 (2018-08-31) ------------------- -* [Fix] Moved C++11 flag to autoware_build_flags (`#1395 `_) -* [Feature] Makes sure that all binaries have their dependencies linked (`#1385 `_) -* [Fix] Extend and Update interface.yaml (`#1291 `_) -* Contributors: Esteve Fernandez, Kenji Funaoka - -1.7.0 (2018-05-18) ------------------- -* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst -* [fix] Fixes for all packages and dependencies (`#1240 `_) - * Initial Cleanup - * fixed also for indigo - * kf cjeck - * Fix road wizard - * Added travis ci - * Trigger CI - * Fixes to cv_tracker and lidar_tracker cmake - * Fix kitti player dependencies - * Removed unnecessary dependencies - * messages fixing for can - * Update build script travis - * Travis Path - * Travis Paths fix - * Travis test - * Eigen checks - * removed unnecessary dependencies - * Eigen Detection - * Job number reduced - * Eigen3 more fixes - * More Eigen3 - * Even more Eigen - * find package cmake modules included - * More fixes to cmake modules - * Removed non ros dependency - * Enable industrial_ci for indidog and kinetic - * Wrong install command - * fix rviz_plugin install - * FastVirtualScan fix - * Fix Qt5 Fastvirtualscan - * Fixed qt5 system dependencies for rosdep - * NDT TKU Fix catkin not pacakged - * More in detail dependencies fixes for more packages - * GLEW library for ORB - * Ignore OrbLocalizer - * Ignore Version checker - * Fix for driveworks interface - * driveworks not catkinpackagedd - * Missing catkin for driveworks - * libdpm opencv not catkin packaged - * catkin lib gnss not included in obj_db - * Points2Polygon fix - * More missing dependencies - * image viewer not packaged - * Fixed SSH2 detection, added viewers for all distros - * Fix gnss localizer incorrect dependency config - * Fixes to multiple packages dependencies - * gnss plib and package - * More fixes to gnss - * gnss dependencies for gnss_loclaizer - * Missing gnss dependency for gnss on localizer - * More fixes for dependencies - Replaced gnss for autoware_gnss_library - * gnss more fixes - * fixes to more dependencies - * header dependency - * Debug message - * more debug messages changed back to gnss - * debud messages - * gnss test - * gnss install command - * Several fixes for OpenPlanner and its lbiraries - * Fixes to ROSInterface - * More fixes to robotsdk and rosinterface - * robotsdk calibration fix - * Fixes to rosinterface robotsdk libraries and its nodes - * Fixes to Qt5 missing dependencies in robotsdk - * glviewer missing dependencies - * Missing qt specific config cmake for robotsdk - * disable cv_tracker - * Fix to open planner un needed dependendecies - * Fixes for libraries indecision maker - * Fixes to libraries decision_maker installation - * Gazebo on Kinetic - * Added Missing library - * * Removed Gazebo and synchonization packages - * Renames vmap in lane_planner - * Added installation commands for missing pakcages - * Fixes to lane_planner - * Added NDT TKU Glut extra dependencies - * ndt localizer/lib fast pcl fixes - re enable cv_tracker - * Fix kf_lib - * Keep industrial_ci - * Fixes for dpm library - * Fusion lib fixed - * dpm and fusion header should match exported project name - * Fixes to dpm_ocv ndt_localizer and pcl_omp - * no fast_pcl anymore - * fixes to libdpm and its package - * CI test - * test with native travis ci - * missing update for apt - * Fixes to pcl_omp installation and headers - * Final fixes for tests, modified README - * * Fixes to README - * Enable industrial_ci - * re enable native travis tests -* Contributors: Abraham Monrroy, Kosuke Murakami - -1.6.3 (2018-03-06) ------------------- - -1.6.2 (2018-02-27) ------------------- -* Update CHANGELOG -* Contributors: Yusuke FUJII - -1.6.1 (2018-01-20) ------------------- -* update CHANGELOG -* Contributors: Yusuke FUJII - -1.6.0 (2017-12-11) ------------------- - -1.5.1 (2017-09-25) ------------------- -* Release/1.5.1 (`#816 `_) - * fix a build error by gcc version - * fix build error for older indigo version - * update changelog for v1.5.1 - * 1.5.1 -* Contributors: Yusuke FUJII - -1.5.0 (2017-09-21) ------------------- -* Update changelog -* Contributors: Yusuke FUJII - -1.4.0 (2017-08-04) ------------------- -* version number must equal current release number so we can start releasing in the future -* added changelogs -* Contributors: Dejan Pangercic - -1.3.1 (2017-07-16) ------------------- - -1.3.0 (2017-07-14) ------------------- -* Modify measurement_range=MAX_MEASUREMENT_RANGE; (200) -* Add Error handring to removePointsByRange() -* fix initializing measurement_range - https://github.com/CPFL/Autoware/issues/693 -* https://github.com/CPFL/Autoware/issues/693 -* make the commit 14f7eca unavailable. -* Localization problem patch - https://github.com/CPFL/Autoware/issues/693 -* convert to autoware_msgs -* Contributors: YamatoAndo, andoh104 - -1.2.0 (2017-06-07) ------------------- -* not call removePointsByRange() when measurement_range is 200 - not compute sqrt -* add measurement_range - refactoring -* Contributors: yukikitsukawa - -1.1.2 (2017-02-27 23:10) ------------------------- - -1.1.1 (2017-02-27 22:25) ------------------------- - -1.1.0 (2017-02-24) ------------------- -* add tf_mapping - select points_topic in points_downsample.launch -* Contributors: yukikitsukawa - -1.0.1 (2017-01-14) ------------------- - -1.0.0 (2016-12-22) ------------------- -* Rename variables. -* Rename package name. - data_filter -> filters - points_filter -> points_downsample -* Contributors: yukikitsukawa diff --git a/core_perception/points_downsampler/CMakeLists.txt b/core_perception/points_downsampler/CMakeLists.txt index 14633848161..f97d1a0e225 100644 --- a/core_perception/points_downsampler/CMakeLists.txt +++ b/core_perception/points_downsampler/CMakeLists.txt @@ -1,73 +1,74 @@ -cmake_minimum_required(VERSION 2.8.3) +# Copyright (C) 2023 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.5) project(points_downsampler) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) +carma_package() +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -find_package(autoware_build_flags REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) -find_package(autoware_config_msgs REQUIRED) -find_package(catkin REQUIRED COMPONENTS - roscpp - pcl_ros - sensor_msgs - pcl_conversions - velodyne_pcl - message_generation - autoware_config_msgs - ) -add_message_files( - FILES - PointsDownsamplerInfo.msg +rosidl_generate_interfaces(points_downsampler + "msg/PointsDownsamplerInfo.msg" + DEPENDENCIES std_msgs ) -generate_messages( - DEPENDENCIES - std_msgs +# Includes +include_directories( + include ) -catkin_package( - CATKIN_DEPENDS - roscpp - pcl_ros - sensor_msgs - pcl_conversions - velodyne_pcl - message_runtime - autoware_config_msgs -) +ament_export_include_directories(include) ########### ## Build ## ########### -include_directories(include ${catkin_INCLUDE_DIRS} +ament_auto_add_library(random_filter SHARED + nodes/random_filter/random_filter.cpp +) +ament_auto_add_library(voxel_grid_filter SHARED + nodes/voxel_grid_filter/voxel_grid_filter.cpp ) -SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}") - -add_executable(voxel_grid_filter nodes/voxel_grid_filter/voxel_grid_filter.cpp) -add_executable(ring_filter nodes/ring_filter/ring_filter.cpp) -add_executable(distance_filter nodes/distance_filter/distance_filter.cpp) -add_executable(random_filter nodes/random_filter/random_filter.cpp) - -add_dependencies(voxel_grid_filter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(ring_filter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(distance_filter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(random_filter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(voxel_grid_filter ${catkin_LIBRARIES}) -target_link_libraries(ring_filter ${catkin_LIBRARIES}) -target_link_libraries(distance_filter ${catkin_LIBRARIES}) -target_link_libraries(random_filter ${catkin_LIBRARIES}) - -install(TARGETS voxel_grid_filter ring_filter distance_filter random_filter - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE) +ament_auto_add_executable(random_filter_exec nodes/random_filter/main.cpp) +ament_auto_add_executable(voxel_grid_filter_exec nodes/voxel_grid_filter/main.cpp) + + +rosidl_target_interfaces(random_filter ${PROJECT_NAME} "rosidl_typesupport_cpp") +rosidl_target_interfaces(voxel_grid_filter ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# Register component +rclcpp_components_register_nodes(random_filter "random_filter::RandomFilter") +rclcpp_components_register_nodes(voxel_grid_filter "voxel_grid_filter::VoxelGridFilter") + +target_link_libraries(random_filter_exec + random_filter +) + +target_link_libraries(voxel_grid_filter_exec + voxel_grid_filter +) + +ament_auto_package( + INSTALL_TO_SHARE launch +) \ No newline at end of file diff --git a/core_perception/points_downsampler/include/points_downsampler.h b/core_perception/points_downsampler/include/points_downsampler.hpp similarity index 88% rename from core_perception/points_downsampler/include/points_downsampler.h rename to core_perception/points_downsampler/include/points_downsampler.hpp index 7e70dabb08c..25168f725a2 100644 --- a/core_perception/points_downsampler/include/points_downsampler.h +++ b/core_perception/points_downsampler/include/points_downsampler.hpp @@ -8,7 +8,7 @@ static pcl::PointCloud removePointsByRange(pcl::PointCloud=max_range ) { - ROS_ERROR_ONCE("min_range>=max_range @(%lf, %lf)", min_range, max_range ); + RCLCPP_ERROR_ONCE(rclcpp::get_logger("points_downsampler"), "min_range>=max_range @(%lf, %lf)", min_range, max_range ); return scan; } #endif diff --git a/core_perception/points_downsampler/include/random_filter.hpp b/core_perception/points_downsampler/include/random_filter.hpp new file mode 100644 index 00000000000..9f0607f1862 --- /dev/null +++ b/core_perception/points_downsampler/include/random_filter.hpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#ifndef RANDOM_FILTER_H +#define RANDOM_FILTER_H + +#include +#include +#include + +#include + +#include "autoware_config_msgs/msg/config_random_filter.hpp" + +#include "points_downsampler/msg/points_downsampler_info.hpp" + +#include + +#include "points_downsampler.hpp" + +#define MAX_MEASUREMENT_RANGE 200.0 + +namespace random_filter +{ + +class RandomFilter : public carma_ros2_utils::CarmaLifecycleNode +{ +public: + explicit RandomFilter(const rclcpp::NodeOptions& options); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); + rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector ¶meters); + + void config_callback(autoware_config_msgs::msg::ConfigRandomFilter::UniquePtr input); + void scan_callback(sensor_msgs::msg::PointCloud2::UniquePtr input); + + +private: + carma_ros2_utils::PubPtr filtered_points_pub_; + carma_ros2_utils::PubPtr points_downsampler_info_pub_; + + carma_ros2_utils::SubPtr config_sub_; + carma_ros2_utils::SubPtr scan_sub_; + + int sample_num = 700; + + points_downsampler::msg::PointsDownsamplerInfo points_downsampler_info_msg; + + std::chrono::time_point filter_start, filter_end; + + bool _output_log = false; + std::ofstream ofs; + std::string filename; + + std::string POINTS_TOPIC= "filtered_points"; + double measurement_range = MAX_MEASUREMENT_RANGE; +}; + +} // namespace random_filter + +#endif // RANDOM_FILTER_H diff --git a/core_perception/points_downsampler/include/voxel_grid_filter.hpp b/core_perception/points_downsampler/include/voxel_grid_filter.hpp new file mode 100644 index 00000000000..73f5ff279a9 --- /dev/null +++ b/core_perception/points_downsampler/include/voxel_grid_filter.hpp @@ -0,0 +1,80 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#ifndef VOXEL_GRID_FILTER_H +#define VOXEL_GRID_FILTER_H + +#include +#include +#include +#include +#include + +#include "autoware_config_msgs/msg/config_voxel_grid_filter.hpp" + +#include "points_downsampler/msg/points_downsampler_info.hpp" + +#include + +#include "points_downsampler.hpp" + +#define MAX_MEASUREMENT_RANGE 200.0 + +namespace voxel_grid_filter +{ + +class VoxelGridFilter : public carma_ros2_utils::CarmaLifecycleNode +{ +public: + explicit VoxelGridFilter(const rclcpp::NodeOptions& options); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); + rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector ¶meters); + + + void config_callback(autoware_config_msgs::msg::ConfigVoxelGridFilter::UniquePtr input); + void scan_callback(sensor_msgs::msg::PointCloud2::UniquePtr input); + + +private: + carma_ros2_utils::PubPtr filtered_points_pub_; + carma_ros2_utils::PubPtr points_downsampler_info_pub_; + + carma_ros2_utils::SubPtr config_sub_; + carma_ros2_utils::SubPtr scan_sub_; + + int sample_num = 1000; + + points_downsampler::msg::PointsDownsamplerInfo points_downsampler_info_msg; + double voxel_leaf_size = 3.0; + + std::chrono::time_point filter_start, filter_end; + + bool _output_log = false; + std::ofstream ofs; + std::string filename; + + std::string POINTS_TOPIC = "points_topic"; + double measurement_range = MAX_MEASUREMENT_RANGE; +}; + +} // namespace voxel_grid_filter + +#endif // VOXEL_GRID_FILTER_H diff --git a/core_perception/points_downsampler/interface.yaml b/core_perception/points_downsampler/interface.yaml deleted file mode 100644 index c7b28922407..00000000000 --- a/core_perception/points_downsampler/interface.yaml +++ /dev/null @@ -1,12 +0,0 @@ -- name: /distance_filter - publish: [/filtered_points, /points_downsampler_info] - subscribe: [/config/distance_filter, /points_raw] -- name: /random_filter - publish: [/filtered_points, /points_downsampler_info] - subscribe: [/config/random_filter, /points_raw] -- name: /ring_filter - publish: [/filtered_points, /points_downsampler_info] - subscribe: [/config/ring_filter, /points_raw] -- name: /voxel_grid_filter - publish: [/filtered_points, /points_downsampler_info] - subscribe: [/config/voxel_grid_filter, /points_raw] diff --git a/core_perception/points_downsampler/launch/points_downsample.launch.py b/core_perception/points_downsampler/launch/points_downsample.launch.py new file mode 100644 index 00000000000..1f4d00b4e94 --- /dev/null +++ b/core_perception/points_downsampler/launch/points_downsample.launch.py @@ -0,0 +1,62 @@ +# Copyright (C) 2023 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +import ament_index_python +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + +def generate_launch_description(): + + # Declare the log_level launch argument + #log_level = LaunchConfiguration('log_level') + #declare_log_level_arg = DeclareLaunchArgument( + # name ='log_level', default_value='DEBUG') + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='points_downsampler_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='points_downsampler', + plugin='voxel_grid_filter::VoxelGridFilter', + name='voxel_grid_filter_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + # {'--log-level' : log_level } + ], + parameters=[ + #ament_index_python.get_package_share_directory('points_downsampler') + '/config/interface.yaml', + {"points_topic": "points_raw"}, + {"output_log": False}, + {"measurement_range": 200.0}, + ], + ), + ] + ) + + return LaunchDescription([ + #declare_log_level_arg, + container + ]) diff --git a/core_perception/points_downsampler/launch/points_downsample_remappable.launch b/core_perception/points_downsampler/launch/points_downsample_remappable.launch deleted file mode 100644 index af45efe6df3..00000000000 --- a/core_perception/points_downsampler/launch/points_downsample_remappable.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - diff --git a/core_perception/points_downsampler/msg/PointsDownsamplerInfo.msg b/core_perception/points_downsampler/msg/PointsDownsamplerInfo.msg index b183f446bfa..52210ff931b 100644 --- a/core_perception/points_downsampler/msg/PointsDownsamplerInfo.msg +++ b/core_perception/points_downsampler/msg/PointsDownsamplerInfo.msg @@ -1,4 +1,4 @@ -Header header +std_msgs/Header header string filter_name float32 measurement_range int32 original_points_size diff --git a/core_perception/points_downsampler/nodes/distance_filter/distance_filter.cpp b/core_perception/points_downsampler/nodes/distance_filter/distance_filter.cpp deleted file mode 100644 index 7d2cb9e2571..00000000000 --- a/core_perception/points_downsampler/nodes/distance_filter/distance_filter.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include - -#include -#include -#include - -#include "autoware_config_msgs/ConfigDistanceFilter.h" - -#include - -#include // For std::min() -#include - -#include "points_downsampler.h" - -#define MAX_MEASUREMENT_RANGE 200.0 - -ros::Publisher filtered_points_pub; - -static int sample_num = 1000; - -static ros::Publisher points_downsampler_info_pub; -static points_downsampler::PointsDownsamplerInfo points_downsampler_info_msg; - -static std::chrono::time_point filter_start, filter_end; - -static bool _output_log = false; -static std::ofstream ofs; -static std::string filename; - -static std::string POINTS_TOPIC; -static double measurement_range = MAX_MEASUREMENT_RANGE; - -static void config_callback(const autoware_config_msgs::ConfigDistanceFilter::ConstPtr& input) -{ - sample_num = input->sample_num; - measurement_range = input->measurement_range; -} - -static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) -{ - pcl::PointXYZI sampled_p; - pcl::PointCloud scan; - - pcl::fromROSMsg(*input, scan); - - if(measurement_range != MAX_MEASUREMENT_RANGE){ - scan = removePointsByRange(scan, 0, measurement_range); - } - - pcl::PointCloud::Ptr filtered_scan_ptr(new pcl::PointCloud()); - filtered_scan_ptr->header = scan.header; - - int points_num = scan.size(); - - double w_total = 0.0; - double w_step = 0.0; - int m = 0; - double c = 0.0; - - filter_start = std::chrono::system_clock::now(); - - for (pcl::PointCloud::const_iterator item = scan.begin(); item != scan.end(); item++) - { - w_total += item->x * item->x + item->y * item->y + item->z * item->z; - } - w_step = w_total / sample_num; - - pcl::PointCloud::const_iterator item = scan.begin(); - for (m = 0; m < sample_num; m++) - { - while (m * w_step > c) - { - item++; - c += item->x * item->x + item->y * item->y + item->z * item->z; - } - sampled_p.x = item->x; - sampled_p.y = item->y; - sampled_p.z = item->z; - sampled_p.intensity = item->intensity; - filtered_scan_ptr->points.push_back(sampled_p); - } - - sensor_msgs::PointCloud2 filtered_msg; - pcl::toROSMsg(*filtered_scan_ptr, filtered_msg); - - filter_end = std::chrono::system_clock::now(); - - filtered_msg.header = input->header; - filtered_points_pub.publish(filtered_msg); - - points_downsampler_info_msg.header = input->header; - points_downsampler_info_msg.filter_name = "distance_filter"; - points_downsampler_info_msg.measurement_range = measurement_range; - points_downsampler_info_msg.original_points_size = points_num; - points_downsampler_info_msg.filtered_points_size = std::min((int)filtered_scan_ptr->size(), points_num); - points_downsampler_info_msg.original_ring_size = 0; - points_downsampler_info_msg.original_ring_size = 0; - points_downsampler_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; - points_downsampler_info_pub.publish(points_downsampler_info_msg); - - if(_output_log == true){ - if(!ofs){ - std::cerr << "Could not open " << filename << "." << std::endl; - exit(1); - } - ofs << points_downsampler_info_msg.header.seq << "," - << points_downsampler_info_msg.header.stamp << "," - << points_downsampler_info_msg.header.frame_id << "," - << points_downsampler_info_msg.filter_name << "," - << points_downsampler_info_msg.original_points_size << "," - << points_downsampler_info_msg.filtered_points_size << "," - << points_downsampler_info_msg.original_ring_size << "," - << points_downsampler_info_msg.filtered_ring_size << "," - << points_downsampler_info_msg.exe_time << "," - << std::endl; - } - -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "distance_filter"); - - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - private_nh.getParam("points_topic", POINTS_TOPIC); - private_nh.getParam("output_log", _output_log); - if(_output_log == true){ - char buffer[80]; - std::time_t now = std::time(NULL); - std::tm *pnow = std::localtime(&now); - std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow); - filename = "distance_filter_" + std::string(buffer) + ".csv"; - ofs.open(filename.c_str(), std::ios::app); - } - private_nh.param("measurement_range", measurement_range, MAX_MEASUREMENT_RANGE); - - // Publishers - filtered_points_pub = nh.advertise("/filtered_points", 10); - points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); - - // Subscribers - ros::Subscriber config_sub = nh.subscribe("config/distance_filter", 10, config_callback); - ros::Subscriber scan_sub = nh.subscribe(POINTS_TOPIC, 10, scan_callback); - - ros::spin(); - - return 0; -} diff --git a/core_perception/points_downsampler/nodes/random_filter/main.cpp b/core_perception/points_downsampler/nodes/random_filter/main.cpp new file mode 100644 index 00000000000..3c1710089e3 --- /dev/null +++ b/core_perception/points_downsampler/nodes/random_filter/main.cpp @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ +#include +#include "random_filter.hpp" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(rclcpp::NodeOptions()); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/core_perception/points_downsampler/nodes/random_filter/random_filter.cpp b/core_perception/points_downsampler/nodes/random_filter/random_filter.cpp index 88fbde5b45a..ba25e7023ac 100644 --- a/core_perception/points_downsampler/nodes/random_filter/random_filter.cpp +++ b/core_perception/points_downsampler/nodes/random_filter/random_filter.cpp @@ -1,58 +1,100 @@ /* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * Copyright (C) 2023 LEIDOS. * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at * - * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. */ -#include -#include +#include "random_filter.hpp" -#include -#include +namespace random_filter +{ + +namespace std_ph = std::placeholders; + +RandomFilter::RandomFilter(const rclcpp::NodeOptions& options) : carma_ros2_utils::CarmaLifecycleNode(options) +{ + declare_parameter("points_topic", POINTS_TOPIC); + declare_parameter("output_log", _output_log); + declare_parameter("measurement_range", measurement_range); + declare_parameter("sample_num", sample_num); + +} -#include "autoware_config_msgs/ConfigRandomFilter.h" +carma_ros2_utils::CallbackReturn RandomFilter::handle_on_configure(const rclcpp_lifecycle::State &) +{ + get_parameter("points_topic", POINTS_TOPIC); + get_parameter("output_log", _output_log); + get_parameter("sample_num", sample_num); -#include + if(_output_log == true){ + char buffer[80]; + std::time_t now = std::time(NULL); + std::tm *pnow = std::localtime(&now); + std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow); + filename = "random_filter_" + std::string(buffer) + ".csv"; + ofs.open(filename.c_str(), std::ios::app); + } + measurement_range = MAX_MEASUREMENT_RANGE; + get_parameter("measurement_range", measurement_range); + + // Publishers + filtered_points_pub_ = create_publisher("random_points", 10); + points_downsampler_info_pub_ = create_publisher("points_downsampler_info", 1000); + + // Subscribers + config_sub_ = create_subscription("config/random_filter", 10, std::bind(&RandomFilter::config_callback, this, std_ph::_1)); + scan_sub_ = create_subscription(POINTS_TOPIC, 10, std::bind(&RandomFilter::scan_callback, this, std_ph::_1)); -#include + return CallbackReturn::SUCCESS; +} -#include "points_downsampler.h" +carma_ros2_utils::CallbackReturn RandomFilter::handle_on_activate(const rclcpp_lifecycle::State &) +{ + return CallbackReturn::SUCCESS; +} -#define MAX_MEASUREMENT_RANGE 200.0 +rcl_interfaces::msg::SetParametersResult RandomFilter::parameter_update_callback(const std::vector ¶meters) +{ + auto error_double = update_params({ + {"measurement_range", measurement_range}, + }, parameters); -ros::Publisher filtered_points_pub; + auto error_string = update_params({ + {"points_topic", POINTS_TOPIC} + }, parameters); -static int sample_num = 1000; + auto error_bool = update_params({ + {"output_log", _output_log}, + }, parameters); -static ros::Publisher points_downsampler_info_pub; -static points_downsampler::PointsDownsamplerInfo points_downsampler_info_msg; + auto error_int = update_params({ + {"sample_num", sample_num}, + }, parameters); -static std::chrono::time_point filter_start, filter_end; + rcl_interfaces::msg::SetParametersResult result; -static bool _output_log = false; -static std::ofstream ofs; -static std::string filename; + result.successful = !error_double && !error_string && !error_bool && !error_int; -static std::string POINTS_TOPIC; -static double measurement_range = MAX_MEASUREMENT_RANGE; + return result; +} -static void config_callback(const autoware_config_msgs::ConfigRandomFilter::ConstPtr& input) +void RandomFilter::config_callback(autoware_config_msgs::msg::ConfigRandomFilter::UniquePtr input) { sample_num = input->sample_num; measurement_range = input->measurement_range; } -static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) +void RandomFilter::scan_callback(sensor_msgs::msg::PointCloud2::UniquePtr input) { pcl::PointXYZI sampled_p; pcl::PointCloud scan; @@ -84,13 +126,13 @@ static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) filtered_scan_ptr = scan.makeShared(); } - sensor_msgs::PointCloud2 filtered_msg; + sensor_msgs::msg::PointCloud2 filtered_msg; pcl::toROSMsg(*filtered_scan_ptr, filtered_msg); filter_end = std::chrono::system_clock::now(); filtered_msg.header = input->header; - filtered_points_pub.publish(filtered_msg); + filtered_points_pub_->publish(filtered_msg); points_downsampler_info_msg.header = input->header; points_downsampler_info_msg.filter_name = "random_filter"; @@ -100,15 +142,14 @@ static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) points_downsampler_info_msg.original_ring_size = 0; points_downsampler_info_msg.filtered_ring_size = 0; points_downsampler_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; - points_downsampler_info_pub.publish(points_downsampler_info_msg); + points_downsampler_info_pub_->publish(points_downsampler_info_msg); if(_output_log == true){ if(!ofs){ std::cerr << "Could not open " << filename << "." << std::endl; exit(1); } - ofs << points_downsampler_info_msg.header.seq << "," - << points_downsampler_info_msg.header.stamp << "," + ofs << std::to_string(rclcpp::Time(points_downsampler_info_msg.header.stamp).seconds()) << "," << points_downsampler_info_msg.header.frame_id << "," << points_downsampler_info_msg.filter_name << "," << points_downsampler_info_msg.original_points_size << "," @@ -121,34 +162,11 @@ static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) } -int main(int argc, char** argv) -{ - ros::init(argc, argv, "random_filter"); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); +} // namespace random_filter - private_nh.getParam("points_topic", POINTS_TOPIC); - private_nh.getParam("output_log", _output_log); - if(_output_log == true){ - char buffer[80]; - std::time_t now = std::time(NULL); - std::tm *pnow = std::localtime(&now); - std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow); - filename = "random_filter_" + std::string(buffer) + ".csv"; - ofs.open(filename.c_str(), std::ios::app); - } - private_nh.param("measurement_range", measurement_range, MAX_MEASUREMENT_RANGE); - // Publishers - filtered_points_pub = nh.advertise("/filtered_points", 10); - points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); +#include "rclcpp_components/register_node_macro.hpp" - // Subscribers - ros::Subscriber config_sub = nh.subscribe("config/random_filter", 10, config_callback); - ros::Subscriber scan_sub = nh.subscribe(POINTS_TOPIC, 10, scan_callback); - - ros::spin(); - - return 0; -} +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(random_filter::RandomFilter) \ No newline at end of file diff --git a/core_perception/points_downsampler/nodes/ring_filter/ring_filter.cpp b/core_perception/points_downsampler/nodes/ring_filter/ring_filter.cpp deleted file mode 100644 index 03a6b520770..00000000000 --- a/core_perception/points_downsampler/nodes/ring_filter/ring_filter.cpp +++ /dev/null @@ -1,186 +0,0 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include - -#include -#include -#include - -#include - -#include "autoware_config_msgs/ConfigRingFilter.h" - -#include - -#include - -#define MAX_MEASUREMENT_RANGE 200.0 - -ros::Publisher filtered_points_pub; - -// Leaf size of VoxelGrid filter. -static double voxel_leaf_size = 2.0; - -int ring_max = 0; -int ring_div = 3; - -static ros::Publisher points_downsampler_info_pub; -static points_downsampler::PointsDownsamplerInfo points_downsampler_info_msg; - -static std::chrono::time_point filter_start, filter_end; - -static bool _output_log = false; -static std::ofstream ofs; -static std::string filename; - -static std::string POINTS_TOPIC; -static double measurement_range = MAX_MEASUREMENT_RANGE; - -static void config_callback(const autoware_config_msgs::ConfigRingFilter::ConstPtr& input) -{ - ring_div = input->ring_div; - voxel_leaf_size = input->voxel_leaf_size; - measurement_range = input->measurement_range; -} - -static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) -{ - pcl::PointCloud scan; - pcl::PointCloud tmp; - sensor_msgs::PointCloud2 filtered_msg; - - pcl::fromROSMsg(*input, scan); - pcl::fromROSMsg(*input, tmp); - - filter_start = std::chrono::system_clock::now(); - - scan.points.clear(); - - double square_measurement_range = measurement_range*measurement_range; - - for (pcl::PointCloud::const_iterator item = tmp.begin(); item != tmp.end(); item++) - { - pcl::PointXYZI p; - p.x = item->x; - p.y = item->y; - p.z = item->z; - p.intensity = item->intensity; - - double square_distance = p.x * p.x + p.y * p.y; - - if (item->ring % ring_div == 0 && square_distance <= square_measurement_range) - { - scan.points.push_back(p); - } - if (item->ring > ring_max) - { - ring_max = item->ring; - } - } - - pcl::PointCloud::Ptr scan_ptr(new pcl::PointCloud(scan)); - pcl::PointCloud::Ptr filtered_scan_ptr(new pcl::PointCloud()); - - // if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL) - if (voxel_leaf_size >= 0.1) - { - // Downsampling the velodyne scan using VoxelGrid filter - pcl::VoxelGrid voxel_grid_filter; - voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size); - voxel_grid_filter.setInputCloud(scan_ptr); - voxel_grid_filter.filter(*filtered_scan_ptr); - - pcl::toROSMsg(*filtered_scan_ptr, filtered_msg); - } - else - { - pcl::toROSMsg(*scan_ptr, filtered_msg); - } - - filter_end = std::chrono::system_clock::now(); - - filtered_msg.header = input->header; - filtered_points_pub.publish(filtered_msg); - - points_downsampler_info_msg.header = input->header; - points_downsampler_info_msg.filter_name = "ring_filter"; - points_downsampler_info_msg.measurement_range = measurement_range; - points_downsampler_info_msg.original_points_size = scan.size(); - if (voxel_leaf_size >= 0.1) - { - points_downsampler_info_msg.filtered_points_size = filtered_scan_ptr->size(); - } - else - { - points_downsampler_info_msg.filtered_points_size = scan_ptr->size(); - } - points_downsampler_info_msg.original_ring_size = ring_max; - points_downsampler_info_msg.filtered_ring_size = ring_max / ring_div; - points_downsampler_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; - points_downsampler_info_pub.publish(points_downsampler_info_msg); - - if(_output_log == true){ - if(!ofs){ - std::cerr << "Could not open " << filename << "." << std::endl; - exit(1); - } - ofs << points_downsampler_info_msg.header.seq << "," - << points_downsampler_info_msg.header.stamp << "," - << points_downsampler_info_msg.header.frame_id << "," - << points_downsampler_info_msg.filter_name << "," - << points_downsampler_info_msg.original_points_size << "," - << points_downsampler_info_msg.filtered_points_size << "," - << points_downsampler_info_msg.original_ring_size << "," - << points_downsampler_info_msg.filtered_ring_size << "," - << points_downsampler_info_msg.exe_time << "," - << std::endl; - } - -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "ring_filter"); - - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - private_nh.getParam("points_topic", POINTS_TOPIC); - private_nh.getParam("output_log", _output_log); - if(_output_log == true){ - char buffer[80]; - std::time_t now = std::time(NULL); - std::tm *pnow = std::localtime(&now); - std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow); - filename = "ring_filter_" + std::string(buffer) + ".csv"; - ofs.open(filename.c_str(), std::ios::app); - } - private_nh.param("measurement_range", measurement_range, MAX_MEASUREMENT_RANGE); - - // Publishers - filtered_points_pub = nh.advertise("/filtered_points", 10); - points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); - - // Subscribers - ros::Subscriber config_sub = nh.subscribe("config/ring_filter", 10, config_callback); - ros::Subscriber scan_sub = nh.subscribe(POINTS_TOPIC, 10, scan_callback); - - ros::spin(); - - return 0; -} diff --git a/core_perception/points_downsampler/nodes/voxel_grid_filter/main.cpp b/core_perception/points_downsampler/nodes/voxel_grid_filter/main.cpp new file mode 100644 index 00000000000..ee00620bbdd --- /dev/null +++ b/core_perception/points_downsampler/nodes/voxel_grid_filter/main.cpp @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2023 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ +#include +#include "voxel_grid_filter.hpp" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(rclcpp::NodeOptions()); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp b/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp index fb6975d4e35..948c7f77572 100644 --- a/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp +++ b/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp @@ -1,60 +1,97 @@ /* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * Copyright (C) 2023 LEIDOS. * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at * - * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. */ -#include -#include +#include "voxel_grid_filter.hpp" -#include -#include -#include +namespace voxel_grid_filter +{ + +namespace std_ph = std::placeholders; -#include "autoware_config_msgs/ConfigVoxelGridFilter.h" +VoxelGridFilter::VoxelGridFilter(const rclcpp::NodeOptions& options) : carma_ros2_utils::CarmaLifecycleNode(options) +{ + declare_parameter("points_topic", POINTS_TOPIC); + declare_parameter("output_log", _output_log); + declare_parameter("measurement_range", measurement_range); + declare_parameter("voxel_leaf_size", voxel_leaf_size); + +} -#include +carma_ros2_utils::CallbackReturn VoxelGridFilter::handle_on_configure(const rclcpp_lifecycle::State &) +{ + get_parameter("points_topic", POINTS_TOPIC); + get_parameter("output_log", _output_log); + get_parameter("voxel_leaf_size", voxel_leaf_size); -#include + if(_output_log == true){ + char buffer[80]; + std::time_t now = std::time(NULL); + std::tm *pnow = std::localtime(&now); + std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow); + filename = "voxel_grid_filter_" + std::string(buffer) + ".csv"; + ofs.open(filename.c_str(), std::ios::app); + } + measurement_range = MAX_MEASUREMENT_RANGE; + get_parameter("measurement_range", measurement_range); -#include "points_downsampler.h" + // Publishers + filtered_points_pub_ = create_publisher("filtered_points", 10); + points_downsampler_info_pub_ = create_publisher("points_downsampler_info", 1000); -#define MAX_MEASUREMENT_RANGE 200.0 + // Subscribers + config_sub_ = create_subscription("config/voxel_grid_filter", 10, std::bind(&VoxelGridFilter::config_callback, this, std_ph::_1)); + scan_sub_ = create_subscription(POINTS_TOPIC, 10, std::bind(&VoxelGridFilter::scan_callback, this, std_ph::_1)); -ros::Publisher filtered_points_pub; + return CallbackReturn::SUCCESS; +} -// Leaf size of VoxelGrid filter. -static double voxel_leaf_size = 2.0; +carma_ros2_utils::CallbackReturn VoxelGridFilter::handle_on_activate(const rclcpp_lifecycle::State &) +{ + return CallbackReturn::SUCCESS; +} -static ros::Publisher points_downsampler_info_pub; -static points_downsampler::PointsDownsamplerInfo points_downsampler_info_msg; +rcl_interfaces::msg::SetParametersResult VoxelGridFilter::parameter_update_callback(const std::vector ¶meters) +{ + auto error_double = update_params({ + {"measurement_range", measurement_range}, + {"voxel_leaf_size", voxel_leaf_size} + }, parameters); -static std::chrono::time_point filter_start, filter_end; + auto error_string = update_params({ + {"points_topic", POINTS_TOPIC} + }, parameters); -static bool _output_log = false; -static std::ofstream ofs; -static std::string filename; + auto error_bool = update_params({ + {"output_log", _output_log}, + }, parameters); -static std::string POINTS_TOPIC; -static double measurement_range = MAX_MEASUREMENT_RANGE; + rcl_interfaces::msg::SetParametersResult result; -static void config_callback(const autoware_config_msgs::ConfigVoxelGridFilter::ConstPtr& input) + result.successful = !error_double && !error_string && !error_bool; + + return result; +} + +void VoxelGridFilter::config_callback(autoware_config_msgs::msg::ConfigVoxelGridFilter::UniquePtr input) { voxel_leaf_size = input->voxel_leaf_size; measurement_range = input->measurement_range; } -static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) +void VoxelGridFilter::scan_callback(sensor_msgs::msg::PointCloud2::UniquePtr input) { pcl::PointCloud scan; pcl::fromROSMsg(*input, scan); @@ -66,7 +103,7 @@ static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) pcl::PointCloud::Ptr scan_ptr(new pcl::PointCloud(scan)); pcl::PointCloud::Ptr filtered_scan_ptr(new pcl::PointCloud()); - sensor_msgs::PointCloud2 filtered_msg; + sensor_msgs::msg::PointCloud2 filtered_msg; filter_start = std::chrono::system_clock::now(); @@ -88,7 +125,7 @@ static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) filter_end = std::chrono::system_clock::now(); filtered_msg.header = input->header; - filtered_points_pub.publish(filtered_msg); + filtered_points_pub_->publish(filtered_msg); points_downsampler_info_msg.header = input->header; points_downsampler_info_msg.filter_name = "voxel_grid_filter"; @@ -105,15 +142,14 @@ static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) points_downsampler_info_msg.original_ring_size = 0; points_downsampler_info_msg.filtered_ring_size = 0; points_downsampler_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; - points_downsampler_info_pub.publish(points_downsampler_info_msg); + points_downsampler_info_pub_->publish(points_downsampler_info_msg); if(_output_log == true){ if(!ofs){ std::cerr << "Could not open " << filename << "." << std::endl; exit(1); } - ofs << points_downsampler_info_msg.header.seq << "," - << points_downsampler_info_msg.header.stamp << "," + ofs << std::to_string(rclcpp::Time(points_downsampler_info_msg.header.stamp).seconds()) << "," << points_downsampler_info_msg.header.frame_id << "," << points_downsampler_info_msg.filter_name << "," << points_downsampler_info_msg.original_points_size << "," @@ -123,37 +159,12 @@ static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) << points_downsampler_info_msg.exe_time << "," << std::endl; } - } -int main(int argc, char** argv) -{ - ros::init(argc, argv, "voxel_grid_filter"); +} // namespace voxel_grid_filter - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - private_nh.getParam("points_topic", POINTS_TOPIC); - private_nh.getParam("output_log", _output_log); - if(_output_log == true){ - char buffer[80]; - std::time_t now = std::time(NULL); - std::tm *pnow = std::localtime(&now); - std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow); - filename = "voxel_grid_filter_" + std::string(buffer) + ".csv"; - ofs.open(filename.c_str(), std::ios::app); - } - private_nh.param("measurement_range", measurement_range, MAX_MEASUREMENT_RANGE); - - // Publishers - filtered_points_pub = nh.advertise("/filtered_points", 10); - points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); +#include "rclcpp_components/register_node_macro.hpp" - // Subscribers - ros::Subscriber config_sub = nh.subscribe("config/voxel_grid_filter", 10, config_callback); - ros::Subscriber scan_sub = nh.subscribe(POINTS_TOPIC, 10, scan_callback); - - ros::spin(); - - return 0; -} +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(voxel_grid_filter::VoxelGridFilter) \ No newline at end of file diff --git a/core_perception/points_downsampler/package.xml b/core_perception/points_downsampler/package.xml index 246d4627420..ee5ead9ad1e 100644 --- a/core_perception/points_downsampler/package.xml +++ b/core_perception/points_downsampler/package.xml @@ -1,23 +1,48 @@ - + + + points_downsampler - 1.12.0 + 4.0.0 The points_downsampler package - Yuki KITSUKAWA - Apache 2 - autoware_build_flags - catkin + carma - message_generation + Apache 2.0 + ament_cmake + carma_cmake_common + ament_auto_cmake + rosidl_default_generators + libpcl-all-dev + carma_ros2_utils autoware_config_msgs pcl_conversions pcl_ros - roscpp + rclcpp sensor_msgs - velodyne_pcl + std_msgs + + ament_lint_auto + ament_cmake_gtest + rosidl_default_runtime + rosidl_interface_packages + launch + launch_ros + + + ament_cmake + - message_runtime - carma_cmake_common diff --git a/core_planning/twist_filter/include/accel_limiter.hpp b/core_planning/twist_filter/include/accel_limiter.hpp index a8ab326b119..69a1e2ced70 100644 --- a/core_planning/twist_filter/include/accel_limiter.hpp +++ b/core_planning/twist_filter/include/accel_limiter.hpp @@ -8,10 +8,12 @@ namespace twist_filter{ constexpr double MAX_LONGITUDINAL_ACCEL_HARDCODED_LIMIT_M_S_2 = hardcoded_params::control_limits::MAX_LONGITUDINAL_ACCEL_MPS2; +constexpr double MAX_SIMULATION_LONGITUDINAL_ACCEL_HARDCODED_LIMIT_M_S_2 = hardcoded_params::control_limits::MAX_SIMULATION_LONGITUDINAL_ACCEL_MPS2; + class LongitudinalAccelLimiter { - public: + public: /** * Default constructor, not recommended for use. */ @@ -24,7 +26,7 @@ class LongitudinalAccelLimiter /** * \brief Constructor for LongitudinalAccelLimiter - * + * * \param accel_limit The acceleration limit that should be enforced, in * units of m/s/s */ @@ -32,13 +34,13 @@ class LongitudinalAccelLimiter _accel_limit(accel_limit), _initialized(false), _prev_v(0.0), - _prev_t(0.0) {}; + _prev_t(0.0) {}; /** * Limit the longitudinal acceleration found in the input ControlCommandStamped - * + * * \param msg The message to be evaluated - * \return A copy of the message with the longitudinal accel limited + * \return A copy of the message with the longitudinal accel limited * based on params or hardcoded limit */ autoware_msgs::msg::ControlCommandStamped @@ -46,9 +48,9 @@ class LongitudinalAccelLimiter /** * Limit the longitudinal acceleration found in the input TwistStamped - * + * * \param msg The message to be evaluated - * \return A copy of the message with the longitudinal accel limited + * \return A copy of the message with the longitudinal accel limited * based on params or hardcoded limit */ geometry_msgs::msg::TwistStamped @@ -58,7 +60,7 @@ class LongitudinalAccelLimiter double _accel_limit; bool _initialized; double _prev_v; - rclcpp::Time _prev_t; + rclcpp::Time _prev_t; }; } \ No newline at end of file diff --git a/core_planning/twist_filter/include/twist_filter.hpp b/core_planning/twist_filter/include/twist_filter.hpp index 1a609265163..b23d86415e5 100644 --- a/core_planning/twist_filter/include/twist_filter.hpp +++ b/core_planning/twist_filter/include/twist_filter.hpp @@ -19,7 +19,7 @@ * Modifications: * - Added limiting for longitudinal velocity per CARMA specifications. Refactored * namespacing and header as needed to support unit testing. - * - Kyle Rush + * - Kyle Rush * - 9/11/2020 * - Refactored for ros2 * - 11/16/2022 @@ -75,12 +75,12 @@ class TwistFilter : public carma_ros2_utils::CarmaLifecycleNode carma_ros2_utils::PubPtr ctrl_lacc_limit_debug_pub_, ctrl_ljerk_limit_debug_pub_; carma_ros2_utils::PubPtr twist_lacc_result_pub_, twist_ljerk_result_pub_; carma_ros2_utils::PubPtr ctrl_lacc_result_pub_, ctrl_ljerk_result_pub_; - + //subscribers carma_ros2_utils::SubPtr twist_sub_; carma_ros2_utils::SubPtr ctrl_sub_; carma_ros2_utils::SubPtr config_sub_; - + //ros params double wheel_base_; double longitudinal_velocity_limit_; diff --git a/core_planning/twist_filter/src/twist_filter.cpp b/core_planning/twist_filter/src/twist_filter.cpp index 8e9ca1e1f5b..75039ab7ba3 100644 --- a/core_planning/twist_filter/src/twist_filter.cpp +++ b/core_planning/twist_filter/src/twist_filter.cpp @@ -18,7 +18,7 @@ * Modifications: * - Added limiting for longitudinal velocity per CARMA specifications. Refactored * namespacing and header as needed to support unit testing. - * - Kyle Rush + * - Kyle Rush * - 9/11/2020 */ @@ -53,17 +53,34 @@ carma_ros2_utils::CallbackReturn TwistFilter::handle_on_configure(const rclcpp_l // parameters on private handles get_parameter("config_speed_limit", longitudinal_velocity_limit_); longitudinal_velocity_limit_ = longitudinal_velocity_limit_ * 0.44704; - + get_parameter("vehicle_acceleration_limit", longitudinal_accel_limit_); + + bool use_sim_time = false; + get_parameter("use_sim_time", use_sim_time); + + auto hardcoded_acceleration_limit = MAX_LONGITUDINAL_ACCEL_HARDCODED_LIMIT_M_S_2; + + // if simulation + if (use_sim_time) + { + hardcoded_acceleration_limit = MAX_SIMULATION_LONGITUDINAL_ACCEL_HARDCODED_LIMIT_M_S_2; + RCLCPP_INFO_STREAM(get_logger(), "Simulation mode detected. Accounting for hardcoded limit, longitudinal acceleration limit used: " << hardcoded_acceleration_limit); + } + else + { + RCLCPP_INFO_STREAM(get_logger(), "Accounting for hardcoded limit, longitudinal acceleration limit used: " << hardcoded_acceleration_limit); + } + _lon_accel_limiter = LongitudinalAccelLimiter{ - std::min(longitudinal_accel_limit_, MAX_LONGITUDINAL_ACCEL_HARDCODED_LIMIT_M_S_2)}; - + std::min(longitudinal_accel_limit_, hardcoded_acceleration_limit)}; + get_parameter("vehicle_lateral_accel_limit", lateral_accel_limit_); get_parameter("vehicle_lateral_jerk_limit", lateral_jerk_limit_); get_parameter("lowpass_gain_linear_x", lowpass_gain_linear_x_); get_parameter("lowpass_gain_angular_x", lowpass_gain_angular_z_); get_parameter("lowpass_gain_steering_angle", lowpass_gain_steering_angle_); - + //Setup subscribers twist_sub_ = create_subscription("twist_raw", 1, std::bind(&TwistFilter::TwistCmdCallback, this, std_ph::_1)); @@ -73,7 +90,7 @@ carma_ros2_utils::CallbackReturn TwistFilter::handle_on_configure(const rclcpp_l //Setup publishers twist_pub_ = create_publisher("twist_cmd", 5); ctrl_pub_ = create_publisher("ctrl_cmd", 5); - + // Publishers on private handles twist_lacc_limit_debug_pub_ = create_publisher("~/limitation_debug/twist/lateral_accel", 5); twist_ljerk_limit_debug_pub_ = create_publisher("~/limitation_debug/twist/lateral_jerk", 5); @@ -83,7 +100,7 @@ carma_ros2_utils::CallbackReturn TwistFilter::handle_on_configure(const rclcpp_l twist_ljerk_result_pub_ = create_publisher("~/result/twist/lateral_jerk", 5); ctrl_lacc_result_pub_ = create_publisher("~/result/ctrl/lateral_accel", 5); ctrl_ljerk_result_pub_ = create_publisher("~/result/ctrl/lateral_jerk", 5); - + return CallbackReturn::SUCCESS; @@ -413,4 +430,4 @@ void TwistFilter::CtrlCmdCallback( #include "rclcpp_components/register_node_macro.hpp" // Register the component with class_loader -RCLCPP_COMPONENTS_REGISTER_NODE(twist_filter::TwistFilter) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(twist_filter::TwistFilter) diff --git a/docker/checkout.bash b/docker/checkout.bash index bf1deb601f3..717b0f17400 100755 --- a/docker/checkout.bash +++ b/docker/checkout.bash @@ -39,9 +39,9 @@ if [[ "$BRANCH" = "develop" ]]; then git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch $BRANCH git clone --depth=1 https://github.com/usdot-fhwa-stol/autoware.auto.git --branch $BRANCH else - git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-msgs.git --branch carma-system-4.4.0 - git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch carma-system-4.4.0 - git clone --depth=1 https://github.com/usdot-fhwa-stol/autoware.auto.git --branch carma-system-4.4.0 + git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-msgs.git --branch carma-system-4.5.0 + git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch carma-system-4.5.0 + git clone --depth=1 https://github.com/usdot-fhwa-stol/autoware.auto.git --branch carma-system-4.5.0 fi # Required to build pacmod_msgs diff --git a/docker/install.sh b/docker/install.sh index eef77314c6a..52654c6db11 100755 --- a/docker/install.sh +++ b/docker/install.sh @@ -74,11 +74,13 @@ echo "ROS 2 Build" if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then if [[ ! -z "$ROS2_PACKAGES" ]]; then echo "Incrementally building ROS2 packages: $ROS1_PACKAGES" - colcon build --install-base /opt/autoware.ai/ros/install_ros2 --build-base build_ros2 --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-above $ROS2_PACKAGES --allow-overriding $ROS2_PACKAGES + # Build with CUDA compile option + AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --install-base /opt/autoware.ai/ros/install_ros2 --build-base build_ros2 --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-above $ROS2_PACKAGES --allow-overriding $ROS2_PACKAGES else echo "Build type is incremental but no ROS2 packages specified, skipping ROS2 build..." fi else echo "Building all ROS2 Autoware.AI Components" - colcon build --install-base /opt/autoware.ai/ros/install_ros2 --build-base build_ros2 --cmake-args -DCMAKE_BUILD_TYPE=Release + # Build with CUDA compile option + AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --install-base /opt/autoware.ai/ros/install_ros2 --build-base build_ros2 --cmake-args -DCMAKE_BUILD_TYPE=Release fi