add obstacle motion model interface

Author: chiragmakwana0296

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

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_

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 <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:
+    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_

nav2_dynamic_motion_model/package.xml

@@ -0,0 +1,18 @@
+<?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>
+
+  <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,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
+