myRos.cpp

00001 #include "myRos.h"
00002 
00003 static bool ack_from_pc_;
00004 static Vec3f drift_;
00005 static bool drift_sub_flag_;
00006 
00007 //メンバ関数でないことに注意
00008 void ack_from_pc_cb(const std_msgs::Empty& ack) {
00009     ack_from_pc_ = true;
00010 }
00011 
00012 void drift_sub_cb(const geometry_msgs::Point& drift){
00013     drift_.x(drift.x);
00014     drift_.y(drift.y);
00015     drift_.angle(drift.z);   
00016     drift_sub_flag_ = true;
00017 }
00018 
00019 void My_Ros::enable_initialize_amcl(){
00020     ack_from_pc_ = false;
00021     drift_sub_flag_ = false;
00022 }
00023 
00024 void My_Ros::initialize(){
00025     ack_from_pc_ = true;
00026 }
00027 
00028 void My_Ros::court_color_publisher(){
00029     static std_msgs::Bool court_color;
00030     static ros::Publisher court_color_pub("nucleo/court_color", &court_color);
00031     
00032     static bool init_flag = false;
00033     if(init_flag == false){
00034         nh_.advertise(court_color_pub);    
00035         init_flag = true;
00036     }
00037    
00038     court_color.data = get_court_color();
00039     court_color_pub.publish(&court_color);
00040 }
00041 
00042 void My_Ros::initial_pose_publisher(){
00043     static geometry_msgs::Point initial_pose;
00044     static ros::Publisher initial_pose_pub("nucleo/initial_pose", &initial_pose);
00045     
00046     static bool init_flag = false;
00047     if(init_flag == false){
00048         nh_.advertise(initial_pose_pub);    
00049         init_flag = true;
00050     }
00051  
00052     initial_pose = get_initial_pose().get_point_msgs();
00053     initial_pose_pub.publish(&initial_pose);
00054 }
00055 
00056 void My_Ros::pose_publisher(){
00057     static geometry_msgs::Point pose;
00058     static ros::Publisher pose_pub("nucleo/pose", &pose);
00059     
00060     static bool init_flag = false;
00061     if(init_flag == false){
00062         nh_.advertise(pose_pub);    
00063         init_flag = true;
00064     }
00065 
00066     pose = get_pose().get_point_msgs();
00067     pose_pub.publish(&pose);
00068 }
00069 
00070 //This function run every 10 msec at main
00071 void My_Ros::loop(){
00072     static bool plev_ack_from_pc = false;
00073     
00074     if(ack_from_pc_ == false){
00075         //Send initialize
00076         court_color_publisher();
00077         initial_pose_publisher();
00078     }
00079     
00080     if(ack_from_pc_ == true && plev_ack_from_pc == false){//初期位置の登録後,ROSからのACKが来た時に実行される
00081         reset_odometry();
00082     }
00083     
00084     plev_ack_from_pc = ack_from_pc_;
00085     
00086     if(drift_sub_flag_ == true){
00087         set_drift(drift_);
00088         drift_sub_flag_ = false;
00089     }
00090     
00091     //Send current position(odom -> base_footprint)
00092     pose_publisher(); 
00093     
00094     nh_.spinOnce();
00095 }