aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myRos.cpp
- Committer:
- nakedt555
- Date:
- 2019-08-30
- Revision:
- 11:375d474b8522
- Parent:
- 10:3b47050b1652
File content as of revision 11:375d474b8522:
#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(); }