Revision ce559b91
ID | ce559b91dc2723f09fa0c381cd4ebff86d4f8e10 |
Fixed the encoders node.
Simplified the node to only advertise a single service - a query allows other nodes to find out, simply, the total distance traveled. The node also only handles plain encoder ticks, because distance and such computation should be done on the library end.
scout/encoders/src/encoders.cpp | ||
---|---|---|
23 | 23 |
* OTHER DEALINGS IN THE SOFTWARE. |
24 | 24 |
*/ |
25 | 25 |
|
26 |
|
|
27 | 26 |
/** |
28 | 27 |
* @file encoders.cpp |
29 | 28 |
* @brief Encoders |
... | ... | |
31 | 30 |
* Implementation of functions for encoder use. |
32 | 31 |
* |
33 | 32 |
* @author Colony Project, CMU Robotics Club |
33 |
* @author Alex Zirbel |
|
34 | 34 |
**/ |
35 | 35 |
|
36 |
#include "ros/ros.h" |
|
36 |
#include <ros/ros.h> |
|
37 |
|
|
37 | 38 |
#include "encoders.h" |
38 |
//#include "libscout/src/constants.h" |
|
39 |
#include <cstdlib> |
|
40 | 39 |
|
41 | 40 |
/** |
42 | 41 |
* @defgroup encoders Encoders |
43 | 42 |
* @brief Functions for using the encoders |
44 | 43 |
* |
45 |
**/ |
|
44 |
* @{ |
|
45 |
*/ |
|
46 | 46 |
|
47 | 47 |
/* Encoder state variables */ |
48 |
/** \todo Fix types: static */ |
|
49 |
/** \todo Fix integer overflow possibility */ |
|
48 |
/** @todo Fix types: static */ |
|
49 |
/** @todo Fix integer overflow possibility */ |
|
50 |
|
|
50 | 51 |
/* The current position of each encoder, measured in encoder ticks */ |
51 |
int encoder_fl_index; /**< The current state of the front left encoder. */ |
|
52 |
int encoder_fr_index; /**< The current state of the front right encoder. */ |
|
53 |
int encoder_bl_index; /**< The current state of the back left encoder. */ |
|
54 |
int encoder_br_index; /**< The current state of the back right encoder. */ |
|
55 |
/* The current velocity of each encoder, measured in ticks per second */ |
|
56 |
int encoder_fl_velocity; |
|
57 |
int encoder_fr_velocity; |
|
58 |
int encoder_bl_velocity; |
|
59 |
int encoder_br_velocity; |
|
52 |
int encoder_fl_index; |
|
53 |
int encoder_fr_index; |
|
54 |
int encoder_bl_index; |
|
55 |
int encoder_br_index; |
|
56 |
|
|
60 | 57 |
/* The current distance each encoder has traveled in ticks from last reset */ |
61 | 58 |
int encoder_fl_distance; |
62 | 59 |
int encoder_fr_distance; |
63 | 60 |
int encoder_bl_distance; |
64 | 61 |
int encoder_br_distance; |
65 |
/* The units that the encoder measurements will be published in */ |
|
66 |
char encoder_units; |
|
67 | 62 |
|
63 |
/** |
|
64 |
* @brief Outputs current encoder data |
|
65 |
* |
|
66 |
* Serves the service query_encoders by responding to service requests with the |
|
67 |
* encoder values. |
|
68 |
* @param req The request. The units that the response should be in. |
|
69 |
* @param res The response. The fields will be filled with values. |
|
70 |
*/ |
|
71 |
bool handle_encoders_query(encoders::query_encoders::Request &req, |
|
72 |
encoders::query_encoders::Response &res) |
|
73 |
{ |
|
74 |
/* Put index, velocity, and distance data in message */ |
|
75 |
res.fl_distance = encoder_fl_distance; |
|
76 |
res.fr_distance = encoder_fr_distance; |
|
77 |
res.bl_distance = encoder_bl_distance; |
|
78 |
res.br_distance = encoder_br_distance; |
|
79 |
|
|
80 |
ROS_DEBUG("Encoder values queried [Unimplemented]"); |
|
81 |
return true; |
|
82 |
} |
|
68 | 83 |
|
69 |
/*!
|
|
70 |
* \brief Encoders driver. This is a ROS node that controls encoders.
|
|
84 |
/**
|
|
85 |
* @brief Encoders driver. This is a ROS node that controls encoders.
|
|
71 | 86 |
* |
72 | 87 |
* This is the main function for the encoders node. It is run when the node |
73 | 88 |
* starts and initializes the encoders. It then publishes to the |
74 | 89 |
* encoder_state topic and advertises the query_encoders service. |
75 |
* |
|
76 |
* \param argc The number of command line arguments (should be 1) |
|
77 |
* \param argv The array of command line arguments |
|
78 |
**/ |
|
79 |
int main(int argc, char **argv){ |
|
80 |
/* Initialize in ROS the encoders driver node */ |
|
81 |
ros::init(argc, argv, "encoders_driver"); |
|
82 |
|
|
83 |
/* Advertise that this serves the query_encoders service */ |
|
84 |
ros::NodeHandle n; |
|
85 |
ros::ServiceServer service = n.advertiseService("query_encoders", encoders_query); |
|
90 |
*/ |
|
91 |
int main(int argc, char **argv) |
|
92 |
{ |
|
93 |
/* Initialize in ROS the encoders driver node */ |
|
94 |
ros::init(argc, argv, "encoders_driver"); |
|
86 | 95 |
|
87 |
/* Subscribe to the encoder_state topic */ |
|
88 |
ros::Subscriber sub0 = n.subscribe("encoder_state", 4, encoder_state); |
|
96 |
/* Advertise that this serves the query_encoders service */ |
|
97 |
ros::NodeHandle n; |
|
98 |
ros::ServiceServer service = |
|
99 |
n.advertiseService("query_encoders", |
|
100 |
handle_encoders_query); |
|
89 | 101 |
|
90 |
/* Initialize hardware for motors */ |
|
91 |
// Hardware init functions here |
|
102 |
/* Initialize hardware for motors */
|
|
103 |
// Hardware init functions here
|
|
92 | 104 |
|
93 |
ROS_INFO("Ready to set encoders."); |
|
94 |
ros::spin(); |
|
105 |
ROS_INFO("Ready to set encoders."); |
|
95 | 106 |
|
96 |
return 0; |
|
97 |
} |
|
107 |
// Acutally measure encoders and update values here |
|
98 | 108 |
|
99 |
/*! |
|
100 |
* \brief Gets encoder state |
|
101 |
* |
|
102 |
* Sets the encoder state based on subscription to the encoder_state topic. |
|
103 |
* |
|
104 |
* \todo Make this function set the state properly. Probably requires |
|
105 |
* creating a new msg type for the mode set. |
|
106 |
* |
|
107 |
* \param msg The message from the encoder_state topic, containing encoder |
|
108 |
* configuration settings. |
|
109 |
*/ |
|
110 |
void encoder_set_mode(const encoders::encoder_state::ConstPtr& msg) |
|
111 |
{ |
|
112 |
|
|
113 |
} |
|
109 |
ros::spin(); |
|
114 | 110 |
|
115 |
/*! |
|
116 |
* \brief Publishes encoder state |
|
117 |
* |
|
118 |
* Publishes the current encoders position, velocty, and distance from last |
|
119 |
* reset. |
|
120 |
*/ |
|
121 |
void encoders_state_publish() |
|
122 |
{ |
|
123 |
encoders::encoders_state msg; |
|
124 |
|
|
125 |
if(!ros::ok()) |
|
126 |
{ |
|
127 |
return LIB_ERROR; |
|
128 |
} |
|
129 |
|
|
130 |
/* Put index, velocity, and distance data in message */ |
|
131 |
msg.fl_index = encoders_ticks_to_units(encoder_fl_index, encoder_units); |
|
132 |
msg.fr_index = encoders_ticks_to_units(encoder_fr_index, encoder_units); |
|
133 |
msg.bl_index = encoders_ticks_to_units(encoder_bl_index, encoder_units); |
|
134 |
msg.br_index = encoders_ticks_to_units(encoder_br_index, encoder_units); |
|
135 |
msg.fl_speed = encoders_ticks_to_units(encoder_fl_velocity, encoder_units); |
|
136 |
msg.fr_speed = encoders_ticks_to_units(encoder_fr_velocity, encoder_units); |
|
137 |
msg.bl_speed = encoders_ticks_to_units(encoder_bl_velocity, encoder_units); |
|
138 |
msg.br_speed = encoders_ticks_to_units(encoder_br_velocity, encoder_units); |
|
139 |
msg.fl_distance = encoders_ticks_to_units(encoder_fl_distance, encoder_units); |
|
140 |
msg.fr_distance = encoders_ticks_to_units(encoder_fr_distance, encoder_units); |
|
141 |
msg.bl_distance = encoders_ticks_to_units(encoder_bl_distance, encoder_units); |
|
142 |
msg.br_distance = encoders_ticks_to_units(encoder_br_distance, encoder_units); |
|
143 |
/* Specifies which units the encoder values are given in */ |
|
144 |
msg.units = encoder_units; |
|
145 |
|
|
146 |
/* Publishes message to encoders_state */ |
|
147 |
encoders_state_publisher.publish(msg); |
|
148 |
ros::spinOnce(); |
|
149 |
|
|
150 |
return LIB_OK; |
|
151 |
} |
|
152 |
|
|
153 |
/*! |
|
154 |
* \brief Outputs current encoder data |
|
155 |
* |
|
156 |
* Serves the service query_encoders by responding to service requests with the |
|
157 |
* encoder values. |
|
158 |
* \param req The request. The units that the response should be in. |
|
159 |
* \param res The response. The fields will be filled with values. |
|
160 |
*/ |
|
161 |
bool encoders_query(encoders::query_encoders::Request &req, |
|
162 |
encoders::query_encoders::Response &res) |
|
163 |
{ |
|
164 |
int units = req.units; |
|
165 |
|
|
166 |
/* Put index, velocity, and distance data in message */ |
|
167 |
res.fl_index = encoders_ticks_to_units(encoder_fl_index, units); |
|
168 |
res.fr_index = encoders_ticks_to_units(encoder_fr_index, units); |
|
169 |
res.bl_index = encoders_ticks_to_units(encoder_bl_index, units); |
|
170 |
res.br_index = encoders_ticks_to_units(encoder_br_index, units); |
|
171 |
res.fl_speed = encoders_ticks_to_units(encoder_fl_velocity, units); |
|
172 |
res.fr_speed = encoders_ticks_to_units(encoder_fr_velocity, units); |
|
173 |
res.bl_speed = encoders_ticks_to_units(encoder_bl_velocity, units); |
|
174 |
res.br_speed = encoders_ticks_to_units(encoder_br_velocity, units); |
|
175 |
res.fl_distance = encoders_ticks_to_units(encoder_fl_distance, units); |
|
176 |
res.fr_distance = encoders_ticks_to_units(encoder_fr_distance, units); |
|
177 |
res.bl_distance = encoders_ticks_to_units(encoder_bl_distance, units); |
|
178 |
res.br_distance = encoders_ticks_to_units(encoder_br_distance, units); |
|
179 |
|
|
180 |
ROS_DEBUG("Encoder values queried"); |
|
181 |
return true; |
|
111 |
return 0; |
|
182 | 112 |
} |
183 | 113 |
|
184 |
int encoders_ticks_to_units(int ticks, char units) |
|
185 |
{ |
|
186 |
switch(encoder_units) |
|
187 |
{ |
|
188 |
case ENCODER_TICKS: /* Just return ticks */ |
|
189 |
return ticks; |
|
190 |
case ENCODER_MM: /* Return mm of linear travel by the wheel */ |
|
191 |
return ticks * (2 * PI * WHEEL_RADIUS_MM / NUM_TICKS); |
|
192 |
case ENCODER_CM: /* Return cm of linear travel by the wheel */ |
|
193 |
return ticks * (2 * PI * WHEEL_RADIUS_MM / NUM_TICKS) / 10; |
|
194 |
case ENCODER_DEG: /* Return degrees of travel from last reset */ |
|
195 |
return ticks * (360 / NUM_TICKS); |
|
196 |
default: /* The units aren't recognized. Return ticks. */ |
|
197 |
return ticks; |
|
198 |
} |
|
199 |
} |
|
114 |
/** @} */ |
Also available in: Unified diff