Skip to content

Commit

Permalink
update parser and server
Browse files Browse the repository at this point in the history
  • Loading branch information
@antbono
antbono committed Apr 28, 2024
1 parent 3a1c200 commit 1129031
Show file tree
Hide file tree
Showing 26 changed files with 327 additions and 254 deletions.
1 change: 1 addition & 0 deletions nao_pos_server/include/nao_pos_server/nao_pos_action_server.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,7 @@ class NaoPosActionServer : public rclcpp::Node
bool file_successfully_read_ = false;
std::vector<KeyFrame> key_frames_;
std::atomic<bool> pos_in_action_;
//bool pos_in_action_;
bool firstTickSinceActionStarted_ = true;
std::unique_ptr<KeyFrame> key_frame_start_;
rclcpp::Time initial_time_;
Expand Down
42 changes: 42 additions & 0 deletions nao_pos_server/launch/swing2_launch.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
return LaunchDescription([
Node(
package='nao_pos_server',
#namespace='turtlesim2',
executable='nao_pos_action_server2',
name='nao_pos_action_server_legs',
remappings=[
('nao_pos_action2/_action/feedback', 'nao_pos_action_legs2/_action/feedback'),
('nao_pos_action2/_action/status', 'nao_pos_action_legs2/_action/status'),
('nao_pos_action2/_action/cancel_goal', 'nao_pos_action_legs2/_action/cancel_goal'),
('nao_pos_action2/_action/get_result', 'nao_pos_action_legs2/_action/get_result'),
('nao_pos_action2/_action/send_goal', 'nao_pos_action_legs2/_action/send_goal'),
],
),
Node(
package='nao_pos_server',
#namespace='turtlesim2',
executable='nao_pos_action_client2',
name='nao_pos_action_client_legs',
remappings=[
('nao_pos_action2/_action/feedback', 'nao_pos_action_legs2/_action/feedback'),
('nao_pos_action2/_action/status', 'nao_pos_action_legs2/_action/status'),
('nao_pos_action2/_action/cancel_goal', 'nao_pos_action_legs2/_action/cancel_goal'),
('nao_pos_action2/_action/get_result', 'nao_pos_action_legs2/_action/get_result'),
('nao_pos_action2/_action/send_goal', 'nao_pos_action_legs2/_action/send_goal'),
('action_req2', 'action_req_legs2'),
],
),
Node(
package='nao_pos_server',
#namespace='turtlesim2',
executable='nao_pos_publisher',
name='nao_pos_publisher_legs',
remappings=[
('action_req', 'action_req_legs2'),
],
),
])
4 changes: 2 additions & 2 deletions nao_pos_server/pos/c0.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 0 0 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 0 0 - - - - - - - - - - - - - - - - - - - - - - - 2000
2 changes: 1 addition & 1 deletion nao_pos_server/pos/d1.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 0 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
4 changes: 2 additions & 2 deletions nao_pos_server/pos/d1l1.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 20 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 20 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
4 changes: 2 additions & 2 deletions nao_pos_server/pos/d1l2.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 40 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 40 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
4 changes: 2 additions & 2 deletions nao_pos_server/pos/d1l3.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 60 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 60 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
4 changes: 2 additions & 2 deletions nao_pos_server/pos/d1r1.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! -20 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! -20 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
4 changes: 2 additions & 2 deletions nao_pos_server/pos/d1r2.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! -40 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! -40 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
4 changes: 2 additions & 2 deletions nao_pos_server/pos/d1r3.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! -60 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! -60 15 - - - - - - - - - - - - - - - - - - - - - - - 2000
4 changes: 2 additions & 2 deletions nao_pos_server/pos/d2.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 0 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 0 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
4 changes: 2 additions & 2 deletions nao_pos_server/pos/d2l1.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 20 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 20 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
4 changes: 2 additions & 2 deletions nao_pos_server/pos/d2l2.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 40 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 40 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
4 changes: 2 additions & 2 deletions nao_pos_server/pos/d2l3.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 60 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 60 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
2 changes: 1 addition & 1 deletion nao_pos_server/pos/d2r1.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! -20 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
! -20 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
4 changes: 2 additions & 2 deletions nao_pos_server/pos/d2r2.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! -40 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! -40 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
4 changes: 2 additions & 2 deletions nao_pos_server/pos/d2r3.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! -60 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! -60 30 - - - - - - - - - - - - - - - - - - - - - - - 2000
2 changes: 1 addition & 1 deletion nao_pos_server/pos/l2.pos
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! 40 0 - - - - - - - - - - - - - - - - - - - - - - - 2000
12 changes: 6 additions & 6 deletions nao_pos_server/pos/only_legs.pos
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
legs swing

HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! - - - - - - - 0 0 0 0 0 0 0 -10 +20 -10 0 - - - - - - - 4000
! - - - - - - - 0 0 0 0 0 0 0 -10 +20 -10 0 - - - - - - - 4000
! - - - - - - - 0 0 -10 +20 -10 0 0 0 0 0 0 - - - - - - - 4000
! - - - - - - - 0 0 -10 +20 -10 0 0 0 0 0 0 - - - - - - - 2000
! - - - - - - - 0 0 0 0 0 0 0 0 0 0 0 - - - - - - - 2000
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
! - - - - - - - 0 0 0 0 0 0 0 -10 +20 -10 0 - - - - - - - 4000
! - - - - - - - 0 0 0 0 0 0 0 -10 +20 -10 0 - - - - - - - 4000
! - - - - - - - 0 0 -10 +20 -10 0 0 0 0 0 0 - - - - - - - 4000
! - - - - - - - 0 0 -10 +20 -10 0 0 0 0 0 0 - - - - - - - 2000
! - - - - - - - 0 0 0 0 0 0 0 0 0 0 0 - - - - - - - 2000
8 changes: 8 additions & 0 deletions nao_pos_server/pos/sit-to-stand.pos
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR

Turn stiffness on, raise arms forward so they dont get caught on the body
$ 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
! 0 0 70 25 -92 -61 54 0 0 -50 125 -70 0 0 -50 125 -70 0 70 -25 92 61 -54 0 0 1000

Stand up
! 0 0 90 10 0 0 0 0 0 -25 50 -25 0 0 -25 50 -25 0 90 -10 0 0 0 0 0 2000
13 changes: 13 additions & 0 deletions nao_pos_server/pos/stand-to-sit.pos
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR

Sit with arms forward, so they dont get caught on the body
$ 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
! 0 0 70 25 -92 -61 54 0 0 -50 125 -70 0 0 -50 125 -70 0 70 -25 92 61 -54 0 0 2000

After crouching, hands on knees to avoid robot falling forward and face planting
$ 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2
! 0 0 72 2 -33 -47 55 0 0 -50 125 -70 0 0 -50 125 -70 0 72 -2 33 47 -55 0 0 1000

Turn stiffness off
$ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
! 0 0 72 2 -33 -47 55 0 0 -50 125 -70 0 0 -50 125 -70 0 72 -2 33 47 -55 0 0 100
10 changes: 10 additions & 0 deletions nao_pos_server/pos/test_parser1.pos
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
lines starting with '!' are for joint positions
lines starting with '$' are for joint stiffnesses

HY HP LSP LSR LEY LER LWY LHYP LHR LHP LKP LAP LAR RHR RHP RKP RAP RAR RSP RSR REY RER RWY LH RH DUR
#$- - - - - - - 0.8 0.8 0.8 0.8 0.8 0.8 0.8 0.8 0.8 0.8 0.8 - - - - - - -
! - - - - - - - 0 0 0 0 0 0 0 -10 +20 -10 0 - - - - - - - 4000
! - - - - - - - 0 0 0 0 0 0 0 -10 +20 -10 0 - - - - - - - 4000
! - - - - - - - 0 0 -10 +20 -10 0 0 0 0 0 0 - - - - - - - 4000
! - - - - - - - 0 0 -10 +20 -10 0 0 0 0 0 0 - - - - - - - 2000
! - - - - - - - 0 0 0 0 0 0 0 0 0 0 0 - - - - - - - 2000
30 changes: 15 additions & 15 deletions nao_pos_server/src/nao_pos_action_client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,14 @@ NaoPosActionClient::~NaoPosActionClient()
{
}

void NaoPosActionClient::action_req_callback(const std_msgs::msg::String::SharedPtr msg)
{
RCLCPP_DEBUG(this->get_logger(), "I heard: '%s'", msg->data.c_str());

std::string action_name = msg->data;
this->send_goal(action_name);
}

void NaoPosActionClient::send_goal(std::string& action_name)
{
using namespace std::placeholders;
Expand Down Expand Up @@ -92,13 +100,6 @@ void NaoPosActionClient::send_goal(std::string& action_name)
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}

void NaoPosActionClient::action_req_callback(const std_msgs::msg::String::SharedPtr msg)
{
RCLCPP_DEBUG(this->get_logger(), "I heard: '%s'", msg->data.c_str());

std::string action_name = msg->data;
this->send_goal(action_name);
}

void NaoPosActionClient::goal_response_callback(const GoalHandlePosAction::SharedPtr& goal_handle)
{
Expand All @@ -112,33 +113,32 @@ void NaoPosActionClient::goal_response_callback(const GoalHandlePosAction::Share
}
}

/*
void NaoPosActionClient::feedback_callback(GoalHandlePosAction::SharedPtr,
const std::shared_ptr<const PosAction::Feedback> feedback)
{
// TODO
}
*/

void NaoPosActionClient::result_callback(const GoalHandlePosAction::WrappedResult& result)
{
switch (result.code)
{
case rclcpp_action::ResultCode::SUCCEEDED:
break;
RCLCPP_INFO(this->get_logger(), "Joints posisitions regulary played.");
return;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
RCLCPP_ERROR(this->get_logger(), " nao pos Goal was aborted");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
RCLCPP_ERROR(this->get_logger(), " nao pos Goal was canceled");
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
RCLCPP_ERROR(this->get_logger(), " nao pos Unknown result code");
return;
}

// if (result.result->success)
RCLCPP_INFO(this->get_logger(), "Joints posisitions regulary played.");

// rclcpp::shutdown();
}

} // namespace nao_pos_action_client_ns
Expand Down
Loading

0 comments on commit 1129031

Please sign in to comment.