| 73 | }; |
| 74 | |
| 75 | InputManager::InputManager(ros::NodeHandle nh) : nh_(nh), tf_listener_{tf_buffer_} |
| 76 | { |
| 77 | nh_.param<float>("min_odom_distance", minOdomDistance_, 0.5); |
| 78 | nh_.param<float>("min_sloam_z", minSLOAMAltitude_, 0.5); |
| 79 | maxQueueSize_ = nh_.param("maxQueueSize", 30); |
| 80 | odomFreqFilter_ = nh_.param("odom_freq_filter", 20); |
| 81 | publishTf_ = nh_.param("publish_tf", true); |
| 82 | nh_.param<std::string>("robot_frame_id", robot_frame_id_, "robot"); |
| 83 | nh_.param<std::string>("odom_frame_id", odom_frame_id_, "odom"); |
| 84 | nh_.param<std::string>("map_frame_id", map_frame_id_, "map"); |
| 85 | nh_.param<std::string>("cloud_topic", cloud_topic_, "cloud"); |
| 86 | nh_.param<std::string>("odom_topic", odom_topic_, "/quadrotor/vio/odom"); |
| 87 | |
| 88 | pubPose_ = |
| 89 | nh_.advertise<geometry_msgs::PoseStamped>("debug/ref_frame_sloam_pose", 10); |
| 90 | OdomSub_ = nh_.subscribe(odom_topic_, 10, &InputManager::OdomCb_, this); |
| 91 | PCSub_ = nh_.subscribe(cloud_topic_, 10, &InputManager::PCCb_, this); |
| 92 | |
| 93 | auto sloam_ptr = boost::make_shared<sloam::SLOAMNode>(nh_); |
| 94 | sloam_ = std::move(sloam_ptr); |
| 95 | firstOdom_ = true; |
| 96 | odomCounter_ = 0; |
| 97 | |
| 98 | ROS_INFO("SLOAM initialized"); |
| 99 | } |
| 100 | |
| 101 | void InputManager::OdomCb_(const nav_msgs::OdometryConstPtr &odom_msg) |
| 102 | { |
nothing calls this directly
no outgoing calls
no test coverage detected