Revision 34a60a3b

View differences:

scout/libscout/CMakeLists.txt
36 36
FILE(GLOB BEHAVIOR_FILES "${PROJECT_SOURCE_DIR}/src/behaviors/*.cpp")
37 37
FILE(GLOB TEST_BEHAVIOR_FILES "${PROJECT_SOURCE_DIR}/src/test_behaviors/*.cpp")
38 38
FILE(GLOB HELPER_FILES "${PROJECT_SOURCE_DIR}/src/helper_classes/*.cpp")
39
FILE(GLOB CONTROL_CLASSES "${PROJECT_SOURCE_DIR}/src/*Control.cpp")
40
set(WIRELESS_CONTROL_CLASSES src/WirelessSender.cpp src/WirelessReceiver.cpp)
39 41

  
40
set(CONTROL_CLASSES src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/WirelessSender.cpp src/WirelessReceiver.cpp src/EncodersControl.cpp src/LinesensorControl.cpp src/MetalDetectorControl.cpp)
41

  
42
rosbuild_add_executable(libscout ${MAIN_FILES} ${BEHAVIOR_FILES} ${TEST_BEHAVIOR_FILES} ${CONTROL_CLASSES} ${HELPER_FILES})
42
rosbuild_add_executable(libscout ${MAIN_FILES} ${CONTROL_CLASSES} ${WIRELESS_CONTROL_CLASSES} ${HELPER_FILES} ${BEHAVIOR_FILES} ${TEST_BEHAVIOR_FILES})
43 43
rosbuild_add_compile_flags(libscout -std=c++0x)
scout/libscout/src/CliffsensorControl.cpp
1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

  
26
/**
27
 * @file CliffsensorControl.cpp
28
 * @brief Contains code to control the cliffsensor.
29
 *
30
 * Implementation of functions for cliffsensor use.
31
 *
32
 * @author Colony Project, CMU Robotics Club
33
 * @author Priyanka Deo
34
 * @author Leon Zhang
35
 **/
36

  
37
#include "ros/ros.h"
38
#include "CliffsensorControl.h"
39
#include <cstdlib>
40

  
41
/**
42
 * @brief Initializes the Cliffsensor module in libscout.
43
 *
44
 * This is the main function for the cliffsensors node. It is run when the node
45
 * starts and initializes the cliffsensors. It then subscribes to the
46
 * cliff_status_changed topics
47
 **/
48
CliffsensorControl::CliffsensorControl(const ros::NodeHandle& libscout_node,
49
                                  std::string scoutname) : node(libscout_node)
50
{
51
    /* Subscribe to the cliff_status_changed topic */
52
    cliff_status_changed_sub = node.subscribe("cliff_status_changed", 
53
                             QUEUE_SIZE, &CliffsensorControl::changed_cliff_status, this);
54

  
55
    ros::spin();
56
}
57

  
58
/**
59
 * @brief Changes cliff sensor status
60
 *
61
 * Changes cliff sensor status based on subscription to topic cliff_status_changed
62
 *
63
 * @param msg The message from the cliff_status_changed topic, containing 
64
 * status of all cliff sensors.
65
 *
66
 */
67
void CliffsensorControl::changed_cliff_status(const ::messages::cliff_status_changed::ConstPtr& msg)
68
{
69
    front_raw = msg->front_raw;
70
    left_raw = msg->left_raw;
71
    right_raw = msg->right_raw;
72
    return;
73
}
74

  
75
/**
76
 * @brief get the current raw value of the front cliffsensor
77
 * @return the current raw value of the front cliffsensor
78
 */
79
int CliffsensorControl::get_front_raw()
80
{
81
    return front_raw;
82
}
83

  
84
/**
85
 * @brief get the current raw value of the left cliffsensor
86
 * @return the current raw value of the left cliffsensor
87
 */
88
int CliffsensorControl::get_left_raw()
89
{
90
    return left_raw;
91
}
92

  
93
/**
94
 * @brief get the current raw value of the right cliffsensor
95
 * @return the current raw value of the right cliffsensor
96
 */
97
int CliffsensorControl::get_right_raw()
98
{
99
    return right_raw;
100
}
101

  
102
/**
103
 * @brief check if a cliff is being detected
104
 * @return true if there is a cliff, otherwise
105
 */
106
bool CliffsensorControl::check_is_cliff()
107
{
108
    return (front_raw || left_raw || right_raw);
109
}
scout/libscout/src/CliffsensorControl.h
1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

  
26
/**
27
 * @file CliffsensorControl.h
28
 * @brief Contains cliffsensor declarations and functions.
29
 * 
30
 * Contains functions and definitions for the use of
31
 * cliffsensors.
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 * @author Priyanka Deo
35
 * @author Leon Zhang
36
 **/
37

  
38
#ifndef _CLIFFSENSOR_H_
39
#define _CLIFFSENSOR_H_
40

  
41
#include <messages/query_cliff.h>
42
#include <messages/cliff_status_changed.h>
43
#include "constants.h"
44

  
45
#define CLIFF_DETECTED 0x0
46
#define NO_CLIFF 0x1
47

  
48
class CliffsensorControl
49
{
50
    public:
51
        CliffsensorControl(const ros::NodeHandle& libscout_node, std::string scoutname);
52

  
53
        /** @brief Responds to topic to change cliff sensor status. **/
54
        void changed_cliff_status(
55
                        const messages::cliff_status_changed::ConstPtr& msg);
56

  
57
        int get_front_raw();
58
        int get_left_raw();
59
        int get_right_raw();
60
        bool check_is_cliff();
61

  
62
    private:
63
        /* Cliffsensor state variables
64
         */
65
        int front_raw; /**< The current raw value 
66
                        data of the front cliffsensor. */
67
        int left_raw; /**< The current raw value data of 
68
                        the left cliffsensor. */
69
        int right_raw; /**< The current raw value data of 
70
                        the right cliffsensor. */
71

  
72
        ros::Subscriber cliff_status_changed_sub;
73
        ros::NodeHandle node; 
74
};
75

  
76
#endif
scout/libscout/src/PaintBoardControl.cpp
1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

  
26
/**
27
 * @file PaintBoardControl.cpp
28
 * @brief Contains paintboard declarations and functions
29
 * 
30
 * Contains functions and definitions for the use of
31
 * paintboards
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 * @author Priyanka Deo
35
 **/
36

  
37
#include "PaintBoardControl.h"
38

  
39
using namespace std;
40

  
41
/**
42
 * @brief Initialize the paintboards module of libscout.
43
 *
44
 * Initialize the libscout node as a publisher of set_paintboards and a client of
45
 * query_paintboards.
46
 */
47
PaintBoardControl::PaintBoardControl(const ros::NodeHandle& libscout_node,
48
                           string scoutname)
49
    : node(libscout_node)
50
{
51
    set_paintboard_pub =
52
        node.advertise< ::messages::set_paintboard>(scoutname + "/set_paintboard",
53
                                           QUEUE_SIZE, true);
54
    query_paintboard_client =
55
        node.serviceClient< ::messages::query_paintboard>(scoutname + "/query_paintboard");
56
}
57

  
58
PaintBoardControl::~PaintBoardControl()
59
{
60
  set(STOP_PAINT);
61
}
62

  
63
/**
64
 * @brief Sets all the speeds according to the specificiation of which paintboard
65
 *        to set.
66
 *
67
 * @param which A bitmask of which paintboard need to be set.
68
 * @param speed The value to set the paintboard to.
69
 * @param units The units the speed is expressed in.
70
 * @return Function status
71
 */
72
void PaintBoardControl::set(int paintboard_setting)
73
{
74
    ::messages::set_paintboard msg;
75
    
76
    msg.setting = paintboard_setting;
77

  
78
    set_paintboard_pub.publish(msg);
79
    ros::spinOnce();
80
}
81

  
82
/**
83
 * @brief Query the current speeds of the paintboard
84
 *
85
 * Sends a request to the query_paintboard service which will reply with the
86
 *  current setting of the paintboard.
87
 *
88
 * @param which A bitmask that will specify which motor speed should be
89
 *  returned
90
 * @return The speed of the selected motor
91
 */
92
int PaintBoardControl::query()
93
{
94
    ::messages::query_paintboard srv;
95
    if(query_paintboard_client.call(srv))
96
    {
97
      return srv.response.setting;
98
    }
99
    else
100
    {
101
        ROS_FATAL("Failed to call service query_paintboard");
102
    }
103

  
104
    return -1;
105
}
scout/libscout/src/PaintBoardControl.h
1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

  
26
/**
27
 * @file PaintBoardControl.h
28
 * @brief Contains motor declarations and functions
29
 * 
30
 * Contains functions and definitions for the use of
31
 * motors
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 * @author Priyanka Deo
35
 **/
36

  
37
#ifndef _PAINT_BOARD_CONTROL_H_
38
#define _PAINT_BOARD_CONTROL_H_
39

  
40
#include <ros/ros.h>
41
#include "constants.h"
42
#include <messages/set_paintboard.h>
43
#include <messages/query_paintboard.h>
44

  
45
#define STOP_PAINT 0
46
#define DISPENSE_PAINT 1
47

  
48
class PaintBoardControl
49
{
50
    public:
51
        /** Set up the paintboard node and prepare to communicate over ROS */
52
        PaintBoardControl(const ros::NodeHandle& libscout_node,
53
                     std::string scoutname);
54

  
55
        ~PaintBoardControl();
56

  
57
        /** Turns the paintboard on or off. */
58
        void set(int paintboard_setting);
59

  
60
        /** Checks whether the paintboard is currently dispensing paint */
61
        int query();
62
    
63
    private:
64
        /** ROS publisher and client declaration */
65
        ros::Publisher set_paintboard_pub;
66
        ros::ServiceClient query_paintboard_client;
67

  
68
        ros::NodeHandle node;
69
};
70

  
71
#endif
72

  
scout/libscout/src/Sensors.h
49 49
#include "LinesensorControl.h"
50 50
#include "WirelessSender.h"
51 51
#include "WirelessReceiver.h"
52
#include "PaintBoardControl.h"
52 53

  
53 54
class Sensors
54 55
{
......
71 72
        LinesensorControl * linesensor;
72 73
        WirelessSender * wl_sender;
73 74
        WirelessReceiver * wl_receiver;
74
        
75
        PaintBoardControl * paintboard;        
75 76
};
76 77

  
77 78

  
scout/libscout/src/helper_classes/RungaKutta.cpp
1
#include "RungaKutta.h"
2
#include <math.h>
3

  
4
using namespace std;
5

  
6
vector<double> RungaKutta::integrate(vector<double> a, vector<double> b, double t)
7
{
8
  //Copy over the a vector.
9
  vector<double> result(a);
10
  //Add half a timestep to the starting conditions.
11
  for(unsigned int i=0; i<a.size(); i++)
12
  {
13
    result[i] += t*b[i];
14
  }
15
  //Return the integrated result.
16
  return result;
17
}
18

  
19
vector<double> RungaKutta::diff_drive_robot(vector<double> x)
20
{
21
  vector<double> dx(5,0);
22
  dx[0] = x[3] * cos(x[2]);
23
  dx[1] = x[3] * sin(x[2]);
24
  dx[2] = x[4];
25
  dx[3] = x[3];
26
  dx[4] = x[4];
27
  return dx;
28
}
29

  
30
vector<double> RungaKutta::rk4(vector<double> start,
31
    vector<double> (*func)(vector<double>),
32
    double loop_time)
33
{
34
  vector<double> k0 = func(start);
35
  vector<double> temp_k0 = integrate(start, k0, loop_time/2);
36
  vector<double> k1 = func(temp_k0);
37
  vector<double> temp_k1 = integrate(start, k1, loop_time/2);
38
  vector<double> k2 = func(temp_k1);
39
  vector<double> temp_k2 = integrate(start, k2, loop_time);
40
  vector<double> k3 = func(temp_k2);
41
  
42
  // Average out each loop's result.
43
  vector<double> result(start);
44
  for(unsigned int i=0; i<result.size(); i++)
45
  {
46
    result[i] = loop_time/6*(k0[i]+2*k1[i]+2*k2[i]+k3[i]);
47
  }
48
  return result;
49
}
scout/libscout/src/helper_classes/RungaKutta.h
1
#ifndef _RUNGA_KUTTA_H_
2
#define _RUNGA_KUTTA_H_
3

  
4
#include <vector>
5

  
6
class RungaKutta
7
{
8
  public:
9
    static std::vector<double> rk4(
10
        std::vector<double> start,
11
        std::vector<double> (*func)(std::vector<double>),
12
        double loop_time);
13
    static std::vector<double> diff_drive_robot(std::vector<double> x);
14
  private:
15
    static std::vector<double> integrate(
16
        std::vector<double> a,
17
        std::vector<double> b,
18
        double t);
19
};
20

  
21
#endif
scout/libscout/src/test_behaviors/Odometry_new.cpp
1

  
2 1
#include "Odometry_new.h"
2
#include "../helper_classes/RungaKutta.h"
3 3

  
4 4
using namespace std;
5 5

  
......
22 22
  float avg_left_ticks, avg_right_ticks;
23 23
  float ang_vl, ang_vr, lin_vl, lin_vr;
24 24
  float robot_v, robot_w;
25
  float k00, k01, k02, k10, k11, k12, k20, k21, k22, k30, k31, k32;
25

  
26 26
  // figure out the encoder ticks during the time
27 27
  encoder_readings scout_enc = encoders->query();
28 28
  motor_fl_ticks_iter = scout_enc.fl_ticks - motor_fl_ticks_last;
......
30 30
  motor_bl_ticks_iter = scout_enc.bl_ticks - motor_bl_ticks_last;
31 31
  motor_br_ticks_iter = scout_enc.br_ticks - motor_br_ticks_last;
32 32
  
33
  // update the 
33
  ROS_INFO("%d  %d", motor_fl_ticks_iter, motor_fr_ticks_iter);
34

  
35
  // update the tick values
34 36
  motor_fl_ticks_last = scout_enc.fl_ticks;
35 37
  motor_fr_ticks_last = scout_enc.fr_ticks;
36 38
  motor_bl_ticks_last = scout_enc.bl_ticks;
......
58 60
  robot_w = (lin_vr - lin_vl)/WHEEL_B;
59 61

  
60 62
  // now use RK4 to integrade the velocities to get the the move in pos
61
  k00 = robot_v * cos(scout_pos->theta);
62
  k01 = robot_v * sin(scout_pos->theta);
63
  k02 = robot_w;
64
  
65
  k10 = robot_v * cos(scout_pos->theta + loop_time/2*k02);
66
  k11 = robot_v * sin(scout_pos->theta + loop_time/2*k02);
67
  k12 = robot_w;
68

  
69
  k20 = robot_v * cos(scout_pos->theta + loop_time/2*k12);
70
  k21 = robot_v * sin(scout_pos->theta + loop_time/2*k12);
71
  k22 = robot_w;
72

  
73
  k30 = robot_v * cos(scout_pos->theta + loop_time * k22);
74
  k31 = robot_v * sin(scout_pos->theta + loop_time * k22);
75
  k32 = robot_w;
76

  
77
  scout_pos->x += loop_time/6 * (k00+2*(k10+k20)+k30);
78
  scout_pos->y += loop_time/6 * (k01+2*(k11+k21)+k31);
79
  scout_pos->theta += loop_time/6 * (k02+2*(k12+k22)+k32);
63
  double state_temp[] = {scout_pos->x, scout_pos->y, scout_pos->theta,
64
      robot_v, robot_w};
65
  vector<double> state (state_temp, state_temp+5);
80 66

  
67
  vector<double> new_state = RungaKutta::rk4(state, RungaKutta::diff_drive_robot, loop_time);
81 68

  
69
  scout_pos->x = new_state[0];
70
  scout_pos->y = new_state[1];
71
  scout_pos->theta = new_state[2];
82 72
}
83 73

  
84 74
void Odometry_new::run()
......
87 77
  double last_time = Time::now().toSec();
88 78
  double loop_time, current_time;
89 79

  
80
  // Reset encoders.
81
  encoders->reset();
82

  
90 83
  //Rate r(LOOP_RATE); 
91 84
  Duration r = Duration(LOOP_TIME);
92 85

  
scout/messages/msg/set_paintboard.msg
1
# The absolute motor speeds to set
2
int8 setting
3

  
scout/messages/srv/query_paintboard.srv
1
---
2
int8 setting

Also available in: Unified diff