Project

General

Profile

Revision 9f547ef7

ID9f547ef719e5b32f52210cdb56bc7cae9eb16c7d
Parent 95bede66
Child 24145262, 0e6831f5

Added by Alex Zirbel over 7 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