From 89ca687668e85b9e4cafb2dab4b47480bc629a49 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sun, 12 Jan 2025 16:42:19 +0900 Subject: [PATCH 1/4] [aerial_robot_estimation] same yaw direction as 326 world --- aerial_robot_estimation/src/sensor/vo.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/aerial_robot_estimation/src/sensor/vo.cpp b/aerial_robot_estimation/src/sensor/vo.cpp index 3e156dc37..89773e6ad 100644 --- a/aerial_robot_estimation/src/sensor/vo.cpp +++ b/aerial_robot_estimation/src/sensor/vo.cpp @@ -263,7 +263,8 @@ namespace sensor_plugin tf::Transform vo_bdash_f = raw_sensor_tf * sensor_tf_.inverse(); // ^{vo}H_{b} double r,p,y; vo_bdash_f.getBasis().getRPY(r,p,y); - vo_bdash_f.setRotation(tf::createQuaternionFromYaw(y)); // ^{vo}H_{b'} + /** set same yaw direction as 326 world**/ + vo_bdash_f.setRotation(tf::createQuaternionFromYaw(y+2.4)); // ^{vo}H_{b'} /** step3: ^{w}H_{vo} = ^{w}H_{b'} * ^{b'}H_{vo} **/ world_offset_tf_ = w_bdash_f * vo_bdash_f.inverse(); From 07713d01eb349d68dc9f3a7bb611f08ecdb1a1a9 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sun, 12 Jan 2025 17:49:24 +0900 Subject: [PATCH 2/4] [sensor] remake sensor increment variable from real flight experiment --- aerial_robot_estimation/src/sensor/vo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aerial_robot_estimation/src/sensor/vo.cpp b/aerial_robot_estimation/src/sensor/vo.cpp index 89773e6ad..ba609be66 100644 --- a/aerial_robot_estimation/src/sensor/vo.cpp +++ b/aerial_robot_estimation/src/sensor/vo.cpp @@ -264,7 +264,7 @@ namespace sensor_plugin double r,p,y; vo_bdash_f.getBasis().getRPY(r,p,y); /** set same yaw direction as 326 world**/ - vo_bdash_f.setRotation(tf::createQuaternionFromYaw(y+2.4)); // ^{vo}H_{b'} + vo_bdash_f.setRotation(tf::createQuaternionFromYaw(y+2.489)); // ^{vo}H_{b'} /** step3: ^{w}H_{vo} = ^{w}H_{b'} * ^{b'}H_{vo} **/ world_offset_tf_ = w_bdash_f * vo_bdash_f.inverse(); From 535c3fba261981ee5a81373791c4327bfda2fb35 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 13 Jan 2025 20:33:03 +0900 Subject: [PATCH 3/4] [aerial_robot_estimation] remake hardcoding to result of current position --- aerial_robot_estimation/src/sensor/vo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aerial_robot_estimation/src/sensor/vo.cpp b/aerial_robot_estimation/src/sensor/vo.cpp index ba609be66..d1ecb9bfd 100644 --- a/aerial_robot_estimation/src/sensor/vo.cpp +++ b/aerial_robot_estimation/src/sensor/vo.cpp @@ -264,7 +264,7 @@ namespace sensor_plugin double r,p,y; vo_bdash_f.getBasis().getRPY(r,p,y); /** set same yaw direction as 326 world**/ - vo_bdash_f.setRotation(tf::createQuaternionFromYaw(y+2.489)); // ^{vo}H_{b'} + vo_bdash_f.setRotation(tf::createQuaternionFromYaw(y+estimator_->getState(State::YAW_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)[0])); // ^{vo}H_{b'} /** step3: ^{w}H_{vo} = ^{w}H_{b'} * ^{b'}H_{vo} **/ world_offset_tf_ = w_bdash_f * vo_bdash_f.inverse(); From 7ad2fd4c9eb561fad4394f97184f835ff7f8f6ce Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 13 Jan 2025 21:11:20 +0900 Subject: [PATCH 4/4] [agile_multirotor] transform initial position of yaw direction --- aerial_robot_estimation/src/sensor/vo.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/aerial_robot_estimation/src/sensor/vo.cpp b/aerial_robot_estimation/src/sensor/vo.cpp index d1ecb9bfd..4f014bf5d 100644 --- a/aerial_robot_estimation/src/sensor/vo.cpp +++ b/aerial_robot_estimation/src/sensor/vo.cpp @@ -242,7 +242,7 @@ namespace sensor_plugin } /** step1: ^{w}H_{b'}, b': level frame of b **/ - tf::Transform w_bdash_f(tf::createQuaternionFromYaw(estimator_->getState(State::YAW_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)[0])); + tf::Transform w_bdash_f(tf::createQuaternionFromYaw(0.0)); tf::Vector3 baselink_pos = estimator_->getPos(Frame::BASELINK, aerial_robot_estimation::EGOMOTION_ESTIMATE); if(estimator_->getStateStatus(State::X_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)) @@ -263,8 +263,7 @@ namespace sensor_plugin tf::Transform vo_bdash_f = raw_sensor_tf * sensor_tf_.inverse(); // ^{vo}H_{b} double r,p,y; vo_bdash_f.getBasis().getRPY(r,p,y); - /** set same yaw direction as 326 world**/ - vo_bdash_f.setRotation(tf::createQuaternionFromYaw(y+estimator_->getState(State::YAW_BASE, aerial_robot_estimation::EGOMOTION_ESTIMATE)[0])); // ^{vo}H_{b'} + vo_bdash_f.setRotation(tf::createQuaternionFromYaw(y)); // ^{vo}H_{b'} /** step3: ^{w}H_{vo} = ^{w}H_{b'} * ^{b'}H_{vo} **/ world_offset_tf_ = w_bdash_f * vo_bdash_f.inverse();