Travel_distance receives 0.0 despite moving from one point to another point
Hi,
I am trying to program my robot to calculate the values of x-axis and y-axis for the total distance and the travel distance based on one point to another. However, the travel distance still displays 0.0 no matter which path I am taking. I have been searching online for hours, however I saw that most of the solutions are in python software. I am currently using C++ program and I am currently using Linux. Attached below are my codes shown
My codes
// Subscribed topic : aiv_cmd_vel
// Published topic : odom
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/String.h>
#include <tf/transform_broadcaster.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Float32.h>
#include <nav_msgs/Odometry.h>
#include <sstream>
#include <fstream>
#include <vector>
#include <iostream>
#include <cstdio>
#include <time.h>
#include <string>
#include <stdio.h> /* Standard input/output definitions */
#include <string.h> /* String function definitions */
#include <unistd.h> /* UNIX standard function definitions */
#include <fcntl.h> /* File control definitions */
#include <errno.h> /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */
#include <stdlib.h>
#include <cmath>
using namespace std;
#define CRC16 0x8005
#define REPLY_SIZE 1
#define TIMEOUT 1000
#define PI 3.14159 //Ratio of the circumference of a circle to its diameter
#define RTD 180.0/PI //Radian to Degree
#define DTR PI/180.0 //Degree to Radian
#define btoa(x) ((x)?"true":"false")
//ROS Parameter Default
#define MAIN_FREQ 40.0 //Controller main frequency
#define TF_REV_DELAY_T 3.0 //TF of MAP_ODOM frame receive start delay units * ts
#define MODBUS_CMD_SEND_DELAY_T 0.05 //unit:s, 0: not appcation, >0: appcation
#define AGV_ROBOT_R 0.09 //unit:m (wheel radius)
#define AGV_ROBOT_L 0.517 //unit:m
#define AGV_ROBOT_H 0.12 //unit:m
#define AGV_WHEEL_MOTOR_GEAR_RATIO 0.05 //(1:20)
#define AGV_WHEEL_RPM_MAX 3000 //RPM
#define AGV_WHEEL_SPEED_MAX AGV_ROBOT_R*AGV_WHEEL_RPM_MAX*0.10472 //(m/s) The RPM to Linear Velocity formular is : v = r × RPM × 0.10472
#define AGV_WHEEL_SPEED_MIN AGV_ROBOT_R*1.0*0.10472
#define AGV_WHEEL_LV_TO_RPM 1/(AGV_ROBOT_R*0.10472)
#define MODBUS_CMD_RESEND_MAX 0 //0: not appcation, >0: appcation
#define MODBUS_CMD_RESEND_TIME 0.5 //unit:s
#define MODBUS_RECVEIE_TIMEOUT 1.0 //unit:s
#define LEFT_MOTOR_ID 3
#define LEFT_MOTOR_INVERSE 0
#define LEFT_MOTOR_ENABLE 1
#define RIGHT_MOTOR_ID 2
#define RIGHT_MOTOR_INVERSE 0
#define RIGHT_MOTOR_ENABLE 1
#define FRAME_BUFF_MAX 64
typedef struct
{
double L;
double r;
double h;
double wmr;
} rbtmdl;
typedef struct
{
double ts;
double w_L;
double w_R;
double v_L;
double v_R;
double v_t;
double w_t;
double v_cmd;
double w_cmd;
double w_La;
double w_Ra;
double v_La; //to Balance Left wheel command
double v_Ra; //to Balance Right wheel command
double ptv;
} rbtctrl;
rbtctrl rc;
rbtmdl rm;
//------------------------------------------
unsigned char data[6];
//------------------------------------------
ros::Publisher chatter_pub_;
ros::Publisher agv_vlr_pub_;
ros::Publisher odom_pub_;
//------------------------------------------
//ROS Parameter
double main_freq_=MAIN_FREQ ;
double tf_rev_delay_t_=TF_REV_DELAY_T;
double modbus_cmd_send_delay_t_=MODBUS_CMD_SEND_DELAY_T;
double agv_wheel_motor_gear_ratio_=AGV_WHEEL_MOTOR_GEAR_RATIO;
double agv_robot_r_=AGV_ROBOT_R;
double agv_robot_l_=AGV_ROBOT_L;
double agv_robot_h_=AGV_ROBOT_H;
std::string base_frame_;
std::string odom_frame_;
std::string modbus_port_;
std::string modbus_baud_;
double modbus_cmd_resend_max_=MODBUS_CMD_RESEND_MAX;
double modbus_cmd_resend_time_=MODBUS_CMD_RESEND_TIME;
double modbus_recveie_timeout_=MODBUS_RECVEIE_TIMEOUT;
int left_motor_id_=LEFT_MOTOR_ID;
bool left_motor_inverse_=LEFT_MOTOR_INVERSE;
bool left_motor_enable_=LEFT_MOTOR_ENABLE;
int right_motor_id_=RIGHT_MOTOR_ID;
bool right_motor_inverse_=RIGHT_MOTOR_INVERSE;
bool right_motor_enable_=RIGHT_MOTOR_ENABLE;
bool tf_output_enable_= false;
bool system_interlock = true ...