odometry_new

catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp

+#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;
+}
+