Feature/dyn obstacle utils

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
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+
+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()

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_

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 <memory>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <nav2_dynamic_msgs/msg/obstacle.hpp>
+#include <nav2_dynamic_msgs/msg/obstacle_array.hpp>
+
+namespace nav2_dynamic_motion_model
+{
+
+class MotionModelInterface
+{
+public:
+    using Ptr = std::shared_ptr<MotionModelInterface>;
+    using ConstPtr = std::shared_ptr<const MotionModelInterface>;
+
+    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_

nav2_dynamic_motion_model/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_dynamic_motion_model</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">Chirag Makwana</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>unique_identifier_msgs</depend>
+  <depend>nav2_dynamic_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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

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
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+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()

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 <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/header.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include <nav2_dynamic_msgs/msg/obstacle.hpp>
+#include <nav2_dynamic_msgs/msg/obstacle_array.hpp>
+#include "nav2_dynamic_motion_model/motion_model_interface.hpp"
+
+#include <random>
+#include <vector>
+#include <cmath>
+
+namespace nav2_dynamic_util
+{
+    geometry_msgs::msg::Pose getObstaclePoseAt(
+        double dt, 
+        const nav2_dynamic_msgs::msg::Obstacle &obstacle, 
+        const std::shared_ptr<nav2_dynamic_motion_model::MotionModelInterface> &motion_model);
+
+    geometry_msgs::msg::PoseArray getObstaclePoseArrayAt(
+        double dt, 
+        const nav2_dynamic_msgs::msg::ObstacleArray &obstacle_array, 
+        const std::shared_ptr<nav2_dynamic_motion_model::MotionModelInterface> &motion_model);
+
+} // namespace
+#endif  // NAV2_DYNAMIC_UTIL__TRACKED_OBSTACLE_UTILS_HPP_

nav2_dynamic_util/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_dynamic_util</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">Chirag Makwana</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>unique_identifier_msgs</depend>
+  <depend>nav2_dynamic_msgs</depend>
+  <depend>nav2_dynamic_motion_model</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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<nav2_dynamic_motion_model::ConstantVelocityModel>();
+        testObstaclePrediction();
+    }
+
+private:
+    std::shared_ptr<nav2_dynamic_motion_model::MotionModelInterface> 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<TestObstacleNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}