From a45f47fcdc532799df0544cbb9157ddd50042729 Mon Sep 17 00:00:00 2001 From: nishia1 Date: Fri, 20 Feb 2026 12:24:53 -0500 Subject: [PATCH 01/25] base state machine --- urc_navigation/nav_testing/CMakeLists.txt | 34 +- .../nav_testing/include/nav_coordinator.hpp | 73 +++-- urc_navigation/nav_testing/package.xml | 4 + .../nav_testing/src/nav_coordinator.cpp | 307 ++++++++---------- 4 files changed, 222 insertions(+), 196 deletions(-) diff --git a/urc_navigation/nav_testing/CMakeLists.txt b/urc_navigation/nav_testing/CMakeLists.txt index 9bac1f84..0d5d561b 100644 --- a/urc_navigation/nav_testing/CMakeLists.txt +++ b/urc_navigation/nav_testing/CMakeLists.txt @@ -1,15 +1,40 @@ cmake_minimum_required(VERSION 3.8) project(nav_testing) +include(../../cmake/default_settings.cmake) + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # 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(geometry_msgs REQUIRED) +find_package(urc_msgs REQUIRED) + +include_directories(include) + +add_executable(nav_coordinator + src/nav_coordinator.cpp +) + +ament_target_dependencies(nav_coordinator + rclcpp + rclcpp_action + geometry_msgs + urc_msgs +) + +install(TARGETS + nav_coordinator + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -23,4 +48,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +ament_export_include_directories(include) +ament_export_dependencies(rclcpp rclcpp_action geometry_msgs urc_msgs) + ament_package() diff --git a/urc_navigation/nav_testing/include/nav_coordinator.hpp b/urc_navigation/nav_testing/include/nav_coordinator.hpp index 3e4ab5b0..5942816d 100644 --- a/urc_navigation/nav_testing/include/nav_coordinator.hpp +++ b/urc_navigation/nav_testing/include/nav_coordinator.hpp @@ -1,37 +1,56 @@ #ifndef NAV_COORDINATOR_HPP_ #define NAV_COORDINATOR_HPP_ +#include +#include +#include +#include + namespace nav_coordinator { class NavCoordinator : public rclcpp::Node { - public: - explicit NavCoordinator(const rclcpp::NodeOptions & options); - ~NavCoordinator(); - - private: - // --- Subscribers --- // - rclcpp::Subscription::SharedPtr localization_subscriber_; - rclcpp::Subscription::SharedPtr goal_subscriber_; - rclcpp::Subscription::SharedPtr cancel_subscriber_; - - // --- Publishers --- // - - // --- Planner Service Client --- // - rclcpp::Client::SharedPtr planner_client_; - - rclcpp::TimerBase::SharedPtr timer_; - - // TODO +public: + explicit NavCoordinator(const rclcpp::NodeOptions & options); + ~NavCoordinator() override = default; + +private: + using NavigateToWaypoint = urc_msgs::action::NavigateToWaypoint; + using GoalHandleNavigate = rclcpp_action::ClientGoalHandle; + + enum class State + { + IDLE, + WAITING_FOR_SERVER, + SENDING_GOAL, + TRACKING_GOAL, + SUCCEEDED, + FAILED, + CANCELED + }; + + void handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint); + + 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); + + State state_; + std::string follower_action_name_; + bool cancel_on_new_waypoint_; + + geometry_msgs::msg::PoseStamped active_waypoint_; + GoalHandleNavigate::SharedPtr active_goal_handle_; + + rclcpp::Subscription::SharedPtr waypoint_subscriber_; + rclcpp_action::Client::SharedPtr follower_client_; +}; } - // void timerCallback(); - - void handleLocalization(const TODO::SharedPtr msg); - void handleGoal(const TODO::SharedPtr msg); - void handleStart(const TODO::SharedPtr msg); - -} // namespace nav_coordinator - -#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..27c588cc 100644 --- a/urc_navigation/nav_testing/package.xml +++ b/urc_navigation/nav_testing/package.xml @@ -8,6 +8,10 @@ MIT ament_cmake + rclcpp + rclcpp_action + geometry_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..73e0a501 100644 --- a/urc_navigation/nav_testing/src/nav_coordinator.cpp +++ b/urc_navigation/nav_testing/src/nav_coordinator.cpp @@ -1,195 +1,170 @@ -#include -#include - #include "nav_coordinator.hpp" -#include "astar.hpp" 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("follower_action_name", "navigate_to_waypoint"); + declare_parameter("cancel_on_new_waypoint", true); + + follower_action_name_ = get_parameter("follower_action_name").as_string(); + cancel_on_new_waypoint_ = get_parameter("cancel_on_new_waypoint").as_bool(); + state_ = State::IDLE; + + 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)); + + RCLCPP_INFO( + get_logger(), + "Nav Coordinator ready. Waiting for waypoints on topic '%s' and forwarding to action '%s'.", + get_parameter("waypoint_topic").as_string().c_str(), + follower_action_name_.c_str()); } -// --- nav action server functions --- // - -rclcpp_action::GoalResponse NavCoordinator::handle_goal( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) +void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received goal request"); - - (void)uuid; - (void)goal; - - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + 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); + + 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_); } -rclcpp_action::CancelResponse NavCoordinator::handle_cancel( - const std::shared_ptr> goal_handle) +void NavCoordinator::sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint) { - RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); - - (void)goal_handle; - - return rclcpp_action::CancelResponse::ACCEPT; + transitionTo(State::WAITING_FOR_SERVER, "checking follower action server"); + if (!follower_client_->wait_for_action_server(std::chrono::seconds(2))) { + transitionTo(State::FAILED, "follower action server unavailable"); + RCLCPP_ERROR( + get_logger(), "Action server '%s' not available.", follower_action_name_.c_str()); + 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) { + transitionTo(State::FAILED, "follower rejected goal"); + RCLCPP_ERROR(get_logger(), "Follower action server rejected the 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"); - 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!"); - 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(); - 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!"); - break; - } + 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::execute( - const std::shared_ptr> goal_handle) +void NavCoordinator::handleResult(const GoalHandleNavigate::WrappedResult & result) { - 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; + 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; } + + transitionTo(State::FAILED, "follower finished with non-success error code"); + RCLCPP_WARN( + get_logger(), "Follower completed with error_code=%u", result.result->error_code); + return; + } + + if (result.code == rclcpp_action::ResultCode::ABORTED) { + transitionTo(State::FAILED, "follower aborted goal"); + return; + } + + if (result.code == rclcpp_action::ResultCode::CANCELED) { + transitionTo(State::CANCELED, "follower canceled goal"); + return; + } + + transitionTo(State::FAILED, "unknown follower result code"); } -// --- planner service request --- // -/* IN PROGRESS -void NavCoordinator::sendPlannerRequest() +void NavCoordinator::transitionTo(State new_state, const std::string & reason) { - 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"); - + if (state_ == new_state) { + return; + } + + const auto state_name = [](State state) { + 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"; + } + return "UNKNOWN"; + }; + + RCLCPP_INFO( + get_logger(), "State transition: %s -> %s (%s)", state_name(state_), + state_name(new_state), reason.c_str()); + state_ = new_state; } -*/ } // namespace nav_coordinator -#include -RCLCPP_COMPONENTS_REGISTER_NODE(nav_coordinator::NavCoordinator) +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(rclcpp::NodeOptions()); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From ff0b2a3d47c603a0113b2764f338b3ae5ebca85f Mon Sep 17 00:00:00 2001 From: Mrinal Jain <2mrinaljain@gmail.com> Date: Fri, 20 Feb 2026 16:43:43 -0500 Subject: [PATCH 02/25] refactor launch files --- urc_bringup/launch/autonomy.launch.py | 50 ++++++++ urc_bringup/launch/bringup.launch.py | 18 ++- urc_bringup/launch/sim.launch.py | 15 +++ urc_bringup/launch/sim.py | 172 -------------------------- 4 files changed, 82 insertions(+), 173 deletions(-) create mode 100644 urc_bringup/launch/autonomy.launch.py delete mode 100644 urc_bringup/launch/sim.py diff --git a/urc_bringup/launch/autonomy.launch.py b/urc_bringup/launch/autonomy.launch.py new file mode 100644 index 00000000..1b3220e9 --- /dev/null +++ b/urc_bringup/launch/autonomy.launch.py @@ -0,0 +1,50 @@ +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") + + perception_config = os.path.join(pkg_urc_perception, "config", "elevation_mapping.yaml") + trajectory_config = os.path.join(pkg_trajectory_following, "config", "pure_pursuit.yaml") + + state_machine_node = Node( + package="nav_testing", + executable="nav_coordinator", + 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", + output="screen", + ) + + return LaunchDescription( + [ + state_machine_node, + path_planning_node, + trajectory_following_node, + traversability_mapping_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, - ] - ) From d9367d99942876f0582414ac1411dc42af979cee Mon Sep 17 00:00:00 2001 From: nishia1 Date: Fri, 20 Feb 2026 16:58:38 -0500 Subject: [PATCH 03/25] error handling part 1 --- .../nav_testing/include/nav_coordinator.hpp | 16 ++++ .../nav_testing/src/nav_coordinator.cpp | 80 +++++++++++++++++-- 2 files changed, 90 insertions(+), 6 deletions(-) diff --git a/urc_navigation/nav_testing/include/nav_coordinator.hpp b/urc_navigation/nav_testing/include/nav_coordinator.hpp index 5942816d..ed4e5397 100644 --- a/urc_navigation/nav_testing/include/nav_coordinator.hpp +++ b/urc_navigation/nav_testing/include/nav_coordinator.hpp @@ -29,6 +29,17 @@ class NavCoordinator : public rclcpp::Node CANCELED }; + enum class ErrorType + { + NONE, + PLANNER_FAILURE, + OBSTACLE_DETECTED, + PLANNING_FAILED_IN_FOLLOWER, + FOLLOWER_FAILURE, + SERVER_UNAVAILABLE, + UNKNOWN_ERROR + }; + void handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg); void sendFollowerGoal(const geometry_msgs::msg::PoseStamped & waypoint); @@ -39,6 +50,8 @@ class NavCoordinator : public rclcpp::Node void handleResult(const GoalHandleNavigate::WrappedResult & result); void transitionTo(State new_state, const std::string & reason); + void handleError(ErrorType error_type, const std::string & details); + std::string errorTypeToString(ErrorType error_type) const; State state_; std::string follower_action_name_; @@ -49,6 +62,9 @@ class NavCoordinator : public rclcpp::Node rclcpp::Subscription::SharedPtr waypoint_subscriber_; rclcpp_action::Client::SharedPtr follower_client_; + + ErrorType last_error_; + std::string last_error_details_; }; } diff --git a/urc_navigation/nav_testing/src/nav_coordinator.cpp b/urc_navigation/nav_testing/src/nav_coordinator.cpp index 73e0a501..d6b9e720 100644 --- a/urc_navigation/nav_testing/src/nav_coordinator.cpp +++ b/urc_navigation/nav_testing/src/nav_coordinator.cpp @@ -12,6 +12,7 @@ NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) follower_action_name_ = get_parameter("follower_action_name").as_string(); cancel_on_new_waypoint_ = get_parameter("cancel_on_new_waypoint").as_bool(); state_ = State::IDLE; + last_error_ = ErrorType::NONE; follower_client_ = rclcpp_action::create_client(this, follower_action_name_); @@ -49,9 +50,10 @@ void NavCoordinator::sendFollowerGoal(const geometry_msgs::msg::PoseStamped & wa { 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"); - RCLCPP_ERROR( - get_logger(), "Action server '%s' not available.", follower_action_name_.c_str()); return; } @@ -77,8 +79,8 @@ void NavCoordinator::sendFollowerGoal(const geometry_msgs::msg::PoseStamped & wa void NavCoordinator::handleGoalResponse(const GoalHandleNavigate::SharedPtr & goal_handle) { if (!goal_handle) { + handleError(ErrorType::FOLLOWER_FAILURE, "Follower action server rejected the goal."); transitionTo(State::FAILED, "follower rejected goal"); - RCLCPP_ERROR(get_logger(), "Follower action server rejected the goal."); return; } @@ -107,13 +109,26 @@ void NavCoordinator::handleResult(const GoalHandleNavigate::WrappedResult & resu return; } - transitionTo(State::FAILED, "follower finished with non-success error code"); - RCLCPP_WARN( - get_logger(), "Follower completed with error_code=%u", result.result->error_code); + switch (result.result->error_code) { + case NavigateToWaypoint::Result::OBSTACLE_DETECTED: + handleError(ErrorType::OBSTACLE_DETECTED, "Obstacle detected during trajectory following."); + break; + case NavigateToWaypoint::Result::PLANNING_FAILED: + handleError(ErrorType::PLANNING_FAILED_IN_FOLLOWER, "Path planning failed in follower."); + break; + case NavigateToWaypoint::Result::FAILURE: + handleError(ErrorType::FOLLOWER_FAILURE, "Follower reported generic failure."); + break; + 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; } @@ -123,6 +138,7 @@ void NavCoordinator::handleResult(const GoalHandleNavigate::WrappedResult & resu return; } + handleError(ErrorType::UNKNOWN_ERROR, "Unknown follower result code: " + std::to_string(static_cast(result.code))); transitionTo(State::FAILED, "unknown follower result code"); } @@ -158,6 +174,58 @@ void NavCoordinator::transitionTo(State new_state, const std::string & reason) state_ = new_state; } +void NavCoordinator::handleError(ErrorType error_type, const std::string & details) +{ + 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; + } +} + +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"; + } +} + } // namespace nav_coordinator int main(int argc, char ** argv) From 77fad0d5d40057479f85f977b09ccce28d6e5c41 Mon Sep 17 00:00:00 2001 From: nishia1 Date: Fri, 20 Feb 2026 17:26:47 -0500 Subject: [PATCH 04/25] added error msgs, pulled existing errs and then also added catch all error in case external nodes can sub to nav_coordinator_state topic for state updates + error updates msg format is state=, error=, details=
not fully tested yet!!!! --- urc_navigation/nav_testing/CMakeLists.txt | 4 +- .../nav_testing/include/nav_coordinator.hpp | 4 ++ urc_navigation/nav_testing/package.xml | 1 + .../nav_testing/src/nav_coordinator.cpp | 37 +++++++++++++++++++ 4 files changed, 45 insertions(+), 1 deletion(-) diff --git a/urc_navigation/nav_testing/CMakeLists.txt b/urc_navigation/nav_testing/CMakeLists.txt index 0d5d561b..bbc24695 100644 --- a/urc_navigation/nav_testing/CMakeLists.txt +++ b/urc_navigation/nav_testing/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(urc_msgs REQUIRED) include_directories(include) @@ -24,6 +25,7 @@ ament_target_dependencies(nav_coordinator rclcpp rclcpp_action geometry_msgs + std_msgs urc_msgs ) @@ -49,6 +51,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_dependencies(rclcpp rclcpp_action geometry_msgs urc_msgs) +ament_export_dependencies(rclcpp rclcpp_action geometry_msgs std_msgs urc_msgs) ament_package() diff --git a/urc_navigation/nav_testing/include/nav_coordinator.hpp b/urc_navigation/nav_testing/include/nav_coordinator.hpp index ed4e5397..d5ccdecb 100644 --- a/urc_navigation/nav_testing/include/nav_coordinator.hpp +++ b/urc_navigation/nav_testing/include/nav_coordinator.hpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace nav_coordinator { @@ -51,7 +52,9 @@ class NavCoordinator : public rclcpp::Node 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; State state_; std::string follower_action_name_; @@ -62,6 +65,7 @@ class NavCoordinator : public rclcpp::Node rclcpp::Subscription::SharedPtr waypoint_subscriber_; rclcpp_action::Client::SharedPtr follower_client_; + rclcpp::Publisher::SharedPtr state_publisher_; ErrorType last_error_; std::string last_error_details_; diff --git a/urc_navigation/nav_testing/package.xml b/urc_navigation/nav_testing/package.xml index 27c588cc..5c948929 100644 --- a/urc_navigation/nav_testing/package.xml +++ b/urc_navigation/nav_testing/package.xml @@ -11,6 +11,7 @@ rclcpp rclcpp_action geometry_msgs + std_msgs urc_msgs ament_lint_auto diff --git a/urc_navigation/nav_testing/src/nav_coordinator.cpp b/urc_navigation/nav_testing/src/nav_coordinator.cpp index d6b9e720..7fe802d5 100644 --- a/urc_navigation/nav_testing/src/nav_coordinator.cpp +++ b/urc_navigation/nav_testing/src/nav_coordinator.cpp @@ -14,6 +14,7 @@ NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) state_ = State::IDLE; last_error_ = ErrorType::NONE; + state_publisher_ = create_publisher("nav_coordinator_state", 10); follower_client_ = rclcpp_action::create_client(this, follower_action_name_); waypoint_subscriber_ = create_subscription( @@ -26,6 +27,7 @@ NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) "Nav Coordinator ready. Waiting for waypoints on topic '%s' and forwarding to action '%s'.", get_parameter("waypoint_topic").as_string().c_str(), follower_action_name_.c_str()); + publishState(); } void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg) @@ -172,6 +174,8 @@ void NavCoordinator::transitionTo(State new_state, const std::string & reason) get_logger(), "State transition: %s -> %s (%s)", state_name(state_), state_name(new_state), reason.c_str()); state_ = new_state; + publishState(); +} } void NavCoordinator::handleError(ErrorType error_type, const std::string & details) @@ -202,6 +206,7 @@ void NavCoordinator::handleError(ErrorType error_type, const std::string & detai RCLCPP_ERROR(get_logger(), "[UNHANDLED_ERROR] %s", details.c_str()); break; } + publishState(); } std::string NavCoordinator::errorTypeToString(ErrorType error_type) const @@ -226,6 +231,38 @@ std::string NavCoordinator::errorTypeToString(ErrorType error_type) const } } +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 int main(int argc, char ** argv) From 823cb600de3f436c5b3c661045ee506c0e4e3c2a Mon Sep 17 00:00:00 2001 From: Mrinal Jain <2mrinaljain@gmail.com> Date: Fri, 20 Feb 2026 17:40:32 -0500 Subject: [PATCH 05/25] autonomy-perception integration fixes --- urc_bringup/launch/autonomy.launch.py | 3 +- .../trajectory_following/CMakeLists.txt | 2 + .../config/pure_pursuit_config.yaml | 8 +- .../include/follower_action_server.hpp | 14 +- .../trajectory_following/package.xml | 3 +- .../src/follower_action_server.cpp | 142 ++++++++++++++---- urc_platform/package.xml | 1 + 7 files changed, 129 insertions(+), 44 deletions(-) diff --git a/urc_bringup/launch/autonomy.launch.py b/urc_bringup/launch/autonomy.launch.py index 1b3220e9..f900d946 100644 --- a/urc_bringup/launch/autonomy.launch.py +++ b/urc_bringup/launch/autonomy.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): pkg_urc_perception = get_package_share_directory("urc_perception") pkg_trajectory_following = get_package_share_directory("trajectory_following") - perception_config = os.path.join(pkg_urc_perception, "config", "elevation_mapping.yaml") + 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( @@ -37,6 +37,7 @@ def generate_launch_description(): package="urc_perception", executable="urc_perception_TraversabilityMapping", name="traversability_mapping", + parameters=[traversability_config], output="screen", ) diff --git a/urc_navigation/trajectory_following/CMakeLists.txt b/urc_navigation/trajectory_following/CMakeLists.txt index bea9ec54..727cf748 100644 --- a/urc_navigation/trajectory_following/CMakeLists.txt +++ b/urc_navigation/trajectory_following/CMakeLists.txt @@ -11,6 +11,7 @@ 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) @@ -35,6 +36,7 @@ set(dependencies rclcpp_components 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..1edf9c41 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 + lethal_cost_threshold: 0.5 cmd_vel_stamped: true - cmd_vel_topic: "/cmd_vel_autonomous" - # odom_topic: "/rover_drivetrain_controller/odom" - odom_topic: "base_link" + 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..00b33f84 100644 --- a/urc_navigation/trajectory_following/include/follower_action_server.hpp +++ b/urc_navigation/trajectory_following/include/follower_action_server.hpp @@ -7,13 +7,12 @@ #include #include #include -#include #include #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 +56,18 @@ 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(const grid_map_msgs::msg::GridMap & costmap, double x, double y); - nav_msgs::msg::OccupancyGrid current_costmap_; - rclcpp::Subscription::SharedPtr costmap_subscriber_; + grid_map_msgs::msg::GridMap current_costmap_; + 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 +75,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..2a6c5c2f 100644 --- a/urc_navigation/trajectory_following/package.xml +++ b/urc_navigation/trajectory_following/package.xml @@ -18,6 +18,7 @@ urc_msgs std_msgs nav_msgs + grid_map_msgs geometry_msgs tf2 tf2_geometry_msgs @@ -28,4 +29,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..687ecf62 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,20 @@ 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(); if (stamped_) { cmd_vel_stamped_pub_ = create_publisher( @@ -43,17 +46,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,7 +71,7 @@ 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; } @@ -188,17 +182,54 @@ void FollowerActionServer::publishZeroVelocity() { } } -int FollowerActionServer::getCost(const nav_msgs::msg::OccupancyGrid &costmap, +float FollowerActionServer::getCost(const grid_map_msgs::msg::GridMap &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; + // Find the layer index + auto it = std::find(costmap.layers.begin(), costmap.layers.end(), costmap_layer_); + if (it == costmap.layers.end()) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Costmap layer '%s' not found", costmap_layer_.c_str()); + return 0.0f; + } + size_t layer_index = std::distance(costmap.layers.begin(), it); + + // Get grid dimensions from the data layout + if (costmap.data.empty() || layer_index >= costmap.data.size()) { + return 0.0f; + } + + const auto &layer_data = costmap.data[layer_index]; + if (layer_data.layout.dim.size() < 2) { + return 0.0f; + } + + // GridMap uses center of grid as origin + double origin_x = costmap.info.pose.position.x; + double origin_y = costmap.info.pose.position.y; + double resolution = costmap.info.resolution; + + // Calculate map indices (GridMap stores data row-major) + int width = layer_data.layout.dim[1].size; + int height = layer_data.layout.dim[0].size; + + // Transform world coordinates to grid coordinates + double rel_x = x - origin_x; + double rel_y = y - origin_y; + + int map_x = static_cast(rel_x / resolution + width / 2.0); + int map_y = static_cast(rel_y / resolution + height / 2.0); + + if (map_x < 0 || map_x >= width || map_y < 0 || map_y >= height) { + return 0.0f; + } - if (map_x < 0 || (unsigned int)map_x >= costmap.info.width || map_y < 0 || - (unsigned int)map_y >= costmap.info.height) { - return 0; + // Access data in row-major order + size_t index = map_y * width + map_x; + if (index >= layer_data.data.size()) { + return 0.0f; } - return costmap.data[map_y * costmap.info.width + map_x]; + return layer_data.data[index]; } nav_msgs::msg::Path FollowerActionServer::callPlanningService( @@ -266,9 +297,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 +368,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( @@ -406,7 +469,7 @@ void FollowerActionServer::execute_navigate( } } else if (getCost(current_costmap_, 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 +478,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 +524,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_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 From 4e88503e42bb2ac5e570d8e4d821a8bd05be9504 Mon Sep 17 00:00:00 2001 From: nishia1 Date: Fri, 20 Feb 2026 17:51:11 -0500 Subject: [PATCH 06/25] fix brace error whoops --- urc_navigation/nav_testing/src/nav_coordinator.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/urc_navigation/nav_testing/src/nav_coordinator.cpp b/urc_navigation/nav_testing/src/nav_coordinator.cpp index 7fe802d5..a35b7b76 100644 --- a/urc_navigation/nav_testing/src/nav_coordinator.cpp +++ b/urc_navigation/nav_testing/src/nav_coordinator.cpp @@ -27,7 +27,11 @@ NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) "Nav Coordinator ready. Waiting for waypoints on topic '%s' and forwarding to action '%s'.", get_parameter("waypoint_topic").as_string().c_str(), follower_action_name_.c_str()); - publishState(); + + // Publish initial state + if (state_publisher_) { + publishState(); + } } void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg) @@ -176,7 +180,6 @@ void NavCoordinator::transitionTo(State new_state, const std::string & reason) state_ = new_state; publishState(); } -} void NavCoordinator::handleError(ErrorType error_type, const std::string & details) { From 3a6ef4bc43d4aac4ef5af5e11db5148e4a1a877e Mon Sep 17 00:00:00 2001 From: nishia1 Date: Fri, 20 Feb 2026 17:59:08 -0500 Subject: [PATCH 07/25] fixed return type, check if err fixed --- .../nav_testing/src/nav_coordinator.cpp | 40 +++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/urc_navigation/nav_testing/src/nav_coordinator.cpp b/urc_navigation/nav_testing/src/nav_coordinator.cpp index a35b7b76..d1f85718 100644 --- a/urc_navigation/nav_testing/src/nav_coordinator.cpp +++ b/urc_navigation/nav_testing/src/nav_coordinator.cpp @@ -28,7 +28,6 @@ NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) get_parameter("waypoint_topic").as_string().c_str(), follower_action_name_.c_str()); - // Publish initial state if (state_publisher_) { publishState(); } @@ -154,25 +153,26 @@ void NavCoordinator::transitionTo(State new_state, const std::string & reason) return; } - const auto state_name = [](State state) { - 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"; - } - return "UNKNOWN"; - }; + 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_), From d33d6cbdbf79da5c116362d5cdfaff8cca75003c Mon Sep 17 00:00:00 2001 From: Mrinal Jain <2mrinaljain@gmail.com> Date: Fri, 20 Feb 2026 18:16:46 -0500 Subject: [PATCH 08/25] fix follower action server executor bug --- urc_navigation/nav_testing/CMakeLists.txt | 49 ++++++++++++------- .../nav_testing/src/nav_coordinator.cpp | 10 +--- .../src/follower_action_server.cpp | 4 +- 3 files changed, 34 insertions(+), 29 deletions(-) diff --git a/urc_navigation/nav_testing/CMakeLists.txt b/urc_navigation/nav_testing/CMakeLists.txt index bbc24695..3d47553e 100644 --- a/urc_navigation/nav_testing/CMakeLists.txt +++ b/urc_navigation/nav_testing/CMakeLists.txt @@ -1,56 +1,69 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.5) project(nav_testing) include(../../cmake/default_settings.cmake) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(urc_msgs REQUIRED) -include_directories(include) +include_directories( + include +) -add_executable(nav_coordinator +# Library creation +add_library(${PROJECT_NAME} SHARED src/nav_coordinator.cpp ) -ament_target_dependencies(nav_coordinator +set(dependencies rclcpp rclcpp_action + rclcpp_components geometry_msgs std_msgs urc_msgs ) -install(TARGETS - nav_coordinator - DESTINATION lib/${PROJECT_NAME} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) -install(DIRECTORY include/ - DESTINATION include +# 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_dependencies(rclcpp rclcpp_action geometry_msgs std_msgs urc_msgs) + +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) ament_package() diff --git a/urc_navigation/nav_testing/src/nav_coordinator.cpp b/urc_navigation/nav_testing/src/nav_coordinator.cpp index d1f85718..53e04433 100644 --- a/urc_navigation/nav_testing/src/nav_coordinator.cpp +++ b/urc_navigation/nav_testing/src/nav_coordinator.cpp @@ -268,11 +268,5 @@ void NavCoordinator::publishState() } // namespace nav_coordinator -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(rclcpp::NodeOptions()); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(nav_coordinator::NavCoordinator) diff --git a/urc_navigation/trajectory_following/src/follower_action_server.cpp b/urc_navigation/trajectory_following/src/follower_action_server.cpp index 687ecf62..9aaff7e0 100644 --- a/urc_navigation/trajectory_following/src/follower_action_server.cpp +++ b/urc_navigation/trajectory_following/src/follower_action_server.cpp @@ -243,9 +243,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) { From fe76e0915cf48dc3d8d3c518389ac57369244893 Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Fri, 20 Feb 2026 23:31:50 +0000 Subject: [PATCH 09/25] Debug issues in A* re unit conversions and costs --- urc_navigation/path_planning/src/astar.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/urc_navigation/path_planning/src/astar.cpp b/urc_navigation/path_planning/src/astar.cpp index cf8add1d..1dd04df5 100644 --- a/urc_navigation/path_planning/src/astar.cpp +++ b/urc_navigation/path_planning/src/astar.cpp @@ -7,6 +7,7 @@ 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; @@ -102,7 +103,11 @@ double AStar::cost(const AStarNode * from, const AStarNode * to) // Check cell cost if index is within the costmap if (costmap_index >= 0 && costmap_index < costmap_.data.size()) { - cell_cost = costmap_.data[costmap_index]; + if (costmap_.data[costmap_index] >= 0) { + cell_cost = costmap_.data[costmap_index] + 1; + } else { + cell_cost = 5; + } } double distance = std::sqrt(std::pow(to->x - from->x, 2) + std::pow(to->y - from->y, 2)); @@ -118,16 +123,17 @@ std::vector AStar::getNeighbors(const AStarNode * node) if (i == 0 && j == 0) { continue; } - + // x and y must be meters double x = node->x + (i * costmap_.info.resolution); double y = node->y + (j * costmap_.info.resolution); + // pose.position.x and pose.position.y are both in meters 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); + // coord is in cells + Coordinate coord = getCoordinateByPose(pose); + if (coord.x >= 0 && coord.x < costmap_.info.width && coord.y >= 0 && coord.y < costmap_.info.height) { AStarNode * neighbor = getNodeRef(coord.x, coord.y); if (neighbor->status == NodeStatus::None) { neighbor->x = x; From baea1faeeed6322aba4ce39bbe9734dfc36e5d7d Mon Sep 17 00:00:00 2001 From: Mrinal Jain <2mrinaljain@gmail.com> Date: Fri, 20 Feb 2026 18:34:27 -0500 Subject: [PATCH 10/25] grid map visualizer is launched --- urc_bringup/launch/autonomy.launch.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/urc_bringup/launch/autonomy.launch.py b/urc_bringup/launch/autonomy.launch.py index f900d946..a9b29194 100644 --- a/urc_bringup/launch/autonomy.launch.py +++ b/urc_bringup/launch/autonomy.launch.py @@ -41,11 +41,20 @@ def generate_launch_description(): 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 ] ) From 2a969b42462e09cd7af8edbcc43fdc2edb134524 Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Sun, 22 Feb 2026 23:03:21 +0000 Subject: [PATCH 11/25] Launch file to test LiDAR --- urc_perception/launch/test_sick.py | 35 ++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 urc_perception/launch/test_sick.py diff --git a/urc_perception/launch/test_sick.py b/urc_perception/launch/test_sick.py new file mode 100644 index 00000000..82c4ca0c --- /dev/null +++ b/urc_perception/launch/test_sick.py @@ -0,0 +1,35 @@ +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", + "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, + ]) + From 1d33feb135823c2316fde7f100aa7eeac67dee7c Mon Sep 17 00:00:00 2001 From: Mrinal Jain <2mrinaljain@gmail.com> Date: Fri, 27 Feb 2026 16:30:58 -0500 Subject: [PATCH 12/25] fix pcl grid map params --- .../config/pcl_grid_map_params.yaml | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/urc_perception/config/pcl_grid_map_params.yaml b/urc_perception/config/pcl_grid_map_params.yaml index 2fcd5a7a..8428db2f 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.1 + height_type: 0 + height_thresh: 1.0 From 093eb2a846bf7d4a870189e21ad2dc3a766dbea5 Mon Sep 17 00:00:00 2001 From: nishia1 Date: Fri, 27 Feb 2026 16:49:42 -0500 Subject: [PATCH 13/25] gps coordinates input converted to map coordinate same state machine logic as before --- urc_navigation/nav_testing/CMakeLists.txt | 4 + .../nav_testing/include/nav_coordinator.hpp | 10 +++ urc_navigation/nav_testing/package.xml | 2 + .../nav_testing/src/nav_coordinator.cpp | 88 ++++++++++++++++++- 4 files changed, 103 insertions(+), 1 deletion(-) diff --git a/urc_navigation/nav_testing/CMakeLists.txt b/urc_navigation/nav_testing/CMakeLists.txt index 3d47553e..7b27b623 100644 --- a/urc_navigation/nav_testing/CMakeLists.txt +++ b/urc_navigation/nav_testing/CMakeLists.txt @@ -9,6 +9,8 @@ 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(urc_msgs REQUIRED) @@ -26,6 +28,8 @@ set(dependencies rclcpp_action rclcpp_components geometry_msgs + geodesy + geographic_msgs std_msgs urc_msgs ) diff --git a/urc_navigation/nav_testing/include/nav_coordinator.hpp b/urc_navigation/nav_testing/include/nav_coordinator.hpp index d5ccdecb..e166e282 100644 --- a/urc_navigation/nav_testing/include/nav_coordinator.hpp +++ b/urc_navigation/nav_testing/include/nav_coordinator.hpp @@ -2,8 +2,11 @@ #define NAV_COORDINATOR_HPP_ #include +#include +#include #include #include +#include #include #include @@ -42,7 +45,10 @@ class NavCoordinator : public rclcpp::Node }; 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) const; void handleGoalResponse(const GoalHandleNavigate::SharedPtr & goal_handle); void handleFeedback( @@ -59,11 +65,15 @@ class NavCoordinator : public rclcpp::Node State state_; std::string follower_action_name_; bool cancel_on_new_waypoint_; + std::string map_frame_id_; + bool gps_conversion_ready_; + geodesy::UTMPoint map_origin_utm_; geometry_msgs::msg::PoseStamped active_waypoint_; GoalHandleNavigate::SharedPtr active_goal_handle_; rclcpp::Subscription::SharedPtr waypoint_subscriber_; + rclcpp::Subscription::SharedPtr gps_waypoint_subscriber_; rclcpp_action::Client::SharedPtr follower_client_; rclcpp::Publisher::SharedPtr state_publisher_; diff --git a/urc_navigation/nav_testing/package.xml b/urc_navigation/nav_testing/package.xml index 5c948929..6455f3f9 100644 --- a/urc_navigation/nav_testing/package.xml +++ b/urc_navigation/nav_testing/package.xml @@ -11,6 +11,8 @@ rclcpp rclcpp_action geometry_msgs + geodesy + geographic_msgs std_msgs urc_msgs diff --git a/urc_navigation/nav_testing/src/nav_coordinator.cpp b/urc_navigation/nav_testing/src/nav_coordinator.cpp index 53e04433..754979ec 100644 --- a/urc_navigation/nav_testing/src/nav_coordinator.cpp +++ b/urc_navigation/nav_testing/src/nav_coordinator.cpp @@ -6,13 +6,40 @@ NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) : rclcpp::Node("nav_coordinator", options) { 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("map_origin_latitude", 0.0); + declare_parameter("map_origin_longitude", 0.0); + declare_parameter("map_origin_altitude", 0.0); 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(); state_ = State::IDLE; last_error_ = ErrorType::NONE; + gps_conversion_ready_ = true; + + geographic_msgs::msg::GeoPoint map_origin; + map_origin.latitude = get_parameter("map_origin_latitude").as_double(); + map_origin.longitude = get_parameter("map_origin_longitude").as_double(); + map_origin.altitude = get_parameter("map_origin_altitude").as_double(); + + if ( + map_origin.latitude < -90.0 || map_origin.latitude > 90.0 || + map_origin.longitude < -180.0 || map_origin.longitude > 180.0) + { + gps_conversion_ready_ = false; + RCLCPP_WARN( + get_logger(), + "Invalid map origin lat/lon for GPS conversion (lat=%.8f lon=%.8f). " + "GPS waypoints will be rejected.", + map_origin.latitude, + map_origin.longitude); + } else { + geodesy::fromMsg(map_origin, map_origin_utm_); + } state_publisher_ = create_publisher("nav_coordinator_state", 10); follower_client_ = rclcpp_action::create_client(this, follower_action_name_); @@ -22,10 +49,16 @@ NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) 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. Waiting for waypoints on topic '%s' and forwarding to action '%s'.", + "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_) { @@ -51,6 +84,59 @@ void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::Share sendFollowerGoal(active_waypoint_); } +void NavCoordinator::handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr msg) +{ + if (!gps_conversion_ready_) { + handleError( + ErrorType::PLANNER_FAILURE, + "Cannot process GPS waypoint: invalid map origin parameters for conversion."); + transitionTo(State::FAILED, "gps waypoint rejected - conversion unavailable"); + return; + } + + const auto converted_waypoint = convertGpsToMapWaypoint(*msg); + active_waypoint_ = converted_waypoint; + + 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_); +} + +geometry_msgs::msg::PoseStamped NavCoordinator::convertGpsToMapWaypoint( + const urc_msgs::msg::Waypoint & waypoint) const +{ + 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); + + geometry_msgs::msg::PoseStamped pose; + pose.header.stamp = now(); + pose.header.frame_id = map_frame_id_; + pose.pose.position.x = waypoint_utm.easting - map_origin_utm_.easting; + pose.pose.position.y = waypoint_utm.northing - map_origin_utm_.northing; + pose.pose.position.z = 0.0; + 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"); From 0cdf1100140a0c895616fff77a40673eaef6ffeb Mon Sep 17 00:00:00 2001 From: nishia1 Date: Fri, 27 Feb 2026 16:58:55 -0500 Subject: [PATCH 14/25] fixed hpp to include math bf geodesy --- urc_navigation/nav_testing/include/nav_coordinator.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/urc_navigation/nav_testing/include/nav_coordinator.hpp b/urc_navigation/nav_testing/include/nav_coordinator.hpp index e166e282..f653ebb3 100644 --- a/urc_navigation/nav_testing/include/nav_coordinator.hpp +++ b/urc_navigation/nav_testing/include/nav_coordinator.hpp @@ -1,6 +1,7 @@ #ifndef NAV_COORDINATOR_HPP_ #define NAV_COORDINATOR_HPP_ +#include #include #include #include From 7580a835e4c8185e422f22fc0d33dee155da4c86 Mon Sep 17 00:00:00 2001 From: Mrinal Jain <2mrinaljain@gmail.com> Date: Fri, 27 Feb 2026 17:07:14 -0500 Subject: [PATCH 15/25] fix sim lidar and costmap resolutions --- .../simplified_swerve_sensors.xacro | 8 ++++---- urc_perception/config/pcl_grid_map_params.yaml | 2 +- .../config/traversability_params.yaml | 18 +++++------------- 3 files changed, 10 insertions(+), 18 deletions(-) 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..d7215c83 100644 --- a/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro +++ b/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro @@ -50,16 +50,16 @@ - 720 + 2880 1 -3.14 3.14 - 720 + 144 1 - -3.14 - 3.14 + -0.366519 + 0.366519 diff --git a/urc_perception/config/pcl_grid_map_params.yaml b/urc_perception/config/pcl_grid_map_params.yaml index 8428db2f..c878817e 100644 --- a/urc_perception/config/pcl_grid_map_params.yaml +++ b/urc_perception/config/pcl_grid_map_params.yaml @@ -25,6 +25,6 @@ pcl_grid_map_extraction: z: 0.02 grid_map: min_num_points_per_cell: 2 - resolution: 0.1 + 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..f1e3e232 100644 --- a/urc_perception/config/traversability_params.yaml +++ b/urc_perception/config/traversability_params.yaml @@ -42,7 +42,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 +60,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: From f6750d9222ef4921be72f1c4e483e7461200470a Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Fri, 27 Feb 2026 22:24:37 +0000 Subject: [PATCH 16/25] LiDAR hardware setup --- urc_perception/launch/test_sick.py | 1 + 1 file changed, 1 insertion(+) diff --git a/urc_perception/launch/test_sick.py b/urc_perception/launch/test_sick.py index 82c4ca0c..47e341e1 100644 --- a/urc_perception/launch/test_sick.py +++ b/urc_perception/launch/test_sick.py @@ -11,6 +11,7 @@ def generate_launch_description(): 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", From 58e6475f128c18fc8fe94407979bef22d82b8acd Mon Sep 17 00:00:00 2001 From: nishia1 Date: Fri, 27 Feb 2026 18:16:11 -0500 Subject: [PATCH 17/25] fixed launch file, had issue with exec name --- urc_bringup/launch/autonomy.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/urc_bringup/launch/autonomy.launch.py b/urc_bringup/launch/autonomy.launch.py index a9b29194..196b98e4 100644 --- a/urc_bringup/launch/autonomy.launch.py +++ b/urc_bringup/launch/autonomy.launch.py @@ -13,7 +13,7 @@ def generate_launch_description(): state_machine_node = Node( package="nav_testing", - executable="nav_coordinator", + executable="nav_testing_NavCoordinator", name="nav_coordinator", output="screen", ) From c995da169b24081a9e4e0f3162ab682324b6d35e Mon Sep 17 00:00:00 2001 From: Mrinal Jain <2mrinaljain@gmail.com> Date: Fri, 27 Feb 2026 18:35:46 -0500 Subject: [PATCH 18/25] fix utm frame and nav coordinator node launch --- urc_bringup/launch/autonomy.launch.py | 2 +- urc_localization/config/ekf_redemption.yaml | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/urc_bringup/launch/autonomy.launch.py b/urc_bringup/launch/autonomy.launch.py index a9b29194..196b98e4 100644 --- a/urc_bringup/launch/autonomy.launch.py +++ b/urc_bringup/launch/autonomy.launch.py @@ -13,7 +13,7 @@ def generate_launch_description(): state_machine_node = Node( package="nav_testing", - executable="nav_coordinator", + executable="nav_testing_NavCoordinator", name="nav_coordinator", output="screen", ) 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 From 6d2000875643e128b6592d678868e2e0434e7340 Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Sun, 1 Mar 2026 22:10:47 +0000 Subject: [PATCH 19/25] Rewrote A* to support GridMaps and support planning for goals outside costmap --- urc_navigation/path_planning/CMakeLists.txt | 2 + .../path_planning/include/astar.hpp | 29 ++- .../path_planning/include/planner_server.hpp | 8 +- urc_navigation/path_planning/package.xml | 3 +- urc_navigation/path_planning/src/astar.cpp | 239 +++++++++++++++--- .../path_planning/src/planner_server.cpp | 7 +- 6 files changed, 235 insertions(+), 53 deletions(-) diff --git a/urc_navigation/path_planning/CMakeLists.txt b/urc_navigation/path_planning/CMakeLists.txt index 1affe8f2..07e30625 100644 --- a/urc_navigation/path_planning/CMakeLists.txt +++ b/urc_navigation/path_planning/CMakeLists.txt @@ -10,6 +10,7 @@ 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) @@ -34,6 +35,7 @@ set(dependencies rclcpp_components urc_msgs std_msgs + grid_map_msgs sensor_msgs diagnostic_updater cv_bridge diff --git a/urc_navigation/path_planning/include/astar.hpp b/urc_navigation/path_planning/include/astar.hpp index fe32fcc3..77b33146 100644 --- a/urc_navigation/path_planning/include/astar.hpp +++ b/urc_navigation/path_planning/include/astar.hpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include #include @@ -72,7 +72,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 +117,24 @@ 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_; geometry_msgs::msg::Pose start_pose_; geometry_msgs::msg::Pose goal_pose_; AStarNode goal_node_; @@ -120,6 +142,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..44e96be4 100644 --- a/urc_navigation/path_planning/package.xml +++ b/urc_navigation/path_planning/package.xml @@ -17,6 +17,7 @@ image_transport std_msgs nav_msgs + grid_map_msgs tf2 tf2_geometry_msgs sensor_msgs @@ -30,4 +31,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 1dd04df5..977c835a 100644 --- a/urc_navigation/path_planning/src/astar.cpp +++ b/urc_navigation/path_planning/src/astar.cpp @@ -7,18 +7,170 @@ 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; } +void AStar::setCostmapLayer(const std::string & layer) +{ + costmap_layer_ = layer; +} + +int AStar::getLayerIndex() const +{ + auto it = std::find(costmap_.layers.begin(), costmap_.layers.end(), costmap_layer_); + if (it == costmap_.layers.end()) { + return -1; + } + return static_cast(std::distance(costmap_.layers.begin(), it)); +} + +bool AStar::getMapDimensions(int & width, int & height) const +{ + int idx = getLayerIndex(); + if (idx < 0) { + return false; + } + if (costmap_.data.empty() || idx >= static_cast(costmap_.data.size())) { + return false; + } + + if (costmap_.data[idx].layout.dim.size() < 2) { + return false; + } + + height = costmap_.data[idx].layout.dim[0].size; + width = costmap_.data[idx].layout.dim[1].size; + return true; +} + +bool AStar::worldToGrid(double x, double y, int & map_x, int & map_y) const +{ + int width, height; + if (!getMapDimensions(width, height)) { + return false; + } + + double resolution = costmap_.info.resolution; + if (resolution <= 0.0) { + return false; + } + + double origin_x = costmap_.info.pose.position.x; + double origin_y = costmap_.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; +} + +double AStar::getCellCost(double x, double y) const +{ + int idx = getLayerIndex(); + if (idx < 0) { + throw std::runtime_error("Costmap layer not found"); + } + if (costmap_.data.empty() || idx >= static_cast(costmap_.data.size())) { + throw std::runtime_error("Costmap data is invalid"); + } + + int width, height; + if (!getMapDimensions(width, height)) { + throw std::runtime_error("Costmap dimensions are invalid"); + } + + int map_x, map_y; + if (!worldToGrid(x, y, map_x, map_y)) { + throw std::runtime_error("Point is outside costmap bounds"); + } + + size_t index = static_cast(map_y * width + map_x); + if (index >= costmap_.data[idx].data.size()) { + throw std::runtime_error("Costmap index is out of range"); + } + + return costmap_.data[idx].data[index]; +} + +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) { goal_node_.x = goal_pose.position.x; @@ -29,18 +181,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); @@ -59,6 +228,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; } @@ -95,23 +267,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()) { - if (costmap_.data[costmap_index] >= 0) { - cell_cost = costmap_.data[costmap_index] + 1; - } else { - cell_cost = 5; - } - } - + 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) @@ -123,25 +281,22 @@ std::vector AStar::getNeighbors(const AStarNode * node) if (i == 0 && j == 0) { continue; } - // x and y must be meters + double x = node->x + (i * costmap_.info.resolution); double y = node->y + (j * costmap_.info.resolution); - // pose.position.x and pose.position.y are both in meters - geometry_msgs::msg::Pose pose; - pose.position.x = x; - pose.position.y = y; - // coord is in cells - Coordinate coord = getCoordinateByPose(pose); - if (coord.x >= 0 && coord.x < costmap_.info.width && coord.y >= 0 && coord.y < costmap_.info.height) { - 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; From 59377aa75f6acc5d6df2f7b05f7588f6f5072b79 Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Mon, 2 Mar 2026 00:02:06 +0000 Subject: [PATCH 20/25] Refactoring all costmap handling for planning/control to avoid repetition --- .../path_planning/include/grid_map_utils.hpp | 27 ++++++ .../path_planning/src/grid_map_utils.cpp | 95 +++++++++++++++++++ 2 files changed, 122 insertions(+) create mode 100644 urc_navigation/path_planning/include/grid_map_utils.hpp create mode 100644 urc_navigation/path_planning/src/grid_map_utils.cpp diff --git a/urc_navigation/path_planning/include/grid_map_utils.hpp b/urc_navigation/path_planning/include/grid_map_utils.hpp new file mode 100644 index 00000000..fa37049a --- /dev/null +++ b/urc_navigation/path_planning/include/grid_map_utils.hpp @@ -0,0 +1,27 @@ +#ifndef GRID_MAP_UTILS_HPP_ +#define 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_; + }; +} +#endif \ No newline at end of file diff --git a/urc_navigation/path_planning/src/grid_map_utils.cpp b/urc_navigation/path_planning/src/grid_map_utils.cpp new file mode 100644 index 00000000..4b5ff61a --- /dev/null +++ b/urc_navigation/path_planning/src/grid_map_utils.cpp @@ -0,0 +1,95 @@ +#include "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; + } + +} From dd141c1d4c9e18020948c0513b268e6655219733 Mon Sep 17 00:00:00 2001 From: Mrinal Jain <2mrinaljain@gmail.com> Date: Sun, 8 Mar 2026 16:33:57 -0400 Subject: [PATCH 21/25] use utm frame for gps coordinate conversion --- urc_navigation/nav_testing/CMakeLists.txt | 4 ++ .../nav_testing/include/nav_coordinator.hpp | 15 ++-- .../nav_testing/src/nav_coordinator.cpp | 71 ++++++++++--------- 3 files changed, 51 insertions(+), 39 deletions(-) diff --git a/urc_navigation/nav_testing/CMakeLists.txt b/urc_navigation/nav_testing/CMakeLists.txt index 7b27b623..5f4ec46c 100644 --- a/urc_navigation/nav_testing/CMakeLists.txt +++ b/urc_navigation/nav_testing/CMakeLists.txt @@ -12,6 +12,8 @@ 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( @@ -31,6 +33,8 @@ set(dependencies geodesy geographic_msgs std_msgs + tf2_geometry_msgs + tf2_ros urc_msgs ) diff --git a/urc_navigation/nav_testing/include/nav_coordinator.hpp b/urc_navigation/nav_testing/include/nav_coordinator.hpp index f653ebb3..03b2af15 100644 --- a/urc_navigation/nav_testing/include/nav_coordinator.hpp +++ b/urc_navigation/nav_testing/include/nav_coordinator.hpp @@ -2,14 +2,18 @@ #define NAV_COORDINATOR_HPP_ #include +#include + #include #include #include #include #include -#include -#include #include +#include +#include +#include +#include namespace nav_coordinator { @@ -49,7 +53,7 @@ class NavCoordinator : public rclcpp::Node 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) const; + const urc_msgs::msg::Waypoint & waypoint); void handleGoalResponse(const GoalHandleNavigate::SharedPtr & goal_handle); void handleFeedback( @@ -67,8 +71,7 @@ class NavCoordinator : public rclcpp::Node std::string follower_action_name_; bool cancel_on_new_waypoint_; std::string map_frame_id_; - bool gps_conversion_ready_; - geodesy::UTMPoint map_origin_utm_; + std::string utm_frame_id_; geometry_msgs::msg::PoseStamped active_waypoint_; GoalHandleNavigate::SharedPtr active_goal_handle_; @@ -77,6 +80,8 @@ class NavCoordinator : public rclcpp::Node 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_; diff --git a/urc_navigation/nav_testing/src/nav_coordinator.cpp b/urc_navigation/nav_testing/src/nav_coordinator.cpp index 754979ec..8e43cec7 100644 --- a/urc_navigation/nav_testing/src/nav_coordinator.cpp +++ b/urc_navigation/nav_testing/src/nav_coordinator.cpp @@ -1,5 +1,10 @@ #include "nav_coordinator.hpp" +#include +#include +#include +#include + namespace nav_coordinator { NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) @@ -10,36 +15,17 @@ NavCoordinator::NavCoordinator(const rclcpp::NodeOptions & options) declare_parameter("follower_action_name", "navigate_to_waypoint"); declare_parameter("cancel_on_new_waypoint", true); declare_parameter("map_frame_id", "map"); - declare_parameter("map_origin_latitude", 0.0); - declare_parameter("map_origin_longitude", 0.0); - declare_parameter("map_origin_altitude", 0.0); + 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; - gps_conversion_ready_ = true; - - geographic_msgs::msg::GeoPoint map_origin; - map_origin.latitude = get_parameter("map_origin_latitude").as_double(); - map_origin.longitude = get_parameter("map_origin_longitude").as_double(); - map_origin.altitude = get_parameter("map_origin_altitude").as_double(); - - if ( - map_origin.latitude < -90.0 || map_origin.latitude > 90.0 || - map_origin.longitude < -180.0 || map_origin.longitude > 180.0) - { - gps_conversion_ready_ = false; - RCLCPP_WARN( - get_logger(), - "Invalid map origin lat/lon for GPS conversion (lat=%.8f lon=%.8f). " - "GPS waypoints will be rejected.", - map_origin.latitude, - map_origin.longitude); - } else { - geodesy::fromMsg(map_origin, map_origin_utm_); - } + + 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_); @@ -86,15 +72,17 @@ void NavCoordinator::handleWaypoint(const geometry_msgs::msg::PoseStamped::Share void NavCoordinator::handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr msg) { - if (!gps_conversion_ready_) { + geometry_msgs::msg::PoseStamped converted_waypoint; + try { + converted_waypoint = convertGpsToMapWaypoint(*msg); + } catch (const std::exception & ex) { handleError( ErrorType::PLANNER_FAILURE, - "Cannot process GPS waypoint: invalid map origin parameters for conversion."); - transitionTo(State::FAILED, "gps waypoint rejected - conversion unavailable"); + std::string("Cannot process GPS waypoint: ") + ex.what()); + transitionTo(State::FAILED, "gps waypoint rejected - transform unavailable"); return; } - const auto converted_waypoint = convertGpsToMapWaypoint(*msg); active_waypoint_ = converted_waypoint; RCLCPP_INFO( @@ -116,7 +104,7 @@ void NavCoordinator::handleGpsWaypoint(const urc_msgs::msg::Waypoint::SharedPtr } geometry_msgs::msg::PoseStamped NavCoordinator::convertGpsToMapWaypoint( - const urc_msgs::msg::Waypoint & waypoint) const + const urc_msgs::msg::Waypoint & waypoint) { geographic_msgs::msg::GeoPoint geo_point; geo_point.latitude = waypoint.latitude; @@ -126,12 +114,27 @@ geometry_msgs::msg::PoseStamped NavCoordinator::convertGpsToMapWaypoint( geodesy::UTMPoint waypoint_utm; geodesy::fromMsg(geo_point, waypoint_utm); + 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; + + 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.stamp = now(); - pose.header.frame_id = map_frame_id_; - pose.pose.position.x = waypoint_utm.easting - map_origin_utm_.easting; - pose.pose.position.y = waypoint_utm.northing - map_origin_utm_.northing; - pose.pose.position.z = 0.0; + 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; From fc3811fbf5d8b11517456425944f2a0aa3cfb66b Mon Sep 17 00:00:00 2001 From: Hiptostee Date: Fri, 6 Mar 2026 17:43:08 -0500 Subject: [PATCH 22/25] cachemap perchance? --- .../simplified_swerve_sensors.xacro | 2 +- .../config/traversability_params.yaml | 8 + .../include/traversability_mapping.hpp | 54 +++- urc_perception/launch/mapping.launch.py | 9 - urc_perception/src/traversability_mapping.cpp | 302 +++++++++++++----- 5 files changed, 273 insertions(+), 102 deletions(-) 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 d7215c83..d988aae1 100644 --- a/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro +++ b/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro @@ -64,7 +64,7 @@ 0.05 - 10.0 + 65.0 0.01 diff --git a/urc_perception/config/traversability_params.yaml b/urc_perception/config/traversability_params.yaml index f1e3e232..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: 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/src/traversability_mapping.cpp b/urc_perception/src/traversability_mapping.cpp index ead54ee2..7443eceb 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,250 @@ 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_); + auto msg2 = grid_map::GridMapRosConverter::toMessage(cache_map_); + + grid_map_publisher_->publish(std::move(msg1)); + cache_map_publisher_->publish(std::move(msg2)); } -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) From 7721202f8acdc927d6339fe65ef0384f1866b0b5 Mon Sep 17 00:00:00 2001 From: Hiptostee Date: Fri, 6 Mar 2026 17:55:30 -0500 Subject: [PATCH 23/25] cachemap just is costmap now topic --- urc_perception/src/traversability_mapping.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/urc_perception/src/traversability_mapping.cpp b/urc_perception/src/traversability_mapping.cpp index 7443eceb..033070e7 100644 --- a/urc_perception/src/traversability_mapping.cpp +++ b/urc_perception/src/traversability_mapping.cpp @@ -187,10 +187,8 @@ void TraversabilityMapping::publishCacheMap() } auto msg1 = grid_map::GridMapRosConverter::toMessage(cache_map_); - auto msg2 = grid_map::GridMapRosConverter::toMessage(cache_map_); grid_map_publisher_->publish(std::move(msg1)); - cache_map_publisher_->publish(std::move(msg2)); } void TraversabilityMapping::handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) From 6736509730581a09de22ac3435d4703c4ea737ea Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Sun, 8 Mar 2026 21:18:17 +0000 Subject: [PATCH 24/25] Refactored astar to use grid_map_utils --- urc_navigation/path_planning/CMakeLists.txt | 1 + .../path_planning/include/astar.hpp | 2 + urc_navigation/path_planning/src/astar.cpp | 73 +++---------------- .../include/follower_action_server.hpp | 4 +- 4 files changed, 15 insertions(+), 65 deletions(-) diff --git a/urc_navigation/path_planning/CMakeLists.txt b/urc_navigation/path_planning/CMakeLists.txt index 07e30625..3a9286f5 100644 --- a/urc_navigation/path_planning/CMakeLists.txt +++ b/urc_navigation/path_planning/CMakeLists.txt @@ -28,6 +28,7 @@ include_directories( add_library(${PROJECT_NAME} SHARED src/planner_server.cpp src/astar.cpp + src/grid_map_utils.cpp ) set(dependencies diff --git a/urc_navigation/path_planning/include/astar.hpp b/urc_navigation/path_planning/include/astar.hpp index 77b33146..814d914f 100644 --- a/urc_navigation/path_planning/include/astar.hpp +++ b/urc_navigation/path_planning/include/astar.hpp @@ -6,6 +6,7 @@ #include #include #include +#include "grid_map_utils.hpp" #include #include #include @@ -135,6 +136,7 @@ class AStar 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_; diff --git a/urc_navigation/path_planning/src/astar.cpp b/urc_navigation/path_planning/src/astar.cpp index 977c835a..becd7173 100644 --- a/urc_navigation/path_planning/src/astar.cpp +++ b/urc_navigation/path_planning/src/astar.cpp @@ -18,93 +18,38 @@ Coordinate AStar::getCoordinateByPose(const geometry_msgs::msg::Pose & pose) 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 { - auto it = std::find(costmap_.layers.begin(), costmap_.layers.end(), costmap_layer_); - if (it == costmap_.layers.end()) { - return -1; - } - return static_cast(std::distance(costmap_.layers.begin(), it)); + return grid_map_utils_.getLayerIndex(); } bool AStar::getMapDimensions(int & width, int & height) const { - int idx = getLayerIndex(); - if (idx < 0) { - return false; - } - if (costmap_.data.empty() || idx >= static_cast(costmap_.data.size())) { - return false; - } - - if (costmap_.data[idx].layout.dim.size() < 2) { - return false; - } - - height = costmap_.data[idx].layout.dim[0].size; - width = costmap_.data[idx].layout.dim[1].size; - return true; + return grid_map_utils_.getMapDimensions(width, height); } bool AStar::worldToGrid(double x, double y, int & map_x, int & map_y) const { - int width, height; - if (!getMapDimensions(width, height)) { - return false; - } - - double resolution = costmap_.info.resolution; - if (resolution <= 0.0) { - return false; - } - - double origin_x = costmap_.info.pose.position.x; - double origin_y = costmap_.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; + return grid_map_utils_.worldToGrid(x, y, map_x, map_y); } double AStar::getCellCost(double x, double y) const { - int idx = getLayerIndex(); - if (idx < 0) { - throw std::runtime_error("Costmap layer not found"); - } - if (costmap_.data.empty() || idx >= static_cast(costmap_.data.size())) { - throw std::runtime_error("Costmap data is invalid"); - } - - int width, height; - if (!getMapDimensions(width, height)) { - throw std::runtime_error("Costmap dimensions are invalid"); - } - - int map_x, map_y; - if (!worldToGrid(x, y, map_x, map_y)) { - throw std::runtime_error("Point is outside costmap bounds"); - } - - size_t index = static_cast(map_y * width + map_x); - if (index >= costmap_.data[idx].data.size()) { - throw std::runtime_error("Costmap index is out of range"); + float cost; + if (!grid_map_utils_.tryGetCellCost(x, y, cost)) { + throw std::runtime_error("Unable to get cell cost"); } - return costmap_.data[idx].data[index]; + return cost; } bool AStar::clipGoalToCostmapBoundary( diff --git a/urc_navigation/trajectory_following/include/follower_action_server.hpp b/urc_navigation/trajectory_following/include/follower_action_server.hpp index 00b33f84..f4e5ac76 100644 --- a/urc_navigation/trajectory_following/include/follower_action_server.hpp +++ b/urc_navigation/trajectory_following/include/follower_action_server.hpp @@ -8,6 +8,7 @@ #include #include #include +#include "grid_map_utils.hpp" #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" #include "urc_msgs/action/navigate_to_waypoint.hpp" @@ -58,9 +59,10 @@ class FollowerActionServer : public rclcpp::Node */ void handleCostmap(const grid_map_msgs::msg::GridMap::SharedPtr msg); - float getCost(const grid_map_msgs::msg::GridMap & costmap, double x, double y); + float getCost(double x, double y); 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_; From e8ec81dd93b19d5df5351925ebc66fef9d3ee546 Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Sun, 8 Mar 2026 21:55:10 +0000 Subject: [PATCH 25/25] Create new grid_map_utils package and refactor pure pursuit --- urc_navigation/grid_map_utils/CMakeLists.txt | 45 ++++++++ .../include/grid_map_utils/grid_map_utils.hpp | 29 +++++ urc_navigation/grid_map_utils/package.xml | 20 ++++ .../grid_map_utils/src/grid_map_utils.cpp | 101 ++++++++++++++++++ urc_navigation/path_planning/CMakeLists.txt | 8 +- .../path_planning/include/astar.hpp | 2 +- .../path_planning/include/grid_map_utils.hpp | 27 ----- urc_navigation/path_planning/package.xml | 1 + .../path_planning/src/grid_map_utils.cpp | 95 ---------------- .../trajectory_following/CMakeLists.txt | 2 + .../config/pure_pursuit_config.yaml | 2 +- .../include/follower_action_server.hpp | 2 +- .../trajectory_following/package.xml | 1 + .../src/follower_action_server.cpp | 53 ++------- 14 files changed, 217 insertions(+), 171 deletions(-) create mode 100644 urc_navigation/grid_map_utils/CMakeLists.txt create mode 100644 urc_navigation/grid_map_utils/include/grid_map_utils/grid_map_utils.hpp create mode 100644 urc_navigation/grid_map_utils/package.xml create mode 100644 urc_navigation/grid_map_utils/src/grid_map_utils.cpp delete mode 100644 urc_navigation/path_planning/include/grid_map_utils.hpp delete mode 100644 urc_navigation/path_planning/src/grid_map_utils.cpp 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/path_planning/CMakeLists.txt b/urc_navigation/path_planning/CMakeLists.txt index 3a9286f5..6e3338d0 100644 --- a/urc_navigation/path_planning/CMakeLists.txt +++ b/urc_navigation/path_planning/CMakeLists.txt @@ -5,6 +5,7 @@ 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) @@ -28,12 +29,12 @@ include_directories( add_library(${PROJECT_NAME} SHARED src/planner_server.cpp src/astar.cpp - src/grid_map_utils.cpp ) set(dependencies rclcpp rclcpp_components + grid_map_utils urc_msgs std_msgs grid_map_msgs @@ -64,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 814d914f..88a99b05 100644 --- a/urc_navigation/path_planning/include/astar.hpp +++ b/urc_navigation/path_planning/include/astar.hpp @@ -6,7 +6,7 @@ #include #include #include -#include "grid_map_utils.hpp" +#include "grid_map_utils/grid_map_utils.hpp" #include #include #include diff --git a/urc_navigation/path_planning/include/grid_map_utils.hpp b/urc_navigation/path_planning/include/grid_map_utils.hpp deleted file mode 100644 index fa37049a..00000000 --- a/urc_navigation/path_planning/include/grid_map_utils.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef GRID_MAP_UTILS_HPP_ -#define 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_; - }; -} -#endif \ No newline at end of file diff --git a/urc_navigation/path_planning/package.xml b/urc_navigation/path_planning/package.xml index 44e96be4..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 diff --git a/urc_navigation/path_planning/src/grid_map_utils.cpp b/urc_navigation/path_planning/src/grid_map_utils.cpp deleted file mode 100644 index 4b5ff61a..00000000 --- a/urc_navigation/path_planning/src/grid_map_utils.cpp +++ /dev/null @@ -1,95 +0,0 @@ -#include "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; - } - -} diff --git a/urc_navigation/trajectory_following/CMakeLists.txt b/urc_navigation/trajectory_following/CMakeLists.txt index 727cf748..55701c88 100644 --- a/urc_navigation/trajectory_following/CMakeLists.txt +++ b/urc_navigation/trajectory_following/CMakeLists.txt @@ -5,6 +5,7 @@ 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) @@ -34,6 +35,7 @@ set(dependencies rclcpp rclcpp_action rclcpp_components + grid_map_utils urc_msgs std_msgs grid_map_msgs diff --git a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml b/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml index 1edf9c41..9f058ce6 100644 --- a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml +++ b/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml @@ -6,7 +6,7 @@ follower_action_server: heading_alignment_tolerance: 0.2 enable_holonomic_motion: true lethal_cost_threshold: 0.5 - cmd_vel_stamped: true + cmd_vel_stamped: false cmd_vel_topic: "/cmd_vel" map_frame: "map" base_link_frame: "base_link" diff --git a/urc_navigation/trajectory_following/include/follower_action_server.hpp b/urc_navigation/trajectory_following/include/follower_action_server.hpp index f4e5ac76..d89c2497 100644 --- a/urc_navigation/trajectory_following/include/follower_action_server.hpp +++ b/urc_navigation/trajectory_following/include/follower_action_server.hpp @@ -8,7 +8,7 @@ #include #include #include -#include "grid_map_utils.hpp" +#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" diff --git a/urc_navigation/trajectory_following/package.xml b/urc_navigation/trajectory_following/package.xml index 2a6c5c2f..f5b00ba8 100644 --- a/urc_navigation/trajectory_following/package.xml +++ b/urc_navigation/trajectory_following/package.xml @@ -13,6 +13,7 @@ ament_lint_common ament_cmake_gtest + grid_map_utils rclcpp rclcpp_components urc_msgs diff --git a/urc_navigation/trajectory_following/src/follower_action_server.cpp b/urc_navigation/trajectory_following/src/follower_action_server.cpp index 9aaff7e0..648115ac 100644 --- a/urc_navigation/trajectory_following/src/follower_action_server.cpp +++ b/urc_navigation/trajectory_following/src/follower_action_server.cpp @@ -32,6 +32,7 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions &options) 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( @@ -71,8 +72,9 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions &options) } void FollowerActionServer::handleCostmap( - const grid_map_msgs::msg::GridMap::SharedPtr msg) { + const grid_map_msgs::msg::GridMap::SharedPtr msg) { current_costmap_ = *msg; + grid_map_utils_.setMap(*msg); } geometry_msgs::msg::TransformStamped @@ -182,54 +184,15 @@ void FollowerActionServer::publishZeroVelocity() { } } -float FollowerActionServer::getCost(const grid_map_msgs::msg::GridMap &costmap, - double x, double y) { - // Find the layer index - auto it = std::find(costmap.layers.begin(), costmap.layers.end(), costmap_layer_); - if (it == costmap.layers.end()) { +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; } - size_t layer_index = std::distance(costmap.layers.begin(), it); - // Get grid dimensions from the data layout - if (costmap.data.empty() || layer_index >= costmap.data.size()) { - return 0.0f; - } - - const auto &layer_data = costmap.data[layer_index]; - if (layer_data.layout.dim.size() < 2) { - return 0.0f; - } - - // GridMap uses center of grid as origin - double origin_x = costmap.info.pose.position.x; - double origin_y = costmap.info.pose.position.y; - double resolution = costmap.info.resolution; - - // Calculate map indices (GridMap stores data row-major) - int width = layer_data.layout.dim[1].size; - int height = layer_data.layout.dim[0].size; - - // Transform world coordinates to grid coordinates - double rel_x = x - origin_x; - double rel_y = y - origin_y; - - int map_x = static_cast(rel_x / resolution + width / 2.0); - int map_y = static_cast(rel_y / resolution + height / 2.0); - - if (map_x < 0 || map_x >= width || map_y < 0 || map_y >= height) { - return 0.0f; - } - - // Access data in row-major order - size_t index = map_y * width + map_x; - if (index >= layer_data.data.size()) { - return 0.0f; - } - - return layer_data.data[index]; + return cost; } nav_msgs::msg::Path FollowerActionServer::callPlanningService( @@ -465,7 +428,7 @@ 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_double()) { // Obstacle detected - attempt to re-plan