vrpn_client_ros.h

Go to the documentation of this file.

#ifndef VRPN_CLIENT_ROS_VRPN_CLIENT_ROS_H
#define VRPN_CLIENT_ROS_VRPN_CLIENT_ROS_H

#include "vrpn_client_ros/vrpn_client_ros.h"

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/TwistStamped.h"
#include "geometry_msgs/AccelStamped.h"
#include "geometry_msgs/TransformStamped.h"

#include <vrpn_Tracker.h>
#include <vrpn_Connection.h>
#include <map>
#include <string>
#include <unordered_map>

namespace vrpn_client_ros
{

  typedef std::shared_ptr<vrpn_Connection> ConnectionPtr;
  typedef std::shared_ptr<vrpn_Tracker_Remote> TrackerRemotePtr;

  class VrpnTrackerRos
  {
  public:

    typedef std::shared_ptr<VrpnTrackerRos> Ptr;
    VrpnTrackerRos(std::string tracker_name, ConnectionPtr connection, ros::NodeHandle nh);

    VrpnTrackerRos(std::string tracker_name, std::string host, ros::NodeHandle nh);

    ~VrpnTrackerRos();

    void mainloop();

  private:
    TrackerRemotePtr tracker_remote_;
    std::vector<ros::Publisher> pose_pubs_, twist_pubs_, accel_pubs_;
    ros::NodeHandle output_nh_;
    bool use_server_time_, broadcast_tf_, process_sensor_id_;
    std::string tracker_name;

    ros::Timer mainloop_timer;

    geometry_msgs::PoseStamped pose_msg_;
    geometry_msgs::TwistStamped twist_msg_;
    geometry_msgs::AccelStamped accel_msg_;
    geometry_msgs::TransformStamped transform_stamped_;

    void init(std::string tracker_name, ros::NodeHandle nh, bool create_mainloop_timer);

    static void VRPN_CALLBACK handle_pose(void *userData, const vrpn_TRACKERCB tracker_pose);

    static void VRPN_CALLBACK handle_twist(void *userData, const vrpn_TRACKERVELCB tracker_twist);

    static void VRPN_CALLBACK handle_accel(void *userData, const vrpn_TRACKERACCCB tracker_accel);
  };

  class VrpnClientRos
  {
  public:

    typedef std::shared_ptr<VrpnClientRos> Ptr;
    typedef std::unordered_map<std::string, VrpnTrackerRos::Ptr> TrackerMap;

    VrpnClientRos(ros::NodeHandle nh, ros::NodeHandle private_nh);

    static std::string getHostStringFromParams(ros::NodeHandle host_nh);

    void mainloop();

    void updateTrackers();

  private:
    std::string host_;
    ros::NodeHandle output_nh_;

    ConnectionPtr connection_;

    TrackerMap trackers_;

    ros::Timer refresh_tracker_timer_, mainloop_timer;
  };
}  // namespace vrpn_client_ros

#endif  // VRPN_CLIENT_ROS_VRPN_CLIENT_ROS_H