Revision 734b51b9

View differences:

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