Hello
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.
Any ideas?
Michael
Hello Michael,
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.
Regards,
Łukasz
Hi Łukasz
here is the relevant code:
while (true)
{
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) /
delay_s;
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) *
cos(robot_angular_pos);
robot_y_vel = (wheel_L_ang_vel * wheel_radius + robot_angular_vel * robot_width / 2) *
sin(robot_angular_pos);
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();
pose_pub.publish(&pose);
leftEncoder.data = enc_L;
rightEncoder.data = enc_R;
leftEncoder_pub.publish(&leftEncoder);
rightEncoder_pub.publish(&rightEncoder);
handleBatteryStatus();
batteryStatus_pub.publish(&batteryStatus);
handleRange();
range_pub.publish(&range);
if (!nh.connected())
{
// safety stop
hMot1.stop();
hMot2.stop();
}
nh.spinOnce();
sys.delay(delay);
hLED1.toggle();
}
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”.
Michael
Hello
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.
Michael
Hello Michael,
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 robot_width or 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.
Regards,
Łukasz
Hi Łukasz
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.
Thanks!
Michael
Hi Michael,
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
Regards,
Łukasz
Hello Łukasz
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.
Michael
Hi Michael,
I made some changes in hFramework. Values you are asking for, are not valid now.
Value 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.
Value 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.
The 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.
Regards,
Łukasz Mitka
Hello Łukasz
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.
Michael
Hello Michael,
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.
Regards,
Łukasz
Hi Michael,
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.
Regards,
Łukasz
Hi Łukasz
that works! Thanks for your quick help!
Michael
Hi Łukasz
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?
Thanks
Michael
Hello Michael,
The 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.
Similarly the 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.
The turnOn() should be called when cause of stopping the robot is safely solved. It will depend on your use case.
Regards,
Łukasz