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

feat(gyro_odometer, imu_corrector): add roll and pitch rate #407

Merged
merged 12 commits into from
Mar 8, 2022
Merged

feat(gyro_odometer, imu_corrector): add roll and pitch rate #407

Show file tree
Hide file tree
Changes from 9 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
5288b6a
feat: add roll pitch rate
kminoda Feb 17, 2022
706e6e7
ci(pre-commit): autofix
pre-commit-ci[bot] Feb 17, 2022
c7098da
Update localization/gyro_odometer/src/gyro_odometer_core.cpp
kminoda Mar 3, 2022
d060a15
Update localization/gyro_odometer/src/gyro_odometer_core.cpp
kminoda Mar 3, 2022
a873606
ci(pre-commit): autofix
pre-commit-ci[bot] Mar 3, 2022
2319815
Update localization/gyro_odometer/src/gyro_odometer_core.cpp
kminoda Mar 3, 2022
955afaf
fix: add new parameter description in imu_corrector/readme.md
kminoda Mar 3, 2022
b74ff30
Merge branch 'tier4/proposal' into feature/add_roll_and_pitch_rate
kminoda Mar 3, 2022
8cca07c
ci(pre-commit): autofix
pre-commit-ci[bot] Mar 3, 2022
fae4c29
Update localization/gyro_odometer/src/gyro_odometer_core.cpp
kminoda Mar 3, 2022
c96dea9
Update localization/gyro_odometer/src/gyro_odometer_core.cpp
kminoda Mar 3, 2022
537b605
Update localization/gyro_odometer/src/gyro_odometer_core.cpp
kminoda Mar 3, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 12 additions & 7 deletions localization/gyro_odometer/src/gyro_odometer_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,8 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
if (
std::fabs(transformed_angular_velocity.vector.z) < 0.01 &&
std::fabs(twist_with_cov_msg_ptr_->twist.twist.linear.x) < 0.01) {
transformed_angular_velocity.vector.x = 0.0;
transformed_angular_velocity.vector.y = 0.0;
transformed_angular_velocity.vector.z = 0.0;
}

Expand All @@ -105,25 +107,28 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
twist.header.stamp = imu_msg_ptr_->header.stamp;
twist.header.frame_id = output_frame_;
twist.twist.linear = twist_with_cov_msg_ptr_->twist.twist.linear;
twist.twist.angular.z = transformed_angular_velocity.vector.z; // TODO(YamatoAndo) yaw_rate only
twist.twist.angular.x = transformed_angular_velocity.vector.x;
twist.twist.angular.y = transformed_angular_velocity.vector.y;
twist.twist.angular.z = transformed_angular_velocity.vector.z;
twist_pub_->publish(twist);

geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance;
twist_with_covariance.header.stamp = imu_msg_ptr_->header.stamp;
twist_with_covariance.header.frame_id = output_frame_;
twist_with_covariance.twist.twist.linear = twist_with_cov_msg_ptr_->twist.twist.linear;
twist_with_covariance.twist.twist.angular.z =
transformed_angular_velocity.vector.z; // TODO(YamatoAndo) yaw_rate only
twist_with_covariance.twist.twist.angular.x = transformed_angular_velocity.vector.x;
twist_with_covariance.twist.twist.angular.y = transformed_angular_velocity.vector.y;
twist_with_covariance.twist.twist.angular.z = transformed_angular_velocity.vector.z;

// NOTE
// linear.y, linear.z, angular.x, and angular.y are not measured values.
// linear.y and linear.z are not measured values.
// Therefore, they should be assigned large variance values.
twist_with_covariance.twist.covariance[0] = twist_with_cov_msg_ptr_->twist.covariance[0];
twist_with_covariance.twist.covariance[7] = 10000.0;
twist_with_covariance.twist.covariance[14] = 10000.0;
twist_with_covariance.twist.covariance[21] = 10000.0;
twist_with_covariance.twist.covariance[28] = 10000.0;
twist_with_covariance.twist.covariance[35] = imu_msg_ptr_->angular_velocity_covariance[8];
twist_with_covariance.twist.covariance[21] = imu_msg_ptr->angular_velocity_covariance[0];
kminoda marked this conversation as resolved.
Show resolved Hide resolved
twist_with_covariance.twist.covariance[28] = imu_msg_ptr->angular_velocity_covariance[4];
kminoda marked this conversation as resolved.
Show resolved Hide resolved
twist_with_covariance.twist.covariance[35] = imu_msg_ptr->angular_velocity_covariance[8];
kminoda marked this conversation as resolved.
Show resolved Hide resolved

twist_with_covariance_pub_->publish(twist_with_covariance);
}
Expand Down
12 changes: 8 additions & 4 deletions sensing/imu_corrector/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,10 +28,14 @@

### Core Parameters

| Name | Type | Description |
| ---------------------------- | ------ | ----------------------------------- |
| `angular_velocity_offset_z` | double | yaw rate offset [rad/s] |
| `angular_velocity_stddev_zz` | double | yaw rate standard deviation [rad/s] |
| Name | Type | Description |
| ---------------------------- | ------ | ------------------------------------- |
| `angular_velocity_offset_x` | double | roll rate offset [rad/s] |
| `angular_velocity_offset_y` | double | pitch rate offset [rad/s] |
| `angular_velocity_offset_z` | double | yaw rate offset [rad/s] |
| `angular_velocity_stddev_xx` | double | roll rate standard deviation [rad/s] |
| `angular_velocity_stddev_yy` | double | pitch rate standard deviation [rad/s] |
| `angular_velocity_stddev_zz` | double | yaw rate standard deviation [rad/s] |

## Assumptions / Known limits

Expand Down
4 changes: 4 additions & 0 deletions sensing/imu_corrector/config/imu_corrector.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,8 @@
/**:
ros__parameters:
angular_velocity_offset_x: 0.0 # [rad/s]
angular_velocity_offset_y: 0.0 # [rad/s]
angular_velocity_offset_z: 0.0 # [rad/s]
angular_velocity_stddev_xx: 0.03 # [rad/s]
angular_velocity_stddev_yy: 0.03 # [rad/s]
angular_velocity_stddev_zz: 0.03 # [rad/s]
4 changes: 4 additions & 0 deletions sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,12 @@ class ImuCorrector : public rclcpp::Node

rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;

double angular_velocity_offset_x_;
double angular_velocity_offset_y_;
double angular_velocity_offset_z_;

double angular_velocity_stddev_xx_;
double angular_velocity_stddev_yy_;
double angular_velocity_stddev_zz_;
};
} // namespace imu_corrector
Expand Down
12 changes: 11 additions & 1 deletion sensing/imu_corrector/src/imu_corrector_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,12 @@ namespace imu_corrector
ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & node_options)
: Node("imu_corrector", node_options)
{
angular_velocity_offset_x_ = declare_parameter<double>("angular_velocity_offset_x", 0.0);
angular_velocity_offset_y_ = declare_parameter<double>("angular_velocity_offset_y", 0.0);
angular_velocity_offset_z_ = declare_parameter<double>("angular_velocity_offset_z", 0.0);

angular_velocity_stddev_xx_ = declare_parameter<double>("angular_velocity_stddev_xx", 0.03);
angular_velocity_stddev_yy_ = declare_parameter<double>("angular_velocity_stddev_yy", 0.03);
angular_velocity_stddev_zz_ = declare_parameter<double>("angular_velocity_stddev_zz", 0.03);

imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
Expand All @@ -34,9 +38,15 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
sensor_msgs::msg::Imu imu_msg;
imu_msg = *imu_msg_ptr;

imu_msg.angular_velocity.x += angular_velocity_offset_x_;
imu_msg.angular_velocity.y += angular_velocity_offset_y_;
imu_msg.angular_velocity.z += angular_velocity_offset_z_;

imu_msg.angular_velocity_covariance[8] =
imu_msg.angular_velocity_covariance[0 * 3 + 0] =
angular_velocity_stddev_xx_ * angular_velocity_stddev_xx_;
imu_msg.angular_velocity_covariance[1 * 3 + 1] =
angular_velocity_stddev_yy_ * angular_velocity_stddev_yy_;
imu_msg.angular_velocity_covariance[2 * 3 + 2] =
angular_velocity_stddev_zz_ * angular_velocity_stddev_zz_;

imu_pub_->publish(imu_msg);
Expand Down