aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myRos.cpp@12:f726eb78b54c, 2019-08-30 (annotated)
- Committer:
- nakedt555
- Date:
- Fri Aug 30 07:55:38 2019 +0000
- Revision:
- 12:f726eb78b54c
- Parent:
- 11:375d474b8522
aaa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nakedt555 | 0:10f626cf3ec4 | 1 | #include "myRos.h" |
nakedt555 | 0:10f626cf3ec4 | 2 | |
nakedt555 | 4:cf1a4e503974 | 3 | static bool ack_from_pc_; |
nakedt555 | 4:cf1a4e503974 | 4 | static Vec3f drift_; |
nakedt555 | 4:cf1a4e503974 | 5 | static bool drift_sub_flag_; |
nakedt555 | 1:bdd17feaa4ce | 6 | |
nakedt555 | 1:bdd17feaa4ce | 7 | //メンバ関数でないことに注意 |
nakedt555 | 1:bdd17feaa4ce | 8 | void ack_from_pc_cb(const std_msgs::Empty& ack) { |
nakedt555 | 1:bdd17feaa4ce | 9 | ack_from_pc_ = true; |
nakedt555 | 0:10f626cf3ec4 | 10 | } |
nakedt555 | 0:10f626cf3ec4 | 11 | |
nakedt555 | 4:cf1a4e503974 | 12 | void drift_sub_cb(const geometry_msgs::Point& drift){ |
nakedt555 | 4:cf1a4e503974 | 13 | drift_.x(drift.x); |
nakedt555 | 4:cf1a4e503974 | 14 | drift_.y(drift.y); |
nakedt555 | 4:cf1a4e503974 | 15 | drift_.angle(drift.z); |
nakedt555 | 4:cf1a4e503974 | 16 | drift_sub_flag_ = true; |
nakedt555 | 4:cf1a4e503974 | 17 | } |
nakedt555 | 4:cf1a4e503974 | 18 | |
nakedt555 | 1:bdd17feaa4ce | 19 | void My_Ros::enable_initialize_amcl(){ |
nakedt555 | 1:bdd17feaa4ce | 20 | ack_from_pc_ = false; |
nakedt555 | 4:cf1a4e503974 | 21 | drift_sub_flag_ = false; |
nakedt555 | 1:bdd17feaa4ce | 22 | } |
nakedt555 | 1:bdd17feaa4ce | 23 | |
nakedt555 | 1:bdd17feaa4ce | 24 | void My_Ros::initialize(){ |
nakedt555 | 3:a45557a0dcb8 | 25 | ack_from_pc_ = true; |
nakedt555 | 1:bdd17feaa4ce | 26 | } |
nakedt555 | 1:bdd17feaa4ce | 27 | |
nakedt555 | 1:bdd17feaa4ce | 28 | void My_Ros::court_color_publisher(){ |
nakedt555 | 0:10f626cf3ec4 | 29 | static std_msgs::Bool court_color; |
nakedt555 | 1:bdd17feaa4ce | 30 | static ros::Publisher court_color_pub("nucleo/court_color", &court_color); |
nakedt555 | 0:10f626cf3ec4 | 31 | |
nakedt555 | 0:10f626cf3ec4 | 32 | static bool init_flag = false; |
nakedt555 | 0:10f626cf3ec4 | 33 | if(init_flag == false){ |
nakedt555 | 0:10f626cf3ec4 | 34 | nh_.advertise(court_color_pub); |
nakedt555 | 0:10f626cf3ec4 | 35 | init_flag = true; |
nakedt555 | 0:10f626cf3ec4 | 36 | } |
nakedt555 | 0:10f626cf3ec4 | 37 | |
nakedt555 | 4:cf1a4e503974 | 38 | court_color.data = get_court_color(); |
nakedt555 | 0:10f626cf3ec4 | 39 | court_color_pub.publish(&court_color); |
nakedt555 | 1:bdd17feaa4ce | 40 | } |
nakedt555 | 1:bdd17feaa4ce | 41 | |
nakedt555 | 1:bdd17feaa4ce | 42 | void My_Ros::initial_pose_publisher(){ |
nakedt555 | 1:bdd17feaa4ce | 43 | static geometry_msgs::Point initial_pose; |
nakedt555 | 3:a45557a0dcb8 | 44 | static ros::Publisher initial_pose_pub("nucleo/initial_pose", &initial_pose); |
nakedt555 | 1:bdd17feaa4ce | 45 | |
nakedt555 | 1:bdd17feaa4ce | 46 | static bool init_flag = false; |
nakedt555 | 1:bdd17feaa4ce | 47 | if(init_flag == false){ |
nakedt555 | 1:bdd17feaa4ce | 48 | nh_.advertise(initial_pose_pub); |
nakedt555 | 1:bdd17feaa4ce | 49 | init_flag = true; |
nakedt555 | 1:bdd17feaa4ce | 50 | } |
nakedt555 | 3:a45557a0dcb8 | 51 | |
nakedt555 | 3:a45557a0dcb8 | 52 | initial_pose = get_initial_pose().get_point_msgs(); |
nakedt555 | 1:bdd17feaa4ce | 53 | initial_pose_pub.publish(&initial_pose); |
nakedt555 | 1:bdd17feaa4ce | 54 | } |
nakedt555 | 1:bdd17feaa4ce | 55 | |
nakedt555 | 1:bdd17feaa4ce | 56 | void My_Ros::pose_publisher(){ |
nakedt555 | 1:bdd17feaa4ce | 57 | static geometry_msgs::Point pose; |
nakedt555 | 3:a45557a0dcb8 | 58 | static ros::Publisher pose_pub("nucleo/pose", &pose); |
nakedt555 | 1:bdd17feaa4ce | 59 | |
nakedt555 | 1:bdd17feaa4ce | 60 | static bool init_flag = false; |
nakedt555 | 1:bdd17feaa4ce | 61 | if(init_flag == false){ |
nakedt555 | 1:bdd17feaa4ce | 62 | nh_.advertise(pose_pub); |
nakedt555 | 1:bdd17feaa4ce | 63 | init_flag = true; |
nakedt555 | 1:bdd17feaa4ce | 64 | } |
nakedt555 | 10:3b47050b1652 | 65 | |
nakedt555 | 3:a45557a0dcb8 | 66 | pose = get_pose().get_point_msgs(); |
nakedt555 | 1:bdd17feaa4ce | 67 | pose_pub.publish(&pose); |
nakedt555 | 1:bdd17feaa4ce | 68 | } |
nakedt555 | 1:bdd17feaa4ce | 69 | |
nakedt555 | 3:a45557a0dcb8 | 70 | //This function run every 10 msec at main |
nakedt555 | 3:a45557a0dcb8 | 71 | void My_Ros::loop(){ |
nakedt555 | 8:80708bacb5b5 | 72 | static bool plev_ack_from_pc = false; |
nakedt555 | 8:80708bacb5b5 | 73 | |
nakedt555 | 1:bdd17feaa4ce | 74 | if(ack_from_pc_ == false){ |
nakedt555 | 3:a45557a0dcb8 | 75 | //Send initialize |
nakedt555 | 1:bdd17feaa4ce | 76 | court_color_publisher(); |
nakedt555 | 1:bdd17feaa4ce | 77 | initial_pose_publisher(); |
nakedt555 | 1:bdd17feaa4ce | 78 | } |
nakedt555 | 1:bdd17feaa4ce | 79 | |
nakedt555 | 8:80708bacb5b5 | 80 | if(ack_from_pc_ == true && plev_ack_from_pc == false){//初期位置の登録後,ROSからのACKが来た時に実行される |
nakedt555 | 8:80708bacb5b5 | 81 | reset_odometry(); |
nakedt555 | 8:80708bacb5b5 | 82 | } |
nakedt555 | 8:80708bacb5b5 | 83 | |
nakedt555 | 8:80708bacb5b5 | 84 | plev_ack_from_pc = ack_from_pc_; |
nakedt555 | 8:80708bacb5b5 | 85 | |
nakedt555 | 4:cf1a4e503974 | 86 | if(drift_sub_flag_ == true){ |
nakedt555 | 4:cf1a4e503974 | 87 | set_drift(drift_); |
nakedt555 | 4:cf1a4e503974 | 88 | drift_sub_flag_ = false; |
nakedt555 | 4:cf1a4e503974 | 89 | } |
nakedt555 | 4:cf1a4e503974 | 90 | |
nakedt555 | 1:bdd17feaa4ce | 91 | //Send current position(odom -> base_footprint) |
nakedt555 | 3:a45557a0dcb8 | 92 | pose_publisher(); |
nakedt555 | 3:a45557a0dcb8 | 93 | |
nakedt555 | 1:bdd17feaa4ce | 94 | nh_.spinOnce(); |
nakedt555 | 0:10f626cf3ec4 | 95 | } |