aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: myRos.cpp
- Revision:
- 1:bdd17feaa4ce
- Parent:
- 0:10f626cf3ec4
- Child:
- 3:a45557a0dcb8
--- a/myRos.cpp Thu Dec 06 10:22:27 2018 +0000 +++ b/myRos.cpp Fri Dec 07 20:59:56 2018 +0000 @@ -1,17 +1,23 @@ #include "myRos.h" -void myRos::initialize(){ -// std_msgs::Bool court_color; -// ros::Publisher court_color_pub("court_color", &court_color); -// nh_.advertise(court_color_pub); -// -// court_color.data = court_color_; -// court_color_pub.publish(&court_color); +bool ack_from_pc_; + +//メンバ関数でないことに注意 +void ack_from_pc_cb(const std_msgs::Empty& ack) { + ack_from_pc_ = true; } -void myRos::publisher(){ +void My_Ros::enable_initialize_amcl(){ + ack_from_pc_ = false; +} + +void My_Ros::initialize(){ + ack_from_pc_ = false; +} + +void My_Ros::court_color_publisher(){ static std_msgs::Bool court_color; - static ros::Publisher court_color_pub("court_color", &court_color); + static ros::Publisher court_color_pub("nucleo/court_color", &court_color); static bool init_flag = false; if(init_flag == false){ @@ -21,4 +27,51 @@ court_color.data = 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_point", &initial_pose); + + static bool init_flag = false; + if(init_flag == false){ + nh_.advertise(initial_pose_pub); + init_flag = true; + } + + initial_pose = 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/point", &pose); + + static bool init_flag = false; + if(init_flag == false){ + nh_.advertise(pose_pub); + init_flag = true; + } + + pose = pose_.get_point_msgs(); + pose_pub.publish(&pose); +} + +//loopで回す +// +void My_Ros::set_amcl_parameter(){ + //Send initialize + if(ack_from_pc_ == false){ + court_color_publisher(); + initial_pose_publisher(); + } + + //Send current position(odom -> base_footprint) + pose_publisher(); +} + +//This function run every 10 msec at main +void My_Ros::loop(){ + set_amcl_parameter(); + nh_.spinOnce(); } \ No newline at end of file