駿 高世 / Mbed 2 deprecated abu2019_mr1

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?

UserRevisionLine numberNew 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 }