aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
myRos.h@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?
User | Revision | Line number | New contents of line |
---|---|---|---|
nakedt555 | 0:10f626cf3ec4 | 1 | #ifndef _MY_ROS_H_ |
nakedt555 | 0:10f626cf3ec4 | 2 | #define _MY_ROS_H_ |
nakedt555 | 0:10f626cf3ec4 | 3 | |
nakedt555 | 1:bdd17feaa4ce | 4 | #include "type.h" |
nakedt555 | 1:bdd17feaa4ce | 5 | #include "odom.h" |
nakedt555 | 1:bdd17feaa4ce | 6 | |
nakedt555 | 1:bdd17feaa4ce | 7 | #include <mbed.h> |
nakedt555 | 1:bdd17feaa4ce | 8 | #include <ros.h> |
nakedt555 | 1:bdd17feaa4ce | 9 | #include <ros/time.h> |
nakedt555 | 1:bdd17feaa4ce | 10 | #include <std_msgs/Bool.h> |
nakedt555 | 1:bdd17feaa4ce | 11 | #include <geometry_msgs/Point.h> |
nakedt555 | 1:bdd17feaa4ce | 12 | #include <geometry_msgs/PointStamped.h> |
nakedt555 | 1:bdd17feaa4ce | 13 | #include <std_msgs/Empty.h> |
nakedt555 | 0:10f626cf3ec4 | 14 | |
nakedt555 | 0:10f626cf3ec4 | 15 | #define COURT_RED false |
nakedt555 | 0:10f626cf3ec4 | 16 | #define COURT_BLUE true |
nakedt555 | 0:10f626cf3ec4 | 17 | |
nakedt555 | 1:bdd17feaa4ce | 18 | void ack_from_pc_cb(const std_msgs::Empty&); |
nakedt555 | 1:bdd17feaa4ce | 19 | |
nakedt555 | 1:bdd17feaa4ce | 20 | class My_Ros : public Odom_Abstract |
nakedt555 | 0:10f626cf3ec4 | 21 | { |
nakedt555 | 1:bdd17feaa4ce | 22 | public: |
nakedt555 | 1:bdd17feaa4ce | 23 | |
nakedt555 | 0:10f626cf3ec4 | 24 | private: |
nakedt555 | 1:bdd17feaa4ce | 25 | ros::NodeHandle nh_; |
nakedt555 | 1:bdd17feaa4ce | 26 | bool court_color_; |
nakedt555 | 1:bdd17feaa4ce | 27 | Vec3f initial_pose_; |
nakedt555 | 1:bdd17feaa4ce | 28 | Vec3f pose_; //current position(odom -> base_footprint) |
nakedt555 | 0:10f626cf3ec4 | 29 | |
nakedt555 | 0:10f626cf3ec4 | 30 | public: |
nakedt555 | 1:bdd17feaa4ce | 31 | //Constructor |
nakedt555 | 1:bdd17feaa4ce | 32 | My_Ros() : court_color_(COURT_RED){ |
nakedt555 | 1:bdd17feaa4ce | 33 | //ROS node initialize |
nakedt555 | 1:bdd17feaa4ce | 34 | nh_.getHardware()->setBaud(115200); |
nakedt555 | 1:bdd17feaa4ce | 35 | nh_.initNode(); |
nakedt555 | 1:bdd17feaa4ce | 36 | |
nakedt555 | 1:bdd17feaa4ce | 37 | initialize(); |
nakedt555 | 1:bdd17feaa4ce | 38 | } |
nakedt555 | 0:10f626cf3ec4 | 39 | |
nakedt555 | 1:bdd17feaa4ce | 40 | //Loop function |
nakedt555 | 1:bdd17feaa4ce | 41 | void loop(); |
nakedt555 | 1:bdd17feaa4ce | 42 | |
nakedt555 | 1:bdd17feaa4ce | 43 | //Setter |
nakedt555 | 1:bdd17feaa4ce | 44 | void set_initial_pose(Vec3f initial_pose){ |
nakedt555 | 1:bdd17feaa4ce | 45 | initial_pose_ = initial_pose; |
nakedt555 | 1:bdd17feaa4ce | 46 | } |
nakedt555 | 1:bdd17feaa4ce | 47 | void set_court_color(bool court_color){ |
nakedt555 | 1:bdd17feaa4ce | 48 | court_color_ = court_color; |
nakedt555 | 1:bdd17feaa4ce | 49 | } |
nakedt555 | 1:bdd17feaa4ce | 50 | template<typename SubscriberT> |
nakedt555 | 1:bdd17feaa4ce | 51 | void set_subscliber(SubscriberT& s){ |
nakedt555 | 1:bdd17feaa4ce | 52 | nh_.subscribe(s); |
nakedt555 | 1:bdd17feaa4ce | 53 | } |
nakedt555 | 1:bdd17feaa4ce | 54 | |
nakedt555 | 1:bdd17feaa4ce | 55 | //amcl parameter の初期化 |
nakedt555 | 1:bdd17feaa4ce | 56 | void enable_initialize_amcl(); |
nakedt555 | 1:bdd17feaa4ce | 57 | |
nakedt555 | 0:10f626cf3ec4 | 58 | private: |
nakedt555 | 1:bdd17feaa4ce | 59 | //Initialise |
nakedt555 | 1:bdd17feaa4ce | 60 | void initialize(); |
nakedt555 | 1:bdd17feaa4ce | 61 | |
nakedt555 | 1:bdd17feaa4ce | 62 | //Publisher |
nakedt555 | 1:bdd17feaa4ce | 63 | void court_color_publisher(); |
nakedt555 | 1:bdd17feaa4ce | 64 | void initial_pose_publisher(); |
nakedt555 | 1:bdd17feaa4ce | 65 | void pose_publisher(); |
nakedt555 | 1:bdd17feaa4ce | 66 | |
nakedt555 | 1:bdd17feaa4ce | 67 | //Amcl parameter |
nakedt555 | 1:bdd17feaa4ce | 68 | void set_amcl_parameter(); |
nakedt555 | 0:10f626cf3ec4 | 69 | }; |
nakedt555 | 0:10f626cf3ec4 | 70 | |
nakedt555 | 0:10f626cf3ec4 | 71 | #endif |