-
Notifications
You must be signed in to change notification settings - Fork 152
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Feature/ci rosbag evaluation (#233) * support for nmea and canless mode (#190) * Add eagleye_pp and trajectory_plot to ci * No smoothingDeadReckoning if there is no rtk fix solution * Add CI for regression test * Add log csv * Add log csv * Add -y flag to apt install commands * Add pip in CI * Add pip install in CI * Add source in CI * Change eagleye log csv * Remove melodic CI --------- Co-authored-by: YutaHoda <[email protected]> Co-authored-by: YutaHoda <[email protected]> * Correction of errors in warning statements * Refactor angular_velocity_offset_stop (#238) * Added comment and refactor * Make it class * Fixed compilation error * Changed stop judgement condition * Update sample rosbag for CI --------- Co-authored-by: autoware <[email protected]> * Feature/covariance position(ROS1) (#240) * covariance output * fix yaml file * Adding parameters to various locations --------- Co-authored-by: map4 <[email protected]> * Change how to install submodules(ROS1) (#242) * Change how to install submodules * Delete submodule checkout in CI * Indatall GeographicLib in CI * Add checkout in CI * Refactor gnss_converter_node by converting it into a class (#245) * Delete unused variables * Removing unnecessary trailing * Fix judgment to judgement * Refactor gnss_converter_node by converting it into a class * Delete submodules * Correction of improper indentation * Refactor eagleye_util excluding fix2pose (#246) * Delete unused variables * Removing unnecessary trailing * Fix judgment to judgement * Refactor gnss_converter_node by converting it into a class * Delete submodules * Correction of improper indentation * Refactor can_velocity_converter * Refactor fix2kml * Refctor eagleye_pp (#247) * Delete unused variables * Removing unnecessary trailing * Fix judgment to judgement * Refactor gnss_converter_node by converting it into a class * Delete submodules * Correction of improper indentation * Refactor can_velocity_converter * Refactor fix2kml * Refactor eagleye_pp.cpp * Fix wrong argument (#248) * Delete unused variables * Removing unnecessary trailing * Fix judgment to judgement * Refactor gnss_converter_node by converting it into a class * Delete submodules * Correction of improper indentation * Refactor can_velocity_converter * Refactor fix2kml * Refactor eagleye_pp.cpp * Fix wrong argument * Add meridional-aberration-angle_correction (#251) * Feature/ros1/reverse imu param (#252) * import Axes3D * Add reverse imu param * Fix path error (#253) * Add input-doppler-filtering-reliability (#255) * Files rename (#257) * Allow warnings to be output when fixes are inaccurate (#260) * Fi x calib launch error (#264) * Feature/ros1/dual antenna mode sync(ROS1) (#266) * Dual antenna refactoring(WIP) * Fix pram bug * Make use_multi_antenna_mode false * Add reverse imu param in post process (#267) * Remove eagleye_pp (#268) * Remove eagleye_pp * Fix CONTRIBUTING.md --------- Co-authored-by: YutaHoda <[email protected]> Co-authored-by: YutaHoda <[email protected]> Co-authored-by: Tomoya Sato <[email protected]> Co-authored-by: Aoki-Takanose <[email protected]>
- Loading branch information
5 people
authored
May 23, 2023
1 parent
761eed4
commit 9772b33
Showing
91 changed files
with
2,232 additions
and
5,606 deletions.
There are no files selected for viewing
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
This file was deleted.
Oops, something went wrong.
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
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
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
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
97 changes: 97 additions & 0 deletions
97
eagleye_core/navigation/include/navigation/angular_velocity_offset_stop.hpp
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,97 @@ | ||
| // Copyright (c) 2019, Map IV, Inc. | ||
| // All rights reserved. | ||
| // | ||
| // Redistribution and use in source and binary forms, with or without | ||
| // modification, are permitted provided that the following conditions are met: | ||
| // * Redistributions of source code must retain the above copyright notice, | ||
| // this list of conditions and the following disclaimer. | ||
| // * Redistributions in binary form must reproduce the above copyright notice, | ||
| // this list of conditions and the following disclaimer in the documentation | ||
| // and/or other materials provided with the distribution. | ||
| // * Neither the name of the Map IV, Inc. nor the names of its contributors | ||
| // may be used to endorse or promote products derived from this software | ||
| // without specific prior written permission. | ||
| // | ||
| // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND | ||
| // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
| // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
| // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY | ||
| // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
| // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
| // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
| // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
| // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
|
|
||
| #ifndef ANGULAR_VELOCITY_OFFSET_STOP_HPP | ||
| #define ANGULAR_VELOCITY_OFFSET_STOP_HPP | ||
|
|
||
| #include <deque> | ||
|
|
||
| #include <Eigen/Core> | ||
| #include <Eigen/Dense> | ||
|
|
||
| #include <yaml-cpp/yaml.h> | ||
|
|
||
| #include <eagleye_msgs/AngularVelocityOffset.h> | ||
|
|
||
| struct AngularVelocityOffsetStopParameter | ||
| { | ||
| size_t buffer_size; | ||
| double velocity_stop_judgement_threshold; | ||
| double angular_stop_judgement_threshold; | ||
|
|
||
| void load(const std::string& yaml_file) | ||
| { | ||
| try | ||
| { | ||
| YAML::Node conf = YAML::LoadFile(yaml_file); | ||
|
|
||
| double imu_rate = conf["common"]["imu_rate"].as<double>(); | ||
| double estimated_interval = conf["angular_velocity_offset_stop"]["estimated_interval"].as<double>(); | ||
| this->buffer_size = imu_rate * estimated_interval; | ||
| this->velocity_stop_judgement_threshold = conf["common"]["stop_judgement_threshold"].as<double>(); | ||
| this->angular_stop_judgement_threshold = conf["angular_velocity_offset_stop"]["outlier_threshold"].as<double>(); | ||
| } | ||
| catch (YAML::Exception& e) | ||
| { | ||
| std::cerr << "\033[1;31mAngularVelocityOffsetStopParameter YAML Error: " << e.msg << "\033[m" << std::endl; | ||
| exit(3); | ||
| } | ||
| } | ||
| }; | ||
|
|
||
| struct AngularVelocityOffsetStopStatus | ||
| { | ||
| bool is_estimated_now; | ||
| bool is_estimation_started; | ||
| Eigen::Vector3d offset_stop; | ||
| }; | ||
|
|
||
| class AngularVelocityOffsetStopEstimator | ||
| { | ||
| public: | ||
| AngularVelocityOffsetStopEstimator(); | ||
| void setParameter(const AngularVelocityOffsetStopParameter& param); | ||
| void velocityCallback(const Eigen::Vector3d& velocity); | ||
| AngularVelocityOffsetStopStatus imuCallback(const Eigen::Vector3d& angular_velocity); | ||
|
|
||
| private: | ||
| // Param | ||
| AngularVelocityOffsetStopParameter param_; | ||
| // Flags | ||
| bool is_estimation_started_; | ||
| bool is_velocity_ready_; | ||
|
|
||
| // Angular velocity | ||
| Eigen::Vector3d estimated_offset_stop_; | ||
| std::deque<Eigen::Vector3d> angular_velocity_buffer_; | ||
| size_t buffer_size_; | ||
| double angular_stop_judgement_threshold_; | ||
|
|
||
| // Velocity | ||
| Eigen::Vector3d reserved_velocity_; | ||
| double velocity_stop_judgement_threshold_; | ||
| }; | ||
|
|
||
| #endif /* ANGULAR_VELOCITY_OFFSET_STOP_HPP */ |
Oops, something went wrong.