From 59e07d7e75d14e455d3ee50d50a1e24629270625 Mon Sep 17 00:00:00 2001
From: Ethan <jiaao@student.unimelb.edu.au>
Date: Wed, 24 Jul 2024 12:27:08 +0800
Subject: [PATCH] modified odometry_new
---
catkin_ws/src/asclinic_pkg/CMakeLists.txt | 5 +-
.../asclinic_pkg/src/forklift/odometry.cpp | 80 -----------
.../odometry_new.cpp} | 128 +++---------------
3 files changed, 25 insertions(+), 188 deletions(-)
delete mode 100644 catkin_ws/src/asclinic_pkg/src/forklift/odometry.cpp
rename catkin_ws/src/asclinic_pkg/src/{ascr/odometry _new.cpp => forklift/odometry_new.cpp} (59%)
diff --git a/catkin_ws/src/asclinic_pkg/CMakeLists.txt b/catkin_ws/src/asclinic_pkg/CMakeLists.txt
index fdac4ef5..e682b6fb 100644
--- a/catkin_ws/src/asclinic_pkg/CMakeLists.txt
+++ b/catkin_ws/src/asclinic_pkg/CMakeLists.txt
@@ -13,12 +13,12 @@ find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
geometry_msgs
+ tf
sensor_msgs
cv_bridge
genmsg
roslib
nav_msgs
- geometry_msgs
)
## System dependencies are found with CMake's conventions
@@ -220,6 +220,7 @@ add_executable(Kalman src/ascr/Kalman.cpp)
add_executable(fcontroller1 src/ascr/fcontroller1.cpp)
add_executable(final_controller src/ascr/final_controller.cpp)
add_executable(control_node src/forklift/control_node.cpp)
+add_executable(odometry_new src/forklift/odometry_new.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
@@ -255,6 +256,7 @@ add_dependencies(Kalman asclinic_pkg_generate_mes
add_dependencies(fcontroller1 asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(final_controller asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(control_node asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(odometry_new asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -287,6 +289,7 @@ target_link_libraries(Kalman ${catkin_LIBRARIES}
target_link_libraries(fcontroller1 ${catkin_LIBRARIES})
target_link_libraries(final_controller ${catkin_LIBRARIES})
target_link_libraries(control_node ${catkin_LIBRARIES})
+target_link_libraries(odometry_new ${catkin_LIBRARIES})
diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/odometry.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/odometry.cpp
deleted file mode 100644
index 1d44517c..00000000
--- a/catkin_ws/src/asclinic_pkg/src/forklift/odometry.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-#include "ros/ros.h"
-#include <ros/package.h>
-#include <cmath>
-#include "asclinic_pkg/LeftRightInt32.h"
-#include "asclinic_pkg/position.h"
-#include "asclinic_pkg/vw.h"
-// Robot's position and orientation variables
-float x = 0.0, y = 0.0, phi = 0.0;
-ros::Time time_previous, time_now; // Timing variables
-ros::Duration elapsed_time; // Duration to calculate elapsed time
-double time_float; // Floating-point representation of elapsed time
-
-// Physical and error parameters
-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
-
-ros::Subscriber m_subscriber;
-ros::Publisher m_publisher;
-ros::Publisher m_angular_velocity_publisher; // Publisher for angular velocity as a single float
-
-void answer(const asclinic_pkg::LeftRightInt32& msg) {
- // Update times for elapsed time calculation
- time_previous = time_now;
- time_now = ros::Time::now();
- elapsed_time = time_now - time_previous;
- time_float = elapsed_time.toSec();
- ROS_INFO_STREAM("the seperation in second is "<<time_float);
-
- // Calculate angular displacements for left and right wheels
- float deltaThetaL = 0.000404 * msg.left / WHEEL_RADIUS_LEFT; // Calculate rotation angle for left wheel
- float deltaThetaR = 0.000404 * msg.right / WHEEL_RADIUS_RIGHT; // Calculate rotation angle for right wheel
-
- // Calculate the linear displacement and change in orientation
- float dS = (WHEEL_RADIUS_LEFT * deltaThetaL + WHEEL_RADIUS_RIGHT * deltaThetaR) / 2;
- float dPhi = (WHEEL_RADIUS_RIGHT * deltaThetaR - WHEEL_RADIUS_LEFT * deltaThetaL) / (2 * HALF_WHEEL_BASE);
-
- // Update position and orientation based on movements
- x += dS * cos(phi + 0.5 * dPhi);
- y += dS * sin(phi + 0.5 * dPhi);
- phi += dPhi;
-
- // Calculate angular velocity
- float angular_velocity = dPhi / time_float;
-
- // Log current position and angular velocity
- ROS_INFO_STREAM("Current Position - X:" << x << ", Y:" << y << ", Phi:" << phi << ", Angular Velocity:" << angular_velocity);
-
- // Publish position
- asclinic_pkg::position position_msg;
- position_msg.x = x;
- position_msg.y = y;
- position_msg.phi = phi;
- m_publisher.publish(position_msg);
-
- // Publish angular velocity
- asclinic_pkg::vw angular_velocity_msg;
- angular_velocity_msg.w = angular_velocity;
- m_angular_velocity_publisher.publish(angular_velocity_msg);
-}
-
-int main(int argc, char **argv) {
- ros::init(argc, argv, "receive_encoder");
-
- std::string ns_for_group = ros::this_node::getNamespace();
- ros::NodeHandle nh(ns_for_group);
-
- // 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);
-
- ROS_INFO_STREAM("Node started");
- ros::spin();
-
- return 0;
-}
-
diff --git a/catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp
similarity index 59%
rename from catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp
rename to catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp
index d0ec5693..22f38dec 100644
--- a/catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp
+++ b/catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp
@@ -2,14 +2,23 @@
#include <ros/package.h>
#include <cmath>
#include "asclinic_pkg/LeftRightInt32.h"
+#include "asclinic_pkg/LeftRightFloat32.h"
#include "asclinic_pkg/position.h"
#include "asclinic_pkg/vw.h"
-// Robot's position and orientation variables
-float x = 0.0, y = 0.0, phi = 0.0;
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Dense>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Twist.h>
+#include <tf/transform_datatypes.h> // For tf::createQuaternionMsgFromYaw
+
ros::Time time_previous, time_now; // Timing variables
ros::Duration elapsed_time; // Duration to calculate elapsed time
+
+// Robot's position and orientation variables
+float x = 0.0, y = 0.0, phi = 0.0;
double time_float; // Floating-point representation of elapsed time
+
// Physical and error parameters
constexpr float WHEEL_RADIUS_LEFT = 0.072; // Left wheel radius in meters
constexpr float WHEEL_RADIUS_RIGHT = 0.072; // Right wheel radius in meters
@@ -17,18 +26,19 @@ 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;
+int left_p = 1, right_p = 1;
+
+using Eigen::MatrixXd;
+
// 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) {
@@ -89,17 +99,7 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
// Log current position and angular velocity
ROS_INFO_STREAM("Current Position - X:" << x << ", Y:" << y << ", Phi:" << phi << ", Angular Velocity:" << angular_velocity);
- // Publish position
- asclinic_pkg::position position_msg;
- position_msg.x = x;
- position_msg.y = y;
- position_msg.phi = phi;
- m_publisher.publish(position_msg);
-
- // Publish angular velocity
- asclinic_pkg::vw angular_velocity_msg;
- angular_velocity_msg.w = angular_velocity;
- m_angular_velocity_publisher.publish(angular_velocity_msg);
+
// Publish odometry
nav_msgs::Odometry odom;
@@ -114,8 +114,8 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(phi);
// Set the velocity
- odom.twist.twist.linear.x = dS / time_float;
- odom.twist.twist.linear.y = 0.0;
+ odom.twist.twist.linear.x = dS * cos(phi + 0.5 * dPhi) / time_float;
+ odom.twist.twist.linear.y = dS * sin(phi + 0.5 * dPhi) / time_float;
odom.twist.twist.angular.z = angular_velocity;
// Covariance Matrix initialization
@@ -133,62 +133,6 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
m_odometry_publisher.publish(odom);
}
-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){
@@ -206,35 +150,11 @@ void pwmAnswer(const asclinic_pkg::LeftRightFloat32& msg){
}
}
-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");
+ ros::init(argc, argv, "odometry_new");
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();
@@ -242,19 +162,13 @@ int main(int argc, char **argv) {
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);
+ m_odometry_publisher = nh.advertise<nav_msgs::Odometry>("/odom", 10);
ROS_INFO_STREAM("Node started");
ros::spin();
--
GitLab