API tools faq
paste
Guest User

Untitled

a guest
Oct 27th, 2019
141
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 12.97 KB
raw download clone embed print report
  1. #include "hFramework.h"
  2. //#include "hCloudClient.h"
  3.  
  4. #include "ros.h"
  5. #include "std_msgs/Int16.h"
  6. #include "std_msgs/Float32.h"
  7. #include "std_msgs/UInt16MultiArray.h"
  8. #include "std_msgs/Empty.h"
  9. #include "std_msgs/Bool.h"
  10. #include "sensor_msgs/JointState.h"
  11. #include "geometry_msgs/TwistStamped.h"
  12. #include "geometry_msgs/Vector3Stamped.h"
  13. #include "std_srvs/Trigger.h"
  14.  
  15. #include "diff_controller.h"
  16. #include "params.h"
  17. #include "utils.h"
  18. #include "config.h"
  19.  
  20. #include "imu.h"
  21.  
  22. using namespace hFramework;
  23.  
  24. ros::NodeHandle nh;
  25.  
  26. std_msgs::Float32 battery;
  27. ros::Publisher *battery_pub;
  28. bool publish_battery = false;
  29.  
  30. geometry_msgs::TwistStamped odom;
  31. ros::Publisher *odom_pub;
  32. bool publish_odom = false;
  33.  
  34. sensor_msgs::JointState joint_states;
  35. ros::Publisher *joint_states_pub;
  36. bool publish_joint = false;
  37.  
  38. IMU* imu;
  39. geometry_msgs::Vector3Stamped imu_gyro_msg;
  40. ros::Publisher *imu_gyro_pub;
  41. geometry_msgs::Vector3Stamped imu_accel_msg;
  42. ros::Publisher *imu_accel_pub;
  43. geometry_msgs::Vector3Stamped imu_mag_msg;
  44. ros::Publisher *imu_mag_pub;
  45. bool publish_imu = false;
  46.  
  47. // <<<
  48. std_msgs::Float32 gpio_in_msg;
  49. ros::Publisher *gpio_in_pub;
  50. bool publish_gpio_in = false;
  51.  
  52. ros::Subscriber<std_msgs::Bool> *gpio_out_sub;
  53. // >>>
  54.  
  55. ros::Subscriber<geometry_msgs::Twist> *twist_sub;
  56.  
  57. ros::Subscriber<std_msgs::Empty> *reset_board_sub;
  58. ros::Subscriber<std_msgs::Empty> *reset_config_sub;
  59. ros::Subscriber<std_msgs::Bool> *set_imu_sub;
  60.  
  61. ros::ServiceServer<std_srvs::TriggerRequest, std_srvs::TriggerResponse>* imu_cal_mpu_srv;
  62. ros::ServiceServer<std_srvs::TriggerRequest, std_srvs::TriggerResponse>* imu_cal_mag_srv;
  63.  
  64. DiffController *dc;
  65.  
  66. ServoWrapper servo1(1, hServo.servo1);
  67. ServoWrapper servo2(2, hServo.servo2);
  68. ServoWrapper servo3(3, hServo.servo3);
  69. ServoWrapper servo4(4, hServo.servo4);
  70. ServoWrapper servo5(5, hServo.servo5);
  71. ServoWrapper servo6(6, hServo.servo6);
  72.  
  73. void cmdVelCallback(const geometry_msgs::Twist& msg)
  74. {
  75.     dc->setSpeed(msg.linear.x, msg.angular.z);
  76. #ifdef DEBUG
  77.     Serial.printf("[cmdVelCallback] linear: %f angular %f\r\n", msg.linear.x, msg.angular.z);
  78. #endif
  79. }
  80.  
  81. void resetBoardCallback(const std_msgs::Empty& msg)
  82. {
  83.     sys.reset();
  84. }
  85.  
  86. void resetConfigCallback(const std_msgs::Empty& msg)
  87. {
  88.     reset_config();
  89. }
  90.  
  91. void setImuCallback(const std_msgs::Bool& msg)
  92. {
  93.     conf.imu_enabled = msg.data;
  94.     store_config();
  95. }
  96.  
  97. void calMpuCallback(const std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
  98. {
  99.     imu->calGyroAccel();
  100.     res.message = "Succesfully calibrated gyroscope and accelerometer biases";
  101.     res.success = true;
  102. }
  103.  
  104. void calMagCallback(const std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
  105. {
  106.     imu->calMag();
  107.     res.message = "Succesfully calibrated magnetometer";
  108.     res.success = true;
  109. }
  110.  
  111. // <<<
  112. void GPIOOutCallback(const std_msgs::Bool& msg)
  113. {
  114.     hExt.pin3.write(msg.data);
  115. }
  116. // >>>
  117.  
  118. void initROS()
  119. {
  120.     battery_pub = new ros::Publisher("/battery", &battery);
  121.     odom_pub = new ros::Publisher("/wheel_odom", &odom);
  122.     joint_states_pub = new ros::Publisher("/joint_states", &joint_states);
  123.  
  124.     twist_sub = new ros::Subscriber<geometry_msgs::Twist>("cmd_vel", &cmdVelCallback);
  125.  
  126.     reset_board_sub = new ros::Subscriber<std_msgs::Empty>("core2/reset_board", &resetBoardCallback);
  127.     reset_config_sub = new ros::Subscriber<std_msgs::Empty>("core2/reset_config", &resetConfigCallback);
  128.     set_imu_sub = new ros::Subscriber<std_msgs::Bool>("core2/set_imu", &setImuCallback);
  129.  
  130.     ros::Subscriber<std_msgs::Int16, ServoWrapper> *servo1_angle_sub =
  131.         new ros::Subscriber<std_msgs::Int16, ServoWrapper>("servo1/angle", &ServoWrapper::angleCallback, &servo1);
  132.     ros::Subscriber<std_msgs::Int16, ServoWrapper> *servo2_angle_sub =
  133.         new ros::Subscriber<std_msgs::Int16, ServoWrapper>("servo2/angle", &ServoWrapper::angleCallback, &servo2);
  134.     ros::Subscriber<std_msgs::Int16, ServoWrapper> *servo3_angle_sub =
  135.         new ros::Subscriber<std_msgs::Int16, ServoWrapper>("servo3/angle", &ServoWrapper::angleCallback, &servo3);
  136.     ros::Subscriber<std_msgs::Int16, ServoWrapper> *servo4_angle_sub =
  137.         new ros::Subscriber<std_msgs::Int16, ServoWrapper>("servo4/angle", &ServoWrapper::angleCallback, &servo4);
  138.     ros::Subscriber<std_msgs::Int16, ServoWrapper> *servo5_angle_sub =
  139.         new ros::Subscriber<std_msgs::Int16, ServoWrapper>("servo5/angle", &ServoWrapper::angleCallback, &servo5);
  140.     ros::Subscriber<std_msgs::Int16, ServoWrapper> *servo6_angle_sub =
  141.         new ros::Subscriber<std_msgs::Int16, ServoWrapper>("servo6/angle", &ServoWrapper::angleCallback, &servo6);
  142.  
  143.     ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper> *servo1_pwm_sub =
  144.         new ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper>("servo1/pwm", &ServoWrapper::pwmCallback, &servo1);
  145.     ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper> *servo2_pwm_sub =
  146.         new ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper>("servo2/pwm", &ServoWrapper::pwmCallback, &servo2);
  147.     ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper> *servo3_pwm_sub =
  148.         new ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper>("servo3/pwm", &ServoWrapper::pwmCallback, &servo3);
  149.     ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper> *servo4_pwm_sub =
  150.         new ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper>("servo4/pwm", &ServoWrapper::pwmCallback, &servo4);
  151.     ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper> *servo5_pwm_sub =
  152.         new ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper>("servo5/pwm", &ServoWrapper::pwmCallback, &servo5);
  153.     ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper> *servo6_pwm_sub =
  154.         new ros::Subscriber<std_msgs::UInt16MultiArray, ServoWrapper>("servo6/pwm", &ServoWrapper::pwmCallback, &servo6);
  155.  
  156.     nh.advertise(*battery_pub);
  157.     nh.advertise(*odom_pub);
  158.     nh.advertise(*joint_states_pub);
  159.     nh.subscribe(*twist_sub);
  160.     nh.subscribe(*reset_board_sub);
  161.     nh.subscribe(*reset_config_sub);
  162.     nh.subscribe(*set_imu_sub);
  163.     nh.subscribe(*servo1_angle_sub);
  164.     nh.subscribe(*servo2_angle_sub);
  165.     nh.subscribe(*servo3_angle_sub);
  166.     nh.subscribe(*servo4_angle_sub);
  167.     nh.subscribe(*servo5_angle_sub);
  168.     nh.subscribe(*servo6_angle_sub);
  169.     nh.subscribe(*servo1_pwm_sub);
  170.     nh.subscribe(*servo2_pwm_sub);
  171.     nh.subscribe(*servo3_pwm_sub);
  172.     nh.subscribe(*servo4_pwm_sub);
  173.     nh.subscribe(*servo5_pwm_sub);
  174.     nh.subscribe(*servo6_pwm_sub);
  175.  
  176.     if (conf.imu_enabled)
  177.     {
  178.         imu_gyro_pub = new ros::Publisher("/imu/gyro", &imu_gyro_msg);
  179.         imu_accel_pub = new ros::Publisher("/imu/accel", &imu_accel_msg);
  180.         imu_mag_pub = new ros::Publisher("/imu/mag", &imu_mag_msg);
  181.         imu_cal_mpu_srv = new ros::ServiceServer<std_srvs::TriggerRequest, std_srvs::TriggerResponse>
  182.             ("imu/calibrate_gyro_accel", &calMpuCallback);
  183.         imu_cal_mag_srv = new ros::ServiceServer<std_srvs::TriggerRequest, std_srvs::TriggerResponse>
  184.             ("imu/calibrate_mag", &calMagCallback);
  185.         nh.advertise(*imu_gyro_pub);
  186.         nh.advertise(*imu_accel_pub);
  187.         nh.advertise(*imu_mag_pub);
  188.         nh.advertiseService<std_srvs::TriggerRequest, std_srvs::TriggerResponse>(*imu_cal_mpu_srv);
  189.         nh.advertiseService<std_srvs::TriggerRequest, std_srvs::TriggerResponse>(*imu_cal_mag_srv);
  190.     }
  191.  
  192. // <<<
  193.     gpio_in_pub = new ros::Publisher("/gpio_in", &gpio_in_msg);
  194.     gpio_out_sub = new ros::Subscriber<std_msgs::Bool>("/gpio_out", &GPIOOutCallback);
  195.  
  196.     nh.advertise(*gpio_in_pub);
  197.     nh.subscribe(*gpio_out_sub);
  198. // >>>
  199. }
  200.  
  201. void setupServos()
  202. {
  203.     hServo.enablePower();
  204.     hServo.setPeriod(SERVO_PERIOD);
  205.  
  206.     switch(SERVO_VOLTAGE) {
  207.         case VOLTAGE_5V:
  208.             hServo.setVoltage5V();
  209.             break;
  210.         case VOLTAGE_6V:
  211.             hServo.setVoltage6V();
  212.             break;
  213.         case VOLTAGE_7V4:
  214.             hServo.setVoltage7V4();
  215.             break;
  216.         case VOLTAGE_8V6:
  217.             hServo.setVoltage8V6();
  218.     }
  219.  
  220.     hServo.servo1.calibrate(SERVO_1_ANGLE_MIN, SERVO_1_WIDTH_MIN,
  221.                             SERVO_1_ANGLE_MAX, SERVO_1_WIDTH_MAX);
  222.     hServo.servo2.calibrate(SERVO_2_ANGLE_MIN, SERVO_2_WIDTH_MIN,
  223.                             SERVO_2_ANGLE_MAX, SERVO_2_WIDTH_MAX);
  224.     hServo.servo3.calibrate(SERVO_3_ANGLE_MIN, SERVO_3_WIDTH_MIN,
  225.                             SERVO_3_ANGLE_MAX, SERVO_3_WIDTH_MAX);
  226.     hServo.servo4.calibrate(SERVO_4_ANGLE_MIN, SERVO_4_WIDTH_MIN,
  227.                             SERVO_4_ANGLE_MAX, SERVO_4_WIDTH_MAX);
  228.     hServo.servo5.calibrate(SERVO_5_ANGLE_MIN, SERVO_5_WIDTH_MIN,
  229.                             SERVO_5_ANGLE_MAX, SERVO_5_WIDTH_MAX);
  230.     hServo.servo6.calibrate(SERVO_6_ANGLE_MIN, SERVO_6_WIDTH_MIN,
  231.                             SERVO_6_ANGLE_MAX, SERVO_6_WIDTH_MAX);
  232. }
  233.  
  234. void setupJoints()
  235. {
  236.     joint_states.header.frame_id = "base_link";
  237.     joint_states.name = new char*[4] {
  238.         "wheel_FL_joint", "wheel_RL_joint",
  239.         "wheel_FR_joint", "wheel_RR_joint"
  240.     };
  241.     joint_states.position = new float[4];
  242.     joint_states.velocity = new float[4];
  243.     joint_states.effort = new float[4];
  244.     joint_states.name_length = 4;
  245.     joint_states.position_length = 4;
  246.     joint_states.velocity_length = 4;
  247.     joint_states.effort_length = 4;
  248. }
  249.  
  250. void setupImu()
  251. {
  252.     IMU_HSENS.selectI2C();
  253.     imu = new IMU(IMU_HSENS.getI2C());
  254.     imu->begin();
  255.     imu_gyro_msg.header.frame_id = "imu";
  256.     imu_accel_msg.header.frame_id = "imu";
  257.     imu_mag_msg.header.frame_id = "imu";
  258. }
  259.  
  260. void setupOdom()
  261. {
  262.     odom.header.frame_id = "base_link";
  263. }
  264.  
  265. void batteryLoop()
  266. {
  267.     uint32_t t = sys.getRefTime();
  268.     long dt = 1000;
  269.     while(true)
  270.     {
  271.         if (!publish_battery)
  272.         {
  273.             battery.data = sys.getSupplyVoltage();
  274.            
  275.             publish_battery = true;
  276.         }
  277.  
  278.         sys.delaySync(t, dt);
  279.     }
  280. }
  281.  
  282. void odomLoop()
  283. {
  284.     uint32_t t = sys.getRefTime();
  285.     long dt = 50;
  286.     while(true)
  287.     {
  288.         if (!publish_odom)
  289.         {
  290.             odom.header.stamp = nh.now();
  291.  
  292.             std::vector<float> odo = dc->getOdom();
  293.             odom.twist.linear.x = odo[0];
  294.             odom.twist.angular.z = odo[1];
  295.  
  296.             publish_odom = true;
  297.         }
  298.  
  299.         sys.delaySync(t, dt);
  300.     }
  301. }
  302.  
  303. void jointStatesLoop()
  304. {
  305.     uint32_t t = sys.getRefTime();
  306.     long dt = 50;
  307.     while(true)
  308.     {
  309.         if (!publish_joint)
  310.         {
  311.             std::vector<float> pos = dc->getWheelPositions();
  312.             std::vector<float> vel = dc->getWheelVelocities();
  313.             std::vector<float> eff = dc->getWheelEfforts();
  314.  
  315.             joint_states.header.stamp = nh.now();
  316.  
  317.             joint_states.position[0] = pos[0];
  318.             joint_states.position[1] = pos[1];
  319.             joint_states.position[2] = pos[2];
  320.             joint_states.position[3] = pos[3];
  321.  
  322.             joint_states.velocity[0] = vel[0];
  323.             joint_states.velocity[1] = vel[1];
  324.             joint_states.velocity[2] = vel[2];
  325.             joint_states.velocity[3] = vel[3];
  326.  
  327.             joint_states.effort[0] = eff[0];
  328.             joint_states.effort[1] = eff[1];
  329.             joint_states.effort[2] = eff[2];
  330.             joint_states.effort[3] = eff[3];
  331.  
  332.             publish_joint = true;
  333.         }
  334.  
  335.         sys.delaySync(t, dt);
  336.     }
  337. }
  338.  
  339. void imuLoop()
  340. {
  341.     uint32_t t = sys.getRefTime();
  342.     long dt = 25;
  343.     while(true)
  344.     {
  345.         imu->update();
  346.  
  347.         ros::Time stamp = nh.now();
  348.  
  349.         imu_gyro_msg.header.stamp = stamp;
  350.  
  351.         imu_gyro_msg.vector.x = imu->gx;
  352.         imu_gyro_msg.vector.y = imu->gy;
  353.         imu_gyro_msg.vector.z = imu->gz;
  354.  
  355.         imu_accel_msg.header.stamp = stamp;
  356.  
  357.         imu_accel_msg.vector.x = imu->ax;
  358.         imu_accel_msg.vector.y = imu->ay;
  359.         imu_accel_msg.vector.z = imu->az;
  360.  
  361.         imu_mag_msg.header.stamp = stamp;
  362.  
  363.         imu_mag_msg.vector.x = imu->mx;
  364.         imu_mag_msg.vector.y = imu->my;
  365.         imu_mag_msg.vector.z = imu->mz;
  366.  
  367.         publish_imu = true;
  368.  
  369.         sys.delaySync(t, dt);
  370.     }
  371. }
  372.  
  373. void LEDLoop()
  374. {
  375.     uint32_t t = sys.getRefTime();
  376.     long dt = 250;
  377.     while(true)
  378.     {
  379.         if(!nh.connected())
  380.             LED.toggle();
  381.         else
  382.             LED.write(true);
  383.  
  384.         sys.delaySync(t, dt);
  385.     }
  386. }
  387.  
  388. // <<<
  389. void GPIOInLoop()
  390. {
  391.     uint32_t t = sys.getRefTime();
  392.     long dt = 100;
  393.     while(true)
  394.     {
  395.         if (!publish_gpio_in)
  396.         {
  397.             gpio_in_msg.data = hExt.pin2.analogReadVoltage();
  398.             publish_gpio_in = true;
  399.         }
  400.  
  401.         sys.delaySync(t, dt);
  402.     }
  403. }
  404. // >>>
  405.  
  406. void hMain()
  407. {
  408.     uint32_t t = sys.getRefTime();
  409.     //platform.begin(&RPi);
  410.     //nh.getHardware()->initWithDevice(&platform.LocalSerial);
  411.     RPi.setBaudrate(250000);
  412.     nh.getHardware()->initWithDevice(&RPi);
  413.     nh.initNode();
  414.    
  415.     dc = new DiffController(INPUT_TIMEOUT);
  416.     dc->start();
  417.  
  418.     load_config();
  419.  
  420. // <<<
  421.     hExt.pin2.enableADC();
  422.     hExt.pin3.setOut();
  423. // >>>
  424.  
  425.     setupOdom();
  426.     setupServos();
  427.     setupJoints();
  428.     initROS();
  429.  
  430. // <<<
  431.     sys.taskCreate(&GPIOInLoop);
  432. // >>>
  433.  
  434.     sys.setLogDev(&Serial);
  435.  
  436.     LED.setOut();
  437.     sys.taskCreate(&LEDLoop);
  438.  
  439.     sys.taskCreate(&batteryLoop);
  440.     sys.taskCreate(&odomLoop);
  441.     sys.taskCreate(&jointStatesLoop);
  442.  
  443.     if (conf.imu_enabled)
  444.     {
  445.         setupImu();
  446.         sys.taskCreate(&imuLoop);
  447.     }
  448.  
  449.     while (true)
  450.     {
  451.         nh.spinOnce();
  452.  
  453.         if (nh.connected())
  454.         {
  455.             if (publish_battery){
  456.                 battery_pub->publish(&battery);
  457.                 publish_battery = false;
  458.             }
  459.  
  460.             if (publish_odom){
  461.                 odom_pub->publish(&odom);
  462.                 publish_odom = false;
  463.             }
  464.  
  465.             if (publish_joint){
  466.                 joint_states_pub->publish(&joint_states);
  467.                 publish_joint = false;
  468.             }
  469.  
  470.             if (publish_imu){
  471.                 imu_gyro_pub->publish(&imu_gyro_msg);
  472.                 imu_accel_pub->publish(&imu_accel_msg);
  473.                 imu_mag_pub->publish(&imu_mag_msg);
  474.                 publish_imu = false;
  475.             }
  476.  
  477. // <<<
  478.             if (publish_gpio_in){
  479.                 gpio_in_pub->publish(&gpio_in_msg);
  480.                 publish_gpio_in = false;
  481.             }
  482. // >>>
  483.  
  484.         }
  485.        
  486.         sys.delaySync(t, 1);
  487.     }
  488.  
  489.  
  490. }
RAW Paste Data
Public Pastes
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!
 

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×