Project

General

Profile

Revision 9f547ef7

ID9f547ef719e5b32f52210cdb56bc7cae9eb16c7d
Parent 95bede66
Child 24145262, 0e6831f5

Added by Alex Zirbel about 12 years ago

Added scoutsim support for encoders.

To see the query_encoders.srv (used for accessing this data) in action, use: rosrun libscout test_encoders. You will see a stream of encoder data.

View differences:

scout/encoders/srv/query_encoders.srv
1 1 
---
2 2 
# All units are simply in encoder ticks since encoders were turned on.
3 
int8 fl_distance
4 
int8 fr_distance
5 
int8 bl_distance
6 
int8 br_distance
3 
uint32 fl_distance
4 
uint32 fr_distance
5 
uint32 bl_distance
6 
uint32 br_distance
scout/libscout/CMakeLists.txt
30 30 
#target_link_libraries(example ${PROJECT_NAME})
31 31 

  		




  32
  32
  
    
rosbuild_add_executable(libscout src/Behavior.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/BehaviorList.cpp src/BehaviorProcess.cpp src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/CliffsensorControl.cpp)


  		




  
  33
  
    
rosbuild_add_executable(test_encoders src/test_encoders.cpp)


  		












  

    
      scout/libscout/manifest.xml
    
  





  11
  11
  
    
  <depend package="std_msgs"/>


  		




  12
  12
  
    
  <depend package="roscpp"/>


  		




  13
  13
  
    
  <depend package="motors"/>


  		




  
  14
  
    
  <depend package="encoders"/>


  		




  14
  15
  
    
  <depend package="headlights"/>


  		




  15
  16
  
    
  <depend package="buttons"/>


  		




  16
  17
  
    
  <depend package="sonar"/>


  		












  

    
      scout/libscout/src/test_encoders.cpp
    
  





  
  1
  
    

  		




  
  2
  
    
#include <ros/ros.h>


  		




  
  3
  
    
#include <encoders/query_encoders.h>


  		




  
  4
  
    

  		




  
  5
  
    
int main(int argc, char **argv)


  		




  
  6
  
    
{


  		




  
  7
  
    
    ros::init(argc, argv, "test_encoders");


  		




  
  8
  
    
    ros::NodeHandle node;


  		




  
  9
  
    

  		




  
  10
  
    
    ros::ServiceClient query_encoders_client =


  		




  
  11
  
    
        node.serviceClient<encoders::query_encoders>("/scout1/query_encoders");


  		




  
  12
  
    

  		




  
  13
  
    
    ros::Rate r(10);


  		




  
  14
  
    

  		




  
  15
  
    
    while (ros::ok())


  		




  
  16
  
    
    {


  		




  
  17
  
    
        encoders::query_encoders srv;


  		




  
  18
  
    
        if (query_encoders_client.call(srv))


  		




  
  19
  
    
        {


  		




  
  20
  
    
            printf("Got encoders: fl:%u, fr:%u, bl:%u, br:%u).\n",


  		




  
  21
  
    
                srv.response.fl_distance,


  		




  
  22
  
    
                srv.response.fr_distance,


  		




  
  23
  
    
                srv.response.bl_distance,


  		




  
  24
  
    
                srv.response.br_distance);


  		




  
  25
  
    
        }


  		




  
  26
  
    
        else


  		




  
  27
  
    
        {


  		




  
  28
  
    
            printf("ERROR: Failed to call service.\n");


  		




  
  29
  
    
        }


  		




  
  30
  
    

  		




  
  31
  
    
        ros::spinOnce();


  		




  
  32
  
    
        r.sleep();


  		




  
  33
  
    
    }


  		




  
  34
  
    

  		




  
  35
  
    
    return 0;


  		




  
  36
  
    
}


  		












  

    
      scout/scoutsim/manifest.xml
    
  





  14
  14
  
    
  <depend package="rosconsole"/>


  		




  15
  15
  
    
  <depend package="std_srvs"/>


  		




  16
  16
  
    
  <depend package="motors" />


  		




  
  17
  
    
  <depend package="encoders" />


  		




  17
  18
  
    
  


  		




  18
  19
  
    
  <rosdep name="wxwidgets"/>


  		




  19
  20
  
    

  		




  ......




  34
  35
  
    
    <depend package="rosconsole"/>


  		




  35
  36
  
    
    <depend package="std_srvs"/>


  		




  36
  37
  
    
    <depend package="motors" />


  		




  
  38
  
    
    <depend package="encoders" />


  		




  37
  39
  
    

  		




  38
  40
  
    
    <msgs>


  		




  39
  41
  
    
      msg/Color.msg  msg/Pose.msg  msg/Velocity.msg 


  		












  

    
      scout/scoutsim/src/scout.cpp
    
  





  56
  56
  
    
namespace scoutsim


  		




  57
  57
  
    
{


  		




  58
  58
  
    
    Scout::Scout(const ros::NodeHandle& nh,


  		




  59
  
  
    
            const wxImage& scout_image,


  		




  60
  
  
    
            const Vector2& pos,


  		




  61
  
  
    
            float orient)


  		




  
  59
  
    
                 const wxImage& scout_image,


  		




  
  60
  
    
                 const Vector2& pos,


  		




  
  61
  
    
                 float orient)


  		




  62
  62
  
    
        : node (nh)


  		




  63
  63
  
    
          , scout_image(scout_image)


  		




  64
  64
  
    
          , pos(pos)


  		




  65
  65
  
    
          , orient(orient)


  		




  66
  
  
    
          , lin_vel(0.0)


  		




  67
  
  
    
          , ang_vel(0.0)


  		




  
  66
  
    
          , motor_fl_speed(0)


  		




  
  67
  
    
          , motor_fr_speed(0)


  		




  
  68
  
    
          , motor_bl_speed(0)


  		




  
  69
  
    
          , motor_br_speed(0)


  		




  
  70
  
    
          , fl_ticks(0)


  		




  
  71
  
    
          , fr_ticks(0)


  		




  
  72
  
    
          , bl_ticks(0)


  		




  
  73
  
    
          , br_ticks(0)


  		




  68
  74
  
    
          , pen_on(true)


  		




  69
  75
  
    
          , pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))


  		




  70
  76
  
    
    {


  		




  ......




  78
  84
  
    
        set_pen_srv = node.advertiseService("set_pen",


  		




  79
  85
  
    
                                            &Scout::setPenCallback,


  		




  80
  86
  
    
                                            this);


  		




  
  87
  
    
        query_encoders_srv =


  		




  
  88
  
    
            node.advertiseService("query_encoders",


  		




  
  89
  
    
                                  &Scout::query_encoders_callback,


  		




  
  90
  
    
                                  this);


  		




  81
  91
  
    

  		




  82
  92
  
    
        meter = scout.GetHeight();


  		




  83
  93
  
    
    }


  		




  ......




  108
  118
  
    
            motor_br_speed = msg->br_speed;


  		




  109
  119
  
    
        }


  		




  110
  120
  
    

  		




  111
  
  
    
        // Assume that the two motors on the same side will be set to


  		




  112
  
  
    
        // roughly the same speed. Does not account for slip conditions


  		




  113
  
  
    
        // when they are set to different speeds.


  		




  114
  
  
    
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;


  		




  115
  
  
    
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;


  		




  116
  
  
    

  		




  117
  
  
    
        // Set the linear and angular speeds


  		




  118
  
  
    
        lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;


  		




  119
  
  
    
        ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);


  		




  120
  121
  
    
    }


  		




  121
  122
  
    

  		




  122
  123
  
    
    float Scout::getSonar(float angle)


  		




  ......




  144
  145
  
    
        return true;


  		




  145
  146
  
    
    }


  		




  146
  147
  
    

  		




  
  148
  
    
    bool Scout::query_encoders_callback(encoders::query_encoders::Request&,


  		




  
  149
  
    
                                        encoders::query_encoders::Response& res)


  		




  
  150
  
    
    {


  		




  
  151
  
    
        res.fl_distance = fl_ticks;


  		




  
  152
  
    
        res.fr_distance = fr_ticks;


  		




  
  153
  
    
        res.bl_distance = bl_ticks;


  		




  
  154
  
    
        res.br_distance = br_ticks;


  		




  
  155
  
    

  		




  
  156
  
    
        return true;


  		




  
  157
  
    
    }


  		




  
  158
  
    

  		




  147
  159
  
    
    void Scout::update(double dt, wxMemoryDC& path_dc,


  		




  148
  160
  
    
                        const wxImage& path_image, wxColour background_color,


  		




  149
  161
  
    
                        float canvas_width, float canvas_height)


  		




  150
  162
  
    
    {


  		




  
  163
  
    
        // Assume that the two motors on the same side will be set to


  		




  
  164
  
    
        // roughly the same speed. Does not account for slip conditions


  		




  
  165
  
    
        // when they are set to different speeds.


  		




  
  166
  
    
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;


  		




  
  167
  
    
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;


  		




  
  168
  
    

  		




  
  169
  
    
        // Set the linear and angular speeds


  		




  
  170
  
    
        float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;


  		




  
  171
  
    
        float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);


  		




  
  172
  
    

  		




  151
  173
  
    
        Vector2 old_pos = pos;


  		




  152
  174
  
    

  		




  
  175
  
    
        // Update encoders


  		




  
  176
  
    
        /// @todo replace


  		




  
  177
  
    
        fl_ticks += (unsigned int) motor_fl_speed;


  		




  
  178
  
    
        fr_ticks += (unsigned int) motor_fr_speed;


  		




  
  179
  
    
        bl_ticks += (unsigned int) motor_bl_speed;


  		




  
  180
  
    
        br_ticks += (unsigned int) motor_br_speed;


  		




  
  181
  
    

  		




  153
  182
  
    
        orient = fmod(orient + ang_vel * dt, 2*PI);


  		




  154
  183
  
    
        pos.x += sin(orient + PI/2.0) * lin_vel * dt;


  		




  155
  184
  
    
        pos.y += cos(orient + PI/2.0) * lin_vel * dt;


  		












  

    
      scout/scoutsim/src/scout.h
    
  





  52
  52
  
    
#include <boost/shared_ptr.hpp>


  		




  53
  53
  
    

  		




  54
  54
  
    
#include <motors/set_motors.h>


  		




  
  55
  
    
#include <encoders/query_encoders.h>


  		




  55
  56
  
    

  		




  56
  57
  
    
#include <scoutsim/Pose.h>


  		




  57
  58
  
    
#include <scoutsim/SetPen.h>


  		




  ......




  110
  111
  
    
            float getSonar(float angle);


  		




  111
  112
  
    
            bool setPenCallback(scoutsim::SetPen::Request&,


  		




  112
  113
  
    
                                scoutsim::SetPen::Response&);


  		




  
  114
  
    
            bool query_encoders_callback(encoders::query_encoders::Request&,


  		




  
  115
  
    
                                         encoders::query_encoders::Response&);


  		




  113
  116
  
    

  		




  114
  117
  
    
            ros::NodeHandle node;


  		




  115
  118
  
    

  		




  ......




  119
  122
  
    
            Vector2 pos;


  		




  120
  123
  
    
            float orient;


  		




  121
  124
  
    

  		




  
  125
  
    
            /// @todo should these be an array or something?


  		




  
  126
  
    

  		




  122
  127
  
    
            // Keep track of the last commanded speeds sent to the sim


  		




  123
  128
  
    
            int motor_fl_speed;


  		




  124
  129
  
    
            int motor_fr_speed;


  		




  125
  130
  
    
            int motor_bl_speed;


  		




  126
  131
  
    
            int motor_br_speed;


  		




  127
  132
  
    

  		




  
  133
  
    
            // Keep track of encoder ticks for each motor


  		




  
  134
  
    
            unsigned int fl_ticks;


  		




  
  135
  
    
            unsigned int fr_ticks;


  		




  
  136
  
    
            unsigned int bl_ticks;


  		




  
  137
  
    
            unsigned int br_ticks;


  		




  
  138
  
    

  		




  128
  139
  
    
            // Each scout has a unique id number, which is also displayed on its image.


  		




  129
  140
  
    
            int scout_id;


  		




  130
  141
  
    

  		




  131
  
  
    
            float lin_vel;


  		




  132
  
  
    
            float ang_vel;


  		




  133
  142
  
    
            bool pen_on;


  		




  134
  143
  
    
            wxPen pen;


  		




  135
  144
  
    

  		




  ......




  137
  146
  
    
            ros::Publisher pose_pub;


  		




  138
  147
  
    
            ros::Publisher color_pub;


  		




  139
  148
  
    
            ros::ServiceServer set_pen_srv;


  		




  
  149
  
    
            ros::ServiceServer query_encoders_srv;


  		




  140
  150
  
    

  		




  141
  151
  
    
            ros::WallTime last_command_time;


  		




  142
  152
  
    

  		












Also available in:  Unified diff