Revision 7a6ed02d
ID | 7a6ed02d47f49de02afc7e0e8cc97db92054f38c |
Major changes in mikrokopter for lost connection
-In nav_lib.cpp, added main_loop, so we don't have to keep rewriting it
:). Also, this now publishes to /mk_wrapper/control, which should be
used from now on. However, old publish_on still works as it did.
-Changed turn_to_target and joystick_control (but not any others yet) to
use new nav_lib main_loop.
-Added mk_wrapper, which simply copies messages from /mk_wrapper/control
to /mikrokopter/req_set_control. After recieving the first message, if
it receives no message for a period of 200ms (rosparam max_quiet_time),
it will instead publish 0's and a thrust of 60, ignoring all future
input until restarted.
-Added launch files onboard.launch and joystick.launch, so that we
remember what should run where and can easily set some parameters.
mikrokopter/src/nav_lib.cpp | ||
---|---|---|
1 | 1 |
#include "nav_lib.h" |
2 | 2 |
|
3 |
|
|
4 | 3 |
MikrokopterControl::MikrokopterControl() |
5 | 4 |
{ |
6 | 5 |
control.digital[0] = 0; |
... | ... | |
70 | 69 |
pub.publish(control); |
71 | 70 |
} |
72 | 71 |
|
72 |
void MikrokopterControl::main_loop() |
|
73 |
{ |
|
74 |
ros::Publisher pub = nh.advertise< ::mikrokopter::Control>( |
|
75 |
"/mk_wrapper/control", 200); |
|
76 |
|
|
77 |
int publish_rate; |
|
78 |
ros::param::param<int>("publish_rate", publish_rate, 20); |
|
79 |
ros::Rate loop_rate(publish_rate); |
|
80 |
|
|
81 |
while (ros::ok()) |
|
82 |
{ |
|
83 |
ros::spinOnce(); |
|
84 |
pub.publish(control); |
|
85 |
loop_rate.sleep(); |
|
86 |
} |
|
87 |
|
|
88 |
} |
Also available in: Unified diff