Revision 734b51b9
ID | 734b51b9841321e2bf7ec4ee2040cb5d87b4df82 |
Added PID subscribers to altitude_node
This should make testing at least a little easier, until we come up with
a better way. Also, moved joystick_control.launch to where it could be
seen globally by ROS.
quad2/altitude_node/src/altitude_node.cpp | ||
---|---|---|
11 | 11 |
void main_loop() {ros::spin();} |
12 | 12 |
void height_cb(const mikrokopter::FcDebug::ConstPtr& fc); |
13 | 13 |
void goal_cb(const std_msgs::Float64::ConstPtr& goal); |
14 |
void p_cb(const std_msgs::Float64::ConstPtr& p); |
|
15 |
void i_cb(const std_msgs::Float64::ConstPtr& i); |
|
16 |
void d_cb(const std_msgs::Float64::ConstPtr& d); |
|
14 | 17 |
void enable_cb(const std_msgs::Bool::ConstPtr& goal); |
15 | 18 |
|
16 | 19 |
private: |
... | ... | |
20 | 23 |
ros::Publisher pub; |
21 | 24 |
ros::Subscriber height_sub; |
22 | 25 |
ros::Subscriber goal_sub; |
26 |
ros::Subscriber p_sub; |
|
27 |
ros::Subscriber i_sub; |
|
28 |
ros::Subscriber d_sub; |
|
23 | 29 |
ros::Subscriber enable_sub; |
24 | 30 |
}; |
25 | 31 |
|
26 |
AltitudeNode::AltitudeNode() : pid(10, 0, 0, 0), enabled(false)
|
|
32 |
AltitudeNode::AltitudeNode() : pid(0, 0, 0, 0), enabled(false) |
|
27 | 33 |
{ |
28 |
height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug", |
|
29 |
100, &AltitudeNode::height_cb, this); |
|
30 |
goal_sub = n.subscribe<std_msgs::Float64>("/mikrokopter/height_goal", |
|
31 |
100, &AltitudeNode::goal_cb, this); |
|
32 |
enable_sub = n.subscribe<std_msgs::Bool>("/altitude_node/enable", |
|
33 |
100, &AltitudeNode::enable_cb, this); |
|
34 |
height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug", 100, |
|
35 |
&AltitudeNode::height_cb, this); |
|
36 |
goal_sub = n.subscribe<std_msgs::Float64>("/altitude_node/height_goal", 100, |
|
37 |
&AltitudeNode::goal_cb, this); |
|
38 |
p_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/p", 100, |
|
39 |
&AltitudeNode::p_cb, this); |
|
40 |
i_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/i", 100, |
|
41 |
&AltitudeNode::i_cb, this); |
|
42 |
d_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/d", 100, |
|
43 |
&AltitudeNode::d_cb, this); |
|
44 |
enable_sub = n.subscribe<std_msgs::Bool>("/altitude_node/enable", 100, |
|
45 |
&AltitudeNode::enable_cb, this); |
|
34 | 46 |
pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100); |
35 | 47 |
|
36 |
float default_goal;
|
|
37 |
ros::param::param<float>("~default_goal", default_goal, 0);
|
|
48 |
double default_goal;
|
|
49 |
ros::param::param<double>("~default_goal", default_goal, 0);
|
|
38 | 50 |
pid.change_goal(default_goal); |
39 | 51 |
} |
40 | 52 |
|
... | ... | |
51 | 63 |
void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal) |
52 | 64 |
{ |
53 | 65 |
pid.change_goal(goal->data); |
66 |
ROS_INFO("Goal: %f", goal->data); |
|
67 |
} |
|
68 |
|
|
69 |
void AltitudeNode::p_cb(const std_msgs::Float64::ConstPtr& p) |
|
70 |
{ |
|
71 |
pid.change_p(p->data); |
|
72 |
ROS_INFO("K_p: %f", p->data); |
|
73 |
} |
|
74 |
|
|
75 |
void AltitudeNode::i_cb(const std_msgs::Float64::ConstPtr& i) |
|
76 |
{ |
|
77 |
pid.change_i(i->data); |
|
78 |
ROS_INFO("K_i: %f", i->data); |
|
79 |
} |
|
80 |
|
|
81 |
void AltitudeNode::d_cb(const std_msgs::Float64::ConstPtr& d) |
|
82 |
{ |
|
83 |
pid.change_d(d->data); |
|
84 |
ROS_INFO("K_d: %f", d->data); |
|
54 | 85 |
} |
55 | 86 |
|
56 | 87 |
void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg) |
quad2/launch/joystick_control.launch | ||
---|---|---|
1 |
<launch> |
|
2 |
|
|
3 |
<node name="joystick_node" pkg="joystick_node" type="joystick_node"/> |
|
4 |
<node name="altitude_node" pkg="altitude_node" type="altitude_node"> |
|
5 |
<param name="default_goal" value="0"/> |
|
6 |
</node> |
|
7 |
<node name="node_control" pkg="mikrokopter" type="node_control"> |
|
8 |
<param name="publish_rate" value="20"/> |
|
9 |
</node> |
|
10 |
<node name="joy_node" pkg="joy" type="joy_node"/> |
|
11 |
|
|
12 |
</launch> |
quad2/mikrokopter/launch/joystick_control.launch | ||
---|---|---|
1 |
<launch> |
|
2 |
|
|
3 |
<node name="joystick_node" pkg="joystick_node" type="joystick_node"/> |
|
4 |
<node name="altitude_node" pkg="altitude_node" type="altitude_node" output="screen"> |
|
5 |
<param name="default_goal" value="60"/> |
|
6 |
</node> |
|
7 |
<node name="node_control" pkg="mikrokopter" type="node_control"> |
|
8 |
<param name="publish_rate" value="20"/> |
|
9 |
</node> |
|
10 |
<node name="joy_node" pkg="joy" type="joy_node"/> |
|
11 |
|
|
12 |
</launch> |
Also available in: Unified diff