駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

myRos.cpp@1:bdd17feaa4ce, 2018-12-07 (annotated)

Committer:
nakedt555
Date:
Fri Dec 07 20:59:56 2018 +0000
Revision:
1:bdd17feaa4ce
Parent:
0:10f626cf3ec4
Child:
3:a45557a0dcb8
create odom_h;

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 1:bdd17feaa4ce 15 ack_from_pc_ = false;
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 1:bdd17feaa4ce 34 static ros::Publisher initial_pose_pub("nucleo/initial_point", &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 1:bdd17feaa4ce 41
nakedt555 1:bdd17feaa4ce 42 initial_pose = 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 1:bdd17feaa4ce 48 static ros::Publisher pose_pub("nucleo/point", &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 1:bdd17feaa4ce 56 pose = pose_.get_point_msgs();
nakedt555 1:bdd17feaa4ce 57 pose_pub.publish(&pose);
nakedt555 1:bdd17feaa4ce 58 }
nakedt555 1:bdd17feaa4ce 59
nakedt555 1:bdd17feaa4ce 60 //loopで回す
nakedt555 1:bdd17feaa4ce 61 //
nakedt555 1:bdd17feaa4ce 62 void My_Ros::set_amcl_parameter(){
nakedt555 1:bdd17feaa4ce 63 //Send initialize
nakedt555 1:bdd17feaa4ce 64 if(ack_from_pc_ == false){
nakedt555 1:bdd17feaa4ce 65 court_color_publisher();
nakedt555 1:bdd17feaa4ce 66 initial_pose_publisher();
nakedt555 1:bdd17feaa4ce 67 }
nakedt555 1:bdd17feaa4ce 68
nakedt555 1:bdd17feaa4ce 69 //Send current position(odom -> base_footprint)
nakedt555 1:bdd17feaa4ce 70 pose_publisher();
nakedt555 1:bdd17feaa4ce 71 }
nakedt555 1:bdd17feaa4ce 72
nakedt555 1:bdd17feaa4ce 73 //This function run every 10 msec at main
nakedt555 1:bdd17feaa4ce 74 void My_Ros::loop(){
nakedt555 1:bdd17feaa4ce 75 set_amcl_parameter();
nakedt555 1:bdd17feaa4ce 76 nh_.spinOnce();
nakedt555 0:10f626cf3ec4 77 }