udp_tracker()
: m_udp_announces(0)
, m_socket(m_ios)
, m_port(0)
{
error_code ec;
m_socket.open(udp::v4(), ec);
if (ec)
{
fprintf(stderr, "UDP Error opening listen UDP tracker socket: %s\n", ec.message().c_str());
return;
}
m_socket.bind(udp::endpoint(address_v4::any(), 0), ec);
if (ec)
{
fprintf(stderr, "UDP Error binding UDP tracker socket to port 0: %s\n", ec.message().c_str());
return;
}
m_port = m_socket.local_endpoint(ec).port();
if (ec)
{
fprintf(stderr, "UDP Error getting local endpoint of UDP tracker socket: %s\n", ec.message().c_str());
return;
}
fprintf(stderr, "%s: UDP tracker initialized on port %d\n", time_now_string(), m_port);
m_thread.reset(new thread(boost::bind(&udp_tracker::thread_fun, this)));
}
dht_server()
: m_dht_requests(0)
, m_socket(m_ios)
, m_port(0)
{
error_code ec;
m_socket.open(udp::v4(), ec);
if (ec)
{
fprintf(stderr, "Error opening listen DHT socket: %s\n", ec.message().c_str());
return;
}
m_socket.bind(udp::endpoint(address_v4::any(), 0), ec);
if (ec)
{
fprintf(stderr, "Error binding DHT socket to port 0: %s\n", ec.message().c_str());
return;
}
m_port = m_socket.local_endpoint(ec).port();
if (ec)
{
fprintf(stderr, "Error getting local endpoint of DHT socket: %s\n", ec.message().c_str());
return;
}
fprintf(stderr, "%s: DHT initialized on port %d\n", time_now_string(), m_port);
m_thread.reset(new libtorrent::thread(boost::bind(&dht_server::thread_fun, this)));
}
udp_tracker()
: m_udp_announces(0)
, m_socket(m_ios)
, m_port(0)
, m_abort(false)
{
error_code ec;
m_socket.open(udp::v4(), ec);
if (ec)
{
std::printf("UDP Error opening listen UDP tracker socket: %s\n", ec.message().c_str());
return;
}
m_socket.bind(udp::endpoint(address_v4::any(), 0), ec);
if (ec)
{
std::printf("UDP Error binding UDP tracker socket to port 0: %s\n", ec.message().c_str());
return;
}
m_port = m_socket.local_endpoint(ec).port();
if (ec)
{
std::printf("UDP Error getting local endpoint of UDP tracker socket: %s\n", ec.message().c_str());
return;
}
std::printf("%s: UDP tracker initialized on port %d\n", time_now_string(), m_port);
m_thread = std::make_shared<std::thread>(&udp_tracker::thread_fun, this);
}
dht_server()
: m_dht_requests(0)
, m_socket(m_ios)
, m_port(0)
{
error_code ec;
m_socket.open(udp::v4(), ec);
if (ec)
{
std::printf("Error opening listen DHT socket: %s\n", ec.message().c_str());
return;
}
m_socket.bind(udp::endpoint(address_v4::any(), 0), ec);
if (ec)
{
std::printf("Error binding DHT socket to port 0: %s\n", ec.message().c_str());
return;
}
m_port = m_socket.local_endpoint(ec).port();
if (ec)
{
std::printf("Error getting local endpoint of DHT socket: %s\n", ec.message().c_str());
return;
}
std::printf("%s: DHT initialized on port %d\n", time_now_string(), m_port);
m_thread = std::make_shared<std::thread>(&dht_server::thread_fun, this);
}
void thread_fun()
{
char buffer[2000];
for (;;)
{
error_code ec;
udp::endpoint from;
int bytes_transferred = m_socket.receive_from(
asio::buffer(buffer, sizeof(buffer)), from, 0, ec);
if (ec == boost::asio::error::operation_aborted
|| ec == boost::asio::error::bad_descriptor) return;
if (ec)
{
fprintf(stderr, "Error receiving on DHT socket: %s\n", ec.message().c_str());
return;
}
try
{
entry msg = bdecode(buffer, buffer + bytes_transferred);
#if defined TORRENT_DEBUG && TORRENT_USE_IOSTREAM
std::cerr << msg << std::endl;
#endif
++m_dht_requests;
}
catch (std::exception& e)
{
fprintf(stderr, "failed to decode DHT message: %s\n", e.what());
}
}
}
void check_deadline(deadline_timer& deadline, udp::socket& socket) {
if (deadline.expires_at() <= deadline_timer::traits_type::now()) {
socket.cancel();
deadline.expires_at(boost::posix_time::pos_infin);
}
deadline.async_wait(boost::bind(&check_deadline, boost::ref(deadline), boost::ref(socket)));
}
void start_receive() {
socket_.async_receive_from(
asio::buffer(recv_buffer_), remote_endpoint_,
boost::bind(&Network::handle_receive, this,
asio::placeholders::error,
asio::placeholders::bytes_transferred));
}
void deadlineCallback(boost::asio::deadline_timer& t, udp::socket& s)
{
if (t.expires_at() <= boost::asio::deadline_timer::traits_type::now())
{
s.cancel();
t.expires_at(boost::posix_time::pos_infin);
}
t.async_wait(boost::bind(&CompactRIOCommunicationNode::deadlineCallback, this, boost::ref(t), boost::ref(s)));
}
void on_next(const core::monitor::message& msg)
{
auto data_ptr = make_safe<std::vector<char>>(write_osc_event(msg));
socket_.async_send_to(boost::asio::buffer(*data_ptr),
endpoint_,
boost::bind(&impl::handle_send_to, this,
boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
}
void receiving()
{
mi_socket_server.async_receive_from(
boost::asio::buffer(recv_buffer), remote_endpoint,
boost::bind(&mi_udp_server::handler, this,
boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
}
void
sendToGroup(udp::socket& sock, const std::vector<uint8_t>& buf, bool needToCheck = true) const
{
sock.async_send_to(boost::asio::buffer(buf), remoteMcastEp,
[needToCheck] (const auto& error, size_t) {
if (needToCheck) {
BOOST_REQUIRE_EQUAL(error, boost::system::errc::success);
}
});
}
void handleRead(const boost::system::error_code& ec, std::size_t ln)
{
ros::Time cur_time = ros::Time::now();
error_ = ec;
if (!socket_.is_open())
return;
if (!ec)
{
std::string msg(data_);
std::istringstream ss;
// ss.unsetf(std::ios::floatfield);
// // set %7.7f
// ss.precision(7);
// ss.width(7);
if (!msg.empty())
{
boost::mutex::scoped_lock lock(mutex_);
double velocity, heading, lat_acc, lon_acc, yaw_rate;
// Emergency: 1 - ok, 0 - fault
bool emergency_state;
ss.str(msg);
ss >> heading >> velocity >> lat_acc >> lon_acc >> yaw_rate >> emergency_state;
kin_->updateState(cur_time, velocity, heading, lat_acc, lon_acc, yaw_rate, emergency_state);
nav_msgs::Odometry msg;
VehicleKinematics::stateStampedMsgToOdometryMsg(kin_->getState(), msg);
// ROS_INFO_STREAM(kin_.getState());
// msg.header.stamp = cur_time;
// msg.header.frame_id = "base_link";
// msg.child_frame_id = "odom";
// Resolve frame names
msg.header.frame_id = tf::resolve(tf_prefix_, msg.header.frame_id);
msg.child_frame_id = tf::resolve(tf_prefix_, msg.child_frame_id);
odom_tf_.header = msg.header;
odom_tf_.child_frame_id = msg.child_frame_id;
odom_tf_.transform.translation.x = msg.pose.pose.position.x;
odom_tf_.transform.translation.y = msg.pose.pose.position.y;
odom_tf_.transform.translation.z = msg.pose.pose.position.z;
odom_tf_.transform.rotation = msg.pose.pose.orientation;
odom_msg_ = msg;
}
void run() {
using namespace boost::posix_time;
boost::system::error_code error;
ptime now;
udp::endpoint endpoint;
while(!this->checked_ || this->alive_) {
socket->receive_from(asio::buffer(buf), endpoint, 0, error);
if(error == boost::system::errc::success) {
last = microsec_clock::local_time();
}
}
}
void thread_fun()
{
char buffer[2000];
for (;;)
{
error_code ec;
udp::endpoint from;
size_t bytes_transferred;
bool done = false;
m_socket.async_receive_from(
boost::asio::buffer(buffer, sizeof(buffer)), from, 0
, boost::bind(&incoming_packet, _1, _2, &bytes_transferred, &ec, &done));
while (!done)
{
m_ios.poll_one();
m_ios.reset();
}
if (ec == boost::asio::error::operation_aborted
|| ec == boost::asio::error::bad_descriptor) return;
if (ec)
{
fprintf(stderr, "Error receiving on DHT socket: %s\n", ec.message().c_str());
return;
}
try
{
entry msg = bdecode(buffer, buffer + bytes_transferred);
#if defined TORRENT_DEBUG && TORRENT_USE_IOSTREAM
std::cerr << msg << std::endl;
#endif
++m_dht_requests;
}
catch (std::exception& e)
{
fprintf(stderr, "failed to decode DHT message: %s\n", e.what());
}
}
}
void thread_fun()
{
char buffer[2000];
error_code ec;
udp::endpoint from;
m_socket.async_receive_from(
boost::asio::buffer(buffer, int(sizeof(buffer))), from, 0
, std::bind(&udp_tracker::on_udp_receive, this, _1, _2, &from, &buffer[0], int(sizeof(buffer))));
m_ios.run(ec);
if (ec)
{
std::printf("UDP Error running UDP tracker service: %s\n", ec.message().c_str());
return;
}
std::printf("UDP exiting UDP tracker thread\n");
}
void stop()
{
m_abort = true;
m_socket.cancel();
m_socket.close();
}
~dht_server()
{
m_socket.cancel();
m_socket.close();
if (m_thread) m_thread->join();
}
void handler(const boost::system::error_code& error, std::size_t /*bytes_transferred*/, udp::socket &sock, udp::endpoint *end)
{
switch(reinterpret_cast<t_Packet *>(ReceiveBuffer)->type)
{
case 0:
{
cout<<"I got a default packet"<<endl;
}
break;
case 1:
{
cout<<"Got a ping packet"<<endl;
cout<<"Its from "<<end->address().to_string()<<endl;
t_pingPacket pingPacket(*reinterpret_cast<t_pingPacket *>(ReceiveBuffer));
cout<<"The time difference was an amazing "<<(boost::posix_time::microsec_clock::universal_time().time_of_day().total_microseconds() - pingPacket.time)<<endl;
cout<<"Sending it back...\n\n"<<endl;
sock.send_to(boost::asio::buffer(ReceiveBuffer),*end);
}
break;
case 2:
{
cout<<"Someone from "<<end->address().to_string()<<" is trying to join"<<endl;
t_connectPacket connectPacket;
bm::right_map::const_iterator lol = table.right.find(*end);
if (table.right.end() == lol)
{
cout<<"They are not already connected"<<endl;
connectPacket.id = CurId;
table.insert(bm::value_type(CurId++,*end));
}
else
{
cout<<"They connected before"<<endl;
connectPacket.id=lol->second;
}
sock.send_to(boost::asio::buffer(&connectPacket,sizeof(connectPacket)),*end);
cout<<"We gave them an id of "<<(connectPacket.id)<<endl<<endl;
}
break;
case 3:
{
cout<<"Someone from "<<end->address().to_string()<<" with id "<<table.right.find(*end)->second<<" wants the world"<<endl;
t_worldPacket worldPacket(*reinterpret_cast<t_worldPacket *>(ReceiveBuffer));
worldPacket.numOfObjects = 6;
cout<<"Telling them the world has five objects"<<endl<<endl;
sock.send_to(boost::asio::buffer(&worldPacket,sizeof(worldPacket)),*end);
}
break;
case 4:
{
cout<<"Someone from "<<end->address().to_string()<<" with id "<<table.right.find(*end)->second<<" wants some objects"<<endl;
t_objectPacket objectPacket(*reinterpret_cast<t_objectPacket *>(ReceiveBuffer));
for (int i = 0;i<objectPacket.numOfObjects;i++)
{
cout<<"He wants the object "<<objectPacket.numbers[i]<<endl;
}
cout<<"\nMaking new packet"<<endl;
t_objectPacket *newObjectPacket = reinterpret_cast<t_objectPacket *>(malloc(sizeof(t_objectPacket) + objectPacket.numOfObjects * sizeof(t_objectData)));
memcpy(newObjectPacket,&objectPacket,sizeof(objectPacket));
cout<<"Created new packet of right size(hopefully)"<<endl;
cout<<"Setting them all to zero, so I can be lazy"<<endl;
//memset(newObjectPacket->objectData,0,newObjectPacket->numOfObjects * sizeof(t_objectData));
cout<<"Setting first to name ";
strcpy(newObjectPacket->objectData[0].name,"wow");
cout<<newObjectPacket->objectData[0].name<<endl;
cout<<"Finially sending the stupid thing over, it has a size of "<<(sizeof(t_objectPacket) + objectPacket.numOfObjects * sizeof(t_objectData))<<endl<<endl;
sock.send_to(boost::asio::buffer(newObjectPacket,sizeof(t_objectPacket) + objectPacket.numOfObjects * sizeof(t_objectData)),*end);
}
default:
cout<<"Who knows what packet I got!!"<<endl;
}
sock.async_receive_from(boost::asio::buffer(ReceiveBuffer),*end,boost::bind(handler,
boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred,
boost::ref(sock),
end));
}
int init()
{
// Use global namespace for node
node_ = ros::NodeHandlePtr(new ros::NodeHandle());
// Get tf_prefix from global namespace
node_->param("tf_prefix", tf_prefix_, std::string(""));
// Use private namespace for parameters
pnode_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
pnode_->param("publish_rate", publish_rate_, PUBLISH_RATE);
pnode_->param("fixed_frame_id", fixed_frame_id_, std::string("odom"));
fixed_frame_id_ = tf::resolve(tf_prefix_, fixed_frame_id_);
pnode_->param("publish_odom_tf", publish_odom_tf_, true);
pnode_->param("crio/ip", crio_ip_, std::string("127.0.0.1"));
pnode_->param("crio/command_port", crio_cmd_port_, std::string("39000"));
pnode_->param("crio/state_port", crio_state_port_, std::string("39001"));
pnode_->param("crio/socket_timeout", socket_timeout_, 10);
VehicleKinematics::Parameters kin_params;
tfScalar minimum_radius_outer;
if (!pnode_->getParam("kinematics/frame_id", kin_params.frame_id))
{
ROS_WARN_STREAM(pnode_->getNamespace() << "/kinematics/frame_id parameter is not set");
}
else
{
kin_params.frame_id = tf::resolve(tf_prefix_, kin_params.frame_id);
}
if (!pnode_->getParam("kinematics/wheelbase", kin_params.wheelbase_length))
{
ROS_FATAL_STREAM(pnode_->getNamespace() << "/kinematics/wheelbase parameter is not set");
return -1;
}
if (!pnode_->getParam("kinematics/track", kin_params.track_width))
{
ROS_FATAL_STREAM( pnode_->getNamespace() << "/kinematics/track parameter is not set");
return -1;
}
if (!pnode_->getParam("kinematics/rotation_center", kin_params.rotation_center))
{
ROS_WARN_STREAM(
pnode_->getNamespace()
<< "/kinematics/rotation_center parameter is not set. Using default: wheelbase/2 = "
<< kin_params.wheelbase_length / 2);
kin_params.rotation_center = kin_params.wheelbase_length / 2;
}
if (!pnode_->getParam("kinematics/minimum_radius_outer", minimum_radius_outer))
{
ROS_FATAL_STREAM(pnode_->getNamespace() << "/kinematics/minimum_radius_outer parameter is not set");
return -1;
}
else
{
kin_params.minimum_radius = minimum_radius_outer - kin_params.track_width / 2;
}
if (!pnode_->getParam("kinematics/steering_ratio", kin_params.steering_ratio))
{
ROS_FATAL_STREAM(pnode_->getNamespace() << "/kinematics/steering_ratio parameter is not set");
return -1;
}
// kin_(2.7, 1.626, 1.35);
kin_ = boost::make_shared<VehicleKinematics>(kin_params);
sub_ = node_->subscribe<kut_ugv_msgs::MotionCommandStamped>("motion_command", 200,
&CompactRIOCommunicationNode::motionCommand, this);
odom_pub_ = node_->advertise<nav_msgs::Odometry>("odom", 200);
state_pub_ = node_->advertise<kut_ugv_vehicle::StateStamped>("state", 200);
tf_br_ = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster);
timeout_ = boost::posix_time::seconds(socket_timeout_);
send_ep_ = udp::endpoint(boost::asio::ip::address::from_string(crio_ip_), std::atoi(crio_cmd_port_.c_str()));
receive_ep_ = udp::endpoint(udp::v4(), std::atoi(crio_state_port_.c_str()));
socket_.open(udp::v4());
deadline_.expires_at(boost::posix_time::pos_infin);
this->deadlineCallback(deadline_, socket_);
return 0;
}
~udp_tracker()
{
m_socket.cancel();
m_socket.close();
if (m_thread) m_thread->join();
}
void on_udp_receive(error_code const& ec, size_t bytes_transferred, udp::endpoint* from, char* buffer, int size)
{
if (ec)
{
std::printf("%s: UDP tracker, read failed: %s\n", time_now_string(), ec.message().c_str());
return;
}
if (bytes_transferred < 16)
{
std::printf("%s: UDP message too short (from: %s)\n", time_now_string(), print_endpoint(*from).c_str());
return;
}
if (m_abort)
{
return;
}
std::printf("%s: UDP message %d bytes\n", time_now_string(), int(bytes_transferred));
char* ptr = buffer;
detail::read_uint64(ptr);
std::uint32_t const action = detail::read_uint32(ptr);
std::uint32_t const transaction_id = detail::read_uint32(ptr);
error_code e;
switch (action)
{
case 0: // connect
std::printf("%s: UDP connect from %s\n", time_now_string()
, print_endpoint(*from).c_str());
ptr = buffer;
detail::write_uint32(0, ptr); // action = connect
detail::write_uint32(transaction_id, ptr); // transaction_id
detail::write_uint64(10, ptr); // connection_id
m_socket.send_to(boost::asio::buffer(buffer, 16), *from, 0, e);
if (e) std::printf("%s: UDP send_to failed. ERROR: %s\n"
, time_now_string(), e.message().c_str());
else std::printf("%s: UDP sent response to: %s\n"
, time_now_string(), print_endpoint(*from).c_str());
break;
case 1: // announce
++m_udp_announces;
std::printf("%s: UDP announce [%d]\n", time_now_string()
, int(m_udp_announces));
ptr = buffer;
detail::write_uint32(1, ptr); // action = announce
detail::write_uint32(transaction_id, ptr); // transaction_id
detail::write_uint32(1800, ptr); // interval
detail::write_uint32(1, ptr); // incomplete
detail::write_uint32(1, ptr); // complete
// 0 peers
m_socket.send_to(boost::asio::buffer(buffer, 20), *from, 0, e);
if (e) std::printf("%s: UDP send_to failed. ERROR: %s\n"
, time_now_string(), e.message().c_str());
else std::printf("%s: UDP sent response to: %s\n"
, time_now_string(), print_endpoint(*from).c_str());
break;
case 2:
// ignore scrapes
std::printf("%s: UDP scrape (ignored)\n", time_now_string());
break;
default:
std::printf("%s: UDP unknown message: %d\n", time_now_string()
, action);
break;
}
m_socket.async_receive_from(
boost::asio::buffer(buffer, size), *from, 0
, std::bind(&udp_tracker::on_udp_receive, this, _1, _2, from, buffer, size));
}
int BasicASIOTransport::setup_socket(udp::socket& socket)
{
int tar_buff_size(settings_.queue_length);
asio::socket_base::send_buffer_size send_buffer_option;
asio::socket_base::receive_buffer_size receive_buffer_option;
try
{
socket.get_option(send_buffer_option);
int send_buff_size = send_buffer_option.value();
socket.get_option(receive_buffer_option);
int rcv_buff_size = receive_buffer_option.value();
madara_logger_log(context_.get_logger(), logger::LOG_MAJOR,
"BasicASIOTransport::setup:"
" default socket buff size is send=%d, rcv=%d\n",
send_buff_size, rcv_buff_size);
if (send_buff_size < tar_buff_size)
{
madara_logger_log(context_.get_logger(), logger::LOG_MAJOR,
"BasicASIOTransport::setup:"
" setting send buff size to settings.queue_length (%d)\n",
tar_buff_size);
send_buffer_option = tar_buff_size;
socket.set_option(send_buffer_option);
socket.get_option(send_buffer_option);
send_buff_size = send_buffer_option.value();
madara_logger_log(context_.get_logger(), logger::LOG_MAJOR,
"BasicASIOTransport::setup:"
" current socket buff size is send=%d, rcv=%d\n",
send_buff_size, rcv_buff_size);
}
if (rcv_buff_size < tar_buff_size)
{
madara_logger_log(context_.get_logger(), logger::LOG_MAJOR,
"BasicASIOTransport::setup:"
" setting rcv buff size to settings.queue_length (%d)\n",
tar_buff_size);
receive_buffer_option = tar_buff_size;
socket.set_option(receive_buffer_option);
socket.get_option(receive_buffer_option);
rcv_buff_size = receive_buffer_option.value();
madara_logger_log(context_.get_logger(), logger::LOG_MAJOR,
"BasicASIOTransport::setup:"
" current socket buff size is send=%d, rcv=%d\n",
send_buff_size, rcv_buff_size);
}
}
catch (const boost::system::system_error& e)
{
madara_logger_log(context_.get_logger(), logger::LOG_MAJOR,
"BasicASIOTransport::setup:"
" Error setting up sockets: %s\n",
e.what());
this->invalidate_transport();
return -1;
}
return 0;
}