update

.gitmodules

@@ -28,3 +28,6 @@
 [submodule "src/Libraries/xmlrpcpp"]
 	path = src/Libraries/xmlrpcpp
 	url = https://git.pnkr.asia/DuongTD/xmlrpcpp.git
+[submodule "src/Libraries/laser_filter"]
+	path = src/Libraries/laser_filter
+	url = https://git.pnkr.asia/DuongTD/laser_filter.git

CMakeLists.txt

@@ -74,6 +74,10 @@ if (NOT TARGET robot_nav_2d_utils)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils) 
 endif()
 
+if (NOT TARGET laser_filter)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/laser_filter) 
+endif()
+
 if (NOT TARGET robot_nav_core)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core)
 endif()

src/APIs/c_api/include/convertor.h

@@ -187,6 +187,13 @@ robot_protocol_msgs::Order convert2CppOrder(const Order& order);
  */
 Order convert2COrder(const robot_protocol_msgs::Order& cpp_order);
 
+/**
+ * @brief Check if a quaternion is valid
+ * @param q Quaternion
+ * @return True if valid, false otherwise
+ */
+bool isQuaternionValid(const Quaternion& q);
+
 /**
  * @brief Free dynamic memory held by an Order returned from convert2COrder
  * @param order Order to free (pointers and counts are zeroed)

src/APIs/c_api/src/convertor.cpp

@@ -795,4 +795,38 @@ extern "C" void order_free(Order* order)
     order->edges = nullptr;
   }
   order->edges_count = 0;
+}
+
+bool isQuaternionValid(const Quaternion& q)
+{
+  // first we need to check if the quaternion has nan's or infs
+  if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
+  {
+    robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
+    return false;
+  }
+
+  tf3::Quaternion tf_q(q.x, q.y, q.z, q.w);
+
+  // next, we need to check if the length of the quaternion is close to zero
+  if (tf_q.length2() < 1e-6)
+  {
+    robot::log_error("Quaternion has length close to zero... discarding as navigation goal");
+    return false;
+  }
+
+  // next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
+  tf_q.normalize();
+
+  tf3::Vector3 up(0, 0, 1);
+
+  double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
+
+  if (fabs(dot - 1) > 1e-3)
+  {
+    robot::log_error("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical. dot: %f", dot);
+    return false;
+  }
+
+  return true;
 }

src/APIs/c_api/src/nav_c_api.cpp

@@ -240,7 +240,11 @@ extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped go
   {
     return false;
   }
-
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
   try
   {
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
@@ -264,6 +268,11 @@ extern "C" bool navigation_move_to_order(NavigationHandle handle, const Order or
   {
     return false;
   }
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
   try
   {
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
@@ -293,6 +302,11 @@ extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const No
   {
     return false;
   }
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
 
   try
   {
@@ -301,7 +315,7 @@ extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const No
             reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
     if (!nav_ptr)
       return false;
-    robot::log_error("navigation_move_to_order goal %f %f", goal.pose.position.x, goal.pose.position.y);
+    robot::log_error("navigation_move_to_nodes_edges goal %f %f", goal.pose.position.x, goal.pose.position.y);
     Order order;
     order.nodes = const_cast<Node *>(nodes);
     order.nodes_count = node_count;
@@ -327,6 +341,11 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker,
   {
     return false;
   }
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
 
   try
   {
@@ -347,10 +366,15 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker,
 
 extern "C" bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal) 
 {
-  if (!handle)
+  if (!handle || !marker)
   {
     return false;
   }
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
   try
   {
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
@@ -379,7 +403,6 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const doubl
   {
     return false;
   }
-
   try
   {
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
@@ -411,7 +434,11 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped
   {
     return false;
   }
-
+  if (!isQuaternionValid(goal.pose.orientation))
+  {
+    robot::log_error("Goal quaternion is invalid");
+    return false;
+  }
   try
   {
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h

@@ -393,13 +393,8 @@ namespace mkt_algorithm
       double near_goal_heading_integral_;
       double near_goal_heading_last_error_;
       bool near_goal_heading_was_active_;
-
+      
-      // Regulated linear velocity scaling
-      bool use_regulated_linear_velocity_scaling_;
-
       double min_approach_linear_velocity_;
-      double regulated_linear_scaling_min_radius_;
-      double regulated_linear_scaling_min_speed_;
 
       bool use_cost_regulated_linear_velocity_scaling_;
       double inflation_cost_scaling_factor_;

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp

@@ -28,10 +28,7 @@ mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory()
     near_goal_heading_integral_(0.0),
     near_goal_heading_last_error_(0.0),
     near_goal_heading_was_active_(false),
-    use_regulated_linear_velocity_scaling_(false),
     min_approach_linear_velocity_(0.0),
-    regulated_linear_scaling_min_radius_(0.0),
-    regulated_linear_scaling_min_speed_(0.0),
     use_cost_regulated_linear_velocity_scaling_(false),
     inflation_cost_scaling_factor_(0.0),
     cost_scaling_dist_(0.0),
@@ -169,11 +166,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   if (trans_stopped_velocity_ > min_approach_linear_velocity_)
     trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.01;
 
-  // Regulated linear velocity scaling
-  nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
-  nh_priv_.param<double>("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9);
-  nh_priv_.param<double>("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25);
-
   // Inflation cost scaling (Limit velocity by proximity to obstacles)
   nh_priv_.param<bool>("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true);
   nh_priv_.param<double>("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0);
@@ -205,6 +197,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
     traj_.get()->getNodeHandle().param<double>("acc_lim_theta", acc_lim_theta_, 0.0);
     traj_.get()->getNodeHandle().param<double>("decel_lim_theta", decel_lim_theta_, 0.0);
     traj_.get()->getNodeHandle().param<double>("min_speed_xy", min_speed_xy_, 0.0);
+    if(fabs(min_speed_xy_) > sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_))
+    {
+      min_speed_xy_ = sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_);
+    }
+    if(fabs(min_speed_xy_) > sqrt(min_vel_x_ * min_vel_x_ + min_vel_y_ * min_vel_y_))
+    {
+      min_speed_xy_ = sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_);
+    }
+
     traj_.get()->getNodeHandle().param<double>("max_speed_xy", max_speed_xy_, 0.0);
   }
 }
@@ -545,7 +546,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       drive_cmd);
     }
     applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
-
     if (this->nav_stop_)
     {
       if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
@@ -578,7 +578,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     // drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
     // drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
   }
-
   result.poses.clear();
   result.poses.reserve(transformed_plan.poses.size());
   for (const auto &pose_stamped : transformed_plan.poses)
@@ -618,7 +617,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   const double L_min = 0.1;  // m, chỉnh theo nhu cầu
   double scale_close = std::clamp(L / L_min, 0.0, 1.0);
   v_target *= scale_close;
-
   const double y_abs = std::fabs(carrot_pose.pose.y);
   const double y_soft = 0.1;
   if (y_abs > y_soft) 
@@ -1196,6 +1194,7 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
     const double v_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, remaining));
     v_limit = std::min(v_limit, v_stop);
   }
+  v_limit = std::min(v_limit, fabs(v_target));
 
   return std::copysign(v_limit, sign_x);
 }

src/Navigations/Packages/move_base/CMakeLists.txt

@@ -45,6 +45,7 @@ if (NOT BUILDING_WITH_CATKIN)
     robot_nav_2d_utils
     robot_cpp
     robot_move_base_msgs
+    laser_filter
   )
   find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
 else()

src/Navigations/Packages/move_base/src/move_base.cpp

@@ -31,6 +31,7 @@
 #include <robot_sensor_msgs/PointCloud.h>
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_nav_2d_utils/conversions.h>
+#include <laser_filter/laser_filter.h>
 
 move_base::MoveBase::MoveBase()
   : initialized_(false),
@@ -499,13 +500,17 @@ robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::strin
 void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan)
 {
   auto it = laser_scans_.find(laser_scan_name);
+  laser_filter::LaserScanSOR sor;
+  sor.setMeanK(10);              // xét 10 điểm láng giềng gần nhất
+  sor.setStddevMulThresh(1.0);   // ngưỡng = mean + 1.0 * stddev
+  robot_sensor_msgs::LaserScan laser_scan_filter = sor.filter(laser_scan);
   if (it == laser_scans_.end())
   {
-    laser_scans_[laser_scan_name] = laser_scan;
+    laser_scans_[laser_scan_name] = laser_scan_filter;
   }
   else
   {
-    it->second = laser_scan;
+    it->second = laser_scan_filter;
   }
   // robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec);
   // robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str());
@@ -529,8 +534,8 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
   // }
   // robot::log_error("intensities: %s", intensities_str.str().c_str());
 
-  updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+  updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
-  updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+  updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
 }
 
 robot_sensor_msgs::LaserScan move_base::MoveBase::getLaserScan(const std::string &laser_scan_name)
@@ -2475,7 +2480,7 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
   // first we need to check if the quaternion has nan's or infs
   if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
   {
-    std::cerr << "Quaternion has nans or infs... discarding as a navigation goal" << std::endl;
+    robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
     return false;
   }
 
@@ -2484,7 +2489,7 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
   // next, we need to check if the length of the quaternion is close to zero
   if (tf_q.length2() < 1e-6)
   {
-    std::cerr << "Quaternion has length close to zero... discarding as navigation goal" << std::endl;
+    robot::log_error("Quaternion has length close to zero... discarding as navigation goal");
     return false;
   }
 
@@ -2497,7 +2502,7 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
 
   if (fabs(dot - 1) > 1e-3)
   {
-    std::cerr << "Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical." << std::endl;
+    robot::log_error("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical. dot: %f", dot);
     return false;
   }
 
@@ -2539,9 +2544,7 @@ bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_
   if (!global_pose.header.stamp.isZero() &&
       current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
   {
-    std::cerr << "Transform timeout for " << costmap->getName().c_str() << ". "
+    robot::log_error("Transform timeout for %s. Current time: %f, pose stamp: %f, tolerance: %f", costmap->getName().c_str(), current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance());
-                                                                           "Current time: "
-              << current_time.toSec() << ", pose stamp: " << global_pose.header.stamp.toSec() << ", tolerance: " << costmap->getTransformTolerance() << std::endl;
     return false;
   }