diff --git a/CMakeLists.txt b/CMakeLists.txt index 9271092..9414c0c 100755 --- a/CMakeLists.txt +++ b/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") diff --git a/include/kansei/fallen/node/fallen_node.hpp b/include/kansei/fallen/node/fallen_node.hpp index 7d643cf..fe42533 100644 --- a/include/kansei/fallen/node/fallen_node.hpp +++ b/include/kansei/fallen/node/fallen_node.hpp @@ -25,8 +25,8 @@ #include #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 { @@ -34,19 +34,22 @@ 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 fallen_determinant); void update(const keisan::Euler & rpy, const keisan::Vector<3> & acc); private: - std::string get_node_prefix() const; - void publish_fallen(); std::shared_ptr fallen_determinant; - rclcpp::Publisher::SharedPtr fallen_publisher; + rclcpp::Publisher::SharedPtr fallen_publisher; }; } // namespace kansei::fallen diff --git a/include/kansei/measurement/node/measurement_node.hpp b/include/kansei/measurement/node/measurement_node.hpp index e5d3a36..ff4188b 100644 --- a/include/kansei/measurement/node/measurement_node.hpp +++ b/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 measurement_unit; - rclcpp::Subscription::SharedPtr reset_orientation_subscriber; + rclcpp::Subscription::SharedPtr reset_orientation_subscriber; rclcpp::Publisher::SharedPtr unit_publisher; rclcpp::Subscription::SharedPtr unit_subscriber; diff --git a/package.xml b/package.xml index 159e438..c1470a4 100755 --- a/package.xml +++ b/package.xml @@ -13,6 +13,7 @@ nlohmann-json-dev rclcpp std_msgs + tachimawari tf2 tf2_geometry_msgs ament_lint_auto diff --git a/src/kansei/fallen/node/fallen_node.cpp b/src/kansei/fallen/node/fallen_node.cpp index 6cdd917..d4712bd 100755 --- a/src/kansei/fallen/node/fallen_node.cpp +++ b/src/kansei/fallen/node/fallen_node.cpp @@ -19,9 +19,8 @@ // SOFTWARE. #include -#include - #include +#include #include #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 fallen_determinant) : fallen_determinant(fallen_determinant) { - fallen_publisher = node->create_publisher( - get_node_prefix() + "/fallen", 10); + fallen_publisher = node->create_publisher(fallen_topic(), 10); } void FallenNode::update(const keisan::Euler & rpy, const keisan::Vector<3> & acc) @@ -55,16 +57,11 @@ void FallenNode::update(const keisan::Euler & 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); } diff --git a/src/kansei/measurement/node/measurement_node.cpp b/src/kansei/measurement/node/measurement_node.cpp index 83fe016..612c44d 100755 --- a/src/kansei/measurement/node/measurement_node.cpp +++ b/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 #include +#include #include #include +#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 measurement_unit) : measurement_unit(measurement_unit) { - reset_orientation_subscriber = node->create_subscription( - reset_orientation_topic(), 10, - [this](const ResetOrientation::SharedPtr message) { - if (message->orientation == 0.0) { + reset_orientation_subscriber = node->create_subscription( + 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_topic(), 10); unit_subscriber = node->create_subscription( - "/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