File content as of revision 12:f726eb78b54c:

#include "myRos.h"

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(){
    ack_from_pc_ = true;
}

void My_Ros::court_color_publisher(){
    static std_msgs::Bool court_color;
    static ros::Publisher court_color_pub("nucleo/court_color", &court_color);
    
    static bool init_flag = false;
    if(init_flag == false){
        nh_.advertise(court_color_pub);    
        init_flag = true;
    }
   
    court_color.data = get_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_pose", &initial_pose);
    
    static bool init_flag = false;
    if(init_flag == false){
        nh_.advertise(initial_pose_pub);    
        init_flag = true;
    }
 
    initial_pose = get_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/pose", &pose);
    
    static bool init_flag = false;
    if(init_flag == false){
        nh_.advertise(pose_pub);    
        init_flag = true;
    }

    pose = get_pose().get_point_msgs();
    pose_pub.publish(&pose);
}

//This function run every 10 msec at main
void My_Ros::loop(){
    static bool plev_ack_from_pc = false;
    
    if(ack_from_pc_ == false){
        //Send initialize
        court_color_publisher();
        initial_pose_publisher();
    }
    
    if(ack_from_pc_ == true && plev_ack_from_pc == false){//初期位置の登録後,ROSからのACKが来た時に実行される
        reset_odometry();
    }
    
    plev_ack_from_pc = ack_from_pc_;
    
    if(drift_sub_flag_ == true){
        set_drift(drift_);
        drift_sub_flag_ = false;
    }
    
    //Send current position(odom -> base_footprint)
    pose_publisher(); 
    
    nh_.spinOnce();
}