with pid foward right left foward

Dependencies:   wheelchaircontrolRosCom

Fork of wheelchaircontrolrealtime by ryan lin

Files at this revision

API Documentation at this revision

Comitter:
jvfausto
Date:
Sat Nov 03 20:36:36 2018 +0000
Parent:
9:1081ebfe3db0
Commit message:
Now with twist and Odom messages

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
wheelchaircontrol.lib Show annotated file Show diff for this revision Revisions of this file
diff -r 1081ebfe3db0 -r beaa2ddfef32 main.cpp
--- 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);
     }
 }
diff -r 1081ebfe3db0 -r beaa2ddfef32 wheelchaircontrol.lib
--- a/wheelchaircontrol.lib	Fri Nov 02 02:28:51 2018 +0000
+++ b/wheelchaircontrol.lib	Sat Nov 03 20:36:36 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/jvfausto/code/wheelchaircontrolRosCom/#6c5b4b82f874
+https://os.mbed.com/users/jvfausto/code/wheelchaircontrolRosCom/#a6a1ccf14c63