Merge pull request #36 from ichiro-its/enhancement/complete-action-st…

…atus

[Enhancement] Add Action Status

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

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

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

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

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

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

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
@@ -46,25 +46,17 @@ class ActionNode
   using SetJoints = tachimawari_interfaces::msg::SetJoints;
   using Status = akushon_interfaces::msg::Status;
 
-  enum
-  {
-    READY,
-    PLAYING
-  };
+  enum { READY, PLAYING };
 
-  enum
-  {
-    RUN_ACTION_BY_NAME,
-    RUN_ACTION_BY_JSON
-  };
+  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<ActionManager> action_manager);
+    rclcpp::Node::SharedPtr node, std::shared_ptr<ActionManager> & action_manager);
 
   bool start(const std::string & action_name);
   bool start(const Action & action);
@@ -73,6 +65,7 @@ class ActionNode
 
 private:
   void publish_joints();
+  void publish_status();
 
   Pose initial_pose;
   rclcpp::Node::SharedPtr node;

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

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

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

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

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

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

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

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

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

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

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

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

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
@@ -18,75 +18,61 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 // SOFTWARE.
 
-#include <iostream>
+#include "akushon/action/node/action_node.hpp"
+
 #include <iomanip>
 #include <memory>
 #include <string>
 #include <thread>
 #include <vector>
 
-#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"
+#include "tachimawari/joint/joint.hpp"
 #include "tachimawari_interfaces/msg/set_joints.hpp"
 
 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<ActionManager> action_manager)
-: node(node), action_manager(action_manager),
-  initial_pose(Pose("initial_pose"))
+  rclcpp::Node::SharedPtr node, std::shared_ptr<ActionManager> & action_manager)
+: node(node), action_manager(action_manager), initial_pose(Pose("initial_pose"))
 {
-  current_joints_subscriber = node->create_subscription<CurrentJoints>(
-    "/joint/current_joints", 10,
-    [this](const CurrentJoints::SharedPtr message) {
-      {
-        using tachimawari::joint::Joint;
-        std::vector<Joint> 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<CurrentJoints>(
+      JointNode::current_joints_topic(), 10, [this](const CurrentJoints::SharedPtr message) {
+        {
+          using tachimawari::joint::Joint;
+          std::vector<Joint> current_joints;
+
+          for (const auto & joint : message->joints) {
+            current_joints.push_back(Joint(joint.id, joint.position));
+          }
 
-  set_joints_publisher = node->create_publisher<SetJoints>(
-    "/joint/set_joints", 10);
+          this->initial_pose.set_joints(current_joints);
+        }
+      });
+
+    set_joints_publisher = node->create_publisher<SetJoints>(JointNode::set_joints_topic(), 10);
+  }
 
   status_publisher = node->create_publisher<Status>(status_topic(), 10);
 
   run_action_subscriber = node->create_subscription<RunAction>(
-    run_action_topic(), 10,
-    [this](std::shared_ptr<RunAction> message) {
+    run_action_topic(), 10, [this](std::shared_ptr<RunAction> message) {
       std::cout << message->action_name << std::endl;
       if (message->control_type == RUN_ACTION_BY_NAME) {
         this->start(message->action_name);
@@ -96,15 +82,11 @@ ActionNode::ActionNode(
 
         this->start(action);
       }
-    }
-  );
+    });
 
   brake_action_subscriber = node->create_subscription<Empty>(
     brake_action_topic(), 10,
-    [this](std::shared_ptr<Empty> message) {
-      this->action_manager->brake();
-    }
-  );
+    [this](std::shared_ptr<Empty> message) {this->action_manager->brake();});
 }
 
 bool ActionNode::start(const std::string & action_name)
@@ -142,6 +124,8 @@ bool ActionNode::update(int time)
     return true;
   }
 
+  publish_status();
+
   return false;
 }
 
@@ -161,4 +145,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

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

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

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

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

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

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

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