Diff: myRos.cpp

Revision:

1:bdd17feaa4ce

Parent:

0:10f626cf3ec4

Child:

3:a45557a0dcb8

--- a/myRos.cpp	Thu Dec 06 10:22:27 2018 +0000
+++ b/myRos.cpp	Fri Dec 07 20:59:56 2018 +0000
@@ -1,17 +1,23 @@
 #include "myRos.h"
 
-void myRos::initialize(){
-//    std_msgs::Bool court_color;
-//    ros::Publisher court_color_pub("court_color", &court_color);
-//    nh_.advertise(court_color_pub);
-//    
-//    court_color.data = court_color_;
-//    court_color_pub.publish(&court_color);
+bool ack_from_pc_;
+
+//メンバ関数でないことに注意
+void ack_from_pc_cb(const std_msgs::Empty& ack) {
+    ack_from_pc_ = true;
 }
 
-void myRos::publisher(){
+void My_Ros::enable_initialize_amcl(){
+    ack_from_pc_ = false;
+}
+
+void My_Ros::initialize(){
+    ack_from_pc_ = false;
+}
+
+void My_Ros::court_color_publisher(){
     static std_msgs::Bool court_color;
-    static ros::Publisher court_color_pub("court_color", &court_color);
+    static ros::Publisher court_color_pub("nucleo/court_color", &court_color);
     
     static bool init_flag = false;
     if(init_flag == false){
@@ -21,4 +27,51 @@
    
     court_color.data = court_color_;
     court_color_pub.publish(&court_color);
+}
+
+void My_Ros::initial_pose_publisher(){
+    static geometry_msgs::Point initial_pose;
+    static ros::Publisher initial_pose_pub("nucleo/initial_point", &initial_pose);
+    
+    static bool init_flag = false;
+    if(init_flag == false){
+        nh_.advertise(initial_pose_pub);    
+        init_flag = true;
+    }
+   
+    initial_pose = initial_pose_.get_point_msgs();
+    initial_pose_pub.publish(&initial_pose);
+}
+
+void My_Ros::pose_publisher(){
+    static geometry_msgs::Point pose;
+    static ros::Publisher pose_pub("nucleo/point", &pose);
+    
+    static bool init_flag = false;
+    if(init_flag == false){
+        nh_.advertise(pose_pub);    
+        init_flag = true;
+    }
+   
+    pose = pose_.get_point_msgs();
+    pose_pub.publish(&pose);
+}
+
+//loopで回す
+//
+void My_Ros::set_amcl_parameter(){
+     //Send initialize
+    if(ack_from_pc_ == false){
+        court_color_publisher();
+        initial_pose_publisher();
+    }
+    
+    //Send current position(odom -> base_footprint)
+    pose_publisher();   
+}
+
+//This function run every 10 msec at main
+void My_Ros::loop(){
+    set_amcl_parameter();
+    nh_.spinOnce();
 }
\ No newline at end of file