Node.h

Go to the documentation of this file.

#pragma once

#include "Parameters.h"
#include "ParticleFilter.h"  

#include <rosinrange_msg/range_pose.h>
#include <tf/transform_broadcaster.h>

namespace amcl3d
{
class Node
{
public:
  explicit Node();
  virtual ~Node();

  void spin();

private:
  void publishMapPointCloud(const ros::TimerEvent&);
  void publishGridSlice(const ros::TimerEvent&);
  void publishGridTf(const ros::TimerEvent&);
  void publishParticles();

  void pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg);
  void odomCallback(const geometry_msgs::TransformStampedConstPtr& msg);
  void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
  void rangeCallback(const rosinrange_msg::range_poseConstPtr& msg);

  bool checkUpdateThresholds();

  void setInitialPose(const tf::Transform& init_pose, const float x_dev, const float y_dev, const float z_dev,
                      const float a_dev);

  double getYawFromTf(const tf::Transform& tf);

  void rvizMarkerPublish(const uint32_t anchor_id, const float r, const geometry_msgs::Point& uav,
                         const geometry_msgs::Point& anchor);

  Parameters parameters_;
  Grid3d grid3d_;
  ParticleFilter pf_;

  ros::NodeHandle nh_;

  sensor_msgs::PointCloud2 map_point_cloud_msg_;
  ros::Publisher map_point_cloud_pub_;
  ros::Timer map_point_cloud_pub_timer_;

  nav_msgs::OccupancyGrid grid_slice_msg_;
  ros::Publisher grid_slice_pub_;
  ros::Timer grid_slice_pub_timer_;

  tf::StampedTransform grid_to_world_tf_;
  ros::Timer grid_to_world_tf_timer_;

  bool is_odom_{ false };
  bool amcl_out_{ false };
  double roll_{ 0 }, pitch_{ 0 };

  std::vector<Range> range_data;
  Particle mean_p_, lastmean_p_;

  ros::Subscriber point_sub_, odom_sub_, range_sub_, initialpose_sub_;
  ros::Publisher particles_pose_pub_, range_markers_pub_, odom_base_pub_;

  tf::Transform lastbase_2_world_tf_, initodom_2_world_tf_, lastodom_2_world_tf_, amcl_out_lastbase_2_odom_tf_,
      lastupdatebase_2_odom_tf_, base_2_odom_tf_, odom_increment_tf;

  ros::Time nextupdate_time_;

  ros::Publisher cloud_filter_pub_;
};

}  // namespace amcl3d