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.

Sign up for GitHub

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

Jump to bottom

extras: add covariance parsing to vision_speed_estimate #996

Merged
merged 1 commit into from
Apr 11, 2018
Merged

extras: add covariance parsing to vision_speed_estimate #996

merged 1 commit into from
Apr 11, 2018

Conversation

TSC21
Copy link
Member

@TSC21 TSC21 commented Apr 8, 2018

No description provided.

@TSC21 TSC21 self-assigned this Apr 8, 2018
@TSC21 TSC21 requested a review from vooon April 8, 2018 16:05
vooon
vooon requested changes Apr 9, 2018
View reviewed changes
Copy link
Member

@vooon vooon left a comment

Choose a reason for hiding this comment

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

Mostly Ok. Only few minor things.

mavros_extras/src/plugins/vision_speed_estimate.cpp Outdated
@@ -46,6 +48,7 @@ class VisionSpeedEstimatePlugin : public plugin::PluginBase {

if (listen_twist)
vision_vel_sub = sp_nh.subscribe("speed_twist", 10, &VisionSpeedEstimatePlugin::vel_twist_cb, this);

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

Copy link
Member Author

Choose a reason for hiding this comment

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

actually I did this PR quickly and didn't realize I forgot to add the speed_twist_cov option.

mavros_extras/src/plugins/vision_speed_estimate.cpp Outdated
@@ -62,12 +65,11 @@ class VisionSpeedEstimatePlugin : public plugin::PluginBase {

/* -*- low-level send -*- */

void vision_speed_estimate(uint64_t usec, Eigen::Vector3d &v)
void vision_speed_estimate(uint64_t usec, const Eigen::Vector3d &v, const mavros::ftf::Covariance3d &cov)
Copy link
Member

Choose a reason for hiding this comment

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

mavros namespace shold be already used. So just ftf::Covariance3d.

mavros_extras/src/plugins/vision_speed_estimate.cpp Outdated
@@ -76,38 +78,53 @@ class VisionSpeedEstimatePlugin : public plugin::PluginBase {
vs.y = v.y();
vs.z = v.z();
// [[[end]]] (checksum: aee3cc9a73a2e736b7bc6c83ea93abdb)
ftf::covariance_to_mavlink(cov, vs.covariance);
Copy link
Member

Choose a reason for hiding this comment

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

Leave blank lines before and after cog block.

Copy link
Member Author

Choose a reason for hiding this comment

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

Any good reason for it or is just for aesthetics?

mavros_extras/src/plugins/vision_speed_estimate.cpp Outdated
void vel_twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req) {
send_vision_speed(req->twist.linear, req->header.stamp);
void vel_twist_with_cov_cb(const geometry_msgs::TwistStamped::ConstPtr &req) {
ftf::Covariance3d cov {}; // zero initialized
Copy link
Member

Choose a reason for hiding this comment

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

Does mavlink propose some value for incorrect cov? Like [-1, 0, 0...] in ROS msgs.

Copy link
Member Author

Choose a reason for hiding this comment

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

It doesn't. But should be a good idea to bring yes.

mavros_extras/src/plugins/vision_speed_estimate.cpp Outdated

// only the linear velocity will be sent
cov6d = req->twist.covariance;
cov3d_map = cov6d_map.block<3, 3>(0, 0);
Copy link
Member

Choose a reason for hiding this comment

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

Unnecessary copy.

ftf::EigenMapConstCovariance6d cov6d_map(req->twist.covariance);

cov3d_map = cov6d_map.block<3, 3>(0, 0);

Copy link
Member Author

Choose a reason for hiding this comment

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

Makes sense. Thanks

Copy link
Member Author

Choose a reason for hiding this comment

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

ftf::EigenMapConstCovariance6d cov6d_map(req->twist.covariance.data()); btw.

@TSC21 TSC21 force-pushed the pr-vision_speed_cov branch 2 times, most recently from a83c251 to 32fe279 Compare April 9, 2018 12:24
vooon
vooon reviewed Apr 9, 2018
View reviewed changes
Copy link
Member

@vooon vooon left a comment

Choose a reason for hiding this comment

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

What about covariance unused/unknown special value? All 0?

mavros_extras/src/plugins/vision_speed_estimate.cpp Outdated
{
mavlink::common::msg::VISION_SPEED_ESTIMATE vs{};

vs.usec = usec;

Copy link
Member

Choose a reason for hiding this comment

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

You forgot to remove that change.

mavros_extras/src/plugins/vision_speed_estimate.cpp Outdated
//Transform from ENU to NED frame
auto vel = ftf::transform_frame_enu_ned(vel_);
Eigen::Vector3d vel;
tf::vectorMsgToEigen(vel_enu, vel);
Copy link
Member

Choose a reason for hiding this comment

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

Use ftf::to_eigen(), lets remove most of tf:vectorMsgToEigen().

@TSC21
Copy link
Member Author

TSC21 commented Apr 10, 2018

What about covariance unused/unknown special value? All 0?

Still no special value. I will do a RFC on Mavlink to that respect.

@TSC21
Copy link
Member Author

TSC21 commented Apr 10, 2018

Jenkins test this please.

@vooon
Copy link
Member

vooon commented Apr 11, 2018

Jenkins already test it. Builds ok, just mavconn test segfaults again. Also i'm not sure that ros jenkins watch on comments.

When you have if inside if block it is safer to explicitly define block.

@vooon vooon merged commit 5e19b17 into mavlink:master Apr 11, 2018
@vooon vooon added this to the Version 0.25 milestone Apr 11, 2018
@TSC21 TSC21 deleted the pr-vision_speed_cov branch April 12, 2018 17:53
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@vooon vooon vooon requested changes

Assignees

@TSC21 TSC21

Labels
enhancement extras plugin
Projects
None yet
Milestone
Version 0.25
Development

Successfully merging this pull request may close these issues.

None yet
2 participants
@TSC21 @vooon