From 895a1166f9d2641b5b69409096e32ebf019c5f0a Mon Sep 17 00:00:00 2001 From: maroqijalil Date: Sat, 20 May 2023 17:03:57 +0700 Subject: [PATCH 1/3] feat: add akushon status publish --- include/akushon/action/node/action_node.hpp | 18 ++---- src/akushon/action/node/action_node.cpp | 65 +++++++++------------ 2 files changed, 32 insertions(+), 51 deletions(-) diff --git a/include/akushon/action/node/action_node.hpp b/include/akushon/action/node/action_node.hpp index 3065472..8914f48 100644 --- a/include/akushon/action/node/action_node.hpp +++ b/include/akushon/action/node/action_node.hpp @@ -46,25 +46,16 @@ class ActionNode using SetJoints = tachimawari_interfaces::msg::SetJoints; using Status = akushon_interfaces::msg::Status; - enum - { - READY, - PLAYING - }; - - enum - { - RUN_ACTION_BY_NAME, - RUN_ACTION_BY_JSON - }; + enum { READY, PLAYING }; + + enum { RUN_ACTION_BY_NAME, RUN_ACTION_BY_JSON }; static std::string get_node_prefix(); static std::string run_action_topic(); static std::string brake_action_topic(); static std::string status_topic(); - explicit ActionNode( - rclcpp::Node::SharedPtr node, std::shared_ptr action_manager); + explicit ActionNode(rclcpp::Node::SharedPtr node, std::shared_ptr action_manager); bool start(const std::string & action_name); bool start(const Action & action); @@ -73,6 +64,7 @@ class ActionNode private: void publish_joints(); + void publish_status(); Pose initial_pose; rclcpp::Node::SharedPtr node; diff --git a/src/akushon/action/node/action_node.cpp b/src/akushon/action/node/action_node.cpp index 3a2b006..d3b6a28 100755 --- a/src/akushon/action/node/action_node.cpp +++ b/src/akushon/action/node/action_node.cpp @@ -18,18 +18,18 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. -#include +#include "akushon/action/node/action_node.hpp" + #include +#include #include #include #include #include -#include "akushon/action/node/action_node.hpp" - -#include "akushon/action/node/action_manager.hpp" #include "akushon/action/model/action_name.hpp" #include "akushon/action/model/pose.hpp" +#include "akushon/action/node/action_manager.hpp" #include "nlohmann/json.hpp" #include "rclcpp/rclcpp.hpp" #include "tachimawari/joint/model/joint.hpp" @@ -38,34 +38,19 @@ namespace akushon { -std::string ActionNode::get_node_prefix() -{ - return "action"; -} +std::string ActionNode::get_node_prefix() { return "action"; } -std::string ActionNode::run_action_topic() -{ - return get_node_prefix() + "/run_action"; -} +std::string ActionNode::run_action_topic() { return get_node_prefix() + "/run_action"; } -std::string ActionNode::brake_action_topic() -{ - return get_node_prefix() + "/brake_action"; -} +std::string ActionNode::brake_action_topic() { return get_node_prefix() + "/brake_action"; } -std::string ActionNode::status_topic() -{ - return get_node_prefix() + "/status"; -} +std::string ActionNode::status_topic() { return get_node_prefix() + "/status"; } -ActionNode::ActionNode( - rclcpp::Node::SharedPtr node, std::shared_ptr action_manager) -: node(node), action_manager(action_manager), - initial_pose(Pose("initial_pose")) +ActionNode::ActionNode(rclcpp::Node::SharedPtr node, std::shared_ptr action_manager) +: node(node), action_manager(action_manager), initial_pose(Pose("initial_pose")) { current_joints_subscriber = node->create_subscription( - "/joint/current_joints", 10, - [this](const CurrentJoints::SharedPtr message) { + "/joint/current_joints", 10, [this](const CurrentJoints::SharedPtr message) { { using tachimawari::joint::Joint; std::vector current_joints; @@ -76,17 +61,14 @@ ActionNode::ActionNode( this->initial_pose.set_joints(current_joints); } - } - ); + }); - set_joints_publisher = node->create_publisher( - "/joint/set_joints", 10); + set_joints_publisher = node->create_publisher("/joint/set_joints", 10); status_publisher = node->create_publisher(status_topic(), 10); run_action_subscriber = node->create_subscription( - run_action_topic(), 10, - [this](std::shared_ptr message) { + 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); @@ -96,15 +78,11 @@ ActionNode::ActionNode( this->start(action); } - } - ); + }); brake_action_subscriber = node->create_subscription( brake_action_topic(), 10, - [this](std::shared_ptr message) { - this->action_manager->brake(); - } - ); + [this](std::shared_ptr message) { this->action_manager->brake(); }); } bool ActionNode::start(const std::string & action_name) @@ -142,6 +120,8 @@ bool ActionNode::update(int time) return true; } + publish_status(); + return false; } @@ -161,4 +141,13 @@ void ActionNode::publish_joints() set_joints_publisher->publish(joints_msg); } +void ActionNode::publish_status() +{ + auto message = Status(); + + message.is_running = action_manager->is_playing(); + + status_publisher->publish(message); +} + } // namespace akushon From 475f84d5d69ba3d0081c97e12abc0b9b2bb339d9 Mon Sep 17 00:00:00 2001 From: maroqijalil Date: Sat, 20 May 2023 17:06:17 +0700 Subject: [PATCH 2/3] refactor: change node initialization --- src/akushon/action/node/action_node.cpp | 32 ++++++++++++++----------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/src/akushon/action/node/action_node.cpp b/src/akushon/action/node/action_node.cpp index d3b6a28..7aaa767 100755 --- a/src/akushon/action/node/action_node.cpp +++ b/src/akushon/action/node/action_node.cpp @@ -32,7 +32,7 @@ #include "akushon/action/node/action_manager.hpp" #include "nlohmann/json.hpp" #include "rclcpp/rclcpp.hpp" -#include "tachimawari/joint/model/joint.hpp" +#include "tachimawari/joint/joint.hpp" #include "tachimawari_interfaces/msg/set_joints.hpp" namespace akushon @@ -49,21 +49,25 @@ std::string ActionNode::status_topic() { return get_node_prefix() + "/status"; } ActionNode::ActionNode(rclcpp::Node::SharedPtr node, std::shared_ptr action_manager) : node(node), action_manager(action_manager), initial_pose(Pose("initial_pose")) { - current_joints_subscriber = node->create_subscription( - "/joint/current_joints", 10, [this](const CurrentJoints::SharedPtr message) { - { - using tachimawari::joint::Joint; - std::vector current_joints; - - for (const auto & joint : message->joints) { - current_joints.push_back(Joint(joint.id, joint.position)); - } + { + using tachimawari::joint::JointNode; - this->initial_pose.set_joints(current_joints); - } - }); + current_joints_subscriber = node->create_subscription( + JointNode::current_joints_topic(), 10, [this](const CurrentJoints::SharedPtr message) { + { + using tachimawari::joint::Joint; + std::vector current_joints; + + for (const auto & joint : message->joints) { + current_joints.push_back(Joint(joint.id, joint.position)); + } - set_joints_publisher = node->create_publisher("/joint/set_joints", 10); + this->initial_pose.set_joints(current_joints); + } + }); + + set_joints_publisher = node->create_publisher(JointNode::set_joints_topic(), 10); + } status_publisher = node->create_publisher(status_topic(), 10); From 2990f1a9f35b8078dc65c93dbadd10fda4083e82 Mon Sep 17 00:00:00 2001 From: maroqijalil Date: Sun, 18 Jun 2023 17:11:17 +0700 Subject: [PATCH 3/3] refactor: change based on comments and run linter --- LICENSE | 2 +- include/akushon/action/action.hpp | 2 +- include/akushon/action/model/action.hpp | 2 +- include/akushon/action/model/action_name.hpp | 2 +- include/akushon/action/model/pose.hpp | 2 +- include/akushon/action/node/action_manager.hpp | 2 +- include/akushon/action/node/action_node.hpp | 5 +++-- include/akushon/action/process/interpolator.hpp | 2 +- include/akushon/action/process/joint_process.hpp | 2 +- include/akushon/akushon.hpp | 2 +- include/akushon/config/config.hpp | 2 +- include/akushon/config/node/config_node.hpp | 2 +- include/akushon/config/utils/config.hpp | 2 +- include/akushon/node/akushon_node.hpp | 2 +- src/action_main.cpp | 2 +- src/akushon/action/model/action.cpp | 2 +- src/akushon/action/model/action_name.cpp | 2 +- src/akushon/action/model/pose.cpp | 2 +- src/akushon/action/node/action_manager.cpp | 2 +- src/akushon/action/node/action_node.cpp | 16 ++++++++-------- src/akushon/action/process/interpolator.cpp | 2 +- src/akushon/action/process/joint_process.cpp | 2 +- src/akushon/config/node/config_node.cpp | 2 +- src/akushon/config/utils/config.cpp | 2 +- src/akushon/node/akushon_node.cpp | 2 +- src/akushon_main.cpp | 2 +- src/interpolator_main.cpp | 2 +- 27 files changed, 36 insertions(+), 35 deletions(-) diff --git a/LICENSE b/LICENSE index 2ae239c..c22a5bc 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ MIT License -Copyright (c) 2021 Ichiro ITS +Copyright (c) 2021-2023 Ichiro ITS Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/include/akushon/action/action.hpp b/include/akushon/action/action.hpp index 8a85f14..f10e32d 100644 --- a/include/akushon/action/action.hpp +++ b/include/akushon/action/action.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 ICHIRO ITS +// Copyright (c) 2021-2023 ICHIRO ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/include/akushon/action/model/action.hpp b/include/akushon/action/model/action.hpp index f582262..409d48e 100644 --- a/include/akushon/action/model/action.hpp +++ b/include/akushon/action/model/action.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/include/akushon/action/model/action_name.hpp b/include/akushon/action/model/action_name.hpp index ac04d30..245b3de 100644 --- a/include/akushon/action/model/action_name.hpp +++ b/include/akushon/action/model/action_name.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/include/akushon/action/model/pose.hpp b/include/akushon/action/model/pose.hpp index 2f1ab7e..9c6af2f 100644 --- a/include/akushon/action/model/pose.hpp +++ b/include/akushon/action/model/pose.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/include/akushon/action/node/action_manager.hpp b/include/akushon/action/node/action_manager.hpp index 1062134..099d58a 100644 --- a/include/akushon/action/node/action_manager.hpp +++ b/include/akushon/action/node/action_manager.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/include/akushon/action/node/action_node.hpp b/include/akushon/action/node/action_node.hpp index 8914f48..ed213fb 100644 --- a/include/akushon/action/node/action_node.hpp +++ b/include/akushon/action/node/action_node.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -55,7 +55,8 @@ class ActionNode static std::string brake_action_topic(); static std::string status_topic(); - explicit ActionNode(rclcpp::Node::SharedPtr node, std::shared_ptr action_manager); + explicit ActionNode( + rclcpp::Node::SharedPtr node, std::shared_ptr & action_manager); bool start(const std::string & action_name); bool start(const Action & action); diff --git a/include/akushon/action/process/interpolator.hpp b/include/akushon/action/process/interpolator.hpp index b767c3d..e47631e 100644 --- a/include/akushon/action/process/interpolator.hpp +++ b/include/akushon/action/process/interpolator.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/include/akushon/action/process/joint_process.hpp b/include/akushon/action/process/joint_process.hpp index 71a2556..9393238 100644 --- a/include/akushon/action/process/joint_process.hpp +++ b/include/akushon/action/process/joint_process.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/include/akushon/akushon.hpp b/include/akushon/akushon.hpp index 232f76e..387fcaf 100644 --- a/include/akushon/akushon.hpp +++ b/include/akushon/akushon.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 ICHIRO ITS +// Copyright (c) 2021-2023 ICHIRO ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/include/akushon/config/config.hpp b/include/akushon/config/config.hpp index 12da550..6b79d89 100644 --- a/include/akushon/config/config.hpp +++ b/include/akushon/config/config.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 ICHIRO ITS +// Copyright (c) 2021-2023 ICHIRO ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/include/akushon/config/node/config_node.hpp b/include/akushon/config/node/config_node.hpp index 6e84c8c..f97c804 100644 --- a/include/akushon/config/node/config_node.hpp +++ b/include/akushon/config/node/config_node.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/include/akushon/config/utils/config.hpp b/include/akushon/config/utils/config.hpp index 268e443..20e53d5 100644 --- a/include/akushon/config/utils/config.hpp +++ b/include/akushon/config/utils/config.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/include/akushon/node/akushon_node.hpp b/include/akushon/node/akushon_node.hpp index 60144fe..17e0820 100644 --- a/include/akushon/node/akushon_node.hpp +++ b/include/akushon/node/akushon_node.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/src/action_main.cpp b/src/action_main.cpp index b705747..d09b48a 100644 --- a/src/action_main.cpp +++ b/src/action_main.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/src/akushon/action/model/action.cpp b/src/akushon/action/model/action.cpp index 3e735c7..70da59f 100644 --- a/src/akushon/action/model/action.cpp +++ b/src/akushon/action/model/action.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/src/akushon/action/model/action_name.cpp b/src/akushon/action/model/action_name.cpp index 4661d79..e955fef 100644 --- a/src/akushon/action/model/action_name.cpp +++ b/src/akushon/action/model/action_name.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/src/akushon/action/model/pose.cpp b/src/akushon/action/model/pose.cpp index 3f18e5a..e1b9edc 100644 --- a/src/akushon/action/model/pose.cpp +++ b/src/akushon/action/model/pose.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/src/akushon/action/node/action_manager.cpp b/src/akushon/action/node/action_manager.cpp index 326f37e..b14ef80 100644 --- a/src/akushon/action/node/action_manager.cpp +++ b/src/akushon/action/node/action_manager.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/src/akushon/action/node/action_node.cpp b/src/akushon/action/node/action_node.cpp index 7aaa767..6a93872 100755 --- a/src/akushon/action/node/action_node.cpp +++ b/src/akushon/action/node/action_node.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -21,7 +21,6 @@ #include "akushon/action/node/action_node.hpp" #include -#include #include #include #include @@ -38,15 +37,16 @@ namespace akushon { -std::string ActionNode::get_node_prefix() { return "action"; } +std::string ActionNode::get_node_prefix() {return "action";} -std::string ActionNode::run_action_topic() { return get_node_prefix() + "/run_action"; } +std::string ActionNode::run_action_topic() {return get_node_prefix() + "/run_action";} -std::string ActionNode::brake_action_topic() { return get_node_prefix() + "/brake_action"; } +std::string ActionNode::brake_action_topic() {return get_node_prefix() + "/brake_action";} -std::string ActionNode::status_topic() { return get_node_prefix() + "/status"; } +std::string ActionNode::status_topic() {return get_node_prefix() + "/status";} -ActionNode::ActionNode(rclcpp::Node::SharedPtr node, std::shared_ptr action_manager) +ActionNode::ActionNode( + rclcpp::Node::SharedPtr node, std::shared_ptr & action_manager) : node(node), action_manager(action_manager), initial_pose(Pose("initial_pose")) { { @@ -86,7 +86,7 @@ ActionNode::ActionNode(rclcpp::Node::SharedPtr node, std::shared_ptrcreate_subscription( brake_action_topic(), 10, - [this](std::shared_ptr message) { this->action_manager->brake(); }); + [this](std::shared_ptr message) {this->action_manager->brake();}); } bool ActionNode::start(const std::string & action_name) diff --git a/src/akushon/action/process/interpolator.cpp b/src/akushon/action/process/interpolator.cpp index ab664fa..7822e91 100644 --- a/src/akushon/action/process/interpolator.cpp +++ b/src/akushon/action/process/interpolator.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/src/akushon/action/process/joint_process.cpp b/src/akushon/action/process/joint_process.cpp index c496cea..5e237a3 100644 --- a/src/akushon/action/process/joint_process.cpp +++ b/src/akushon/action/process/joint_process.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/src/akushon/config/node/config_node.cpp b/src/akushon/config/node/config_node.cpp index 4125da7..7baa907 100644 --- a/src/akushon/config/node/config_node.cpp +++ b/src/akushon/config/node/config_node.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/src/akushon/config/utils/config.cpp b/src/akushon/config/utils/config.cpp index f38f666..462eaab 100644 --- a/src/akushon/config/utils/config.cpp +++ b/src/akushon/config/utils/config.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/src/akushon/node/akushon_node.cpp b/src/akushon/node/akushon_node.cpp index c13fb7e..0db0315 100644 --- a/src/akushon/node/akushon_node.cpp +++ b/src/akushon/node/akushon_node.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/src/akushon_main.cpp b/src/akushon_main.cpp index 16265c4..235997a 100644 --- a/src/akushon_main.cpp +++ b/src/akushon_main.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal diff --git a/src/interpolator_main.cpp b/src/interpolator_main.cpp index 5e2813d..4400612 100644 --- a/src/interpolator_main.cpp +++ b/src/interpolator_main.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2021-2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal