Ask Your Question
2

rflex/b21 cmd_vel not working as expected

asked 2011-08-15 00:44:27 -0500

Tim Heinrich gravatar image

updated 2014-01-28 17:10:12 -0500

ngrennan gravatar image

Hi, I have a problem with a B21r. Here is what I did:

  • Start roscore
  • Start b21 drivers (rosrun rflex b21)
  • Publish this message to /b21/cmd_vel:
    • rostopic pub -1 /b21/cmd_vel geometry_msgs/Twist '[0, 0, 0]' '[0, 0, 0.3]'

What I expect and get in simulations is that the robot turns a step and stops.

What I get with is that it takes a while until the robot starts to move, then turns, stops, turns another step, stops, and so on (with different step sizes). I watched /b21/cmd_vel, the command is only sent once. Sometimes the b21 driver prints a message like "B21 Home 26.523571".

Is this this correct behavior and am I missing something? It would be nice if someone could explain this behavior to me. We tried navigating with ROSE, and it worked correctly, so it seems not to be a hardware issue.

edit retag flag offensive close merge delete

6 Answers

Sort by » oldest newest most voted
0

answered 2011-08-23 04:29:13 -0500

David Lu gravatar image

When I wrote the RFLEX driver, I tested it with the joystick teleop and navigation stacks. I had not really considered the effects of sending just one message.

When the driver recieves the Twist message, it stores the velocities and sends them to the hardware continuously. The effect I would expect from running that command is for the robot to turn in a circle indefinitely. I don't know why it stops.

The B21 Home message is leftover debug information (sorry about that) for when it hits its 'home' orientation.

I wrote the driver awhile ago. If this is not the standard behavior for working with cmd_vel commands, I can fix it. Or better yet, you can submit a patch :)

edit flag offensive delete link more
1

answered 2011-11-29 02:11:50 -0500

michikarg gravatar image

We could actually fix the problem. Having a look at the Player-implementation, we found the following:

// * workaround for stupid hardware bug, cause unknown, but this works
// *
with minimal detriment to control // ** avoids all values with 1b in highest or 3'rd highest order byte

// 0x1b is part of the packet terminating string // which is most likely what causes the bug

// * if 2'nd order byte is 1b, round to nearest 1c, or 1a
if((urvel&0xff00)==0x1b00){ // *
if lowest order byte is>127 round up, otherwise round down urvel=(urvel&0xffff0000)|((urvel&0xff)>127?0x1c00:0x1aff); }

So we also included this into the rflex_driver.cc of the rflex ROS-package and this seemed to fix the strange behaviour with the delays and the stops between the movement. Apparently i don´t have enough Karma to upload files yet, so i´ll send you a patch via mail so you can test it.

Cheers

Michi

edit flag offensive delete link more

Comments

If you create a ticket in the tracker (https://sourceforge.net/tracker/?group_id=261576&atid=1333666) and attach a patch, we can incorporate your fix.
Dan Lazewatsky gravatar image Dan Lazewatsky  ( 2011-11-29 08:54:53 -0500 )edit
A created a ticked and appended the patch!
michikarg gravatar image michikarg  ( 2011-12-12 03:23:29 -0500 )edit
1

answered 2012-09-12 10:24:41 -0500

mmedvede gravatar image

I confirm that michikarg's fix does work.

I was also able to fix the issue 'in code' by gently ripping out the port initialization function from original player's rflex driver, and adding it to ROS rflex_driver.cc. I did not clean the code and now my driver has two redundant initializations.

To save you some time, here is the code I have added (rflex-player.hh):

/*
 * Functions ripped straight from player's rflex driver to fix the issue
 * with port initialization.
 */

#ifndef RFLEX_PLAYER_HH_
#define RFLEX_PLAYER_HH_

#include <rflex/rflex_info.h>
#include <fcntl.h>
#include <termios.h>

#define MAX_NAME_LENGTH                256

enum PARITY_TYPE   { N, E, O };

typedef struct {
  char                       ttyport[MAX_NAME_LENGTH];
  int                        baud;
  enum PARITY_TYPE           parity;
  int                        fd;
  int                        databits;
  int                        stopbits;
  int                        hwf;
  int                        swf;
} RFLEX_Device;

// From rflex-io.cc
int iParity(enum PARITY_TYPE par) {
    if (par == N)
        return (IGNPAR);
    else
        return (INPCK);
}

int iSoftControl(int flowcontrol) {
    if (flowcontrol)
        return (IXON);
    else
        return (IXOFF);
}

int cDataSize(int numbits) {
    switch (numbits) {
    case 5:
        return (CS5);
        break;
    case 6:
        return (CS6);
        break;
    case 7:
        return (CS7);
        break;
    case 8:
        return (CS8);
        break;
    default:
        return (CS8);
        break;
    }
}

int cStopSize(int numbits) {
    if (numbits == 2) {
        return (CSTOPB);
    } else {
        return (0);
    }
}

int cFlowControl(int flowcontrol) {
    if (flowcontrol) {
        return (CRTSCTS);
    } else {
        return (CLOCAL);
    }
}

int cParity(enum PARITY_TYPE par) {
    if (par != N) {
        if (par == O) {
            return (PARENB | PARODD);
        } else {
            return (PARENB);
        }
    } else {
        return (0);
    }
}

int cBaudrate(int baudrate) {
    switch (baudrate) {
    case 0:
        return (B0);
        break;
    case 300:
        return (B300);
        break;
    case 600:
        return (B600);
        break;
    case 1200:
        return (B1200);
        break;
    case 2400:
        return (B2400);
        break;
    case 4800:
        return (B4800);
        break;
    case 9600:
        return (B9600);
        break;
    case 19200:
        return (B19200);
        break;
    case 38400:
        return (B38400);
        break;
    case 57600:
        return (B57600);
        break;
    case 115200:
        return (B115200);
        break;
#ifdef B230400
    case 230400:
        return (B230400);
        break;
#endif

    default:
        return (B9600);
        break;
    }

}

void DEVICE_set_params(RFLEX_Device dev) {
    struct termios ctio;

    tcgetattr(dev.fd, &ctio); /* save current port settings */

    ctio.c_iflag = iSoftControl(dev.swf) | iParity(dev.parity);
    ctio.c_oflag = 0;
    ctio.c_cflag = CREAD | cFlowControl(dev.hwf || dev.swf)
            | cParity(dev.parity) | cDataSize(dev.databits)
            | cStopSize(dev.stopbits);

    ctio.c_lflag = 0;
    ctio.c_cc[VTIME] = 0; /* inter-character timer unused */
    ctio.c_cc[VMIN] = 0; /* blocking read until 0 chars received */

    cfsetispeed(&ctio, (speed_t) cBaudrate(dev.baud));
    cfsetospeed(&ctio, (speed_t) cBaudrate(dev.baud));

    tcflush(dev.fd, TCIFLUSH);
    tcsetattr(dev.fd, TCSANOW, &ctio);

}

int DEVICE_connect_port(RFLEX_Device *dev) {
    if ((dev->fd = open((dev->ttyport), (O_RDWR | O_NOCTTY), 0)) < 0) {
        return (-1);
    }
    DEVICE_set_params(*dev);
    return (dev->fd);
}
// /rflex-io.h

int rflex_open_connection(const char *device_name, int *fd) {
    RFLEX_Device rdev;

    strncpy(rdev.ttyport, device_name, MAX_NAME_LENGTH);
    rdev.baud = 115200;
    rdev.databits = 8;
    rdev.parity = N;
    rdev.stopbits = 1;
    rdev.hwf = 0;
    rdev.swf = 0;

    printf("trying port %s\n", rdev.ttyport);
    if (DEVICE_connect_port(&rdev) < 0) {
        fprintf(stderr, "Can't open device %s\n", rdev.ttyport);
        return -1;
    }

    *fd = rdev.fd;
    return 0;
}

#endif /* RFLEX_PLAYER_HH_ */

Then include the above into rflex_driver.cc and replace (in rflex_driver.cc):

fd = open(device_name, O_RDWR | O_NONBLOCK);
if (fd == -1) {

for

if (rflex_open_connection(device_name, &fd) < 0) {

If someone wants to go through the ... (more)

edit flag offensive delete link more
0

answered 2011-12-13 05:59:51 -0500

robpan gravatar image

Does anyone found a solution for this? the patch doesnt work for me, ia m using atrv jr with telecomport

edit flag offensive delete link more
0

answered 2012-01-12 03:11:34 -0500

michikarg gravatar image

Hey guys, seems like i was celebrating too soon... Today we had the error again although we applied the patch... trying to find the issue, is installed a serial-port sniffer and ran it on the port of the rflex (ttyR3 in our case). After running the sniffer once, the robot moved perfectly, even without our patch...

So it seems like there is some initialization of the serial port missing in the ROS rflex code. Because when we tested it last time we ran the robot with Player first, then applied the patch and ran it with ROS... so it seems like the Player Code and the Serial Sniffer do some initialization on the port (maybe setting of the baudrate?) that is not done when running the ROS rflex package... i´m not sure about the exact details yet, but here´s what we did today:

sudo aptitude install jpnevaluator
jpnevulator --ascii --timing-print --tty /dev/ttyR3 --read

After doing this once while running the rflex node, everything worked fine...

edit flag offensive delete link more

Comments

I'm glad you figured things out. We had many problems with the serial ports for very similar unknown reasons. I think there's subtle differences in how the Linux serial drivers handle initialization, causing all sorts of weird problems.
David Lu gravatar image David Lu  ( 2012-01-12 03:59:39 -0500 )edit

I can confirm that this command also changed the behaviour of our B21 to make one command repeat. Odd!

hawesie gravatar image hawesie  ( 2012-06-07 05:27:44 -0500 )edit
0

answered 2011-11-29 00:01:09 -0500

michikarg gravatar image

Hi,

we have the same problem trying to run our B21 with ROS. Did you find a solution for that issue?

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2011-08-15 00:44:27 -0500

Seen: 690 times

Last updated: Sep 12 '12