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