Diff: ROS_Handler.cpp

Revision:

0:3dfee562823a

Child:

1:6a10e58b3d43

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ROS_Handler.cpp	Wed Nov 27 18:28:06 2019 +0000
@@ -0,0 +1,42 @@
+#include "General.hpp"
+
+DigitalOut myLED(LED1);
+
+/******************************************************************************/
+/*  ROS serial hardware setup                                                 */
+/******************************************************************************/
+// Create the ROS node handle
+class HaseHardware : public MbedHardware
+{
+public:
+    HaseHardware(): MbedHardware(ROS_Tx, ROS_Rx, ROS_BaudRate) {};
+};
+/******************************************************************************/
+
+ros::NodeHandle_<HaseHardware>   Node_;
+void CallBack(const std_msgs::Int32MultiArray& msg);
+ros::Subscriber<std_msgs::Int32MultiArray> Sub_("/cmd_vel", &CallBack);
+std_msgs::Int32 PUB_MSG;
+ros::Publisher Pub_("/TOF_Data", &PUB_MSG);
+
+// Main function for thread to handle ROS comms
+void ROS_Handler(void){
+    myLED = 1;
+    Node_.initNode();
+    Node_.advertise(Pub_);
+    Node_.subscribe(Sub_);
+    
+    PUB_MSG.data = 10;
+
+    while (1) {
+        Pub_.publish(&PUB_MSG);
+        Node_.spinOnce();
+        wait_ms(20);
+    }
+}
+
+void CallBack(const std_msgs::Int32MultiArray& msg)
+{
+    myLED != myLED;
+}
+