Project

General

Profile

Revision c492be62

IDc492be629f06e3cf71fdf832ce2cd5ac1ec779f8

Added by Alex Zirbel over 12 years ago

Updated the licensing information in many files.

This is a broken commit because I decided to do this at a bad time. Sorry! The build will be working after next commit, I promise (and I won't push till then). This commit reflects the licensing in the files after I used my auto-add/remove script.

View differences:

scout/scoutsim/src/scout.cpp
1 
/*
2 
 * This package was developed by the CMU Robotics Club project using code
3 
 * from Willow Garage, Inc.  Please see licensing.txt for details.
1 
/**
2 
 * The code in this package was developed using the structure of Willow
3 
 * Garage's turtlesim package.  It was modified by the CMU Robotics Club
4 
 * to be used as a simulator for the Colony Scout robot.
4 5 
 *
6 
 * All redistribution of this code is limited to the terms of Willow Garage's
7 
 * licensing terms, as well as under permission from the CMU Robotics Club.
8 
 *
9 
 * Copyright (c) 2011 Colony Project
10 
 *
11 
 * Permission is hereby granted, free of charge, to any person
12 
 * obtaining a copy of this software and associated documentation
13 
 * files (the "Software"), to deal in the Software without
14 
 * restriction, including without limitation the rights to use,
15 
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
16 
 * copies of the Software, and to permit persons to whom the
17 
 * Software is furnished to do so, subject to the following
18 
 * conditions:
19 
 *
20 
 * The above copyright notice and this permission notice shall be
21 
 * included in all copies or substantial portions of the Software.
22 
 *
23 
 * Copyright (c) 2009, Willow Garage, Inc.
5 24 
 * All rights reserved.
6 
 *
7 
 * @brief Keeps track of a single scout robot.
8 
 * @file scout.cpp
9 
 * @author Colony Project, CMU Robotics Club
10 
 * @author Alex Zirbel
25 
 *
26 
 * Redistribution and use in source and binary forms, with or without
27 
 * modification, are permitted provided that the following conditions are met:
28 
 *
29 
 *    Redistributions of source code must retain the above copyright
30 
 *       notice, this list of conditions and the following disclaimer.
31 
 *    Redistributions in binary form must reproduce the above copyright
32 
 *       notice, this list of conditions and the following disclaimer in the
33 
 *       documentation and/or other materials provided with the distribution.
34 
 *    Neither the name of the Willow Garage, Inc. nor the names of its
35 
 *       contributors may be used to endorse or promote products derived from
36 
 *       this software without specific prior written permission.
37 
 *
38 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
39 
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
40 
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
41 
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
42 
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
43 
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
44 
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
45 
 * OTHER DEALINGS IN THE SOFTWARE.
11 46 
 */
12 47 

  		




  13
  48
  
    
#include "scout.h"


  		




  ......




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


  		




  44
  79
  
    
                                            &Scout::setPenCallback,


  		




  45
  80
  
    
                                            this);


  		




  46
  
  
    
        teleport_relative_srv =


  		




  47
  
  
    
            node.advertiseService("teleport_relative",


  		




  48
  
  
    
                                 &Scout::teleportRelativeCallback,


  		




  49
  
  
    
                                 this);


  		




  50
  
  
    
        teleport_absolute_srv =


  		




  51
  
  
    
            node.advertiseService("teleport_absolute",


  		




  52
  
  
    
                                 &Scout::teleportAbsoluteCallback,


  		




  53
  
  
    
                                 this);


  		




  54
  81
  
    

  		




  55
  82
  
    
        meter = scout.GetHeight();


  		




  56
  83
  
    
    }


  		




  ......




  58
  85
  
    
    /**


  		




  59
  86
  
    
     * A callback function that sets velocity based on a set_motors


  		




  60
  87
  
    
     * request.


  		




  
  88
  
    
     * @todo Use "callback" in all callback function names? Or remove?


  		




  61
  89
  
    
     */


  		




  62
  90
  
    
    void Scout::setMotors(const motors::set_motors::ConstPtr& msg)


  		




  63
  91
  
    
    {


  		




  ......




  91
  119
  
    
        ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);


  		




  92
  120
  
    
    }


  		




  93
  121
  
    

  		




  
  122
  
    
    float Scout::getSonar(float angle)


  		




  
  123
  
    
    {


  		




  
  124
  
    

  		




  
  125
  
    
        return 0.0;


  		




  
  126
  
    
    }


  		




  
  127
  
    

  		




  94
  128
  
    
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,


  		




  95
  129
  
    
                               scoutsim::SetPen::Response&)


  		




  96
  130
  
    
    {


  		




  ......




  110
  144
  
    
        return true;


  		




  111
  145
  
    
    }


  		




  112
  146
  
    

  		




  113
  
  
    
    /// @TODO remove


  		




  114
  
  
    
    bool Scout::teleportRelativeCallback(scoutsim::TeleportRelative::Request& req,


  		




  115
  
  
    
                                         scoutsim::TeleportRelative::Response&)


  		




  116
  
  
    
    {


  		




  117
  
  
    
        teleport_requests.push_back(TeleportRequest(0,


  		




  118
  
  
    
                                                     0,


  		




  119
  
  
    
                                                     req.angular,


  		




  120
  
  
    
                                                     req.linear,


  		




  121
  
  
    
                                                     true));


  		




  122
  
  
    
        return true;


  		




  123
  
  
    
    }


  		




  124
  
  
    

  		




  125
  
  
    
    /// @TODO remove


  		




  126
  
  
    
    bool Scout::teleportAbsoluteCallback(scoutsim::TeleportAbsolute::Request& req,


  		




  127
  
  
    
                                         scoutsim::TeleportAbsolute::Response&)


  		




  128
  
  
    
    {


  		




  129
  
  
    
        teleport_requests.push_back(TeleportRequest(req.x,


  		




  130
  
  
    
                                                     req.y,


  		




  131
  
  
    
                                                     req.theta,


  		




  132
  
  
    
                                                     0,


  		




  133
  
  
    
                                                     false));


  		




  134
  
  
    
        return true;


  		




  135
  
  
    
    }


  		




  136
  
  
    

  		




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


  		




  138
  148
  
    
                        const wxImage& path_image, wxColour background_color,


  		




  139
  149
  
    
                        float canvas_width, float canvas_height)


  		




  140
  150
  
    
    {


  		




  141
  
  
    
        // first process any teleportation requests, in order


  		




  142
  
  
    
        V_TeleportRequest::iterator it = teleport_requests.begin();


  		




  143
  
  
    
        V_TeleportRequest::iterator end = teleport_requests.end();


  		




  144
  
  
    
        for (; it != end; ++it)


  		




  145
  
  
    
        {


  		




  146
  
  
    
            const TeleportRequest& req = *it;


  		




  147
  
  
    

  		




  148
  
  
    
            Vector2 old_pos = pos;


  		




  149
  
  
    
            if (req.relative)


  		




  150
  
  
    
            {


  		




  151
  
  
    
                orient += req.theta;


  		




  152
  
  
    
                pos.x += sin(orient + PI/2.0) * req.linear;


  		




  153
  
  
    
                pos.y += cos(orient + PI/2.0) * req.linear;


  		




  154
  
  
    
            }


  		




  155
  
  
    
            else


  		




  156
  
  
    
            {


  		




  157
  
  
    
                pos.x = req.pos.x;


  		




  158
  
  
    
                pos.y = std::max(0.0f, canvas_height - req.pos.y);


  		




  159
  
  
    
                orient = req.theta;


  		




  160
  
  
    
            }


  		




  161
  
  
    

  		




  162
  
  
    
            path_dc.SetPen(pen);


  		




  163
  
  
    
            path_dc.DrawLine(pos.x * meter, pos.y * meter,


  		




  164
  
  
    
                             old_pos.x * meter, old_pos.y * meter);


  		




  165
  
  
    
        }


  		




  166
  
  
    

  		




  167
  
  
    
        teleport_requests.clear();


  		




  168
  
  
    

  		




  169
  
  
    
        if (ros::WallTime::now() - last_command_time > ros::WallDuration(1.0))


  		




  170
  
  
    
        {


  		




  171
  
  
    
            lin_vel = 0.0f;


  		




  172
  
  
    
            ang_vel = 0.0f;


  		




  173
  
  
    
        }


  		




  174
  
  
    

  		




  175
  151
  
    
        Vector2 old_pos = pos;


  		




  176
  152
  
    

  		




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


  		












Also available in:  Unified diff