pose_tracker_main.cpp

Go to the documentation of this file.

/*
 * pose_tracker_main.cpp
 *
 *  Created on: Feb 21, 2012
 *      Author: sprunkc
 *
 *  ported to ros from robular implementation by joerg mueller
 */


#include <ros/ros.h>

#include "posetracker.h"
#include <vector>
#include <XmlRpcValue.h>
#include <iomanip>
using namespace std;

std::vector<PoseTracker*> pts;

void markerCallback(const visualization_msgs::MarkerArray::ConstPtr &msg){
  for(unsigned int i=0; i<pts.size(); i++){
    pts.at(i)->process(msg);
  }
}
#include <iomanip>
int main(int argc, char** argv)
{

  ros::init(argc, argv, "posetracker");

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  ros::Subscriber sub_marker = nh.subscribe<visualization_msgs::MarkerArray>("cortex_marker_array", 10, &markerCallback);
  tf::TransformBroadcaster pose_broadcaster;



  double scale;
  double sqrErrThreshold;
  bool useRansac = false;
  int numRansacSamples;

  private_nh.param<double>("scale", scale, 0.001);
  private_nh.param<double>("sqrErrThreshold", sqrErrThreshold, 0.05);
  private_nh.param<bool>("useRansac", useRansac, false);
  private_nh.param<int>("numRansacSamples", numRansacSamples, 10);


  XmlRpc::XmlRpcValue propfiles;
  private_nh.getParam("propfiles", propfiles);
  ROS_ASSERT(propfiles.getType() == XmlRpc::XmlRpcValue::TypeArray);

  for (int32_t i = 0; i < propfiles.size(); ++i)
  {
    ROS_ASSERT(propfiles[i].getType() == XmlRpc::XmlRpcValue::TypeString);
    std::string propfile = static_cast<std::string>(propfiles[i]);
    PoseTracker* pt = new PoseTracker();
    pt->init(propfile, scale, &pose_broadcaster, sqrErrThreshold, useRansac, numRansacSamples);
    pts.push_back(pt);
  }

  ros::spin();
}