Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[PI Sprint 24/25 / PD-456] - [Enhancement] Publish Delta Position #36

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
+38 −2
Open

[PI Sprint 24/25 / PD-456] - [Enhancement] Publish Delta Position #36

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c2fef27
feat: publish delta position
mbsaloka Nov 30, 2024
3442e4e
fix: publish delta position separated
mbsaloka Dec 5, 2024
a518bcb
feat: publish delta odometry when not zero
mbsaloka Dec 19, 2024
2074b61
fix: remove zero values checker for delta_position
mbsaloka Jan 9, 2025
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
2 changes: 2 additions & 0 deletions include/aruku/walking/node/walking_manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ class WalkingManager

void set_position(const keisan::Point2 & position);
const keisan::Point2 & get_position() const;
const keisan::Point2 & get_delta_position() const;

void run(double x_move, double y_move, double a_move, bool aim_on = false);
void stop();
Expand Down Expand Up @@ -82,6 +83,7 @@ class WalkingManager
std::array<double, 19> joints_direction;

keisan::Point2 position;
keisan::Point2 delta_position;

keisan::Angle<double> orientation;
keisan::Vector<3> gyro;
Expand Down
3 changes: 3 additions & 0 deletions include/aruku/walking/node/walking_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@ class WalkingNode
static std::string set_walking_topic();
static std::string status_topic();
static std::string set_odometry_topic();
static std::string delta_position_topic();

explicit WalkingNode(
rclcpp::Node::SharedPtr node, std::shared_ptr<WalkingManager> walking_manager);
Expand All @@ -61,6 +62,7 @@ class WalkingNode

void publish_joints();
void publish_status();
void publish_delta_position();
private:

rclcpp::Node::SharedPtr node;
Expand All @@ -72,6 +74,7 @@ class WalkingNode

rclcpp::Subscription<Point2>::SharedPtr set_odometry_subscriber;
rclcpp::Publisher<WalkingStatus>::SharedPtr status_publisher;
rclcpp::Publisher<Point2>::SharedPtr delta_position_publisher;

rclcpp::Subscription<MeasurementStatus>::SharedPtr measurement_status_subscriber;
rclcpp::Subscription<Unit>::SharedPtr unit_subscriber;
Expand Down
15 changes: 13 additions & 2 deletions src/aruku/walking/node/walking_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -220,6 +220,11 @@ const keisan::Point2 & WalkingManager::get_position() const
return position;
}

const keisan::Point2 & WalkingManager::get_delta_position() const
{
return delta_position;
}

void WalkingManager::run(double x_move, double y_move, double a_move, bool aim_on)
{
kinematic.set_move_amplitude(x_move, y_move, keisan::make_degree(a_move), aim_on);
Expand All @@ -234,6 +239,9 @@ void WalkingManager::stop()
bool WalkingManager::process()
{
if (kinematic.run_kinematic()) {
delta_position.x = 0.0;
delta_position.y = 0.0;

if (kinematic.time_to_compute_odometry()) {
double x_amplitude = kinematic.get_x_move_amplitude();
double y_amplitude = kinematic.get_y_move_amplitude();
Expand All @@ -253,8 +261,11 @@ bool WalkingManager::process()
dy = -y_amplitude * odometry_ry_coefficient / 30.0;
}

position.x += dx * orientation.cos() - dy * orientation.sin();
position.y += dx * orientation.sin() + dy * orientation.cos();
delta_position.x = dx * orientation.cos() - dy * orientation.sin();
delta_position.y = dx * orientation.sin() + dy * orientation.cos();

position.x += delta_position.x;
position.y += delta_position.y;
}
}

Expand Down
20 changes: 20 additions & 0 deletions src/aruku/walking/node/walking_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,11 @@ std::string WalkingNode::set_odometry_topic()
return get_node_prefix() + "/set_odometry";
}

std::string WalkingNode::delta_position_topic()
{
return get_node_prefix() + "/delta_position";
}

WalkingNode::WalkingNode(
rclcpp::Node::SharedPtr node, std::shared_ptr<WalkingManager> walking_manager)
: walking_manager(walking_manager)
Expand Down Expand Up @@ -95,6 +100,9 @@ WalkingNode::WalkingNode(
keisan::Point2(message->x, message->y));
});

delta_position_publisher = node->create_publisher<Point2>(
delta_position_topic(), 10);

set_joints_publisher = node->create_publisher<SetJoints>(
"/joint/set_joints", 10);
}
Expand All @@ -107,6 +115,7 @@ void WalkingNode::update()

publish_joints();
publish_status();
publish_delta_position();
}

void WalkingNode::publish_joints()
Expand Down Expand Up @@ -144,4 +153,15 @@ void WalkingNode::publish_status()
status_publisher->publish(status_msg);
}

void WalkingNode::publish_delta_position()
{
auto delta_position = walking_manager->get_delta_position();

auto delta_position_msg = Point2();
delta_position_msg.x = delta_position.x;
delta_position_msg.y = delta_position.y;

delta_position_publisher->publish(delta_position_msg);
}

} // namespace aruku
Loading