I implemented the code from “rosbot.cpp”, “odometryUpdater()” (with some small modifications) for my robot. These data are sent as pose message to the rqt topic monitor.
When rotating the wheels forward, the x-position increases like expected, but after a while the position decreases again, which is not obvious for me.
The robot is mounted (for testing) on a plate so it does not move really, only the wheels can rotate free and the encoder gives also increasing counts.
Could you post here complete code that you implemented?
Could you also record and post here a rosbag file showing the issue?
To record a rosbag file issue:
rosbag record -a
This will save to file all messages that are pubilished until
Ctrl + C are pressed.
here is the relevant code:
enc_L = hMot1.getEncoderCnt();
enc_R = hMot2.getEncoderCnt();
wheel_L_ang_vel = ((2 * M_PI * enc_L / enc_res) - wheel_L_ang_pos) / delay_s;
wheel_R_ang_vel = ((2 * M_PI * enc_R / enc_res) - wheel_R_ang_pos) / delay_s;
wheel_L_ang_pos = 2 * M_PI * enc_L / enc_res;
wheel_R_ang_pos = 2 * M_PI * enc_R / enc_res;
robot_angular_vel = (((wheel_R_ang_pos - wheel_L_ang_pos) * wheel_radius / robot_width) -
robot_angular_pos = (wheel_R_ang_pos - wheel_L_ang_pos) * wheel_radius / robot_width;
robot_x_vel = (wheel_L_ang_vel * wheel_radius + robot_angular_vel * robot_width / 2) *
robot_y_vel = (wheel_L_ang_vel * wheel_radius + robot_angular_vel * robot_width / 2) *
robot_x_pos = robot_x_pos + robot_x_vel * delay_s;
robot_y_pos = robot_y_pos + robot_y_vel * delay_s;
pose.pose.position.x = robot_x_pos;
pose.pose.position.y = robot_y_pos;
pose.pose.orientation = tf::createQuaternionFromYaw(robot_angular_pos);
pose.header.stamp = nh.now();
leftEncoder.data = enc_L;
rightEncoder.data = enc_R;
// safety stop
Unfortunately I can’t upload the bag file (Upload shows the text “Uploading” and after some time it ends without showing a file …
BTW: it is not allowed to upload a file with the extension “bag”.
Maybe I found my problem:
Seems that the values for left and right encoder (at least the rotation of the wheels) is too different. So the value of “robot_angular_pos” is too big and the cosinus becomes negative. I did not recognize this because my robot did not move.
Is there a ROS package which handles the movement of a robot for a given velocity (“cmd_vel”) and handles the left and right motor speed in relation to the robot sizes and the encoder values? To be clear I don’t mean navigation but only (manual) steering of the robot.
A bag file can be too big to attach it to forum post, it will be better to use file sharing service like Dropbox or Google Drive and post here only a link.
I am not aware of a universal ROS package that handles motor speeds accordingly to given
cmd_vel, this is a device specific implementation. And this is already implemented in rosbot.cpp, there are PID regulators that are handling the motors speed accordingly to desired velocities. In our tests, we always get straight lines for linear speed, but it is very vulnerable to inaccuracies in robot dimensions. I mean, if values specified as
wheel_radius are different from real dimensions, you may get errors like yours. All dimensions in our code are for ROSbot, if you are using different platform, please make sure that you adjusted all values to your device.
Thanks for your reply. I looked at the rosbot.cpp code but it is not clear to me how to integrate it in the ROS environment. Unfortunately I can’t find any example for it.
Can you give me any hints.
File rosbot.cpp is part of hFramework - our library for robotic devices.
Rosbot.cpp is not communicating with ROS, this is only controller for motors, sensors, etc.
To integrate with ROS environment you need to build your program as described in Tutorial 3
thanks for the hint. I wasn’t aware that the contents of tutorial 3 changed.
While looking at this, I have a question about the setSpeed functions. What is the meaning of the two constants (0.001 and 2.05)? Are they robot specific?
BTW the call of “initBatteryMonitor” in “initROSbot” is missing a parameter.
I made some changes in hFramework. Values you are asking for, are not valid now.
2.05 was used to get radius from
robot_width plus a little increase for compensation of wheels shift to front and rear. Now it is moved to separate constant in appropriate place.
0.001 was a workaround for time units (seconds vs. milliseconds).
Both issues are fixed now, you can check changes in repository. They will be visible in web IDE soon.
initBatteryMonitor is not missing a parameter, it has a default value of
10.5V. If you do not specify any value, this one will be used.
I modified my program with your changes. But now both motors are running a while (about 5 seconds) after sending a “reset_odom” message.
Another problem is that after using the “i” command in the “teleop_twist_keyboard” the left and right encoder values are nearly the same (I included publichers for left and right encoders) but the pose.y value is not “0” as I would expect.
I will make some additional tests for odometry reset and will get back to you.
The pose.y value will be exactly “0” only in ideal model. In real conditions it will differ a little. Just a few ticks difference between encoders is enough to alter the y position.
If it is near to zero (an order of magnitude smaller than pose.x), then everything is correct.
I found the problem with
reset_odom, now it stops robot and sets all odometry parameters to zero.
The change is available in repository or in webIDE with
Development version of hFramework.
that works! Thanks for your quick help!
can you lease explain the function of the “tyre_deflection” and “diameter_mod” parameters?
How can I implement a kind of security stop (in case of an obstacle)? Is the turnOff() the correct way? But when call turnOn() again?
tyre_deflection is coefficient for compensation of tyre flexibility. If we assume wheels as non-deformable rigid bodies, we could calculate traveled distance with use of standard inverse-kinematics. In real world we need to take into account that tyres are deflecting a little, thus traveled distance is changed.
diameter_mod is coefficient for compensation of wheels placement. In our model we calculate average rotation of front and rear wheel and assume it as virtual side wheel between both of them. When robot rotates, its angular position is calculated as quotient of distance traveled by wheels and rotation diameter (
robot_width in code). The rotation diameter could be a distance between geometrical centers of bot wheels if they are non-deformable rigid bodies with infinitesimal width. In our case, we need to take into account, that wheels are quite wide and each of them is moved to the front or rear relative to robot geometrical center.
For security stop, the
turnOff() will be correct way, the motors will stop immediately.
turnOn() should be called when cause of stopping the robot is safely solved. It will depend on your use case.