API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jul 11th, 2019
128
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 7.54 KB | None | 0 0
raw download clone embed print report
  1. #include "hFramework.h"
  2. #include "hCloudClient.h"
  3.  
  4. #include "ros.h"
  5. #include "geometry_msgs/Twist.h"
  6. #include "std_msgs/Int16.h"
  7. #include "std_msgs/Float32.h"
  8. #include "std_msgs/UInt16MultiArray.h"
  9. #include "std_msgs/Bool.h"
  10.  
  11. #include "diff_controller.h"
  12. #include "params.h"
  13.  
  14. using namespace hFramework;
  15.  
  16. ros::NodeHandle nh;
  17.  
  18. hMutex publish_mutex;
  19.  
  20. std_msgs::Float32 battery;
  21. ros::Publisher *battery_pub;
  22. bool publish_battery = false;
  23.  
  24. geometry_msgs::Twist odom;
  25. ros::Publisher *odom_pub;
  26. bool publish_odom = false;
  27.  
  28. ros::Subscriber<geometry_msgs::Twist> *twist_sub;
  29. ros::Subscriber<std_msgs::Bool> *gpio2_sub;
  30.  
  31. DiffController *dc;
  32.  
  33. class ServoWrapper
  34. {
  35.     int num;
  36.     int per;
  37.     IServo& servo;
  38.  
  39. public:
  40.     ServoWrapper(int num, IServo& servo)
  41.         : num(num),
  42.           servo(servo) {}
  43.  
  44.     void angleCallback(const std_msgs::Int16& msg)
  45.     {
  46.         if (per!=SERVO_PERIOD)
  47.         {
  48.             servo.setPeriod(SERVO_PERIOD);
  49.             per=SERVO_PERIOD;
  50.         }
  51.         servo.rotAbs(msg.data);
  52. #ifdef DEBUG
  53.         Serial.printf("[servo%dAngleCallback] angle: %d\r\n", num, msg.data);
  54. #endif
  55.     }
  56.  
  57.     void pwmCallback(const std_msgs::UInt16MultiArray& msg)
  58.     {
  59.         if (msg.data_length >= 2){
  60.             per=msg.data[0];
  61.             servo.setPeriod(msg.data[0]);
  62.             servo.setWidth(msg.data[1]);
  63. #ifdef DEBUG
  64.             Serial.printf("[servo%dPWMCallback] period: %d width: %d\r\n", num, msg.data[0], msg.data[1]);
  65.         } else {
  66.             Serial.printf("ERROR: [servo%dPWMCallback] data array should have 2 members\r\n", num);
  67. #endif
  68.         }
  69.     }
  70. };
  71.  
  72. ServoWrapper servo1(1, hServo.servo1);
  73. ServoWrapper servo2(2, hServo.servo2);
  74. ServoWrapper servo3(3, hServo.servo3);
  75. ServoWrapper servo4(4, hServo.servo4);
  76. ServoWrapper servo5(5, hServo.servo5);
  77. ServoWrapper servo6(6, hServo.servo6);
  78.  
  79. void cmdVelCallback(const geometry_msgs::Twist& msg)
  80. {
  81.     dc->setSpeed(msg.linear.x, msg.angular.z);
  82. #ifdef DEBUG
  83.     Serial.printf("[cmdVelCallback] linear: %f angular %f\r\n", msg.linear.x, msg.angular.z);
  84. #endif
  85. }
  86.  
  87. void gpio2Callback(const std_msgs::Bool& msg)
  88. {
  89.     hExt.pin2.write(msg.data);
  90. }
  91.  
  92. void initROS()
  93. {
  94.     battery_pub = new ros::Publisher("/battery", &battery);
  95.     odom_pub = new ros::Publisher("/odom", &odom);
  96.     twist_sub = new ros::Subscriber<geometry_msgs::Twist>("/cmd_vel", &cmdVelCallback);
  97.     gpio2_sub = new ros::Subscriber<std_msgs::Bool>("/gpio2", &gpio2Callback);
  98.  
  99.     ros::Subscriber<std_msgs::Int16, ServoWrapper> *servo1_angle_sub =
  100.         new ros::Subscriber<std_msgs::Int16, ServoWrapper>("/servo1/angle", &ServoWrapper::angleCallback, &servo1);
  101.     ros::Subscriber<std_msgs::Int16, ServoWrapper> *servo2_angle_sub =
  102.         new ros::Subscriber<std_msgs::Int16, ServoWrapper>("/servo2/angle", &ServoWrapper::angleCallback, &servo2);
  103.     ros::Subscriber<std_msgs::Int16, ServoWrapper> *servo3_angle_sub =
  104.         new ros::Subscriber<std_msgs::Int16, ServoWrapper>("/servo3/angle", &ServoWrapper::angleCallback, &servo3);
  105.     ros::Subscriber<std_msgs::Int16, ServoWrapper> *servo4_angle_sub =
  106.         new ros::Subscriber<std_msgs::Int16, ServoWrapper>("/servo4/angle", &ServoWrapper::angleCallback, &servo4);
  107.     ros::Subscriber<std_msgs::Int16, ServoWrapper> *servo5_angle_sub =
  108.         new ros::Subscriber<std_msgs::Int16, ServoWrapper>("/servo5/angle", &ServoWrapper::angleCallback, &servo5);
  109.     ros::Subscriber<std_msgs::Int16, ServoWrapper> *servo6_angle_sub =
  110.         new ros::Subscriber<std_msgs::Int16, ServoWrapper>("/servo6/angle", &ServoWrapper::angleCallback, &servo6);
  111.  
  112.     ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper> *servo1_pwm_sub =
  113.         new ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper>("/servo1/pwm", &ServoWrapper::pwmCallback, &servo1);
  114.     ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper> *servo2_pwm_sub =
  115.         new ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper>("/servo2/pwm", &ServoWrapper::pwmCallback, &servo2);
  116.     ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper> *servo3_pwm_sub =
  117.         new ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper>("/servo3/pwm", &ServoWrapper::pwmCallback, &servo3);
  118.     ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper> *servo4_pwm_sub =
  119.         new ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper>("/servo4/pwm", &ServoWrapper::pwmCallback, &servo4);
  120.     ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper> *servo5_pwm_sub =
  121.         new ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper>("/servo5/pwm", &ServoWrapper::pwmCallback, &servo5);
  122.     ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper> *servo6_pwm_sub =
  123.         new ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper>("/servo6/pwm", &ServoWrapper::pwmCallback, &servo6);
  124.  
  125.     nh.advertise(*battery_pub);
  126.     nh.advertise(*odom_pub);
  127.     nh.subscribe(*twist_sub);
  128.     nh.subscribe(*servo1_angle_sub);
  129.     nh.subscribe(*servo2_angle_sub);
  130.     nh.subscribe(*servo3_angle_sub);
  131.     nh.subscribe(*servo4_angle_sub);
  132.     nh.subscribe(*servo5_angle_sub);
  133.     nh.subscribe(*servo6_angle_sub);
  134.     nh.subscribe(*servo1_pwm_sub);
  135.     nh.subscribe(*servo2_pwm_sub);
  136.     nh.subscribe(*servo3_pwm_sub);
  137.     nh.subscribe(*servo4_pwm_sub);
  138.     nh.subscribe(*servo5_pwm_sub);
  139.     nh.subscribe(*servo6_pwm_sub);
  140.     nh.subscribe(*gpio2_sub);
  141. }
  142.  
  143. void setupServos()
  144. {
  145.     hServo.enablePower();
  146.     hServo.setPeriod(SERVO_PERIOD);
  147.  
  148.     switch(SERVO_VOLTAGE) {
  149.         case VOLTAGE_5V:
  150.             hServo.setVoltage5V();
  151.             break;
  152.         case VOLTAGE_6V:
  153.             hServo.setVoltage6V();
  154.             break;
  155.         case VOLTAGE_7V4:
  156.             hServo.setVoltage7V4();
  157.             break;
  158.         case VOLTAGE_8V6:
  159.             hServo.setVoltage8V6();
  160.     }
  161.  
  162.     hServo.servo1.calibrate(SERVO_1_ANGLE_MIN, SERVO_1_WIDTH_MIN,
  163.                             SERVO_1_ANGLE_MAX, SERVO_1_WIDTH_MAX);
  164.     hServo.servo2.calibrate(SERVO_2_ANGLE_MIN, SERVO_2_WIDTH_MIN,
  165.                             SERVO_2_ANGLE_MAX, SERVO_2_WIDTH_MAX);
  166.     hServo.servo3.calibrate(SERVO_3_ANGLE_MIN, SERVO_3_WIDTH_MIN,
  167.                             SERVO_3_ANGLE_MAX, SERVO_3_WIDTH_MAX);
  168.     hServo.servo4.calibrate(SERVO_4_ANGLE_MIN, SERVO_4_WIDTH_MIN,
  169.                             SERVO_4_ANGLE_MAX, SERVO_4_WIDTH_MAX);
  170.     hServo.servo5.calibrate(SERVO_5_ANGLE_MIN, SERVO_5_WIDTH_MIN,
  171.                             SERVO_5_ANGLE_MAX, SERVO_5_WIDTH_MAX);
  172.     hServo.servo6.calibrate(SERVO_6_ANGLE_MIN, SERVO_6_WIDTH_MIN,
  173.                             SERVO_6_ANGLE_MAX, SERVO_6_WIDTH_MAX);
  174. }
  175.  
  176. void batteryLoop()
  177. {
  178.     uint32_t t = sys.getRefTime();
  179.     long dt = 1000;
  180.     while(true)
  181.     {
  182.         battery.data = sys.getSupplyVoltage();
  183.        
  184.         publish_mutex.lock();
  185.         publish_battery = true;
  186.         publish_mutex.unlock();
  187.  
  188.         sys.delaySync(t, dt);
  189.     }
  190. }
  191.  
  192. void odomLoop()
  193. {
  194.     uint32_t t = sys.getRefTime();
  195.     long dt = 50;
  196.     while(true)
  197.     {
  198.         std::vector<float> odo = dc->getOdom();
  199.         odom.linear.x = odo[0];
  200.         odom.angular.z = odo[1];
  201.  
  202.         publish_mutex.lock();
  203.         publish_odom = true;
  204.         publish_mutex.unlock();
  205.  
  206.         sys.delaySync(t, dt);
  207.     }
  208. }
  209.  
  210. void LEDLoop()
  211. {
  212.     uint32_t t = sys.getRefTime();
  213.     long dt = 250;
  214.     while(true)
  215.     {
  216.         if(!nh.connected())
  217.             LED.toggle();
  218.         else
  219.             LED.write(true);
  220.  
  221.         sys.delaySync(t, dt);
  222.     }
  223. }
  224.  
  225. void hMain()
  226. {
  227.     uint32_t t = sys.getRefTime();
  228.     platform.begin(&RPi);
  229.     nh.getHardware()->initWithDevice(&platform.LocalSerial);
  230.     //nh.getHardware()->initWithDevice(&RPi);
  231.     nh.initNode();
  232.    
  233.     dc = new DiffController(INPUT_TIMEOUT);
  234.     dc->start();
  235.  
  236.     setupServos();
  237.     initROS();
  238.  
  239.     LED.setOut();
  240.     sys.taskCreate(&LEDLoop);
  241.  
  242.     sys.taskCreate(&batteryLoop);
  243.     sys.taskCreate(&odomLoop);
  244.  
  245.     hExt.pin2.setOut();
  246.  
  247.     while (true)
  248.     {
  249.         nh.spinOnce();
  250.  
  251.         publish_mutex.lock();
  252.  
  253.         if (publish_battery){
  254.             battery_pub->publish(&battery);
  255.             publish_battery = false;
  256.         }
  257.  
  258.         if (publish_odom){
  259.             odom_pub->publish(&odom);
  260.             publish_odom = false;
  261.         }
  262.  
  263.         publish_mutex.unlock();
  264.  
  265.         sys.delaySync(t, 10);
  266.     }
  267.  
  268.  
  269. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!