駿 高世 / Mbed 2 deprecated abu2019_mr1

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?

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