Project

General

Profile

Revision ddfeb111

IDddfeb11183e90127649a5f69425bedc926bda71c

Added by Priya about 4 years ago

Fixing bugs with ghost scout (these were problems in odometry related
to constants in the simulator and coordinate frame and theta representation
in the simulator). Also made ghost scout translucent in order to differentiate
it.

View differences:

scout/libscout/src/behaviors/Odometry.cpp
23 23
  motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
24 24
  motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
25 25

  
26
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", scout_enc.fl_ticks, scout_enc.fr_ticks, scout_enc.bl_ticks, scout_enc.br_ticks);
27
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_ticks, motor_fr_ticks, motor_bl_ticks, motor_br_ticks);
28
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_dist, motor_fr_dist, motor_bl_dist, motor_br_dist);
29

  
30 26
  // Get Left and Right distance
31 27
  left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +
32 28
                             ((float)(motor_bl_dist))/2.0);
......
34 30
                              ((float)(motor_br_dist))/2.0);
35 31

  
36 32
  total_dist = (left_dist)/2.0 + (right_dist)/2.0;
37
  theta = scout_pos->theta + atan2(left_dist - right_dist, WHEEL_BASE);
38

  
39
  ROS_INFO("l: %f   r: %f   theta: %f", left_dist, right_dist, theta);
40

  
41
  scout_pos->x += total_dist*sin(theta);
42
  scout_pos->y += total_dist*cos(theta);
33
  theta = scout_pos->theta + atan2(right_dist - left_dist, WHEEL_BASE);
34

  
35
  //Use negative theta because we measure theta from 0 on the
36
  //right to 180 on the left counter-clock-wise, but this is
37
  //negative theta in the coordinate frame.
38
  //Also, subtract the delta from y because positive y is down.
39
  scout_pos->x += total_dist*cos(-theta);
40
  scout_pos->y -= total_dist*sin(-theta);
43 41
  scout_pos->theta = fmod(theta, 2*M_PI);
44 42

  
45 43
  //Save state for next time in.
......
65 63
    position.y = scout_pos->y;
66 64
    position.theta = scout_pos->theta;
67 65
    scout_position.publish(position);
68

  
69
    ROS_INFO("Scout is at x: %f  y: %f theta: %f\n", scout_pos->x, scout_pos->y, scout_pos->theta);
70 66
  }
71 67
}
scout/libscout/src/behaviors/Odometry.h
6 6
#include "messages/ScoutPosition.h"
7 7

  
8 8

  
9
#define WHEEL_RADIUS  2
9
#define WHEEL_RADIUS  .2
10 10
#define WHEEL_CIRCUM  (2*M_PI*WHEEL_RADIUS)
11
#define WHEEL_BASE    5
12
#define ENCODER_COUNT (4*1200*M_PI/5)
11
#define WHEEL_BASE    1.2
12
#define ENCODER_COUNT (2*WHEEL_RADIUS*350*M_PI/WHEEL_BASE)
13 13
#define DIST_PER_TICK (WHEEL_CIRCUM/ENCODER_COUNT)
14 14

  
15 15
typedef struct{
scout/scoutsim/src/ghost_scout.cpp
76 76

  
77 77
  void GhostScout::setPosition(const ::messages::ScoutPosition& msg)
78 78
  {
79
    ROS_INFO("UPDATING GHOST SCOUT POSITION");
80

  
81 79
    pos.x = start_x + msg.x;
82 80
    pos.y = start_y - msg.y;
83 81
    orient = msg.theta;
......
104 102
      world_state state)
105 103
  {
106 104

  
105
    wxImage rotated_image = scout_image.Rotate(orient - M_PI/2.0,
106
        wxPoint(scout_image.GetWidth() / 2,
107
          scout_image.GetHeight() / 2),
108
        false);
109

  
110
    for (int y = 0; y < rotated_image.GetHeight(); ++y)
111
    {
112
      for (int x = 0; x < rotated_image.GetWidth(); ++x)
113
      {
114
        if (rotated_image.GetRed(x, y) == 255
115
            && rotated_image.GetBlue(x, y) == 255
116
            && rotated_image.GetGreen(x, y) == 255)
117
        {
118
          rotated_image.SetAlpha(x, y, 0);
119
        }
120
        else
121
        {
122
          rotated_image.SetAlpha(x, y, 125);
123
        }
124
      }
125
    }
126

  
127
    scout = wxBitmap(rotated_image);
128

  
107 129
    geometry_msgs::Pose2D my_pose;
108 130
    my_pose.x = pos.x;
109 131
    my_pose.y = pos.y;
110 132
    my_pose.theta = orient;
111 133

  
112
    //ROS_INFO("Ghost scout is at position %f  %f  %f", pos.x, pos.y, orient);
134
    ROS_INFO("Scout position %f  %f  %f", pos.x, pos.y, orient);
113 135

  
114 136
    return my_pose;
115 137
  }
scout/scoutsim/src/scout.cpp
465 465

  
466 466
        Pose p;
467 467
        p.x = pos.x;
468
        p.y = state.canvas_height - pos.y;
468
        p.y = pos.y;
469 469
        p.theta = orient;
470 470
        p.linear_velocity = l_speed;
471 471
        p.angular_velocity = r_speed;
scout/scoutsim/src/sim_frame.cpp
122 122
            &SimFrame::wirelessCallback, this);
123 123

  
124 124
        // Teleop
125
        teleop_type = TELEOP_OFF;
125
        teleop_type = TELEOP_PRECISE;
126 126
        teleop_l_speed = 0;
127 127
        teleop_r_speed = 0;
128 128
        teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000);

Also available in: Unified diff