[Add] Use generate parameter library

OUXT-Polaris · Nov 21, 2023 · 5c8090d · 5c8090d
1 parent a262494
commit 5c8090d
Show file tree

Hide file tree

Showing 5 changed files with 29 additions and 1 deletion.
diff --git a/local_waypoint_server/CMakeLists.txt b/local_waypoint_server/CMakeLists.txt
@@ -20,12 +20,18 @@ find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 find_package(Eigen3 REQUIRED)
+find_package(generate_parameter_library REQUIRED)
+
 
 include_directories(
   include
   ${EIGEN3_INCLUDE_DIR}
 )
 
+generate_parameter_library(obstracle_waypoint
+  config/obstracle_waypoint.yaml
+)
+
 add_library(local_waypoint_server_component SHARED
   src/local_waypoint_server_component.cpp)
 target_compile_definitions(local_waypoint_server_component
@@ -89,4 +95,4 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_auto_package()
+ament_auto_package()
diff --git a/local_waypoint_server/config/obstracle_waypoint.yaml b/local_waypoint_server/config/obstracle_waypoint.yaml
@@ -0,0 +1,9 @@
+obstracle_waypoint_goals:
+  obstracle_distance: {
+    type: double,
+    default_value: 1.0,
+    description: "WayPoint and obstacle distance to reject goal_pose and stop in place",
+    validation: {
+      bounds<>: [ 0.0, 10.0 ]
+    }
+  }
diff --git a/local_waypoint_server/include/local_waypoint_server/local_waypoint_server_component.hpp b/local_waypoint_server/include/local_waypoint_server/local_waypoint_server_component.hpp
@@ -59,6 +59,8 @@ extern "C" {
 }  // extern "C"
 #endif
 
+#include "obstracle_waypoint/obstracle_waypoint.hpp"
+
 #include <quaternion_operation/quaternion_operation.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
@@ -118,6 +120,8 @@ class LocalWaypointServerComponent : public rclcpp::Node
   boost::optional<double> checkCollisionToCurrentPath();
   boost::optional<double> checkCollisionToPath(const hermite_path_msgs::msg::HermitePath & path);
   std::vector<geometry_msgs::msg::Pose> getLocalWaypointCandidates(double obstacle_t);
+  std::shared_ptr<obstracle_waypoint::ParamListener> param_listener_;
+  obstracle_waypoint::Params obstracle_waypoint_params_;
   bool checkObstacleInGoal() const;
   bool checkGoalReached() const;
   boost::optional<geometry_msgs::msg::Pose> evaluateCandidates(

diff --git a/local_waypoint_server/package.xml b/local_waypoint_server/package.xml
@@ -27,6 +27,9 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ouxt_lint_common</test_depend>
 
+  <depend>generate_parameter_library</depend>
+  <depend>parameter_traits</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

diff --git a/local_waypoint_server/src/local_waypoint_server_component.cpp b/local_waypoint_server/src/local_waypoint_server_component.cpp
@@ -47,6 +47,9 @@ LocalWaypointServerComponent::LocalWaypointServerComponent(const rclcpp::NodeOpt
   declare_parameter("goal_reached_threshold", 0.3);
   get_parameter("goal_reached_threshold", goal_reached_threshold_);
   planner_status_.status = hermite_path_msgs::msg::PlannerStatus::WAITING_FOR_GOAL;
+  this->param_listener_ = std::make_shared<obstracle_waypoint::ParamListener>(
+    this->get_node_parameters_interface());
+  this->params_ = param_listener_->get_params();
   /**
    * Publishers
    */
@@ -107,6 +110,9 @@ void LocalWaypointServerComponent::updatePlannerStatus()
     }
   }
   status_pub_->publish(planner_status_);
+  RCLCPP_INFO_STREAM(
+  rclcpp::get_logger("dynamixel_hardware_interface"),
+  "Obstracle waypoint param  = " << param_);
 }
 
 void LocalWaypointServerComponent::hermitePathCallback(