From 4c6085d4e63f3fcab2bf011676b7d4bdc9e6a36c Mon Sep 17 00:00:00 2001 From: Chiragkumar Makwana Date: Mon, 10 Feb 2025 01:51:02 +0530 Subject: [PATCH 1/7] add obstacle motion model interface Signed-off-by: Chiragkumar Makwana --- nav2_dynamic_motion_model/CMakeLists.txt | 61 +++++++++++++++++++ .../constant_velocity_model.hpp | 20 ++++++ .../motion_model_base.hpp | 28 +++++++++ nav2_dynamic_motion_model/package.xml | 18 ++++++ .../src/constant_velocity_model.cpp | 26 ++++++++ 5 files changed, 153 insertions(+) create mode 100644 nav2_dynamic_motion_model/CMakeLists.txt create mode 100644 nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/constant_velocity_model.hpp create mode 100644 nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_base.hpp create mode 100644 nav2_dynamic_motion_model/package.xml create mode 100644 nav2_dynamic_motion_model/src/constant_velocity_model.cpp diff --git a/nav2_dynamic_motion_model/CMakeLists.txt b/nav2_dynamic_motion_model/CMakeLists.txt new file mode 100644 index 0000000..325a940 --- /dev/null +++ b/nav2_dynamic_motion_model/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.8) +project(nav2_dynamic_motion_model) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_dynamic_msgs REQUIRED) + + + +include_directories(include) + +add_library(nav2_dynamic_motion_model + src/constant_velocity_model.cpp +) +ament_target_dependencies(nav2_dynamic_motion_model + rclcpp + nav2_dynamic_msgs + geometry_msgs +) +# Export header files +# target_include_directories(nav2_dynamic_motion_model PUBLIC +# $ +# $ +# ) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) +# Install libraries +install( + TARGETS nav2_dynamic_motion_model + EXPORT export_nav2_dynamic_motion_model + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Export library for other packages +ament_export_include_directories(include) +ament_export_libraries(nav2_dynamic_motion_model) + +ament_package() diff --git a/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/constant_velocity_model.hpp b/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/constant_velocity_model.hpp new file mode 100644 index 0000000..8cbd721 --- /dev/null +++ b/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/constant_velocity_model.hpp @@ -0,0 +1,20 @@ +#ifndef NAV2_DYNAMIC_MOTION_MODEL__CONSTANT_VELOCITY_MODEL_HPP_ +#define NAV2_DYNAMIC_MOTION_MODEL__CONSTANT_VELOCITY_MODEL_HPP_ + +#include "nav2_dynamic_motion_model/motion_model_base.hpp" + + +namespace nav2_dynamic_motion_model +{ + +class ConstantVelocityModel : public MotionModelInterface +{ +public: + geometry_msgs::msg::Pose predictObstaclePose( + const nav2_dynamic_msgs::msg::Obstacle &obstacle, + double dt) const override; +}; + + +} //namespace +#endif //NAV2_DYNAMIC_MOTION_MODEL__CONSTANT_VELOCITY_MODEL_HPP_ \ No newline at end of file diff --git a/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_base.hpp b/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_base.hpp new file mode 100644 index 0000000..639d350 --- /dev/null +++ b/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_base.hpp @@ -0,0 +1,28 @@ +#ifndef NAV2_DYNAMIC_MOTION_MODEL__MOTION_MODEL_INTERFACE_HPP_ +#define NAV2_DYNAMIC_MOTION_MODEL__MOTION_MODEL_INTERFACE_HPP_ + +#include +#include +#include +#include +#include + +namespace nav2_dynamic_motion_model +{ + +class MotionModelInterface +{ +public: + virtual ~MotionModelInterface() = default; + + virtual geometry_msgs::msg::Pose predictObstaclePose( + const nav2_dynamic_msgs::msg::Obstacle &obstacle, + double dt) const = 0; + + virtual geometry_msgs::msg::PoseArray predictObstaclePoseArray( + const nav2_dynamic_msgs::msg::ObstacleArray &obstacle_array, + double dt) const = 0; +}; + +} //namespace +#endif //NAV2_DYNAMIC_MOTION_MODEL__MOTION_MODEL_INTERFACE_HPP_ \ No newline at end of file diff --git a/nav2_dynamic_motion_model/package.xml b/nav2_dynamic_motion_model/package.xml new file mode 100644 index 0000000..35e14c6 --- /dev/null +++ b/nav2_dynamic_motion_model/package.xml @@ -0,0 +1,18 @@ + + + + nav2_dynamic_motion_model + 0.0.0 + TODO: Package description + Chirag Makwana + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/nav2_dynamic_motion_model/src/constant_velocity_model.cpp b/nav2_dynamic_motion_model/src/constant_velocity_model.cpp new file mode 100644 index 0000000..822270d --- /dev/null +++ b/nav2_dynamic_motion_model/src/constant_velocity_model.cpp @@ -0,0 +1,26 @@ +#ifndef CONSTANT_VELOCITY_MODEL_HPP +#define CONSTANT_VELOCITY_MODEL_HPP + +#include "nav2_dynamic_motion_model/constant_velocity_model.hpp" + +namespace nav2_dynamic_motion_model +{ + +geometry_msgs::msg::Pose ConstantVelocityModel::predictObstaclePose( + const nav2_dynamic_msgs::msg::Obstacle &obstacle, + double dt) const +{ + geometry_msgs::msg::Pose predicted_pose; + predicted_pose.position.x = obstacle.position.x; + predicted_pose.position.y = obstacle.position.y; + predicted_pose.position.z = obstacle.position.z; + + predicted_pose.position.x += obstacle.velocity.x * dt; + predicted_pose.position.y += obstacle.velocity.y * dt; + predicted_pose.position.z += obstacle.velocity.z * dt; + return predicted_pose; +} + +} +#endif // CONSTANT_VELOCITY_MODEL_HPP + From a8b0ed2518bb0b79841aa34eba543a2d59a68ec7 Mon Sep 17 00:00:00 2001 From: Chiragkumar Makwana Date: Mon, 10 Feb 2025 01:51:51 +0530 Subject: [PATCH 2/7] add util pkg Signed-off-by: Chiragkumar Makwana --- nav2_dynamic_util/CMakeLists.txt | 73 +++++++++ .../tracked_obstacle_utils.hpp | 24 +++ nav2_dynamic_util/package.xml | 26 ++++ .../src/tracked_obstacle_publisher.cpp | 139 ++++++++++++++++++ .../src/tracked_obstacle_utils.cpp | 15 ++ 5 files changed, 277 insertions(+) create mode 100644 nav2_dynamic_util/CMakeLists.txt create mode 100644 nav2_dynamic_util/include/nav2_dynamic_util/tracked_obstacle_utils.hpp create mode 100644 nav2_dynamic_util/package.xml create mode 100644 nav2_dynamic_util/src/tracked_obstacle_publisher.cpp create mode 100644 nav2_dynamic_util/src/tracked_obstacle_utils.cpp diff --git a/nav2_dynamic_util/CMakeLists.txt b/nav2_dynamic_util/CMakeLists.txt new file mode 100644 index 0000000..c6ecbc4 --- /dev/null +++ b/nav2_dynamic_util/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.8) +project(nav2_dynamic_util) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(unique_identifier_msgs REQUIRED) +find_package(nav2_dynamic_msgs REQUIRED) +find_package(nav2_dynamic_motion_model REQUIRED) +find_package(visualization_msgs REQUIRED) + +include_directories(include) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +add_library(${PROJECT_NAME} SHARED + src/tracked_obstacle_utils.cpp +) +# Link dependencies +ament_target_dependencies(${PROJECT_NAME} + rclcpp + geometry_msgs + nav2_dynamic_msgs + nav2_dynamic_motion_model +) + +# Export headers +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + + +add_executable(tracked_obstacle_publisher src/tracked_obstacle_publisher.cpp) +ament_target_dependencies(tracked_obstacle_publisher + rclcpp + nav2_dynamic_msgs + std_msgs + geometry_msgs + unique_identifier_msgs + visualization_msgs) + + +install(TARGETS tracked_obstacle_publisher + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + + +# Export package +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + + +ament_package() diff --git a/nav2_dynamic_util/include/nav2_dynamic_util/tracked_obstacle_utils.hpp b/nav2_dynamic_util/include/nav2_dynamic_util/tracked_obstacle_utils.hpp new file mode 100644 index 0000000..cc6a955 --- /dev/null +++ b/nav2_dynamic_util/include/nav2_dynamic_util/tracked_obstacle_utils.hpp @@ -0,0 +1,24 @@ +#ifndef NAV2_DYNAMIC_UTIL__TRACKED_OBSTACLE_UTILS_HPP_ +#define NAV2_DYNAMIC_UTIL__TRACKED_OBSTACLE_UTILS_HPP_ + +#include +#include +#include + +#include +#include +#include "nav2_dynamic_motion_model/motion_model_base.hpp" + +#include +#include +#include + +namespace nav2_dynamic_util +{ + geometry_msgs::msg::Pose getObstaclePoseAt(const double dt, + const nav2_dynamic_msgs::msg::Obstacle obstacle, + const std::shared_ptr &motion_model); + + +} // namespace +#endif // NAV2_DYNAMIC_UTIL__TRACKED_OBSTACLE_UTILS_HPP_ \ No newline at end of file diff --git a/nav2_dynamic_util/package.xml b/nav2_dynamic_util/package.xml new file mode 100644 index 0000000..6370373 --- /dev/null +++ b/nav2_dynamic_util/package.xml @@ -0,0 +1,26 @@ + + + + nav2_dynamic_util + 0.0.0 + TODO: Package description + Chirag Makwana + Apache-2.0 + + ament_cmake + + rclcpp + std_msgs + geometry_msgs + unique_identifier_msgs + nav2_dynamic_msgs + nav2_dynamic_motion_model + visualization_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/nav2_dynamic_util/src/tracked_obstacle_publisher.cpp b/nav2_dynamic_util/src/tracked_obstacle_publisher.cpp new file mode 100644 index 0000000..0d4de80 --- /dev/null +++ b/nav2_dynamic_util/src/tracked_obstacle_publisher.cpp @@ -0,0 +1,139 @@ +#include +#include +#include +#include +#include +#include +#include "nav2_dynamic_msgs/msg/obstacle.hpp" +#include "nav2_dynamic_msgs/msg/obstacle_array.hpp" + +#include +#include +#include + +class ObstacleTracker : public rclcpp::Node { +public: + ObstacleTracker() : Node("obstacle_tracker") { + publisher_ = this->create_publisher("/tracked_obstacles", 10); + marker_publisher_ = this->create_publisher("/obstacle_markers", 10); + + timer_ = this->create_wall_timer(std::chrono::milliseconds(100), + std::bind(&ObstacleTracker::update_obstacles, this)); + initialize_waypoints(); + initialize_obstacles(); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr marker_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + std::vector obstacles_; + std::vector> waypoints_; + std::vector waypoint_indices_; + std::vector progresses_; + + void initialize_waypoints() { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution pos_dist(-10.0, 10.0); + + waypoints_.resize(4); + waypoint_indices_.resize(4, 0); + progresses_.resize(4, 0.0); + + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 5; ++j) { + geometry_msgs::msg::Point p; + p.x = pos_dist(gen); + p.y = pos_dist(gen); + p.z = 0.0; + waypoints_[i].push_back(p); + } + } + } + + void initialize_obstacles() { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution pos_dist(-10.0, 10.0); + + for (int i = 0; i < 4; ++i) { + nav2_dynamic_msgs::msg::Obstacle obs; + obs.uuid = generate_uuid(); + obs.score = 1.0; + obs.position = waypoints_[i][0]; + obs.velocity.x = 0.0; + obs.velocity.y = 0.0; + obs.velocity.z = 0.0; + obs.size.x = 1.0; + obs.size.y = 1.0; + obs.size.z = 1.0; + std::fill(std::begin(obs.position_covariance), std::end(obs.position_covariance), 0.1); + std::fill(std::begin(obs.velocity_covariance), std::end(obs.velocity_covariance), 0.05); + + obstacles_.push_back(obs); + } + } + + unique_identifier_msgs::msg::UUID generate_uuid() { + unique_identifier_msgs::msg::UUID uuid; + std::random_device rd; + for (auto &byte : uuid.uuid) { + byte = static_cast(rd()); + } + return uuid; + } + + void update_obstacles() { + if (obstacles_.empty() || waypoints_.empty()) return; + + nav2_dynamic_msgs::msg::ObstacleArray msg; + msg.header.stamp = this->now(); + msg.header.frame_id = "map"; + + visualization_msgs::msg::MarkerArray marker_array; + int id = 0; + + for (size_t i = 0; i < obstacles_.size(); ++i) { + geometry_msgs::msg::Point start = waypoints_[i][waypoint_indices_[i]]; + geometry_msgs::msg::Point end = waypoints_[i][(waypoint_indices_[i] + 1) % waypoints_[i].size()]; + + progresses_[i] += 0.02; // Adjust speed of movement + if (progresses_[i] >= 1.0) { + progresses_[i] = 0.0; + waypoint_indices_[i] = (waypoint_indices_[i] + 1) % waypoints_[i].size(); + } + + obstacles_[i].position.x = start.x + (end.x - start.x) * progresses_[i]; + obstacles_[i].position.y = start.y + (end.y - start.y) * progresses_[i]; + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = this->now(); + marker.ns = "obstacles"; + marker.id = id++; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position = obstacles_[i].position; + marker.scale.x = obstacles_[i].size.x; + marker.scale.y = obstacles_[i].size.y; + marker.scale.z = obstacles_[i].size.z; + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker_array.markers.push_back(marker); + } + + msg.obstacles = obstacles_; + publisher_->publish(msg); + marker_publisher_->publish(marker_array); + } +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/nav2_dynamic_util/src/tracked_obstacle_utils.cpp b/nav2_dynamic_util/src/tracked_obstacle_utils.cpp new file mode 100644 index 0000000..bec80e6 --- /dev/null +++ b/nav2_dynamic_util/src/tracked_obstacle_utils.cpp @@ -0,0 +1,15 @@ +#include "nav2_dynamic_util/tracked_obstacle_utils.hpp" + +namespace nav2_dynamic_util +{ + + geometry_msgs::msg::Pose getObstaclePoseAt(const double dt, + const nav2_dynamic_msgs::msg::Obstacle obstacle, + const std::shared_ptr &motion_model) + { + + geometry_msgs::msg::Pose predicted_pose = motion_model->predictObstaclePose(obstacle, dt); + return predicted_pose; + + } +} From 41a9c8ddc8dae7a744881f0f71e7600dc02e6b46 Mon Sep 17 00:00:00 2001 From: Chiragkumar Makwana Date: Tue, 11 Feb 2025 01:24:13 +0530 Subject: [PATCH 3/7] refactor Signed-off-by: Chiragkumar Makwana --- nav2_dynamic_motion_model/src/constant_velocity_model.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/nav2_dynamic_motion_model/src/constant_velocity_model.cpp b/nav2_dynamic_motion_model/src/constant_velocity_model.cpp index 822270d..618b1bc 100644 --- a/nav2_dynamic_motion_model/src/constant_velocity_model.cpp +++ b/nav2_dynamic_motion_model/src/constant_velocity_model.cpp @@ -1,6 +1,3 @@ -#ifndef CONSTANT_VELOCITY_MODEL_HPP -#define CONSTANT_VELOCITY_MODEL_HPP - #include "nav2_dynamic_motion_model/constant_velocity_model.hpp" namespace nav2_dynamic_motion_model @@ -22,5 +19,4 @@ geometry_msgs::msg::Pose ConstantVelocityModel::predictObstaclePose( } } -#endif // CONSTANT_VELOCITY_MODEL_HPP From c85d4ddd8f9b991340bcc9b86bc6c5cc702988ed Mon Sep 17 00:00:00 2001 From: Chiragkumar Makwana Date: Tue, 11 Feb 2025 21:04:38 +0530 Subject: [PATCH 4/7] add util test node Signed-off-by: Chiragkumar Makwana --- nav2_dynamic_util/src/test_util_node.cpp | 44 ++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 nav2_dynamic_util/src/test_util_node.cpp diff --git a/nav2_dynamic_util/src/test_util_node.cpp b/nav2_dynamic_util/src/test_util_node.cpp new file mode 100644 index 0000000..74965a5 --- /dev/null +++ b/nav2_dynamic_util/src/test_util_node.cpp @@ -0,0 +1,44 @@ +#include "nav2_dynamic_util/tracked_obstacle_utils.hpp" +#include "nav2_dynamic_motion_model/constant_velocity_model.hpp" + + +class TestObstacleNode : public rclcpp::Node +{ +public: + TestObstacleNode() + : Node("test_obstacle_node") + { + motion_model_ = std::make_shared(); + testObstaclePrediction(); + } + +private: + std::shared_ptr motion_model_; + + void testObstaclePrediction() + { + nav2_dynamic_msgs::msg::Obstacle obstacle; + obstacle.position.x = 1.0; + obstacle.position.y = 2.0; + obstacle.velocity.x = 0.5; + obstacle.velocity.y = 0.2; + double dt = 0.5; + double sim_time = 4.0; + + for (; dt <= sim_time; dt += 0.5) + { + geometry_msgs::msg::Pose predicted_pose = nav2_dynamic_util::getObstaclePoseAt(dt, obstacle, motion_model_); + RCLCPP_INFO(this->get_logger(), "Predicted Pose at t=%.1f: x=%.2f, y=%.2f", + dt, predicted_pose.position.x, predicted_pose.position.y); + } + } +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From 2f1e051edd8f4270650aada18810c53f082b181e Mon Sep 17 00:00:00 2001 From: Chiragkumar Makwana Date: Tue, 11 Feb 2025 21:05:15 +0530 Subject: [PATCH 5/7] refactor Signed-off-by: Chiragkumar Makwana --- ...otion_model_base.hpp => motion_model_interface.hpp} | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) rename nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/{motion_model_base.hpp => motion_model_interface.hpp} (76%) diff --git a/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_base.hpp b/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_interface.hpp similarity index 76% rename from nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_base.hpp rename to nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_interface.hpp index 639d350..147ee69 100644 --- a/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_base.hpp +++ b/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_interface.hpp @@ -1,6 +1,7 @@ #ifndef NAV2_DYNAMIC_MOTION_MODEL__MOTION_MODEL_INTERFACE_HPP_ #define NAV2_DYNAMIC_MOTION_MODEL__MOTION_MODEL_INTERFACE_HPP_ +#include #include #include #include @@ -13,16 +14,21 @@ namespace nav2_dynamic_motion_model class MotionModelInterface { public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + virtual ~MotionModelInterface() = default; virtual geometry_msgs::msg::Pose predictObstaclePose( const nav2_dynamic_msgs::msg::Obstacle &obstacle, double dt) const = 0; + virtual geometry_msgs::msg::PoseArray predictObstaclePoseArray( const nav2_dynamic_msgs::msg::ObstacleArray &obstacle_array, double dt) const = 0; }; -} //namespace -#endif //NAV2_DYNAMIC_MOTION_MODEL__MOTION_MODEL_INTERFACE_HPP_ \ No newline at end of file +} // namespace nav2_dynamic_motion_model + +#endif // NAV2_DYNAMIC_MOTION_MODEL__MOTION_MODEL_INTERFACE_HPP_ From 0e36213691c2271991ab3b8f786c6ca6936659f9 Mon Sep 17 00:00:00 2001 From: Chiragkumar Makwana Date: Tue, 11 Feb 2025 21:08:26 +0530 Subject: [PATCH 6/7] create motion interface in runtime instead explicitly including model type Signed-off-by: Chiragkumar Makwana --- nav2_dynamic_motion_model/CMakeLists.txt | 48 +++++++++++-------- .../constant_velocity_model.hpp | 11 +++-- nav2_dynamic_motion_model/package.xml | 7 +++ .../src/constant_velocity_model.cpp | 27 +++++++---- 4 files changed, 61 insertions(+), 32 deletions(-) diff --git a/nav2_dynamic_motion_model/CMakeLists.txt b/nav2_dynamic_motion_model/CMakeLists.txt index 325a940..66eb585 100644 --- a/nav2_dynamic_motion_model/CMakeLists.txt +++ b/nav2_dynamic_motion_model/CMakeLists.txt @@ -11,37 +11,40 @@ find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_dynamic_msgs REQUIRED) - - include_directories(include) -add_library(nav2_dynamic_motion_model - src/constant_velocity_model.cpp +add_library( + ${PROJECT_NAME} + SHARED + src/constant_velocity_model.cpp ) -ament_target_dependencies(nav2_dynamic_motion_model - rclcpp - nav2_dynamic_msgs - geometry_msgs + +ament_target_dependencies( + ${PROJECT_NAME} + SYSTEM + rclcpp + nav2_dynamic_msgs + geometry_msgs +) + +target_include_directories(${PROJECT_NAME} + INTERFACE + "$" + "$" ) -# Export header files -# target_include_directories(nav2_dynamic_motion_model PUBLIC -# $ -# $ -# ) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME} ) # Install libraries install( - TARGETS nav2_dynamic_motion_model - EXPORT export_nav2_dynamic_motion_model - LIBRARY DESTINATION lib + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -55,7 +58,12 @@ if(BUILD_TESTING) endif() # Export library for other packages -ament_export_include_directories(include) -ament_export_libraries(nav2_dynamic_motion_model) - +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + nav2_dynamic_msgs + geometry_msgs +) ament_package() diff --git a/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/constant_velocity_model.hpp b/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/constant_velocity_model.hpp index 8cbd721..0729453 100644 --- a/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/constant_velocity_model.hpp +++ b/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/constant_velocity_model.hpp @@ -1,8 +1,7 @@ #ifndef NAV2_DYNAMIC_MOTION_MODEL__CONSTANT_VELOCITY_MODEL_HPP_ #define NAV2_DYNAMIC_MOTION_MODEL__CONSTANT_VELOCITY_MODEL_HPP_ -#include "nav2_dynamic_motion_model/motion_model_base.hpp" - +#include "nav2_dynamic_motion_model/motion_model_interface.hpp" namespace nav2_dynamic_motion_model { @@ -13,8 +12,12 @@ class ConstantVelocityModel : public MotionModelInterface geometry_msgs::msg::Pose predictObstaclePose( const nav2_dynamic_msgs::msg::Obstacle &obstacle, double dt) const override; + + geometry_msgs::msg::PoseArray predictObstaclePoseArray( + const nav2_dynamic_msgs::msg::ObstacleArray &obstacle_array, + double dt) const override; }; +} // namespace nav2_dynamic_motion_model -} //namespace -#endif //NAV2_DYNAMIC_MOTION_MODEL__CONSTANT_VELOCITY_MODEL_HPP_ \ No newline at end of file +#endif // NAV2_DYNAMIC_MOTION_MODEL__CONSTANT_VELOCITY_MODEL_HPP_ diff --git a/nav2_dynamic_motion_model/package.xml b/nav2_dynamic_motion_model/package.xml index 35e14c6..984094a 100644 --- a/nav2_dynamic_motion_model/package.xml +++ b/nav2_dynamic_motion_model/package.xml @@ -9,6 +9,13 @@ ament_cmake + rclcpp + std_msgs + geometry_msgs + unique_identifier_msgs + nav2_dynamic_msgs + visualization_msgs + ament_lint_auto ament_lint_common diff --git a/nav2_dynamic_motion_model/src/constant_velocity_model.cpp b/nav2_dynamic_motion_model/src/constant_velocity_model.cpp index 618b1bc..4ad1354 100644 --- a/nav2_dynamic_motion_model/src/constant_velocity_model.cpp +++ b/nav2_dynamic_motion_model/src/constant_velocity_model.cpp @@ -4,19 +4,30 @@ namespace nav2_dynamic_motion_model { geometry_msgs::msg::Pose ConstantVelocityModel::predictObstaclePose( - const nav2_dynamic_msgs::msg::Obstacle &obstacle, - double dt) const + const nav2_dynamic_msgs::msg::Obstacle &obstacle, + double dt) const { geometry_msgs::msg::Pose predicted_pose; - predicted_pose.position.x = obstacle.position.x; - predicted_pose.position.y = obstacle.position.y; - predicted_pose.position.z = obstacle.position.z; + predicted_pose.position.x = obstacle.position.x + obstacle.velocity.x * dt; + predicted_pose.position.y = obstacle.position.y + obstacle.velocity.y * dt; + predicted_pose.position.z = obstacle.position.z + obstacle.velocity.z * dt; - predicted_pose.position.x += obstacle.velocity.x * dt; - predicted_pose.position.y += obstacle.velocity.y * dt; - predicted_pose.position.z += obstacle.velocity.z * dt; return predicted_pose; } +geometry_msgs::msg::PoseArray ConstantVelocityModel::predictObstaclePoseArray( + const nav2_dynamic_msgs::msg::ObstacleArray &obstacle_array, + double dt) const +{ + geometry_msgs::msg::PoseArray pose_array; + pose_array.poses.reserve(obstacle_array.obstacles.size()); + + for (const auto &obstacle : obstacle_array.obstacles) + { + pose_array.poses.push_back(predictObstaclePose(obstacle, dt)); + } + + return pose_array; } +} // namespace nav2_dynamic_motion_model From fc61c9cbb3b0c6ef1ab3c66304ed824650337ab7 Mon Sep 17 00:00:00 2001 From: Chiragkumar Makwana Date: Tue, 11 Feb 2025 21:10:29 +0530 Subject: [PATCH 7/7] use ObstacleArray and MotionModelInterface ptr args Signed-off-by: Chiragkumar Makwana --- nav2_dynamic_util/CMakeLists.txt | 55 ++++++++++++++----- .../tracked_obstacle_utils.hpp | 15 +++-- .../src/tracked_obstacle_utils.cpp | 21 ++++--- 3 files changed, 65 insertions(+), 26 deletions(-) diff --git a/nav2_dynamic_util/CMakeLists.txt b/nav2_dynamic_util/CMakeLists.txt index c6ecbc4..6bf6677 100644 --- a/nav2_dynamic_util/CMakeLists.txt +++ b/nav2_dynamic_util/CMakeLists.txt @@ -17,26 +17,27 @@ find_package(visualization_msgs REQUIRED) include_directories(include) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} +add_library( + ${PROJECT_NAME} + SHARED + src/tracked_obstacle_utils.cpp ) -add_library(${PROJECT_NAME} SHARED - src/tracked_obstacle_utils.cpp -) -# Link dependencies -ament_target_dependencies(${PROJECT_NAME} +ament_target_dependencies( + ${PROJECT_NAME} + SYSTEM rclcpp geometry_msgs nav2_dynamic_msgs nav2_dynamic_motion_model ) -# Export headers -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ +target_include_directories(${PROJECT_NAME} + INTERFACE + "$" + "$" ) +target_link_libraries(${PROJECT_NAME} nav2_dynamic_motion_model::nav2_dynamic_motion_model) add_executable(tracked_obstacle_publisher src/tracked_obstacle_publisher.cpp) @@ -49,9 +50,37 @@ ament_target_dependencies(tracked_obstacle_publisher visualization_msgs) -install(TARGETS tracked_obstacle_publisher +add_executable(test_util_node src/test_util_node.cpp) +ament_target_dependencies(test_util_node + rclcpp + std_msgs + geometry_msgs + nav2_dynamic_msgs + unique_identifier_msgs + # nav2_dynamic_motion_model + visualization_msgs) +target_link_libraries(test_util_node ${PROJECT_NAME}) + + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + + +install(TARGETS + tracked_obstacle_publisher + test_util_node DESTINATION lib/${PROJECT_NAME}) +# Install libraries +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -66,7 +95,7 @@ endif() # Export package -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${PROJECT_NAME}) diff --git a/nav2_dynamic_util/include/nav2_dynamic_util/tracked_obstacle_utils.hpp b/nav2_dynamic_util/include/nav2_dynamic_util/tracked_obstacle_utils.hpp index cc6a955..77d8eb9 100644 --- a/nav2_dynamic_util/include/nav2_dynamic_util/tracked_obstacle_utils.hpp +++ b/nav2_dynamic_util/include/nav2_dynamic_util/tracked_obstacle_utils.hpp @@ -4,10 +4,10 @@ #include #include #include - +#include "geometry_msgs/msg/pose_stamped.hpp" #include #include -#include "nav2_dynamic_motion_model/motion_model_base.hpp" +#include "nav2_dynamic_motion_model/motion_model_interface.hpp" #include #include @@ -15,10 +15,15 @@ namespace nav2_dynamic_util { - geometry_msgs::msg::Pose getObstaclePoseAt(const double dt, - const nav2_dynamic_msgs::msg::Obstacle obstacle, - const std::shared_ptr &motion_model); + geometry_msgs::msg::Pose getObstaclePoseAt( + double dt, + const nav2_dynamic_msgs::msg::Obstacle &obstacle, + const std::shared_ptr &motion_model); + geometry_msgs::msg::PoseArray getObstaclePoseArrayAt( + double dt, + const nav2_dynamic_msgs::msg::ObstacleArray &obstacle_array, + const std::shared_ptr &motion_model); } // namespace #endif // NAV2_DYNAMIC_UTIL__TRACKED_OBSTACLE_UTILS_HPP_ \ No newline at end of file diff --git a/nav2_dynamic_util/src/tracked_obstacle_utils.cpp b/nav2_dynamic_util/src/tracked_obstacle_utils.cpp index bec80e6..ff2640f 100644 --- a/nav2_dynamic_util/src/tracked_obstacle_utils.cpp +++ b/nav2_dynamic_util/src/tracked_obstacle_utils.cpp @@ -3,13 +3,18 @@ namespace nav2_dynamic_util { - geometry_msgs::msg::Pose getObstaclePoseAt(const double dt, - const nav2_dynamic_msgs::msg::Obstacle obstacle, - const std::shared_ptr &motion_model) - { - - geometry_msgs::msg::Pose predicted_pose = motion_model->predictObstaclePose(obstacle, dt); - return predicted_pose; + geometry_msgs::msg::Pose getObstaclePoseAt( + double dt, + const nav2_dynamic_msgs::msg::Obstacle &obstacle, + const std::shared_ptr &motion_model){ + return motion_model->predictObstaclePose(obstacle, dt); + } + geometry_msgs::msg::PoseArray getObstaclePoseArrayAt( + double dt, + const nav2_dynamic_msgs::msg::ObstacleArray &obstacle_array, + const std::shared_ptr &motion_model){ + return motion_model->predictObstaclePoseArray(obstacle_array, dt); } -} + +} // namespace nav2_dynamic_util