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 21, 2024
1 parent 3a1c200 commit f71000d
Show file tree
Hide file tree
Showing 23 changed files with 174 additions and 160 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
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
26 changes: 18 additions & 8 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,18 +120,26 @@ 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_);

if (goal_handle_->is_canceling()) {
auto result = std::make_shared<nao_pos_interfaces::action::PosPlay::Result>();
result->success = false;
goal_handle_->canceled(result);
RCLCPP_DEBUG(this->get_logger(), "pos action goal canceled");
return;
}

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

if (posFinished(time_ms))
{
// We've finished the motion, set to DONE
pos_in_action_ = false;
RCLCPP_DEBUG(this->get_logger(), "Pos finished before");
auto result = std::make_shared<nao_pos_interfaces::action::PosPlay::Result>();
result->success = true;
goal_handle_->succeed(result);
RCLCPP_DEBUG(this->get_logger(), "Pos finished after");
RCLCPP_DEBUG(this->get_logger(), "Pos finished");
return;
}

Expand Down Expand Up @@ -198,7 +206,9 @@ void NaoPosActionServer::calculateEffectorJoints(nao_lola_sensor_msgs::msg::Join
pub_joint_positions_->publish(effector_joints);
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 +259,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,8 +281,8 @@ 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_);
// RCLCPP_INFO(get_logger(), "Received request to cancel goal");
//std::lock_guard<std::mutex> lock(mutex_);
RCLCPP_INFO(get_logger(), "Received request to cancel goal");
(void)goal_handle;
pos_in_action_ = false;
goal_handle_.reset();
Expand All @@ -281,12 +291,12 @@ 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;
firstTickSinceActionStarted_ = true;
selected_joints_.clear();
selected_joints_.clear(); // std::vector<uint8_t>
goal_handle_ = goal_handle;
}

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 f71000d

Please sign in to comment.