aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myRos.cpp@4:cf1a4e503974, 2018-12-12 (annotated)
- Committer:
- nakedt555
- Date:
- Wed Dec 12 03:35:52 2018 +0000
- Revision:
- 4:cf1a4e503974
- Parent:
- 3:a45557a0dcb8
- Child:
- 8:80708bacb5b5
toriaezu ok;
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 | 1:bdd17feaa4ce | 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 | 1:bdd17feaa4ce | 72 | if(ack_from_pc_ == false){ |
nakedt555 | 3:a45557a0dcb8 | 73 | //Send initialize |
nakedt555 | 1:bdd17feaa4ce | 74 | court_color_publisher(); |
nakedt555 | 1:bdd17feaa4ce | 75 | initial_pose_publisher(); |
nakedt555 | 1:bdd17feaa4ce | 76 | } |
nakedt555 | 1:bdd17feaa4ce | 77 | |
nakedt555 | 4:cf1a4e503974 | 78 | if(drift_sub_flag_ == true){ |
nakedt555 | 4:cf1a4e503974 | 79 | set_drift(drift_); |
nakedt555 | 4:cf1a4e503974 | 80 | drift_sub_flag_ = false; |
nakedt555 | 4:cf1a4e503974 | 81 | } |
nakedt555 | 4:cf1a4e503974 | 82 | |
nakedt555 | 1:bdd17feaa4ce | 83 | //Send current position(odom -> base_footprint) |
nakedt555 | 3:a45557a0dcb8 | 84 | pose_publisher(); |
nakedt555 | 3:a45557a0dcb8 | 85 | |
nakedt555 | 1:bdd17feaa4ce | 86 | nh_.spinOnce(); |
nakedt555 | 0:10f626cf3ec4 | 87 | } |