Hodgepodge of fixes

toid_lidar/src/toid_lidar_node.cpp

@@ -47,7 +47,7 @@ void ToidRivalDetect::process_scan(LaserScan::ConstSharedPtr msg)
   geometry_msgs::msg::TransformStamped transform;
 
   try {
-    transform = tf_buf_->lookupTransform(global_frame_, msg->header.frame_id, msg->header.stamp);
+    transform = tf_buf_->lookupTransform(global_frame_, msg->header.frame_id, tf2::TimePointZero);
   } catch (const tf2::TransformException & e) {
     RCLCPP_WARN_THROTTLE(
       this->get_logger(), *this->get_clock(), 1000, "TF Link missing: %s", e.what());

toid_navigation/launch/main.py

@@ -11,6 +11,7 @@ def generate_launch_description():
     nav_pkg_share = FindPackageShare("").find('toid_navigation')
     control_pkg_share = FindPackageShare("").find('toid_control')
     lidar_pkg_share = FindPackageShare("").find('toid_lidar')
+    vision_pkg_share = FindPackageShare("").find('toid_vision')
     params = os.path.join(nav_pkg_share, 'params', 'toid_general_params.yaml')
     map = os.path.join(nav_pkg_share, 'maps', 'mapb2_5cm.yaml')
     ctrl_launch_dir = os.path.join(control_pkg_share, 'launch')
@@ -70,11 +71,29 @@ def generate_launch_description():
         ),
         launch_arguments={
             'visualize': 'False',
-            'lidar_frame': 'lidar_frame', 
+            'lidar_frame': 'lidar', 
         }.items(),
         condition=IfCondition(AllSubstitution(run_nodes, use_lidar)),
     ) 
 
+    toid_vision = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(vision_pkg_share, 'launch' , 'launch.py')
+        ),
+        launch_arguments={
+            'is_blue': is_blue,
+        }.items(),
+        condition=IfCondition(AllSubstitution(run_nodes, use_lidar)),
+    ) 
+
+    toid_interaction = Node(
+        package='toid_interaction',
+        executable='node',
+        name='toid_interaction',
+        output='screen',
+        condition=IfCondition(run_nodes),
+    )
+
     map_server = Node(
         package='nav2_map_server',
         executable='map_server',
@@ -145,7 +164,10 @@ def generate_launch_description():
         use_mock_arg,
         run_nodes_arg,
         use_lidar_arg,
+        is_blue_arg,
         toid_lidar,
+        toid_vision,
+        toid_interaction,
         rviz_node,
         map_server,
         bt_navigator,

toid_navigation/params/toid_general_params.yaml

@@ -22,7 +22,7 @@ behavior_server:
     local_footprint_topic: local_costmap/published_footprint
     global_footprint_topic: global_costmap/published_footprint
     cycle_frequency: 50.0
-    behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords", "approachAcorns", rotateAcorns]
+    behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords", "approachAcorns", "rotateAcorns"]
     spin:
       plugin: "nav2_behaviors::Spin"
     backup:
@@ -91,7 +91,7 @@ local_costmap:
       height: 1
       resolution: 0.01
       introspection_mode: "disabled"
-      plugins: ["static_layer", rival_layer, "inflation_layer"]
+      plugins: ["static_layer", "rival_layer", "inflation_layer"]
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
         footprint_clearing_enabled: true

toid_vision/package.xml

@@ -13,7 +13,7 @@
   <depend>sensor_msgs</depend>
   <depend>message_filters</depend>
   <depend>cv_bridge</depend>
-  <depend>opencv</depend>
+  <depend>libopencv-dev</depend>
   <depend>rclcpp_components</depend>
 
   <depend>tf2</depend>