Diff: myRos.cpp

Revision:

4:cf1a4e503974

Parent:

3:a45557a0dcb8

Child:

8:80708bacb5b5

--- a/myRos.cpp	Tue Dec 11 17:51:47 2018 +0000
+++ b/myRos.cpp	Wed Dec 12 03:35:52 2018 +0000
@@ -1,14 +1,24 @@
 #include "myRos.h"
 
-bool ack_from_pc_;
+static bool ack_from_pc_;
+static Vec3f drift_;
+static bool drift_sub_flag_;
 
 //メンバ関数でないことに注意
 void ack_from_pc_cb(const std_msgs::Empty& ack) {
     ack_from_pc_ = true;
 }
 
+void drift_sub_cb(const geometry_msgs::Point& drift){
+    drift_.x(drift.x);
+    drift_.y(drift.y);
+    drift_.angle(drift.z);   
+    drift_sub_flag_ = true;
+}
+
 void My_Ros::enable_initialize_amcl(){
     ack_from_pc_ = false;
+    drift_sub_flag_ = false;
 }
 
 void My_Ros::initialize(){
@@ -25,7 +35,7 @@
         init_flag = true;
     }
    
-    court_color.data = court_color_;
+    court_color.data = get_court_color();
     court_color_pub.publish(&court_color);
 }
 
@@ -65,6 +75,11 @@
         initial_pose_publisher();
     }
     
+    if(drift_sub_flag_ == true){
+        set_drift(drift_);
+        drift_sub_flag_ = false;
+    }
+    
     //Send current position(odom -> base_footprint)
     pose_publisher();