diff --git a/CMakeLists.txt b/CMakeLists.txt index b1bc7ba..f314d6a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(aruku REQUIRED) find_package(aruku_interfaces REQUIRED) +find_package(atama REQUIRED) find_package(atama_interfaces REQUIRED) find_package(kansei REQUIRED) find_package(kansei_interfaces REQUIRED) @@ -46,6 +47,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC ament_target_dependencies(${PROJECT_NAME} aruku aruku_interfaces + atama atama_interfaces kansei kansei_interfaces @@ -110,6 +112,7 @@ ament_export_dependencies( ament_index_cpp aruku aruku_interfaces + atama atama_interfaces kansei kansei_interfaces diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 86c4517..9321413 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -37,7 +37,6 @@ class Robot keisan::Angle get_tilt() const; // member for getting - bool is_calibrated; keisan::Angle orientation; keisan::Point2 position; diff --git a/package.xml b/package.xml index e2c329f..08c75f3 100644 --- a/package.xml +++ b/package.xml @@ -10,6 +10,7 @@ ament_index_cpp aruku aruku_interfaces + atama atama_interfaces kansei kansei_interfaces diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 6e18c78..1158042 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -18,10 +18,10 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#include - #include "suiryoku/locomotion/model/robot.hpp" +#include + #include "keisan/keisan.hpp" using keisan::literals::operator""_deg; @@ -30,21 +30,25 @@ namespace suiryoku { Robot::Robot() -: pan(0_deg), tilt(0_deg), pan_center(0_deg), tilt_center(0_deg), x_speed(0.0), - y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), - orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), - a_amplitude(0.0), is_calibrated(false) +: pan(0_deg), + tilt(0_deg), + pan_center(0_deg), + tilt_center(0_deg), + x_speed(0.0), + y_speed(0.0), + a_speed(0.0), + aim_on(false), + is_walking(false), + orientation(0_deg), + position(0.0, 0.0), + x_amplitude(0.0), + y_amplitude(0.0), + a_amplitude(0.0) { } -keisan::Angle Robot::get_pan() const -{ - return pan + pan_center; -} +keisan::Angle Robot::get_pan() const { return pan + pan_center; } -keisan::Angle Robot::get_tilt() const -{ - return tilt + tilt_center; -} +keisan::Angle Robot::get_tilt() const { return tilt + tilt_center; } } // namespace suiryoku diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 0ca5e76..7788a2b 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -18,12 +18,13 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#include "suiryoku/locomotion/node/locomotion_node.hpp" + #include #include -#include "suiryoku/locomotion/node/locomotion_node.hpp" - #include "aruku/walking/walking.hpp" +#include "atama/head/head.hpp" #include "kansei/measurement/measurement.hpp" #include "keisan/keisan.hpp" #include "nlohmann/json.hpp" @@ -33,32 +34,25 @@ namespace suiryoku { -std::string LocomotionNode::get_node_prefix() -{ - return "locomotion"; -} +std::string LocomotionNode::get_node_prefix() { return "locomotion"; } -LocomotionNode::LocomotionNode( - rclcpp::Node::SharedPtr node, std::shared_ptr locomotion) +LocomotionNode::LocomotionNode(rclcpp::Node::SharedPtr node, std::shared_ptr locomotion) : locomotion(locomotion), robot(locomotion->get_robot()), walking_state(false) { - set_walking_publisher = node->create_publisher( - aruku::WalkingNode::set_walking_topic(), 10); + set_walking_publisher = + node->create_publisher(aruku::WalkingNode::set_walking_topic(), 10); measurement_status_subscriber = node->create_subscription( - "/measurement/orientation", 10, + kansei::measurement::MeasurementNode::status_topic(), 10, [this](const MeasurementStatus::SharedPtr message) { - this->robot->is_calibrated = message->is_calibrated; this->robot->orientation = keisan::make_degree(message->orientation.yaw); }); - set_odometry_publisher = node->create_publisher( - aruku::WalkingNode::set_odometry_topic(), 10); + set_odometry_publisher = + node->create_publisher(aruku::WalkingNode::set_odometry_topic(), 10); walking_status_subscriber = node->create_subscription( - aruku::WalkingNode::status_topic(), 10, - [this](const WalkingStatus::SharedPtr message) - { + aruku::WalkingNode::status_topic(), 10, [this](const WalkingStatus::SharedPtr message) { this->robot->is_walking = message->is_running; this->robot->x_amplitude = message->x_amplitude; this->robot->y_amplitude = message->y_amplitude; @@ -68,20 +62,16 @@ LocomotionNode::LocomotionNode( }); head_subscriber = node->create_subscription( - "/head/set_head_data", 10, - [this](const Head::SharedPtr message) { + atama::HeadNode::head_topic(), 10, [this](const Head::SharedPtr message) { this->robot->pan = keisan::make_degree(message->pan_angle); this->robot->tilt = keisan::make_degree(message->tilt_angle); }); - locomotion->stop = [this]() {this->walking_state = false;}; - locomotion->start = [this]() {this->walking_state = true;}; + locomotion->stop = [this]() { this->walking_state = false; }; + locomotion->start = [this]() { this->walking_state = true; }; } -void LocomotionNode::update() -{ - publish_walking(); -} +void LocomotionNode::update() { publish_walking(); } void LocomotionNode::publish_walking() {