Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -46,6 +47,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
ament_target_dependencies(${PROJECT_NAME}
aruku
aruku_interfaces
atama
atama_interfaces
kansei
kansei_interfaces
Expand Down Expand Up @@ -110,6 +112,7 @@ ament_export_dependencies(
ament_index_cpp
aruku
aruku_interfaces
atama
atama_interfaces
kansei
kansei_interfaces
Expand Down
1 change: 0 additions & 1 deletion include/suiryoku/locomotion/model/robot.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,6 @@ class Robot
keisan::Angle<double> get_tilt() const;

// member for getting
bool is_calibrated;
keisan::Angle<double> orientation;
keisan::Point2 position;

Expand Down
1 change: 1 addition & 0 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
<depend>ament_index_cpp</depend>
<depend>aruku</depend>
<depend>aruku_interfaces</depend>
<depend>atama</depend>
<depend>atama_interfaces</depend>
<depend>kansei</depend>
<depend>kansei_interfaces</depend>
Expand Down
32 changes: 18 additions & 14 deletions src/suiryoku/locomotion/model/robot.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,10 +18,10 @@
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include <string>

#include "suiryoku/locomotion/model/robot.hpp"

#include <string>

#include "keisan/keisan.hpp"

using keisan::literals::operator""_deg;
Expand All @@ -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<double> Robot::get_pan() const
{
return pan + pan_center;
}
keisan::Angle<double> Robot::get_pan() const { return pan + pan_center; }

keisan::Angle<double> Robot::get_tilt() const
{
return tilt + tilt_center;
}
keisan::Angle<double> Robot::get_tilt() const { return tilt + tilt_center; }

} // namespace suiryoku
40 changes: 15 additions & 25 deletions src/suiryoku/locomotion/node/locomotion_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 <memory>
#include <string>

#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"
Expand All @@ -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> locomotion)
LocomotionNode::LocomotionNode(rclcpp::Node::SharedPtr node, std::shared_ptr<Locomotion> locomotion)
: locomotion(locomotion), robot(locomotion->get_robot()), walking_state(false)
{
set_walking_publisher = node->create_publisher<SetWalking>(
aruku::WalkingNode::set_walking_topic(), 10);
set_walking_publisher =
node->create_publisher<SetWalking>(aruku::WalkingNode::set_walking_topic(), 10);

measurement_status_subscriber = node->create_subscription<MeasurementStatus>(
"/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<Point2>(
aruku::WalkingNode::set_odometry_topic(), 10);
set_odometry_publisher =
node->create_publisher<Point2>(aruku::WalkingNode::set_odometry_topic(), 10);

walking_status_subscriber = node->create_subscription<WalkingStatus>(
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;
Expand All @@ -68,20 +62,16 @@ LocomotionNode::LocomotionNode(
});

head_subscriber = node->create_subscription<Head>(
"/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()
{
Expand Down