Revision 1057a46d

View differences:

scout/libscout/CMakeLists.txt
38 38

  
39 39
rosbuild_add_executable(libscout ${MAIN_FILES} ${BEHAVIOR_FILES} ${TEST_BEHAVIOR_FILES} ${CONTROL_CLASSES} ${HELPER_FILES})
40 40
rosbuild_add_compile_flags(libscout -std=c++0x)
41
rosbuild_add_executable(test_encoders src/test_encoders.cpp)
scout/libscout/kickstart
1
#!/bin/bash
2
sleep 1
3
rosservice call /spawn 9.6 2.8 3.14 scout2 &
4
exec "$@"
5

  
6
#rostopic pub /scout2_topic std_msgs/String "REG_SUCCESS 1"
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/BehaviorGUI.py
1
#!/usr/bin/python
2

  
3
import wx
4
import wx.lib.intctrl 
5
import wx.lib.agw.floatspin
6
import string
7
import subprocess
8
import signal
9
from collections import defaultdict
10

  
11
# ROS imports
12
import roslib; roslib.load_manifest("scoutsim")
13
import rospy
14
from scoutsim.srv import *
15

  
16
# Everyone global variables (PLEASE REFACTOR ME)
17
scouts = defaultdict(str) # Dict in the form [scout_name: current_behavior]
18
processes = defaultdict() # Dict in the form [scout_name: process_handler]
19

  
20
#
21
# ROS functions
22
#
23
def AddToSimulator(name, x, y, theta):
24
    if name == "":
25
        return False
26
    if name in scouts:
27
        return False
28
    scouts[name] = ""
29
    print "Calling ROS function to create scout "+name
30
    try:
31
        rospy.wait_for_service('/spawn');
32
        service = rospy.ServiceProxy('/spawn', Spawn);
33
        resp = service(x, y, theta, name)
34
        return True
35
    except rospy.ServiceException, e:
36
        return False
37

  
38
def GetBehaviors():
39
    return ["CW Circle", "CCW Circle", "Odometry", "Navigation Map",
40
        "Scheduler", "Warehouse", "Line Follow", "WL Test"]
41

  
42
def SetBehavior(name, behaviorLabel, behaviorButton=False, behavior="Pause", selectedBehavior="Pause"):
43
    if behavior == "Pause":
44
        if name not in processes:
45
            return False
46
        if not behaviorButton.GetValue():
47
            processes[name] = subprocess.Popen("rosrun libscout libscout " 
48
                    + name + " " + str(GetBehaviors().index(selectedBehavior) + 1) 
49
                    + "", shell=True)
50
            print "Scout " + name + " was resumed."
51
            behaviorButton.SetLabel("Pause")
52
            behaviorLabel.SetLabel("  |  Current Behavior: " + scouts[name])
53
            return True
54
        else:
55
            if name in processes:
56
                subprocess.Popen("rosnode kill /" + name + "_behavior", shell=True)
57
                del processes[name]
58
            processes[name] = subprocess.Popen("rosrun libscout libscout " + name + " 0", shell=True) 
59
            print "Scout "+name+" was paused."
60
            behaviorButton.SetLabel("Resume")
61
            behaviorLabel.SetLabel("  |  Current Behavior: Paused")
62
            return True
63
    if name in processes:
64
        subprocess.Popen("rosnode kill /" + name + "_behavior", shell=True)
65
        # TODO Running a new behavior while paused does not do anything and
66
        # Gives lots of errors.
67
    #
68
    # -- Matt 11/30 --
69
    # Might be fixed, can't run it to tell.
70
    processes[name] = subprocess.Popen("rosrun libscout libscout " + name + " "
71
            + str(GetBehaviors().index(behavior) + 1) + "",
72
        shell=True) 
73
    behaviorButton.SetValue(False)
74
    behaviorButton.SetLabel("Pause")
75
    print "Scout " + name + " was set to behavior: " + behavior
76
    behaviorLabel.SetLabel("  |  Current Behavior: " + behavior)
77
    scouts[name] = behavior
78

  
79
def KillScout(name):
80
    try:
81
        rospy.wait_for_service('/kill')
82
        service = rospy.ServiceProxy('/kill', Kill)
83
        if name in processes:
84
            subprocess.Popen("rosnode kill /" + name + "_behavior", shell=True)
85
            del processes[name]
86
        resp = service(name)
87
        if name in scouts:
88
            del scouts[name]
89
        return True
90
    except rospy.ServiceException, e:
91
        return False
92

  
93
def PauseScout(name, behaviorLabel, behaviorButton, selectedBehavior):
94
    return SetBehavior(name, behaviorLabel, behaviorButton,
95
        "Pause", selectedBehavior)
96
#
97
# End ROS functions
98
#
99

  
100
class GUI(wx.Frame):
101
    def __init__(self, parent, title):
102
        super(GUI, self).__init__(parent, title=title,
103
            style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
104
        self.InitUI()
105
        self.Show()     
106
        self.AddScoutBox("scout1")     
107

  
108
    def InitUI(self):
109
        self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
110
        self.mainArea = wx.GridSizer(cols=1)
111
        sizer = wx.FlexGridSizer(rows=4, cols=8, hgap=5, vgap=5)
112

  
113
        # Labels
114
        blankText = wx.StaticText(self.window, label="")
115
        newScout = wx.StaticText(self.window, label="New Scout")
116
        newScoutName = wx.StaticText(self.window, label="Name:")
117
        startXTitle = wx.StaticText(self.window, label="X:")
118
        startYTitle = wx.StaticText(self.window, label="Y:")
119
        startThetaTitle = wx.StaticText(self.window, label="Rad:")
120

  
121
        # Inputs
122
        newScoutInput = wx.TextCtrl(self.window)
123
        startX = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
124
        startY = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
125
        startTheta = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
126
        addButton = wx.Button(self.window, id=wx.ID_ADD)
127

  
128
        # Pretty Stuff
129
        hLine = wx.StaticLine(self.window, size=(600, 5))
130
        bottomHLine = wx.StaticLine(self.window, size=(600, 5))
131

  
132
        # Row 0
133
        sizer.Add(newScout)
134
        for i in range(7):
135
            sizer.AddStretchSpacer(1)
136
        # Row 1
137
        sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle,
138
            startX, startYTitle, startY, startThetaTitle, startTheta])
139
        # Row 2
140
        for i in range(7):
141
            sizer.AddStretchSpacer(1)
142
        sizer.Add(addButton)
143
        # Row 3
144

  
145
        # Events
146
        addButton.Bind(wx.EVT_BUTTON, lambda event: self.NewScout(event,
147
            newScoutInput, startX, startY, startTheta))
148

  
149
        sizer.AddGrowableCol(idx=1)
150
        self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
151
        self.window.SetSizer(self.mainArea)
152
        self.sizer = defaultdict()
153

  
154
    def NewScout(self, e, name, x, y, theta):
155
        if (name):
156
            scoutCreated = self.AddScout(name.GetValue(), x.GetValue(),
157
                y.GetValue(), theta.GetValue())
158
            if (scoutCreated):
159
                wx.MessageBox("Scout '" + name.GetValue()
160
                    + "' created successfully at (" + str(x.GetValue()) + ", "
161
                    + str(y.GetValue()) + ") facing " + str(theta.GetValue())
162
                    + u"\u00B0.", 'Scout Created', wx.OK | wx.ICON_INFORMATION)
163
                name.SetValue("")
164
                x.SetValue(0)
165
                y.SetValue(0)
166
                theta.SetValue(0)
167
                return True
168
        wx.MessageBox('Error creating scout.  Please check that all information'
169
            'was entered properly and there are no duplicate scout names.',
170
            'Info', wx.OK | wx.ICON_ERROR)
171
    
172
    def AddScoutBox(self, name):
173
        self.sizer[name] = wx.FlexGridSizer(rows=2, cols=4, hgap=5, vgap=5)
174
        # Labels
175
        scoutName = wx.StaticText(self.window, label="Scout: " + name)
176
        behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
177
        currBehaviorLabel = wx.StaticText(self.window,
178
            label="  |  Current Behavior: Slacking off")
179

  
180
        # Inputs
181
        scoutChoices = wx.Choice(self.window, choices=GetBehaviors())
182
        pauseButton = wx.ToggleButton(self.window, label="Pause")
183
        runButton = wx.Button(self.window, label="Run")
184
        killButton = wx.Button(self.window, label="Kill")
185

  
186
        self.sizer[name].Add(scoutName)
187
        self.sizer[name].Add(currBehaviorLabel, wx.EXPAND | wx.ALIGN_RIGHT)
188
        self.sizer[name].AddStretchSpacer(1)
189
        self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
190

  
191
        self.sizer[name].Add(behaviorLabel)
192
        self.sizer[name].Add(scoutChoices, wx.EXPAND)
193
        self.sizer[name].Add(runButton)
194
        self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
195
        
196
        # Events
197
        pauseButton.Bind(wx.EVT_TOGGLEBUTTON,
198
            lambda event: PauseScout(name, currBehaviorLabel, pauseButton,
199
                scoutChoices.GetStringSelection()))
200
        runButton.Bind(wx.EVT_BUTTON,
201
            lambda event: SetBehavior(name, currBehaviorLabel, pauseButton,
202
                scoutChoices.GetStringSelection()))
203
        killButton.Bind(wx.EVT_BUTTON, lambda event: self.RemoveScout(name))
204

  
205
        self.mainArea.Add(self.sizer[name], proportion=1,
206
            flag=wx.ALL | wx.EXPAND, border=10)
207
        self.window.Layout()
208
        return True
209

  
210
    def AddScout(self, name, x, y, theta):
211
        if AddToSimulator(name, x, y, theta):
212
            return self.AddScoutBox(name)
213
        wx.MessageBox("ROS call failed!", 'ROS Error', wx.OK | wx.ICON_ERROR)
214
        return False
215

  
216
    def RemoveScout(self, name):
217
        print("Hidden scoutLayout for " + name + ": "
218
            + str(self.mainArea.Hide(self.sizer[name], True)))
219
        print("Removed scoutLayout for " + name + ": "
220
            + str(self.mainArea.Remove(self.sizer[name])) + " (Maybe...)")
221
        del self.sizer[name]
222
        if KillScout(name):
223
            self.window.Layout()
224
            self.window.Refresh()
225
            self.window.Update()
226
        return
227

  
228
if __name__ == '__main__':
229
    ScoutSimGui = subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)
230
    rospy.wait_for_service('/spawn')
231
    app = wx.App()
232
    GUI(None, title='Colony Scout Manager')
233
    app.MainLoop()
234
    #Figure out how to terminate this for reals.
235
    for process in processes:
236
        process.terminate()
237
    subprocess.Popen("rosnode kill /scoutsim", shell=True)
scout/scoutsim/CMakeLists.txt
31 31
rosbuild_add_executable(scoutsim_node src/scoutsim.cpp src/scout.cpp src/ghost_scout.cpp src/sim_frame.cpp)
32 32
rosbuild_link_boost(scoutsim_node thread)
33 33
target_link_libraries(scoutsim_node ${wxWidgets_LIBRARIES})
34

  
35
# @TODO are these useful or necessary? Determine; add as needed
36
#rosbuild_add_executable(turtle_teleop_key tutorials/teleop_turtle_key.cpp)
37
#rosbuild_add_executable(draw_square tutorials/draw_square.cpp)
38
#rosbuild_add_executable(mimic tutorials/mimic.cpp)
39
rosbuild_add_executable(sonar_viz src/sonar_viz.cpp)
scout/scoutsim/src/sonar_viz.cpp
1
#include <iostream>
2
#include <iomanip>
3

  
4
#include <ros/ros.h>
5
#include <sonar/sonar_distance.h>
6

  
7
#define QUEUE_SIZE 10
8

  
9
using namespace std;
10

  
11
vector<unsigned int> distances;
12

  
13
void sonar_distance_callback(const sonar::sonar_distance::ConstPtr& msg)
14
{
15
    distances[msg->pos] = msg->distance0;
16
    distances[msg->pos + 24] = msg->distance1;
17

  
18
    cout << endl << endl << endl << "[ ";
19
    for (int i = 0; i < 48; i++)
20
    {
21
        cout << setw(3) << distances[i] << " ";
22
    }
23
    cout << "]" << endl;
24
}
25

  
26
int main(int argc, char **argv)
27
{
28
    if (argc < 2)
29
    {
30
        cout << "Usage: " << argv[0] << " <scout_name>" << endl;
31
        exit(1);
32
    }
33

  
34
    ros::init(argc, argv, "sonar_viz");
35

  
36
    distances = vector<unsigned int>(48, 0);
37
    ros::NodeHandle n;
38

  
39
    string scoutname = argv[1];
40
    ros::Subscriber sonar_distance_sub = n.subscribe(scoutname + "/sonar_distance",
41
                                                     QUEUE_SIZE,
42
                                                     sonar_distance_callback);
43

  
44
    ros::spin();
45
}
scout/scoutsim/src/turtle_teleop.cpp
1
#include <ros/ros.h>
2
#include <signal.h>
3
#include <termios.h>
4
#include <stdio.h>
5
#include "constants.h"
6

  
7
#define KEYCODE_R 0x43 
8
#define KEYCODE_L 0x44
9
#define KEYCODE_U 0x41
10
#define KEYCODE_D 0x42
11
#define KEYCODE_Q 0x71
12

  
13
class TeleopTurtle
14
{
15
public:
16
  TeleopTurtle();
17
  void keyLoop();
18

  
19
private:
20

  
21
  
22
  ros::NodeHandle nh_;
23
  double linear_, angular_, l_scale_, a_scale_;
24
  ros::Publisher vel_pub_;
25
  
26
};
27

  
28
TeleopTurtle::TeleopTurtle():
29
  linear_(0),
30
  angular_(0),
31
  l_scale_(2.0),
32
  a_scale_(2.0)
33
{
34
  nh_.param("scale_angular", a_scale_, a_scale_);
35
  nh_.param("scale_linear", l_scale_, l_scale_);
36

  
37
  vel_pub_ = nh_.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1);
38
}
39

  
40
int kfd = 0;
41
struct termios cooked, raw;
42

  
43
void quit(int sig)
44
{
45
  tcsetattr(kfd, TCSANOW, &cooked);
46
  ros::shutdown();
47
  exit(0);
48
}
49

  
50

  
51
int main(int argc, char** argv)
52
{
53
  ros::init(argc, argv, "teleop_turtle");
54
  TeleopTurtle teleop_turtle;
55

  
56
  signal(SIGINT,quit);
57

  
58
  teleop_turtle.keyLoop();
59
  
60
  return(0);
61
}
62

  
63

  
64
void TeleopTurtle::keyLoop()
65
{
66
  char c;
67
  bool dirty=false;
68

  
69

  
70
  // get the console in raw mode                                                              
71
  tcgetattr(kfd, &cooked);
72
  memcpy(&raw, &cooked, sizeof(struct termios));
73
  raw.c_lflag &=~ (ICANON | ECHO);
74
  // Setting a new line, then end of file                         
75
  raw.c_cc[VEOL] = 1;
76
  raw.c_cc[VEOF] = 2;
77
  tcsetattr(kfd, TCSANOW, &raw);
78

  
79
  puts("Reading from keyboard");
80
  puts("---------------------------");
81
  puts("Use arrow keys to move the turtle.");
82

  
83

  
84
  for(;;)
85
  {
86
    // get the next event from the keyboard  
87
    if(read(kfd, &c, 1) < 0)
88
    {
89
      perror("read():");
90
      exit(-1);
91
    }
92

  
93
    linear_=angular_=0;
94
    ROS_DEBUG("value: 0x%02X\n", c);
95
  
96
    switch(c)
97
    {
98
      case KEYCODE_L:
99
        ROS_DEBUG("LEFT");
100
        angular_ = 1.0;
101
        dirty = true;
102
        break;
103
      case KEYCODE_R:
104
        ROS_DEBUG("RIGHT");
105
        angular_ = -1.0;
106
        dirty = true;
107
        break;
108
      case KEYCODE_U:
109
        ROS_DEBUG("UP");
110
        linear_ = 1.0;
111
        dirty = true;
112
        break;
113
      case KEYCODE_D:
114
        ROS_DEBUG("DOWN");
115
        linear_ = -1.0;
116
        dirty = true;
117
        break;
118
    }
119
   
120

  
121
    turtlesim::Velocity vel;
122
    vel.angular = a_scale_*angular_;
123
    vel.linear = l_scale_*linear_;
124
    if(dirty ==true)
125
    {
126
      vel_pub_.publish(vel);    
127
      dirty=false;
128
    }
129
  }
130

  
131

  
132
  return;
133
}
134

  
135

  
136

  

Also available in: Unified diff