diff --git a/nav2_dynamic_motion_model/CMakeLists.txt b/nav2_dynamic_motion_model/CMakeLists.txt new file mode 100644 index 0000000..66eb585 --- /dev/null +++ b/nav2_dynamic_motion_model/CMakeLists.txt @@ -0,0 +1,69 @@ +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( + ${PROJECT_NAME} + SHARED + src/constant_velocity_model.cpp +) + +ament_target_dependencies( + ${PROJECT_NAME} + SYSTEM + rclcpp + nav2_dynamic_msgs + geometry_msgs +) + +target_include_directories(${PROJECT_NAME} + INTERFACE + "$" + "$" +) + +install(DIRECTORY include/ + DESTINATION include/${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 + # 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/${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 new file mode 100644 index 0000000..0729453 --- /dev/null +++ b/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/constant_velocity_model.hpp @@ -0,0 +1,23 @@ +#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_interface.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; + + geometry_msgs::msg::PoseArray predictObstaclePoseArray( + const nav2_dynamic_msgs::msg::ObstacleArray &obstacle_array, + double dt) const override; +}; + +} // namespace nav2_dynamic_motion_model + +#endif // NAV2_DYNAMIC_MOTION_MODEL__CONSTANT_VELOCITY_MODEL_HPP_ diff --git a/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_interface.hpp b/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_interface.hpp new file mode 100644 index 0000000..147ee69 --- /dev/null +++ b/nav2_dynamic_motion_model/include/nav2_dynamic_motion_model/motion_model_interface.hpp @@ -0,0 +1,34 @@ +#ifndef NAV2_DYNAMIC_MOTION_MODEL__MOTION_MODEL_INTERFACE_HPP_ +#define NAV2_DYNAMIC_MOTION_MODEL__MOTION_MODEL_INTERFACE_HPP_ + +#include +#include +#include +#include +#include +#include + +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 nav2_dynamic_motion_model + +#endif // NAV2_DYNAMIC_MOTION_MODEL__MOTION_MODEL_INTERFACE_HPP_ diff --git a/nav2_dynamic_motion_model/package.xml b/nav2_dynamic_motion_model/package.xml new file mode 100644 index 0000000..984094a --- /dev/null +++ b/nav2_dynamic_motion_model/package.xml @@ -0,0 +1,25 @@ + + + + nav2_dynamic_motion_model + 0.0.0 + TODO: Package description + Chirag Makwana + Apache-2.0 + + ament_cmake + + rclcpp + std_msgs + geometry_msgs + unique_identifier_msgs + nav2_dynamic_msgs + visualization_msgs + + 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..4ad1354 --- /dev/null +++ b/nav2_dynamic_motion_model/src/constant_velocity_model.cpp @@ -0,0 +1,33 @@ +#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 + 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; + + 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 diff --git a/nav2_dynamic_util/CMakeLists.txt b/nav2_dynamic_util/CMakeLists.txt new file mode 100644 index 0000000..6bf6677 --- /dev/null +++ b/nav2_dynamic_util/CMakeLists.txt @@ -0,0 +1,102 @@ +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) + +add_library( + ${PROJECT_NAME} + SHARED + src/tracked_obstacle_utils.cpp +) + +ament_target_dependencies( + ${PROJECT_NAME} + SYSTEM + rclcpp + geometry_msgs + nav2_dynamic_msgs + nav2_dynamic_motion_model +) + +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) +ament_target_dependencies(tracked_obstacle_publisher + rclcpp + nav2_dynamic_msgs + std_msgs + geometry_msgs + unique_identifier_msgs + visualization_msgs) + + +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 + # 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/${PROJECT_NAME}) +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..77d8eb9 --- /dev/null +++ b/nav2_dynamic_util/include/nav2_dynamic_util/tracked_obstacle_utils.hpp @@ -0,0 +1,29 @@ +#ifndef NAV2_DYNAMIC_UTIL__TRACKED_OBSTACLE_UTILS_HPP_ +#define NAV2_DYNAMIC_UTIL__TRACKED_OBSTACLE_UTILS_HPP_ + +#include +#include +#include +#include "geometry_msgs/msg/pose_stamped.hpp" +#include +#include +#include "nav2_dynamic_motion_model/motion_model_interface.hpp" + +#include +#include +#include + +namespace nav2_dynamic_util +{ + 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/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/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; +} 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..ff2640f --- /dev/null +++ b/nav2_dynamic_util/src/tracked_obstacle_utils.cpp @@ -0,0 +1,20 @@ +#include "nav2_dynamic_util/tracked_obstacle_utils.hpp" + +namespace nav2_dynamic_util +{ + + 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