24 #include <core/exceptions/system.h>
25 #include <core/exceptions/software.h>
27 #include <netcomm/worldinfo/transceiver.h>
28 #include <netcomm/worldinfo/messages.h>
29 #include <netcomm/worldinfo/encrypt.h>
30 #include <netcomm/worldinfo/decrypt.h>
32 #include <netcomm/socket/datagram_broadcast.h>
33 #include <netcomm/socket/datagram_multicast.h>
34 #include <netcomm/utils/resolver.h>
36 #include <arpa/inet.h>
37 #include <netinet/in.h>
100 const char *addr,
unsigned short port,
101 const char *key,
const char *iv,
103 pose_changed( false ),
104 vel_changed( false ),
105 rel_ball_changed( false ),
106 rel_ball_vel_changed( false ),
107 glob_ball_changed( false ),
108 glob_ball_vel_changed( false ),
109 gamestate_changed( false )
112 switch (socket_type) {
128 e.
append(
"WorldInfoTransceiver cannot instantiate socket for %s:%u", addr, port);
132 in_buffer = malloc(WORLDINFO_MTU);
133 out_buffer = malloc(WORLDINFO_MTU);
134 if (! in_buffer || ! out_buffer) {
138 fatmsg_enabled =
false;
141 fatmsg_header = NULL;
142 fatmsg_msgheader = NULL;
155 crypted_out_buffer = malloc(crypt_buffer_size);
156 crypted_in_buffer = malloc(crypt_buffer_size);
158 if (! crypted_in_buffer || ! crypted_out_buffer) {
166 if ( resolver == NULL ) {
168 resolver_delete =
true;
170 this->resolver = resolver;
171 resolver_delete =
false;
184 free(crypted_out_buffer);
185 free(crypted_in_buffer);
191 if ( resolver_delete ) {
223 if ( this->fatmsg_enabled && ! fatmsg_enabled ) {
227 fatmsg_msgheader = NULL;
228 fatmsg_header = NULL;
231 }
else if (! this->fatmsg_enabled && fatmsg_enabled ) {
235 fatmsg_buf = calloc(1, fatmsg_bufsize);
242 this->fatmsg_enabled = fatmsg_enabled;
255 handlers.push_back(h);
271 handlers.remove_locked(h);
286 time_t limit = time(NULL) - sec;
288 std::map<uint32_t, time_t>::iterator lrtit2;
289 lrtit = last_received_time.begin();
290 while (lrtit != last_received_time.end()) {
291 if ( (*lrtit).second < limit ) {
292 sequence_numbers.erase((*lrtit).first);
295 last_received_time.erase(lrtit2);
326 pose_covariance = covariance;
348 this->vel_theta = vel_theta;
349 this->vel_covariance = covariance;
350 this->vel_changed =
true;
372 rel_ball_dist = dist;
373 rel_ball_bearing = bearing;
374 rel_ball_slope = slope;
375 rel_ball_covariance = covariance;
376 rel_ball_changed =
true;
400 glob_ball_covariance = covariance;
401 glob_ball_changed =
true;
420 rel_ball_visible = visible;
421 rel_ball_visibility_history = visibility_history;
422 rel_ball_changed =
true;
434 glob_ball_visible = visible;
435 glob_ball_visibility_history = visibility_history;
436 glob_ball_changed =
true;
455 rel_ball_vel_x = vel_x;
456 rel_ball_vel_y = vel_y;
457 rel_ball_vel_z = vel_z;
458 rel_ball_vel_covariance = covariance;
459 rel_ball_vel_changed =
true;
478 glob_ball_vel_x = vel_x;
479 glob_ball_vel_y = vel_y;
480 glob_ball_vel_z = vel_z;
481 glob_ball_vel_covariance = covariance;
482 glob_ball_vel_changed =
true;
494 if ((gamestate < 0) || (gamestate >= 16)) {
499 gamestate_changed =
true;
512 gamestate_changed =
true;
523 unsigned int seconds_remaining)
530 penalties[player] = pm;
543 gamestate_changed =
true;
553 gamestate_msg.
half = half;
554 gamestate_changed =
true;
587 float distance,
float bearing,
float *covariance)
589 opponent_t o = { uid,
distance, bearing, covariance };
590 opponents.push_back(o);
603 disappeared_opponents.push_back(uid);
614 WorldInfoTransceiver::append_outbound(uint16_t msg_type,
615 void *msg, uint16_t msg_size)
619 if ( (outbound_bytes +
sizeof(mh) + msg_size ) > WORLDINFO_MTU ) {
624 mh.
type = htons(msg_type);
625 mh.
size = htons(msg_size);
626 memcpy(outbound_buffer, &mh,
sizeof(mh));
628 outbound_bytes +=
sizeof(mh);
629 outbound_buffer +=
sizeof(mh);
632 memcpy(outbound_buffer, msg, msg_size);
633 outbound_bytes += msg_size;
634 outbound_buffer += msg_size;
642 WorldInfoTransceiver::reset_outbound()
644 worldinfo_header_t *header = (worldinfo_header_t *)out_buffer;
645 header->beef = htons(0xBEEF);
646 header->version = WORLDINFO_VERSION;
648 if ( fatmsg_enabled ) {
649 memset(fatmsg_buf, 0, fatmsg_bufsize);
650 fatmsg_header->
beef = htons(0xBEEF);
651 fatmsg_header->
version = WORLDINFO_VERSION;
653 fatmsg_msgheader->
size = htons(
sizeof(worldinfo_fat_message_t));
656 outbound_buffer = (
unsigned char *)out_buffer +
sizeof(worldinfo_header_t);
657 outbound_bytes =
sizeof(worldinfo_header_t);
658 outbound_num_msgs = 0;
673 if ( pose_changed ) {
677 pm.
theta = pose_theta;
679 pose_changed =
false;
683 if ( fatmsg_enabled ) {
685 memcpy(&(fatmsg->
pose), &pm,
sizeof(pm));
689 if ( fatmsg_enabled ) {
704 if ( fatmsg_enabled ) {
706 memcpy(&(fatmsg->
velo), &vm,
sizeof(vm));
710 if ( fatmsg_enabled ) {
715 if ( rel_ball_changed ) {
717 bm.
dist = rel_ball_dist;
719 bm.
slope = rel_ball_slope;
720 bm.
history = rel_ball_visibility_history;
721 bm.
visible = rel_ball_visible ? -1 : 0;
724 rel_ball_changed =
false;
728 if ( fatmsg_enabled ) {
734 if ( fatmsg_enabled ) {
739 if ( glob_ball_changed ) {
744 bm.
history = glob_ball_visibility_history;
745 bm.
visible = glob_ball_visible ? -1 : 0;
748 glob_ball_changed =
false;
753 if ( gamestate_changed ) {
756 gamestate_changed =
false;
759 if ( rel_ball_vel_changed ) {
761 rbvm.
vel_x = rel_ball_vel_x;
762 rbvm.
vel_y = rel_ball_vel_y;
763 rbvm.
vel_z = rel_ball_vel_z;
765 rel_ball_vel_changed =
false;
769 if ( fatmsg_enabled ) {
775 if ( fatmsg_enabled ) {
780 if ( glob_ball_vel_changed ) {
782 rbvm.
vel_x = glob_ball_vel_x;
783 rbvm.
vel_y = glob_ball_vel_y;
784 rbvm.
vel_z = glob_ball_vel_z;
786 glob_ball_vel_changed =
false;
792 for (penit = penalties.begin(); penit != penalties.end(); ++penit) {
799 unsigned int num_opponents = 0;
800 for ( oppit = opponents.begin(); oppit != opponents.end(); ++oppit) {
802 opm.
uid = (*oppit).uid;
803 opm.
dist = (*oppit).distance;
804 opm.
bearing = (*oppit).bearing;
809 if ( fatmsg_enabled ) {
810 if ( num_opponents < WORLDINFO_FATMSG_NUMOPPS ) {
812 memcpy(&(fatmsg->
opponents[num_opponents]), &opm,
sizeof(opm));
820 for ( doppit = disappeared_opponents.begin(); doppit != disappeared_opponents.end(); ++doppit) {
826 disappeared_opponents.clear();
828 if ( outbound_num_msgs > 0 ) {
830 header->
seq = htonl(out_seq++);
833 crypted_out_bytes = encryptor->
encrypt();
835 s->
send(crypted_out_buffer, crypted_out_bytes);
837 if ( fatmsg_enabled ) {
839 fatmsg_header->
seq = htonl(out_seq++);
842 crypted_out_bytes = encryptor->
encrypt();
844 s->
send(crypted_out_buffer, crypted_out_bytes);
885 unsigned int num_msgs = (max_num_msgs == 0 ? 0 : 1);
887 struct sockaddr_in from;
888 socklen_t addr_len =
sizeof(from);
889 size_t bytes = crypt_buffer_size;
891 if ( max_num_msgs != 0 ) ++num_msgs;
893 bytes = s->
recv(crypted_in_buffer, bytes, (
struct sockaddr *)&from, &addr_len);
897 struct in_addr localhost;
898 ::inet_aton(
"127.0.0.1", &localhost);
899 if (from.sin_addr.s_addr == localhost.s_addr) {
907 inbound_bytes = decryptor->
decrypt();
925 if ( ntohs(header->
beef) != 0xBEEF ) {
931 if ( header->
version != WORLDINFO_VERSION ) {
938 unsigned int cseq = ntohl(header->
seq);
939 if ( sequence_numbers.find(from.sin_addr.s_addr) != sequence_numbers.end() ) {
940 if ( cseq <= sequence_numbers[from.sin_addr.s_addr] ) {
946 sequence_numbers[from.sin_addr.s_addr] = cseq;
947 last_received_time[from.sin_addr.s_addr] = time(NULL);
952 std::string hostname_s;
953 if ( ! resolver->
resolve_address((
struct sockaddr *)&from,
sizeof(from), hostname_s) ) {
954 hostname_s =
"unknown";
956 const char *hostname = hostname_s.c_str();
959 while ( inbound_bytes > 0 ) {
963 uint16_t msg_type = ntohs(msgh->
type);
964 uint16_t msg_size = ntohs(msgh->
size);
967 if ( inbound_bytes < msg_size ) {
973 switch ( msg_type ) {
977 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
978 (*hit)->pose_rcvd(hostname,
979 pose_msg->
x, pose_msg->
y, pose_msg->
theta,
992 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
993 (*hit)->velocity_rcvd(hostname,
1007 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
1008 (*hit)->ball_pos_rcvd(hostname,
1023 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
1024 (*hit)->global_ball_pos_rcvd(hostname,
1026 ball_msg->
x, ball_msg->
y, ball_msg->
z,
1039 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
1040 (*hit)->ball_velocity_rcvd(hostname,
1054 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
1055 (*hit)->global_ball_velocity_rcvd(hostname,
1069 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
1070 (*hit)->opponent_pose_rcvd(hostname,
1084 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
1085 (*hit)->opponent_disapp_rcvd(hostname, oppd_msg->
uid);
1097 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
1098 (*hit)->gamestate_rcvd(hostname,
1116 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
1117 (*hit)->penalty_rcvd(hostname,
1131 for ( hit = handlers.begin(); hit != handlers.end(); ++hit ) {
1133 (*hit)->pose_rcvd(hostname,
1139 (*hit)->velocity_rcvd(hostname,
1144 (*hit)->ball_pos_rcvd(hostname,
1151 (*hit)->ball_velocity_rcvd(hostname,
1164 for (
unsigned int i = 0; i < fat_msg->
num_opponents; ++i ) {
1165 (*hit)->opponent_pose_rcvd(hostname,
1186 inbound_bytes -= msg_size;
1187 inbound_buffer += msg_size;
1190 }
while ( s->
available() && (num_msgs <= max_num_msgs) );
1214 return outbound_bytes;
1225 return crypted_out_buffer;
1236 return crypted_out_bytes;
float vel_z
relative velocity of the ball in z direction
void set_rel_ball_pos(float dist, float bearing, float slope, float *covariance)
Set ball position.
void set_glob_ball_velocity(float vel_x, float vel_y, float vel_z, float *covariance)
Set global ball velocity.
Global ball position message.
worldinfo_velocity_message_t velo
sending robot's velocity
uint32_t state_team
Team the game state references.
uint32_t valid_pose
1 if pose is valid, 0 otherwise
Relative ball position message.
void clear_opponents()
Clear opponents list.
void add_disappeared_opponent(unsigned int uid)
Add disappeared opponent.
float covariance[WORLDINFO_COVARIANCE_SIZE_2X2]
opponent position covariance matrix
void send()
Send information.
WorldInfoException(const char *msg)
Constructor.
uint32_t our_goal_color
Our own goal color.
uint32_t penalty
Penalty code, cf.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
void set_team_goal(worldinfo_gamestate_team_t our_color, worldinfo_gamestate_goalcolor_t goal_color)
Set team and goal info.
float bearing
bearing to the ball, this is the angle between the robots forward direction and the ball on the groun...
void set_half(worldinfo_gamestate_half_t half)
Set current half of the game time.
WorldInfo message decryptor.
int32_t visible
-1 if ball visible, 0 otherwise.
float vel_x
relative velocity of the ball in x direction
void set_glob_ball_pos(float x, float y, float z, float *covariance)
Set global ball position.
float covariance[WORLDINFO_COVARIANCE_SIZE_3X3]
ball velocity covariance matrix
virtual size_t recv(void *buf, size_t buf_len)
Read from socket.
uint32_t num_opponents
number of opponents with valid data in opponents
void set_rel_ball_velocity(float vel_x, float vel_y, float vel_z, float *covariance)
Set ball velocity.
void set_loop(bool loop)
Set loopback of sent packets.
uint32_t valid_velo
1 if velo is valid, 0 otherwise
size_t recommended_crypt_buffer_size()
Get recommended crypted buffer size.
void set_plain_buffer(void *buffer, size_t buffer_length)
Set plain buffer.
float slope
slope to the ball, this is the angle between the robots center position on the ground plane and the b...
float covariance[WORLDINFO_COVARIANCE_SIZE_3X3]
ball covariance matrix
Message decryption failed.
void set_crypt_buffer(void *buffer, size_t buffer_length)
Set crypted buffer.
Observed relative ball position.
void set_plain_buffer(void *buffer, size_t buffer_length)
Set plain buffer.
void add_opponent(unsigned int uid, float distance, float bearing, float *covariance)
Add opponent to transmit list.
virtual void bind()
Bind socket.
uint32_t player
Number of penalized robot.
uint32_t score_magenta
Score of team magenta.
uint32_t uid
unique ID of the disappeared opponent
float vel_x
Velocity in X direction.
virtual bool available()
Check if data is available.
uint32_t valid_relball_velo
1 if relball_velo is valid, 0 otherwise
worldinfo_gamestate_half_t
Game time half.
uint32_t score_cyan
Score of team cyan.
float dist
distance to the opponent.
float x
x-coordinate of the global ball positions
float vel_z
global velocity of the ball in z direction
size_t last_sent_plain_buffer_size()
Get last sent plain buffer size.
uint32_t reserved
Reserved for future use.
void add_penalty(unsigned int player, unsigned int penalty, unsigned int seconds_remaining)
Add penalty message.
worldinfo_opppose_message_t opponents[WORLDINFO_FATMSG_NUMOPPS]
best seen opponents
Observed opponent disappered.
worldinfo_pose_message_t pose
sending robot's pose
void set_pose(float x, float y, float theta, float *covariance)
Set global pose of robot.
Base class for exceptions in Fawkes.
uint32_t half
Game time half.
Use multicast socket for communication.
void * last_sent_crypted_buffer()
Get last sent crypted buffer.
Multicast datagram socket.
Broadcast datagram socket.
void set_loop(bool loop)
Set loopback of sent packets.
float vel_theta
Rotational velocity.
float vel_y
relative velocity of the ball in y direction
void set_rel_ball_visible(bool visible, int visibility_history)
Set ball visibility.
float z
z-coordinate of the global ball positions
Sending robot's velocity.
void * last_sent_plain_buffer()
Get last sent plain buffer.
void rem_handler(WorldInfoHandler *h)
Remove handler for world information.
void set_score(unsigned int score_cyan, unsigned int score_magenta)
Set score.
virtual void bind()
Bind socket.
worldinfo_relballpos_message_t relball_pos
ball position relative to sending robot
void recv(bool block=false, unsigned int max_num_msgs=0)
Receive information.
~WorldInfoTransceiver()
Destructor.
float covariance[WORLDINFO_COVARIANCE_SIZE_3X3]
velocity covariance matrix
bool resolve_address(struct sockaddr *addr, socklen_t addr_len, std::string &name)
Resolve address.
uint32_t game_state
Current game state, can be freely chosen, worldinfo_gamestate_t provides recommended values for socce...
Relative ball velocity message.
Sending robot's pose.
int32_t history
visibility history, positive means number of positive observations in a row, 0 means vision has just ...
int32_t history
visibility history, positive means number of positive observations in a row, 0 means vision has just ...
void flush_sequence_numbers(unsigned int sec)
Flush sequence numbers conditionally.
Network name and address resolver.
uint32_t valid_relball_pos
1 if relball_pos is valid, 0 otherwise
void set_gamestate(int gamestate, worldinfo_gamestate_team_t state_team)
Set current game state.
worldinfo_gamestate_team_t
Team.
uint32_t seconds_remaining
Estimate in seconds when unpenalized.
Global ball velocity message.
WorldInfo message encryptor.
worldinfo_relballvelo_message_t relball_velo
ball velocity relative to sending robot
int32_t visible
-1 if ball visible, 0 otherwise.
float covariance[WORLDINFO_COVARIANCE_SIZE_3X3]
position covariance matrix
void set_glob_ball_visible(bool visible, int visibility_history)
Set ball visibility for the global ball.
virtual void send(void *buf, size_t buf_len)
Write to the socket.
Observed relative ball velocity.
void add_handler(WorldInfoHandler *h)
Add a handler for world information.
Fat message containing all the information,.
float vel_y
Velocity in Y direction.
Use broadcase socket for communication.
uint32_t uid
unique ID of this opponent
size_t last_sent_crypted_buffer_size()
Get last sent crypted buffer size.
void set_velocity(float vel_x, float vel_y, float vel_theta, float *covariance)
Set velocity of the robot.
float vel_y
global velocity of the ball in y direction
float bearing
bearing to the opponent, this is the angle between the robots forward direction and the opponent on t...
WorldInfoTransceiver(SocketType socket_type, const char *addr, unsigned short port, const char *key, const char *iv, NetworkNameResolver *resolver=NULL)
Constructor.
float dist
distance to the robot
worldinfo_gamestate_goalcolor_t
Goal color.
float covariance[WORLDINFO_COVARIANCE_SIZE_3X3]
ball covariance matrix
void set_crypt_buffer(void *buffer, size_t buffer_length)
Set crypted buffer.
uint32_t our_team
Our team color.
System ran out of memory and desired operation could not be fulfilled.
void append(const char *format,...)
Append messages to the message list.
float covariance[WORLDINFO_COVARIANCE_SIZE_3X3]
ball velocity covariance matrix
float y
y-coordinate of the global ball positions
void set_fatmsg_enabled(bool fatmsg_enabled)
Enable or disable sending of fat message.
float vel_x
global velocity of the ball in x direction