Skip to content
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

Merged
merged 8 commits into from
Jun 28, 2017
Merged

extras: mocap_fake_gps to fake_gps #742

merged 8 commits into from
Jun 28, 2017

Conversation

TSC21
Copy link
Member

@TSC21 TSC21 commented Jun 27, 2017

This PR:

  1. Replaces mocap_fake_gps by fake_gps, by generalizing the plugin to other pose sources besides MoCap, like vision-based ones, and letting the user choose which one he wants;
  2. Uses the capabilities of GeographicLib to compute geocentric coordinates and velocities and apply the reverse translation to geodetic coordinates (using the WGS-84 ellipsoid);
  3. Allows user input of the origin coordinates, eph, epv, GPS data rate and number of virtual visible satellites to the faked GPS;
  4. Adds GPS_FIX_TYPE enumeration.

@TSC21
Copy link
Member Author

TSC21 commented Jun 27, 2017

@rmackay9 FYI

/* 16 */ "UNASSGINED3", //
/* 17 */ "EMERGENCY_SURFACE", //
/* 18 */ "SERVICE_SURFACE", //
/* 19 */ "POINT_OBSTACLE", //
Copy link
Member

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).

Copy link
Member Author

@TSC21 TSC21 Jun 28, 2017

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.

Copy link
Member

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?

Copy link
Member Author

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
Copy link
Member

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.

Copy link
Member Author

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)
Copy link
Member

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.

Copy link
Member Author

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.

Copy link
Member Author

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?

Copy link
Member

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?

Copy link
Member

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.

Copy link
Member Author

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]
Copy link
Member

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.

Copy link
Member

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.

Copy link
Member Author

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

Copy link
Member Author

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?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy link
Member Author

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?

Copy link
Member

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;
Copy link
Member

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?

Copy link
Member Author

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();
Copy link
Member

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?

Copy link
Member Author

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/

Copy link
Member Author

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.

@TSC21 TSC21 closed this Jun 28, 2017
@TSC21 TSC21 reopened this Jun 28, 2017
@TSC21 TSC21 merged commit f2b8421 into mavlink:master Jun 28, 2017
@TSC21 TSC21 deleted the fake_gps branch June 28, 2017 15:52
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants