How to use cmd_vel data in navigation stack?

asked 2016-07-16 14:05:50 -0500

Oper
67 ●9 ●13 ●15

updated 2016-07-16 17:44:50 -0500

I'm trying to convert geometry_msgs::Twist message received from cmd_vel topic to something that can make my arduino robot move. I know that from geometry_msgs::Twist message i'll receive linear velocity and angular velocity. My motors are actually controlled by calling functions like that:

void RobotControl::goAhead(int PWM){

digitalWrite(_IN1, LOW); digitalWrite(_IN2, HIGH);

digitalWrite(_IN3, LOW);

digitalWrite(_IN4, HIGH);

analogWrite(_ENB, PWM);

analogWrite(_ENA, PWM); }

in this case, PWM is a value between 0 and 200 and sets speed for my motors in this range.

The main question is:

Is there a way to use received cmd_vel data and convert this to instructions that can make my robot move by simpy call goAhead() function? If my goAhead function has not been implemented in the right way, how to implement a funciton that solve the problem?

Thank u!

--- EDIT--- ok, let's see if i understand this:

velCallback(Twist &twistmsg) {
vel_x = twistmsg.linear.x;
vel_th = twistmsg.angular.z;
if(vel_x == 0){
right_vel = vel_th * width_robot / 2.0;
left_vel = (-1) * right_vel; }
else if(vel_th == 0){
left_vel = right_vel = vel_x;
}
else{
left_vel = vel_x - vel_th / 2.0;
right_vel = vel_x + vel_th / 2.0;
}

now if everything is right i can calcolate RPM:

RPMleft = ((60 * left_vel) / (diameter * PI))
RPMright = ((60 * right_vel) / (diameter * PI))

now i should calcalate PWM, i googled for almost one hour but i can't find anything. Do you know how can i do it?

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2016-07-16 14:15:45 -0500

spmaniato

1788 ●40 ●56 ●69 http://www.spmaniato.me/

updated 2016-07-16 14:16:30 -0500

You have to write a function that performs the body-frame velocities to wheel velocities transformation. The body-frame velocities are contained in the Twist messages published on /cmd_vel. Then you have to also write a second, much simpler function that converts the wheel velocities to motor RPM commands and then PWM commands.

If your robot is a differential-drive one, you can probably Google search for the corresponding equations. There might even be a ROS package that can help with that. The key takeaway is that this function is robot-specific, as it depends on things like number of wheels, wheel configuration, distance between wheels, wheel radius, etc.

Various functions in RobotControl might already be doing some of that work. But based on the input argument of goAhead, I doubt it does the transformation. It probably just has all wheels spin at the same, fixed RPM (in an open-loop fashion).

edit flag offensive delete link

Comments

Thank you man for this fast response, i've just update the main question. Do you know how to convert PWM to RPM? i can't find the formula

Oper ( 2016-07-16 17:42:13 -0500 )edit

I don't think there's a standard way of doing that unfortunately. If you have wheel encoders, you can use feedback from those to write a PID controller. The PID controller will take in the desired RPS (which you calculated already) and the current RPS and spit out PWM values.

spmaniato ( 2016-07-17 09:39:18 -0500 )edit

If you don't have wheel encoders, try something simple, like PWMleft = k * RPMleft and PWMright = k * RPMright, where k is a constant that you figure out based on trial and error. (It may depend on things like terrain, such as wood vs carpet)

spmaniato ( 2016-07-17 09:40:48 -0500 )edit

Yes man, i have wheel encoders, I've just seen some examples about PID controller, my only doubt now is how to calcolate values like Kp Ki Kd and Ko. I think that i can reduce Ki to zero, but i don't have any idea on how to calcolate other values. Do you know how i can do this?

Oper ( 2016-07-17 13:37:03 -0500 )edit

Start simple, just P control first. Get something kinda working and then add complexity and rigor :-)

spmaniato ( 2016-07-17 14:06:42 -0500 )edit

Could anyone explain how to convert the Twist data from the base_link frame into the wheel_link frame?

From the answer, I understood to receive the twist data and extract the X-component of the linear velocity and the Z-component of the angular velocity. Then convert them into the wheel_link frame and send them to my controllers, Am I correct?

MH_Ahmed ( 2019-12-03 08:40:45 -0500 )edit

add a comment

answered 2016-08-02 19:57:12 -0500

Mark Rose

1563 ●8 ●17 ●21

I've used the differential_drive ROS package successfully ( http://wiki.ros.org/differential_drive ). However, since then I've written my own package for one robot and used the built-in code in ros_arduino_bridge for another robot that is Arduino based. The differential_drive package expects to send commands to the motors in meters/second of travel. You will have to use feedback from encoders in a PID controller (or some empirical method) to determine the PWM values.

As for setting PID constants, @spmaniato is right, start simple. Increase Kp until you get quite a bit of overshoot and then back off until the overshoot is reduced to a more reasonable value. Kd can allow a higher Kp while damping overshoot. Ki is often not needed unless there is some external force that will keep you from achieving the setpoint using Kp alone. (In incline, or high friction terrain, or wind, for example.) Ki can also be used to decrease the response time, similar to increasing Kp. Some way of graphing the actual speeds – from the encoders – is good to have, real-time is best. (On an Arduino-based controller you can print the calculated speed to Serial and use the built-in Tools > Serial Plotter to show the plot.)

edit flag offensive delete link

add a comment

How to use cmd_vel data in navigation stack?

2 Answers

Comments

Question Tools

Stats

Related questions

How to use cmd_vel data in navigation stack? edit

2 Answers

Comments

Question Tools

Stats

Related questions

How to use cmd_vel data in navigation stack?