Travel_distance receives 0.0 despite moving from one point to another point

asked 2021-03-26 03:27:35 -0500

Jefferson gravatar image

updated 2021-04-22 11:13:37 -0500

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 ...
(more)
edit retag flag offensive close merge delete