Project

General

Profile

Revision 7bc485ed

ID7bc485ed1e7e508acaedf3dada02bec443ac9856

Added by Thomas Mullins about 12 years ago

Added /altitude_node/enable topic to disable PID

Also, added node_control.cpp and Velocity2D.msg, which should have been
commited before but apparently weren't.

View differences:

mikrokopter/altitude_node/src/altitude_node.cpp
1 1 
#include "ros/ros.h"
2 
#include "std_msgs/Float64.h"
3 2 
#include "mikrokopter/FcDebug.h"
3 
#include "std_msgs/Float64.h"
4 
#include "std_msgs/Bool.h"
4 5 
#include "../../../control/pid_control.h"
5 6 

  		




  6
  7
  
    
class AltitudeNode


  		




  ......




  10
  11
  
    
  void main_loop() {ros::spin();}


  		




  11
  12
  
    
  void height_cb(const mikrokopter::FcDebug::ConstPtr& fc);


  		




  12
  13
  
    
  void goal_cb(const std_msgs::Float64::ConstPtr& goal);


  		




  
  14
  
    
  void enable_cb(const std_msgs::Bool::ConstPtr& goal);


  		




  13
  15
  
    

  		




  14
  16
  
    
private:


  		




  
  17
  
    
  bool enabled;


  		




  15
  18
  
    
  PID_control pid;


  		




  16
  19
  
    
  ros::NodeHandle n;


  		




  17
  20
  
    
  ros::Publisher pub;


  		




  18
  21
  
    
  ros::Subscriber height_sub;


  		




  19
  22
  
    
  ros::Subscriber goal_sub;


  		




  
  23
  
    
  ros::Subscriber enable_sub;


  		




  20
  24
  
    
};


  		




  21
  25
  
    

  		




  22
  
  
    
AltitudeNode::AltitudeNode() : pid(10, 0, 0, 0)


  		




  
  26
  
    
AltitudeNode::AltitudeNode() : pid(10, 0, 0, 0), enabled(false)


  		




  23
  27
  
    
{


  		




  24
  28
  
    
  height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug",


  		




  25
  29
  
    
      100, &AltitudeNode::height_cb, this);


  		




  26
  30
  
    
  goal_sub = n.subscribe<std_msgs::Float64>("/mikrokopter/height_goal",


  		




  27
  31
  
    
      100, &AltitudeNode::goal_cb, this);


  		




  
  32
  
    
  enable_sub = n.subscribe<std_msgs::Bool>("/altitude_node/enable",


  		




  
  33
  
    
      100, &AltitudeNode::enable_cb, this);


  		




  28
  34
  
    
  pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100);


  		




  29
  35
  
    
}


  		




  30
  36
  
    

  		




  31
  37
  
    
void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc)


  		




  32
  38
  
    
{


  		




  33
  
  
    
  std_msgs::Float64 msg;


  		




  34
  
  
    
  msg.data = pid.pid(fc->heightValue, fc->header.stamp.toNSec());


  		




  35
  
  
    
  pub.publish(msg);


  		




  
  39
  
    
  if (enabled)


  		




  
  40
  
    
  {


  		




  
  41
  
    
    std_msgs::Float64 msg;


  		




  
  42
  
    
    msg.data = pid.pid(fc->heightValue, fc->header.stamp.toNSec());


  		




  
  43
  
    
    pub.publish(msg);


  		




  
  44
  
    
  }


  		




  36
  45
  
    
}


  		




  37
  46
  
    

  		




  38
  47
  
    
void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal)


  		




  ......




  40
  49
  
    
  pid.change_goal(goal->data);


  		




  41
  50
  
    
}


  		




  42
  51
  
    

  		




  
  52
  
    
void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg)


  		




  
  53
  
    
{


  		




  
  54
  
    
  enabled = msg->data;


  		




  
  55
  
    
}


  		




  
  56
  
    

  		




  43
  57
  
    
int main(int argc, char **argv) {


  		




  44
  58
  
    
  ros::init(argc, argv, "altitude_node");


  		




  45
  59
  
    
  AltitudeNode an = AltitudeNode();


  		












  

    
      mikrokopter/msg/Velocity2D.msg
    
  





  
  1
  
    
Header header


  		




  
  2
  
    
float64 forward


  		




  
  3
  
    
float64 lateral


  		












  

    
      mikrokopter/src/node_control.cpp
    
  





  
  1
  
    
#include "ros/ros.h"


  		




  
  2
  
    
#include "std_msgs/Float64.h"


  		




  
  3
  
    
#include "mikrokopter/Velocity2D.h"


  		




  
  4
  
    
#include "nav_lib.h"


  		




  
  5
  
    

  		




  
  6
  
    
class NodeControl


  		




  
  7
  
    
{


  		




  
  8
  
    
public:


  		




  
  9
  
    
  NodeControl();


  		




  
  10
  
    
  void main_loop() {control.main_loop();}


  		




  
  11
  
    
  void cb_velocity(const mikrokopter::Velocity2D::ConstPtr& n);


  		




  
  12
  
    
  void cb_yaw(const std_msgs::Float64::ConstPtr& n);


  		




  
  13
  
    
  void cb_thrust(const std_msgs::Float64::ConstPtr& n);


  		




  
  14
  
    

  		




  
  15
  
    
private:


  		




  
  16
  
    
  MikrokopterControl control;


  		




  
  17
  
    
  ros::NodeHandle n;


  		




  
  18
  
    
  ros::Subscriber sub_velocity;


  		




  
  19
  
    
  ros::Subscriber sub_yaw;


  		




  
  20
  
    
  ros::Subscriber sub_thrust;


  		




  
  21
  
    
};


  		




  
  22
  
    

  		




  
  23
  
    
NodeControl::NodeControl()


  		




  
  24
  
    
{


  		




  
  25
  
    
  sub_velocity = n.subscribe<mikrokopter::Velocity2D>("/mikrokopter/velocity",


  		




  
  26
  
    
      100, &NodeControl::cb_velocity, this);


  		




  
  27
  
    
  sub_yaw = n.subscribe<std_msgs::Float64>("/mikrokopter/yaw", 100,


  		




  
  28
  
    
      &NodeControl::cb_yaw, this);


  		




  
  29
  
    
  sub_thrust= n.subscribe<std_msgs::Float64>("/mikrokopter/thrust", 100,


  		




  
  30
  
    
      &NodeControl::cb_thrust, this);


  		




  
  31
  
    
}


  		




  
  32
  
    

  		




  
  33
  
    
void NodeControl::cb_velocity(const mikrokopter::Velocity2D::ConstPtr& n)


  		




  
  34
  
    
{


  		




  
  35
  
    
  control.velocity_control(n->forward, n->lateral);


  		




  
  36
  
    
}


  		




  
  37
  
    
void NodeControl::cb_yaw(const std_msgs::Float64::ConstPtr& n)


  		




  
  38
  
    
{


  		




  
  39
  
    
  control.set_yaw(n->data);


  		




  
  40
  
    
}


  		




  
  41
  
    
void NodeControl::cb_thrust(const std_msgs::Float64::ConstPtr& n)


  		




  
  42
  
    
{


  		




  
  43
  
    
  control.set_thrust(n->data);


  		




  
  44
  
    
}


  		




  
  45
  
    

  		




  
  46
  
    
int main(int argc, char **argv) {


  		




  
  47
  
    
  ros::init(argc, argv, "node_control");


  		




  
  48
  
    
  NodeControl control = NodeControl();


  		




  
  49
  
    
  control.main_loop();


  		




  
  50
  
    
  return 0;


  		




  
  51
  
    
}


  		












Also available in:  Unified diff