Revision b00761a0
Updated ButtonControl and SonarControl.
Finally, the behavior compiles. Had to fix a lot of problems involving misunderstandings with Publisher/Client/ServiceServer/ServiceClient confusion.
We can now run priya_behavior as an executable, though the process for generating behavior executables still needs some work.
| scout/libscout/src/SonarControl.cpp | ||
|---|---|---|
| 36 | 36 |
|
| 37 | 37 |
#include "SonarControl.h" |
| 38 | 38 |
|
| 39 |
#define MAX_ATTEMPTS 10 |
|
| 40 |
|
|
| 39 | 41 |
using namespace std; |
| 40 | 42 |
|
| 41 | 43 |
/** |
| ... | ... | |
| 48 | 50 |
string scoutname) |
| 49 | 51 |
: node(libscout_node) |
| 50 | 52 |
{
|
| 51 |
sonar_set_scan_pub =
|
|
| 52 |
node.advertise<sonar::sonar_set_scan>(scoutname + "/set_sonar_scan",
|
|
| 53 |
QUEUE_SIZE);
|
|
| 54 |
sonar_toggle_pub =
|
|
| 55 |
node.advertise<sonar::sonar_toggle>(scoutname + "/toggle_sonar",
|
|
| 56 |
QUEUE_SIZE);
|
|
| 57 |
sonar_distance_sub = node.subscribe(scoutname + "sonar_distance",
|
|
| 58 |
QUEUE_SIZE, distance_callback);
|
|
| 53 |
sonar_set_scan_client =
|
|
| 54 |
node.serviceClient<sonar::sonar_set_scan>(scoutname+"/set_sonar_scan");
|
|
| 55 |
sonar_toggle_client =
|
|
| 56 |
node.serviceClient<sonar::sonar_toggle>(scoutname + "/toggle_sonar");
|
|
| 57 |
sonar_distance_sub = node.subscribe(scoutname + "/sonar_distance",
|
|
| 58 |
QUEUE_SIZE,
|
|
| 59 |
&SonarControl::distance_callback,
|
|
| 60 |
this);
|
|
| 59 | 61 |
|
| 60 | 62 |
// Initialize all the sonar readings to 0 and timestamps to undefined |
| 61 | 63 |
for (int i = 0; i < 48; i++) |
| ... | ... | |
| 63 | 65 |
readings[i] = 0; |
| 64 | 66 |
timestamps[i] = ros::Time::now(); // A common shortcut for no reading |
| 65 | 67 |
} |
| 68 |
|
|
| 69 |
/// @todo Test, and replace 10 with a const int |
|
| 70 |
num_attempts = MAX_ATTEMPTS; |
|
| 66 | 71 |
} |
| 67 | 72 |
|
| 68 | 73 |
/** |
| ... | ... | |
| 88 | 93 |
* @brief Sets the sonar to a position. |
| 89 | 94 |
* |
| 90 | 95 |
* @param position Value between 0-180 of degree position to set sonar |
| 91 |
* @return NONE |
|
| 92 |
* @todo should return bool. See below. |
|
| 93 | 96 |
*/ |
| 94 | 97 |
void SonarControl::set_single(int position) |
| 95 | 98 |
{
|
| ... | ... | |
| 101 | 104 |
* |
| 102 | 105 |
* @param start_pos The leftmost (smallest) value that the sonar can take |
| 103 | 106 |
* @param end_pos The rightmost (largest) value that the sonar can take |
| 104 |
* @return NONE |
|
| 105 |
* @todo Also misimplemented, should return bool. Look at srv def in sonar/ |
|
| 106 | 107 |
*/ |
| 107 | 108 |
void SonarControl::set_range(int start_pos, int end_pos) |
| 108 | 109 |
{
|
| ... | ... | |
| 121 | 122 |
end_pos = temp; |
| 122 | 123 |
} |
| 123 | 124 |
|
| 124 |
// Turn sonar on |
|
| 125 |
sonar::sonar_toggle on_msg; |
|
| 126 |
on_msg.set_on = true; |
|
| 127 |
sonar_toggle_pub.publish(on_msg); |
|
| 125 |
// Make sure sonar is on |
|
| 126 |
set_on(); |
|
| 128 | 127 |
|
| 129 | 128 |
// Set scan range |
| 130 |
sonar::sonar_set_scan msg; |
|
| 131 |
msg.stop_l = start_pos; |
|
| 132 |
msg.stop_r = end_pos; |
|
| 129 |
sonar::sonar_set_scan srv; |
|
| 130 |
srv.request.stop_l = start_pos; |
|
| 131 |
srv.request.stop_r = end_pos; |
|
| 132 |
|
|
| 133 |
// Check if the service call failed or if the response was false |
|
| 134 |
if (!sonar_set_scan_client.call(srv) || srv.response.ack == false) |
|
| 135 |
{
|
|
| 136 |
if (--num_attempts == 0) |
|
| 137 |
{
|
|
| 138 |
ROS_ERROR("SonarControl::set_range() failed permanently.");
|
|
| 139 |
} |
|
| 133 | 140 |
|
| 134 |
sonar_set_scan_pub.publish(msg); |
|
| 135 |
ros::spinOnce(); ///<! Sure? Does this belong here? |
|
| 141 |
ROS_WARN("SonarControl::set_range() failed once.");
|
|
| 142 |
} |
|
| 143 |
} |
|
| 144 |
|
|
| 145 |
/** |
|
| 146 |
* @brief Turn on sonar readings |
|
| 147 |
* |
|
| 148 |
* (Re)starts sonar panning and taking readings. |
|
| 149 |
*/ |
|
| 150 |
void SonarControl::set_on() |
|
| 151 |
{
|
|
| 152 |
set_power(true); |
|
| 136 | 153 |
} |
| 137 | 154 |
|
| 138 | 155 |
/** |
| 139 | 156 |
* @brief Turn off sonar readings |
| 140 | 157 |
* |
| 141 | 158 |
* Stops sonar from panning and taking readings. |
| 142 |
* @todo Misimplemented. Needs to use a service call and return the ack as bool |
|
| 159 |
*/ |
|
| 160 |
void SonarControl::set_off() |
|
| 161 |
{
|
|
| 162 |
set_power(false); |
|
| 163 |
} |
|
| 164 |
|
|
| 165 |
/** |
|
| 166 |
* @brief Turn sonar readings either off or on. |
|
| 167 |
* |
|
| 168 |
* Attempts to turn the readings off or on, num_attempts times. |
|
| 143 | 169 |
* |
| 144 |
* @return NONE
|
|
| 170 |
* @param is_on True if power should be set on, false if should be off.
|
|
| 145 | 171 |
*/ |
| 146 |
float SonarControl::set_off()
|
|
| 172 |
void SonarControl::set_power(bool is_on)
|
|
| 147 | 173 |
{
|
| 148 |
sonar::sonar_toggle on_msg; |
|
| 149 |
on_msg.set_on = false; |
|
| 150 |
sonar_toggle_pub.publish(on_msg); |
|
| 174 |
sonar::sonar_toggle srv; |
|
| 175 |
srv.request.set_on = is_on; |
|
| 176 |
|
|
| 177 |
// Check if the service call failed or if the response was false |
|
| 178 |
if (!sonar_toggle_client.call(srv) || srv.response.ack == false) |
|
| 179 |
{
|
|
| 180 |
if (--num_attempts == 0) |
|
| 181 |
{
|
|
| 182 |
ROS_ERROR("SonarControl::set_power() failed permanently.");
|
|
| 183 |
} |
|
| 184 |
|
|
| 185 |
ROS_WARN("SonarControl::set_power() failed once.");
|
|
| 186 |
} |
|
| 187 |
|
|
| 188 |
// Reset num_attempts |
|
| 189 |
num_attempts = MAX_ATTEMPTS; |
|
| 151 | 190 |
} |
| 152 | 191 |
|
| 153 | 192 |
/** |
Also available in: Unified diff