Revision 3bdc7bfd
ID | 3bdc7bfd383eff8b8dc04226581ac742720e33b6 |
Changed rviz target to publish constantly
vision/src/rviz_interactive_target.cpp | ||
---|---|---|
9 | 9 |
|
10 | 10 |
/* TODO: |
11 | 11 |
* -replace the three MOVE_AXIS directions with one MOVE_AXIS and one MOVE_PLANE |
12 |
* -have it constantly publish current position, instead of only publishing |
|
13 |
* when the marker is moved |
|
14 | 12 |
*/ |
15 | 13 |
|
16 | 14 |
class Marker3DOF |
17 | 15 |
{ |
18 | 16 |
public: |
19 |
Marker3DOF(interactive_markers::InteractiveMarkerServer& server); |
|
17 |
Marker3DOF(interactive_markers::InteractiveMarkerServer& server, int pubRate);
|
|
20 | 18 |
void processFeedback(const InteractiveMarkerFeedbackConstPtr& feedback); |
19 |
void timerCallback(const ros::TimerEvent& event); |
|
21 | 20 |
private: |
22 | 21 |
void addControl(string name, float x, float y, float z, float w); |
23 | 22 |
ros::NodeHandle nh; |
24 |
InteractiveMarker marker; |
|
25 | 23 |
ros::Publisher pub; |
24 |
ros::Timer timer; |
|
25 |
InteractiveMarker marker; |
|
26 | 26 |
}; |
27 | 27 |
|
28 |
Marker3DOF::Marker3DOF(interactive_markers::InteractiveMarkerServer& server) |
|
28 |
Marker3DOF::Marker3DOF(interactive_markers::InteractiveMarkerServer& server, |
|
29 |
int pubRate = 0) |
|
29 | 30 |
{ |
30 | 31 |
marker.header.frame_id = "/quadrotor"; |
31 | 32 |
marker.pose.position.x = 5; |
... | ... | |
40 | 41 |
server.setCallback("target", bind(&Marker3DOF::processFeedback, this, _1)); |
41 | 42 |
server.applyChanges(); |
42 | 43 |
pub = nh.advertise<geometry_msgs::Point>("/v2v3_converter/target_3d", 100); |
44 |
if (pubRate > 0) |
|
45 |
timer = nh.createTimer(ros::Duration(1./pubRate), |
|
46 |
bind(&Marker3DOF::timerCallback, this, _1)); |
|
43 | 47 |
} |
44 | 48 |
|
45 | 49 |
void Marker3DOF::addControl(string name, float x, float y, float z, float w) |
... | ... | |
54 | 58 |
marker.controls.push_back(control); |
55 | 59 |
} |
56 | 60 |
|
57 |
void Marker3DOF::processFeedback(const InteractiveMarkerFeedbackConstPtr& |
|
58 |
feedback) |
|
61 |
void Marker3DOF::processFeedback(const InteractiveMarkerFeedbackConstPtr& feedback) |
|
62 |
{ |
|
63 |
marker.pose.position = feedback->pose.position; |
|
64 |
} |
|
65 |
|
|
66 |
void Marker3DOF::timerCallback(const ros::TimerEvent& event) |
|
59 | 67 |
{ |
60 |
pub.publish(feedback->pose.position);
|
|
68 |
pub.publish(marker.pose.position);
|
|
61 | 69 |
} |
62 | 70 |
|
63 | 71 |
int main(int argc, char **argv) |
... | ... | |
65 | 73 |
ros::init(argc, argv, "rviz_interactive_targets"); |
66 | 74 |
interactive_markers::InteractiveMarkerServer server( |
67 | 75 |
"rviz_interactive_targets"); |
68 |
Marker3DOF marker(server); |
|
76 |
Marker3DOF marker(server, 10);
|
|
69 | 77 |
ros::spin(); |
70 | 78 |
return 0; |
71 | 79 |
} |
Also available in: Unified diff