From 71d5e1e3d30b247fc829127461d79fadc2a9cf16 Mon Sep 17 00:00:00 2001
From: Ethan <jiaao@student.unimelb.edu.au>
Date: Tue, 21 May 2024 11:43:42 +0800
Subject: [PATCH] limit to kalman
---
catkin_ws/src/asclinic_pkg/src/Kalman.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/catkin_ws/src/asclinic_pkg/src/Kalman.cpp b/catkin_ws/src/asclinic_pkg/src/Kalman.cpp
index 75c700d0..cc1d6481 100644
--- a/catkin_ws/src/asclinic_pkg/src/Kalman.cpp
+++ b/catkin_ws/src/asclinic_pkg/src/Kalman.cpp
@@ -149,7 +149,7 @@ void ImageAnswer(const asclinic_pkg::position& msg) {
//ROS_INFO_STREAM("new_pose: " << new_pose);
- if (std::isnan(new_pose(0)) || std::isnan(new_pose(1)) || std::isnan(new_pose(2))) {
+ if (std::isnan(new_pose(0)) || std::isnan(new_pose(1)) || std::isnan(new_pose(2))||new_pose(0)<-2 ||new_pose(1)<-4) {
x = odo_position(0);
y = odo_position(1);
phi = odo_position(2);
--
GitLab