From a13b42e8ce730bb4cf6d0625112377b92342e883 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sun, 14 Apr 2024 21:36:51 +0700 Subject: [PATCH 01/11] fix: fix load config and add logging --- src/akushon/action/node/action_manager.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/akushon/action/node/action_manager.cpp b/src/akushon/action/node/action_manager.cpp index 6919c3b..c07e07b 100644 --- a/src/akushon/action/node/action_manager.cpp +++ b/src/akushon/action/node/action_manager.cpp @@ -75,8 +75,6 @@ void ActionManager::load_config(const std::string & path) name += file_name[i]; } - // remove "/" from the start of the name string - name.erase(0, 1); try { std::ifstream file(file_name); nlohmann::json action_data = nlohmann::json::parse(file); @@ -85,8 +83,9 @@ void ActionManager::load_config(const std::string & path) actions.insert({name, action}); } catch (nlohmann::json::parse_error & ex) { - // TODO(any): will be used for logging - // std::cerr << "parse error at byte " << ex.byte << std::endl; + std::cerr << "failed to load action: " << name << std::endl; + std::cerr << "parse error at byte " << ex.byte << std::endl; + throw ex; } } } @@ -130,8 +129,7 @@ Action ActionManager::load_action( } } } catch (nlohmann::json::parse_error & ex) { - // TODO(any): will be used for logging - // std::cerr << "parse error at byte " << ex.byte << std::endl; + throw ex; } return action; From e0ceeec583dbbe2ef7726cc6263676638d304a8e Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sun, 14 Apr 2024 21:37:26 +0700 Subject: [PATCH 02/11] fix: restart pose index on next action --- src/akushon/action/process/interpolator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/akushon/action/process/interpolator.cpp b/src/akushon/action/process/interpolator.cpp index 7822e91..12e0bf5 100644 --- a/src/akushon/action/process/interpolator.cpp +++ b/src/akushon/action/process/interpolator.cpp @@ -96,6 +96,7 @@ void Interpolator::process(int time) if (current_action_index == actions.size()) { change_state(END); } else { + current_pose_index = 0; change_state(START_DELAY); } } From 8c352cdf7632bedddf6149b61338dda07d7396a5 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sun, 14 Apr 2024 21:38:06 +0700 Subject: [PATCH 03/11] refactor: run action based on argument --- src/action_main.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/action_main.cpp b/src/action_main.cpp index d09b48a..ec99a1f 100644 --- a/src/action_main.cpp +++ b/src/action_main.cpp @@ -35,9 +35,17 @@ using namespace std::chrono_literals; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); + + const char * help_message = + "Usage: ros2 run akushon action \n" + " Path to the action configuration file\n" + " Name of the action to run\n"; - if (argc < 2) { - std::cerr << "Please specify the path!" << std::endl; + std::string action_name = ""; + + if (argc < 3) { + std::cout << "Bad arguments!\n"; + std::cout << help_message; return 0; } @@ -51,7 +59,7 @@ int main(int argc, char * argv[]) rclcpp::Rate rcl_rate(8ms); int time = 0; - if (action_node->start(akushon::ActionName::WALKREADY)) { + if (action_node->start(action_name)) { while (rclcpp::ok()) { rcl_rate.sleep(); From 26079721180e01353a1b2018a731e654292cfaa4 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sun, 14 Apr 2024 21:40:48 +0700 Subject: [PATCH 04/11] refactor: remove data config --- data/action/left_kick.json | 89 ------------------------------------- data/action/right_kick.json | 89 ------------------------------------- data/action/sit_down.json | 89 ------------------------------------- data/action/walk_ready.json | 35 --------------- data/action/walkready.json | 35 --------------- 5 files changed, 337 deletions(-) delete mode 100644 data/action/left_kick.json delete mode 100644 data/action/right_kick.json delete mode 100644 data/action/sit_down.json delete mode 100644 data/action/walk_ready.json delete mode 100644 data/action/walkready.json diff --git a/data/action/left_kick.json b/data/action/left_kick.json deleted file mode 100644 index 1319cfa..0000000 --- a/data/action/left_kick.json +++ /dev/null @@ -1,89 +0,0 @@ -{ - "name": "Left Kick", - "next": "left_kick", - "poses": [ - { - "joints": { - "left_ankle_pitch": 25, - "left_ankle_roll": 0, - "left_elbow": 45, - "left_hip_pitch": 55, - "left_hip_roll": -2, - "left_hip_yaw": 0, - "left_knee": -60, - "left_shoulder_pitch": 5, - "left_shoulder_roll": -20, - "neck_pitch": -5, - "neck_yaw": 1, - "right_ankle_pitch": 25, - "right_ankle_roll": 0, - "right_elbow": 45, - "right_hip_pitch": 55, - "right_hip_roll": 2, - "right_hip_yaw": 0, - "right_knee": -60, - "right_shoulder_pitch": 5, - "right_shoulder_roll": -20 - }, - "name": "walkready", - "pause": 0, - "speed": 0.003 - }, - { - "joints": { - "left_ankle_pitch": 0, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "feet", - "pause": 1, - "speed": 0.01 - }, - { - "joints": { - "left_ankle_pitch": 0, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "kick", - "pause": 0, - "speed": 0.3 - } - ], - "start_delay": 0, - "stop_delay": 0 -} \ No newline at end of file diff --git a/data/action/right_kick.json b/data/action/right_kick.json deleted file mode 100644 index 2f3dc86..0000000 --- a/data/action/right_kick.json +++ /dev/null @@ -1,89 +0,0 @@ -{ - "name": "Right Kick", - "next": "right_kick", - "poses": [ - { - "joints": { - "left_ankle_pitch": 0, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "r_walkready", - "pause": 0, - "speed": 1 - }, - { - "joints": { - "left_ankle_pitch": 0, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "r_feet", - "pause": 1, - "speed": 0.01 - }, - { - "joints": { - "left_ankle_pitch": 0, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "r_kick", - "pause": 0, - "speed": 0.3 - } - ], - "start_delay": 0, - "stop_delay": 0 -} \ No newline at end of file diff --git a/data/action/sit_down.json b/data/action/sit_down.json deleted file mode 100644 index fc419d8..0000000 --- a/data/action/sit_down.json +++ /dev/null @@ -1,89 +0,0 @@ -{ - "name": "Sit Down", - "next": "right_kick", - "poses": [ - { - "joints": { - "left_ankle_pitch": -100, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "down", - "pause": 0, - "speed": 1 - }, - { - "joints": { - "left_ankle_pitch": 50, - "left_ankle_roll": 50, - "left_elbow": 50, - "left_hip_pitch": 50, - "left_hip_roll": 50, - "left_hip_yaw": 50, - "left_knee": 50, - "left_shoulder_pitch": 53, - "left_shoulder_roll": 51, - "neck_pitch": 52, - "neck_yaw": 51, - "right_ankle_pitch": 51, - "right_ankle_roll": 50, - "right_elbow": 50, - "right_hip_pitch": 50, - "right_hip_roll": 50, - "right_hip_yaw": 50, - "right_knee": 50, - "right_shoulder_pitch": 53, - "right_shoulder_roll": 51 - }, - "name": "sit_feet", - "pause": 1, - "speed": 0.01 - }, - { - "joints": { - "left_ankle_pitch": 0, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "sit_up", - "pause": 0, - "speed": 0.3 - } - ], - "start_delay": 0, - "stop_delay": 0 -} \ No newline at end of file diff --git a/data/action/walk_ready.json b/data/action/walk_ready.json deleted file mode 100644 index 90945ab..0000000 --- a/data/action/walk_ready.json +++ /dev/null @@ -1,35 +0,0 @@ -{ - "name": "Walk Ready", - "next": "", - "poses": [ - { - "joints": { - "left_ankle_pitch": -25, - "left_ankle_roll": 0, - "left_elbow": -45, - "left_hip_pitch": 55, - "left_hip_roll": -2, - "left_hip_yaw": 0, - "left_knee": -60, - "left_shoulder_pitch": -5, - "left_shoulder_roll": 18, - "neck_pitch": -5, - "neck_yaw": 1, - "right_ankle_pitch": 25, - "right_ankle_roll": 0, - "right_elbow": 45, - "right_hip_pitch": -55, - "right_hip_roll": 2, - "right_hip_yaw": 0, - "right_knee": 60, - "right_shoulder_pitch": 5, - "right_shoulder_roll": -20 - }, - "name": "walkready", - "pause": 0, - "speed": 0.003 - } - ], - "start_delay": 0, - "stop_delay": 0 -} \ No newline at end of file diff --git a/data/action/walkready.json b/data/action/walkready.json deleted file mode 100644 index d8f1aec..0000000 --- a/data/action/walkready.json +++ /dev/null @@ -1,35 +0,0 @@ -{ - "name": "Walkready", - "next": "", - "poses": [ - { - "joints": { - "left_ankle_pitch": -25, - "left_ankle_roll": 0, - "left_elbow": -45, - "left_hip_pitch": 55, - "left_hip_roll": -2, - "left_hip_yaw": 0, - "left_knee": -60, - "left_shoulder_pitch": -5, - "left_shoulder_roll": 20, - "neck_pitch": -5, - "neck_yaw": 1, - "right_ankle_pitch": 25, - "right_ankle_roll": 0, - "right_elbow": 45, - "right_hip_pitch": -55, - "right_hip_roll": 2, - "right_hip_yaw": 0, - "right_knee": 60, - "right_shoulder_pitch": 5, - "right_shoulder_roll": -20 - }, - "name": "walkready", - "pause": 0, - "speed": 0.0031 - } - ], - "start_delay": 0, - "stop_delay": 0 -} \ No newline at end of file From cf3c36e56982694f5970d2c85991b8a6e7b59fe8 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Wed, 17 Apr 2024 15:07:30 +0700 Subject: [PATCH 05/11] fix: fix action name typo --- src/akushon/action/model/action_name.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/akushon/action/model/action_name.cpp b/src/akushon/action/model/action_name.cpp index e955fef..8548cc8 100644 --- a/src/akushon/action/model/action_name.cpp +++ b/src/akushon/action/model/action_name.cpp @@ -41,7 +41,7 @@ const char * ActionName::WALKREADY = "walk_ready"; const char * ActionName::SIT_DOWN = "sit_down"; const char * ActionName::FORWARD_UP = "forward_up"; const char * ActionName::BACKWARD_UP = "backward_up"; -const char * ActionName::LEFTWARD_UP = "leftward_ups"; +const char * ActionName::LEFTWARD_UP = "leftward_up"; const char * ActionName::RIGHTWARD_UP = "rightward_up"; const char * ActionName::RIGHT_KICK = "right_kick"; const char * ActionName::LEFT_KICK = "left_kick"; From 1a73760a4fff625d0dfde9b699ccd45c864f8636 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Wed, 17 Apr 2024 22:19:49 +0700 Subject: [PATCH 06/11] fix: fix arg handle --- src/action_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/action_main.cpp b/src/action_main.cpp index ec99a1f..241ba0e 100644 --- a/src/action_main.cpp +++ b/src/action_main.cpp @@ -41,7 +41,6 @@ int main(int argc, char * argv[]) " Path to the action configuration file\n" " Name of the action to run\n"; - std::string action_name = ""; if (argc < 3) { std::cout << "Bad arguments!\n"; @@ -51,6 +50,7 @@ int main(int argc, char * argv[]) auto action_manager = std::make_shared(); std::string path = argv[1]; + std::string action_name = argv[2]; action_manager->load_config(path); auto node = std::make_shared("akushon_node"); From 76efcec5344938b55ddbc47b8c1df0fe3bafe064 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sat, 20 Apr 2024 01:50:27 +0700 Subject: [PATCH 07/11] fix: redundant operation --- src/akushon/action/process/interpolator.cpp | 6 +++--- src/akushon/node/akushon_node.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/akushon/action/process/interpolator.cpp b/src/akushon/action/process/interpolator.cpp index 12e0bf5..3c9acea 100644 --- a/src/akushon/action/process/interpolator.cpp +++ b/src/akushon/action/process/interpolator.cpp @@ -56,7 +56,7 @@ void Interpolator::process(int time) start_stop_time = time; } - if ((time - start_stop_time) > (get_current_action().get_start_delay() * 1000)) { + if ((time - start_stop_time) > (get_current_action().get_start_delay())) { change_state(PLAYING); } @@ -74,7 +74,7 @@ void Interpolator::process(int time) if (current_pose_index == get_current_action().get_pose_count()) { change_state(STOP_DELAY); init_pause = true; - } else if ((time - pause_time) > (get_current_pose().get_pause() * 1000)) { + } else if ((time - pause_time) > (get_current_pose().get_pause())) { next_pose(); init_pause = true; } @@ -90,7 +90,7 @@ void Interpolator::process(int time) start_stop_time = time; } - if ((time - start_stop_time) > (get_current_action().get_stop_delay() * 1000)) { + if ((time - start_stop_time) > (get_current_action().get_stop_delay())) { ++current_action_index; if (current_action_index == actions.size()) { diff --git a/src/akushon/node/akushon_node.cpp b/src/akushon/node/akushon_node.cpp index 0db0315..c23af71 100644 --- a/src/akushon/node/akushon_node.cpp +++ b/src/akushon/node/akushon_node.cpp @@ -44,7 +44,7 @@ AkushonNode::AkushonNode(rclcpp::Node::SharedPtr node) [this]() { if (this->action_node) { double time = this->node->now().seconds() - this->start_time; - this->action_node->update(time * 1000); + this->action_node->update(time); } } ); From ed5228db1dcc51f85dce0db0a7cf6cfc1e2877fe Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sat, 20 Apr 2024 01:51:11 +0700 Subject: [PATCH 08/11] fix: ensure run action when no action is running --- src/akushon/action/node/action_node.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/akushon/action/node/action_node.cpp b/src/akushon/action/node/action_node.cpp index 6a93872..f87c36f 100755 --- a/src/akushon/action/node/action_node.cpp +++ b/src/akushon/action/node/action_node.cpp @@ -73,14 +73,15 @@ ActionNode::ActionNode( run_action_subscriber = node->create_subscription( run_action_topic(), 10, [this](std::shared_ptr message) { - std::cout << message->action_name << std::endl; - if (message->control_type == RUN_ACTION_BY_NAME) { - this->start(message->action_name); - } else { - nlohmann::json action_data = nlohmann::json::parse(message->json); - Action action = this->action_manager->load_action(action_data, message->action_name); - - this->start(action); + if (!this->action_manager->is_playing()) { + if (message->control_type == RUN_ACTION_BY_NAME) { + this->start(message->action_name); + } else { + nlohmann::json action_data = nlohmann::json::parse(message->json); + Action action = this->action_manager->load_action(action_data, message->action_name); + + this->start(action); + } } }); From 03ec45e040e744902c9daec91eec9eb09e97a66a Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Sat, 20 Apr 2024 17:31:45 +0700 Subject: [PATCH 09/11] fix: fix set joint msg control type --- src/akushon/action/node/action_node.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/akushon/action/node/action_node.cpp b/src/akushon/action/node/action_node.cpp index f87c36f..2100afa 100755 --- a/src/akushon/action/node/action_node.cpp +++ b/src/akushon/action/node/action_node.cpp @@ -143,6 +143,8 @@ void ActionNode::publish_joints() joint_msgs[i].position = joints[i].get_position(); } + joints_msg.control_type = tachimawari::joint::Middleware::FOR_ACTION; + set_joints_publisher->publish(joints_msg); } From 7ef5224f59b0f7c878f2dc4ad9c3e8c5ad58ea71 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Fri, 26 Apr 2024 22:58:28 +0700 Subject: [PATCH 10/11] refactor: use cerr for error log --- src/action_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/action_main.cpp b/src/action_main.cpp index 241ba0e..dafe790 100644 --- a/src/action_main.cpp +++ b/src/action_main.cpp @@ -43,8 +43,8 @@ int main(int argc, char * argv[]) if (argc < 3) { - std::cout << "Bad arguments!\n"; - std::cout << help_message; + std::cerr << "Bad arguments!\n"; + std::cerr << help_message; return 0; } From f9633380e88431de600948a2c2a05da76bb2ec42 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Fri, 26 Apr 2024 23:00:04 +0700 Subject: [PATCH 11/11] fix: remove redundant try catch --- src/akushon/action/node/action_manager.cpp | 56 ++++++++++------------ 1 file changed, 26 insertions(+), 30 deletions(-) diff --git a/src/akushon/action/node/action_manager.cpp b/src/akushon/action/node/action_manager.cpp index c07e07b..f58e1e9 100644 --- a/src/akushon/action/node/action_manager.cpp +++ b/src/akushon/action/node/action_manager.cpp @@ -96,40 +96,36 @@ Action ActionManager::load_action( { Action action = Action(action_name); - try { - action.set_name(action_data["name"]); - - for (const auto & [key, val] : action_data.items()) { - if (key.find("poses") != std::string::npos) { - for (const auto & raw_pose : action_data["poses"]) { - { - using tachimawari::joint::JointId; - using tachimawari::joint::Joint; - - Pose pose(raw_pose["name"]); - std::vector joints; - - for (const auto & [joint_key, joint_val] : raw_pose["joints"].items()) { - Joint joint(JointId::by_name.at(joint_key), joint_val); - joints.push_back(joint); - } - - pose.set_pause(raw_pose["pause"]); - pose.set_speed(raw_pose["speed"]); - pose.set_joints(joints); - action.add_pose(pose); + action.set_name(action_data["name"]); + + for (const auto & [key, val] : action_data.items()) { + if (key.find("poses") != std::string::npos) { + for (const auto & raw_pose : action_data["poses"]) { + { + using tachimawari::joint::JointId; + using tachimawari::joint::Joint; + + Pose pose(raw_pose["name"]); + std::vector joints; + + for (const auto & [joint_key, joint_val] : raw_pose["joints"].items()) { + Joint joint(JointId::by_name.at(joint_key), joint_val); + joints.push_back(joint); } + + pose.set_pause(raw_pose["pause"]); + pose.set_speed(raw_pose["speed"]); + pose.set_joints(joints); + action.add_pose(pose); } - } else if (key == "start_delay") { - action.set_start_delay(val); - } else if (key == "stop_delay") { - action.set_stop_delay(val); - } else if (key == "next") { - action.set_next_action(val); } + } else if (key == "start_delay") { + action.set_start_delay(val); + } else if (key == "stop_delay") { + action.set_stop_delay(val); + } else if (key == "next") { + action.set_next_action(val); } - } catch (nlohmann::json::parse_error & ex) { - throw ex; } return action;