I can't see the movement I made with the robot on the rviz screen, what is the reason? [closed]

asked 2022-05-06 03:02:13 -0500

ahmet gravatar image

updated 2022-05-09 01:48:27 -0500

I can't see the movement I made with the robot on the rviz screen, what is the reason?i chose tf

the video shows what I am trying to do

link text

my problem odom turn around base_link axes

    #include "Motor.h"
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <ros/time.h>
#include <std_msgs/Int16.h>
ros::NodeHandle  nh;

int updatenh = 0;

#define LOOPTIME 10

Motor right(11,10,3,2);
Motor left(9,8,5,4);

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

double left_kp = 17, left_ki = 0 , left_kd = 0;             // modify for optimal performance
double right_kp = 20, right_ki = 0 , right_kd = 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 );     //create a "speed_msg" ROS message
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);                         //create a publisher to ROS topic "speed" using the "speed_msg" type

double pos_act_left = 0;                    
double pos_act_right = 0;                    

void setup() {

  nh.advertise(right_wheel_pub);      //prepare to publish speed in ROS topic
//  Serial.begin(115200);

  rightPID.SetOutputLimits(-100, 100);

  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.2);
demand_speed_right = demandx + (demandz*0.2);

/*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;

//    Serial.print(encoder0Pos);
//    Serial.print(",");
//    Serial.println(encoder1Pos);



//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 ...
edit retag flag offensive reopen merge delete

Closed for the following reason duplicate question by Joe28965
close date 2022-05-09 01:59:31.887409


You're still not publishing anything related to Odom, neither are you actually creating a TF broadcaster. This was addressed the last time you asked this question.

I suggest reading these tf tutorials and also reading up about odometry. The construct has some videos on odometry data.

Joe28965 gravatar image Joe28965  ( 2022-05-09 01:58:57 -0500 )edit

If I'm not publish about odom, how does it move?

ahmet gravatar image ahmet  ( 2022-05-09 04:48:57 -0500 )edit

i added video link about my project

ahmet gravatar image ahmet  ( 2022-05-09 04:49:24 -0500 )edit

Fair enough, let me rephrase. The code that you have published only publishes the wheel encoders/speed. It does not publish anything related to odom. Yet your problem is related to odom.

I suggest looking where your odom TF comes from and first start looking there. Then if you still can't get it working, you can ask a question with the relevant code.

Joe28965 gravatar image Joe28965  ( 2022-05-09 04:59:45 -0500 )edit

I don't understand what you mean when you tell me to check where your Odom TF came from, can you help?

ahmet gravatar image ahmet  ( 2022-05-09 05:09:20 -0500 )edit

Are you a student? In that case I highly recommend asking your peers or professors to help you answer this question.

First, check the tf tutorials I linked in the first comment.

Second, tf has something called a broadcaster which broadcasts the tf (the transformation matrix) between two links, in this case odom and base_link (or base_footprint, I don't know which you use, but that's not too important).

That TF broadcaster uses the encoder data to figure out how much your robot has moved and then creates the broadcast. It's obviously not broadcasting correctly.

That's what I mean, but as I said, I suggest doing some googling and watching some videos on TF and odom (for instance from the construct).

Joe28965 gravatar image Joe28965  ( 2022-05-09 05:22:20 -0500 )edit

I'm a mechanical engineer, but I'm new to these issues, I'm someone who is curious to learn:)

ahmet gravatar image ahmet  ( 2022-05-09 05:43:29 -0500 )edit

I understand, the problem is that your question is too broad for this forum, which is why I pointed you towards the tutorials and videos.

As I said, I'd advise you to find out where the odom transform is coming from. If you do not know how, I suggest you first dive into TF and odom by watching videos and doing some tutorials.

Joe28965 gravatar image Joe28965  ( 2022-05-09 06:05:59 -0500 )edit