laser_transform_core.h

Go to the documentation of this file.

#ifndef LASER_TRANSFORM_CORE_H
#define LASER_TRANSFORM_CORE_H

// ROS includes
#include <fstream>
#include "ros/ros.h"
#include "ros/time.h"
#include "pcl_ros/transforms.h"
#include "sensor_msgs/PointCloud2.h"
#include "nav_msgs/Odometry.h"
#include "ip_connection.h"
#include "brick_imu.h"
#include "brick_imu_v2.h"
#include "bricklet_gps.h"
#include "bricklet_industrial_digital_in_4.h"
#include "bricklet_dual_button.h"
#include "octomap_msgs/BoundingBoxQuery.h"

#define M_PI    3.14159265358979323846  /* pi */

class LaserTransform
{
public:
  LaserTransform();

  ~LaserTransform();

  int init();

  void publishPclMessage(ros::Publisher *pub_message);

  void publishImuMessage(ros::Publisher *pub_message);

  void publishNavSatFixMessage(ros::Publisher *pub_message);

  void publishMagneticFieldMessage(ros::Publisher *pub_message);

  void publishOdometryMessage(ros::Publisher *pub_message);

  void callbackPcl(const sensor_msgs::PointCloud2::ConstPtr& msg);

  void callbackOdometryFiltered(const nav_msgs::Odometry::ConstPtr& msg);

  void setLaserPose(double x, double y, double z, double yaw, double pitch, double roll);

  void checkConvergenceSpeed();

  void clearOctomap(ros::ServiceClient *client);

  void setPclPublisher(ros::Publisher *pub)
  {
    pcl_pub = pub;
  }
public:
  int setImuConvergenceSpeed(uint16_t imu_convergence_speed)
  {
    this->imu_convergence_speed = imu_convergence_speed;
    return 0;
  }
private:
  static void callbackConnected(uint8_t connect_reason, void *user_data);

  static void callbackEnumerate(const char *uid, const char *connected_uid,
    char position, uint8_t hardware_version[3],
    uint8_t firmware_version[3], uint16_t device_identifier,
    uint8_t enumeration_type, void *user_data);

  static void callbackIdi4(uint8_t interrupt_mask, uint8_t value_mask, void *user_data);

  static void callbackDb(uint8_t button_l, uint8_t button_r,
                      uint8_t led_l, uint8_t led_r,
                      void *user_data);

  tf::Quaternion getQuaternion();

  int getPosition(float *x_pos, float *y_pos, float *z_pos);

  float rad2deg(float x)
  {
    return x*180.0/M_PI;
  }

  float deg2rad(float x)
  {
    return x*M_PI/180.0;
  }
private:
  ros::Publisher *pcl_pub;
  IPConnection ipcon;
  IMU imu;
  bool is_imu_connected;
  int imu_convergence_speed;
  ros::Time imu_init_time;
  IMUV2 imu_v2;
  bool is_imu_v2_connected;
  GPS gps;
  bool is_gps_connected;
  IndustrialDigitalIn4 idi4;
  uint16_t rpm_cnt;
  bool is_idi4_connected;
  DualButton db;
  bool isMeasure;
  float rev;
  ros::Time last_rev;
  sensor_msgs::PointCloud2 pcl_out;
  bool publish_new_pcl;
  bool new_pcl_filtered;
  double start_latitude;
  double start_longitude;
  tf::Transform laser_pose;
  //tf::Quaternion laser_orientation;
  bool isPlc;
  float xpos;
  float ypos;
  float velocity;
  float velocity_gps;
  float course_gps;
  float yy;
  std::fstream gps_log;
  std::fstream velo_log;
  std::fstream full_log;
};

#endif