Refactor code

CMakeLists.txt

@@ -4,7 +4,7 @@ project(continuous_tracking)
 find_package(catkin REQUIRED COMPONENTS roscpp tf2_eigen tf2_ros object_instance_msgs visualization_msgs dynamic_reconfigure message_filters image_geometry)
 
 generate_dynamic_reconfigure_options(
- cfg/ContinuousTracker.cfg
+ cfg/ContinuousTracking.cfg
 )
 
 catkin_package(
@@ -19,5 +19,5 @@ include_directories(
  ${catkin_INCLUDE_DIRS}
 )
 
-add_executable(continuous_tracker_node src/continuous_tracker_node.cpp)
-target_link_libraries(continuous_tracker_node ${catkin_LIBRARIES})
+add_executable(continuous_tracking_node src/continuous_tracking_node.cpp)
+target_link_libraries(continuous_tracking_node ${catkin_LIBRARIES})

README.md

@@ -47,7 +47,11 @@ In-Service Support (BAAINBw).
 
 ## 1. Download Sensor Data
 
-Download the [example rosbag](https://mega.nz/file/7NU11QxQ#-h3AotgPuyCyZaFWPGN0yxfDGNF6YZZM2ppw9QkMxEc) from our VW Touareg test vehicle.
+Download the [example rosbag](https://mega.nz/file/7NU11QxQ#-h3AotgPuyCyZaFWPGN0yxfDGNF6YZZM2ppw9QkMxEc) from our VW 
+Touareg test vehicle. Save the file's parent directory path to the environment variable:
+```bash
+export ROSBAG_PATH=/path/to/parent/directory/of/downloaded/file
+```
 
 ## 2. Setup Environment
 

cfg/ContinuousTracker.cfg → cfg/ContinuousTracking.cfg

@@ -64,4 +64,4 @@ bounding_box_yaw_by_enum = gen.enum([
 ], "bounding_box_yaw_by_enum")
 bb.add("bounding_box_yaw_by", int_t, 0, "todo", 0, 0, 3, edit_method=bounding_box_yaw_by_enum)
 
-exit(gen.generate(PACKAGE, "continuous_tracker", "ContinuousTracker"))
+exit(gen.generate(PACKAGE, "continuous_tracking", "ContinuousTracking"))

launch/continuous_tracker.launch → launch/continuous_tracking.launch

@@ -6,7 +6,7 @@
  <arg name="bag_file" default="" doc="path to bag file; if empty no bag file is replayed"/>
 
  <!-- launch continuous tracking node -->
- <node name="continuous_tracking" pkg="continuous_tracking" type="continuous_tracker_node"
+ <node name="continuous_tracking" pkg="continuous_tracking" type="continuous_tracking_node"
  ns="perception/object_tracking" output="screen">
  <remap from="lidar_detections" to="/perception/detections/lidar_roof/cluster/continuous_clusters"/>
  <remap from="continuous_ground_point_segmentation"
@@ -33,6 +33,6 @@
 
  <!-- launch RViz if desired -->
  <node name="rviz" pkg="rviz" type="rviz"
- args="--display-config $(find continuous_tracking)/rviz/continuous_tracker.rviz" output="log"
+ args="--display-config $(find continuous_tracking)/rviz/continuous_tracking.rviz" output="log"
  if="$(arg use_rviz)"/>
 </launch>

package.xml

@@ -17,7 +17,8 @@
  <depend>message_filters</depend>
  <depend>image_geometry</depend>
 
- <depend>rviz_object_instance</depend>
+ <exec_depend>rviz_object_instance</exec_depend>
+ <exec_depend>compressed_image_transport</exec_depend>
 
  <export>
  </export>

rviz/continuous_tracker.rviz → rviz/continuous_tracking.rviz

@@ -35,7 +35,7 @@ Panels:
  - Class: rviz/Time
  Name: Time
  SyncMode: 0
- SyncSource: ""
+ SyncSource: Camera
  - Class: rosbag_panel/RosbagPanel
  LastOpenedDirectory: ""
  Name: RosbagPanel
@@ -1179,7 +1179,7 @@ Window Geometry:
  Hide Right Dock: true
  InstanceSegmentation:
  collapsed: false
- QMainWindow State: 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
+ QMainWindow State: 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
  RosbagPanel:
  collapsed: false
  Selection:

src/continuous_tracker_node.cpp → src/continuous_tracking_node.cpp

@@ -1,6 +1,6 @@
 #include <ros/ros.h>
 
-#include <continuous_tracking/ContinuousTrackerConfig.h>
+#include <continuous_tracking/ContinuousTrackingConfig.h>
 #include <continuous_tracking/dataset.h>
 #include <dynamic_reconfigure/server.h>
 #include <list>
@@ -174,18 +174,18 @@ bool operator!=(const Track& t1, const Track& t2)
  return (t1.id == t2.id);
 }
 
-class ContinuousTrackerNode
+class ContinuousTrackingNode
 {
  public:
- explicit ContinuousTrackerNode(ros::NodeHandle nh, const ros::NodeHandle& private_nh)
+ explicit ContinuousTrackingNode(ros::NodeHandle nh, const ros::NodeHandle& private_nh)
  : t(ros::TransportHints().tcp().tcpNoDelay(true)),
- reset_sub_(nh.subscribe("reset", 1, &ContinuousTrackerNode::callbackReset, this, t)),
+ reset_sub_(nh.subscribe("reset", 1, &ContinuousTrackingNode::callbackReset, this, t)),
  lidar_detections_sub_(
- nh.subscribe("lidar_detections", 10000, &ContinuousTrackerNode::callbackLidarDetections, this, t)),
+ nh.subscribe("lidar_detections", 10000, &ContinuousTrackingNode::callbackLidarDetections, this, t)),
  continuous_ground_point_segmentation_sub_(
  nh.subscribe("continuous_ground_point_segmentation",
  10000,
- &ContinuousTrackerNode::callbackContinuousGroundPointSegmentation,
+ &ContinuousTrackingNode::callbackContinuousGroundPointSegmentation,
  this,
  t)),
  tf_buffer_(),
@@ -197,7 +197,7 @@ class ContinuousTrackerNode
  pub_update_ = nh.advertise<object_instance_msgs::ObjectInstance3DArray>("update", 10000);
  pub_tracks_ = nh.advertise<visualization_msgs::Marker>("predict", 10000);
 
- reconfigure_server_.setCallback([this](ContinuousTrackerConfig& config, uint32_t level)
+ reconfigure_server_.setCallback([this](ContinuousTrackingConfig& config, uint32_t level)
  { callbackReconfigure(config, level); });
  }
 
@@ -567,16 +567,16 @@ class ContinuousTrackerNode
  bool bounding_box_enabled;
  switch (config_.bounding_box_enable_if)
  {
- case ContinuousTracker_Yaw_Initialized:
+ case ContinuousTracking_Yaw_Initialized:
  bounding_box_enabled = !std::isnan(updated_track->yaw);
  break;
- case ContinuousTracker_Has_Line:
+ case ContinuousTracking_Has_Line:
  bounding_box_enabled = !std::isnan(yaw_by_line);
  break;
- case ContinuousTracker_Had_Previous_Motion:
+ case ContinuousTracking_Had_Previous_Motion:
  bounding_box_enabled = !std::isnan(yaw_by_motion);
  break;
- case ContinuousTracker_Always:
+ case ContinuousTracking_Always:
  bounding_box_enabled = true;
  break;
  default:
@@ -591,16 +591,16 @@ class ContinuousTrackerNode
  float bounding_box_yaw;
  switch (config_.bounding_box_yaw_by)
  {
- case ContinuousTracker_Track_Yaw:
+ case ContinuousTracking_Track_Yaw:
  bounding_box_yaw = updated_track->yaw;
  break;
- case ContinuousTracker_Longest_Line:
+ case ContinuousTracking_Longest_Line:
  bounding_box_yaw = yaw_by_line; // Todo: track based
  break;
- case ContinuousTracker_Principal_Component:
+ case ContinuousTracking_Principal_Component:
  bounding_box_yaw = calculateYawByCovariance(msg); // Todo: track based
  break;
- case ContinuousTracker_Previous_Motion:
+ case ContinuousTracking_Previous_Motion:
  bounding_box_yaw = yaw_by_motion;
  break;
  default:
@@ -616,7 +616,7 @@ class ContinuousTrackerNode
  fitBoundingBox(box_center,
  box_dimensions,
  bounding_box_yaw,
- config_.bounding_box_by == ContinuousTracker_Cluster ?
+ config_.bounding_box_by == ContinuousTracking_Cluster ?
  std::list<sensor_msgs::PointCloud2::ConstPtr>({msg}) :
  updated_track->centroid.getMessages());
  q.setRPY(0, 0, bounding_box_yaw);
@@ -1228,17 +1228,17 @@ class ContinuousTrackerNode
  return output_msg;
  }
 
- void callbackReconfigure(ContinuousTrackerConfig& config, uint32_t level)
+ void callbackReconfigure(ContinuousTrackingConfig& config, uint32_t level)
  {
- ContinuousTrackerConfig old_config = config_;
+ ContinuousTrackingConfig old_config = config_;
 
  config_ = config;
  config_max_centroid_distance_squared_ = config.max_centroid_distance * config.max_centroid_distance;
  }
 
  private:
- dynamic_reconfigure::Server<ContinuousTrackerConfig> reconfigure_server_;
- ContinuousTrackerConfig config_;
+ dynamic_reconfigure::Server<ContinuousTrackingConfig> reconfigure_server_;
+ ContinuousTrackingConfig config_;
  double config_max_centroid_distance_squared_{};
  ros::TransportHints t;
  ros::Subscriber reset_sub_;
@@ -1269,8 +1269,8 @@ class ContinuousTrackerNode
 
 int main(int argc, char** argv)
 {
- ros::init(argc, argv, "continuous_tracker");
- continuous_tracking::ContinuousTrackerNode node(ros::NodeHandle(), ros::NodeHandle("~"));
+ ros::init(argc, argv, "continuous_tracking");
+ continuous_tracking::ContinuousTrackingNode node(ros::NodeHandle(), ros::NodeHandle("~"));
  node.init();
  ros::spin();