distance_sensor: Initialize sensor orientation quaternion to zero #1652
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Without this, you'll get random garbage data for the quaternion field of the DISTANCE_SENSOR MAVLink messages sent to the autopilot. The quaternion field should be set to zero when unused, according to the MAVLink message's field description.
vooon
approved these changes
Nov 23, 2021
Yeah, I initially expected the same. The only good reason I can see for the Eigen quaternion constructor to not initialize is possibly performance, which might make sense for a linear algebra library. |
vooon
added a commit
that referenced
this pull request
Nov 24, 2021
Eigen::Quaternion[d|f] () does not initialize with zeroes or identity. So we must initialize with identity vector objects that can be left unassigned. Related to #1652 Signed-off-by: Vladimir Ermakov <vooon341@gmail.com>
chengguizi
added a commit
to chengguizi/mavros
that referenced
this pull request
Mar 26, 2022
* bugfix - add estimator type in odom message Add missing estimator_type field in Odometry message. Issue mavlink#1524 * readme: add source install note for Noetic release Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * update changelog * 1.5.2 * Fixed a bug in mavros_extras/src/plugins/odom.cpp by switching lines 175 and 180. Rationale: The pose covariance matrix published to the /mavros/odometry/in topic is exclusively zeros. This is because the transformation matrix r_pose is initialised as zeros (line 140), then applied to the covariance matrix cov_pose (line 176) and then populated (line 180). Clearly the latter two steps should be the other way around, and the comments in the code appear to suggest that this was the intention, but that lines 175 and 180 were accidentally written the wrong way around. Having switched them, the pose covariance is now published to /mavros/odometry/in as expected. JohnG897 * fix inconsistency in direction of yaw when using set_position in BODY frames and fix problems with yaw in setponit_raw * update changelog Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * 1.6.0 * Fix mavlink#849 * README: Update PX4 Autopilot references Much needed fixes to clarify the project is named correctly throughout the README for the PX4 Autopilot, QGroundControl, and MAVLink * MissionBase: breakout mission protocol from waypoint.cpp * Waypoint: inherit MissionBase class for mission protocol * Rallypoint: add rallypoint plugin * Geofence: add geofence plugin * add rallypoint and geofence plugins to CMakeList * add rallypoint and geofence plugins to mavros plugins xml * whitespace * uncrustify * MissionBase: add copyright from origional waypoint.cpp * MissionBase: correction to file information * plugins: mission: re-generate the code * lib: re-generate the code * msgs: re-generate the code * update changelog * 1.7.0 * re-generate all pymavlink enums * update changelog * 1.7.1 * convert whole expression to mm * distance_sensor: Fill horizontal_fov, vertical_fov, quaternion * distance_sensor: Add horizontal_fov_ratio, vertical_fov_ratio, sensor_orientation parameters * px4_config: Add distance_sensor parameters * extras: distance_sensor: rename param for custom orientation, apply uncrustify Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * lib: ftf: allow both Quaterniond and Quaternionf for quaternion_to_mavlink() * extras: mavlink#1370: set obstacle aangle offset * ci: install geographiclib datasets * ci: github uses yaml parser which do not support anchors. surprise, surprise! * Create codeql-analysis.yml * Create semgrep-analysis.yml * update changelog * 1.8.0 * readme: update * ci: remove travis * ci: update industrial-ci usage * Add compass calibration feedback status. Add service to call the 'Next' button in calibrations. * Add message and service definition. * Remove extra message from CMakeLists. * Move Compass calibration report to extras. Rewrite code based on instructions. * Set progress array to global to prevent erasing data. * Apply uncrustify changes. * Delete debug files. * Exclude changes to launch files. * Reset calibration flag when re-calibrating. Prevent wrong data output. * Add Mount angles message for communications with ardupilotmega. * added esc_telemetry plugin * fixed some compile errors * actually allocate memory for the telemetry information * msgs: fix types for apm's esc telemetry * extras: fix apm esc_telemetry * extras: reformat plugins xml Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * plugins: reformat xml * extras: uncrustify all plugins Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * extras: fix esc_telemetry centi-volt/amp conversion Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * extras: esc_telemetry: fix build Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * Remove Mount_Status plugin. Add Status data to Mount_Control plugin. Remove Mount_Status message. * Add missing subscription. * Publish quaternion information with Mount Status mavlink message. * Convert status data from cdeg to rad. * Update esc_status plugin with datatype change on MAVLink. ESC_INFO MAVLink message was updated to have negative temperates and also at a different resolution. This commit updates those changes on this side. Signed-off-by: Ricardo Marques <marques.ricardo17@gmail.com> * Added GPS_INPUT plugin * Added NAV_CONTROLLER_OUTPUT Plugin * Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 * publish BATTERY2 message as /mavros/battery2 topic * This adds functionality to erase all logs on the SD card via mavlink * Changed OverrideRCIn to 18 channels * Spelling corrections * update changelog Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * 1.9.0 * sys_time: publish /clock for simulation times * sys_time.cpp: typo * MountControl.msg: fix copy-paste * mavros_msgs: Add Tunnel message * mavros_extras: Create tunnel plugin * Tunnel.msg: Generate enum with cog * Tunnel: Check for invalid payload length * Show ENOTCONN error instead of crash When a client suddenly drops the connection, socket.shutdown() will throw an exception: boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::system::system_error> > what(): shutdown: Transport endpoint is not connected Showing an error in this common case looks more reasonable than crashing. * Catch std::length_error in send_message Instead of crashing the process * Remove reference * msgs: update gpsraw to have yaw field * prepare release Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * 1.10.0 * msgs: add yaw field to GPS_INPUT * Add camera plugin for interfacing with mavlink camera protocol Add camera image captured message for handling camera trigger information * Address review comments * distance_sensor: Initialize sensor orientation quaternion to zero Without this, you'll get random garbage data for the quaternion field of the DISTANCE_SENSOR MAVLink messages sent to the autopilot. The quaternion field should be set to zero when unused, according to the MAVLink message's field description. * Use meters for relative altitude * plugin: sys: Use wall timers for connection management Fixes mavlink#1629 Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * plugin: initialize quaternions with identity Eigen::Quaternion[d|f] () does not initialize with zeroes or identity. So we must initialize with identity vector objects that can be left unassigned. Related to mavlink#1652 Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * plugin: sys: fix compillation error Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * msgs: try to hide 'unaligned pointer' warning * plugin: setpoint_raw: fix misprint Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * msgs: fix convert const * extras: landing_target: fix misprint Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * msgs: use pragmas to ignore unaligned pointer warnings * lib: fix ftf warnings Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * update changelog Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * 1.11.0 * lib: fix build * update changelog Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * 1.11.1 * extras: distance_sensor: revert back to zero quaternion Fix mavlink#1653 Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * lib: fix mission frame debug print Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * Fix multiple bugs - fix bad_weak_ptr on connect and disconnect - introduce new API to avoid thread race when assigning callbacks - fix uninitialized variable in TCP client constructor which would randomly block TCP server This is an API breaking change: if client code creates connections using make_shared<>() instead of open_url(), it is now necessary to call new connect() method explicitly. * update changelog Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * 1.12.0 * mavros: Fix some warnings * mavros_extras: Fix some warnings * mavconn: fix connection issue introduced by mavlink#1658 Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * update changelog Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * 1.12.1 * extras: trajectory: backport mavlink#1667 Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * plugin: setpoint_raw: move getParam to initializer Repeatedly getting the thrust_scaling parameter in a callback that can be invoked from a fast control loop may fail spuriously and trigger a fatal error * Set time/publish_sim_time to false by default * update changelog Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * 1.12.2 * Add plugin for reporting terrain height estimate from FCU * Fixed topic names to match more closely what other plugins use. Fixed a typo. * Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html * Fixed callback name to match `handle_{MESSAGE_NAME.lower()}` convention * More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages * Removed CamelCase for class members. Publish to "report" * lib: Fix rotation search for CUSTOM Fix mavlink#1688. * test: add checks for ROTATION_CUSTOM * re-generate all coglets * py-lib: fix compatibility with py3 for Noetic * update changelog Signed-off-by: Vladimir Ermakov <vooon341@gmail.com> * 1.13.0 Co-authored-by: Ashwin Varghese Kuruttukulam <35679537+ashwinvk94@users.noreply.github.com> Co-authored-by: Vladimir Ermakov <vooon341@gmail.com> Co-authored-by: John Gifford <john.gifford@leonardo-itg.com> Co-authored-by: zhouzhiwen2000 <zhouzhiwen2000@gmail.com> Co-authored-by: Tobias Fischer <info@tobiasfischer.info> Co-authored-by: Ramon Roche <mrpollo@gmail.com> Co-authored-by: Charlie-Burge <charlie.burge@manna.aero> Co-authored-by: Thomas <thomas@rigi.tech> Co-authored-by: Alexey Rogachevskiy <sfalexrog@gmail.com> Co-authored-by: André Filipe <andre.ferreira@beyond-vision.pt> Co-authored-by: BV-OpenSource <opensource@beyond-vision.pt> Co-authored-by: Russell <russell@rizse.io> Co-authored-by: Abhijith Thottumadayil Jagadeesh <abhijith@rizse.io> Co-authored-by: Ricardo Marques <marques.ricardo17@gmail.com> Co-authored-by: Dr.-Ing. Amilcar do Carmo Lucas <amilcar.lucas@iav.de> Co-authored-by: Karthik Desai <karthik.desai@iav.de> Co-authored-by: David Jablonski <dayjaby@gmail.com> Co-authored-by: Morten Fyhn Amundsen <morten.fyhn.amundsen@gmail.com> Co-authored-by: Val Doroshchuk <val@sevendof.com> Co-authored-by: Marcelino Almeida <marcelino@pensasystems.com> Co-authored-by: Jaeyoung-Lim <jalim@ethz.ch> Co-authored-by: Alexander Sherikov <alexander@sevendof.com> Co-authored-by: hs293go <hellston20a@gmail.com> Co-authored-by: Oleg Kalachev <okalachev@gmail.com> Co-authored-by: matt <anderson_rayner@hotmail.com> Co-authored-by: Ubuntu <ubuntu@ubuntu1804.lxd>
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
I noticed an issue where distance sensor data I send via from a ROS system via MAVROS to an autopilot gets invalid quaternion values. This turned out to be caused by the (Eigen) quaternion orientation not being initialized in the distance_sensor plugin, so if the default quaternion orientation is used, it's values will be random garbage data.
This is because the default constructor for an Eigen quaternion doesn't initialize it.
Here's an example of
listener distance_sensoroutput from PX4 before this fix:...and after:
Note that if you set orientation = ROTATION_CUSTOM, then the quaternion field is set to your configured custom orientation value, and this issue won't appear.