aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myRos.cpp
- Committer:
- nakedt555
- Date:
- 2018-12-12
- Revision:
- 4:cf1a4e503974
- Parent:
- 3:a45557a0dcb8
- Child:
- 8:80708bacb5b5
File content as of revision 4:cf1a4e503974:
#include "myRos.h" static bool ack_from_pc_; static Vec3f drift_; static bool drift_sub_flag_; //メンバ関数でないことに注意 void ack_from_pc_cb(const std_msgs::Empty& ack) { ack_from_pc_ = true; } void drift_sub_cb(const geometry_msgs::Point& drift){ drift_.x(drift.x); drift_.y(drift.y); drift_.angle(drift.z); drift_sub_flag_ = true; } void My_Ros::enable_initialize_amcl(){ ack_from_pc_ = false; drift_sub_flag_ = false; } void My_Ros::initialize(){ ack_from_pc_ = true; } void My_Ros::court_color_publisher(){ static std_msgs::Bool court_color; static ros::Publisher court_color_pub("nucleo/court_color", &court_color); static bool init_flag = false; if(init_flag == false){ nh_.advertise(court_color_pub); init_flag = true; } court_color.data = get_court_color(); court_color_pub.publish(&court_color); } void My_Ros::initial_pose_publisher(){ static geometry_msgs::Point initial_pose; static ros::Publisher initial_pose_pub("nucleo/initial_pose", &initial_pose); static bool init_flag = false; if(init_flag == false){ nh_.advertise(initial_pose_pub); init_flag = true; } initial_pose = get_initial_pose().get_point_msgs(); initial_pose_pub.publish(&initial_pose); } void My_Ros::pose_publisher(){ static geometry_msgs::Point pose; static ros::Publisher pose_pub("nucleo/pose", &pose); static bool init_flag = false; if(init_flag == false){ nh_.advertise(pose_pub); init_flag = true; } pose = get_pose().get_point_msgs(); pose_pub.publish(&pose); } //This function run every 10 msec at main void My_Ros::loop(){ if(ack_from_pc_ == false){ //Send initialize court_color_publisher(); initial_pose_publisher(); } if(drift_sub_flag_ == true){ set_drift(drift_); drift_sub_flag_ = false; } //Send current position(odom -> base_footprint) pose_publisher(); nh_.spinOnce(); }