駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

myRos.cpp@10:3b47050b1652, 2019-03-22 (annotated)

Committer:
nakedt555
Date:
Fri Mar 22 12:29:18 2019 +0000
Revision:
10:3b47050b1652
Parent:
8:80708bacb5b5
Child:
11:375d474b8522
check ok

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 10:3b47050b1652 54
nakedt555 10:3b47050b1652 55 // Vec3f tmp = get_initial_pose();
nakedt555 10:3b47050b1652 56 // tmp.z(-tmp.z());
nakedt555 10:3b47050b1652 57 // initial_pose = tmp.get_point_msgs();
nakedt555 10:3b47050b1652 58 // initial_pose_pub.publish(&initial_pose);
nakedt555 1:bdd17feaa4ce 59 }
nakedt555 1:bdd17feaa4ce 60
nakedt555 1:bdd17feaa4ce 61 void My_Ros::pose_publisher(){
nakedt555 1:bdd17feaa4ce 62 static geometry_msgs::Point pose;
nakedt555 3:a45557a0dcb8 63 static ros::Publisher pose_pub("nucleo/pose", &pose);
nakedt555 1:bdd17feaa4ce 64
nakedt555 1:bdd17feaa4ce 65 static bool init_flag = false;
nakedt555 1:bdd17feaa4ce 66 if(init_flag == false){
nakedt555 1:bdd17feaa4ce 67 nh_.advertise(pose_pub);
nakedt555 1:bdd17feaa4ce 68 init_flag = true;
nakedt555 1:bdd17feaa4ce 69 }
nakedt555 10:3b47050b1652 70
nakedt555 3:a45557a0dcb8 71 pose = get_pose().get_point_msgs();
nakedt555 1:bdd17feaa4ce 72 pose_pub.publish(&pose);
nakedt555 10:3b47050b1652 73
nakedt555 10:3b47050b1652 74 // Vec3f tmp = get_pose();
nakedt555 10:3b47050b1652 75 // tmp.z(-tmp.z());
nakedt555 10:3b47050b1652 76 // pose = tmp.get_point_msgs();
nakedt555 10:3b47050b1652 77 // pose_pub.publish(&pose);
nakedt555 1:bdd17feaa4ce 78 }
nakedt555 1:bdd17feaa4ce 79
nakedt555 3:a45557a0dcb8 80 //This function run every 10 msec at main
nakedt555 3:a45557a0dcb8 81 void My_Ros::loop(){
nakedt555 8:80708bacb5b5 82 static bool plev_ack_from_pc = false;
nakedt555 8:80708bacb5b5 83
nakedt555 1:bdd17feaa4ce 84 if(ack_from_pc_ == false){
nakedt555 3:a45557a0dcb8 85 //Send initialize
nakedt555 1:bdd17feaa4ce 86 court_color_publisher();
nakedt555 1:bdd17feaa4ce 87 initial_pose_publisher();
nakedt555 1:bdd17feaa4ce 88 }
nakedt555 1:bdd17feaa4ce 89
nakedt555 8:80708bacb5b5 90 if(ack_from_pc_ == true && plev_ack_from_pc == false){//初期位置の登録後,ROSからのACKが来た時に実行される
nakedt555 8:80708bacb5b5 91 reset_odometry();
nakedt555 8:80708bacb5b5 92 }
nakedt555 8:80708bacb5b5 93
nakedt555 8:80708bacb5b5 94 plev_ack_from_pc = ack_from_pc_;
nakedt555 8:80708bacb5b5 95
nakedt555 4:cf1a4e503974 96 if(drift_sub_flag_ == true){
nakedt555 4:cf1a4e503974 97 set_drift(drift_);
nakedt555 4:cf1a4e503974 98 drift_sub_flag_ = false;
nakedt555 4:cf1a4e503974 99 }
nakedt555 4:cf1a4e503974 100
nakedt555 1:bdd17feaa4ce 101 //Send current position(odom -> base_footprint)
nakedt555 3:a45557a0dcb8 102 pose_publisher();
nakedt555 3:a45557a0dcb8 103
nakedt555 1:bdd17feaa4ce 104 nh_.spinOnce();
nakedt555 0:10f626cf3ec4 105 }