Diff: main.cpp

Revision:

10:beaa2ddfef32

Parent:

9:1081ebfe3db0

--- a/main.cpp	Fri Nov 02 02:28:51 2018 +0000
+++ b/main.cpp	Sat Nov 03 20:36:36 2018 +0000
@@ -24,26 +24,50 @@
 
 ros::NodeHandle nh;
 nav_msgs::Odometry odom;
-ros::Publisher chatter("odom", &odom);
+geometry_msgs::Twist commandRead;
 
-/*void setOdomNode(){
-    nh.initNode();
-    nh.advertise(chatter);
-}*/
+void handlerFunction(const geometry_msgs::Twist& command){
+    commandRead = command;
+}
+
+ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction);
+ros::Publisher chatter("odom", &odom);
+ros::Publisher chatter2("cmd_vel", &commandRead);
 
 Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS); //Initialize wheelchair object
 Thread compass;                      //Thread for compass
 Thread velosity;                      //Thread for velosity
 Thread ros_com;                      //Thread for velosity
 
+void rosCom_thread()
+{
+        odom.pose.pose.position.x = smart.x_position;
+        odom.pose.pose.position.y = smart.y_position;
+        odom.pose.pose.position.z = 0;
+        //set the orientation
+        odom.pose.pose.orientation.z = smart.z_angular;
+        odom.pose.pose.orientation.x = 0;
+        odom.pose.pose.orientation.y = 0;
+        odom.twist.twist.linear.x = smart.curr_vel;
+        odom.twist.twist.linear.y = 0;
+        odom.twist.twist.linear.z = 0;
+        odom.twist.twist.angular.x = 0;
+        odom.twist.twist.angular.y = 0;
+        odom.twist.twist.angular.z = smart.getTwistZ();
+        chatter.publish(&odom);
+        chatter2.publish(&commandRead);
+        nh.spinOnce();
+}    
 int main(void)
 {   
     nh.initNode();
     nh.advertise(chatter);
+    nh.advertise(chatter2);
+    nh.subscribe(sub);
     
     queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);  //Sets up sampling frequency of the compass_thread
     queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velosity_thread); //Sets up sampling frequency of the velosity_thread
-    queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::rosCom_thread); //Sets up sampling frequency of the velosity_thread
+    queue.call_every(200, rosCom_thread); //Sets up sampling frequency of the velosity_thread
     t.reset();
     compass.start(callback(&queue, &EventQueue::dispatch_forever));      //Starts running the compass thread
     velosity.start(callback(&queue, &EventQueue::dispatch_forever));     //Starts running the velosity thread
@@ -130,6 +154,7 @@
         else {
             smart.stop();                                              //If nothing else is happening stop the chair
         }
+
         wait(process);
     }
 }