kinematics_exchanger.h
Go to the documentation of this file.
1 #ifndef KINEMATICS_EXCHANGER_H
2 #define KINEMATICS_EXCHANGER_H
3 
4 #include <map>
5 #include <vector>
6 #include <numeric>
7 #include <ros/ros.h>
8 #include <tf2/utils.h>
9 #include <geometry_msgs/Pose.h>
10 #include <geometry_msgs/PoseStamped.h>
11 #include <geometry_msgs/Twist.h>
12 #include <geometry_msgs/TwistStamped.h>
13 #include <cpswarm_msgs/ArrayOfPositions.h>
14 #include <cpswarm_msgs/Position.h>
15 #include <cpswarm_msgs/Velocity.h>
16 #include <cpswarm_msgs/ArrayOfVectors.h>
17 #include <cpswarm_msgs/VectorStamped.h>
18 
19 using namespace std;
20 using namespace ros;
21 
25 typedef struct polar_vector_t {
26  string uuid;
27  vector<float> mag;
28  vector<float> dir;
31 
35 typedef struct cartesian_vector_t {
36  string uuid;
37  vector<float> x;
38  vector<float> y;
41 
45 geometry_msgs::Pose pose;
46 
51 
55 geometry_msgs::Twist velo;
56 
60 bool vel_valid;
61 
65 map<string, cartesian_vector_t> swarm_positions;
66 
70 map<string, polar_vector_t> swarm_positions_rel;
71 
75 map<string, polar_vector_t> swarm_velocities;
76 
81 
86 
91 
92 #endif // KINEMATICS_EXCHANGER_H
geometry_msgs::Pose pose
Current position of the CPS.
int sample_size
The number of data samples to average over for reliable results.
struct cartesian_vector_t cartesian_vector_t
A vector type in Cartesian format containing UUID of the corresponding CPS together with last updated...
struct polar_vector_t polar_vector_t
A vector type in polar format containing UUID of the corresponding CPS together with last updated tim...
int pos_init
The number of position messages to ignore during initialization. This is because the first messages a...
vector< float > mag
A vector type in polar format containing UUID of the corresponding CPS together with last updated tim...
bool vel_valid
Whether a valid velocity has been received.
map< string, polar_vector_t > swarm_positions_rel
The relative positions of all known swarm members.
geometry_msgs::Twist velo
Current velocity of the CPS.
bool pose_valid
Whether a valid position has been received.
map< string, polar_vector_t > swarm_velocities
The velocities of all known swarm members.
vector< float > dir
A vector type in Cartesian format containing UUID of the corresponding CPS together with last updated...
int vel_init
The number of velocity messages to ignore during initialization. This is because the first messages a...
map< string, cartesian_vector_t > swarm_positions
The absolute positions of all known swarm members.


kinematics_exchanger
Author(s): Micha Sende
autogenerated on Tue Jan 19 2021 03:30:05