diff --git a/CMakeLists.txt b/CMakeLists.txt index a19f20a..31e8edb 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/README.md b/README.md index 28720f5..095ec34 100644 --- a/README.md +++ b/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 diff --git a/cfg/ContinuousTracker.cfg b/cfg/ContinuousTracking.cfg similarity index 97% rename from cfg/ContinuousTracker.cfg rename to cfg/ContinuousTracking.cfg index 35dc208..f7d40fd 100755 --- a/cfg/ContinuousTracker.cfg +++ b/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")) diff --git a/launch/continuous_tracker.launch b/launch/continuous_tracking.launch similarity index 96% rename from launch/continuous_tracker.launch rename to launch/continuous_tracking.launch index df54f58..973019d 100644 --- a/launch/continuous_tracker.launch +++ b/launch/continuous_tracking.launch @@ -6,7 +6,7 @@ - diff --git a/package.xml b/package.xml index 12ec1cd..2deed23 100644 --- a/package.xml +++ b/package.xml @@ -17,7 +17,8 @@ message_filters image_geometry - rviz_object_instance + rviz_object_instance + compressed_image_transport diff --git a/rviz/continuous_tracker.rviz b/rviz/continuous_tracking.rviz similarity index 93% rename from rviz/continuous_tracker.rviz rename to rviz/continuous_tracking.rviz index db26905..84b0e91 100644 --- a/rviz/continuous_tracker.rviz +++ b/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: 000000ff00000000fd0000000400000000000002d70000039efc020000000dfb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000c00430061006d006500720061010000003d0000030c0000001600fffffffb000000160052006f007300620061006700500061006e0065006c010000034f0000008c0000008c00fffffffb00000020004f0062006a0065006300740049006e007300740061006e006300650032004400000001bd0000016f0000000000000000fb00000020004f0062006a0065006300740049006e007300740061006e006300650032004402000003ba0000001c000003ba0000015bfb000000280049006e007300740061006e00630065005300650067006d0065006e0074006100740069006f006e0000000264000001400000001600fffffffb000000280049006e007300740061006e00630065005300650067006d0065006e0074006100740069006f006e00000001a4000000df0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160052006f007300620061006700500061006e0065006c00000002f90000008c0000007700ffffff0000000100000156000002effc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d000001ee0000000000fffffffaffffffff0100000002fb000000100044006900730070006c0061007900730000000000ffffffff0000015600fffffffb0000001200530065006c0065006300740069006f006e00000006290000010f0000007400fffffffb0000000a005600690065007700730000000231000000fb000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000738000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000738000000a9fc0100000009fb000000280049006e007300740061006e00630065005300650067006d0065006e0074006100740069006f006e000000000000000738000000ce00fffffffb0000001400520061006e006700650049006d00610067006502000000000000002d00000280000001e0fb0000002e00470072006f0075006e00640050006f0069006e0074005300650067006d0065006e0074006100740069006f006e000000000000000738000000eb00fffffffb0000000e0046006900720069006e006700730000000000000007380000006200fffffffc00000000000007380000000000fffffffaffffffff0200000002fb0000002e00470072006f0075006e00640050006f0069006e0074005300650067006d0065006e0074006100740069006f006e0000000000ffffffff0000001600fffffffb0000000e0046006900720069006e006700730000000000ffffffff0000001600fffffffb0000002e00470072006f0075006e00640050006f0069006e0074005300650067006d0065006e0074006100740069006f006e000000039f00000399000000eb00fffffffb0000000e0046006900720069006e006700730000000000000007380000006200fffffffc00000000000007380000000000fffffffa000000000200000008fb000000160052006f007300620061006700500061006e0065006c0100000000ffffffff0000000000000000fb000000160052006f007300620061006700500061006e0065006c0100000000ffffffff0000000000000000fb000000280049006e007300740061006e00630065005300650067006d0065006e0074006100740069006f006e0000000000ffffffff0000000000000000fb000000160052006f007300620061006700500061006e0065006c0100000000ffffffff0000000000000000fb000000280049006e007300740061006e00630065005300650067006d0065006e0074006100740069006f006e0000000000ffffffff0000000000000000fb0000000e0046006900720069006e006700730000000000ffffffff0000000000000000fb0000002e00470072006f0075006e00640050006f0069006e0074005300650067006d0065006e0074006100740069006f006e0000000000ffffffff0000000000000000fb0000000800540069006d006500000004150000003e0000003900fffffffb0000000800540069006d006501000000000000045000000000000000000000045b0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RosbagPanel: collapsed: false Selection: diff --git a/src/continuous_tracker_node.cpp b/src/continuous_tracking_node.cpp similarity index 97% rename from src/continuous_tracker_node.cpp rename to src/continuous_tracking_node.cpp index 8547fe9..8a9d63b 100644 --- a/src/continuous_tracker_node.cpp +++ b/src/continuous_tracking_node.cpp @@ -1,6 +1,6 @@ #include -#include +#include #include #include #include @@ -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("update", 10000); pub_tracks_ = nh.advertise("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({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 reconfigure_server_; - ContinuousTrackerConfig config_; + dynamic_reconfigure::Server 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();