Merge pull request #30 from ichiro-its/enhancement/prepare-soccer

[Enhancement] Update Node

CMakeLists.txt

@@ -19,6 +19,7 @@ find_package(kansei_interfaces REQUIRED)
 find_package(keisan REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(tachimawari REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 
@@ -43,6 +44,7 @@ ament_target_dependencies(${PROJECT_NAME}
   kansei_interfaces
   rclcpp
   std_msgs
+  tachimawari
   tf2
   tf2_geometry_msgs)
 
@@ -96,6 +98,7 @@ ament_export_dependencies(
   kansei_interfaces
   rclcpp
   std_msgs
+  tachimawari
   tf2
   tf2_geometry_msgs)
 ament_export_include_directories("include")

include/kansei/fallen/node/fallen_node.hpp

@@ -25,28 +25,31 @@
 #include <string>
 
 #include "kansei/fallen/node/fallen_determinant.hpp"
-#include "kansei_interfaces/msg/fallen.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int8.hpp"
 
 namespace kansei::fallen
 {
 
 class FallenNode
 {
 public:
+  using Int8 = std_msgs::msg::Int8;
+
+  static std::string get_node_prefix();
+  static std::string fallen_topic();
+
   explicit FallenNode(
     rclcpp::Node::SharedPtr node, std::shared_ptr<FallenDeterminant> fallen_determinant);
 
   void update(const keisan::Euler<double> & rpy, const keisan::Vector<3> & acc);
 
 private:
-  std::string get_node_prefix() const;
-
   void publish_fallen();
 
   std::shared_ptr<FallenDeterminant> fallen_determinant;
 
-  rclcpp::Publisher<kansei_interfaces::msg::Fallen>::SharedPtr fallen_publisher;
+  rclcpp::Publisher<Int8>::SharedPtr fallen_publisher;
 };
 
 }  // namespace kansei::fallen

include/kansei/measurement/node/measurement_node.hpp

@@ -26,10 +26,10 @@
 
 #include "kansei/measurement/node/measurement_unit.hpp"
 #include "kansei_interfaces/msg/axis.hpp"
-#include "kansei_interfaces/msg/reset_orientation.hpp"
-#include "kansei_interfaces/msg/unit.hpp"
 #include "kansei_interfaces/msg/status.hpp"
+#include "kansei_interfaces/msg/unit.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float64.hpp"
 
 namespace kansei::measurement
 {
@@ -38,7 +38,7 @@ class MeasurementNode
 {
 public:
   using Axis = kansei_interfaces::msg::Axis;
-  using ResetOrientation = kansei_interfaces::msg::ResetOrientation;
+  using Float64 = std_msgs::msg::Float64;
   using Status = kansei_interfaces::msg::Status;
   using Unit = kansei_interfaces::msg::Unit;
 
@@ -57,11 +57,10 @@ class MeasurementNode
 private:
   void publish_status();
   void publish_unit();
-  void subscribe_unit();
 
   std::shared_ptr<MeasurementUnit> measurement_unit;
 
-  rclcpp::Subscription<ResetOrientation>::SharedPtr reset_orientation_subscriber;
+  rclcpp::Subscription<Float64>::SharedPtr reset_orientation_subscriber;
 
   rclcpp::Publisher<Unit>::SharedPtr unit_publisher;
   rclcpp::Subscription<Unit>::SharedPtr unit_subscriber;

package.xml

@@ -13,6 +13,7 @@
   <depend>nlohmann-json-dev</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <depend>tachimawari</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <test_depend>ament_lint_auto</test_depend>

src/kansei/fallen/node/fallen_node.cpp

@@ -19,9 +19,8 @@
 // SOFTWARE.
 
 #include <kansei_interfaces/msg/fallen.hpp>
-#include <rclcpp/rclcpp.hpp>
-
 #include <memory>
+#include <rclcpp/rclcpp.hpp>
 #include <string>
 
 #include "kansei/fallen/fallen.hpp"
@@ -32,12 +31,15 @@ using namespace keisan::literals;  // NOLINT
 namespace kansei::fallen
 {
 
+std::string FallenNode::get_node_prefix() { return "fallen"; }
+
+std::string FallenNode::fallen_topic() { return get_node_prefix() + "/fallen"; }
+
 FallenNode::FallenNode(
   rclcpp::Node::SharedPtr node, std::shared_ptr<FallenDeterminant> fallen_determinant)
 : fallen_determinant(fallen_determinant)
 {
-  fallen_publisher = node->create_publisher<kansei_interfaces::msg::Fallen>(
-    get_node_prefix() + "/fallen", 10);
+  fallen_publisher = node->create_publisher<Int8>(fallen_topic(), 10);
 }
 
 void FallenNode::update(const keisan::Euler<double> & rpy, const keisan::Vector<3> & acc)
@@ -55,16 +57,11 @@ void FallenNode::update(const keisan::Euler<double> & rpy, const keisan::Vector<
   }
 }
 
-std::string FallenNode::get_node_prefix() const
-{
-  return "fallen";
-}
-
 void FallenNode::publish_fallen()
 {
-  auto fallen_msg = kansei_interfaces::msg::Fallen();
+  auto fallen_msg = Int8();
 
-  fallen_msg.fallen_status = fallen_determinant->get_fallen_status();
+  fallen_msg.data = fallen_determinant->get_fallen_status();
 
   fallen_publisher->publish(fallen_msg);
 }

src/kansei/measurement/node/measurement_node.cpp

@@ -18,53 +18,44 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 // SOFTWARE.
 
-#include <experimental/array>
 #include <array>
+#include <experimental/array>
 #include <memory>
 #include <string>
 
+#include "kansei/measurement/measurement.hpp"
 #include "kansei_interfaces/msg/axis.hpp"
 #include "kansei_interfaces/msg/unit.hpp"
-#include "kansei/measurement/measurement.hpp"
 #include "keisan/keisan.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "tachimawari/imu/node/imu_node.hpp"
 
 using namespace keisan::literals;  // NOLINT
 
 namespace kansei::measurement
 {
 
-std::string MeasurementNode::get_node_prefix()
-{
-  return "measurement";
-}
+std::string MeasurementNode::get_node_prefix() { return "measurement"; }
 
 std::string MeasurementNode::reset_orientation_topic()
 {
   return get_node_prefix() + "/reset_orientation";
 }
 
-std::string MeasurementNode::status_topic()
-{
-  return get_node_prefix() + "/status";
-}
+std::string MeasurementNode::status_topic() { return get_node_prefix() + "/status"; }
 
-std::string MeasurementNode::unit_topic()
-{
-  return get_node_prefix() + "/unit";
-}
+std::string MeasurementNode::unit_topic() { return get_node_prefix() + "/unit"; }
 
 MeasurementNode::MeasurementNode(
   rclcpp::Node::SharedPtr node, std::shared_ptr<MeasurementUnit> measurement_unit)
 : measurement_unit(measurement_unit)
 {
-  reset_orientation_subscriber = node->create_subscription<ResetOrientation>(
-    reset_orientation_topic(), 10,
-    [this](const ResetOrientation::SharedPtr message) {
-      if (message->orientation == 0.0) {
+  reset_orientation_subscriber = node->create_subscription<Float64>(
+    reset_orientation_topic(), 10, [this](const Float64::SharedPtr message) {
+      if (message->data == 0.0) {
         this->measurement_unit->reset_orientation();
       } else {
-        this->measurement_unit->set_orientation_to(keisan::make_degree(message->orientation));
+        this->measurement_unit->set_orientation_to(keisan::make_degree(message->data));
       }
     });
 
@@ -73,16 +64,9 @@ MeasurementNode::MeasurementNode(
   status_publisher = node->create_publisher<Status>(status_topic(), 10);
 
   unit_subscriber = node->create_subscription<Unit>(
-    "/imu/unit", 10,
-    [this](const Unit::SharedPtr message) {
-      keisan::Vector<3> gy(
-        message->gyro.roll,
-        message->gyro.pitch,
-        message->gyro.yaw);
-      keisan::Vector<3> acc(
-        message->accelero.x,
-        message->accelero.y,
-        message->accelero.z);
+    tachimawari::imu::ImuNode::unit_topic(), 10, [this](const Unit::SharedPtr message) {
+      keisan::Vector<3> gy(message->gyro.roll, message->gyro.pitch, message->gyro.yaw);
+      keisan::Vector<3> acc(message->accelero.x, message->accelero.y, message->accelero.z);
 
       this->measurement_unit->update_gy_acc(gy, acc);
     });
@@ -145,9 +129,4 @@ void MeasurementNode::publish_unit()
   unit_publisher->publish(unit_msg);
 }
 
-void MeasurementNode::subscribe_unit()
-{
-  // do unit value subscribing
-}
-
 }  // namespace kansei::measurement