Statistics
| Branch: | Revision:

scoutos / scout / headlights / src / headlights.cpp @ ac0cc6b8

History | View | Annotate | Download (3.45 KB)

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 headlights.cpp
28
 * @brief Contains code to control the headlights.
29
 *
30
 * Implementation of functions for headlights use.
31
 *
32
 * @author Colony Project, CMU Robotics Club
33
 **/
34

    
35
#include "ros/ros.h"
36
#include "headlights.h"
37
//#include "libscout/src/constants.h"
38
#include <cstdlib>
39

    
40
/**
41
 * @defgroup headlights headlights
42
 * @brief Functions for using the headlights
43
 *
44
 * @{
45
 **/
46

    
47
/* Headlight state variables
48
 * Each color intensity is stored as a value out of the max bunary value that 
49
 * can be written to the output pins that control the LEDs.
50
 */
51
/** @todo Fix types: static */
52
struct headlight
53
{
54
        char red;
55
        char green;
56
        char blue;
57
};
58
headlight left, right;
59

    
60
/**
61
 * @brief Sets headlight colors
62
 *
63
 * Sets the headlight colors based on subscription to the set_headlights topic.
64
 *
65
 * @param msg The message from the set_headlights topic, containing colors.
66
 */
67
void headlights_set(const headlights::set_headlights::ConstPtr& msg)
68
{
69
                /* Set state variables from message */
70
                if(msg->left_red > NO_SET)
71
                {
72
                        left.red = msg->left_red;
73
                }
74
                if(msg->left_green > NO_SET)
75
                {
76
                        left.green = msg->left_green;
77
                }
78
                if(msg->left_blue > NO_SET)
79
                {
80
                        left.blue = msg->left_blue;
81
                }
82
                if(msg->right_red > NO_SET)
83
                {
84
                        right.red = msg->right_red;
85
                }
86
                if(msg->right_green > NO_SET)
87
                {
88
                        right.green = msg->right_green;
89
                }
90
                if(msg->right_blue > NO_SET)
91
                {
92
                        right.blue = msg->right_blue;
93
                }
94
                
95
    /* Write colors to hardware */
96
    /** @todo Add code to write colors to hardware */
97
}
98

    
99
/**
100
 * @brief headlights driver. This is a ROS node that controls headlight colors.
101
 *
102
 * This is the main function for the headlights node. It is run when the node
103
 * starts and initializes the headlights. It then subscribes to the
104
 * set_headlights topic.
105
 * 
106
 * @param argc The number of command line arguments (should be 1)
107
 * @param argv The array of command line arguments
108
 **/
109
int main(int argc, char **argv)
110
{
111
    /* Initialize in ROS the headlights driver node */
112
    ros::init(argc, argv, "headlights_driver");
113

    
114
    /* Create the nodehandle for this node*/
115
    ros::NodeHandle n;
116

    
117
    /* Subscribe to the set_headlights topic */
118
    ros::Subscriber sub0 = n.subscribe("set_headlights", QUEUE_SIZE, headlights_set);
119

    
120
    /* Initialize hardware for headlights */
121
    // Hardware init functions here
122

    
123
    ROS_INFO("Ready to set headlights.");
124
    ros::spin();
125

    
126
    return 0;
127
}
128

    
129
/** @} **/