diff --git a/urc_bringup/launch/autonomy.launch.py b/urc_bringup/launch/autonomy.launch.py new file mode 100644 index 00000000..196b98e4 --- /dev/null +++ b/urc_bringup/launch/autonomy.launch.py @@ -0,0 +1,60 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + pkg_urc_perception = get_package_share_directory("urc_perception") + pkg_trajectory_following = get_package_share_directory("trajectory_following") + + traversability_config = os.path.join(pkg_urc_perception, "config", "traversability_params.yaml") + trajectory_config = os.path.join(pkg_trajectory_following, "config", "pure_pursuit.yaml") + + state_machine_node = Node( + package="nav_testing", + executable="nav_testing_NavCoordinator", + name="nav_coordinator", + output="screen", + ) + + path_planning_node = Node( + package="path_planning", + executable="path_planning_PlannerServer", + name="planner_server", + output="screen", + ) + + trajectory_following_node = Node( + package="trajectory_following", + executable="trajectory_following_FollowerActionServer", + name="follower_action_server", + parameters=[trajectory_config], + output="screen", + ) + + traversability_mapping_node = Node( + package="urc_perception", + executable="urc_perception_TraversabilityMapping", + name="traversability_mapping", + parameters=[traversability_config], + output="screen", + ) + + grid_map_visualization_node = Node( + package="grid_map_visualization", + executable="grid_map_visualization", + name="grid_map_visualization", + output="screen", + parameters=[traversability_config], + ) + + return LaunchDescription( + [ + state_machine_node, + path_planning_node, + trajectory_following_node, + traversability_mapping_node, + grid_map_visualization_node + ] + ) diff --git a/urc_bringup/launch/bringup.launch.py b/urc_bringup/launch/bringup.launch.py index 3b7157f9..e382d91a 100644 --- a/urc_bringup/launch/bringup.launch.py +++ b/urc_bringup/launch/bringup.launch.py @@ -3,8 +3,9 @@ from xacro import process_file import yaml from launch import LaunchDescription -from launch.actions import GroupAction, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node, SetRemap from launch_ros.substitutions import FindPackageShare @@ -44,6 +45,12 @@ def generate_launch_description(): ) robot_desc = robot_description_config.toxml() + autonomy_arg = DeclareLaunchArgument( + "autonomy", + default_value="false", + description="Launch autonomy nodes", + ) + heartbeat_node = Node( package="urc_bringup", executable="urc_bringup_HeartbeatPublisher", @@ -169,8 +176,16 @@ def generate_launch_description(): parameters=[{"port": 9090}], ) + launch_autonomy = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_urc_bringup, "launch", "autonomy.launch.py") + ), + condition=IfCondition(LaunchConfiguration("autonomy")), + ) + return LaunchDescription( [ + autonomy_arg, control_node, load_robot_state_publisher, load_joint_state_broadcaster, @@ -186,5 +201,6 @@ def generate_launch_description(): vectornav_sensor_msg_node, heartbeat_node, sick_node, + launch_autonomy, ] ) diff --git a/urc_bringup/launch/sim.launch.py b/urc_bringup/launch/sim.launch.py index 9142d814..78a3382e 100644 --- a/urc_bringup/launch/sim.launch.py +++ b/urc_bringup/launch/sim.launch.py @@ -47,6 +47,12 @@ def generate_launch_description(): description="Spawn rocker effort controller (Option B)", ) + autonomy_arg = DeclareLaunchArgument( + "autonomy", + default_value="false", + description="Launch autonomy nodes", + ) + world_filename = LaunchConfiguration("world") walli_xacro_config = LaunchConfiguration("walli_xacro") @@ -128,6 +134,13 @@ def generate_launch_description(): ) ) + launch_autonomy = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(path_urc_bringup, "launch", "autonomy.launch.py") + ), + condition=IfCondition(LaunchConfiguration("autonomy")), + ) + @@ -229,12 +242,14 @@ def generate_launch_description(): walli_xacro, bridge_yaml, spawn_rocker_effort, + autonomy_arg, gz_sim, bridge, robot_state_publisher_node, covariances_on_imu, covariances_on_gps, launch_ekf, + launch_autonomy, rocker_tf_broadcaster, rocker_effort_pid_node, spawn, diff --git a/urc_bringup/launch/sim.py b/urc_bringup/launch/sim.py deleted file mode 100644 index e8c15027..00000000 --- a/urc_bringup/launch/sim.py +++ /dev/null @@ -1,172 +0,0 @@ -import os -from tempfile import NamedTemporaryFile -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution -from launch_ros.descriptions import ParameterValue -from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import Node -from xacro import process_file - - -def generate_launch_description(): - path_ros_gazebo_sim = get_package_share_directory("ros_gz_sim") - path_urc_hw_description = get_package_share_directory("urc_hw_description") - path_urc_bringup = get_package_share_directory("urc_bringup") - - controller_config_file_dir = os.path.join( - path_urc_bringup, "config", "test_controllers.yaml" - ) - - sim_world_arg = DeclareLaunchArgument( - "world", - default_value=os.path.join(path_urc_hw_description, "world", "marsyard2020.sdf"), - description="Path to gz world file", - ) - - walli_xacro = DeclareLaunchArgument( - "walli_xacro", - default_value=os.path.join( - path_urc_hw_description, - "urdf/simplified_swerve", - "simplified_swerve.urdf.xacro", - ), - description="Path to xacro file", - ) - - bridge_yaml = DeclareLaunchArgument( - "bridge_yaml", - default_value=os.path.join(path_urc_bringup, "config", "sim_config.yaml"), - description="bridge YAML config", - ) - - world_filename = LaunchConfiguration("world") - walli_xacro_config = LaunchConfiguration("walli_xacro") - - # Construct the full world path from the filename - world_path = PathJoinSubstitution( - [path_urc_hw_description, "world", world_filename] - ) - - """ - robot_urdf_file = process_file( - ParameterValue(walli_xacro_config, value_type = str), - mappings = {"use_sim": "true"} - ).toxml() - """ - - robot_urdf_file = ParameterValue( - Command( - [ - "xacro ", - walli_xacro_config, - " use_sim:=", - "true", - ] - ), - value_type=str, - ) - - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(path_ros_gazebo_sim, "launch", "gz_sim.launch.py") - ), - launch_arguments={"gz_args": world_path}.items(), - ) - - bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="ros_gz_bridge", - output="screen", - parameters=[{"config_file": LaunchConfiguration("bridge_yaml")}], - ) - - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - parameters=[{"robot_description": robot_urdf_file}], - output="screen", - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[controller_config_file_dir], - output="screen", - remappings=[("/controller_manager/robot_description", "/robot_description")], - ) - - load_joint_state_broadcaster = Node( - package="controller_manager", - executable="spawner", - arguments=["-p", controller_config_file_dir, "joint_state_broadcaster"], - ) - - load_position_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["-p", controller_config_file_dir, "position_controller"], - ) - - load_velocity_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["-p", controller_config_file_dir, "velocity_controller"], - ) - - covariances_on_imu = Node( - package="urc_localization", - executable="urc_localization_CovariancesOnImu", - name="covariances_on_imu", - parameters=[ - { - "imu_input_topic": "/imu/data_raw", - "imu_output_topic": "/imu/fused", - } - ], - output="screen", - ) - - spawn = Node( - package="ros_gz_sim", - executable="create", - output="screen", - arguments=[ - "-name", - "walli", - "-x", - "0", - "-y", - "0", - "-z", - "1.5", - "-R", - "0", - "-P", - "0", - "-Y", - "0", - "-topic", - "robot_description", - ], - ) - - return LaunchDescription( - [ - sim_world_arg, - walli_xacro, - gz_sim, - spawn, - covariances_on_imu, - bridge_yaml, - bridge, - control_node, - robot_state_publisher_node, - load_joint_state_broadcaster, - load_position_controller, - load_velocity_controller, - ] - ) diff --git a/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro b/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro index 2b58b43b..d988aae1 100644 --- a/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro +++ b/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro @@ -50,21 +50,21 @@ - 720 + 2880 1 -3.14 3.14 - 720 + 144 1 - -3.14 - 3.14 + -0.366519 + 0.366519 0.05 - 10.0 + 65.0 0.01 diff --git a/urc_localization/config/ekf_redemption.yaml b/urc_localization/config/ekf_redemption.yaml index 469911d6..595f0707 100644 --- a/urc_localization/config/ekf_redemption.yaml +++ b/urc_localization/config/ekf_redemption.yaml @@ -76,6 +76,7 @@ navsat_transform: yaw_offset: 0.0 zero_altitude: true broadcast_cartesian_transform: false + broadcast_utm_transform: true publish_filtered_gps: true use_odometry_yaw: false wait_for_datum: false diff --git a/urc_navigation/grid_map_utils/CMakeLists.txt b/urc_navigation/grid_map_utils/CMakeLists.txt new file mode 100644 index 00000000..9e70ebac --- /dev/null +++ b/urc_navigation/grid_map_utils/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.5) +project(grid_map_utils) + +include(../../cmake/default_settings.cmake) + +find_package(ament_cmake REQUIRED) +find_package(grid_map_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/grid_map_utils.cpp +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +ament_target_dependencies(${PROJECT_NAME} + grid_map_msgs +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(grid_map_msgs) + +ament_package() diff --git a/urc_navigation/grid_map_utils/include/grid_map_utils/grid_map_utils.hpp b/urc_navigation/grid_map_utils/include/grid_map_utils/grid_map_utils.hpp new file mode 100644 index 00000000..302f76cb --- /dev/null +++ b/urc_navigation/grid_map_utils/include/grid_map_utils/grid_map_utils.hpp @@ -0,0 +1,29 @@ +#ifndef GRID_MAP_UTILS__GRID_MAP_UTILS_HPP_ +#define GRID_MAP_UTILS__GRID_MAP_UTILS_HPP_ + +#include + +#include + +namespace grid_map_utils +{ +class GridMapUtils +{ +public: + GridMapUtils(); + + void setMap(const grid_map_msgs::msg::GridMap & grid_map); + void setLayer(const std::string & layer_name); + + int getLayerIndex() const; + bool getMapDimensions(int & width, int & height) const; + bool worldToGrid(double x, double y, int & map_x, int & map_y) const; + bool tryGetCellCost(double x, double y, float & cost) const; + +private: + grid_map_msgs::msg::GridMap grid_map_; + std::string layer_name_; +}; +} // namespace grid_map_utils + +#endif // GRID_MAP_UTILS__GRID_MAP_UTILS_HPP_ diff --git a/urc_navigation/grid_map_utils/package.xml b/urc_navigation/grid_map_utils/package.xml new file mode 100644 index 00000000..38305bcc --- /dev/null +++ b/urc_navigation/grid_map_utils/package.xml @@ -0,0 +1,20 @@ + + + + grid_map_utils + 0.0.0 + Shared GridMap access utilities + yambati03 + MIT + + ament_cmake + + grid_map_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/urc_navigation/grid_map_utils/src/grid_map_utils.cpp b/urc_navigation/grid_map_utils/src/grid_map_utils.cpp new file mode 100644 index 00000000..9512e4d6 --- /dev/null +++ b/urc_navigation/grid_map_utils/src/grid_map_utils.cpp @@ -0,0 +1,101 @@ +#include "grid_map_utils/grid_map_utils.hpp" + +#include + +namespace grid_map_utils +{ +GridMapUtils::GridMapUtils() {} + +void GridMapUtils::setMap(const grid_map_msgs::msg::GridMap & grid_map) +{ + grid_map_ = grid_map; +} + +void GridMapUtils::setLayer(const std::string & layer_name) +{ + layer_name_ = layer_name; +} + +int GridMapUtils::getLayerIndex() const +{ + auto it = std::find(grid_map_.layers.begin(), grid_map_.layers.end(), layer_name_); + if (it == grid_map_.layers.end()) { + return -1; + } + return static_cast(std::distance(grid_map_.layers.begin(), it)); +} + +bool GridMapUtils::getMapDimensions(int & width, int & height) const +{ + int idx = getLayerIndex(); + if (idx < 0) { + return false; + } + if (grid_map_.data.empty() || idx >= static_cast(grid_map_.data.size())) { + return false; + } + if (grid_map_.data[idx].layout.dim.size() < 2) { + return false; + } + + height = grid_map_.data[idx].layout.dim[0].size; + width = grid_map_.data[idx].layout.dim[1].size; + return true; +} + +bool GridMapUtils::worldToGrid(double x, double y, int & map_x, int & map_y) const +{ + int width, height; + if (!getMapDimensions(width, height)) { + return false; + } + + double resolution = grid_map_.info.resolution; + if (resolution <= 0.0) { + return false; + } + + double origin_x = grid_map_.info.pose.position.x; + double origin_y = grid_map_.info.pose.position.y; + double rel_x = x - origin_x; + double rel_y = y - origin_y; + + map_x = static_cast(rel_x / resolution + width / 2.0); + map_y = static_cast(rel_y / resolution + height / 2.0); + + if (map_x < 0 || map_x >= width || map_y < 0 || map_y >= height) { + return false; + } + + return true; +} + +bool GridMapUtils::tryGetCellCost(double x, double y, float & cost) const +{ + int idx = getLayerIndex(); + if (idx < 0) { + return false; + } + if (grid_map_.data.empty() || idx >= static_cast(grid_map_.data.size())) { + return false; + } + + int width, height; + if (!getMapDimensions(width, height)) { + return false; + } + + int map_x, map_y; + if (!worldToGrid(x, y, map_x, map_y)) { + return false; + } + + size_t index = static_cast(map_y * width + map_x); + if (index >= grid_map_.data[idx].data.size()) { + return false; + } + + cost = grid_map_.data[idx].data[index]; + return true; +} +} // namespace grid_map_utils diff --git a/urc_navigation/nav_testing/CMakeLists.txt b/urc_navigation/nav_testing/CMakeLists.txt index 9bac1f84..5f4ec46c 100644 --- a/urc_navigation/nav_testing/CMakeLists.txt +++ b/urc_navigation/nav_testing/CMakeLists.txt @@ -1,26 +1,77 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.5) project(nav_testing) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() +include(../../cmake/default_settings.cmake) # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(geodesy REQUIRED) +find_package(geographic_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(urc_msgs REQUIRED) + +include_directories( + include +) + +# Library creation +add_library(${PROJECT_NAME} SHARED + src/nav_coordinator.cpp +) + +set(dependencies + rclcpp + rclcpp_action + rclcpp_components + geometry_msgs + geodesy + geographic_msgs + std_msgs + tf2_geometry_msgs + tf2_ros + urc_msgs +) + +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} +) + +# Node registration +rclcpp_components_register_node( + ${PROJECT_NAME} + PLUGIN "nav_coordinator::NavCoordinator" + EXECUTABLE ${PROJECT_NAME}_NavCoordinator +) + +# Install library +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files + + # the following line skips the copyright linker set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() endif() +ament_export_include_directories(include) + +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) + ament_package() diff --git a/urc_navigation/nav_testing/include/nav_coordinator.hpp b/urc_navigation/nav_testing/include/nav_coordinator.hpp index 3e4ab5b0..03b2af15 100644 --- a/urc_navigation/nav_testing/include/nav_coordinator.hpp +++ b/urc_navigation/nav_testing/include/nav_coordinator.hpp @@ -1,37 +1,92 @@ #ifndef NAV_COORDINATOR_HPP_ #define NAV_COORDINATOR_HPP_ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + namespace nav_coordinator { class NavCoordinator : public rclcpp::Node { - public: - explicit NavCoordinator(const rclcpp::NodeOptions & options); - ~NavCoordinator(); +public: + explicit NavCoordinator(const rclcpp::NodeOptions & options); + ~NavCoordinator() override = default; - private: - // --- Subscribers --- // - rclcpp::Subscription::SharedPtr localization_subscriber_; - rclcpp::Subscription::SharedPtr goal_subscriber_; - rclcpp::Subscription::SharedPtr cancel_subscriber_; +private: + using NavigateToWaypoint = urc_msgs::action::NavigateToWaypoint; + using GoalHandleNavigate = rclcpp_action::ClientGoalHandle; - // --- Publishers --- // + enum class State + { + IDLE, + WAITING_FOR_SERVER, + SENDING_GOAL, + TRACKING_GOAL, + SUCCEEDED, + FAILED, + CANCELED + }; - // --- Planner Service Client --- // - rclcpp::Client::SharedPtr planner_client_; + enum class ErrorType + { + NONE, + PLANNER_FAILURE, + OBSTACLE_DETECTED, + PLANNING_FAILED_IN_FOLLOWER, + FOLLOWER_FAILURE, + SERVER_UNAVAILABLE, + UNKNOWN_ERROR + }; - rclcpp::TimerBase::SharedPtr timer_; + void handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr msg); + void sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint); + geometry_msgs::msg::PoseStamped convertGpsToMapWaypoint( + const urc_msgs::msg::Waypoint & waypoint); - // TODO + void handleGoalResponse(const GoalHandleNavigate::SharedPtr & goal_handle); + void handleFeedback( + GoalHandleNavigate::SharedPtr, + const std::shared_ptr feedback); + void handleResult(const GoalHandleNavigate::WrappedResult & result); -} + void transitionTo(State new_state, const std::string & reason); + void handleError(ErrorType error_type, const std::string & details); + void publishState(); + std::string errorTypeToString(ErrorType error_type) const; + std::string stateToString(State state) const; - // void timerCallback(); + State state_; + std::string follower_action_name_; + bool cancel_on_new_waypoint_; + std::string map_frame_id_; + std::string utm_frame_id_; - void handleLocalization(const TODO::SharedPtr msg); - void handleGoal(const TODO::SharedPtr msg); - void handleStart(const TODO::SharedPtr msg); + geometry_msgs::msg::PoseStamped active_waypoint_; + GoalHandleNavigate::SharedPtr active_goal_handle_; -} // namespace nav_coordinator + rclcpp::Subscription::SharedPtr waypoint_subscriber_; + rclcpp::Subscription::SharedPtr gps_waypoint_subscriber_; + rclcpp_action::Client::SharedPtr follower_client_; + rclcpp::Publisher::SharedPtr state_publisher_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + ErrorType last_error_; + std::string last_error_details_; +}; + +} -#endif // NAV_COORDINATOR_HPP_ \ No newline at end of file +#endif \ No newline at end of file diff --git a/urc_navigation/nav_testing/package.xml b/urc_navigation/nav_testing/package.xml index 07e85c22..6455f3f9 100644 --- a/urc_navigation/nav_testing/package.xml +++ b/urc_navigation/nav_testing/package.xml @@ -8,6 +8,13 @@ MIT ament_cmake + rclcpp + rclcpp_action + geometry_msgs + geodesy + geographic_msgs + std_msgs + urc_msgs ament_lint_auto ament_lint_common diff --git a/urc_navigation/nav_testing/src/nav_coordinator.cpp b/urc_navigation/nav_testing/src/nav_coordinator.cpp index 8df0f375..8e43cec7 100644 --- a/urc_navigation/nav_testing/src/nav_coordinator.cpp +++ b/urc_navigation/nav_testing/src/nav_coordinator.cpp @@ -1,193 +1,359 @@ -#include -#include - #include "nav_coordinator.hpp" -#include "astar.hpp" + +#include +#include +#include +#include namespace nav_coordinator { NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) : rclcpp::Node("nav_coordinator", options) { - RCLCPP_INFO(get_logger(), "Nav Coordinator started."); - - // --- Nav Action Server --- // - // to mannually send a goal, use - // ros2 action send_goal /nav_coordinator urc_msgs/action/NavToGoal "{path: {poses: [{header: {frame_id: 'map'}, pose: {position: {x: 1.0, y: 1.0, z: 0,0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}]}}" - nav_action_server_ = rclcpp_action::create_server( - this, - "nav_coordinator", - std::bind( - &NavCoordinator::handle_goal, this, std::placeholders::_1, - std::placeholders::_2), - std::bind(&NavCoordinator::handle_cancel, this, std::placeholders::_1), - std::bind(&NavCoordinator::handle_accepted, this, std::placeholders::_1)); - - // --- Subscribers --- // - // Create localization subscriber - localization_subscriber_ = create_subscription( - "/rover_ground_truth", //TODO_localization_topic - rclcpp::SystemDefaultsQoS(), - [this](const nav_msgs::msg::Odometry::SharedPtr msg) - { - geometry_msgs::msg::PoseStamped pose; - pose.header = msg->header; - pose.pose = msg->pose.pose; - current_pose_ = pose; - }); - - - // --- Planner Service Client --- // - /* IN PROGRESS - planner_client_ = rclcpp::create_client( - this, - "/plan" - ); - std::thread(std::bind(&NavCoordinator::sendPlannerRequest, this)).detach(); - */ - - // // --- NavCoordinator Timer Callback --- // - // // one second timer callback - // timer_ = this->create_wall_timer( - // std::chrono::seconds(1), - // std::bind(&NavCoordinator::timerCallback, this) - // ); + declare_parameter("waypoint_topic", "/nav/waypoint"); + declare_parameter("gps_waypoint_topic", "/waypoint"); + declare_parameter("follower_action_name", "navigate_to_waypoint"); + declare_parameter("cancel_on_new_waypoint", true); + declare_parameter("map_frame_id", "map"); + declare_parameter("utm_frame_id", "utm"); + + follower_action_name_ = get_parameter("follower_action_name").as_string(); + cancel_on_new_waypoint_ = get_parameter("cancel_on_new_waypoint").as_bool(); + map_frame_id_ = get_parameter("map_frame_id").as_string(); + utm_frame_id_ = get_parameter("utm_frame_id").as_string(); + state_ = State::IDLE; + last_error_ = ErrorType::NONE; + + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + state_publisher_ = create_publisher("nav_coordinator_state", 10); + follower_client_ = rclcpp_action::create_client(this, follower_action_name_); + waypoint_subscriber_ = create_subscription( + get_parameter("waypoint_topic").as_string(), + rclcpp::SystemDefaultsQoS(), + std::bind(&NavCoordinator::handleWaypoint, this, std::placeholders::_1)); + + gps_waypoint_subscriber_ = create_subscription( + get_parameter("gps_waypoint_topic").as_string(), + rclcpp::SystemDefaultsQoS(), + std::bind(&NavCoordinator::handleGpsWaypoint, this, std::placeholders::_1)); + + RCLCPP_INFO( + get_logger(), + "Nav Coordinator ready. Pose waypoints on '%s', GPS waypoints on '%s', forwarding to action '%s'.", + get_parameter("waypoint_topic").as_string().c_str(), + get_parameter("gps_waypoint_topic").as_string().c_str(), + follower_action_name_.c_str()); + + if (state_publisher_) { + publishState(); + } } -// --- nav action server functions --- // +void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +{ + active_waypoint_ = *msg; + RCLCPP_INFO( + get_logger(), "Received waypoint: frame=%s x=%.3f y=%.3f", + active_waypoint_.header.frame_id.c_str(), + active_waypoint_.pose.position.x, + active_waypoint_.pose.position.y); -rclcpp_action::GoalResponse NavCoordinator::handle_goal( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) + if (active_goal_handle_ && cancel_on_new_waypoint_) { + transitionTo(State::CANCELED, "canceling current goal due to new waypoint"); + follower_client_->async_cancel_goal(active_goal_handle_); + active_goal_handle_.reset(); + } + + sendFollowerGoal(active_waypoint_); +} + +void NavCoordinator::handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received goal request"); + geometry_msgs::msg::PoseStamped converted_waypoint; + try { + converted_waypoint = convertGpsToMapWaypoint(*msg); + } catch (const std::exception & ex) { + handleError( + ErrorType::PLANNER_FAILURE, + std::string("Cannot process GPS waypoint: ") + ex.what()); + transitionTo(State::FAILED, "gps waypoint rejected - transform unavailable"); + return; + } - (void)uuid; - (void)goal; + active_waypoint_ = converted_waypoint; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + RCLCPP_INFO( + get_logger(), + "Received GPS waypoint: lat=%.8f lon=%.8f -> frame=%s x=%.3f y=%.3f", + msg->latitude, + msg->longitude, + active_waypoint_.header.frame_id.c_str(), + active_waypoint_.pose.position.x, + active_waypoint_.pose.position.y); + + if (active_goal_handle_ && cancel_on_new_waypoint_) { + transitionTo(State::CANCELED, "canceling current goal due to new GPS waypoint"); + follower_client_->async_cancel_goal(active_goal_handle_); + active_goal_handle_.reset(); + } + + sendFollowerGoal(active_waypoint_); } -rclcpp_action::CancelResponse NavCoordinator::handle_cancel( - const std::shared_ptr> goal_handle) +geometry_msgs::msg::PoseStamped NavCoordinator::convertGpsToMapWaypoint( + const urc_msgs::msg::Waypoint & waypoint) { - RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = waypoint.latitude; + geo_point.longitude = waypoint.longitude; + geo_point.altitude = 0.0; + + geodesy::UTMPoint waypoint_utm; + geodesy::fromMsg(geo_point, waypoint_utm); - (void)goal_handle; + geometry_msgs::msg::PointStamped utm_point; + utm_point.header.stamp = now(); + utm_point.header.frame_id = utm_frame_id_; + utm_point.point.x = waypoint_utm.easting; + utm_point.point.y = waypoint_utm.northing; + utm_point.point.z = 0.0; - return rclcpp_action::CancelResponse::ACCEPT; + geometry_msgs::msg::PointStamped map_point; + try { + tf_buffer_->transform(utm_point, map_point, map_frame_id_); + } catch (const tf2::TransformException & ex) { + throw std::runtime_error( + "Failed to transform waypoint from '" + utm_frame_id_ + "' to '" + map_frame_id_ + "': " + + ex.what()); + } + + geometry_msgs::msg::PoseStamped pose; + pose.header = map_point.header; + pose.pose.position.x = map_point.point.x; + pose.pose.position.y = map_point.point.y; + pose.pose.position.z = map_point.point.z; + pose.pose.orientation.w = 1.0; + + return pose; +} + +void NavCoordinator::sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint) +{ + transitionTo(State::WAITING_FOR_SERVER, "checking follower action server"); + if (!follower_client_->wait_for_action_server(std::chrono::seconds(2))) { + handleError( + ErrorType::SERVER_UNAVAILABLE, + "Follower action server '" + follower_action_name_ + "' not available."); + transitionTo(State::FAILED, "follower action server unavailable"); + return; + } + + NavigateToWaypoint::Goal goal_msg; + goal_msg.goal = waypoint; + goal_msg.has_goal = true; + goal_msg.has_path = false; + goal_msg.enforce_goal_heading = false; + + transitionTo(State::SENDING_GOAL, "forwarding waypoint to follower"); + + rclcpp_action::Client::SendGoalOptions options; + options.goal_response_callback = std::bind( + &NavCoordinator::handleGoalResponse, this, std::placeholders::_1); + options.feedback_callback = std::bind( + &NavCoordinator::handleFeedback, this, std::placeholders::_1, std::placeholders::_2); + options.result_callback = std::bind( + &NavCoordinator::handleResult, this, std::placeholders::_1); + + follower_client_->async_send_goal(goal_msg, options); } -void NavCoordinator::handle_accepted( - const std::shared_ptr> goal_handle) +void NavCoordinator::handleGoalResponse(const GoalHandleNavigate::SharedPtr & goal_handle) { - // this needs to return quickly to avoid blocking the executor, so spin up a new thread - std::thread{std::bind(&NavCoordinator::execute, this, std::placeholders::_1), - goal_handle} - .detach(); + if (!goal_handle) { + handleError(ErrorType::FOLLOWER_FAILURE, "Follower action server rejected the goal."); + transitionTo(State::FAILED, "follower rejected goal"); + return; + } + + active_goal_handle_ = goal_handle; + transitionTo(State::TRACKING_GOAL, "follower accepted goal"); } -/* -void NavCoordinator::executeTODO( - const std::shared_ptr> goal_handle) + +void NavCoordinator::handleFeedback( + GoalHandleNavigate::SharedPtr, + const std::shared_ptr feedback) { - RCLCPP_INFO(this->get_logger(), "Executing goal"); - - auto feedback = std::make_shared(); - feedback->distance_to_goal = std::numeric_limits::max(); - - auto result = std::make_shared(); - auto & path = goal_handle->get_goal()->path; - - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_INFO(this->get_logger(), "Follower Goal has been canceled"); + RCLCPP_DEBUG( + get_logger(), "Feedback: dist=%.2f planning=%s replans=%u", + feedback->distance_to_goal, + feedback->is_planning ? "true" : "false", + feedback->replan_count); +} + +void NavCoordinator::handleResult(const GoalHandleNavigate::WrappedResult & result) +{ + active_goal_handle_.reset(); + + if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { + if (result.result->error_code == NavigateToWaypoint::Result::SUCCESS) { + transitionTo(State::SUCCEEDED, "follower reported success"); + return; + } + + switch (result.result->error_code) { + case NavigateToWaypoint::Result::OBSTACLE_DETECTED: + handleError(ErrorType::OBSTACLE_DETECTED, "Obstacle detected during trajectory following."); break; - // } else if (feedback->distance_to_goal < get_parameter("goal_tolerance").as_double()) { - } else if (TODO_next_path_point_reached_condition) { - // result->error_code = urc_msgs::action::NavToGoal::Result::SUCCESS; - // goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Moving to next path point"); - sendNextPathPoint(); - } else if (TODO_last_path_point_reached_condition) { - result->error_code = urc_msgs::action::NavToGoal::Result::SUCCESS; - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Final Goal has been reached!"); + case NavigateToWaypoint::Result::PLANNING_FAILED: + handleError(ErrorType::PLANNING_FAILED_IN_FOLLOWER, "Path planning failed in follower."); break; - // } else if (getCost( - // current_costmap_, output.lookahead_point.point.x, - // output.lookahead_point.point.y) > get_parameter("lethal_cost_threshold").as_int()) - // } else if (TODO_obstacle_detected_condition || TODO_new_goal_received) // repath - { - RCLCPP_INFO(this->get_logger(), "Replanning Path: Obstacle detected!"); - // TODO: Call planner service to generate new path - sendPlannerRequest(); + case NavigateToWaypoint::Result::FAILURE: + handleError(ErrorType::FOLLOWER_FAILURE, "Follower reported generic failure."); break; - } else if (TODO_path_planner_failure) { - result->error_code = urc_msgs::action::TODO::Result::PLANNER_FAILURE; - goal_handle->abort(result); - RCLCPP_INFO(this->get_logger(), "Path Planner Failure!"); + default: + handleError(ErrorType::UNKNOWN_ERROR, "Follower finished with error_code=" + std::to_string(result.result->error_code)); break; } + transitionTo(State::FAILED, "follower finished with error"); + return; + } + + if (result.code == rclcpp_action::ResultCode::ABORTED) { + handleError(ErrorType::FOLLOWER_FAILURE, "Follower aborted goal."); + transitionTo(State::FAILED, "follower aborted goal"); + return; + } + + if (result.code == rclcpp_action::ResultCode::CANCELED) { + transitionTo(State::CANCELED, "follower canceled goal"); + return; + } + + handleError(ErrorType::UNKNOWN_ERROR, "Unknown follower result code: " + std::to_string(static_cast(result.code))); + transitionTo(State::FAILED, "unknown follower result code"); } -*/ -void NavCoordinator::execute( - const std::shared_ptr> goal_handle) +void NavCoordinator::transitionTo(State new_state, const std::string & reason) { - RCLCPP_INFO(this->get_logger(), "Executing goal"); - - auto feedback = std::make_shared(); - feedback->distance_to_goal = std::numeric_limits::max(); - - auto result = std::make_shared(); - auto & path = goal_handle->get_goal()->path; - - while (rclcpp::ok()) { - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_INFO(this->get_logger(), "Follower Goal has been canceled"); - break; - // } else if (feedback->distance_to_goal < get_parameter("goal_tolerance").as_double()) { - } else if (goal_handle->) { - result->error_code = urc_msgs::action::NavToGoal::Result::SUCCESS; - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Final Goal has been reached!"); - break; + if (state_ == new_state) { + return; + } + + const auto state_name = [](State state) -> const char* { + switch (state) { + case State::IDLE: + return "IDLE"; + case State::WAITING_FOR_SERVER: + return "WAITING_FOR_SERVER"; + case State::SENDING_GOAL: + return "SENDING_GOAL"; + case State::TRACKING_GOAL: + return "TRACKING_GOAL"; + case State::SUCCEEDED: + return "SUCCEEDED"; + case State::FAILED: + return "FAILED"; + case State::CANCELED: + return "CANCELED"; + default: + return "UNKNOWN"; } + }; + + RCLCPP_INFO( + get_logger(), "State transition: %s -> %s (%s)", state_name(state_), + state_name(new_state), reason.c_str()); + state_ = new_state; + publishState(); } -// --- planner service request --- // -/* IN PROGRESS -void NavCoordinator::sendPlannerRequest() +void NavCoordinator::handleError(ErrorType error_type, const std::string & details) { - RCLCPP_INFO(this->get_logger(), "Waiting for planner service..."); - // if (!planner_client_->wait_for_service(std::chrono::seconds(10))){ - // RCLCPP_ERROR(this->get_logger(), "Couldn't find planner service"); - // return; - // } - while (!planner_client_->wait_for_action_server(std::chrono::seconds(1))) { - RCLCPP_INFO(this->get_logger(), "Waiting for planner server..."); - } - RCLCPP_INFO(this->get_logger(), "Planner service ready"); - auto planner_request = std::make_shared(); - - // TODO Request stuff - // Request: Start pose and goal pose (geometry_msgs/PoseStamped) - planner_request->start.pose = TODO_start_msg_; // rover starting position - planner_request->goal.pose = TODO_goal_msg_; - - auto result = planner_client_->async_send_request(planner_request); - RCLCPP_INFO(this->get_logger(), "Planner request sent"); - if (result.wait_for(std::chrono::seconds(10)) == std::future_status::timeout) { - RCLCPP_ERROR(this->get_logger(), "Planner service took too long to complete!"); - return; - } - RCLCPP_INFO(this->get_logger(), "Planner service completed"); - + last_error_ = error_type; + last_error_details_ = details; + + switch (error_type) { + case ErrorType::PLANNER_FAILURE: + RCLCPP_ERROR(get_logger(), "[PLANNER_FAILURE] %s", details.c_str()); + break; + case ErrorType::OBSTACLE_DETECTED: + RCLCPP_WARN(get_logger(), "[OBSTACLE_DETECTED] %s", details.c_str()); + break; + case ErrorType::PLANNING_FAILED_IN_FOLLOWER: + RCLCPP_ERROR(get_logger(), "[PLANNING_FAILED] %s", details.c_str()); + break; + case ErrorType::FOLLOWER_FAILURE: + RCLCPP_ERROR(get_logger(), "[FOLLOWER_FAILURE] %s", details.c_str()); + break; + case ErrorType::SERVER_UNAVAILABLE: + RCLCPP_ERROR(get_logger(), "[SERVER_UNAVAILABLE] %s", details.c_str()); + break; + case ErrorType::UNKNOWN_ERROR: + RCLCPP_ERROR(get_logger(), "[UNKNOWN_ERROR] %s", details.c_str()); + break; + default: + RCLCPP_ERROR(get_logger(), "[UNHANDLED_ERROR] %s", details.c_str()); + break; + } + publishState(); +} + +std::string NavCoordinator::errorTypeToString(ErrorType error_type) const +{ + switch (error_type) { + case ErrorType::NONE: + return "NONE"; + case ErrorType::PLANNER_FAILURE: + return "PLANNER_FAILURE"; + case ErrorType::OBSTACLE_DETECTED: + return "OBSTACLE_DETECTED"; + case ErrorType::PLANNING_FAILED_IN_FOLLOWER: + return "PLANNING_FAILED"; + case ErrorType::FOLLOWER_FAILURE: + return "FOLLOWER_FAILURE"; + case ErrorType::SERVER_UNAVAILABLE: + return "SERVER_UNAVAILABLE"; + case ErrorType::UNKNOWN_ERROR: + return "UNKNOWN_ERROR"; + default: + return "UNHANDLED"; + } +} + +std::string NavCoordinator::stateToString(State state) const +{ + switch (state) { + case State::IDLE: + return "IDLE"; + case State::WAITING_FOR_SERVER: + return "WAITING_FOR_SERVER"; + case State::SENDING_GOAL: + return "SENDING_GOAL"; + case State::TRACKING_GOAL: + return "TRACKING_GOAL"; + case State::SUCCEEDED: + return "SUCCEEDED"; + case State::FAILED: + return "FAILED"; + case State::CANCELED: + return "CANCELED"; + default: + return "UNKNOWN"; + } +} + +void NavCoordinator::publishState() +{ + auto msg = std::make_shared(); + msg->data = "state=" + stateToString(state_) + " error=" + errorTypeToString(last_error_); + if (!last_error_details_.empty()) { + msg->data += " details=" + last_error_details_; + } + state_publisher_->publish(*msg); } -*/ } // namespace nav_coordinator diff --git a/urc_navigation/path_planning/CMakeLists.txt b/urc_navigation/path_planning/CMakeLists.txt index 1affe8f2..6e3338d0 100644 --- a/urc_navigation/path_planning/CMakeLists.txt +++ b/urc_navigation/path_planning/CMakeLists.txt @@ -5,11 +5,13 @@ include(../../cmake/default_settings.cmake) # find dependencies find_package(ament_cmake REQUIRED) +find_package(grid_map_utils REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(urc_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(grid_map_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(OpenCV 4 REQUIRED) @@ -32,8 +34,10 @@ add_library(${PROJECT_NAME} SHARED set(dependencies rclcpp rclcpp_components + grid_map_utils urc_msgs std_msgs + grid_map_msgs sensor_msgs diagnostic_updater cv_bridge @@ -61,6 +65,11 @@ install( DESTINATION share/${PROJECT_NAME}/ ) +install( + DIRECTORY include/ + DESTINATION include +) + # Install library install(TARGETS ${PROJECT_NAME} diff --git a/urc_navigation/path_planning/include/astar.hpp b/urc_navigation/path_planning/include/astar.hpp index fe32fcc3..88a99b05 100644 --- a/urc_navigation/path_planning/include/astar.hpp +++ b/urc_navigation/path_planning/include/astar.hpp @@ -5,7 +5,8 @@ #include #include #include -#include +#include +#include "grid_map_utils/grid_map_utils.hpp" #include #include #include @@ -72,7 +73,12 @@ class AStar /** * @brief Set the costmap for the A* algorithm */ - void setMap(const nav_msgs::msg::OccupancyGrid & costmap); + void setMap(const grid_map_msgs::msg::GridMap & costmap); + + /** + * @brief Set the GridMap layer to use for traversal costs + */ + void setCostmapLayer(const std::string & layer); /** * @brief Get the path generated by the A* algorithm @@ -112,7 +118,25 @@ class AStar void setGoalNode(const geometry_msgs::msg::Pose & goal_pose); - nav_msgs::msg::OccupancyGrid costmap_; + int getLayerIndex() const; + + bool getMapDimensions(int &width, int &height) const; + + bool worldToGrid(double x, double y, int &map_x, int &map_y) const; + + double getCellCost(double x, double y) const; + + bool clipGoalToCostmapBoundary( + const geometry_msgs::msg::Pose & start_pose, + const geometry_msgs::msg::Pose & goal_pose, + geometry_msgs::msg::Pose & clipped_goal) const; + + void appendStraightLineSegment( + const geometry_msgs::msg::Pose & from_pose, + const geometry_msgs::msg::Pose & to_pose); + + grid_map_msgs::msg::GridMap costmap_; + grid_map_utils::GridMapUtils grid_map_utils_; geometry_msgs::msg::Pose start_pose_; geometry_msgs::msg::Pose goal_pose_; AStarNode goal_node_; @@ -120,6 +144,7 @@ class AStar std::vector path_; std::unordered_map closed_set_; const double EPSILON = 0.5; + std::string costmap_layer_ = "traversability_inflated"; }; } // namespace astar diff --git a/urc_navigation/path_planning/include/planner_server.hpp b/urc_navigation/path_planning/include/planner_server.hpp index 7ec00b0a..d6d05c77 100644 --- a/urc_navigation/path_planning/include/planner_server.hpp +++ b/urc_navigation/path_planning/include/planner_server.hpp @@ -2,7 +2,7 @@ #define PLANNER_SERVER_HPP_ #include -#include +#include #include #include "urc_msgs/srv/generate_plan.hpp" @@ -39,10 +39,10 @@ class PlannerServer : public rclcpp::Node * @brief Handle the costmap data * @param msg The costmap data */ - void handleCostmap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + void handleCostmap(const grid_map_msgs::msg::GridMap::SharedPtr msg); - nav_msgs::msg::OccupancyGrid current_costmap_; - rclcpp::Subscription::SharedPtr costmap_subscriber_; + grid_map_msgs::msg::GridMap current_costmap_; + rclcpp::Subscription::SharedPtr costmap_subscriber_; rclcpp::Publisher::SharedPtr plan_publisher_; rclcpp::Service::SharedPtr plan_service_; }; diff --git a/urc_navigation/path_planning/package.xml b/urc_navigation/path_planning/package.xml index 1d478217..be56e2e3 100644 --- a/urc_navigation/path_planning/package.xml +++ b/urc_navigation/path_planning/package.xml @@ -9,6 +9,7 @@ ament_cmake + grid_map_utils rclcpp rclcpp_components urc_msgs @@ -17,6 +18,7 @@ image_transport std_msgs nav_msgs + grid_map_msgs tf2 tf2_geometry_msgs sensor_msgs @@ -30,4 +32,4 @@ ament_cmake - \ No newline at end of file + diff --git a/urc_navigation/path_planning/src/astar.cpp b/urc_navigation/path_planning/src/astar.cpp index cf8add1d..becd7173 100644 --- a/urc_navigation/path_planning/src/astar.cpp +++ b/urc_navigation/path_planning/src/astar.cpp @@ -7,15 +7,113 @@ AStar::AStar() {} Coordinate AStar::getCoordinateByPose(const geometry_msgs::msg::Pose & pose) { - int x = (pose.position.x - costmap_.info.origin.position.x) / costmap_.info.resolution; - int y = (pose.position.y - costmap_.info.origin.position.y) / costmap_.info.resolution; + int map_x, map_y; + if (!worldToGrid(pose.position.x, pose.position.y, map_x, map_y)) { + throw std::runtime_error("Pose is outside costmap bounds"); + } - return {x, y}; + return {map_x, map_y}; } -void AStar::setMap(const nav_msgs::msg::OccupancyGrid & costmap) +void AStar::setMap(const grid_map_msgs::msg::GridMap & costmap) { costmap_ = costmap; + grid_map_utils_.setMap(costmap); +} + +void AStar::setCostmapLayer(const std::string & layer) +{ + costmap_layer_ = layer; + grid_map_utils_.setLayer(layer); +} + +int AStar::getLayerIndex() const +{ + return grid_map_utils_.getLayerIndex(); +} + +bool AStar::getMapDimensions(int & width, int & height) const +{ + return grid_map_utils_.getMapDimensions(width, height); +} + +bool AStar::worldToGrid(double x, double y, int & map_x, int & map_y) const +{ + return grid_map_utils_.worldToGrid(x, y, map_x, map_y); +} + +double AStar::getCellCost(double x, double y) const +{ + float cost; + if (!grid_map_utils_.tryGetCellCost(x, y, cost)) { + throw std::runtime_error("Unable to get cell cost"); + } + + return cost; +} + +bool AStar::clipGoalToCostmapBoundary( + const geometry_msgs::msg::Pose & start_pose, + const geometry_msgs::msg::Pose & goal_pose, + geometry_msgs::msg::Pose & clipped_goal) const +{ + double resolution = costmap_.info.resolution; + if (resolution <= 0.0) { + return false; + } + + double dx = goal_pose.position.x - start_pose.position.x; + double dy = goal_pose.position.y - start_pose.position.y; + double distance = std::sqrt((dx * dx) + (dy * dy)); + + clipped_goal = start_pose; + clipped_goal.orientation = goal_pose.orientation; + if (distance <= 0.0) { + return true; + } + + for (double traveled = resolution; traveled <= distance; traveled += resolution) { + double t = std::min(1.0, traveled / distance); + geometry_msgs::msg::Pose candidate = start_pose; + candidate.position.x = start_pose.position.x + (dx * t); + candidate.position.y = start_pose.position.y + (dy * t); + candidate.orientation = goal_pose.orientation; + + int map_x, map_y; + if (!worldToGrid(candidate.position.x, candidate.position.y, map_x, map_y)) { + break; + } + + clipped_goal = candidate; + } + + return true; +} + +void AStar::appendStraightLineSegment( + const geometry_msgs::msg::Pose & from_pose, + const geometry_msgs::msg::Pose & to_pose) +{ + double resolution = costmap_.info.resolution; + if (resolution <= 0.0) { + return; + } + + double dx = to_pose.position.x - from_pose.position.x; + double dy = to_pose.position.y - from_pose.position.y; + double distance = std::sqrt((dx * dx) + (dy * dy)); + if (distance <= 0.0) { + return; + } + + int steps = std::max(1, static_cast(std::ceil(distance / resolution))); + for (int step = 1; step <= steps; ++step) { + double t = static_cast(step) / static_cast(steps); + AStarNode node; + node.x = from_pose.position.x + (dx * t); + node.y = from_pose.position.y + (dy * t); + path_.push_back(node); + } } void AStar::setGoalNode(const geometry_msgs::msg::Pose & goal_pose) @@ -28,18 +126,35 @@ void AStar::createPlan( const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) { - if (costmap_.data.empty() || costmap_.info.width == 0 || costmap_.info.height == 0 || - costmap_.info.resolution == 0.0) - { + int width, height; + if (costmap_.info.resolution <= 0.0 || getLayerIndex() < 0 || !getMapDimensions(width, height)) { throw std::runtime_error("Costmap is invalid"); } - AStarNodeQueue open_set; + int start_x, start_y; + int goal_x, goal_y; + if (!worldToGrid(start_pose.position.x, start_pose.position.y, start_x, start_y)) { + throw std::runtime_error("Start is outside costmap bounds"); + } + + const geometry_msgs::msg::Pose requested_goal = goal_pose; + geometry_msgs::msg::Pose planning_goal = goal_pose; + bool goal_inside_costmap = worldToGrid(goal_pose.position.x, goal_pose.position.y, goal_x, goal_y); + if (!goal_inside_costmap && + !clipGoalToCostmapBoundary(start_pose, goal_pose, planning_goal)) + { + throw std::runtime_error("Unable to project goal onto costmap boundary"); + } + + closed_set_.clear(); + path_.clear(); start_pose_ = start_pose; - goal_pose_ = goal_pose; + goal_pose_ = planning_goal; - setGoalNode(goal_pose); + setGoalNode(planning_goal); + + AStarNodeQueue open_set; Coordinate start_coord = getCoordinateByPose(start_pose); AStarNode * start_node = getNodeRef(start_coord.x, start_coord.y); @@ -58,6 +173,9 @@ void AStar::createPlan( std::fabs(current_node->y - goal_node_.y) < EPSILON) { reconstructPath(current_node); + if (!goal_inside_costmap) { + appendStraightLineSegment(planning_goal, requested_goal); + } return; } @@ -94,19 +212,9 @@ double AStar::estimateCostToGoal(const geometry_msgs::msg::Pose & pose) double AStar::cost(const AStarNode * from, const AStarNode * to) { - Coordinate to_coord = getCoordinateByPose(to->getPose()); - int costmap_index = to_coord.y * costmap_.info.width + to_coord.x; - - - double cell_cost = 1.0; - - // Check cell cost if index is within the costmap - if (costmap_index >= 0 && costmap_index < costmap_.data.size()) { - cell_cost = costmap_.data[costmap_index]; - } - + double cell_cost = getCellCost(to->x, to->y); double distance = std::sqrt(std::pow(to->x - from->x, 2) + std::pow(to->y - from->y, 2)); - return distance * cell_cost; + return distance * (cell_cost + 1.0); } std::vector AStar::getNeighbors(const AStarNode * node) @@ -122,20 +230,18 @@ std::vector AStar::getNeighbors(const AStarNode * node) double x = node->x + (i * costmap_.info.resolution); double y = node->y + (j * costmap_.info.resolution); - geometry_msgs::msg::Pose pose; - pose.position.x = x; - pose.position.y = y; - - if (x >= 0 && x < costmap_.info.width && y >= 0 && y < costmap_.info.height) { - Coordinate coord = getCoordinateByPose(pose); - AStarNode * neighbor = getNodeRef(coord.x, coord.y); - if (neighbor->status == NodeStatus::None) { - neighbor->x = x; - neighbor->y = y; - neighbor->g_cost = std::numeric_limits::max(); - } - neighbors.push_back(neighbor); + int map_x, map_y; + if (!worldToGrid(x, y, map_x, map_y)) { + continue; + } + + AStarNode * neighbor = getNodeRef(map_x, map_y); + if (neighbor->status == NodeStatus::None) { + neighbor->x = x; + neighbor->y = y; + neighbor->g_cost = std::numeric_limits::max(); } + neighbors.push_back(neighbor); } } diff --git a/urc_navigation/path_planning/src/planner_server.cpp b/urc_navigation/path_planning/src/planner_server.cpp index 7c935b10..fcba0fee 100644 --- a/urc_navigation/path_planning/src/planner_server.cpp +++ b/urc_navigation/path_planning/src/planner_server.cpp @@ -22,13 +22,13 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) rclcpp::SystemDefaultsQoS()); // Setup the costmap - costmap_subscriber_ = create_subscription( - "/traversability", + costmap_subscriber_ = create_subscription( + "/costmap", rclcpp::SystemDefaultsQoS(), std::bind(&PlannerServer::handleCostmap, this, std::placeholders::_1)); } -void PlannerServer::handleCostmap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +void PlannerServer::handleCostmap(const grid_map_msgs::msg::GridMap::SharedPtr msg) { // RCLCPP_INFO(get_logger(), "Received Costmap!"); current_costmap_ = *msg; @@ -42,6 +42,7 @@ void PlannerServer::generatePlan( { try { astar::AStar astar; + astar.setCostmapLayer("traversability_inflated"); astar.setMap(current_costmap_); auto start = request->start.pose; diff --git a/urc_navigation/trajectory_following/CMakeLists.txt b/urc_navigation/trajectory_following/CMakeLists.txt index bea9ec54..55701c88 100644 --- a/urc_navigation/trajectory_following/CMakeLists.txt +++ b/urc_navigation/trajectory_following/CMakeLists.txt @@ -5,12 +5,14 @@ include(../../cmake/default_settings.cmake) # find dependencies find_package(ament_cmake REQUIRED) +find_package(grid_map_utils REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(urc_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(grid_map_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(diagnostic_updater REQUIRED) @@ -33,8 +35,10 @@ set(dependencies rclcpp rclcpp_action rclcpp_components + grid_map_utils urc_msgs std_msgs + grid_map_msgs sensor_msgs geometry_msgs diagnostic_updater diff --git a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml b/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml index 83e6140c..9f058ce6 100644 --- a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml +++ b/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml @@ -5,12 +5,12 @@ follower_action_server: max_angular_velocity: 1.0 heading_alignment_tolerance: 0.2 enable_holonomic_motion: true - lethal_cost_threshold: 10 - cmd_vel_stamped: true - cmd_vel_topic: "/cmd_vel_autonomous" - # odom_topic: "/rover_drivetrain_controller/odom" - odom_topic: "base_link" + lethal_cost_threshold: 0.5 + cmd_vel_stamped: false + cmd_vel_topic: "/cmd_vel" map_frame: "map" + base_link_frame: "base_link" + costmap_layer: "traversability_inflated" goal_tolerance: 0.1 enforce_goal_heading: false goal_heading_tolerance: 0.1 diff --git a/urc_navigation/trajectory_following/include/follower_action_server.hpp b/urc_navigation/trajectory_following/include/follower_action_server.hpp index bae4eab8..d89c2497 100644 --- a/urc_navigation/trajectory_following/include/follower_action_server.hpp +++ b/urc_navigation/trajectory_following/include/follower_action_server.hpp @@ -7,13 +7,13 @@ #include #include #include -#include #include +#include "grid_map_utils/grid_map_utils.hpp" #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" #include "urc_msgs/action/navigate_to_waypoint.hpp" #include "urc_msgs/srv/generate_plan.hpp" -#include +#include namespace follower_action_server { @@ -57,18 +57,19 @@ class FollowerActionServer : public rclcpp::Node * @brief Handle the costmap data * @param msg The costmap data */ - void handleCostmap(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + void handleCostmap(const grid_map_msgs::msg::GridMap::SharedPtr msg); - int getCost(const nav_msgs::msg::OccupancyGrid & costmap, double x, double y); + float getCost(double x, double y); - nav_msgs::msg::OccupancyGrid current_costmap_; - rclcpp::Subscription::SharedPtr costmap_subscriber_; + grid_map_msgs::msg::GridMap current_costmap_; + grid_map_utils::GridMapUtils grid_map_utils_; + rclcpp::Subscription::SharedPtr costmap_subscriber_; + std::string costmap_layer_; rclcpp::Publisher::SharedPtr carrot_pub_; rclcpp::Publisher::SharedPtr cmd_vel_pub_; rclcpp::Publisher::SharedPtr cmd_vel_stamped_pub_; rclcpp_action::Server::SharedPtr navigate_server_; rclcpp::Client::SharedPtr planning_client_; - rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Publisher::SharedPtr rover_position_pub_; std::unique_ptr tf_buffer_; @@ -76,7 +77,6 @@ class FollowerActionServer : public rclcpp::Node rclcpp::Publisher::SharedPtr marker_pub_; - geometry_msgs::msg::PoseStamped current_pose_; bool stamped_; }; } // namespace follower_action_server diff --git a/urc_navigation/trajectory_following/package.xml b/urc_navigation/trajectory_following/package.xml index 25202408..f5b00ba8 100644 --- a/urc_navigation/trajectory_following/package.xml +++ b/urc_navigation/trajectory_following/package.xml @@ -13,11 +13,13 @@ ament_lint_common ament_cmake_gtest + grid_map_utils rclcpp rclcpp_components urc_msgs std_msgs nav_msgs + grid_map_msgs geometry_msgs tf2 tf2_geometry_msgs @@ -28,4 +30,4 @@ ament_cmake - \ No newline at end of file + diff --git a/urc_navigation/trajectory_following/src/follower_action_server.cpp b/urc_navigation/trajectory_following/src/follower_action_server.cpp index 4ac5850d..648115ac 100644 --- a/urc_navigation/trajectory_following/src/follower_action_server.cpp +++ b/urc_navigation/trajectory_following/src/follower_action_server.cpp @@ -5,6 +5,7 @@ #include "tf2/exceptions.h" #include #include +#include namespace follower_action_server { FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions &options) @@ -17,18 +18,21 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions &options) declare_parameter("heading_alignment_tolerance", 0.2); declare_parameter("enable_swerve_motion", true); declare_parameter("cmd_vel_topic", "/cmd_vel"); - declare_parameter("odom_topic", "base_link"); declare_parameter("map_frame", "map"); + declare_parameter("base_link_frame", "base_link"); declare_parameter("goal_tolerance", 0.5); declare_parameter("cmd_vel_stamped", false); - declare_parameter("lethal_cost_threshold", 50); + declare_parameter("lethal_cost_threshold", 50.0); declare_parameter("enforce_goal_heading", false); declare_parameter("goal_heading_tolerance", 0.1); + declare_parameter("costmap_layer", "traversability_inflated"); tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); stamped_ = get_parameter("cmd_vel_stamped").as_bool(); + costmap_layer_ = get_parameter("costmap_layer").as_string(); + grid_map_utils_.setLayer(costmap_layer_); if (stamped_) { cmd_vel_stamped_pub_ = create_publisher( @@ -43,17 +47,8 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions &options) marker_pub_ = create_publisher("lookahead_circle", 10); - odom_sub_ = create_subscription( - get_parameter("odom_topic").as_string(), 10, - [this](const nav_msgs::msg::Odometry::SharedPtr msg) { - geometry_msgs::msg::PoseStamped pose; - pose.header = msg->header; - pose.pose = msg->pose.pose; - current_pose_ = pose; - }); - // Setup the costmap - costmap_subscriber_ = create_subscription( + costmap_subscriber_ = create_subscription( "/costmap", rclcpp::SystemDefaultsQoS(), std::bind(&FollowerActionServer::handleCostmap, this, std::placeholders::_1)); @@ -77,8 +72,9 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions &options) } void FollowerActionServer::handleCostmap( - const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + const grid_map_msgs::msg::GridMap::SharedPtr msg) { current_costmap_ = *msg; + grid_map_utils_.setMap(*msg); } geometry_msgs::msg::TransformStamped @@ -188,17 +184,15 @@ void FollowerActionServer::publishZeroVelocity() { } } -int FollowerActionServer::getCost(const nav_msgs::msg::OccupancyGrid &costmap, - double x, double y) { - int map_x = (x - costmap.info.origin.position.x) / costmap.info.resolution; - int map_y = (y - costmap.info.origin.position.y) / costmap.info.resolution; - - if (map_x < 0 || (unsigned int)map_x >= costmap.info.width || map_y < 0 || - (unsigned int)map_y >= costmap.info.height) { - return 0; +float FollowerActionServer::getCost(double x, double y) { + float cost; + if (!grid_map_utils_.tryGetCellCost(x, y, cost)) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Costmap layer '%s' not found", costmap_layer_.c_str()); + return 0.0f; } - return costmap.data[map_y * costmap.info.width + map_x]; + return cost; } nav_msgs::msg::Path FollowerActionServer::callPlanningService( @@ -212,9 +206,7 @@ nav_msgs::msg::Path FollowerActionServer::callPlanningService( auto result = planning_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), - result, std::chrono::seconds(10)) == - rclcpp::FutureReturnCode::SUCCESS) { + if (result.wait_for(std::chrono::seconds(10)) == std::future_status::ready) { auto response = result.get(); if (response->error_code == urc_msgs::srv::GeneratePlan::Response::SUCCESS) { @@ -266,9 +258,26 @@ void FollowerActionServer::execute_navigate( goal_pose = goal_msg->goal; + // Get current pose from TF geometry_msgs::msg::PoseStamped start_pose; - start_pose.header = current_pose_.header; - start_pose.pose = current_pose_.pose; + try { + auto transform = tf_buffer_->lookupTransform( + get_parameter("map_frame").as_string(), + get_parameter("base_link_frame").as_string(), tf2::TimePointZero); + start_pose.header = transform.header; + start_pose.pose.position.x = transform.transform.translation.x; + start_pose.pose.position.y = transform.transform.translation.y; + start_pose.pose.position.z = transform.transform.translation.z; + start_pose.pose.orientation = transform.transform.rotation; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Could not get current pose: %s", + ex.what()); + result->error_code = + urc_msgs::action::NavigateToWaypoint::Result::PLANNING_FAILED; + goal_handle->abort(result); + publishZeroVelocity(); + return; + } bool planning_success = false; path = callPlanningService(start_pose, goal_pose, planning_success); @@ -320,11 +329,26 @@ void FollowerActionServer::execute_navigate( rclcpp::Rate rate(10); while (rclcpp::ok()) { - // Get current pose in map frame at the start of each iteration - auto odom_to_map_ = - lookup_transform(get_parameter("map_frame").as_string(), "base_link"); + // Get current pose in map frame from TF geometry_msgs::msg::PoseStamped current_pose_map_frame_; - tf2::doTransform(current_pose_, current_pose_map_frame_, odom_to_map_); + try { + auto transform = tf_buffer_->lookupTransform( + get_parameter("map_frame").as_string(), + get_parameter("base_link_frame").as_string(), tf2::TimePointZero); + current_pose_map_frame_.header = transform.header; + current_pose_map_frame_.pose.position.x = + transform.transform.translation.x; + current_pose_map_frame_.pose.position.y = + transform.transform.translation.y; + current_pose_map_frame_.pose.position.z = + transform.transform.translation.z; + current_pose_map_frame_.pose.orientation = transform.transform.rotation; + } catch (tf2::TransformException &ex) { + RCLCPP_WARN(this->get_logger(), "Could not get current pose: %s", + ex.what()); + rate.sleep(); + continue; + } // Update feedback distance feedback->distance_to_goal = geometry_util::dist2D( @@ -404,9 +428,9 @@ void FollowerActionServer::execute_navigate( RCLCPP_INFO(this->get_logger(), "Goal has been reached!"); break; } - } else if (getCost(current_costmap_, output.lookahead_point.point.x, + } else if (getCost(output.lookahead_point.point.x, output.lookahead_point.point.y) > - get_parameter("lethal_cost_threshold").as_int()) { + get_parameter("lethal_cost_threshold").as_double()) { // Obstacle detected - attempt to re-plan RCLCPP_WARN(this->get_logger(), "Obstacle detected! Attempting to re-plan..."); @@ -415,9 +439,27 @@ void FollowerActionServer::execute_navigate( feedback->replan_count++; goal_handle->publish_feedback(feedback); + // Get current pose from TF for replanning geometry_msgs::msg::PoseStamped start_pose; - start_pose.header = current_pose_.header; - start_pose.pose = current_pose_.pose; + try { + auto transform = tf_buffer_->lookupTransform( + get_parameter("map_frame").as_string(), + get_parameter("base_link_frame").as_string(), tf2::TimePointZero); + start_pose.header = transform.header; + start_pose.pose.position.x = transform.transform.translation.x; + start_pose.pose.position.y = transform.transform.translation.y; + start_pose.pose.position.z = transform.transform.translation.z; + start_pose.pose.orientation = transform.transform.rotation; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), + "Could not get current pose for replanning: %s", + ex.what()); + result->error_code = + urc_msgs::action::NavigateToWaypoint::Result::PLANNING_FAILED; + goal_handle->abort(result); + publishZeroVelocity(); + return; + } bool planning_success = false; nav_msgs::msg::Path new_path = @@ -443,7 +485,8 @@ void FollowerActionServer::execute_navigate( output = pure_pursuit.getCommandVelocity( this->get_logger(), - lookup_transform("base_link", get_parameter("map_frame").as_string())); + lookup_transform(get_parameter("base_link_frame").as_string(), + get_parameter("map_frame").as_string())); if (stamped_) { cmd_vel_stamped_pub_->publish(output.cmd_vel); diff --git a/urc_perception/config/pcl_grid_map_params.yaml b/urc_perception/config/pcl_grid_map_params.yaml index 2fcd5a7a..c878817e 100644 --- a/urc_perception/config/pcl_grid_map_params.yaml +++ b/urc_perception/config/pcl_grid_map_params.yaml @@ -1,5 +1,30 @@ pcl_grid_map_extraction: + num_processing_threads: 4 + cloud_transform: + translation: + x: 0.0 + y: 0.0 + z: 0.0 + rotation: #intrinsic rotation X-Y-Z (r-p-y)sequence + r: 0.0 + p: 0.0 + y: 0.0 + cluster_extraction: + cluster_tolerance: 0.05 + min_num_points: 4 + max_num_points: 1000000 outlier_removal: is_remove_outliers: true mean_K: 50 stddev_threshold: 1.0 + downsampling: + is_downsample_cloud: false + voxel_size: + x: 0.02 + y: 0.02 + z: 0.02 + grid_map: + min_num_points_per_cell: 2 + resolution: 0.25 + height_type: 0 + height_thresh: 1.0 diff --git a/urc_perception/config/traversability_params.yaml b/urc_perception/config/traversability_params.yaml index dd55c809..a86465b2 100644 --- a/urc_perception/config/traversability_params.yaml +++ b/urc_perception/config/traversability_params.yaml @@ -27,9 +27,17 @@ grid_map_visualization: traversability_mapping: ros__parameters: + map_frame: map + odometry_topic: /odometry/filtered_global pointcloud_topic: /scan/points output_map_topic: /costmap + cache_map_topic: /cache_map + filter_chain_parameter_name: filters filter_radius: 5.0 + cache_size_x: 10.0 + cache_size_y: 10.0 + resolution: 0.25 + recenter_rate_hz: 20.0 filters: filter1: @@ -42,7 +50,7 @@ traversability_mapping: params: input_layer: elevation output_layer: elevation_smooth - radius: 0.1 + radius: 0.5 filter3: # Compute surface normals. name: surface_normals @@ -60,29 +68,21 @@ traversability_mapping: output_layer: slope expression: acos(normal_vectors_z) - filter5: # Zero out slope values below a threshold. - name: slope_lower_threshold - type: gridMapFilters/ThresholdFilter - params: - layer: slope - lower_threshold: 1.0 - set_to: 0.0 - - filter6: # Compute roughness as absolute difference from map to smoothened map. + filter5: # Compute roughness as absolute difference from map to smoothened map. name: roughness type: gridMapFilters/MathExpressionFilter params: output_layer: roughness expression: abs(elevation - elevation_smooth) - filter7: # Compute traversability as normalized weighted sum of slope and roughness. + filter6: # Compute traversability as normalized weighted sum of slope and roughness. name: traversability type: gridMapFilters/MathExpressionFilter params: output_layer: traversability - expression: slope / 1.57 + expression: slope + roughness - filter8: # Apply a gaussian kernel with a given radius and coefficient. + filter7: # Apply a gaussian kernel with a given radius and coefficient. name: gaussian_kernel type: urcPerception/GaussianFilter params: diff --git a/urc_perception/include/traversability_mapping.hpp b/urc_perception/include/traversability_mapping.hpp index b719a3b8..029cb068 100644 --- a/urc_perception/include/traversability_mapping.hpp +++ b/urc_perception/include/traversability_mapping.hpp @@ -3,10 +3,13 @@ #include #include +#include +#include #include #include #include #include +#include #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" @@ -20,36 +23,53 @@ class TraversabilityMapping : public rclcpp::Node explicit TraversabilityMapping(const rclcpp::NodeOptions & options); ~TraversabilityMapping(); - bool readParameters(); - private: - geometry_msgs::msg::TransformStamped lookup_transform( - std::string target_frame, - std::string source_frame, - rclcpp::Time time); + geometry_msgs::msg::TransformStamped lookupTransformSafe( + const std::string & target_frame, + const std::string & source_frame, + const rclcpp::Time & stamp, + const tf2::Duration & timeout, + bool & success); void handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void filterSphere(pcl::PointCloud::Ptr cloud, float radius); + void handleOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); + void filterSphere( + pcl::PointCloud::Ptr cloud, + float min_radius, + float max_radius); + void recenterCache(double center_x_world, double center_y_world); + void publishCacheMap(); rclcpp::Subscription::SharedPtr pointcloud_subscriber_; + rclcpp::Subscription::SharedPtr odometry_subscriber_; rclcpp::Publisher::SharedPtr grid_map_publisher_; + rclcpp::Publisher::SharedPtr cache_map_publisher_; std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; - grid_map::GridMap map_; + grid_map::GridMap cache_map_; filters::FilterChain filter_chain_; - double filter_radius_; - double resolution_; - double min_z_; - double max_z_; - unsigned int width_; + bool use_filter_chain_ = false; + bool cache_initialized_ = false; + + double min_filter_radius_ = 0.2; + double filter_radius_ = 5.0; + double cache_size_x_ = 10.0; + double cache_size_y_ = 10.0; + double resolution_ = 0.25; + double recenter_rate_hz_ = 20.0; + double min_z_ = 0.0; + double max_z_ = 0.0; + unsigned int width_ = 0; - std::string map_frame_; - std::string pointcloud_topic_; - std::string output_map_topic_; - std::string filter_chain_parameter_name_; + std::string map_frame_ = "map"; + std::string odometry_topic_ = "/odometry/filtered_global"; + std::string pointcloud_topic_ = "/scan/points"; + std::string output_map_topic_ = "/costmap"; + std::string cache_map_topic_ = "/cache_map"; + std::string filter_chain_parameter_name_ = "filters"; }; } // namespace urc_perception diff --git a/urc_perception/launch/mapping.launch.py b/urc_perception/launch/mapping.launch.py index bf7f129e..a926a9ab 100644 --- a/urc_perception/launch/mapping.launch.py +++ b/urc_perception/launch/mapping.launch.py @@ -20,17 +20,8 @@ def generate_launch_description(): parameters=[traversability_params], ) - grid_map_visualization_node = Node( - package="grid_map_visualization", - executable="grid_map_visualization", - name="grid_map_visualization", - output="screen", - parameters=[traversability_params], - ) - return LaunchDescription( [ traversability_mapping_node, - grid_map_visualization_node, ] ) diff --git a/urc_perception/launch/test_sick.py b/urc_perception/launch/test_sick.py new file mode 100644 index 00000000..47e341e1 --- /dev/null +++ b/urc_perception/launch/test_sick.py @@ -0,0 +1,36 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + sick_node = Node( + package = "sick_scan_xd", + executable = "sick_generic_caller", + parameters = [ + { + "hostname": "192.168.1.10", + "udp_receiver_ip": "192.168.1.3", + "scanner_type": "sick_multiscan", + "publish_frame_id": "lidar_link", + "tf_base_frame_id": "lidar_link", + "publish_laserscan_segment_topic": "scan_segment", + "publish_laserscan_fullframe_topic": "scan_fullframe", + "custom_pointclouds": "cloud_unstructured_fullframe", + "verbose_level": 0, + "cloud_unstructured_fullframe": ( + "coordinateNotation=0 updateMethod=0 echos=0,1,2 " + "layers=1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16 " + "reflectors=0,1 infringed=0,1 rangeFilter=0.05,999,1 " + "topic=/scan/points frameid=lidar_link publish=1" + ) + } + ], + output = "screen", + ) + + return LaunchDescription([ + sick_node, + ]) + diff --git a/urc_perception/src/traversability_mapping.cpp b/urc_perception/src/traversability_mapping.cpp index ead54ee2..033070e7 100644 --- a/urc_perception/src/traversability_mapping.cpp +++ b/urc_perception/src/traversability_mapping.cpp @@ -2,24 +2,39 @@ #include -#include -#include -#include +#include +#include +#include +#include +#include -#include -#include #include #include +#include +#include +#include +#include +#include namespace urc_perception { + TraversabilityMapping::TraversabilityMapping(const rclcpp::NodeOptions & options) : Node("traversability_mapping", options), filter_chain_("grid_map::GridMap") { - if (!readParameters()) { - RCLCPP_ERROR(this->get_logger(), "Could not read parameters. Shutting down node."); - return; - } + pointcloud_topic_ = this->declare_parameter("pointcloud_topic", pointcloud_topic_); + output_map_topic_ = this->declare_parameter("output_map_topic", output_map_topic_); + cache_map_topic_ = this->declare_parameter("cache_map_topic", cache_map_topic_); + map_frame_ = this->declare_parameter("map_frame", map_frame_); + odometry_topic_ = this->declare_parameter("odometry_topic", odometry_topic_); + filter_chain_parameter_name_ = + this->declare_parameter("filter_chain_parameter_name", filter_chain_parameter_name_); + filter_radius_ = this->declare_parameter("filter_radius", filter_radius_); + cache_size_x_ = this->declare_parameter("cache_size_x", cache_size_x_); + cache_size_y_ = this->declare_parameter("cache_size_y", cache_size_y_); + resolution_ = this->declare_parameter("resolution", resolution_); + recenter_rate_hz_ = this->declare_parameter("recenter_rate_hz", recenter_rate_hz_); + min_filter_radius_ = this->declare_parameter("min_filter_radius", 0.2); RCLCPP_INFO(this->get_logger(), "Traversability mapping node has been started."); @@ -27,113 +42,248 @@ TraversabilityMapping::TraversabilityMapping(const rclcpp::NodeOptions & options tf_listener_ = std::make_shared(*tf_buffer_); grid_map_publisher_ = create_publisher(output_map_topic_, 10); + cache_map_publisher_ = create_publisher(cache_map_topic_, 10); + pointcloud_subscriber_ = create_subscription( - pointcloud_topic_, 10, + pointcloud_topic_, + rclcpp::SensorDataQoS().keep_last(1), std::bind(&TraversabilityMapping::handlePointcloud, this, std::placeholders::_1)); - if (filter_chain_.configure( - filter_chain_parameter_name_, this->get_node_logging_interface(), - this->get_node_parameters_interface())) - { + odometry_subscriber_ = create_subscription( + odometry_topic_, + rclcpp::SensorDataQoS().keep_last(1), + std::bind(&TraversabilityMapping::handleOdometry, this, std::placeholders::_1)); + + cache_map_.setFrameId(map_frame_); + cache_map_.setGeometry(grid_map::Length(cache_size_x_, cache_size_y_), resolution_); + cache_map_.add("elevation", 0.0); + cache_map_.add("traversability_inflated", 0.0); + + use_filter_chain_ = filter_chain_.configure( + filter_chain_parameter_name_, + this->get_node_logging_interface(), + this->get_node_parameters_interface()); + + if (use_filter_chain_) { RCLCPP_INFO(this->get_logger(), "Filter chain configured."); } else { - RCLCPP_ERROR(this->get_logger(), "Could not configure the filter chain!"); - return; + RCLCPP_WARN( + this->get_logger(), + "Could not configure the filter chain. Falling back to unfiltered local map."); } } -geometry_msgs::msg::TransformStamped TraversabilityMapping::lookup_transform( - std::string target_frame, - std::string source_frame, - rclcpp::Time time) +TraversabilityMapping::~TraversabilityMapping() = default; + +geometry_msgs::msg::TransformStamped TraversabilityMapping::lookupTransformSafe( + const std::string & target_frame, + const std::string & source_frame, + const rclcpp::Time & stamp, + const tf2::Duration & timeout, + bool & success) { geometry_msgs::msg::TransformStamped transform; + success = false; try { - transform = tf_buffer_->lookupTransform(target_frame, source_frame, time); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(this->get_logger(), "Could not lookup transform: %s", ex.what()); + transform = tf_buffer_->lookupTransform(target_frame, source_frame, stamp, timeout); + success = true; + return transform; + } catch (const tf2::TransformException & ex1) { + try { + transform = tf_buffer_->lookupTransform(target_frame, source_frame, rclcpp::Time(0), timeout); + success = true; + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "Stamped transform unavailable; using latest transform from %s to %s.", + source_frame.c_str(), target_frame.c_str()); + return transform; + } catch (const tf2::TransformException & ex2) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "Transform lookup failed from %s to %s. stamped='%s', latest='%s'", + source_frame.c_str(), target_frame.c_str(), ex1.what(), ex2.what()); + return transform; + } } - return transform; } -TraversabilityMapping::~TraversabilityMapping() = default; - -void TraversabilityMapping::filterSphere(pcl::PointCloud::Ptr cloud, float radius) +void TraversabilityMapping::filterSphere( + pcl::PointCloud::Ptr cloud, + float min_radius, + float max_radius) { - double radiusSquared = radius * radius; - pcl::PointCloud::Ptr filteredCloud(new pcl::PointCloud()); - filteredCloud->points.reserve(cloud->points.size()); + const double min_r2 = static_cast(min_radius) * static_cast(min_radius); + const double max_r2 = static_cast(max_radius) * static_cast(max_radius); + + pcl::PointCloud::Ptr filtered_cloud(new pcl::PointCloud()); + filtered_cloud->points.reserve(cloud->points.size()); for (const auto & point : cloud->points) { - float distSquared = point.x * point.x + point.y * point.y + point.z * point.z; - if (distSquared <= radiusSquared && distSquared >= 0.5) { - filteredCloud->points.push_back(point); + if (!std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z)) { + continue; + } + + const double d2 = + static_cast(point.x) * point.x + + static_cast(point.y) * point.y + + static_cast(point.z) * point.z; + + if (d2 >= min_r2 && d2 <= max_r2) { + filtered_cloud->points.push_back(point); } } - *cloud = std::move(*filteredCloud); + filtered_cloud->width = static_cast(filtered_cloud->points.size()); + filtered_cloud->height = 1; + filtered_cloud->is_dense = true; + + *cloud = std::move(*filtered_cloud); } -void TraversabilityMapping::handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) +void TraversabilityMapping::recenterCache(double center_x_world, double center_y_world) { - // Transform the point cloud from lidar_link to map frame - auto lidar_to_map = lookup_transform("map", msg->header.frame_id, msg->header.stamp); - sensor_msgs::msg::PointCloud2 cloud_global_; - tf2::doTransform(*msg, cloud_global_, lidar_to_map); + const double snapped_x = std::round(center_x_world / resolution_) * resolution_; + const double snapped_y = std::round(center_y_world / resolution_) * resolution_; + const grid_map::Position cache_center(snapped_x, snapped_y); - // Convert the transformed point cloud to a PCL point cloud - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud); + if (!cache_initialized_) { + cache_map_.setPosition(cache_center); + cache_initialized_ = true; + return; + } - // Filter the point cloud - filterSphere(cloud, filter_radius_); + std::vector regions; + cache_map_.move(cache_center, regions); - std::string filePath = ament_index_cpp::get_package_share_directory("urc_perception") + - "/config/pcl_grid_map_params.yaml"; + for (const auto & region : regions) { + const auto start = region.getStartIndex(); + const auto size = region.getSize(); + const auto map_size = cache_map_.getSize(); - // Construct a GridMapPclLoader object and set the input cloud - grid_map::GridMapPclLoader gridMapPclLoader(this->get_logger()); - gridMapPclLoader.setInputCloud(cloud); - gridMapPclLoader.loadParameters(filePath); + for (int r = 0; r < size(0); ++r) { + for (int c = 0; c < size(1); ++c) { + grid_map::Index idx(start(0) + r, start(1) + c); + idx(0) = (idx(0) + map_size(0)) % map_size(0); + idx(1) = (idx(1) + map_size(1)) % map_size(1); - gridMapPclLoader.preProcessInputCloud(); - gridMapPclLoader.initializeGridMapGeometryFromInputCloud(); - gridMapPclLoader.addLayerFromInputCloud(std::string("elevation")); + cache_map_.at("elevation", idx) = 0.0; + cache_map_.at("traversability_inflated", idx) = 0.0; + } + } + } +} - grid_map::GridMap map = gridMapPclLoader.getGridMap(); - map.setFrameId(msg->header.frame_id); +void TraversabilityMapping::handleOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + recenterCache(msg->pose.pose.position.x, msg->pose.pose.position.y); + publishCacheMap(); +} - grid_map::GridMap filtered_map; - if (!filter_chain_.update(map, filtered_map)) { - RCLCPP_ERROR(this->get_logger(), "Could not update the grid map filter chain!"); +void TraversabilityMapping::publishCacheMap() +{ + if (!cache_initialized_) { return; } - std::unique_ptr outputMessage; - outputMessage = grid_map::GridMapRosConverter::toMessage(filtered_map); - grid_map_publisher_->publish(std::move(outputMessage)); + auto msg1 = grid_map::GridMapRosConverter::toMessage(cache_map_); + + grid_map_publisher_->publish(std::move(msg1)); } -bool TraversabilityMapping::readParameters() +void TraversabilityMapping::handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - this->declare_parameter("pointcloud_topic", std::string("pointcloud")); - this->declare_parameter("output_map_topic", std::string("costmap")); - this->declare_parameter("filter_chain_parameter_name", std::string("filters")); - this->declare_parameter("filter_radius", 5.0); - - if (!this->get_parameter("pointcloud_topic", pointcloud_topic_)) { - RCLCPP_ERROR(this->get_logger(), "Could not read parameter pointcloud_topic."); - return false; + if (!cache_initialized_) { + return; + } + + pcl::PointCloud::Ptr cloud_sensor(new pcl::PointCloud); + pcl::fromROSMsg(*msg, *cloud_sensor); + + filterSphere(cloud_sensor, min_filter_radius_, filter_radius_); + + sensor_msgs::msg::PointCloud2 cloud_sensor_filtered_msg; + pcl::toROSMsg(*cloud_sensor, cloud_sensor_filtered_msg); + cloud_sensor_filtered_msg.header = msg->header; + + bool tf_ok = false; + const auto sensor_to_map = lookupTransformSafe( + map_frame_, + msg->header.frame_id, + msg->header.stamp, + tf2::durationFromSec(0.05), + tf_ok); + + if (!tf_ok) { + return; + } + + sensor_msgs::msg::PointCloud2 cloud_global_msg; + tf2::doTransform(cloud_sensor_filtered_msg, cloud_global_msg, sensor_to_map); + + pcl::PointCloud::Ptr cloud_global(new pcl::PointCloud); + pcl::fromROSMsg(cloud_global_msg, *cloud_global); + + if (cloud_global->empty()) { + return; + } + + const std::string file_path = + ament_index_cpp::get_package_share_directory("urc_perception") + + "/config/pcl_grid_map_params.yaml"; + + grid_map::GridMapPclLoader grid_map_loader(this->get_logger()); + grid_map_loader.setInputCloud(cloud_global); + grid_map_loader.loadParameters(file_path); + + grid_map_loader.preProcessInputCloud(); + grid_map_loader.initializeGridMapGeometryFromInputCloud(); + grid_map_loader.addLayerFromInputCloud("elevation"); + grid_map_loader.addLayerFromInputCloud("traversability_inflated"); + + grid_map::GridMap local_map = grid_map_loader.getGridMap(); + local_map.setFrameId(map_frame_); + + grid_map::GridMap filtered_local_map; + if (use_filter_chain_) { + if (!filter_chain_.update(local_map, filtered_local_map)) { + RCLCPP_ERROR(this->get_logger(), "Could not update the grid map filter chain."); + return; + } + } else { + filtered_local_map = local_map; } - this->get_parameter("output_map_topic", output_map_topic_); - this->get_parameter("filter_chain_parameter_name", filter_chain_parameter_name_); - this->get_parameter("filter_radius", filter_radius_); + for (grid_map::GridMapIterator it(filtered_local_map); !it.isPastEnd(); ++it) { + const grid_map::Index src_index = *it; + + if (!filtered_local_map.isValid(src_index, "elevation") || + !filtered_local_map.isValid(src_index, "traversability_inflated")) + { + continue; + } + + grid_map::Position world_position; + filtered_local_map.getPosition(src_index, world_position); + + grid_map::Index cache_index; + if (!cache_map_.getIndex(world_position, cache_index)) { + continue; + } + + const float src_elevation = filtered_local_map.at("elevation", src_index); + const float src_trav = filtered_local_map.at("traversability_inflated", src_index); - return true; + cache_map_.at("elevation", cache_index) = + std::max(cache_map_.at("elevation", cache_index), src_elevation); + + cache_map_.at("traversability_inflated", cache_index) = src_trav; + } + + publishCacheMap(); } -} // namespace urc_perception + +} // namespace urc_perception #include RCLCPP_COMPONENTS_REGISTER_NODE(urc_perception::TraversabilityMapping) diff --git a/urc_platform/package.xml b/urc_platform/package.xml index e9b64546..03f5e248 100644 --- a/urc_platform/package.xml +++ b/urc_platform/package.xml @@ -22,6 +22,7 @@ image_transport_plugins vectornav_msgs geometry_msgs + grid_map_filters tf2 tf2_geometry_msgs tf2_ros