-
Notifications
You must be signed in to change notification settings - Fork 993
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
extras: mocap_fake_gps to fake_gps #742
Conversation
…cLib possibilites
03c4ed4
to
66fd20f
Compare
@rmackay9 FYI |
/* 16 */ "UNASSGINED3", // | ||
/* 17 */ "EMERGENCY_SURFACE", // | ||
/* 18 */ "SERVICE_SURFACE", // | ||
/* 19 */ "POINT_OBSTACLE", // |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Should not be done! That brokes cog.py md5sum.
If that done uncrustify, then re-run cog on that file (possible you need cog.py -x
first).
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
It was cog that did that after I cleared the md5sum. Didn't run uncrustify on this file.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
But what remove trailing spaces?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I deleted all the md5sum's of the code befor running cog to add the new enumeration, and it did that. Maybe a change on pymavlink?
int ft_i; | ||
fp_nh.param<int>("fix_type", ft_i, utils::enum_value(GPS_FIX_TYPE::NO_GPS)); | ||
fix_type = static_cast<GPS_FIX_TYPE>(ft_i); | ||
fp_nh.param("gps_period", _gps_period, 0.2); // GPS data rate of 5hz |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why period? Rate may be better.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Well bellow it is used to add throttle to the msg (I kept Thomas version on that). But I suppose that can br changed.
Eigen::Vector3d old_ecef; //!< previous geocentric position [m] | ||
double old_stamp; //!< previous stamp [s] | ||
|
||
inline Eigen::Vector3d enu_to_ecef_transform(const Eigen::Affine3d &map_point) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I see that function second time! Candidate for geo_utils.h
.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The other is ecef_to_enu_transform
. Is the reverse transform.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Is the same as the inverse of the other rotation matrix basically. So I suppose it can be in a header file. Though, since this is a coordinate frame transform (both are local coordinates), this can go to the same place as the other frame transforms are. What do you think? We keep it here or move it?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
frame_tf.h
mostly about local coords, but it's ok too.
Maybe place it into gtf
namespace?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
And generalize names to be consistent with ftf::
. And perhaps make it normal function.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
ECEF is a geographical coordinate system but represents the coordinates in a cartesian way (X,Y,Z just like local coordinates).
Yeah I will find a way of generalizing this.
fix.lon = geodetic.y() * 1e7; // [degrees * 1e7] | ||
fix.alt = (geodetic.z() + GeographicLib::Geoid::ELLIPSOIDTOGEOID * | ||
(*m_uas->egm96_5)(geodetic.x(), geodetic.y())) * 1e3; // [meters * 1e3] | ||
fix.vel = sqrt(vel.x() * vel.x() + vel.y() * vel.y()); // [cm/s] |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Check Eigen API, there should be method to get vector length.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
But perhaps you need 2d vector.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yeah I suppose. Kept the previous version here. Will check it
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
But perhaps you need 2d vector
Yeah this is velocity in XY plane. Maybe we can keep it this way instead of creating an auxiliar vector?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Something like that:
auto l = v3d.block<2, 1>(0, 0).norm();
http://eigen.tuxfamily.org/dox/classEigen_1_1MatrixBase.html#a8c247d700df304eb9a560bab60335ee1
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yeah I suppose that works but at a computational level, would that really be more efficient?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Who knows :)
0000000000000000 <_Z2m1RN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEE>:
0: f2 0f 10 07 movsd (%rdi),%xmm0
4: f2 0f 10 4f 08 movsd 0x8(%rdi),%xmm1
9: f2 0f 59 c0 mulsd %xmm0,%xmm0
d: f2 0f 59 c9 mulsd %xmm1,%xmm1
11: 66 0f ef d2 pxor %xmm2,%xmm2
15: f2 0f 58 c1 addsd %xmm1,%xmm0
19: 66 0f 2e d0 ucomisd %xmm0,%xmm2
1d: f2 0f 51 c8 sqrtsd %xmm0,%xmm1
21: 77 05 ja 28 <_Z2m1RN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEE+0x28>
23: 66 0f 28 c1 movapd %xmm1,%xmm0
27: c3 retq
28: 48 83 ec 18 sub $0x18,%rsp
2c: f2 0f 11 4c 24 08 movsd %xmm1,0x8(%rsp)
32: e8 00 00 00 00 callq 37 <_Z2m1RN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEE+0x37>
37: f2 0f 10 4c 24 08 movsd 0x8(%rsp),%xmm1
3d: 48 83 c4 18 add $0x18,%rsp
41: 66 0f 28 c1 movapd %xmm1,%xmm0
45: c3 retq
46: 66 2e 0f 1f 84 00 00 nopw %cs:0x0(%rax,%rax,1)
4d: 00 00 00
0000000000000050 <_Z2m2RN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEE>:
50: 66 0f 10 07 movupd (%rdi),%xmm0
54: 66 0f 59 c0 mulpd %xmm0,%xmm0
58: 66 0f 28 c8 movapd %xmm0,%xmm1
5c: 66 0f 15 c8 unpckhpd %xmm0,%xmm1
60: f2 0f 58 c1 addsd %xmm1,%xmm0
64: 66 0f 13 44 24 f0 movlpd %xmm0,-0x10(%rsp)
6a: f2 0f 10 44 24 f0 movsd -0x10(%rsp),%xmm0
70: f2 0f 51 c0 sqrtsd %xmm0,%xmm0
74: c3 retq
fp_nh.param("gps_period", _gps_period, 0.2); // GPS data rate of 5hz | ||
gps_period = ros::Duration(_gps_period); | ||
fp_nh.param("gps_rate", _gps_rate, 5.0); // GPS data rate of 5hz | ||
gps_rate: _gps_rate; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
What is that? Constructor call?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Inits Rate constructor with _gps_rate
value.
@@ -266,6 +225,9 @@ class FakeGPSPlugin : public plugin::PluginBase, | |||
old_ecef = current_ecef; | |||
|
|||
UAS_FCU(m_uas)->send_message_ignore_drop(fix); | |||
|
|||
// Throttle incoming messages to 5hz | |||
gps_rate.sleep(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
No! Sleeping here is not good idea. You may convert Rate to Duration.
Or look how rate works, maybe it has "elapsed" method?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
There are three ways of we can throttle the msg: one is the way it was before, the other is using Ros::Timer
and the other is using Ros::Rate
, which I'm doing now.
http://answers.ros.org/question/194418/change-publish-rate-of-a-topic/
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Done. Applied the previous method with conversion from Rate
to Duration
so then I'm sure it will function properly.
This PR:
mocap_fake_gps
byfake_gps
, by generalizing the plugin to other pose sources besides MoCap, like vision-based ones, and letting the user choose which one he wants;GeographicLib
to compute geocentric coordinates and velocities and apply the reverse translation to geodetic coordinates (using the WGS-84 ellipsoid);eph
,epv
, GPS data rate and number of virtual visible satellites to the faked GPS;GPS_FIX_TYPE
enumeration.