Skip to content

Commit

Permalink
update parser
Browse files Browse the repository at this point in the history
  • Loading branch information
@antbono
antbono committed Apr 21, 2024
1 parent 3a1c200 commit 5ceed46
Show file tree
Hide file tree
Showing 22 changed files with 146 additions and 141 deletions.
3 changes: 2 additions & 1 deletion 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 @@ -66,7 +66,8 @@ class NaoPosActionServer : public rclcpp::Node

bool file_successfully_read_ = false;
std::vector<KeyFrame> key_frames_;
std::atomic<bool> pos_in_action_;
//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
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
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
9 changes: 5 additions & 4 deletions nao_pos_server/src/nao_pos_action_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@ float NaoPosActionServer::findElem(const std::vector<uint8_t>& indexes, const st

void NaoPosActionServer::calculateEffectorJoints(nao_lola_sensor_msgs::msg::JointPositions& sensor_joints)
{
std::lock_guard<std::mutex> lock(mutex_);
//std::lock_guard<std::mutex> lock(mutex_);

int time_ms = (rclcpp::Node::now() - initial_time_).nanoseconds() / 1e6;

Expand Down Expand Up @@ -199,6 +199,7 @@ void NaoPosActionServer::calculateEffectorJoints(nao_lola_sensor_msgs::msg::Join
pub_joint_stiffnesses_->publish(effector_joints_stiff);
RCLCPP_DEBUG(this->get_logger(), "published to nao_lola topic");
}

const KeyFrame& NaoPosActionServer::findPreviousKeyFrame(int time_ms)
{
for (auto it = key_frames_.rbegin(); it != key_frames_.rend(); ++it)
Expand Down Expand Up @@ -249,7 +250,7 @@ bool NaoPosActionServer::posFinished(int time_ms)
rclcpp_action::GoalResponse NaoPosActionServer::handleGoal(
const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const nao_pos_interfaces::action::PosPlay::Goal> goal)
{
std::lock_guard<std::mutex> lock(mutex_);
//std::lock_guard<std::mutex> lock(mutex_);
RCLCPP_INFO(get_logger(), ("Received goal request for: " + goal->action_name).c_str());
(void)uuid;
(void)goal;
Expand All @@ -271,7 +272,7 @@ rclcpp_action::GoalResponse NaoPosActionServer::handleGoal(
rclcpp_action::CancelResponse NaoPosActionServer::handleCancel(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<nao_pos_interfaces::action::PosPlay>> goal_handle)
{
std::lock_guard<std::mutex> lock(mutex_);
//std::lock_guard<std::mutex> lock(mutex_);
// RCLCPP_INFO(get_logger(), "Received request to cancel goal");
(void)goal_handle;
pos_in_action_ = false;
Expand All @@ -281,7 +282,7 @@ rclcpp_action::CancelResponse NaoPosActionServer::handleCancel(
void NaoPosActionServer::handleAccepted(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<nao_pos_interfaces::action::PosPlay>> goal_handle)
{
std::lock_guard<std::mutex> lock(mutex_);
//std::lock_guard<std::mutex> lock(mutex_);
RCLCPP_INFO(this->get_logger(), "Starting Pos Action");
initial_time_ = rclcpp::Node::now();
pos_in_action_ = true;
Expand Down
2 changes: 1 addition & 1 deletion nao_pos_server/src/nao_pos_publisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ class NaoPosPublisher : public rclcpp::Node
void timer_callback()
{
auto message = std_msgs::msg::String();
message.data = "only_legs";
message.data = "test_parser1";
RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
publisher_->publish(message);
}
Expand Down
Loading

0 comments on commit 5ceed46

Please sign in to comment.