-
Notifications
You must be signed in to change notification settings - Fork 0
/
Projector.hpp
241 lines (188 loc) · 9.83 KB
/
Projector.hpp
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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
#ifndef _PROJECTOR_HPP_
#define _PROJECTOR_HPP_
#include <cmath>
#include <limits>
#include <initializer_list>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Point32.h>
#include <Cue.hpp>
#include <Pulser.hpp>
using namespace geometry_msgs;
using namespace visualization_msgs;
namespace commbot{
class Projector : public Cue{
public:
Projector(){
// Init publisher
std::string topic;
ros_helpers::get_param("topics/projector", topic);
pub_cue_ = nh_.advertise<MarkerArray>(topic, 0);
nh_.getParam("frame_ids/projection", FRAME_PROJECTION_);
nh_.getParam("frame_ids/robot", FRAME_ROBOT_);
tf_buffer_.canTransform(FRAME_PROJECTION_, FRAME_ROBOT_, ros::Time::now(), ros::Duration(3.0)); // wait for robot->projection tf to appear
tf_robot_to_projection_ = tf_buffer_.lookupTransform(FRAME_ROBOT_, FRAME_PROJECTION_, ros::Time(0), ros::Duration(1.0));
ROS_INFO("%s", "Projector instantiated.");
}
// Configures markers_goal_
void set_markers_config_goal(const double_t &total_length,
const double_t &ratio_arrow_length_total_length,
const double_t &ratio_dashes_width_total_length,
const double_t &ratio_arrow_width_dashes_width,
const double_t &shift_x, const double_t &shift_y,
const double_t &k_freq_prox_waypoint, const double_t &max_freq,
const double_t &prox_origin_min, const double_t &prox_origin_max){
length_arrow_goal_ = total_length;
translation_goal_.x = shift_x;
translation_goal_.y = shift_y;
if(k_freq_prox_waypoint > 0) k_freq_prox_waypoint_ = k_freq_prox_waypoint;
else ROS_ERROR("%s", "Projector::set_markers_config_goal - illegal k_freq_prox_waypoint.");
if(max_freq > 0) max_freq_ = max_freq;
else ROS_ERROR("%s", "Projector::set_markers_config_goal - illegal max_freq.");
PROX_ORIGIN_MIN_ = prox_origin_min;
PROX_ORIGIN_MAX_ = prox_origin_max;
markers_goal_.markers.clear();
Marker line_list;
line_list.type = Marker::LINE_LIST;
line_list.action = Marker::MODIFY;
line_list.color.g = 1.0;
line_list.color.a = 1.0;
line_list.scale.x = ratio_dashes_width_total_length * total_length;
markers_goal_.markers.push_back(line_list);
Marker arrow;
arrow.type = Marker::ARROW;
arrow.action = Marker::MODIFY;
arrow.color.g = 1.0;
arrow.color.a = 1.0;
arrow.scale.x = line_list.scale.x;
arrow.scale.y = ratio_arrow_width_dashes_width * line_list.scale.x; // head width : dashes width
arrow.scale.z = ratio_arrow_length_total_length * total_length; // head length : total length
markers_goal_.markers.push_back(arrow);
pub_cue_.publish(ros_helpers::visualization::array_delete_all(FRAME_PROJECTION_));
}
// Configures markers_traj_
void set_markers_config_traj(const double_t &length,
const double_t &ratio_shaft_width_arrow_length,
const double_t &ratio_head_width_shaft_width,
const double_t &ratio_head_length_arrow_length,
const double_t &shift_x, const double_t &shift_y){
markers_traj_.markers.clear();
Marker arrow;
arrow.type = Marker::ARROW;
arrow.action = Marker::MODIFY;
arrow.color.g = 1.0;
arrow.color.a = 1.0;
length_arrow_traj_ = length;
translation_traj_.x = shift_y;
translation_traj_.y = -1*shift_x;
// set scale using arrow length and set ratios
arrow.scale.x = ratio_shaft_width_arrow_length * length; // shaft width
arrow.scale.y = ratio_head_width_shaft_width * arrow.scale.x; // head width
arrow.scale.z = ratio_head_length_arrow_length * length; // head length
markers_traj_.markers.push_back(arrow);
pub_cue_.publish(ros_helpers::visualization::array_delete_all(FRAME_PROJECTION_));
}
private:
tf2_ros::Buffer tf_buffer_;
std::unique_ptr<tf2_ros::TransformListener> tfl_ = std::unique_ptr<tf2_ros::TransformListener>(new tf2_ros::TransformListener(tf_buffer_, nh_));
TransformStamped tf_robot_to_projection_;
std::string FRAME_ROBOT_, FRAME_PROJECTION_;
MarkerArray markers_traj_, markers_goal_;
Pulser pulser_pub_goal_;
const int num_dashes_goal_arrow_ = 8;
// dynamically reconfigurable
double_t length_arrow_traj_, length_arrow_goal_;
double_t prox_origin_goal_; // for placing goal marker start
Vector3 translation_traj_, translation_goal_;
double_t k_freq_prox_waypoint_, max_freq_;
double_t PROX_ORIGIN_MIN_, PROX_ORIGIN_MAX_;
// Sets markers_traj_
void set_cue_traj_(const PointStamped &next_position) override{
Marker arrow = markers_traj_.markers.front(); // use existing config
arrow.header = next_position.header; // base_link
arrow.points.clear();
arrow.points.push_back(*std::unique_ptr<Point>(new Point)); // add empty point to start
// calculate end point to give desired length
Point end;
double_t angle = std::atan2(next_position.point.y, next_position.point.x);
end.x = length_arrow_traj_ * std::cos(angle);
end.y = length_arrow_traj_ * std::sin(angle);
arrow.points.push_back(end);
markers_traj_.markers.clear();
markers_traj_.markers.push_back(arrow);
ros_helpers::visualization::tidy(markers_traj_);
ros_helpers::visualization::limit_upper_half_plane(markers_traj_);
ros_helpers::visualization::transform_points(markers_traj_, tf_robot_to_projection_);
ros_helpers::visualization::translate(markers_traj_, translation_traj_);
}
// Sets markers_goal_ and activates pulser_pub_goal_.
// Pivot about origin at angle between waypoints.
void set_cue_goal_(const PointStamped &waypoint0, const PointStamped &waypoint1) override{
int num_dashes = num_dashes_goal_arrow_;
Marker line_list = markers_goal_.markers.front(); // retain marker config
Marker arrow = markers_goal_.markers.back(); // retain marker config
// calculate end point given desired length
Point start, end;
Point end_wrt_start = ros_helpers::geometry::subtract(waypoint1.point, waypoint0.point);
const double_t angle_w0_to_w1 = std::atan2(end_wrt_start.y, end_wrt_start.x);
const double_t angle_to_w0 = std::atan2(waypoint0.point.y, waypoint0.point.x);
double_t prox_origin = std::hypot(waypoint0.point.x, waypoint0.point.y);
ros_helpers::remap(prox_origin, 0.0, min_prox_waypoint_, PROX_ORIGIN_MIN_, PROX_ORIGIN_MAX_);
start.x = prox_origin * std::cos(angle_to_w0);
start.y = prox_origin * std::sin(angle_to_w0);
end.x = start.x + length_arrow_goal_ * std::cos(angle_w0_to_w1);
end.y = start.y + length_arrow_goal_ * std::sin(angle_w0_to_w1);
// dashes
line_list.points.clear();
line_list.header.frame_id = waypoint0.header.frame_id;
double_t length_dashes = length_arrow_goal_ - arrow.scale.z;
Point end_dashes;
end_dashes.x = start.x + length_dashes * std::cos(angle_w0_to_w1);
end_dashes.y = start.y + length_dashes * std::sin(angle_w0_to_w1);
line_list.points = ros_helpers::geometry::linspace_xy(start, end_dashes, num_dashes);
line_list.points = ros_helpers::even_size(line_list.points);
// arrow
arrow.header.frame_id = waypoint0.header.frame_id;
arrow.points.clear();
arrow.points.push_back(end_dashes);
arrow.points.push_back(end);
// array
markers_goal_.markers.clear();
markers_goal_.markers.push_back(line_list);
markers_goal_.markers.push_back(arrow);
ros_helpers::visualization::transform_points(markers_goal_, tf_robot_to_projection_);
ros_helpers::visualization::translate(markers_goal_, translation_goal_);
// set proximity to robot
const double_t robot_to_waypoint0 = std::hypot(waypoint0.point.x, waypoint0.point.y);
double_t freq = k_freq_prox_waypoint_ / robot_to_waypoint0;
freq = boost::algorithm::clamp(freq, 0, max_freq_);
pulser_pub_goal_.activate(freq);
}
MarkerArray markers_;
// Combines markers_traj_ and markers_goal_, then publishes
void publish_() override{
markers_.markers.clear();
if(get_is_active_traj()){
markers_ = markers_traj_;
}
else if(get_is_active_goal() && pulser_pub_goal_.get_state()){
markers_ = markers_goal_;
}
if(markers_.markers.empty()) markers_ = ros_helpers::visualization::array_delete_all(FRAME_PROJECTION_);
else ros_helpers::visualization::tidy(markers_);
pub_cue_.publish(markers_);
}
// Makes an integer odd by decreasing if necessary
void make_odd(int &i) const{
if(i%2 != 0) return;
else i -= 1;
}
// Does nothing
void clear_cue_traj_() override{}
void clear_cue_goal_() override{}
};
}
#endif