From 3dc767d79e71aed765878e67105994520b74c602 Mon Sep 17 00:00:00 2001
From: guanxuany <guanxuany@student.unimelb.edu.au>
Date: Tue, 23 Jul 2024 15:06:52 +1000
Subject: [PATCH] odometry_new
---
.../asclinic_pkg/src/ascr/odometry _new.cpp | 122 ++++++++++++++++++
1 file changed, 122 insertions(+)
create mode 100644 catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp
diff --git a/catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp b/catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp
new file mode 100644
index 00000000..e7069045
--- /dev/null
+++ b/catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp
@@ -0,0 +1,122 @@
+#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
+ros::Publisher m_odometry_publisher; // Publisher for odometry
+
+
+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);
+
+ // Publish odometry
+ nav_msgs::Odometry odom;
+ odom.header.stamp = time_now;
+ odom.header.frame_id = "odom";
+ odom.child_frame_id = "base_link";
+
+ // Set the position
+ odom.pose.pose.position.x = x;
+ odom.pose.pose.position.y = y;
+ odom.pose.pose.position.z = 0.0;
+ 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.angular.z = angular_velocity;
+
+ // 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;
+ }
+ }
+ // 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;
+ }
+ }
+ m_odometry_publisher.publish(odom);
+}
+
+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;
+}
+
--
GitLab