-
Notifications
You must be signed in to change notification settings - Fork 0
/
Disparity_Alg.cpp
135 lines (102 loc) · 3.84 KB
/
Disparity_Alg.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
/*
Template code for subscribe and publish node
*/
#include <ackermann_msgs/AckermannDriveStamped.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64.h>
#include <vector>
#include <string>
#include "math.h"
enum turn_direction {right, left};
#define DISPARITY 0.1 // meters
#define CAR_WIDTH 0.4
#define PI 3.1415926535
class SubscribeAndPublish {
public:
SubscribeAndPublish() {
//Topic you want to publish
pub_ = n_.advertise<ackermann_msgs::AckermannDriveStamped>("/nav", 1000);
//Topic you want to subscribe
sub_ = n_.subscribe("/scan", 1000, &SubscribeAndPublish::callback, this);
}
void callback(const sensor_msgs::LaserScan& lidar_info) {
turn_direction turn_dir;
angle_increment = lidar_info.angle_increment;
ranges = std::vector<double>(std::begin(lidar_info.ranges), std::end(lidar_info.ranges));
double min_angle = -100 / 180.0 * PI;
size_t min_indx = (size_t)(std::floor((min_angle - lidar_info.angle_min) / lidar_info.angle_increment));
double max_angle = 100 / 180.0 * PI;
size_t max_indx = (size_t)(std::ceil((max_angle - lidar_info.angle_min) / lidar_info.angle_increment));
// Clean data
for (size_t i = min_indx; i < max_indx; i++) {
if (lidar_info.ranges[i] > lidar_info.range_max || std::isnan(lidar_info.ranges[i])) {
ranges[i] = 0.0;
} else if (std::isinf(lidar_info.ranges[i])) {
ranges[i] = lidar_info.range_max;
}
}
double distance;
double last_distance;
size_t interval;
last_distance = ranges[min_indx];
for (size_t i = min_indx+1; i < max_indx; i++) {
if (ranges[i] == 0) continue;
if (ranges[i] >= last_distance + DISPARITY) {
distance = ranges[i];
interval = i + getLidarIncrements(ranges[i]);
for (; i < interval; i++)
ranges[i] = distance;
}
else if (ranges[i] <= last_distance - DISPARITY){
distance = ranges[i];
i = i - getLidarIncrements(ranges[i]);
interval = i + getLidarIncrements(ranges[i]);
for (; i < interval; i++)
ranges[i] = distance;
}
last_distance = ranges[i];
}
double max_value = ranges[min_indx];
angle = 0;
for (unsigned int i = min_indx + 1; i < max_indx; i++) {
if (ranges[i] > max_value) {
max_value = ranges[i];
angle = lidar_info.angle_min + i * lidar_info.angle_increment;
}
}
speed = max_speed;
if (max_value < 2) speed = max_speed / 2;
SubscribeAndPublish::reactive_control();
}
void reactive_control() {
ackermann_msgs::AckermannDriveStamped ackermann_drive_result;
ackermann_drive_result.drive.steering_angle = angle;
ackermann_drive_result.drive.speed = speed;
pub_.publish(ackermann_drive_result);
}
size_t getLidarIncrements(double distance){
double theta = CAR_WIDTH / distance;
return (size_t) ceil(theta / angle_increment);
// s= r0
// width + tolerance = r0 - our width includes tolerance
}
private:
ros::NodeHandle n_;
ros::Publisher pub_;
ros::Subscriber sub_;
double angle, speed;
double angle_increment;
double max_speed = 2;
double max_steering_angle;
std::vector<double> ranges;
};
int main(int argc, char** argv) {
//Initiate ROS
ros::init(argc, argv, "random_walker");
//Create an object of class SubscribeAndPublish that will take care of everything
SubscribeAndPublish SAPObject;
ros::spin();
return 0;
}