-
Notifications
You must be signed in to change notification settings - Fork 24
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* fix: ekf initialize * chore: add gyro odmetor * feat: add imu corrector * fix: covariance for pose * revert: pure pursuit and fix: imu corrector launch * fix: imu name space * chore: add pkill script for developer * fix: sensor kit param * fix: gyro odometor angular x, y to zero * fix: sensor kit calibration param * chore: set better pp param * revert: gyro stdddev * chore: change covariance to zero * chore: fix ll2
- Loading branch information
1 parent
5620528
commit 6a3996f
Showing
23 changed files
with
1,149 additions
and
47 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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,17 @@ | ||
| pkill -9 robot_state_pub | ||
| pkill -9 vehicle_velocit | ||
| pkill -9 imu_gnss_poser_ | ||
| pkill -9 twist2accel | ||
| pkill -9 empty_objects_p | ||
| pkill -9 goal_pose_visua | ||
| pkill -9 path_to_traject | ||
| pkill -9 goal_pose_sette | ||
| pkill -9 ros2 | ||
| pkill -9 python3 | ||
| pkill -9 rviz2 | ||
| pkill -9 ekf_localizer | ||
| pkill -9 mission_planner | ||
| pkill -9 component_conta | ||
| pkill -9 simple_pure_pur | ||
| pkill -9 initial_pose_ad | ||
| pkill -9 routing_adaptor |
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
35 changes: 35 additions & 0 deletions
35
aichallenge/workspace/src/aichallenge_submit/gyro_odometer/CMakeLists.txt
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,35 @@ | ||
| cmake_minimum_required(VERSION 3.14) | ||
| project(gyro_odometer) | ||
|
|
||
| find_package(autoware_cmake REQUIRED) | ||
| autoware_package() | ||
|
|
||
| ament_auto_add_executable(${PROJECT_NAME} | ||
| src/gyro_odometer_node.cpp | ||
| src/gyro_odometer_core.cpp | ||
| ) | ||
|
|
||
| target_link_libraries(${PROJECT_NAME} fmt) | ||
|
|
||
| ament_auto_add_library(gyro_odometer_node SHARED | ||
| src/gyro_odometer_core.cpp | ||
| ) | ||
|
|
||
| if(BUILD_TESTING) | ||
| ament_add_ros_isolated_gtest(test_gyro_odometer | ||
| test/test_main.cpp | ||
| test/test_gyro_odometer_pubsub.cpp | ||
| test/test_gyro_odometer_helper.cpp | ||
| ) | ||
| ament_target_dependencies(test_gyro_odometer | ||
| rclcpp | ||
| ) | ||
| target_link_libraries(test_gyro_odometer | ||
| gyro_odometer_node | ||
| ) | ||
| endif() | ||
|
|
||
|
|
||
| ament_auto_package(INSTALL_TO_SHARE | ||
| launch | ||
| ) |
39 changes: 39 additions & 0 deletions
39
aichallenge/workspace/src/aichallenge_submit/gyro_odometer/README.md
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,39 @@ | ||
| # gyro_odometer | ||
|
|
||
| ## Purpose | ||
|
|
||
| `gyro_odometer` is the package to estimate twist by combining imu and vehicle speed. | ||
|
|
||
| ## Inputs / Outputs | ||
|
|
||
| ### Input | ||
|
|
||
| | Name | Type | Description | | ||
| | ------------------------------- | ------------------------------------------------ | ---------------------------------- | | ||
| | `vehicle/twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | twist with covariance from vehicle | | ||
| | `imu` | `sensor_msgs::msg::Imu` | imu from sensor | | ||
|
|
||
| ### Output | ||
|
|
||
| | Name | Type | Description | | ||
| | ----------------------- | ------------------------------------------------ | ------------------------------- | | ||
| | `twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | estimated twist with covariance | | ||
|
|
||
| ## Parameters | ||
|
|
||
| | Parameter | Type | Description | | ||
| | --------------------- | ------ | -------------------------------- | | ||
| | `output_frame` | String | output's frame id | | ||
| | `message_timeout_sec` | Double | delay tolerance time for message | | ||
|
|
||
| ## Assumptions / Known limits | ||
|
|
||
| - [Assumption] The frame_id of input twist message must be set to base_link. | ||
|
|
||
| - [Assumption] The covariance in the input messages must be properly assigned. | ||
|
|
||
| - [Assumption] The angular velocity is set to zero if both the longitudinal vehicle velocity and the angular velocity around the yaw axis are sufficiently small. This is for suppression of the IMU angular velocity bias. Without this process, we misestimate the vehicle status when stationary. | ||
|
|
||
| - [Limitation] The frequency of the output messages depends on the frequency of the input IMU message. | ||
|
|
||
| - [Limitation] We cannot produce reliable values for the lateral and vertical velocities. Therefore we assign large values to the corresponding elements in the output covariance matrix. |
76 changes: 76 additions & 0 deletions
76
...rkspace/src/aichallenge_submit/gyro_odometer/include/gyro_odometer/gyro_odometer_core.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,76 @@ | ||
| // Copyright 2015-2019 Autoware Foundation | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ | ||
| #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ | ||
|
|
||
| #include "tier4_autoware_utils/ros/transform_listener.hpp" | ||
| #include "tier4_autoware_utils/tier4_autoware_utils.hpp" | ||
|
|
||
| #include <rclcpp/rclcpp.hpp> | ||
|
|
||
| #include <geometry_msgs/msg/twist_stamped.hpp> | ||
| #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp> | ||
| #include <sensor_msgs/msg/imu.hpp> | ||
|
|
||
| #include <tf2/transform_datatypes.h> | ||
| #ifdef ROS_DISTRO_GALACTIC | ||
| #include <tf2_geometry_msgs/tf2_geometry_msgs.h> | ||
| #else | ||
| #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> | ||
| #endif | ||
|
|
||
| #include <deque> | ||
| #include <memory> | ||
| #include <string> | ||
|
|
||
| class GyroOdometer : public rclcpp::Node | ||
| { | ||
| private: | ||
| using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; | ||
|
|
||
| public: | ||
| explicit GyroOdometer(const rclcpp::NodeOptions & options); | ||
| ~GyroOdometer(); | ||
|
|
||
| private: | ||
| void callbackVehicleTwist( | ||
| const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr); | ||
| void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); | ||
| void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); | ||
|
|
||
| rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr | ||
| vehicle_twist_sub_; | ||
| rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_; | ||
|
|
||
| rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_raw_pub_; | ||
| rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr | ||
| twist_with_covariance_raw_pub_; | ||
|
|
||
| rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_; | ||
| rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr | ||
| twist_with_covariance_pub_; | ||
|
|
||
| std::shared_ptr<tier4_autoware_utils::TransformListener> transform_listener_; | ||
|
|
||
| std::string output_frame_; | ||
| double message_timeout_sec_; | ||
|
|
||
| bool vehicle_twist_arrived_; | ||
| bool imu_arrived_; | ||
| std::deque<geometry_msgs::msg::TwistWithCovarianceStamped> vehicle_twist_queue_; | ||
| std::deque<sensor_msgs::msg::Imu> gyro_queue_; | ||
| }; | ||
|
|
||
| #endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ |
29 changes: 29 additions & 0 deletions
29
aichallenge/workspace/src/aichallenge_submit/gyro_odometer/launch/gyro_odometer.launch.xml
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,29 @@ | ||
| <launch> | ||
| <arg name="input_vehicle_twist_with_covariance_topic" default="/sensing/vehicle_velocity_converter/twist_with_covariance" description="input twist with covariance topic name from vehicle"/> | ||
|
|
||
| <arg name="input_imu_topic" default="/sensing/imu/imu_data" description="input imu topic name"/> | ||
|
|
||
| <arg name="output_twist_raw_topic" default="gyro_twist_raw" description="output raw twist topic name"/> | ||
| <arg name="output_twist_with_covariance_raw_topic" default="gyro_twist_with_covariance_raw" description="output raw twist with covariance topic name"/> | ||
|
|
||
| <arg name="output_twist_topic" default="gyro_twist" description="output twist topic name"/> | ||
| <arg name="output_twist_with_covariance_topic" default="gyro_twist_with_covariance" description="output twist with covariance topic name"/> | ||
|
|
||
| <arg name="output_frame" default="base_link" description="output frame id"/> | ||
| <arg name="message_timeout_sec" default="0.2" description="delay tolerance time for message"/> | ||
|
|
||
| <node pkg="gyro_odometer" exec="gyro_odometer" name="gyro_odometer" output="screen"> | ||
| <remap from="vehicle/twist_with_covariance" to="$(var input_vehicle_twist_with_covariance_topic)"/> | ||
|
|
||
| <remap from="imu" to="$(var input_imu_topic)"/> | ||
|
|
||
| <remap from="twist_raw" to="$(var output_twist_raw_topic)"/> | ||
| <remap from="twist_with_covariance_raw" to="$(var output_twist_with_covariance_raw_topic)"/> | ||
|
|
||
| <remap from="twist" to="$(var output_twist_topic)"/> | ||
| <remap from="twist_with_covariance" to="$(var output_twist_with_covariance_topic)"/> | ||
|
|
||
| <param name="output_frame" value="$(var output_frame)"/> | ||
| <param name="message_timeout_sec" value="$(var message_timeout_sec)"/> | ||
| </node> | ||
| </launch> |
34 changes: 34 additions & 0 deletions
34
aichallenge/workspace/src/aichallenge_submit/gyro_odometer/package.xml
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,34 @@ | ||
| <?xml version="1.0"?> | ||
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>gyro_odometer</name> | ||
| <version>0.1.0</version> | ||
| <description>The gyro_odometer package as a ROS2 node</description> | ||
| <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer> | ||
| <license>Apache License 2.0</license> | ||
| <author email="yamato.ando@tier4.jp">Yamato Ando</author> | ||
|
|
||
| <buildtool_depend>ament_cmake_auto</buildtool_depend> | ||
|
|
||
| <build_depend>autoware_cmake</build_depend> | ||
|
|
||
| <depend>fmt</depend> | ||
| <depend>geometry_msgs</depend> | ||
| <depend>sensor_msgs</depend> | ||
| <depend>std_msgs</depend> | ||
| <depend>tf2</depend> | ||
| <depend>tf2_geometry_msgs</depend> | ||
| <depend>tf2_ros</depend> | ||
| <depend>tier4_autoware_utils</depend> | ||
|
|
||
| <exec_depend>rclcpp</exec_depend> | ||
| <exec_depend>rclcpp_components</exec_depend> | ||
|
|
||
| <test_depend>ament_cmake_ros</test_depend> | ||
| <test_depend>ament_lint_auto</test_depend> | ||
| <test_depend>autoware_lint_common</test_depend> | ||
|
|
||
| <export> | ||
| <build_type>ament_cmake</build_type> | ||
| </export> | ||
| </package> |
Oops, something went wrong.