Skip to content

Commit

Permalink
agent-task-skiller-bridge: send pose info of robot
Browse files Browse the repository at this point in the history
  • Loading branch information
@TarikViehmann
TarikViehmann committed Aug 22, 2024
1 parent 58c6b2f commit f76c68e
Show file tree
Hide file tree
Showing 3 changed files with 94 additions and 26 deletions.
4 changes: 3 additions & 1 deletion src/plugins/agent-task-skiller-bridge/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,8 @@ set(PLUGIN_agent-task-skiller-bridge
CACHE BOOL "Build agent-task-skiller-bridge")

if(PLUGIN_agent-task-skiller-bridge)
add_library(agent-task-skiller-bridge MODULE bridge-plugin.cpp bridge-thread.cpp)
add_library(agent-task-skiller-bridge MODULE bridge-plugin.cpp
bridge-thread.cpp)
depend_on_protobuf(agent-task-skiller-bridge)
depend_on_boost_libs(agent-task-skiller-bridge system)
target_link_libraries(
Expand All @@ -37,6 +38,7 @@ if(PLUGIN_agent-task-skiller-bridge)
fawkesblackboard
fawkesconfig
SkillerInterface
Position3DInterface
rcll-protobuf-msgs
protobuf_comm
protobuf::libprotobuf )
Expand Down
105 changes: 82 additions & 23 deletions src/plugins/agent-task-skiller-bridge/bridge-thread.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,9 @@
#include <core/threading/mutex_locker.h>
#include <llsf_msgs/AgentTask.pb.h>
#include <llsf_msgs/GameState.pb.h>
#include <llsf_msgs/RobotInfo.pb.h>

#include <chrono>

using namespace fawkes;

Expand All @@ -47,7 +50,6 @@ AgentTaskSkillerBridgeThread::~AgentTaskSkillerBridgeThread()
void
AgentTaskSkillerBridgeThread::init()
{
logger->log_info(name(), "try load stuff");
peer_address_ = config->get_string("/agent-task-skiller-bridge/peer_address");
recv_port_magenta_ = config->get_int("/agent-task-skiller-bridge/recv_port_magenta");
recv_port_cyan_ = config->get_int("/agent-task-skiller-bridge/recv_port_cyan");
Expand Down Expand Up @@ -82,10 +84,11 @@ AgentTaskSkillerBridgeThread::init()
logger->log_error(name(), "could not load stuff, i break");
return;
}
logger->log_info(name(), "could load stuff");

skiller_if_ = blackboard->open_for_reading<SkillerInterface>("Skiller");
pos_if_ = blackboard->open_for_reading<Position3DInterface>("Pose");
bbil_add_data_interface(skiller_if_);
bbil_add_data_interface(pos_if_);
blackboard->register_listener(this);
//bbil_add_reader_interface(skiller_if_);

Expand Down Expand Up @@ -214,6 +217,7 @@ AgentTaskSkillerBridgeThread::handle_peer_msg(boost::asio::ip::udp::endpoint &,
this,
boost::placeholders::_1,
boost::placeholders::_2));
team_color_ = "CYAN";
} else if (game_state_msg->team_magenta() == team_name_) {
logger->log_info(name(),
"Listening to MAGENTA peer %s:%i for team %s",
Expand All @@ -234,6 +238,7 @@ AgentTaskSkillerBridgeThread::handle_peer_msg(boost::asio::ip::udp::endpoint &,
this,
boost::placeholders::_1,
boost::placeholders::_2));
team_color_ = "MAGENTA";
} else {
logger->log_info(name(),
"Waiting for RefBox to send information on team %s",
Expand Down Expand Up @@ -291,28 +296,82 @@ void
AgentTaskSkillerBridgeThread::bb_interface_data_refreshed(fawkes::Interface *interface) throw()
{
SkillerInterface *skiller_if = dynamic_cast<SkillerInterface *>(interface);
if (!skiller_if)
return;
skiller_if->read();
if (skiller_if->serial().get_string() != std::string(skiller_if->exclusive_controller())) {
successful_ = false;
error_code_ = 0; // Skiller control lost
wakeup();
return;
if (skiller_if) {
skiller_if->read();
if (skiller_if->serial().get_string() != std::string(skiller_if->exclusive_controller())) {
successful_ = false;
error_code_ = 0; // Skiller control lost
wakeup();
return;
}
switch (skiller_if->status()) {
case fawkes::SkillerInterface::SkillStatusEnum::S_INACTIVE: running_ = false; break;
case fawkes::SkillerInterface::SkillStatusEnum::S_FINAL:
successful_ = true;
running_ = false;
wakeup();
break;
case fawkes::SkillerInterface::SkillStatusEnum::S_RUNNING: running_ = true; break;
case fawkes::SkillerInterface::SkillStatusEnum::S_FAILED:
successful_ = false;
running_ = false;
wakeup();
break;
}
}
switch (skiller_if->status()) {
case fawkes::SkillerInterface::SkillStatusEnum::S_INACTIVE: running_ = false; break;
case fawkes::SkillerInterface::SkillStatusEnum::S_FINAL:
successful_ = true;
running_ = false;
wakeup();
break;
case fawkes::SkillerInterface::SkillStatusEnum::S_RUNNING: running_ = true; break;
case fawkes::SkillerInterface::SkillStatusEnum::S_FAILED:
successful_ = false;
running_ = false;
wakeup();
break;
Position3DInterface *pos_if = dynamic_cast<Position3DInterface *>(interface);
if (pos_if) {
pos_if->read();
rotation_ = pos_if->rotation();
translation_ = pos_if->translation();
send_pose();
}
}
void
AgentTaskSkillerBridgeThread::send_pose()
{
if (private_peer_) {
// Create and populate the outer message (Robot)
llsf_msgs::Robot pose;
pose.set_name(std::to_string(robot_id_));
pose.set_team(team_name_);
pose.set_host("robot");
auto now = std::chrono::system_clock::now();
auto seconds = std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count();
auto nanoseconds =
std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count()
% 1000000000;

llsf_msgs::Time *robot_time = new llsf_msgs::Time();
robot_time->set_sec(static_cast<int32_t>(seconds));
robot_time->set_nsec(static_cast<int32_t>(nanoseconds));
pose.set_allocated_last_seen(robot_time);

if (team_color_ == "CYAN") {
pose.set_team_color(llsf_msgs::CYAN);
} else {
pose.set_team_color(llsf_msgs::MAGENTA);
}

pose.set_number(robot_id_);

llsf_msgs::Time *pose_time = new llsf_msgs::Time();
pose_time->set_sec(static_cast<int32_t>(seconds));
pose_time->set_nsec(static_cast<int32_t>(nanoseconds));

llsf_msgs::Pose2D *pose_info = new llsf_msgs::Pose2D();
pose_info->set_allocated_timestamp(pose_time);
pose_info->set_x(static_cast<float>(translation_[0]));
pose_info->set_y(static_cast<float>(translation_[1]));
pose_info->set_ori(0);

pose.set_allocated_pose(pose_info);

// Log the information
logger->log_info(name(), "Send robot pose (%f,%f)", translation_[0], translation_[1]);

// Send the message using the peer
private_peer_->send(pose);
}
}

Expand Down
11 changes: 9 additions & 2 deletions src/plugins/agent-task-skiller-bridge/bridge-thread.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
#include <blackboard/interface_listener.h>
#include <core/threading/thread.h>
#include <google/protobuf/message.h>
#include <interfaces/Position3DInterface.h>
#include <interfaces/SkillerInterface.h>
#include <llsf_msgs/AgentTask.pb.h>
#include <protobuf_comm/message_register.h>
Expand Down Expand Up @@ -66,8 +67,9 @@ class AgentTaskSkillerBridgeThread : public fawkes::Thread,
}

private:
std::string cfg_skiller_iface_id_;
fawkes::SkillerInterface *skiller_if_;
std::string cfg_skiller_iface_id_;
fawkes::SkillerInterface *skiller_if_;
fawkes::Position3DInterface *pos_if_;

std::shared_ptr<protobuf_comm::ProtobufBroadcastPeer> private_peer_;
std::shared_ptr<protobuf_comm::ProtobufBroadcastPeer> public_peer_;
Expand All @@ -79,9 +81,11 @@ class AgentTaskSkillerBridgeThread : public fawkes::Thread,
std::shared_ptr<google::protobuf::Message> msg);
void handle_peer_recv_error(boost::asio::ip::udp::endpoint &endpoint, std::string msg);
void send_response();
void send_pose();
std::string construct_task_string(const llsf_msgs::AgentTask &agent_task_msg);

std::string team_name_;
std::string team_color_;
std::string crypto_key_;
int robot_id_;

Expand All @@ -98,6 +102,9 @@ class AgentTaskSkillerBridgeThread : public fawkes::Thread,
unsigned short recv_port_cyan_;
unsigned short recv_port_public_;
std::string peer_address_;

double *rotation_;
double *translation_;
};
} // namespace fawkes

Expand Down

0 comments on commit f76c68e

Please # to comment.