Add tracking_frame

JerryBonjour · May 10, 2018 · f6efc37 · f6efc37
1 parent e3a9c1a
commit f6efc37
Show file tree

Hide file tree

Showing 4 changed files with 45 additions and 50 deletions.
diff --git a/...c/computing/perception/detection/packages/lidar_tracker/launch/imm_ukf_pda_tracker.launch b/...c/computing/perception/detection/packages/lidar_tracker/launch/imm_ukf_pda_tracker.launch
@@ -9,6 +9,7 @@
   <arg name="tracker_input_topic" default="/cloud_clusters" />
   <arg name="tracker_output_topic" default="/tracking_cluster_array" />
   <arg name="pointcloud_frame" default="/velodyne" />
+  <arg name="tracking_frame" default="/world" />
 
   <!-- rosrun lidar_tracker euclidean_cluster _points_node:="" -->
   <node pkg="lidar_tracker" type="imm_ukf_pda_tracker" name="imm_ukf_pda_tracker" output="screen">
@@ -19,6 +20,7 @@
     <param name="life_time_thres" value="$(arg life_time_thres)" />
     <param name="static_distance_thres" value="$(arg static_distance_thres)" />
     <param name="pointcloud_frame" value="$(arg pointcloud_frame)" />
+    <param name="tracking_frame" value="$(arg tracking_frame)" />
 
     <remap from="cloud_clusters" to="$(arg tracker_input_topic)" />
     <remap from="tracking_cluster_array" to="$(arg tracker_output_topic)" />

diff --git a/.../detection/packages/lidar_tracker/nodes/imm_ukf_pda_tracker/include/imm_ukf_pda_tracker.h b/.../detection/packages/lidar_tracker/nodes/imm_ukf_pda_tracker/include/imm_ukf_pda_tracker.h
@@ -17,53 +17,60 @@
 #include "ukf.h"
 
 
-// using namespace pcl;
-// using namespace Eigen;
-
-
-
 class ImmUkfPda
 {
 private:
   bool init_;
   double timestamp_ ;
 
   std::vector<UKF> targets_;
-  // std::vector<int> trackNumVec_;
 
   // probabilistic data association params
-  double gating_thres_;//9.22; // 99%
+  double gating_thres_;//9.22
   double gate_probability_;//0.99;
-  // extern double gammaG_ = 5.99; // 99%
-  // extern double pG_ = 0.95;
-  // extern double gammaG_ = 15.22; // 99%
   double detection_probability_;//0.9;
 
   //bbox association param
   double distance_thres_;//0.25;
   int life_time_thres_;//8;
   //bbox update params
   double bb_yaw_change_thres_;//0.2;
-  // double bb_area_change_thres_;//0.5;
 
   double static_distance_thres_;
 
   double init_yaw_;
 
+  //Tracking state paramas
+  int stable_num_;
+  int lost_num_;
+
   std::string input_topic_;
   std::string output_topic_;
 
   std::string pointcloud_frame_;
-  // std::vector<UKF> targets_;
-  // std::vector<int> trackNumVec_;
+  std::string tracking_frame_;
 
-  tf::TransformListener* tran_;
+  tf::TransformListener* tf_listener_;
 
   ros::NodeHandle node_handle_;
   ros::Subscriber sub_cloud_array_;
   ros::Publisher pub_cloud_array_;
 
-  void callBack(autoware_msgs::CloudClusterArray input);
+  enum TrackingState: int
+    {
+       Die = 0,
+       Init = 1,
+       Stable = 4,
+       Lost = 10,
+    };
+
+  enum IsMatch: int
+    {
+       False = 0,
+       True = 1,
+    };
+
+  void callback(autoware_msgs::CloudClusterArray input);
   void transformPoseToGlobal(autoware_msgs::CloudClusterArray& input);
   void transformPoseToLocal(autoware_msgs::CloudClusterArray& input);
   void findMaxZandS(const UKF target, Eigen::VectorXd& max_det_z, Eigen::MatrixXd& max_det_s);

diff --git a/...on/detection/packages/lidar_tracker/nodes/imm_ukf_pda_tracker/src/imm_ukf_pda_tracker.cpp b/...on/detection/packages/lidar_tracker/nodes/imm_ukf_pda_tracker/src/imm_ukf_pda_tracker.cpp
@@ -2,54 +2,32 @@
 #include "imm_ukf_pda_tracker.h"
 
 
-namespace States
-{
-  enum TrackingState: int
-  {
-     Die = 0,
-     Init = 1,
-     Stable = 4,
-     Lost = 10,
-
-  };
-}
-typedef States::TrackingState TrackingState;
-
-namespace Matches
-{
-  enum IsMatch: int
-  {
-     False = 0,
-     True = 1,
-  };
-}
-typedef Matches::IsMatch IsMatch;
-
-
-
 ImmUkfPda::ImmUkfPda()
 {
   ros::NodeHandle private_nh_("~");
   private_nh_.param<std::string>("pointcloud_frame", pointcloud_frame_, "velodyne");
+  private_nh_.param<std::string>("tracking_frame", tracking_frame_, "world");
   private_nh_.param<int>("life_time_thres", life_time_thres_, 8);
   private_nh_.param<double>("gating_thres", gating_thres_, 9.22);
   private_nh_.param<double>("gate_probability", gate_probability_, 0.99);
   private_nh_.param<double>("detection_probability", detection_probability_, 0.9);
   private_nh_.param<double>("distance_thres", distance_thres_, 99);
   private_nh_.param<double>("static_distance_thres", static_distance_thres_, 3.0);
+  private_nh_.param<int>("stable_num", stable_num_, 4);
+  private_nh_.param<int>("lost_num", lost_num_, 10);
 
 
   init_ = false;
 
   tf::TransformListener *lr (new  tf::TransformListener);
-  tran_=lr;
+  tf_listener_=lr;
 
-  sub_cloud_array_   = node_handle_.subscribe ("cloud_clusters", 1, &ImmUkfPda::callBack, this);
+  sub_cloud_array_   = node_handle_.subscribe ("cloud_clusters", 1, &ImmUkfPda::callback, this);
   pub_cloud_array_  = node_handle_.advertise<autoware_msgs::CloudClusterArray> ("tracking_cluster_array", 1);
 
 }
 
-void ImmUkfPda::callBack(autoware_msgs::CloudClusterArray input)
+void ImmUkfPda::callback(autoware_msgs::CloudClusterArray input)
 {
   autoware_msgs::CloudClusterArray output;
   // only transform pose(clusteArray.clusters.bouding_box.pose)
@@ -69,8 +47,8 @@ void ImmUkfPda::transformPoseToGlobal(autoware_msgs::CloudClusterArray& input)
 
     pose_in.header = input.header;
     pose_in.pose = input.clusters[i].bounding_box.pose;
-    tran_->waitForTransform(pointcloud_frame_, "/world",input.header.stamp, ros::Duration(3.0));
-    tran_->transformPose("world", ros::Time(0), pose_in, input.header.frame_id, pose_out);
+    tf_listener_->waitForTransform(pointcloud_frame_, tracking_frame_, input.header.stamp, ros::Duration(1.0));
+    tf_listener_->transformPose(tracking_frame_, ros::Time(0), pose_in, input.header.frame_id, pose_out);
     input.clusters[i].bounding_box.pose = pose_out.pose;
   }
 }
@@ -83,11 +61,11 @@ void ImmUkfPda::transformPoseToLocal(autoware_msgs::CloudClusterArray& output)
     geometry_msgs::PoseStamped pose_in, pose_out;
 
     pose_in.header = output.header;
-    pose_in.header.frame_id = "world";
+    pose_in.header.frame_id = tracking_frame_;
     pose_in.pose = output.clusters[i].bounding_box.pose;
 
-    tran_->waitForTransform("/world", pointcloud_frame_, ros::Time(0), ros::Duration(3.0));
-    tran_->transformPose(pointcloud_frame_, ros::Time(0), pose_in, "world", pose_out);
+    tf_listener_->waitForTransform(tracking_frame_, pointcloud_frame_, ros::Time(0), ros::Duration(1.0));
+    tf_listener_->transformPose(pointcloud_frame_, ros::Time(0), pose_in, tracking_frame_, pose_out);
     pose_out.header.frame_id = output.header.frame_id = pointcloud_frame_;
     output.clusters[i].bounding_box.pose = pose_out.pose;
   }
@@ -725,9 +703,9 @@ void ImmUkfPda::makeOutput(autoware_msgs::CloudClusterArray input,
                            autoware_msgs::CloudClusterArray& output)
 {
   tf::StampedTransform transform;
-  tran_->lookupTransform("/world", "/base_link", ros::Time(0), transform);
+  tf_listener_->lookupTransform(tracking_frame_, pointcloud_frame_, ros::Time(0), transform);
 
-  // get yaw angle from "world" to "velodyne" for direction(arrow) visualization
+  // get yaw angle from tracking_frame_ to pointcloud_frame_ for direction(arrow) visualization
   tf::Matrix3x3 m(transform.getRotation());
   double roll, pitch, yaw;
   m.getRPY(roll, pitch, yaw);

diff --git a/ros/src/util/packages/runtime_manager/scripts/computing.yaml b/ros/src/util/packages/runtime_manager/scripts/computing.yaml
@@ -1275,6 +1275,14 @@ params :
       cmd_param :
         dash        : ''
         delim       : ':='
+    - name    : tracking_frame_
+      desc    : trakcing_frame_ desc sample
+      label   : 'tracking frame'
+      kind    : str
+      v       : world
+      cmd_param :
+        dash        : ''
+        delim       : ':='
     - name    : life_time_thres_
       desc    : life_time_thres_ desc sample
       label   : 'life time thres'