motion_capture.hpp

Go to the documentation of this file.

// Copyright 2024 João Vitor Silva Mendes
 
#ifndef MOTION_CAPTURE_HPP_
#define MOTION_CAPTURE_HPP_
 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/centroid.h>
#include <pcl_conversions/pcl_conversions.h>
 
#include <Eigen/Core>
#include <queue>
#include <string>
 
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/vector3.hpp>
 
namespace pointcloud_motion_capture
{
class MotionCapture : public rclcpp::Node
{
public:
  enum CentroidIndex
  {
    kCentroidX = 0,  
    kCentroidY = 1,  
    kCentroidZ = 2   
  };
 
  enum VectorIndex
  {
    kVectorX = 0,  
    kVectorY = 1,  
    kVectorZ = 2   
  };
 
  std::string kDefaultPointCloudTopicFiltered {"/point_cloud/filtered"};
 
  std::string kDefaultPositionPublishTopic {"/object_position"};
 
  std::string kDefaultVelocityPublishTopic {"/object_velocity"};
 
  std::string kDefaultAccelerationPublishTopic {"/object_acceleration"};
 
  int kDefaultPositionSamples{5};
 
  int kDefaultVelocitySamples{5};
 
  double kDefaultPublishRate{10.0};
 
  int kDefaultQueueSize{10};
 
  double kMillisecondsPerSecond{1000.0};
 
  MotionCapture();
 
private:
  void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
 
  void updatePositionSamples(const Eigen::Vector4f & centroid);
 
  Eigen::Vector4f getFilteredPosition();
 
  void updateVelocitySamples(const Eigen::Vector3f & velocity);
 
  Eigen::Vector3f getFilteredVelocity();
 
  void publishData();
 
  void publishPosition(const Eigen::Vector4f & centroid);
 
  Eigen::Vector3f publishVelocity(const Eigen::Vector4f & current_position);
 
  void publishAcceleration(const Eigen::Vector3f & current_velocity);
 
  void loadParameters();
 
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
 
  rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr position_publisher_;
 
  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr velocity_publisher_;
 
  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr acceleration_publisher_;
 
  std::queue<Eigen::Vector4f> position_queue_;
 
  std::queue<Eigen::Vector3f> velocity_queue_;
 
  Eigen::Vector4f previous_position_;
 
  Eigen::Vector3f previous_velocity_;
 
  rclcpp::Time previous_time_;
 
  rclcpp::Time previous_velocity_time_;
 
  int position_samples_;
 
  int velocity_samples_;
 
  int queue_size_;
 
  double publish_rate_;
 
  rclcpp::TimerBase::SharedPtr timer_;
 
  std::string point_cloud_topic_filtered_;
 
  std::string position_publish_topic_;
 
  std::string velocity_publish_topic_;
 
  std::string acceleration_publish_topic_;
};
 
}  // namespace pointcloud_motion_capture
 
#endif  // MOTION_CAPTURE_HPP_