From aa6f38947016a495acaa74b44f9f2e56e2a7f664 Mon Sep 17 00:00:00 2001
From: Ethan <jiaao@student.unimelb.edu.au>
Date: Tue, 21 May 2024 11:56:45 +0800
Subject: [PATCH] contraint on kalman and image
---
catkin_ws/src/asclinic_pkg/src/Kalman.cpp | 2 +-
catkin_ws/src/asclinic_pkg/src/image.py | 2 +-
2 files changed, 2 insertions(+), 2 deletions(-)
diff --git a/catkin_ws/src/asclinic_pkg/src/Kalman.cpp b/catkin_ws/src/asclinic_pkg/src/Kalman.cpp
index cc1d6481..eb21fdf1 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))||new_pose(0)<-2 ||new_pose(1)<-4) {
+ if (std::isnan(new_pose(0)) || std::isnan(new_pose(1)) || std::isnan(new_pose(2))||new_pose(0)<-2 ||new_pose(1)<-4||std::abs(odo_position[0]-new_pose[0])>1.5||std::abs(odo_position[1]-new_pose[1]>1.5)) {
x = odo_position(0);
y = odo_position(1);
phi = odo_position(2);
diff --git a/catkin_ws/src/asclinic_pkg/src/image.py b/catkin_ws/src/asclinic_pkg/src/image.py
index 3f688618..a695e4c8 100755
--- a/catkin_ws/src/asclinic_pkg/src/image.py
+++ b/catkin_ws/src/asclinic_pkg/src/image.py
@@ -63,7 +63,7 @@ def answer_callback(msg, markers, publisher):
ry = fiducial_marker.rvec[1]
rz = fiducial_marker.rvec[2]
now_d = math.sqrt(tx**2 + tz**2)
- if now_d > 4:
+ if now_d > 3.5:
continue
valid_number+=1
# Compute rotation matrix
--
GitLab