diff --git a/panther_docking/README.md b/panther_docking/README.md index 5ab935783..47ab38ea5 100644 --- a/panther_docking/README.md +++ b/panther_docking/README.md @@ -9,7 +9,8 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht ## Configuration Files -- [`panther_docking_server.yaml`](./config/panther_docking_server.yaml): Defines parameters for a `docking_server` and a `PantherChargingDock` plugin. +- [`panther_docking_server.yaml`](./config/panther_docking_server.yaml): Defines parameters for a `docking_server` and a `PantherChargingDock` plugin. Defines poses where charging docks are spawned in the Gazebo. + ## ROS Nodes diff --git a/panther_docking/config/apriltag.yaml b/panther_docking/config/apriltag.yaml index 31da6137d..0e5de81ea 100644 --- a/panther_docking/config/apriltag.yaml +++ b/panther_docking/config/apriltag.yaml @@ -6,7 +6,6 @@ size: 0.06 profile: false - max_hamming: 0 detector: threads: 1 @@ -16,7 +15,6 @@ sharpening: 0.25 debug: false - tag: ids: [0, 1] frames: [main_apriltag_link, backup_apriltag_link] diff --git a/panther_docking/config/panther_docking_server.yaml b/panther_docking/config/panther_docking_server.yaml index 0b17f1b39..10aee0db8 100644 --- a/panther_docking/config/panther_docking_server.yaml +++ b/panther_docking/config/panther_docking_server.yaml @@ -36,7 +36,7 @@ type: panther_charging_dock frame: /map dock_frame: backup_wibotic_receiver_requested_pose_link - pose: [-1.0, 1.5, 1.57] # position of the dock on the map. Used also for spawning dock in the simulation. + pose: [-1.0, 1.5, 1.57] # [x, y, yaw] pose of the dock on the map. Used also for spawning dock in the simulation. apriltag_id: 1 controller: diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py index 0daf96092..da25834a0 100644 --- a/panther_docking/launch/docking.launch.py +++ b/panther_docking/launch/docking.launch.py @@ -36,13 +36,6 @@ def generate_launch_description(): description=("Path to docking server configuration file."), ) - declare_use_docking_arg = DeclareLaunchArgument( - "use_docking", - default_value="True", - description="Enable docking server.", - choices=["True", "False", "true", "false"], - ) - apriltag_config_path = LaunchConfiguration("apriltag_config_path") declare_apriltag_config_path_arg = DeclareLaunchArgument( "apriltag_config_path", @@ -170,9 +163,8 @@ def generate_launch_description(): [ declare_apriltag_config_path_arg, declare_docking_server_config_path_arg, - declare_use_docking_arg, - declare_use_wibotic_info_arg, declare_log_level, + declare_use_wibotic_info_arg, station_launch, docking_server_node, docking_server_activate_node, diff --git a/panther_docking/launch/station.launch.py b/panther_docking/launch/station.launch.py index 7f86d1b93..584dad4dd 100644 --- a/panther_docking/launch/station.launch.py +++ b/panther_docking/launch/station.launch.py @@ -128,10 +128,18 @@ def generate_launch_description(): description="Size in meters of a generated apriltag on the station", ) + declare_use_docking_arg = DeclareLaunchArgument( + "use_docking", + default_value="True", + description="Enable docking server.", + choices=["True", "False", "true", "false"], + ) + return LaunchDescription( [ declare_apriltag_id, declare_apriltag_size, + declare_use_docking_arg, OpaqueFunction(function=launch_stations_descriptions), ] ) diff --git a/panther_docking/src/panther_charging_dock.cpp b/panther_docking/src/panther_charging_dock.cpp index f33f0922a..744db7227 100644 --- a/panther_docking/src/panther_charging_dock.cpp +++ b/panther_docking/src/panther_charging_dock.cpp @@ -159,8 +159,10 @@ bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose) auto duration = rclcpp::Time(request_detection_time) - rclcpp::Time(dock_pose_.header.stamp); if (duration > timeout) { RCLCPP_WARN_STREAM( - logger_, "Lost detection or did not detect: timeout exceeded: " - << duration.seconds() << "s timeout: " << timeout.seconds() << "s"); + logger_, "Detection timeout exceeded. Duration since last detection: " + << duration.seconds() << " seconds (timeout threshold: " << timeout.seconds() + << " seconds). " + << "No detection received or lost detection for external detection."); return false; } diff --git a/panther_gazebo/README.md b/panther_gazebo/README.md index a724fb16d..a4d3bb85a 100644 --- a/panther_gazebo/README.md +++ b/panther_gazebo/README.md @@ -14,7 +14,6 @@ The package contains a launch file and source files used to run the robot simula - [`battery_plugin_config.yaml`](./config/battery_plugin_config.yaml): Simulated LinearBatteryPlugin configuration. - [`gz_bridge.yaml`](./config/gz_bridge.yaml): Specify data to exchange between ROS and Gazebo simulation. - [`teleop_with_estop.config`](./config/teleop_with_estop.config): Gazebo layout configuration file, which adds E-Stop and Teleop widgets. -- [`panther_docking_server.yaml`](../panther_docking/config/panther_docking_server.yaml): Defines poses for charging docks. ## ROS Nodes diff --git a/panther_gazebo/launch/apriltag.launch.py b/panther_gazebo/launch/apriltag.launch.py deleted file mode 100644 index deeaa24e6..000000000 --- a/panther_gazebo/launch/apriltag.launch.py +++ /dev/null @@ -1,62 +0,0 @@ -# Copyright 2024 Husarion sp. z o.o. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from nav2_common.launch import ReplaceString - - -def generate_launch_description(): - use_docking = LaunchConfiguration("use_docking") - use_sim = LaunchConfiguration("use_sim") - namespace = LaunchConfiguration("namespace") - - apriltag_config_path = LaunchConfiguration("apriltag_config_path") - apriltag_config_path_arg = DeclareLaunchArgument( - "apriltag_config_path", - default_value=PathJoinSubstitution( - [FindPackageShare("panther_gazebo"), "config", "apriltag.yaml"] - ), - description=("Path to apriltag configuration file."), - ) - - namespaced_apriltag_config_path = ReplaceString( - source_file=apriltag_config_path, - replacements={"": namespace, "//": "/"}, - ) - - apriltag_node = Node( - package="apriltag_ros", - executable="apriltag_node", - parameters=[{"use_sim_time": use_sim}, namespaced_apriltag_config_path], - namespace=namespace, - emulate_tty=True, - remappings={ - "camera_info": "camera/color/camera_info", - "image_rect": "camera/color/image_raw", - "detections": "docking/april_tags", - }.items(), - condition=IfCondition(use_docking), - ) - - return LaunchDescription( - [ - apriltag_config_path_arg, - apriltag_node, - ] - ) diff --git a/panther_gazebo/launch/simulate_robot.launch.py b/panther_gazebo/launch/simulate_robot.launch.py index 8f7fa280c..aa55478a1 100644 --- a/panther_gazebo/launch/simulate_robot.launch.py +++ b/panther_gazebo/launch/simulate_robot.launch.py @@ -198,19 +198,6 @@ def generate_launch_description(): launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), ) - apriltag_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("panther_gazebo"), - "launch", - "apriltag.launch.py", - ] - ), - ), - launch_arguments={"namespace": namespace, "use_sim": "True"}.items(), - ) - return LaunchDescription( [ declare_battery_config_path_arg, @@ -227,6 +214,5 @@ def generate_launch_description(): simulate_components, gz_bridge, docking_launch, - apriltag_launch, ] ) diff --git a/panther_gazebo/package.xml b/panther_gazebo/package.xml index ecadf84cb..184e07c1c 100644 --- a/panther_gazebo/package.xml +++ b/panther_gazebo/package.xml @@ -27,7 +27,6 @@ std_msgs std_srvs - apriltag_ros controller_manager husarion_gz_worlds launch