diff --git a/toid_lidar/src/toid_lidar_node.cpp b/toid_lidar/src/toid_lidar_node.cpp index 52062cf..47532d1 100644 --- a/toid_lidar/src/toid_lidar_node.cpp +++ b/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()); diff --git a/toid_navigation/launch/main.py b/toid_navigation/launch/main.py index 92ee94c..e009910 100644 --- a/toid_navigation/launch/main.py +++ b/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, diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 3069468..48da697 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/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 diff --git a/toid_vision/package.xml b/toid_vision/package.xml index 4893019..4250143 100644 --- a/toid_vision/package.xml +++ b/toid_vision/package.xml @@ -13,7 +13,7 @@ sensor_msgs message_filters cv_bridge - opencv + libopencv-dev rclcpp_components tf2