From 4defd51e7ffc1efede38a5d77c455376ebd03d8d Mon Sep 17 00:00:00 2001
From: Junichiro Sugihara <j-sugihara@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 27 Jan 2025 20:05:08 +0900
Subject: [PATCH] [Ninja][Navigation] Add joy command to reset target com pos
 during pseudo assembly state.

---
 robots/ninja/src/ninja_navigation.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/robots/ninja/src/ninja_navigation.cpp b/robots/ninja/src/ninja_navigation.cpp
index 4ab425a44..1c555eabd 100644
--- a/robots/ninja/src/ninja_navigation.cpp
+++ b/robots/ninja/src/ninja_navigation.cpp
@@ -1161,6 +1161,11 @@ void NinjaNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
       int raw_right_cmd = joy_cmd.buttons[PS3_BUTTON_CROSS_RIGHT];
       int raw_left_cmd = joy_cmd.buttons[PS3_BUTTON_CROSS_LEFT];
       pseudo_cog_com_dist_ += (raw_right_cmd - raw_left_cmd) * pseudo_radius_change_rate_;
+
+      if(joy_cmd.buttons[PS3_BUTTON_ACTION_TRIANGLE] == 1 && joy_cmd.buttons[PS3_BUTTON_CROSS_UP] == 1)
+        {
+          setTargetCoMPoseFromCurrState();
+        }
     }
   
   BeetleNavigator::joyStickControl(copied_joy_msg);