From b61ea55314cf63de777f055a72699504d867b5b4 Mon Sep 17 00:00:00 2001
From: guanxuany <guanxuany@student.unimelb.edu.au>
Date: Tue, 23 Jul 2024 15:53:44 +1000
Subject: [PATCH] odometry_new update
---
.../asclinic_pkg/src/ascr/odometry _new.cpp | 184 ++++++++++++++++--
1 file changed, 163 insertions(+), 21 deletions(-)
diff --git a/catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp b/catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp
index e7069045..d0ec5693 100644
--- a/catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp
+++ b/catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp
@@ -14,12 +14,32 @@ double time_float; // Floating-point representation of elapsed time
constexpr float WHEEL_RADIUS_LEFT = 0.072; // Left wheel radius in meters
constexpr float WHEEL_RADIUS_RIGHT = 0.072; // Right wheel radius in meters
constexpr float HALF_WHEEL_BASE = 0.125; // Half of the wheelbase in meters
+constexpr float k_l = 0.00023; // Uncertainty in the left wheel
+constexpr float k_r = 0.0001; // Uncertainty in the right wheel
+int authority =0;
+
+// Covariance matrix for pose
+MatrixXd pose_covariance(3, 3); // Initialize a 3x3 matrix for pose covariance
+MatrixXd z_cov(3, 3);
ros::Subscriber m_subscriber;
+ros::Subscriber image_subscriber;
+ros::Subscriber pwm_subscriber;
+ros::Subscriber kalman_subscriber;
+
ros::Publisher m_publisher;
ros::Publisher m_angular_velocity_publisher; // Publisher for angular velocity as a single float
ros::Publisher m_odometry_publisher; // Publisher for odometry
+float normalize_angle(float angle) {
+ if (angle >= 0) {
+ return fmod(angle, 2 * M_PI);
+ } else {
+ float value = fmod(angle, 2 * M_PI);
+ return 2 * M_PI - value;
+ }
+}
+
void answer(const asclinic_pkg::LeftRightInt32& msg) {
// Update times for elapsed time calculation
@@ -41,7 +61,28 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
x += dS * cos(phi + 0.5 * dPhi);
y += dS * sin(phi + 0.5 * dPhi);
phi += dPhi;
+ phi = normalize_angle(phi);
+
+ // Compute Covariance
+ MatrixXd jacob_p(3, 3);
+ jacob_p << 1, 0, -dS * sin(phi + 0.5 * dPhi),
+ 0, 1, dS * cos(phi + 0.5 * dPhi),
+ 0, 0, 1;
+ MatrixXd theta_covariance(2, 2);
+ theta_covariance << k_l * std::abs(deltaThetaL), 0,
+ 0, k_r * std::abs(deltaThetaR);
+
+ float r = WHEEL_RADIUS_RIGHT; // Moved this declaration here for consistency
+
+ MatrixXd jacob_phi(3, 2);
+ jacob_phi << r/2 * cos(phi + 0.5 * dPhi) + r/4 / HALF_WHEEL_BASE * dS * sin(phi + 0.5 * dPhi), r/2 * cos(phi + 0.5 * dPhi) - r/4 / HALF_WHEEL_BASE * dS * sin(phi + 0.5 * dPhi),
+ r/2 * sin(phi + 0.5 * dPhi) - r/4 / HALF_WHEEL_BASE * dS * cos(phi + 0.5 * dPhi), r/2 * sin(phi + 0.5 * dPhi) + r/4 / HALF_WHEEL_BASE * dS * cos(phi + 0.5 * dPhi),
+ -r / (2 * HALF_WHEEL_BASE), r / (2 * HALF_WHEEL_BASE);
+
+ MatrixXd pose_covariance_update = jacob_p * pose_covariance * jacob_p.transpose() + jacob_phi * theta_covariance * jacob_phi.transpose();
+ pose_covariance = pose_covariance_update;
+
// Calculate angular velocity
float angular_velocity = dPhi / time_float;
@@ -79,40 +120,141 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
// Covariance Matrix initialization
// Pose Covariance
- for (int i = 0; i < 36; ++i) {
- if (i == 0 || i == 7 || i == 14) { // Covariance for x, y, z positions
- odom.pose.covariance[i] = 0.01;
- } else if (i == 21 || i == 28 || i == 35) { // Covariance for roll, pitch, yaw
- odom.pose.covariance[i] = 0.1;
- } else {
- odom.pose.covariance[i] = 0.0;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ odom.pose.covariance[i * 6 + j] = pose_covariance(i, j);
}
}
- // Twist Covariance
+
+ // Twist covariance
for (int i = 0; i < 36; ++i) {
- if (i == 0 || i == 7 || i == 14) { // Covariance for linear velocities
- odom.twist.covariance[i] = 0.01;
- } else if (i == 21 || i == 28 || i == 35) { // Covariance for angular velocities
- odom.twist.covariance[i] = 0.1;
- } else {
- odom.twist.covariance[i] = 0.0;
- }
+ odom.twist.covariance[i] = 0.1;
}
m_odometry_publisher.publish(odom);
}
-int main(int argc, char **argv) {
- ros::init(argc, argv, "receive_encoder");
+void ImageAnswer(const asclinic_pkg::position& msg) {
+ constrain_time = ros::Time::now();
+ constrain_duration = constrain_time - begin_time;
+ float temp_value = constrain_duration.toSec();
+ if (temp_value < 50) {
+ return;
+ }
+ // Check if elapsed time is less than 0.5 seconds
+ if (authority ==0) {
+ return;
+ }
+
+ // Update the last time to the current time
+
+ MatrixXd z_position(3, 1);
+ z_position << msg.x,
+ msg.y,
+ msg.phi;
+
+ MatrixXd K = pose_covariance * (pose_covariance + z_cov).inverse();
+ MatrixXd odo_position(3, 1);
+ odo_position << x,
+ y,
+ phi;
+
+ //ROS_INFO_STREAM("odo_position: " << odo_position);
+ //ROS_INFO_STREAM("z_position: " << z_position);
+ //ROS_INFO_STREAM("K: " << K);
+
+ MatrixXd new_pose = odo_position + K * (z_position - odo_position);
+
+ //ROS_INFO_STREAM("new_pose: " << new_pose);
+ if (std::isnan(new_pose(0)) || std::isnan(new_pose(1)) || std::isnan(new_pose(2))||new_pose(0)<-2 ||new_pose(1)<-4||std::abs(odo_position(0)-new_pose(0))>1.5||std::abs(odo_position(1)-new_pose(1)>1.5)) {
+ x = odo_position(0);
+ y = odo_position(1);
+ phi = odo_position(2);
+ ROS_WARN("Fail: new_pose contains NaN values");
+ } else {
+ x = new_pose(0);
+ y = new_pose(1);
+ phi = new_pose(2);
+ pose_covariance = pose_covariance - K * (pose_covariance + z_cov) * K.transpose();
+ ROS_INFO_STREAM("Updated pose: X:" << x << ", Y:" << y << ", Phi:" << phi);
+ }
+
+ phi = normalize_angle(phi);
+ //ROS_INFO_STREAM("Normalized Phi: " << phi);
+
+ ROS_INFO("Image updated");
+ authority = 0;
+
+ std_msgs::UInt32 Umsg;
+ Umsg.data = 1;
+ kalman_output.publish(Umsg);
+}
+
+
+void pwmAnswer(const asclinic_pkg::LeftRightFloat32& msg){
+ if(msg.left<0){
+ left_p = -1;
+ }
+ else{
+ left_p = 1;
+ }
+ if(msg.right<0){
+ right_p = -1;
+ }
+ else{
+ right_p = 1;
+ }
+}
+
+void update_authority(const std_msgs::UInt32& msg) {
+ authority = 1;
+ ros::Time last_time = ros::Time::now();
+ ros::Time current_time = ros::Time::now();
+ ros::Duration lastest_duration;
+ float duration_authority;
+ ROS_INFO_STREAM("Authority updated to: " << authority);
+ while(authority){
+ current_time = ros::Time::now();
+ lastest_duration = current_time - last_time;
+ duration_authority = lastest_duration.toSec();
+ if(duration_authority > 2){
+ authority = 0;
+ std_msgs::UInt32 Umsg;
+ Umsg.data = 1;
+ kalman_output.publish(Umsg);
+ ROS_INFO_STREAM("go with no update");
+ }
+
+ ros::spinOnce();
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+}
+
+int main(int argc, char **argv) {
+ ros::init(argc, argv, "Kalman");
std::string ns_for_group = ros::this_node::getNamespace();
ros::NodeHandle nh(ns_for_group);
-
+ begin_time = ros::Time::now();
// Initialize time_now to current time
time_now = ros::Time::now();
- m_subscriber = nh.subscribe("asc/encoder_counts", 1, answer);
- m_publisher = nh.advertise<asclinic_pkg::position>("current_position", 1);
- m_angular_velocity_publisher = nh.advertise<asclinic_pkg::vw>("angular_velocity_topic", 1);
+ pose_covariance << 1, 1, 1,
+ 1, 1, 1,
+ 1, 1, 1;
+
+ z_cov << 10, 10, 10,
+ 10, 10, 10,
+ 10, 10, 10;
+
+ m_subscriber = nh.subscribe("/asc/encoder_counts", 1, answer);
+ image_subscriber = nh.subscribe("/image_estimation", 1, ImageAnswer);
+ pwm_subscriber = nh.subscribe("/asc/current_motor_duty_cycle", 1, pwmAnswer);
+ kalman_subscriber = nh.subscribe("/kalman_demand", 1, update_authority);
+
+ m_publisher = nh.advertise<asclinic_pkg::position>("/current_position", 10);
+ m_angular_velocity_publisher = nh.advertise<asclinic_pkg::vw>("/angular_velocity_topic", 10);
+ kalman_output = nh.advertise<std_msgs::UInt32>("/kalman_info", 1);
+ m_odometry_publisher = nh.advertise<nav_msgs/Odometry>("/odom", 10);
ROS_INFO_STREAM("Node started");
ros::spin();
--
GitLab