#181 CMD_CAR fix

Release-2.0
closed-rejected
Stage (103)
5
2009-01-04
2007-05-22
Brian Gerkey
No

Discussion

  • Toby Collett
    Toby Collett
    2007-06-09

    • labels: 680444 --> Stage
     
  • Toby Collett
    Toby Collett
    2007-06-09

    Logged In: YES
    user_id=974361
    Originator: NO

    moved to stage category... body of message was
    Hello,
    below is a patch to address part of bug # "[ 1591448 ] fix CAR commands"

    it adds support to the CMD_CAR to stage. The patch is against stage 2.0.3

    I've compiled and tested it briefly.

    --brendan

    diff -up stage-2.0.3/src/model_position.c stage-2.0.3.new/src/model_position.c
    --- stage-2.0.3/src/model_position.c 2006-03-28 18:35:35.000000000 -0500
    +++ stage-2.0.3.new/src/model_position.c 2007-05-19 17:41:09.000000000 -0
    400
    @@ -376,6 +376,45 @@ int position_update( stg_model_t* mod )
    }
    } break;

    + case STG_POSITION_CONTROL_CAR :
    + {
    + PRINT_DEBUG( "car control mode" );
    + PRINT_DEBUG4( "model %s command(%.2f %.2f %.2f)",
    + mod->token,
    + mod->cmd.x,
    + mod->cmd.y,
    + mod->cmd.a );
    + double a_error = NORMALIZE( cmd->a - data->pose.a );
    + PRINT_DEBUG1( "error: %.2f\n", a_error );
    +
    + // speed limits for controllers
    + // TODO - have these configurable
    + double max_speed_a = 1.0;
    +
    +
    + switch( cfg->drive_mode )
    + {
    + case STG_POSITION_DRIVE_DIFFERENTIAL:
    + // differential-steering model, like a Pioneer
    + vel->x = cmd->x;
    + vel->y = 0;
    + vel->a = MIN(a_error, max_speed_a);
    + vel->a = MAX(a_error, -max_speed_a);
    + break;
    +
    + case STG_POSITION_DRIVE_OMNI:
    + // direct steering model, like an omnidirectional robot
    + vel->x = cmd->x;
    + vel->y = cmd->y;
    + vel->a = MIN( a_error, max_speed_a );
    + break;
    +
    + default:
    + PRINT_ERR1( "unknown steering mode %d", cfg->drive_mode );
    + }
    + } break;
    +
    +
    case STG_POSITION_CONTROL_POSITION:
    {
    PRINT_DEBUG( "position control mode" );
    @@ -589,7 +628,6 @@ int position_render_data( stg_model_t* m
    if( mod->subs )
    {
    stg_position_data_t* odom = (stg_position_data_t*)mod->data;
    - stg_velocity_t* vel = &mod->velocity;
    stg_geom_t *geom = &mod->geom;

    //printf( "odom pose [%.2f %.2f %.2f] origin [%.2f %.2f %.2f]\n",
    diff -up stage-2.0.3/src/p_position.cc stage-2.0.3.new/src/p_position.cc
    --- stage-2.0.3/src/p_position.cc 2006-03-14 15:03:53.000000000 -0500
    +++ stage-2.0.3.new/src/p_position.cc 2007-05-19 17:32:08.000000000 -0400
    @@ -116,7 +116,7 @@ int InterfacePosition::ProcessMessage(Me
    scmd.x = pcmd->velocity;
    scmd.y = 0;
    scmd.a = pcmd->angle;
    - scmd.mode = STG_POSITION_CONTROL_VELOCITY;
    + scmd.mode = STG_POSITION_CONTROL_CAR;
    stg_model_set_cmd( this->mod, &scmd, sizeof(scmd));
    }

    diff -up stage-2.0.3/src/stage.h stage-2.0.3.new/src/stage.h
    --- stage-2.0.3/src/stage.h 2006-03-29 00:11:00.000000000 -0500
    +++ stage-2.0.3.new/src/stage.h 2007-05-19 17:31:58.000000000 -0400
    @@ -964,7 +964,7 @@ For help with libstage, please use the m
    // POSITION MODEL --------------------------------------------------------

    typedef enum
    - { STG_POSITION_CONTROL_VELOCITY, STG_POSITION_CONTROL_POSITION }
    + { STG_POSITION_CONTROL_VELOCITY, STG_POSITION_CONTROL_POSITION, STG_POSITIO
    N_CONTROL_CAR }
    stg_position_control_mode_t;

    #define STG_POSITION_CONTROL_DEFAULT STG_POSITION_CONTROL_VELOCITY

    -------------------------------------------------------------------------
    This SF.net email is sponsored by DB2 Express
    Download DB2 Express C - the FREE version of DB2 express and take
    control of your XML. No limits. Just data. Click to get it now.
    http://sourceforge.net/powerbar/db2/
    _______________________________________________
    Playerstage-developers mailing list
    Playerstage-developers@lists.sourceforge.net
    https://lists.sourceforge.net/lists/listinfo/playerstage-developers

     
  • Brian Gerkey
    Brian Gerkey
    2007-10-18

    • assigned_to: gerkey --> rtv
     
  • Jeremy Asher
    Jeremy Asher
    2008-08-01

    • milestone: --> Release-2.0
     
  • Toby Collett
    Toby Collett
    2009-01-04

    • status: open --> closed-rejected
     
  • Toby Collett
    Toby Collett
    2009-01-04

    behaviour of car cmd is as designed, vehicle relative steering angle and velocity in X. happy to accept patches that add other modes and possibly clarifies naming of command types.