aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myRos.cpp@3:a45557a0dcb8, 2018-12-11 (annotated)
- Committer:
- nakedt555
- Date:
- Tue Dec 11 17:51:47 2018 +0000
- Revision:
- 3:a45557a0dcb8
- Parent:
- 1:bdd17feaa4ce
- Child:
- 4:cf1a4e503974
12/10iikanji
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 | 1:bdd17feaa4ce | 3 | bool ack_from_pc_; |
nakedt555 | 1:bdd17feaa4ce | 4 | |
nakedt555 | 1:bdd17feaa4ce | 5 | //メンバ関数でないことに注意 |
nakedt555 | 1:bdd17feaa4ce | 6 | void ack_from_pc_cb(const std_msgs::Empty& ack) { |
nakedt555 | 1:bdd17feaa4ce | 7 | ack_from_pc_ = true; |
nakedt555 | 0:10f626cf3ec4 | 8 | } |
nakedt555 | 0:10f626cf3ec4 | 9 | |
nakedt555 | 1:bdd17feaa4ce | 10 | void My_Ros::enable_initialize_amcl(){ |
nakedt555 | 1:bdd17feaa4ce | 11 | ack_from_pc_ = false; |
nakedt555 | 1:bdd17feaa4ce | 12 | } |
nakedt555 | 1:bdd17feaa4ce | 13 | |
nakedt555 | 1:bdd17feaa4ce | 14 | void My_Ros::initialize(){ |
nakedt555 | 3:a45557a0dcb8 | 15 | ack_from_pc_ = true; |
nakedt555 | 1:bdd17feaa4ce | 16 | } |
nakedt555 | 1:bdd17feaa4ce | 17 | |
nakedt555 | 1:bdd17feaa4ce | 18 | void My_Ros::court_color_publisher(){ |
nakedt555 | 0:10f626cf3ec4 | 19 | static std_msgs::Bool court_color; |
nakedt555 | 1:bdd17feaa4ce | 20 | static ros::Publisher court_color_pub("nucleo/court_color", &court_color); |
nakedt555 | 0:10f626cf3ec4 | 21 | |
nakedt555 | 0:10f626cf3ec4 | 22 | static bool init_flag = false; |
nakedt555 | 0:10f626cf3ec4 | 23 | if(init_flag == false){ |
nakedt555 | 0:10f626cf3ec4 | 24 | nh_.advertise(court_color_pub); |
nakedt555 | 0:10f626cf3ec4 | 25 | init_flag = true; |
nakedt555 | 0:10f626cf3ec4 | 26 | } |
nakedt555 | 0:10f626cf3ec4 | 27 | |
nakedt555 | 0:10f626cf3ec4 | 28 | court_color.data = court_color_; |
nakedt555 | 0:10f626cf3ec4 | 29 | court_color_pub.publish(&court_color); |
nakedt555 | 1:bdd17feaa4ce | 30 | } |
nakedt555 | 1:bdd17feaa4ce | 31 | |
nakedt555 | 1:bdd17feaa4ce | 32 | void My_Ros::initial_pose_publisher(){ |
nakedt555 | 1:bdd17feaa4ce | 33 | static geometry_msgs::Point initial_pose; |
nakedt555 | 3:a45557a0dcb8 | 34 | static ros::Publisher initial_pose_pub("nucleo/initial_pose", &initial_pose); |
nakedt555 | 1:bdd17feaa4ce | 35 | |
nakedt555 | 1:bdd17feaa4ce | 36 | static bool init_flag = false; |
nakedt555 | 1:bdd17feaa4ce | 37 | if(init_flag == false){ |
nakedt555 | 1:bdd17feaa4ce | 38 | nh_.advertise(initial_pose_pub); |
nakedt555 | 1:bdd17feaa4ce | 39 | init_flag = true; |
nakedt555 | 1:bdd17feaa4ce | 40 | } |
nakedt555 | 3:a45557a0dcb8 | 41 | |
nakedt555 | 3:a45557a0dcb8 | 42 | initial_pose = get_initial_pose().get_point_msgs(); |
nakedt555 | 1:bdd17feaa4ce | 43 | initial_pose_pub.publish(&initial_pose); |
nakedt555 | 1:bdd17feaa4ce | 44 | } |
nakedt555 | 1:bdd17feaa4ce | 45 | |
nakedt555 | 1:bdd17feaa4ce | 46 | void My_Ros::pose_publisher(){ |
nakedt555 | 1:bdd17feaa4ce | 47 | static geometry_msgs::Point pose; |
nakedt555 | 3:a45557a0dcb8 | 48 | static ros::Publisher pose_pub("nucleo/pose", &pose); |
nakedt555 | 1:bdd17feaa4ce | 49 | |
nakedt555 | 1:bdd17feaa4ce | 50 | static bool init_flag = false; |
nakedt555 | 1:bdd17feaa4ce | 51 | if(init_flag == false){ |
nakedt555 | 1:bdd17feaa4ce | 52 | nh_.advertise(pose_pub); |
nakedt555 | 1:bdd17feaa4ce | 53 | init_flag = true; |
nakedt555 | 1:bdd17feaa4ce | 54 | } |
nakedt555 | 1:bdd17feaa4ce | 55 | |
nakedt555 | 3:a45557a0dcb8 | 56 | pose = get_pose().get_point_msgs(); |
nakedt555 | 1:bdd17feaa4ce | 57 | pose_pub.publish(&pose); |
nakedt555 | 1:bdd17feaa4ce | 58 | } |
nakedt555 | 1:bdd17feaa4ce | 59 | |
nakedt555 | 3:a45557a0dcb8 | 60 | //This function run every 10 msec at main |
nakedt555 | 3:a45557a0dcb8 | 61 | void My_Ros::loop(){ |
nakedt555 | 1:bdd17feaa4ce | 62 | if(ack_from_pc_ == false){ |
nakedt555 | 3:a45557a0dcb8 | 63 | //Send initialize |
nakedt555 | 1:bdd17feaa4ce | 64 | court_color_publisher(); |
nakedt555 | 1:bdd17feaa4ce | 65 | initial_pose_publisher(); |
nakedt555 | 1:bdd17feaa4ce | 66 | } |
nakedt555 | 1:bdd17feaa4ce | 67 | |
nakedt555 | 1:bdd17feaa4ce | 68 | //Send current position(odom -> base_footprint) |
nakedt555 | 3:a45557a0dcb8 | 69 | pose_publisher(); |
nakedt555 | 3:a45557a0dcb8 | 70 | |
nakedt555 | 1:bdd17feaa4ce | 71 | nh_.spinOnce(); |
nakedt555 | 0:10f626cf3ec4 | 72 | } |