fakeTracking.cpp

Go to the documentation of this file.

#include <nav_msgs/Odometry.h>
#include <dynamic_obs_msgs/DynamicObstacles.h>
#include <dynamic_obs_msgs/DynamicObstacle.h>
#include <dynamic_obs_msgs/DynObsTrajectory.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <ros/ros.h>
#include <stdio.h>
#include <../include/fake_tracking/fakeTracking.h>

using namespace std;
using namespace ros;
using namespace nav_msgs;
using namespace dynamic_obs_msgs;

FakeTracking::FakeTracking(){
  //init_sub = n.subscribe("time", 1, initCallback);
  dynObs_pub = n.advertise<dynamic_obs_msgs::DynamicObstacles>("/robot_0/dynamic_obstacles", 1);

  DynamicObstacle o1;
  o1.radius = 0.5;
  DynObsTrajectory t1;
  t1.exists_after = false;
  t1.probability = 1;
  o1.trajectories.push_back(t1);
  d.dyn_obs.push_back(o1);
  dynObs1_sub = n.subscribe("robot_1/base_pose_ground_truth", 1, &FakeTracking::dynamicObstacleCallback1,this);

  DynamicObstacle o2;
  o2.radius = 0.5;
  DynObsTrajectory t2;
  t2.exists_after = false;
  t2.probability = 1;
  o2.trajectories.push_back(t2);
  d.dyn_obs.push_back(o2);
  dynObs2_sub = n.subscribe("robot_2/base_pose_ground_truth", 1, &FakeTracking::dynamicObstacleCallback2,this);

  DynamicObstacle o3;
  o3.radius = 0.5;
  DynObsTrajectory t3;
  t3.exists_after = false;
  t3.probability = 1;
  o3.trajectories.push_back(t3);
  d.dyn_obs.push_back(o3);
  dynObs3_sub = n.subscribe("robot_3/base_pose_ground_truth", 1, &FakeTracking::dynamicObstacleCallback3,this);

  DynamicObstacle o4;
  o4.radius = 0.5;
  DynObsTrajectory t4;
  t4.exists_after = false;
  t4.probability = 1;
  o4.trajectories.push_back(t4);
  d.dyn_obs.push_back(o4);
  dynObs4_sub = n.subscribe("robot_4/base_pose_ground_truth", 1, &FakeTracking::dynamicObstacleCallback4,this);
}

void FakeTracking::dynamicObstacleCallback1(const nav_msgs::OdometryConstPtr& msg){
  int idx = 0;
  //ROS_INFO("I got the callback 1!, (%f,%f)\n", msg->pose.pose.position.x, msg->pose.pose.position.y);
  double x = msg->pose.pose.position.x;
  double y = msg->pose.pose.position.y;
  double t = msg->header.stamp.toSec();

  double pred_sec = 20;
  double t_gran = 0.1;
  
  if(d.dyn_obs[idx].trajectories[0].points.empty()){
    for(int i=0; i<pred_sec/t_gran+1; i++){
      geometry_msgs::PoseWithCovarianceStamped p;
      p.pose.pose.position.x = x;
      p.pose.pose.position.y = y;
      p.header.stamp = ros::Time(t + i*t_gran);
      p.header.frame_id = msg->header.frame_id;
      for(int i=0; i<36; i++)
        p.pose.covariance[i] = 0;
      p.pose.pose.orientation.w = 1;
      d.dyn_obs[idx].trajectories[0].points.push_back(p);
    }
  }
  else{
    double delta_t = t - d.dyn_obs[idx].trajectories[0].points[0].header.stamp.toSec();
    double x_dot = (x-d.dyn_obs[idx].trajectories[0].points[0].pose.pose.position.x)/delta_t;
    double y_dot = (y-d.dyn_obs[idx].trajectories[0].points[0].pose.pose.position.y)/delta_t;

    for(int i=0; i<pred_sec/t_gran+1; i++){
      double timestep = i*t_gran;
      d.dyn_obs[idx].trajectories[0].points[i].pose.pose.position.x = x + x_dot*timestep;
      d.dyn_obs[idx].trajectories[0].points[i].pose.pose.position.y = y + y_dot*timestep;
      d.dyn_obs[idx].trajectories[0].points[i].header.stamp = ros::Time(t + timestep);
    }
  }

  d.header.stamp = msg->header.stamp;
  d.header.frame_id = msg->header.frame_id;
  dynObs_pub.publish(d);
}

void FakeTracking::dynamicObstacleCallback2(const nav_msgs::OdometryConstPtr& msg){
  int idx = 1;
  //ROS_INFO("I got the callback 1!, (%f,%f)\n", msg->pose.pose.position.x, msg->pose.pose.position.y);
  double x = msg->pose.pose.position.x;
  double y = msg->pose.pose.position.y;
  double t = msg->header.stamp.toSec();

  double pred_sec = 20;
  double t_gran = 0.1;
  
  if(d.dyn_obs[idx].trajectories[0].points.empty()){
    for(int i=0; i<pred_sec/t_gran+1; i++){
      geometry_msgs::PoseWithCovarianceStamped p;
      p.pose.pose.position.x = x;
      p.pose.pose.position.y = y;
      p.header.stamp = ros::Time(t + i*t_gran);
      p.header.frame_id = msg->header.frame_id;
      for(int i=0; i<36; i++)
        p.pose.covariance[i] = 0;
      p.pose.pose.orientation.w = 1;
      d.dyn_obs[idx].trajectories[0].points.push_back(p);
    }
  }
  else{
    double delta_t = t - d.dyn_obs[idx].trajectories[0].points[0].header.stamp.toSec();
    double x_dot = (x-d.dyn_obs[idx].trajectories[0].points[0].pose.pose.position.x)/delta_t;
    double y_dot = (y-d.dyn_obs[idx].trajectories[0].points[0].pose.pose.position.y)/delta_t;

    for(int i=0; i<pred_sec/t_gran+1; i++){
      double timestep = i*t_gran;
      d.dyn_obs[idx].trajectories[0].points[i].pose.pose.position.x = x + x_dot*timestep;
      d.dyn_obs[idx].trajectories[0].points[i].pose.pose.position.y = y + y_dot*timestep;
      d.dyn_obs[idx].trajectories[0].points[i].header.stamp = ros::Time(t + timestep);
    }
  }

  d.header.stamp = msg->header.stamp;
  d.header.frame_id = msg->header.frame_id;
  dynObs_pub.publish(d);
}

void FakeTracking::dynamicObstacleCallback3(const nav_msgs::OdometryConstPtr& msg){
  int idx = 2;
  //ROS_INFO("I got the callback 1!, (%f,%f)\n", msg->pose.pose.position.x, msg->pose.pose.position.y);
  double x = msg->pose.pose.position.x;
  double y = msg->pose.pose.position.y;
  double t = msg->header.stamp.toSec();

  double pred_sec = 20;
  double t_gran = 0.1;
  
  if(d.dyn_obs[idx].trajectories[0].points.empty()){
    for(int i=0; i<pred_sec/t_gran+1; i++){
      geometry_msgs::PoseWithCovarianceStamped p;
      p.pose.pose.position.x = x;
      p.pose.pose.position.y = y;
      p.header.stamp = ros::Time(t + i*t_gran);
      p.header.frame_id = msg->header.frame_id;
      for(int i=0; i<36; i++)
        p.pose.covariance[i] = 0;
      p.pose.pose.orientation.w = 1;
      d.dyn_obs[idx].trajectories[0].points.push_back(p);
    }
  }
  else{
    double delta_t = t - d.dyn_obs[idx].trajectories[0].points[0].header.stamp.toSec();
    double x_dot = (x-d.dyn_obs[idx].trajectories[0].points[0].pose.pose.position.x)/delta_t;
    double y_dot = (y-d.dyn_obs[idx].trajectories[0].points[0].pose.pose.position.y)/delta_t;

    for(int i=0; i<pred_sec/t_gran+1; i++){
      double timestep = i*t_gran;
      d.dyn_obs[idx].trajectories[0].points[i].pose.pose.position.x = x + x_dot*timestep;
      d.dyn_obs[idx].trajectories[0].points[i].pose.pose.position.y = y + y_dot*timestep;
      d.dyn_obs[idx].trajectories[0].points[i].header.stamp = ros::Time(t + timestep);
    }
  }

  d.header.stamp = msg->header.stamp;
  d.header.frame_id = msg->header.frame_id;
  dynObs_pub.publish(d);
}

void FakeTracking::dynamicObstacleCallback4(const nav_msgs::OdometryConstPtr& msg){
  int idx = 3;
  //ROS_INFO("I got the callback 1!, (%f,%f)\n", msg->pose.pose.position.x, msg->pose.pose.position.y);
  double x = msg->pose.pose.position.x;
  double y = msg->pose.pose.position.y;
  double t = msg->header.stamp.toSec();

  double pred_sec = 20;
  double t_gran = 0.1;
  
  if(d.dyn_obs[idx].trajectories[0].points.empty()){
    for(int i=0; i<pred_sec/t_gran+1; i++){
      geometry_msgs::PoseWithCovarianceStamped p;
      p.pose.pose.position.x = x;
      p.pose.pose.position.y = y;
      p.header.stamp = ros::Time(t + i*t_gran);
      p.header.frame_id = msg->header.frame_id;
      for(int i=0; i<36; i++)
        p.pose.covariance[i] = 0;
      p.pose.pose.orientation.w = 1;
      d.dyn_obs[idx].trajectories[0].points.push_back(p);
    }
  }
  else{
    double delta_t = t - d.dyn_obs[idx].trajectories[0].points[0].header.stamp.toSec();
    double x_dot = (x-d.dyn_obs[idx].trajectories[0].points[0].pose.pose.position.x)/delta_t;
    double y_dot = (y-d.dyn_obs[idx].trajectories[0].points[0].pose.pose.position.y)/delta_t;

    for(int i=0; i<pred_sec/t_gran+1; i++){
      double timestep = i*t_gran;
      d.dyn_obs[idx].trajectories[0].points[i].pose.pose.position.x = x + x_dot*timestep;
      d.dyn_obs[idx].trajectories[0].points[i].pose.pose.position.y = y + y_dot*timestep;
      d.dyn_obs[idx].trajectories[0].points[i].header.stamp = ros::Time(t + timestep);
    }
  }

  d.header.stamp = msg->header.stamp;
  d.header.frame_id = msg->header.frame_id;
  dynObs_pub.publish(d);
}


int main(int argc, char** argv){
  ros::init(argc, argv, "fake_tracking");
  FakeTracking f;
  ros::spin();
}

---

fake_tracking
Author(s): Michael Phillips
autogenerated on Fri Jan 11 09:42:09 2013