Project

General

Profile

Revision 34a60a3b

ID34a60a3b2ffaef82b1e09ae7402d4c1283165361
Parent a38fa869
Child d1ba04dc

Added by Priya over 4 years ago

Added RungeKutta to the helper classes and changed the odometry to use that instead. Also created PaintboardControl files, changed the Makefile to compile all control classes and deleted Cliffsensor control becase it did not work.

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