diff --git a/CMakeLists.txt b/CMakeLists.txt index ce3551b..1abd9ae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() # find dependencies @@ -24,7 +24,7 @@ ament_target_dependencies(nav_to_anywhere rclcpp rclcpp_action nav2_msgs nav_2d_ target_include_directories(nav_to_anywhere PUBLIC $ $) -target_compile_features(nav_to_anywhere PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_features(nav_to_anywhere PUBLIC c_std_99 cxx_std_20) # Require C99 and C++20 install(TARGETS nav_to_anywhere DESTINATION lib/${PROJECT_NAME}) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index 5264356..8ca068d 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -13,6 +13,36 @@ #include "nav2_costmap_2d/footprint.hpp" #include "nav_to_anywhere/utils.hpp" +using NavigateToPose = nav2_msgs::action::NavigateToPose; +using GoalHandleNavigateToPose = rclcpp_action::ServerGoalHandle; + +struct Params +{ + std::string topic_footprint; + std::string footprint_loaded; + std::string footprint_unloaded; +}; + +struct Config +{ + const std::vector footprint_loaded; + const std::vector footprint_unloaded; + const float interval = 0.2; // seconds +}; + +struct Agent +{ + std::vector footprint; + geometry_msgs::msg::Pose2D pos_active{}; + const float velocity = 1; // m/s + std::shared_ptr current_goal_handle{}; + rclcpp::Time nav_start_time; + std::shared_ptr get_goal() const + { + return this->current_goal_handle->get_goal(); + } +}; + geometry_msgs::msg::PolygonStamped transformFootprint( const geometry_msgs::msg::Pose2D & pose, const std::vector & footprint) @@ -22,6 +52,74 @@ geometry_msgs::msg::PolygonStamped transformFootprint( return oriented_footprint; } +std::vector makeFootprintFromString(const std::string & fp_string) +{ + std::vector fp; + return nav2_costmap_2d::makeFootprintFromString(fp_string, fp) ? + fp : nav2_costmap_2d::makeFootprintFromRadius(0.3); +} + +/* map -> base_footprint tf */ +geometry_msgs::msg::TransformStamped +get_transform(const geometry_msgs::msg::Pose2D & pose, const rclcpp::Time & stamp) +{ + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "map"; + transform.child_frame_id = "base_footprint"; + transform.transform.translation.x = pose.x; + transform.transform.translation.y = pose.y; + transform.transform.rotation = nav_2d_utils::pose2DToPose(pose).orientation; + transform.header.stamp = stamp; + return transform; +} + +/* increment mission progress */ +bool update_current_pos( + Agent & agent, const Config & config, const rclcpp::Time & stamp, + const std::vector & bt_actions) +{ + const auto current_action = get_action( + bt_actions, + agent.get_goal()->behavior_tree); + + if (current_action.type == ACTION_PICK || current_action.type == ACTION_DROP) { + const auto elapsed_time = stamp - agent.nav_start_time; + if (elapsed_time.seconds() > current_action.duration) { + agent.footprint = current_action.type == ACTION_PICK ? + config.footprint_loaded : + config.footprint_unloaded; + return true; + } + return false; + } + + if (current_action.type != ACTION_NAV) { + return true; + } + const auto pos_target = nav_2d_utils::poseToPose2D(agent.get_goal()->pose.pose); + const auto dy = pos_target.y - agent.pos_active.y; + const auto dx = pos_target.x - agent.pos_active.x; + const auto theta = std::atan2(dy, dx); + + const auto vx = std::cos(theta) * agent.velocity; + const auto vy = std::sin(theta) * agent.velocity; + const auto idx = vx * config.interval; + const auto idy = vy * config.interval; + + /* if we are within one step of our goal */ + if (dy * dy + dx * dx < idy * idy + idx * idx) { + agent.pos_active.x = pos_target.x; + agent.pos_active.y = pos_target.y; + agent.pos_active.theta = pos_target.theta; + return true; + } else { + agent.pos_active.x += idx; + agent.pos_active.y += idy; + agent.pos_active.theta = theta; + } + return false; +} + int main(int argc, char * argv[]) { const auto footprint_default_loaded = @@ -32,128 +130,81 @@ int main(int argc, char * argv[]) const auto node = std::make_shared("nav_to_anywhere"); + const Params params { + .topic_footprint = + node->declare_parameter("topic_footprint", "local_costmap/published_footprint"), + .footprint_loaded = + node->declare_parameter("footprint_loaded", footprint_default_loaded), + .footprint_unloaded = + node->declare_parameter("footprint_unloaded", footprint_default_unloaded), + }; + + Config config { + .footprint_loaded = makeFootprintFromString(params.footprint_loaded), + .footprint_unloaded = makeFootprintFromString(params.footprint_unloaded), + }; + + Agent agent { + .footprint = config.footprint_unloaded, + .nav_start_time = node->get_clock()->now(), + }; + const auto bt_actions = get_action_details(node); - RCLCPP_INFO(node->get_logger(), "Action config:"); + std::stringstream ss; for (const auto & detail : bt_actions) { - RCLCPP_INFO_STREAM( - node->get_logger(), - " name: " << detail.name << ", regex: '" << detail.regex << - "', type: " << detail.type << ", duration: " << detail.duration - ); + ss << + "\n Name: " << detail.name << + "\n Regex: '" << detail.regex << "'" << + "\n Type: " << detail.type << + "\n Duration: " << detail.duration; } + RCLCPP_INFO_STREAM( + node->get_logger(), "nav_to_anywhere config:" << + "\n Params: " << + "\n topic_footprint: " << params.topic_footprint << + "\n footprint_loaded: " << params.footprint_loaded << + "\n footprint_unloaded: " << params.footprint_unloaded << + "\n Config: " << + "\n fp loaded points: " << config.footprint_loaded.size() << + "\n fp unloaded points: " << config.footprint_unloaded.size() << + "\n interval (seconds): " << config.interval << + "\n Action Config: " << ss.str() + ); tf2_ros::TransformBroadcaster tf_broadcaster(*node); const auto local_footprint_pub = node->create_publisher( - node->declare_parameter("topic_footprint", "local_costmap/published_footprint"), + params.topic_footprint, rclcpp::SystemDefaultsQoS()); - std::vector footprint; - const auto footprint_loaded = nav2_costmap_2d::makeFootprintFromString( - node->declare_parameter("footprint_loaded", footprint_default_loaded), - footprint - ) ? footprint : nav2_costmap_2d::makeFootprintFromRadius(0.3); - - footprint.clear(); - const auto footprint_unloaded = nav2_costmap_2d::makeFootprintFromString( - node->declare_parameter("footprint_unloaded", footprint_default_unloaded), - footprint - ) ? footprint : nav2_costmap_2d::makeFootprintFromRadius(0.3); - - using NavigateToPose = nav2_msgs::action::NavigateToPose; - using GoalHandleNavigateToPose = rclcpp_action::ServerGoalHandle; - - geometry_msgs::msg::Pose2D pos_active; - std::shared_ptr current_goal_handle; - auto nav_start_time = node->get_clock()->now(); - - const auto interval = 0.2; // seconds - const auto velocity = 1; // m/s - - - /* broadcast map -> base_footprint tf */ - const auto broadcast_tf = [&]() { - geometry_msgs::msg::TransformStamped transform; - transform.header.frame_id = "map"; - transform.child_frame_id = "base_footprint"; - transform.transform.translation.x = pos_active.x; - transform.transform.translation.y = pos_active.y; - transform.transform.rotation = nav_2d_utils::pose2DToPose(pos_active).orientation; - transform.header.stamp = node->get_clock()->now(); - tf_broadcaster.sendTransform(transform); - }; - - - /* increment mission progress */ - const auto update_current_pos = [&]() { - const auto current_action = get_action( - bt_actions, - current_goal_handle->get_goal()->behavior_tree); - - if (current_action.type == ACTION_PICK || current_action.type == ACTION_DROP) { - const auto elapsed_time = node->get_clock()->now() - nav_start_time; - if (elapsed_time.seconds() > current_action.duration) { - footprint = current_action.type == ACTION_PICK ? - footprint_loaded : - footprint_unloaded; - return true; - } - return false; - } - - if (current_action.type != ACTION_NAV) { - RCLCPP_INFO(node->get_logger(), "Beep boop - doing robot stuff"); - return true; - } - - const auto pos_target = nav_2d_utils::poseToPose2D( - current_goal_handle->get_goal()->pose.pose); - const auto dy = pos_target.y - pos_active.y; - const auto dx = pos_target.x - pos_active.x; - const auto theta = std::atan2(dy, dx); - - const auto vx = std::cos(theta) * velocity; - const auto vy = std::sin(theta) * velocity; - const auto idx = vx * interval; - const auto idy = vy * interval; - - /* if we are within one step of our goal */ - if (dy * dy + dx * dx < idy * idy + idx * idx) { - pos_active.x = pos_target.x; - pos_active.y = pos_target.y; - pos_active.theta = pos_target.theta; - return true; - } else { - pos_active.x += idx; - pos_active.y += idy; - pos_active.theta = theta; - } - return false; - }; - /* periodic timer for incrementing progress */ const auto tick = node->create_wall_timer( - std::chrono::milliseconds(static_cast(interval * 1000)), [&]() { + std::chrono::milliseconds(static_cast(config.interval * 1000)), [&]() { /* if we have an active mission */ - if (current_goal_handle) { - const auto goal_reached = update_current_pos(); + if (agent.current_goal_handle) { + const auto goal_reached = update_current_pos( + agent, config, node->get_clock()->now(), bt_actions); auto feedback = std::make_unique(); feedback->number_of_recoveries = 0; - feedback->current_pose.pose = nav_2d_utils::pose2DToPose(pos_active); - feedback->navigation_time = node->get_clock()->now() - nav_start_time; - current_goal_handle->publish_feedback(std::move(feedback)); + feedback->current_pose.pose = nav_2d_utils::pose2DToPose(agent.pos_active); + feedback->navigation_time = node->get_clock()->now() - agent.nav_start_time; + agent.current_goal_handle->publish_feedback(std::move(feedback)); if (goal_reached) { - current_goal_handle->succeed(std::make_unique()); - current_goal_handle.reset(); + agent.current_goal_handle->succeed(std::make_unique()); + agent.current_goal_handle.reset(); + } else if (agent.current_goal_handle->is_canceling()) { + auto result = std::make_shared(); + agent.current_goal_handle->canceled(result); + agent.current_goal_handle.reset(); } } /* broadcast base_footprint in map frame */ - broadcast_tf(); + tf_broadcaster.sendTransform(get_transform(agent.pos_active, node->get_clock()->now())); /* publish local footprint */ - local_footprint_pub->publish(transformFootprint(pos_active, footprint)); + local_footprint_pub->publish(transformFootprint(agent.pos_active, agent.footprint)); }); @@ -187,12 +238,12 @@ int main(int argc, char * argv[]) /* handle_accepted */ [&](const std::shared_ptr goal_handle) { - if (current_goal_handle) { - current_goal_handle->abort(std::make_unique()); + if (agent.current_goal_handle) { + agent.current_goal_handle->abort(std::make_unique()); } else { - nav_start_time = node->get_clock()->now(); + agent.nav_start_time = node->get_clock()->now(); } - current_goal_handle = goal_handle; + agent.current_goal_handle = goal_handle; } );