ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Odometry does not transform in rviz

asked 2022-04-23 08:46:58 -0500

ahmet gravatar image

updated 2022-04-25 03:39:34 -0500

When I move my vehicle, odomety does not move in rviz, what is the reason , please help me..

#include "Motor.h"
#include <ros.h>
#include <ros/time.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Int16.h>
#include<PID_v1.h>




ros::NodeHandle  nh;

#define LOOPTIME 10
int updatenh;

Motor right(11,10,20,21);
Motor left(9,8,18,19);

volatile long encoder0Pos = 0;    // encoder 1
volatile long encoder1Pos = 0;    // encoder 2

double left_kp = 17 , left_ki = 0 , left_kd = 0.0;             // modify for optimal performance
double right_kp = 20 , right_ki = 0 , right_kd = 0.0;

float demandx=0;
float demandz=0;

double demand_speed_left;
double demand_speed_right;

double right_input = 0, right_output = 0, right_setpoint = 0;
PID rightPID(&right_input, &right_output, &right_setpoint, right_kp, right_ki, right_kd, DIRECT);  

double left_input = 0, left_output = 0, left_setpoint = 0;
PID leftPID(&left_input, &left_output, &left_setpoint, left_kp, left_ki, left_kd, DIRECT);  

unsigned long currentMillis;
unsigned long prevMillis;

float encoder0Diff;
float encoder1Diff;

float encoder0Error;
float encoder1Error;

float encoder0Prev;
float encoder1Prev;

void cmd_vel_cb( const geometry_msgs::Twist& twist){
  demandx = twist.linear.x;
  demandz = twist.angular.z;
}

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", cmd_vel_cb );
std_msgs::Int16 left_wheel_msg;
ros::Publisher left_wheel_pub("lwheel" , &left_wheel_msg);
std_msgs::Int16 right_wheel_msg;
ros::Publisher right_wheel_pub("rwheel" , &right_wheel_msg);


double pos_act_left = 0;                    //Actual speed for left wheel in m/s
double pos_act_right = 0;                    //Command speed for left wheel in m/s 

void setup() {
  nh.initNode();
  nh.subscribe(sub);
  nh.advertise(left_wheel_pub); 
  nh.advertise(right_wheel_pub);    //prepare to publish speed in ROS topic
//  Serial.begin(115200);

  rightPID.SetMode(AUTOMATIC);
  rightPID.SetSampleTime(1);
  rightPID.SetOutputLimits(-100, 100);

  leftPID.SetMode(AUTOMATIC);
  leftPID.SetSampleTime(1);
  leftPID.SetOutputLimits(-100, 100);

//  Serial.println("Basic Encoder Test:");
  attachInterrupt(digitalPinToInterrupt(left.en_a), change_left_a, CHANGE);
  attachInterrupt(digitalPinToInterrupt(left.en_b), change_left_b, CHANGE);
  attachInterrupt(digitalPinToInterrupt(right.en_a), change_right_a, CHANGE);
  attachInterrupt(digitalPinToInterrupt(right.en_b), change_right_b, CHANGE);
}

void loop() {
  currentMillis = millis();
  if (currentMillis - prevMillis >= LOOPTIME){
    prevMillis = currentMillis;

    demand_speed_left = demandx - (demandz*0.1075);
    demand_speed_right = demandx + (demandz*0.1075);

    /*PID controller for speed control
      Base speed being 1 ms and the demand_speed variables controlling it at fractions of the base.
      The PID controller keeps trying to match the difference 
      in encoder counts to match with the required amount, hence controlling the speed. */
    encoder0Diff = encoder0Pos - encoder0Prev; // Get difference between ticks to compute speed
    encoder1Diff = encoder1Pos - encoder1Prev;

    pos_act_left = encoder0Pos;                    
    pos_act_right = encoder1Pos; 

    encoder0Error = (demand_speed_left*39.65)-encoder0Diff; // 3965 ticks in 1m = 39.65 ticks in 10ms, due to the 10 millis loop
    encoder1Error = (demand_speed_right*39.65)-encoder1Diff;

    encoder0Prev = encoder0Pos; // Saving values
    encoder1Prev = encoder1Pos;

    left_setpoint = demand_speed_left*39.65;  //Setting required speed as a mul/frac of 1 m/s
    right_setpoint = demand_speed_right*39.65;

    left_input = encoder0Diff;  //Input to PID controller is the current difference
    right_input = encoder1Diff;

    leftPID.Compute();
    left.rotate(left_output);
    rightPID.Compute();
    right.rotate(right_output);
//    Serial.print(encoder0Pos);
//    Serial.print(",");
//    Serial.println(encoder1Pos);

  publishPos(LOOPTIME);

  if(updatenh>10){
    nh.spinOnce();
    updatenh=0;
  }else{
    updatenh++;
  }  
  }


}


//Publish function for odometry, uses a vector type message to send the data (message type is not meant for that but that's easier than creating a specific message type)
void publishPos(double time) {
  int pos_act_left;
  int pos_act_right;
  left_wheel_msg.data = pos_act_left;      //timestamp for odometry data
  right_wheel_msg.data = pos_act_right;    //left ...
(more)
edit retag flag offensive close merge delete

Comments

Hi @ahmet, please provide more details of your exact problem like screenshots of the terminal window and tf tree so that we can help you better in solving your problem.

Robo_guy gravatar image Robo_guy  ( 2022-04-25 02:10:02 -0500 )edit

i added my code

ahmet gravatar image ahmet  ( 2022-04-25 02:28:31 -0500 )edit

car wheels are moving but tf tree not move

ahmet gravatar image ahmet  ( 2022-04-25 03:41:53 -0500 )edit

Ok, what do you mean by tf tree not move? a particular frame like base_footprint or odom is not moving in rviz?

Robo_guy gravatar image Robo_guy  ( 2022-04-25 04:07:17 -0500 )edit

yes,odom is not moving in rviz

ahmet gravatar image ahmet  ( 2022-04-25 04:43:36 -0500 )edit

Ok, it could be you are keeping wrong frame of reference in the global frame in rviz and that is why you cannot see the odom frame moving in rviz, but I think as @aarsh_t mentioned you need to publish the nav_msgs/Odometry Message and also launch robot_state_publisher node as this publishes all the tf information for visualizing in rviz and gazebo.

Robo_guy gravatar image Robo_guy  ( 2022-04-25 07:32:50 -0500 )edit

I'm new to ros, can you help me make this edit? How should I add a code?

ahmet gravatar image ahmet  ( 2022-04-26 00:41:48 -0500 )edit
1

I agree with @Joe28965, it is better to watch vide tutorials about odometry so that you can understand the concept properly or ask your professor who can explain you better about it. To start, with you can have a look at this - http://wiki.ros.org/navigation/Tutori... You can also find many other tutorials here which can clear your concepts, also this - https://www.theconstructsim.com/ is a great place to learn ros through video tutorials, you can also find some of their videos on youtube. Hope this helps you. Good luck with your project !!

Robo_guy gravatar image Robo_guy  ( 2022-04-26 02:47:52 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
3

answered 2022-04-25 04:20:44 -0500

aarsh_t gravatar image

updated 2022-04-25 10:07:03 -0500

Hello, You do not have any topic corresponding to the Odometry. You should be publishing the this msg in order to visualize the odometry of the robot. Also, you should be launching the robot_state_publisher node along with the gazebo simulation(if you are using) to convert/update the state of the robot into tf2.

Edit-1

I guess you only need a current position of the robot in the rviz. Do you have the same launch file drive.launch as the person is launching in he video? If not, you need differential-drive package. it will convert the left wheel velocity and right wheel velocity to the nav_msgs/Odometry and the tf frame.

before launching the rviz, do rosrun differential_drive diff_tf.py

edit flag offensive delete link more

Comments

//Publish function for odometry, uses a vector type message to send the data (message type is not meant for that but that's easier than creating a specific message type)

void publishPos(double time) {
  int pos_act_left;
  int pos_act_right;
  left_wheel_msg.data = pos_act_left;      //timestamp for odometry data
  right_wheel_msg.data = pos_act_right;    //left wheel speed (in m/s)
  left_wheel_pub.publish(&left_wheel_msg);   //right wheel speed (in m/s)
  right_wheel_pub.publish(&right_wheel_msg);         //looptime, should be the same as specified in LOOPTIME (in s)
ahmet gravatar image ahmet  ( 2022-04-25 04:50:53 -0500 )edit

Isn't this part about odometry?

ahmet gravatar image ahmet  ( 2022-04-25 04:51:22 -0500 )edit

also i wont use gazebo

ahmet gravatar image ahmet  ( 2022-04-25 04:52:23 -0500 )edit

How should I edit the code, do you have any suggestions?

ahmet gravatar image ahmet  ( 2022-04-25 04:53:10 -0500 )edit

You will need to create a publisher for publishing the nav_msgs/Odometry Message and create a launch file for launching all the nodes where you will need to include the robot_state_publisher node also.

Robo_guy gravatar image Robo_guy  ( 2022-04-25 07:35:00 -0500 )edit

link textI am running the code here, there is no problem there because there is a video on the youtube page.(https://www.youtube.com/watch?v...

ahmet gravatar image ahmet  ( 2022-04-25 07:48:33 -0500 )edit
1

Odometry is how much your robot has moved as seen from the base_link/base_footprint. You only publish how much the wheels have rotated. You're missing the calculation from wheel rotation to robot movement. It's basically the reverse of what you do with cmd_vel. Cmd_vel is the speed the robot should move, so you calculate how fast each wheel should go to accomplish that. Odometry is the reverse: my wheels go this fast, so how fast does my robot go?

Joe28965 gravatar image Joe28965  ( 2022-04-25 09:22:50 -0500 )edit

I'm new to ros, can you help me make this edit? How should I add a code?

ahmet gravatar image ahmet  ( 2022-04-26 00:42:01 -0500 )edit

Question Tools

Stats

Asked: 2022-04-23 08:46:58 -0500

Seen: 360 times

Last updated: Apr 25 '22