Colony » Colony Scout

rosbuild_add_executable(test_encoders src/test_encoders.cpp)

#!/bin/bash

sleep 1

rosservice call /spawn 9.6 2.8 3.14 scout2 &

exec "$@"

#rostopic pub /scout2_topic std_msgs/String "REG_SUCCESS 1"

#include <ros/ros.h>

#include <encoders/query_encoders.h>

int main(int argc, char **argv)

{

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

    ros::NodeHandle node;

    ros::ServiceClient query_encoders_client =

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

    ros::Rate r(10);

    while (ros::ok())

    {

        encoders::query_encoders srv;

        if (query_encoders_client.call(srv))

        {

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

                srv.response.fl_distance,

                srv.response.fr_distance,

                srv.response.bl_distance,

                srv.response.br_distance);

        }

        else

        {

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

        }

        ros::spinOnce();

        r.sleep();

    }

    return 0;

}

#!/usr/bin/python

import wx

import wx.lib.intctrl 

import wx.lib.agw.floatspin

import string

import subprocess

import signal

from collections import defaultdict

# ROS imports

import roslib; roslib.load_manifest("scoutsim")

import rospy

from scoutsim.srv import *

# Everyone global variables (PLEASE REFACTOR ME)

scouts = defaultdict(str) # Dict in the form [scout_name: current_behavior]

processes = defaultdict() # Dict in the form [scout_name: process_handler]

#

# ROS functions

#

def AddToSimulator(name, x, y, theta):

    if name == "":

        return False

    if name in scouts:

        return False

    scouts[name] = ""

    print "Calling ROS function to create scout "+name

    try:

        rospy.wait_for_service('/spawn');

        service = rospy.ServiceProxy('/spawn', Spawn);

        resp = service(x, y, theta, name)

        return True

    except rospy.ServiceException, e:

        return False

def GetBehaviors():

    return ["CW Circle", "CCW Circle", "Odometry", "Navigation Map",

        "Scheduler", "Warehouse", "Line Follow", "WL Test"]

def SetBehavior(name, behaviorLabel, behaviorButton=False, behavior="Pause", selectedBehavior="Pause"):

    if behavior == "Pause":

        if name not in processes:

            return False

        if not behaviorButton.GetValue():

            processes[name] = subprocess.Popen("rosrun libscout libscout " 

                    + name + " " + str(GetBehaviors().index(selectedBehavior) + 1) 

                    + "", shell=True)

            print "Scout " + name + " was resumed."

            behaviorButton.SetLabel("Pause")

            behaviorLabel.SetLabel("  |  Current Behavior: " + scouts[name])

            return True

        else:

            if name in processes:

                subprocess.Popen("rosnode kill /" + name + "_behavior", shell=True)

                del processes[name]

            processes[name] = subprocess.Popen("rosrun libscout libscout " + name + " 0", shell=True) 

            print "Scout "+name+" was paused."

            behaviorButton.SetLabel("Resume")

            behaviorLabel.SetLabel("  |  Current Behavior: Paused")

            return True

    if name in processes:

        subprocess.Popen("rosnode kill /" + name + "_behavior", shell=True)

        # TODO Running a new behavior while paused does not do anything and

        # Gives lots of errors.

    #

    # -- Matt 11/30 --

    # Might be fixed, can't run it to tell.

    processes[name] = subprocess.Popen("rosrun libscout libscout " + name + " "

            + str(GetBehaviors().index(behavior) + 1) + "",

        shell=True) 

    behaviorButton.SetValue(False)

    behaviorButton.SetLabel("Pause")

    print "Scout " + name + " was set to behavior: " + behavior

    behaviorLabel.SetLabel("  |  Current Behavior: " + behavior)

    scouts[name] = behavior

def KillScout(name):

    try:

        rospy.wait_for_service('/kill')

        service = rospy.ServiceProxy('/kill', Kill)

        if name in processes:

            subprocess.Popen("rosnode kill /" + name + "_behavior", shell=True)

            del processes[name]

        resp = service(name)

        if name in scouts:

            del scouts[name]

        return True

    except rospy.ServiceException, e:

        return False

def PauseScout(name, behaviorLabel, behaviorButton, selectedBehavior):

    return SetBehavior(name, behaviorLabel, behaviorButton,

        "Pause", selectedBehavior)

#

# End ROS functions

#

class GUI(wx.Frame):

    def __init__(self, parent, title):

        super(GUI, self).__init__(parent, title=title,

            style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))

        self.InitUI()

        self.Show()     

        self.AddScoutBox("scout1")     

    def InitUI(self):

        self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)

        self.mainArea = wx.GridSizer(cols=1)

        sizer = wx.FlexGridSizer(rows=4, cols=8, hgap=5, vgap=5)

        # Labels

        blankText = wx.StaticText(self.window, label="")

        newScout = wx.StaticText(self.window, label="New Scout")

        newScoutName = wx.StaticText(self.window, label="Name:")

        startXTitle = wx.StaticText(self.window, label="X:")

        startYTitle = wx.StaticText(self.window, label="Y:")

        startThetaTitle = wx.StaticText(self.window, label="Rad:")

        # Inputs

        newScoutInput = wx.TextCtrl(self.window)

        startX = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)

        startY = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)

        startTheta = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)

        addButton = wx.Button(self.window, id=wx.ID_ADD)

        # Pretty Stuff

        hLine = wx.StaticLine(self.window, size=(600, 5))

        bottomHLine = wx.StaticLine(self.window, size=(600, 5))

        # Row 0

        sizer.Add(newScout)

        for i in range(7):

            sizer.AddStretchSpacer(1)

        # Row 1

        sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle,

            startX, startYTitle, startY, startThetaTitle, startTheta])

        # Row 2

        for i in range(7):

            sizer.AddStretchSpacer(1)

        sizer.Add(addButton)

        # Row 3

        # Events

        addButton.Bind(wx.EVT_BUTTON, lambda event: self.NewScout(event,

            newScoutInput, startX, startY, startTheta))

        sizer.AddGrowableCol(idx=1)

        self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)

        self.window.SetSizer(self.mainArea)

        self.sizer = defaultdict()

    def NewScout(self, e, name, x, y, theta):

        if (name):

            scoutCreated = self.AddScout(name.GetValue(), x.GetValue(),

                y.GetValue(), theta.GetValue())

            if (scoutCreated):

                wx.MessageBox("Scout '" + name.GetValue()

                    + "' created successfully at (" + str(x.GetValue()) + ", "

                    + str(y.GetValue()) + ") facing " + str(theta.GetValue())

                    + u"\u00B0.", 'Scout Created', wx.OK | wx.ICON_INFORMATION)

                name.SetValue("")

                x.SetValue(0)

                y.SetValue(0)

                theta.SetValue(0)

                return True

        wx.MessageBox('Error creating scout.  Please check that all information'

            'was entered properly and there are no duplicate scout names.',

            'Info', wx.OK | wx.ICON_ERROR)

    def AddScoutBox(self, name):

        self.sizer[name] = wx.FlexGridSizer(rows=2, cols=4, hgap=5, vgap=5)

        # Labels

        scoutName = wx.StaticText(self.window, label="Scout: " + name)

        behaviorLabel = wx.StaticText(self.window, label="Behavior: ")

        currBehaviorLabel = wx.StaticText(self.window,

            label="  |  Current Behavior: Slacking off")

        # Inputs

        scoutChoices = wx.Choice(self.window, choices=GetBehaviors())

        pauseButton = wx.ToggleButton(self.window, label="Pause")

        runButton = wx.Button(self.window, label="Run")

        killButton = wx.Button(self.window, label="Kill")

        self.sizer[name].Add(scoutName)

        self.sizer[name].Add(currBehaviorLabel, wx.EXPAND | wx.ALIGN_RIGHT)

        self.sizer[name].AddStretchSpacer(1)

        self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)

        self.sizer[name].Add(behaviorLabel)

        self.sizer[name].Add(scoutChoices, wx.EXPAND)

        self.sizer[name].Add(runButton)

        self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)

        # Events

        pauseButton.Bind(wx.EVT_TOGGLEBUTTON,

            lambda event: PauseScout(name, currBehaviorLabel, pauseButton,

                scoutChoices.GetStringSelection()))

        runButton.Bind(wx.EVT_BUTTON,

            lambda event: SetBehavior(name, currBehaviorLabel, pauseButton,

                scoutChoices.GetStringSelection()))

        killButton.Bind(wx.EVT_BUTTON, lambda event: self.RemoveScout(name))

        self.mainArea.Add(self.sizer[name], proportion=1,

            flag=wx.ALL | wx.EXPAND, border=10)

        self.window.Layout()

        return True

    def AddScout(self, name, x, y, theta):

        if AddToSimulator(name, x, y, theta):

            return self.AddScoutBox(name)

        wx.MessageBox("ROS call failed!", 'ROS Error', wx.OK | wx.ICON_ERROR)

        return False

    def RemoveScout(self, name):

        print("Hidden scoutLayout for " + name + ": "

            + str(self.mainArea.Hide(self.sizer[name], True)))

        print("Removed scoutLayout for " + name + ": "

            + str(self.mainArea.Remove(self.sizer[name])) + " (Maybe...)")

        del self.sizer[name]

        if KillScout(name):

            self.window.Layout()

            self.window.Refresh()

            self.window.Update()

        return

if __name__ == '__main__':

    ScoutSimGui = subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)

    rospy.wait_for_service('/spawn')

    app = wx.App()

    GUI(None, title='Colony Scout Manager')

    app.MainLoop()

    #Figure out how to terminate this for reals.

    for process in processes:

        process.terminate()

    subprocess.Popen("rosnode kill /scoutsim", shell=True)

# @TODO are these useful or necessary? Determine; add as needed

#rosbuild_add_executable(turtle_teleop_key tutorials/teleop_turtle_key.cpp)

#rosbuild_add_executable(draw_square tutorials/draw_square.cpp)

#rosbuild_add_executable(mimic tutorials/mimic.cpp)

rosbuild_add_executable(sonar_viz src/sonar_viz.cpp)

#include <iostream>

#include <iomanip>

#include <ros/ros.h>

#include <sonar/sonar_distance.h>

#define QUEUE_SIZE 10

using namespace std;

vector<unsigned int> distances;

void sonar_distance_callback(const sonar::sonar_distance::ConstPtr& msg)

{

    distances[msg->pos] = msg->distance0;

    distances[msg->pos + 24] = msg->distance1;

    cout << endl << endl << endl << "[ ";

    for (int i = 0; i < 48; i++)

    {

        cout << setw(3) << distances[i] << " ";

    }

    cout << "]" << endl;

}

int main(int argc, char **argv)

{

    if (argc < 2)

    {

        cout << "Usage: " << argv[0] << " <scout_name>" << endl;

        exit(1);

    }

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

    distances = vector<unsigned int>(48, 0);

    ros::NodeHandle n;

    string scoutname = argv[1];

    ros::Subscriber sonar_distance_sub = n.subscribe(scoutname + "/sonar_distance",

                                                     QUEUE_SIZE,

                                                     sonar_distance_callback);

    ros::spin();

}

#include <ros/ros.h>

#include <signal.h>

#include <termios.h>

#include <stdio.h>

#include "constants.h"

#define KEYCODE_R 0x43 

#define KEYCODE_L 0x44

#define KEYCODE_U 0x41

#define KEYCODE_D 0x42

#define KEYCODE_Q 0x71

class TeleopTurtle

{

public:

  TeleopTurtle();

  void keyLoop();

private:

  ros::NodeHandle nh_;

  double linear_, angular_, l_scale_, a_scale_;

  ros::Publisher vel_pub_;

};

TeleopTurtle::TeleopTurtle():

  linear_(0),

  angular_(0),

  l_scale_(2.0),

  a_scale_(2.0)

{

  nh_.param("scale_angular", a_scale_, a_scale_);

  nh_.param("scale_linear", l_scale_, l_scale_);

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

}

int kfd = 0;

struct termios cooked, raw;

void quit(int sig)

{

  tcsetattr(kfd, TCSANOW, &cooked);

  ros::shutdown();

  exit(0);

}

int main(int argc, char** argv)

{

  ros::init(argc, argv, "teleop_turtle");

  TeleopTurtle teleop_turtle;

  signal(SIGINT,quit);

  teleop_turtle.keyLoop();

  return(0);

}

void TeleopTurtle::keyLoop()

{

  char c;

  bool dirty=false;

  // get the console in raw mode                                                              

  tcgetattr(kfd, &cooked);

  memcpy(&raw, &cooked, sizeof(struct termios));

  raw.c_lflag &=~ (ICANON | ECHO);

  // Setting a new line, then end of file                         

  raw.c_cc[VEOL] = 1;

  raw.c_cc[VEOF] = 2;

  tcsetattr(kfd, TCSANOW, &raw);

  puts("Reading from keyboard");

  puts("---------------------------");

  puts("Use arrow keys to move the turtle.");

  for(;;)

  {

    // get the next event from the keyboard  

    if(read(kfd, &c, 1) < 0)

    {

      perror("read():");

      exit(-1);

    }

    linear_=angular_=0;

    ROS_DEBUG("value: 0x%02X\n", c);

    switch(c)

    {

      case KEYCODE_L:

        ROS_DEBUG("LEFT");

        angular_ = 1.0;

        dirty = true;

        break;

      case KEYCODE_R:

        ROS_DEBUG("RIGHT");

        angular_ = -1.0;

        dirty = true;

        break;

      case KEYCODE_U:

        ROS_DEBUG("UP");

        linear_ = 1.0;

        dirty = true;

        break;

      case KEYCODE_D:

        ROS_DEBUG("DOWN");

        linear_ = -1.0;

        dirty = true;

        break;

    }

    turtlesim::Velocity vel;

    vel.angular = a_scale_*angular_;

    vel.linear = l_scale_*linear_;

    if(dirty ==true)

    {

      vel_pub_.publish(vel);    

      dirty=false;

    }

  }

  return;

}