From 914f118e6a8b9a8792fcb96c7e7dfebd6055efe8 Mon Sep 17 00:00:00 2001 From: Aaron Date: Sat, 22 Apr 2023 18:02:15 +0200 Subject: [PATCH 01/18] add cancel support --- src/nav_to_anywhere.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index 5264356..20fa8da 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -147,6 +147,10 @@ int main(int argc, char * argv[]) if (goal_reached) { current_goal_handle->succeed(std::make_unique()); current_goal_handle.reset(); + } else if (current_goal_handle->is_canceling()) { + auto result = std::make_shared(); + current_goal_handle->canceled(result); + current_goal_handle.reset(); } } /* broadcast base_footprint in map frame */ From 035a5ef3aceb3a82e4891938323b179e1636246f Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 15:10:40 +0200 Subject: [PATCH 02/18] extract params to struct --- src/nav_to_anywhere.cpp | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index 20fa8da..bf04d39 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -13,6 +13,13 @@ #include "nav2_costmap_2d/footprint.hpp" #include "nav_to_anywhere/utils.hpp" +struct Params +{ + std::string topic_footprint; + std::string footprint_loaded; + std::string footprint_unloaded; +}; + geometry_msgs::msg::PolygonStamped transformFootprint( const geometry_msgs::msg::Pose2D & pose, const std::vector & footprint) @@ -42,20 +49,29 @@ int main(int argc, char * argv[]) ); } + 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), + }; + 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), + params.footprint_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), + params.footprint_unloaded, footprint ) ? footprint : nav2_costmap_2d::makeFootprintFromRadius(0.3); From 6f10477644042d291eba183554ce2a7735ac4922 Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 15:11:54 +0200 Subject: [PATCH 03/18] use designated initializers with cpp20 --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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}) From fcf2a58198b84c2dafe4b3862c7d0c1721bdcd50 Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 15:20:37 +0200 Subject: [PATCH 04/18] move locals to config --- src/nav_to_anywhere.cpp | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index bf04d39..da19e44 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -20,6 +20,13 @@ struct Params std::string footprint_unloaded; }; +struct Config +{ + std::vector footprint; + std::vector footprint_loaded; + std::vector footprint_unloaded; +}; + geometry_msgs::msg::PolygonStamped transformFootprint( const geometry_msgs::msg::Pose2D & pose, const std::vector & footprint) @@ -58,22 +65,23 @@ int main(int argc, char * argv[]) node->declare_parameter("footprint_unloaded", footprint_default_unloaded), }; + Config config {}; + tf2_ros::TransformBroadcaster tf_broadcaster(*node); const auto local_footprint_pub = node->create_publisher( params.topic_footprint, rclcpp::SystemDefaultsQoS()); - std::vector footprint; - const auto footprint_loaded = nav2_costmap_2d::makeFootprintFromString( + config.footprint_loaded = nav2_costmap_2d::makeFootprintFromString( params.footprint_loaded, - footprint - ) ? footprint : nav2_costmap_2d::makeFootprintFromRadius(0.3); + config.footprint + ) ? config.footprint : nav2_costmap_2d::makeFootprintFromRadius(0.3); - footprint.clear(); - const auto footprint_unloaded = nav2_costmap_2d::makeFootprintFromString( + config.footprint.clear(); + config.footprint_unloaded = nav2_costmap_2d::makeFootprintFromString( params.footprint_unloaded, - footprint - ) ? footprint : nav2_costmap_2d::makeFootprintFromRadius(0.3); + config.footprint + ) ? config.footprint : nav2_costmap_2d::makeFootprintFromRadius(0.3); using NavigateToPose = nav2_msgs::action::NavigateToPose; using GoalHandleNavigateToPose = rclcpp_action::ServerGoalHandle; @@ -108,9 +116,9 @@ int main(int argc, char * argv[]) 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; + config.footprint = current_action.type == ACTION_PICK ? + config.footprint_loaded : + config.footprint_unloaded; return true; } return false; @@ -173,7 +181,7 @@ int main(int argc, char * argv[]) broadcast_tf(); /* publish local footprint */ - local_footprint_pub->publish(transformFootprint(pos_active, footprint)); + local_footprint_pub->publish(transformFootprint(pos_active, config.footprint)); }); From ad58e9b9c6926e2f29b7b9c1888bc44ca6ba4700 Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 15:27:14 +0200 Subject: [PATCH 05/18] move pose_active to config --- src/nav_to_anywhere.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index da19e44..503c3fe 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -25,6 +25,7 @@ struct Config std::vector footprint; std::vector footprint_loaded; std::vector footprint_unloaded; + geometry_msgs::msg::Pose2D pos_active{}; }; geometry_msgs::msg::PolygonStamped transformFootprint( @@ -86,7 +87,6 @@ int main(int argc, char * argv[]) 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(); @@ -99,9 +99,9 @@ int main(int argc, char * argv[]) 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.transform.translation.x = config.pos_active.x; + transform.transform.translation.y = config.pos_active.y; + transform.transform.rotation = nav_2d_utils::pose2DToPose(config.pos_active).orientation; transform.header.stamp = node->get_clock()->now(); tf_broadcaster.sendTransform(transform); }; @@ -131,8 +131,8 @@ int main(int argc, char * argv[]) 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 dy = pos_target.y - config.pos_active.y; + const auto dx = pos_target.x - config.pos_active.x; const auto theta = std::atan2(dy, dx); const auto vx = std::cos(theta) * velocity; @@ -142,14 +142,14 @@ int main(int argc, char * argv[]) /* 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; + config.pos_active.x = pos_target.x; + config.pos_active.y = pos_target.y; + config.pos_active.theta = pos_target.theta; return true; } else { - pos_active.x += idx; - pos_active.y += idy; - pos_active.theta = theta; + config.pos_active.x += idx; + config.pos_active.y += idy; + config.pos_active.theta = theta; } return false; }; @@ -164,7 +164,7 @@ int main(int argc, char * argv[]) auto feedback = std::make_unique(); feedback->number_of_recoveries = 0; - feedback->current_pose.pose = nav_2d_utils::pose2DToPose(pos_active); + feedback->current_pose.pose = nav_2d_utils::pose2DToPose(config.pos_active); feedback->navigation_time = node->get_clock()->now() - nav_start_time; current_goal_handle->publish_feedback(std::move(feedback)); @@ -181,7 +181,7 @@ int main(int argc, char * argv[]) broadcast_tf(); /* publish local footprint */ - local_footprint_pub->publish(transformFootprint(pos_active, config.footprint)); + local_footprint_pub->publish(transformFootprint(config.pos_active, config.footprint)); }); From 670f54d8581e36d77e39d84a7557ba4c6838fe26 Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 15:33:14 +0200 Subject: [PATCH 06/18] move velocity to config --- src/nav_to_anywhere.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index 503c3fe..a64444b 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -26,6 +26,7 @@ struct Config std::vector footprint_loaded; std::vector footprint_unloaded; geometry_msgs::msg::Pose2D pos_active{}; + const float velocity = 1; // m/s }; geometry_msgs::msg::PolygonStamped transformFootprint( @@ -91,7 +92,6 @@ int main(int argc, char * argv[]) 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 */ @@ -135,8 +135,8 @@ int main(int argc, char * argv[]) const auto dx = pos_target.x - config.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 vx = std::cos(theta) * config.velocity; + const auto vy = std::sin(theta) * config.velocity; const auto idx = vx * interval; const auto idy = vy * interval; From 155a8d02224638e74f16ba6f578555b7f78ba360 Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 15:34:39 +0200 Subject: [PATCH 07/18] move interval to config --- src/nav_to_anywhere.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index a64444b..d28ea72 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -27,6 +27,7 @@ struct Config std::vector footprint_unloaded; geometry_msgs::msg::Pose2D pos_active{}; const float velocity = 1; // m/s + const float interval = 0.2; // seconds }; geometry_msgs::msg::PolygonStamped transformFootprint( @@ -91,9 +92,6 @@ int main(int argc, char * argv[]) std::shared_ptr current_goal_handle; auto nav_start_time = node->get_clock()->now(); - const auto interval = 0.2; // seconds - - /* broadcast map -> base_footprint tf */ const auto broadcast_tf = [&]() { geometry_msgs::msg::TransformStamped transform; @@ -137,8 +135,8 @@ int main(int argc, char * argv[]) const auto vx = std::cos(theta) * config.velocity; const auto vy = std::sin(theta) * config.velocity; - const auto idx = vx * interval; - const auto idy = vy * interval; + 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) { @@ -157,7 +155,7 @@ int main(int argc, char * argv[]) /* 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(); From 36269c299150b8735443ba9f61b280eb01099d2d Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 15:49:24 +0200 Subject: [PATCH 08/18] refactor to pure function --- src/nav_to_anywhere.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index d28ea72..d0649e6 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -92,16 +92,17 @@ int main(int argc, char * argv[]) std::shared_ptr current_goal_handle; auto nav_start_time = node->get_clock()->now(); - /* broadcast map -> base_footprint tf */ - const auto broadcast_tf = [&]() { + /* map -> base_footprint tf */ + const auto 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 = config.pos_active.x; - transform.transform.translation.y = config.pos_active.y; - transform.transform.rotation = nav_2d_utils::pose2DToPose(config.pos_active).orientation; - transform.header.stamp = node->get_clock()->now(); - tf_broadcaster.sendTransform(transform); + 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; }; @@ -176,7 +177,7 @@ int main(int argc, char * argv[]) } } /* broadcast base_footprint in map frame */ - broadcast_tf(); + tf_broadcaster.sendTransform(get_transform(config.pos_active, node->get_clock()->now())); /* publish local footprint */ local_footprint_pub->publish(transformFootprint(config.pos_active, config.footprint)); From 25424f59c0d2ef2f5f92a10431beefa63147c26d Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 16:01:15 +0200 Subject: [PATCH 09/18] extract function makeFootprintFromString --- src/nav_to_anywhere.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index d0649e6..3a1c290 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -39,6 +39,13 @@ 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); +} + int main(int argc, char * argv[]) { const auto footprint_default_loaded = @@ -75,16 +82,8 @@ int main(int argc, char * argv[]) params.topic_footprint, rclcpp::SystemDefaultsQoS()); - config.footprint_loaded = nav2_costmap_2d::makeFootprintFromString( - params.footprint_loaded, - config.footprint - ) ? config.footprint : nav2_costmap_2d::makeFootprintFromRadius(0.3); - - config.footprint.clear(); - config.footprint_unloaded = nav2_costmap_2d::makeFootprintFromString( - params.footprint_unloaded, - config.footprint - ) ? config.footprint : nav2_costmap_2d::makeFootprintFromRadius(0.3); + config.footprint_loaded = makeFootprintFromString(params.footprint_loaded); + config.footprint_unloaded = makeFootprintFromString(params.footprint_unloaded); using NavigateToPose = nav2_msgs::action::NavigateToPose; using GoalHandleNavigateToPose = rclcpp_action::ServerGoalHandle; From 5564a00d52ae86005339320d9beeddd44e481d6a Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 16:10:48 +0200 Subject: [PATCH 10/18] use designated initialisers for config --- src/nav_to_anywhere.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index 3a1c290..4dbfc59 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -23,8 +23,8 @@ struct Params struct Config { std::vector footprint; - std::vector footprint_loaded; - std::vector footprint_unloaded; + const std::vector footprint_loaded; + const std::vector footprint_unloaded; geometry_msgs::msg::Pose2D pos_active{}; const float velocity = 1; // m/s const float interval = 0.2; // seconds @@ -75,16 +75,17 @@ int main(int argc, char * argv[]) node->declare_parameter("footprint_unloaded", footprint_default_unloaded), }; - Config config {}; + Config config { + .footprint = {}, + .footprint_loaded = makeFootprintFromString(params.footprint_loaded), + .footprint_unloaded = makeFootprintFromString(params.footprint_unloaded), + }; tf2_ros::TransformBroadcaster tf_broadcaster(*node); const auto local_footprint_pub = node->create_publisher( params.topic_footprint, rclcpp::SystemDefaultsQoS()); - config.footprint_loaded = makeFootprintFromString(params.footprint_loaded); - config.footprint_unloaded = makeFootprintFromString(params.footprint_unloaded); - using NavigateToPose = nav2_msgs::action::NavigateToPose; using GoalHandleNavigateToPose = rclcpp_action::ServerGoalHandle; From 1b0b25d3bc7608304dea941c1fd399a880d920d8 Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 16:18:18 +0200 Subject: [PATCH 11/18] extract dynamic values to agent --- src/nav_to_anywhere.cpp | 41 ++++++++++++++++++++++++----------------- 1 file changed, 24 insertions(+), 17 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index 4dbfc59..375b7b6 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -22,12 +22,16 @@ struct Params struct Config { - std::vector footprint; 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 - const float interval = 0.2; // seconds }; geometry_msgs::msg::PolygonStamped transformFootprint( @@ -76,11 +80,14 @@ int main(int argc, char * argv[]) }; Config config { - .footprint = {}, .footprint_loaded = makeFootprintFromString(params.footprint_loaded), .footprint_unloaded = makeFootprintFromString(params.footprint_unloaded), }; + Agent agent { + .footprint = config.footprint_unloaded, + }; + tf2_ros::TransformBroadcaster tf_broadcaster(*node); const auto local_footprint_pub = node->create_publisher( params.topic_footprint, @@ -115,7 +122,7 @@ int main(int argc, char * argv[]) 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) { - config.footprint = current_action.type == ACTION_PICK ? + agent.footprint = current_action.type == ACTION_PICK ? config.footprint_loaded : config.footprint_unloaded; return true; @@ -130,25 +137,25 @@ int main(int argc, char * argv[]) const auto pos_target = nav_2d_utils::poseToPose2D( current_goal_handle->get_goal()->pose.pose); - const auto dy = pos_target.y - config.pos_active.y; - const auto dx = pos_target.x - config.pos_active.x; + 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) * config.velocity; - const auto vy = std::sin(theta) * config.velocity; + 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) { - config.pos_active.x = pos_target.x; - config.pos_active.y = pos_target.y; - config.pos_active.theta = pos_target.theta; + agent.pos_active.x = pos_target.x; + agent.pos_active.y = pos_target.y; + agent.pos_active.theta = pos_target.theta; return true; } else { - config.pos_active.x += idx; - config.pos_active.y += idy; - config.pos_active.theta = theta; + agent.pos_active.x += idx; + agent.pos_active.y += idy; + agent.pos_active.theta = theta; } return false; }; @@ -163,7 +170,7 @@ int main(int argc, char * argv[]) auto feedback = std::make_unique(); feedback->number_of_recoveries = 0; - feedback->current_pose.pose = nav_2d_utils::pose2DToPose(config.pos_active); + feedback->current_pose.pose = nav_2d_utils::pose2DToPose(agent.pos_active); feedback->navigation_time = node->get_clock()->now() - nav_start_time; current_goal_handle->publish_feedback(std::move(feedback)); @@ -177,10 +184,10 @@ int main(int argc, char * argv[]) } } /* broadcast base_footprint in map frame */ - tf_broadcaster.sendTransform(get_transform(config.pos_active, node->get_clock()->now())); + tf_broadcaster.sendTransform(get_transform(agent.pos_active, node->get_clock()->now())); /* publish local footprint */ - local_footprint_pub->publish(transformFootprint(config.pos_active, config.footprint)); + local_footprint_pub->publish(transformFootprint(agent.pos_active, agent.footprint)); }); From 260200015f057f96e44bf34aa62fdd361ffb5fb1 Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 16:57:04 +0200 Subject: [PATCH 12/18] move goal_handle and nav_start_time to agent --- src/nav_to_anywhere.cpp | 46 ++++++++++++++++++++++------------------- 1 file changed, 25 insertions(+), 21 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index 375b7b6..5ce0544 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -13,6 +13,9 @@ #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; @@ -32,6 +35,12 @@ 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( @@ -86,6 +95,7 @@ int main(int argc, char * argv[]) Agent agent { .footprint = config.footprint_unloaded, + .nav_start_time = node->get_clock()->now(), }; tf2_ros::TransformBroadcaster tf_broadcaster(*node); @@ -93,12 +103,6 @@ int main(int argc, char * argv[]) params.topic_footprint, rclcpp::SystemDefaultsQoS()); - using NavigateToPose = nav2_msgs::action::NavigateToPose; - using GoalHandleNavigateToPose = rclcpp_action::ServerGoalHandle; - - std::shared_ptr current_goal_handle; - auto nav_start_time = node->get_clock()->now(); - /* map -> base_footprint tf */ const auto get_transform = [](const geometry_msgs::msg::Pose2D & pose, const rclcpp::Time & stamp) { @@ -117,10 +121,10 @@ int main(int argc, char * argv[]) const auto update_current_pos = [&]() { const auto current_action = get_action( bt_actions, - current_goal_handle->get_goal()->behavior_tree); + agent.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; + const auto elapsed_time = node->get_clock()->now() - agent.nav_start_time; if (elapsed_time.seconds() > current_action.duration) { agent.footprint = current_action.type == ACTION_PICK ? config.footprint_loaded : @@ -136,7 +140,7 @@ int main(int argc, char * argv[]) } const auto pos_target = nav_2d_utils::poseToPose2D( - current_goal_handle->get_goal()->pose.pose); + 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); @@ -165,22 +169,22 @@ int main(int argc, char * argv[]) const auto tick = node->create_wall_timer( std::chrono::milliseconds(static_cast(config.interval * 1000)), [&]() { /* if we have an active mission */ - if (current_goal_handle) { + if (agent.current_goal_handle) { const auto goal_reached = update_current_pos(); auto feedback = std::make_unique(); feedback->number_of_recoveries = 0; feedback->current_pose.pose = nav_2d_utils::pose2DToPose(agent.pos_active); - feedback->navigation_time = node->get_clock()->now() - nav_start_time; - current_goal_handle->publish_feedback(std::move(feedback)); + 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(); - } else if (current_goal_handle->is_canceling()) { + 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(); - current_goal_handle->canceled(result); - current_goal_handle.reset(); + agent.current_goal_handle->canceled(result); + agent.current_goal_handle.reset(); } } /* broadcast base_footprint in map frame */ @@ -221,12 +225,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; } ); From 8a3ccad085ecf240a5eb2ed9179e030ec3def72a Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 17:02:40 +0200 Subject: [PATCH 13/18] remove closure from update_current_pos --- src/nav_to_anywhere.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index 5ce0544..a7ce06c 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -118,13 +118,15 @@ int main(int argc, char * argv[]) /* increment mission progress */ - const auto update_current_pos = [&]() { + const auto 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 = node->get_clock()->now() - agent.nav_start_time; + 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 : @@ -135,7 +137,6 @@ int main(int argc, char * argv[]) } if (current_action.type != ACTION_NAV) { - RCLCPP_INFO(node->get_logger(), "Beep boop - doing robot stuff"); return true; } @@ -170,7 +171,8 @@ int main(int argc, char * argv[]) std::chrono::milliseconds(static_cast(config.interval * 1000)), [&]() { /* if we have an active mission */ if (agent.current_goal_handle) { - const auto goal_reached = update_current_pos(); + 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; From db1cbe0f2667e75d907b5b3dfbd76b663f010c9e Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 17:04:03 +0200 Subject: [PATCH 14/18] use the freedup line length --- src/nav_to_anywhere.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index a7ce06c..8f6a794 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -139,9 +139,7 @@ int main(int argc, char * argv[]) if (current_action.type != ACTION_NAV) { return true; } - - const auto pos_target = nav_2d_utils::poseToPose2D( - agent.get_goal()->pose.pose); + 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); From e1fb52f46c3b70cbffcc533c609d82b5f3a40467 Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 17:06:30 +0200 Subject: [PATCH 15/18] move lambda to function --- src/nav_to_anywhere.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index 8f6a794..6159ea2 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -59,6 +59,20 @@ std::vector makeFootprintFromString(const std::string 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; +} + int main(int argc, char * argv[]) { const auto footprint_default_loaded = @@ -103,20 +117,6 @@ int main(int argc, char * argv[]) params.topic_footprint, rclcpp::SystemDefaultsQoS()); - /* map -> base_footprint tf */ - const auto 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 */ const auto update_current_pos = [](Agent & agent, const Config & config, const rclcpp::Time & stamp, From 0da688641941fcae7df034f1fe03acf9e1f18eae Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 17:08:03 +0200 Subject: [PATCH 16/18] move lambda update_current_pos to function --- src/nav_to_anywhere.cpp | 93 +++++++++++++++++++++-------------------- 1 file changed, 47 insertions(+), 46 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index 6159ea2..957b3a3 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -73,6 +73,53 @@ get_transform(const geometry_msgs::msg::Pose2D & pose, const rclcpp::Time & stam 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 = @@ -117,52 +164,6 @@ int main(int argc, char * argv[]) params.topic_footprint, rclcpp::SystemDefaultsQoS()); - /* increment mission progress */ - const auto 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; - }; - /* periodic timer for incrementing progress */ const auto tick = node->create_wall_timer( From 88553d27b296b7a64ff1a42f167f79e563097fc5 Mon Sep 17 00:00:00 2001 From: Aaron Date: Sun, 23 Apr 2023 17:25:18 +0200 Subject: [PATCH 17/18] print setup on start --- src/nav_to_anywhere.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index 957b3a3..56c866c 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -159,6 +159,17 @@ int main(int argc, char * argv[]) .nav_start_time = node->get_clock()->now(), }; + 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 + ); tf2_ros::TransformBroadcaster tf_broadcaster(*node); const auto local_footprint_pub = node->create_publisher( params.topic_footprint, From bf82febb073e37191f795f4d9f6a5eff27326cf0 Mon Sep 17 00:00:00 2001 From: Aaron Date: Wed, 10 May 2023 18:42:48 +0200 Subject: [PATCH 18/18] print action config with rest --- src/nav_to_anywhere.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/nav_to_anywhere.cpp b/src/nav_to_anywhere.cpp index 56c866c..8ca068d 100644 --- a/src/nav_to_anywhere.cpp +++ b/src/nav_to_anywhere.cpp @@ -130,16 +130,6 @@ int main(int argc, char * argv[]) const auto node = std::make_shared("nav_to_anywhere"); - const auto bt_actions = get_action_details(node); - RCLCPP_INFO(node->get_logger(), "Action config:"); - for (const auto & detail : bt_actions) { - RCLCPP_INFO_STREAM( - node->get_logger(), - " name: " << detail.name << ", regex: '" << detail.regex << - "', type: " << detail.type << ", duration: " << detail.duration - ); - } - const Params params { .topic_footprint = node->declare_parameter("topic_footprint", "local_costmap/published_footprint"), @@ -159,6 +149,16 @@ int main(int argc, char * argv[]) .nav_start_time = node->get_clock()->now(), }; + const auto bt_actions = get_action_details(node); + std::stringstream ss; + for (const auto & detail : bt_actions) { + 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: " << @@ -168,7 +168,8 @@ int main(int argc, char * argv[]) "\n Config: " << "\n fp loaded points: " << config.footprint_loaded.size() << "\n fp unloaded points: " << config.footprint_unloaded.size() << - "\n interval (seconds): " << config.interval + "\n interval (seconds): " << config.interval << + "\n Action Config: " << ss.str() ); tf2_ros::TransformBroadcaster tf_broadcaster(*node); const auto local_footprint_pub = node->create_publisher(