create motion interface in runtime instead explicitly including model type

Signed-off-by: Chiragkumar Makwana <[email protected]>

Author: chiragmakwana0296

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

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_
+#endif // NAV2_DYNAMIC_MOTION_MODEL__CONSTANT_VELOCITY_MODEL_HPP_

nav2_dynamic_motion_model/package.xml

@@ -9,6 +9,13 @@
 
   <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>
 

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